ksenia_mikhailova
/
modbus_test
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

pick

This commit is contained in:
Kseninia Mikhaylova 2024-12-13 16:00:40 +03:00
parent dacbe2ec74
commit fbd148ed31
1 changed files with 9 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
robot/client_socket.py
View File

@ -204,19 +204,19 @@ class SocketRobotArm:
threading.Thread(target=self.imitate_func, daemon=True).start() threading.Thread(target=self.imitate_func, daemon=True).start()
def palletizing(self, position, box_id): def palletizing(self, position, box_id):
pos = position[:] dir_wall = (self.urdf_manager.conv_direction + 1) % 2
conv = position[:]
conv[1] += 0.4
position[1] += 0.02 conv = position[:]
conv[dir_wall] -= 0.4
position[dir_wall] -= 0.02
json_res = [] json_res = []
tool_pos = [0, 0, 0]
for pos in [conv, position]: for pos in [conv, position]:
jointPoses = self.convert_to_joint_base( jointPoses = self.convert_to_joint_base(
pos, pos,
# np.radians([0, 0, 0]), np.radians(tool_pos),
np.radians([0, 0, -90]),
) )
json_res.append( json_res.append(
self.prepare_data.make_step( self.prepare_data.make_step(
@ -225,23 +225,10 @@ class SocketRobotArm:
) )
self.motionFund(jointPoses) self.motionFund(jointPoses)
time.sleep(2) time.sleep(2)
with open(f"data/palletizing.json", "w") as myfile: with open(f"data/palletizing.json", "w") as myfile:
myfile.write(json.dumps(json_res)) myfile.write(json.dumps(json_res))
end_effector_state = p.getLinkState(
self.body_id, self.num_joints - 1, computeForwardKinematics=True
)
end_effector_pos = end_effector_state[0] # Позиция наконечника
end_effector_orn = end_effector_state[1] # Ориентация наконечника
object_pos, object_orn = p.getBasePositionAndOrientation(box_id)
# Используем ориентацию объекта (коробки) как ориентацию родителя
parentFrameOrientation = object_orn
# # Определяем смещение для фиксации позиции (без вращения)
parentFramePosition = [pos[i] - end_effector_pos[i] for i in range(3)]
# Создаём констрейнт для прикрепления # Создаём констрейнт для прикрепления
constraint_id = p.createConstraint( constraint_id = p.createConstraint(
parentBodyUniqueId=self.body_id, parentBodyUniqueId=self.body_id,
@ -250,7 +237,7 @@ class SocketRobotArm:
childLinkIndex=-1, childLinkIndex=-1,
jointType=p.JOINT_FIXED, jointType=p.JOINT_FIXED,
jointAxis=[0, 0, 0], jointAxis=[0, 0, 0],
parentFramePosition=[0,0,0], parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0.2, 0], childFramePosition=[0, 0.2, 0],
parentFrameOrientation=p.getQuaternionFromEuler(np.radians([0, 0, 90])), parentFrameOrientation=p.getQuaternionFromEuler(np.radians([0, 0, 90])),
childFrameOrientation=p.getQuaternionFromEuler(np.radians([0, 0, 0])), childFrameOrientation=p.getQuaternionFromEuler(np.radians([0, 0, 0])),