model fix

robot/client_socket.py

@@ -135,7 +135,9 @@ class UrdfManager:
     def load_models(self):
         # p.loadURDF("urdf/plane.urdf", physicsClientId=self.physics_client)
 
-        urdf_path = os.path.join(self.application_path, "urdf", f"{self.urdf_filename}.urdf")
+        urdf_path = os.path.join(
+            self.application_path, "urdf", f"{self.urdf_filename}.urdf"
+        )
         self.body_id = p.loadURDF(
             urdf_path,
             self.robot_start_position,
@@ -196,15 +198,17 @@ class SocketRobotArm:
         self.imitate_point = None
 
         robot_start_position = [
-            -0.075458,  # новое значение x в метрах
+            -0.008700576263427698,
-            0.068477,  # новое значение y в метрах
+            0.04579925857591629,
-            0.082201,  # новое значение z в метрах
+            0.0036192361602783203,
         ]
-        robot_start_position = [0,0,0]
+        robot_start_position = [0, 0, 0]
 
         self.prepare_data = PrepareRobotData()
         self.socket_manager = SocketManager()
-        self.urdf_manager = UrdfManager(robot_start_position=robot_start_position, application_path=application_path)
+        self.urdf_manager = UrdfManager(
+            robot_start_position=robot_start_position, application_path=application_path
+        )
 
     def __exit__(self, exc_type, exc_value, traceback):
         logger.info("exiting")
@@ -236,39 +240,50 @@ class SocketRobotArm:
         threading.Thread(target=self.upd_model_func, daemon=True).start()
 
     def upd_model_func(self):
-        logger.info("UPDATE MODEL FUNC")
+        # logger.info("UPDATE MODEL FUNC")
 
         self.get_axis()
         self.set_text(text=f"Координаты осей {self.start_axis_coordinates}")
-        time.sleep(0.5)
         self.set_joint([math.radians(c) for c in self.start_axis_coordinates])
+        time.sleep(1)
 
         self.get_world()
         self.set_text(text=f"Мировые координаты {self.start_world_coordinates}")
-        time.sleep(0.5)
+        angles = self.start_world_coordinates[3:]
         self.set_joint(
             self.convert_to_joint(
-                [s * 0.001 for s in self.start_world_coordinates[0:3]]
+                [s * 0.001 for s in self.start_world_coordinates[0:3]],
+                angles,
             )
         )
+        time.sleep(1)
 
         self._get_command_count()
         self.set_text(text=f"Команд в очереди {self.remote_command_count}")
         time.sleep(0.5)
 
-        num_joints = p.getNumJoints(self.urdf_manager.body_id)
+        # num_joints = p.getNumJoints(self.urdf_manager.body_id)
-        end_effector_state = p.getLinkState(
+        # end_effector_state = p.getLinkState(
-            self.urdf_manager.body_id, num_joints - 1, computeForwardKinematics=True
+        #     self.urdf_manager.body_id, num_joints - 1, computeForwardKinematics=True
-        )
+        # )
 
-        end_effector_position = end_effector_state[4]
+        # end_effector_position = end_effector_state[4]
-        end_effector_orientation = end_effector_state[5]
+        # end_effector_orientation = end_effector_state[5]
-        euler_orientation = p.getEulerFromQuaternion(end_effector_orientation)
+        # euler_orientation = list(p.getEulerFromQuaternion(end_effector_orientation))
+        # euler_orientation[1] += math.radians(90)
         
-        logger.info(
+        # logger.info(
-            f"Вычисленное положение {[a * 1000 for a in end_effector_position]}"
+        #     f"Вычисленное положение {[a * 1000 for a in end_effector_position]}"
-        )
+        # )
-        logger.info(f"Вычисленная ориентация {np.degrees(euler_orientation)}")
+        # logger.info(
+        #     f"""{[f'{a*1000} -- {self.start_world_coordinates[i]} -- {self.start_world_coordinates[i] - a*1000}' 
+        #     for i,a in enumerate(end_effector_position)]}"""
+        # )
+        # logger.info(f"Вычисленная ориентация {np.degrees(euler_orientation)}")
+        # logger.info(
+        #     f"""{[f'{a} -- {self.start_world_coordinates[i + 3]} -- {self.start_world_coordinates[i + 3] - a}' 
+        #     for i,a in enumerate(np.degrees(euler_orientation))]}"""
+        # )
 
     def cycle_base(self):
         self.upd_model()
@@ -313,26 +328,28 @@ class SocketRobotArm:
 
     def convert_to_joint(self, coordinates, angles=[]):
         num_joints = p.getNumJoints(self.urdf_manager.body_id)
+        if len(angles) == 3:
+            angles[1] = float(angles[1]) - 90
+            target_orientation = p.getQuaternionFromEuler(
+                [math.radians(float(a)) for a in angles]
+            )
+        else:
+            target_orientation = None
 
         joint_angles = p.calculateInverseKinematics(
             self.urdf_manager.body_id,
             endEffectorLinkIndex=num_joints - 1,
             targetPosition=coordinates,
-            targetOrientation=(
+            targetOrientation=target_orientation,
-                p.getQuaternionFromEuler([math.radians(float(a)) for a in angles])
-                if len(angles) == 3
-                else None
-            ),
         )
-        logger.info(f"convrt to joint {joint_angles}")
+        # logger.info(f"convrt to joint {joint_angles}")
-        # logger.info(joint_angles)
         return joint_angles
 
     def set_text(self, text):
         logger.info(text)
 
     def set_joint(self, coordinates):
-        logger.info("set joints")
+        # logger.info("set joints")
         num_joints = p.getNumJoints(self.urdf_manager.body_id)
         if coordinates is None:
             return
@@ -345,7 +362,7 @@ class SocketRobotArm:
                 controlMode=p.POSITION_CONTROL,
                 targetPosition=coordinates[joint_index],
             )
