pick

robot/client_socket.py

@@ -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,