model fix

delta.json

@@ -0,0 +1,6 @@
+
+0.00632926238978882
+
+0.006367644030288622
+
+0.006349076001385698

robot/client_socket.py

@@ -194,12 +194,7 @@ class SocketRobotArm:
         self.q_app = None
         self.imitate_point = None
 
-        robot_start_position = [
+        robot_start_position = [0, -0, 0]
-            -0.008700576263427698,
-            0.04579925857591629,
-            0.0036192361602783203,
-        ]
-        # robot_start_position = [0, 0, 0]
 
         self.prepare_data = PrepareRobotData()
         self.socket_manager = SocketManager()
@@ -285,7 +280,9 @@ class SocketRobotArm:
         #     logger.info(f"delta by WORLD {i} {pos[0] - pos[1]}")
         position_diff = np.linalg.norm(self.world_coordinates[:3] - np.array(calcWorldPosition[:3]))
         print("Position Difference:", position_diff)
-
+        with open('delta.json', 'a') as f:
+            f.write(f'\n{position_diff}\n')
+            
         # logger.info('-------')
 
         # for i, pos in enumerate(zip(axisJointPoses, worldJointPoses)):

urdf/sample.urdf

@@ -33,12 +33,12 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0" />
       <geometry>
-        <mesh filename="../urdf_support/sample/1.stl"/>
+        <mesh filename="../urdf_support/sample/1.stl" />
       </geometry>
-      <material name="grey"/>
+      <material name="grey" />
     </visual>
   </link>
-  
+
   <link name="link_1">
     <!-- <inertial>
       <mass value="1.0" />
@@ -47,12 +47,12 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0" />
       <geometry>
-        <mesh filename="../urdf_support/sample/2.stl"/>
+        <mesh filename="../urdf_support/sample/2.stl" />
       </geometry>
-      <material name="red"/>
+      <material name="red" />
     </visual>
   </link>
-  
+
   <link name="link_2">
     <!-- <inertial>
       <mass value="1.0" />
@@ -61,12 +61,12 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0.0" />
       <geometry>
-        <mesh filename="../urdf_support/sample/3.stl"/>
+        <mesh filename="../urdf_support/sample/3.stl" />
       </geometry>
-      <material name="blue"/>
+      <material name="blue" />
     </visual>
   </link>
-  
+
   <link name="link_3">
     <!-- <inertial>
       <mass value="1.0" />
@@ -75,9 +75,9 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0.0" />
       <geometry>
-        <mesh filename="../urdf_support/sample/4.stl"/>
+        <mesh filename="../urdf_support/sample/4.stl" />
       </geometry>
-      <material name="lightblue"/>
+      <material name="lightblue" />
     </visual>
   </link>
 
@@ -89,9 +89,9 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0.0" />
       <geometry>
-        <mesh filename="../urdf_support/sample/5.stl"/>
+        <mesh filename="../urdf_support/sample/5.stl" />
       </geometry>
-      <material name="yellow"/>
+      <material name="yellow" />
     </visual>
   </link>
 
@@ -103,9 +103,9 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0.0" />
       <geometry>
-        <mesh filename="../urdf_support/sample/6.stl"/>
+        <mesh filename="../urdf_support/sample/6.stl" />
       </geometry>
-      <material name="orange"/>
+      <material name="orange" />
     </visual>
   </link>
 
@@ -117,16 +117,16 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0.0" />
       <geometry>
-        <mesh filename="../urdf_support/sample/7.stl"/>
+        <mesh filename="../urdf_support/sample/7.stl" />
         <!-- <box size="0.1 0.1 0.1"/> -->
       </geometry>
-      <material name="black"/>
+      <material name="black" />
     </visual>
   </link>
 
   <!-- Joints: main serial chain -->
   <joint name="joint_1" type="revolute">
-    <limit lower="-2.705" upper="2.705" effort="0" velocity="0"/>
+    <limit lower="-2.705" upper="2.705" effort="0" velocity="0" />
     <origin rpy="0 0 0" xyz="0 0 0" />
     <parent link="base_link" />
     <child link="link_1" />
@@ -134,43 +134,43 @@
   </joint>
 
   <joint name="joint_2" type="revolute">
-    <limit lower="-2.444" upper="1.135" effort="0" velocity="0"/>
+    <limit lower="-2.444" upper="1.135" effort="0" velocity="0" />
     <parent link="link_1" />
     <child link="link_2" />
     <axis xyz="0 -1 0" />
-    <origin rpy="0 0 0" xyz="0.168621 -0.103 0.4946" />
+    <origin rpy="0 0 0" xyz="0.17045 0 0.4946" />
   </joint>
 
   <joint name="joint_3" type="revolute">
-    <limit lower="-1.309" upper="1.919" effort="0" velocity="0"/>
+    <limit lower="-1.309" upper="1.919" effort="0" velocity="0" />
     <parent link="link_2" />
     <child link="link_3" />
     <axis xyz="0 -1 0" />
-    <origin rpy="0 0 0" xyz="0.006871 0.002321 0.729921" />
+    <origin rpy="0 0 0" xyz="0 -0.001963 0.729921" />
   </joint>
 
   <joint name="joint_4" type="revolute">
-    <limit lower="-3.142" upper="3.142" effort="0" velocity="0"/>
+    <limit lower="-3.142" upper="3.142" effort="0" velocity="0" />
     <parent link="link_3" />
     <child link="link_4" />
     <axis xyz="1 0 0" />
-    <origin rpy="0 0 0" xyz="0.240065 0.101766 0.098277" />
+    <origin rpy="0 0 0" xyz="0 0 0.098277" />
   </joint>
 
   <joint name="joint_5" type="revolute">
-    <limit lower="-2.007" upper="2.007" effort="0" velocity="0"/>
+    <limit lower="-2.007" upper="2.007" effort="0" velocity="0" />
     <parent link="link_4" />
     <child link="link_5" />
     <axis xyz="0 -1 0" />
-    <origin rpy="0 0 0" xyz="1.109039 0.037912 0.000177" />
+    <origin rpy="0 0 0" xyz="1.109039 0 0" />
   </joint>
 
   <joint name="joint_6" type="revolute">
-    <limit lower="-6.283" upper="6.283" effort="0" velocity="0"/>
+    <limit lower="-6.283" upper="6.283" effort="0" velocity="0" />
     <parent link="link_5" />
     <child link="link_6" />
     <axis xyz="1 0 0" />
-    <origin rpy="0 0 0" xyz="0.117 -0.039029 -0.000419" />
+    <origin rpy="0 0 0" xyz="0.117 0 0" />
   </joint>
-  
+
 </robot>