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,6 +280,8 @@ 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('-------')
 

urdf/sample.urdf

@@ -138,7 +138,7 @@
     <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">
@@ -146,7 +146,7 @@
     <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">
@@ -154,7 +154,7 @@
     <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">
@@ -162,7 +162,7 @@
     <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">
@@ -170,7 +170,7 @@
     <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>