test tool

robot/client_socket.py

@@ -105,9 +105,9 @@ class SocketRobotArm:
 
         num_joints = p.getNumJoints(self.urdf_manager.body_id)
         axisJointPoses = self.axis_coordinates
-        for i in range(num_joints):
+        for i in range(num_joints-1):
-            p.resetJointState(
+            p.setJointMotorControl2(
-                self.urdf_manager.body_id, i, math.radians(axisJointPoses[i])
+                self.urdf_manager.body_id, i, p.POSITION_CONTROL,math.radians(axisJointPoses[i])
             )
         
         time.sleep(1)
@@ -123,7 +123,8 @@ class SocketRobotArm:
             [float(p) * 0.001 for p in coordinates], [math.radians(p) for p in angles]
         )
         for i in range(num_joints):
-            p.resetJointState(self.urdf_manager.body_id, i, worldJointPoses[i])
+            p.setJointMotorControl2(self.urdf_manager.body_id, i,p.POSITION_CONTROL, worldJointPoses[i])
+        time.sleep(2)
 
         calcWorldPosition = p.getLinkState(
             self.urdf_manager.body_id, num_joints - 1, computeForwardKinematics=True
@@ -132,6 +133,10 @@ class SocketRobotArm:
             [p * 1000 for p in calcWorldPosition[0]]
             + [np.degrees(p) for p in p.getEulerFromQuaternion(calcWorldPosition[1])]
         )
+        time.sleep(2)
+
+        position_diff = np.linalg.norm(self.world_coordinates[:3] - np.array(calcWorldPosition[:3]))
+        print("Position Difference:", position_diff)
 
         self.set_text(text=f"Команд в очереди {self.command_count}")
         time.sleep(0.5)
@@ -252,9 +257,10 @@ class SocketRobotArm:
         for point in points:
             jointPoses = [np.radians(float(point[f"m{i}"])) for i in range(6)]
             num_joints = p.getNumJoints(self.urdf_manager.body_id)
-
+            if True:
                 for i in range(num_joints):
-                p.resetJointState(self.urdf_manager.body_id, i, jointPoses[i])
+                    p.setJointMotorControl2(self.urdf_manager.body_id, i, p.POSITION_CONTROL, jointPoses[i])
+            # p.setJointMotorControlArray(self.urdf_manager.body_id, p.POSITION_CONTROL, jointPoses)
             time.sleep(1)
 
     def convert_point_to_free(self):
@@ -276,7 +282,11 @@ class SocketRobotArm:
                 jointPoses = self.convert_to_joint_base(
                     [float(p) * 0.001 for p in points], angles
                 )
-                res.append(self.prepare_data.make_step('free', np.degrees(jointPoses), [0,0,0,0,0,0]))
+                res.append(
+                    self.prepare_data.make_step(
+                        "free", np.degrees(jointPoses), [0, 0, 0, 0, 0, 0]
+                    )
+                )
         return res
 
     def convert_to_joint_base(self, coordinates, angles):

robot/urdf_manager.py

@@ -14,8 +14,10 @@ import pybullet_industrial as pi
 
 from logger import logger
 
+
 class UrdfManager:
     urdf_filename = None
+    urdf_filename_tool = "welding_gun"
     physics_client = None
     body_id = None
 
@@ -39,7 +41,27 @@ class UrdfManager:
         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)
+        self.body_id = p.loadURDF(
+            urdf_path, self.robot_start_position, useFixedBase=True
+        )
+
+        urdf_path_tool = os.path.join(
+            self.application_path, "urdf", f"{self.urdf_filename_tool}.urdf"
+        )
+        num_joints = p.getNumJoints(self.body_id)
+        end_state = p.getLinkState(self.body_id, num_joints - 1)
+        self.tool_id = p.loadURDF(urdf_path_tool)
+
+        p.createConstraint(
+            self.body_id,
+            num_joints - 1,
+            self.tool_id,
+            -1,
+            p.JOINT_FIXED,
+            [0, 0, 0],
+            [0, 0, 0],
+            [0, 0, 0],
+        )
 
         time.sleep(1)
 

urdf/coord.py

@@ -1,11 +0,0 @@
-# Мировые координаты
-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

@@ -26,10 +26,6 @@
 
   <!-- Links: main serial chain -->
   <link name="base_link">
-    <!-- <inertial>
-      <mass value="1.0" />
-      <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" />
       <geometry>
@@ -40,10 +36,6 @@
   </link>
 
   <link name="link_1">
