diff --git a/robot/client_socket.py b/robot/client_socket.py index ba31583..b77a8b5 100644 --- a/robot/client_socket.py +++ b/robot/client_socket.py @@ -77,7 +77,7 @@ class SocketRobotArm: def load_palletizing(self): self.urdf_manager.load_palletizing() - + def start_moving(self): self.urdf_manager.conveyor.moving = True @@ -204,19 +204,19 @@ class SocketRobotArm: threading.Thread(target=self.imitate_func, daemon=True).start() def palletizing(self, position, box_id): - pos = position[:] - conv = position[:] - conv[1] += 0.4 + dir_wall = (self.urdf_manager.conv_direction + 1) % 2 - position[1] += 0.02 + conv = position[:] + conv[dir_wall] -= 0.4 + position[dir_wall] -= 0.02 json_res = [] + tool_pos = [0, 0, 0] for pos in [conv, position]: jointPoses = self.convert_to_joint_base( pos, - # np.radians([0, 0, 0]), - np.radians([0, 0, -90]), + np.radians(tool_pos), ) json_res.append( self.prepare_data.make_step( @@ -225,23 +225,10 @@ class SocketRobotArm: ) self.motionFund(jointPoses) time.sleep(2) + with open(f"data/palletizing.json", "w") as myfile: 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( parentBodyUniqueId=self.body_id, @@ -250,7 +237,7 @@ class SocketRobotArm: childLinkIndex=-1, jointType=p.JOINT_FIXED, jointAxis=[0, 0, 0], - parentFramePosition=[0,0,0], + parentFramePosition=[0, 0, 0], childFramePosition=[0, 0.2, 0], parentFrameOrientation=p.getQuaternionFromEuler(np.radians([0, 0, 90])), childFrameOrientation=p.getQuaternionFromEuler(np.radians([0, 0, 0])),