exp

robot/client_socket.py

@@ -201,6 +201,7 @@ 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
 
@@ -211,6 +212,7 @@ class SocketRobotArm:
         for pos in [conv, position]:
             jointPoses = self.convert_to_joint_base(
                 pos,
+                # np.radians([0, 0, 0]),
                 np.radians([0, 0, -90]),
             )
             json_res.append(
@@ -223,29 +225,31 @@ class SocketRobotArm:
         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)
+        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)
 
-        # Определяем смещение, которое будет применено для фиксации позиции (без вращения)
-        parentFramePosition = [position[i] - end_effector_pos[i] for i in range(3)]
-        parentFrameOrientation = end_effector_orn  # Ориентация робота (наконечника)
+        # Используем ориентацию объекта (коробки) как ориентацию родителя
+        parentFrameOrientation = object_orn
+
+        # # Определяем смещение для фиксации позиции (без вращения)
+        parentFramePosition = [pos[i] - end_effector_pos[i] for i in range(3)]
 
         # Создаём констрейнт для прикрепления
         constraint_id = p.createConstraint(
-            parentBodyUniqueId=self.body_id,  # Идентификатор робота
-            parentLinkIndex=self.num_joints
-            - 1,  # Индекс последнего звена (наконечника)
-            childBodyUniqueId=box_id,  # Идентификатор коробки
-            childLinkIndex=-1,  # -1 означает базовую ссылку объекта
-            jointType=p.JOINT_FIXED,  # Фиксированный констрейнт
-            jointAxis=[0, 0, 0],  # Без вращательной оси
-            parentFramePosition=parentFramePosition,  # Позиция относительно робота
-            childFramePosition=[1, 1, 1],  # Позиция объекта (относительно себя)
-            parentFrameOrientation=parentFrameOrientation,  # Ориентация относительно робота
+            parentBodyUniqueId=self.body_id,
+            parentLinkIndex=self.num_joints - 1,
+            childBodyUniqueId=box_id,
+            childLinkIndex=-1,
+            jointType=p.JOINT_FIXED,
+            jointAxis=[0, 0, 0],
+            parentFramePosition=[0,0,0],
+            childFramePosition=[0, 0, 0],
+            parentFrameOrientation=p.getQuaternionFromEuler(np.radians([0, 0, 0])),
         )
 
     def set_text(self, text):
@@ -326,7 +330,7 @@ class SocketRobotArm:
         for point in points:
             jointPoses = [np.radians(float(point[f"m{i}"])) for i in range(6)]
             self.motionFund(jointPoses)
-            
+
     def get_palletizing_json(self):
         with open(f"data/palletizing.json", "r") as fp:
             data = json.load(fp)

urdf/sample_without.urdf

@@ -186,10 +186,11 @@
     <origin rpy="0 0 0" xyz="0.117 0 0" />
   </joint>
 
-    <link name="tcp">
+  <link name="tcp">
     <visual>
+      <origin rpy="0 0 0" xyz="0.04 0 0" />
       <geometry>
-        <box size="0.05 0.05 0.05" />
+        <box size="0.02 0.02 0.02" />
       </geometry>
       <material name="red" />
     </visual>