-    <!-- <inertial>
-      <mass value="1.0" />
-      <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" />
       <geometry>
@@ -54,10 +46,6 @@
   </link>
 
   <link name="link_2">
-    <!-- <inertial>
-      <mass value="1.0" />
-      <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" />
       <geometry>
@@ -68,10 +56,6 @@
   </link>
 
   <link name="link_3">
-    <!-- <inertial>
-      <mass value="1.0" />
-      <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" />
       <geometry>
@@ -82,12 +66,8 @@
   </link>
 
   <link name="link_4">
-    <!-- <inertial>
-      <mass value="1.0" />
-      <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.5 0 0.0" />
+      <origin rpy="0 0 0" xyz="0 0 0.0" />
       <geometry>
         <mesh filename="../urdf_support/sample/5.stl" />
       </geometry>
@@ -96,10 +76,6 @@
   </link>
 
   <link name="link_5">
-    <!-- <inertial>
-      <mass value="1.0" />
-      <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" />
       <geometry>
@@ -110,15 +86,10 @@
   </link>
 
   <link name="link_6">
-    <!-- <inertial>
-      <mass value="1.0" />
-      <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" />
       <geometry>
-        <mesh filename="../urdf_support/sample/7.stl" />
+        <mesh filename="../urdf_support/sample/777.stl" />
-        <!-- <box size="0.1 0.1 0.1"/> -->
       </geometry>
       <material name="black" />
     </visual>
@@ -172,5 +143,4 @@
     <axis xyz="1 0 0" />
     <origin rpy="0 0 0" xyz="0.117 0 0" />
   </joint>
-
 </robot>

urdf/welding_gun.urdf

@@ -0,0 +1,32 @@
+<?xml version="1.0"?>
+<robot name="welding_gun_robot">
+
+    <!-- Основной корпус робота (base_link) -->
+    <link name="base_link">
+        <visual>
+            <geometry>
+                <mesh filename="../urdf_support/sample/777.stl" /> 
+            </geometry>
+            <material name="gray"/>
+        </visual>
+    </link>
+
+    <!-- Основной сварочный пистолет (welding_gun_body) -->
+    <link name="welding_gun_body">
+        <visual>
+            <geometry>
+                <mesh filename="../urdf_support/sample/7.stl" />  <!-- Замените на путь к вашему STL-файлу -->
+            </geometry>
+            <material name="metal"/>
+            <origin xyz="0 0 0" rpy="0 0 0"/>
+        </visual>
+    </link>
+
+    <!-- Соединение сварочного пистолета с роботом (фиксированное соединение) -->
+    <joint name="welding_gun_fixed_joint" type="fixed">
+        <origin xyz="-0.02 0 0" rpy="0 0 0"/>
+        <parent link="base_link"/>  <!-- Родительский элемент (например, рука робота или корпус) -->
+        <child link="welding_gun_body"/>  <!-- Сварочный пистолет -->
+    </joint>
+
+</robot>

utils/test_socket_server.py

@@ -85,6 +85,21 @@ c_angles = [
         "axis": [-17.409513, 10.897068, -33.003525, 176.892288, 38.957874, -159.934479],
     },
 ]
+tool_angles = [
+    {
+        "world": [1292.602, -119.999, 1023.335, 20.742, 0.367, -91.615],
+        "axis": [-9.053, 37.962, -47.169, -44.937, 12.879, 44.209],
+    },
+    {
+        "world": [1084.151, 643.621, 644.947, 20.743, 0.367, -91.616],
+        "axis": [45.855, 19.494, -55.437, 60.337, 55.666, -44.703],
+    },
+    {
+        "world": [768.497, -577.398, 1688.266, 20.738, 0.382, -91.610],
+        "axis": [-63.241, 38.711, -11.748, -102.888, 66.357, 119.693],
+    },
+]
+c_angles = tool_angles
 coordinates_index = 0
 
 
@@ -116,7 +131,9 @@ def handle_client(conn, addr):
             if "queryAddr" in req and "world-0" in req["queryAddr"]:
                 res["queryData"] = c["world"]
 
-            if "reqType" in req and (req["reqType"] == "command" or req["reqType"] == "AddRCC"):
+            if "reqType" in req and (
+                req["reqType"] == "command" or req["reqType"] == "AddRCC"
+            ):
                 res["cmdReply"] = ["it is local dear"]
             # print(res)
             conn.sendall(json.dumps(res).encode())