Use fixed urdf to do ik

examples/so100-remote/no_torque.yml

@@ -3,7 +3,7 @@ nodes:
     path: dora-rustypot
     inputs:
       tick: dora/timer/millis/33
-      pose: pytorch-kinematics/action
+      #pose: pytorch-kinematics/action
     outputs:
       - pose
     env:
@@ -20,7 +20,7 @@ nodes:
       - pose
       - action
     env:
-      URDF_PATH: so100.urdf
+      URDF_PATH: so100_2.urdf
       END_EFFECTOR_LINK: "Fixed_Jaw"
       so100_transform: 0 -0.3 0
 
@@ -30,7 +30,7 @@ nodes:
     inputs:
       # series_so100: so100/pose
       series_pose: pytorch-kinematics/pose
-      #jointstate_so100: so100/position
+      jointstate_so100: so100/pose
     env:
-      so100_urdf: so100.urdf
+      so100_urdf: so100_2.urdf
       so100_transform: 0 -0.3 0

examples/so100-remote/parse_keyboard.py

@@ -14,16 +14,11 @@ now = time.time()
 for j in range(0, 100):
     for i in range(110):
         time.sleep(0.033)
-        x = 0.0 + (j * 0.005)
-        y = -0.1 - (i * 0.001)
-        if x == 0:
-            theta = -1.8
-        else:
-            theta = np.arctan(y / x)
+        y = 0.0 + (j * 0.1)
+        x = -0.1 - (i * 0.01)
 
-        print("Sending action: ", x, y, theta)
         node.send_output(
             "action",
-            pa.array([0.0 + (j * 0.005), -0.1 - (i * 0.001), 0.18, -3.1, theta, 0.45]),
+            pa.array([0.0 + (j * 0.05), -0.1 - (i * 0.001), 0.1, -1.6, -0.9, -3.14]),
             metadata={"encoding": "xyzrpy"},
         )

examples/so100-remote/so100.urdf

@@ -95,7 +95,7 @@
     <child
       link="Rotation_Pitch" />
     <axis
-      xyz="0 -1 0" />
+      xyz="0 1 0" />
       <!-- note for the so100 arm there is no well defined effort/velocity limits at the moment -->
     <limit
       lower="-2.1"

examples/so100-remote/so100_2.urdf

@@ -5,7 +5,7 @@
 <robot
   name="SO_5DOF_ARM100_8j_URDF.SLDASM">
   <link
-    name="Base">
+    name="base_link">
     <inertial>
       <origin
         xyz="-2.45960666746703E-07 0.0311418169687909 0.0175746661003382"
@@ -91,11 +91,11 @@
       xyz="0 -0.0452 0.0165"
       rpy="1.5708 0 0" />
     <parent
-      link="Base" />
+      link="base_link" />
     <child
       link="Rotation_Pitch" />
     <axis
-      xyz="0 1 0" />
+      xyz="0 -1 0" />
   </joint>
   <link
     name="Upper_Arm">
@@ -248,7 +248,7 @@
     type="continuous">
     <origin
       xyz="0 0.0052 0.1349"
-      rpy="0 0 0" />
+      rpy="-1.57 0 0" />
     <parent
       link="Lower_Arm" />
     <child
@@ -301,7 +301,7 @@
     type="continuous">
     <origin
       xyz="0 -0.0601 0"
-      rpy="0 0 0" />
+      rpy="0 1.57 0" />
     <parent
       link="Wrist_Pitch_Roll" />
     <child
@@ -354,7 +354,7 @@
     type="continuous">
     <origin
       xyz="-0.0202 -0.0244 0"
-      rpy="3.1416 0 3.1416" />
+      rpy="0 0 0" />
     <parent
       link="Fixed_Jaw" />
     <child

examples/so100-remote/test.yml

@@ -36,8 +36,8 @@ nodes:
     inputs:
       # series_so100: so100/pose
       series_pose: pytorch-kinematics/pose
-      # series_action: pytorch-kinematics/action
-      #jointstate_so100: so100/position
+      series_action: pytorch-kinematics/action
+      jointstate_so100: so100/pose
     env:
       so100_urdf: so100_2.urdf
       so100_transform: 0 -0.3 0