-            time.sleep(0.1)
+            time.sleep(0.01)
 
     def send_data(self, data):
         return self.socket_manager.send_data(data)

urdf/coord.py

@@ -0,0 +1,11 @@
+# Мировые координаты
+world_coordinates = [
+    1131.959351, -588.689941, 1277.805054,
+    6.181231, 54.227802, -100.604988
+]
+
+# Поворот осей
+axis_rotation = [
+    -41.612457, 31.759747, -26.773878,
+    74.869049, -45.417992, -20.86335
+]

urdf/sample.urdf

@@ -45,7 +45,7 @@
       <inertia ixx="0.005" ixy="0.0" ixz="0.0" iyy="0.005" iyz="0.0" izz="0.005" />
     </inertial>
     <visual>
-      <origin rpy="0 0 0" xyz="0 0 0.0" />
+      <origin rpy="0 0 0" xyz="0 0 0" />
       <geometry>
         <mesh filename="../urdf_support/sample/2.stl"/>
       </geometry>
@@ -126,7 +126,7 @@
   <!-- Joints: main serial chain -->
   <joint name="joint_1" type="revolute">
     <limit lower="-2.705" upper="2.705" effort="0" velocity="0"/>
-    <origin rpy="0 0 0" xyz="0.011789 -0.0278 0.26158" />
+    <origin rpy="0 0 0" xyz="0.014359931 0.001723413 0.261326995" />
     <parent link="base_link" />
     <child link="link_1" />
     <axis xyz="0 0 1" />

utils/pybullet_server.py

@@ -24,7 +24,7 @@ def main(type="SHARED_MEMORY_SERVER"):
         except Exception as e:
             print(f"Ошибка в pybullet_server{e}")
             time.sleep(1)
-        time.sleep(30 * 0.001)
+        time.sleep(1/240)
 
 
 if __name__ == "__main__":

utils/test_socket_server.py

@@ -4,6 +4,37 @@ import json
 HOST = "127.0.0.1"  # Standard loopback interface address (localhost)
 PORT = 9760  # Port to listen on (non-privileged ports are > 1023)
 
+coordinates = [
+    {
+        "world": [
+            1131.959351,
+            -588.689941,
+            1277.805054,
+            6.181231,
+            54.227802,
+            -100.604988,
+        ],
+        "axis": [-41.612457, 31.759747, -26.773878, 74.869049, -45.417992, -20.86335],
+    },
+    {
+        "world": [1282.244, -75.427, 1772.476, 84.629, 34.519, 153.999],
+        "axis": [-8.487, -8.681, 33.058, 88.070, -75.010, -10.566],
+    },
+    {
+        "world": [1630.006, 628.326, 1322.554, 0, 90, 0],
+        "axis": [22.493, -30.696, 35.997, -77.419, -23.007, 76.362],
+    },
+    {
+        "world": [1052.757, -1028.846, 334.417, 162.374, 20.467, 75.065],
+        "axis": [-43.742, -30.698, -12.227, -21.884, -23.078, 76.362],
+    },
+    {
+        "world": [1396.525, 1.687, 1322.697, 0, 90, 0],
+        "axis": [0, -0.002, -0.002, 0.003, -0.001, 0],
+    },
+]
+coordinates_index = 2
+
 
 def handle_client(conn, addr):
     print(f"Connected by {addr}")
@@ -16,24 +47,10 @@ def handle_client(conn, addr):
             res = {"queryData": ["ok"]}
 
             if "queryAddr" in req.keys() and "axis-0" in req["queryAddr"]:
-                res["queryData"] = [
+                res["queryData"] = coordinates[coordinates_index]["axis"]
-                    -41.612457,
-                    31.759747,
-                    -26.773878,
-                    74.869049,
-                    -45.417992,
-                    -20.86335,
-                ]
 
             if "queryAddr" in req and "world-0" in req["queryAddr"]:
-                res["queryData"] = [
+                res["queryData"] = coordinates[coordinates_index]["world"]
-                    1131.959351,
-                    -588.689941,
-                    1277.805054,
-                    6.181231,
-                    54.227802,
-                    -100.604988,
-                ]
 
             if req["reqType"] == "command":
                 res["cmdReply"] = ["ok"]