Add checks on IK to avoid breakage

examples/so100-remote/parse_keyboard.py

@@ -2,23 +2,95 @@
 
 import time
 
-import numpy as np
 import pyarrow as pa
 from dora import Node
 
 node = Node()
 
+target_x = -0.08
+target_y = -0.20
+
+place_x = -0.08
+place_y = 0.20
 
 now = time.time()
+time.sleep(1.5)
+
+node.send_output(
+    "action",
+    pa.array([target_y, target_x, 0.15, -1.6, -0.0, -1]),
+    metadata={"encoding": "xyzrpy"},
+)
+
+time.sleep(0.8)
+
+node.send_output(
+    "action",
+    pa.array([target_y, target_x, 0.15, -1.6, -0.0, -1]),
+    metadata={"encoding": "xyzrpy"},
+)
+
+time.sleep(0.5)
+
+node.send_output(
+    "action",
+    pa.array([target_y, target_x, 0.09, -1.6, -0.0, -1]),
+    metadata={"encoding": "xyzrpy"},
+)
+time.sleep(0.2)
+
+
+node.send_output(
+    "action",
+    pa.array([target_y, target_x, 0.09, -1.6, -0.0, -3]),
+    metadata={"encoding": "xyzrpy"},
+)
+
+
+time.sleep(1.0)
+
+node.send_output(
+    "action",
+    pa.array([target_y, target_x, 0.15, -1.6, -0.0, -3]),
+    metadata={"encoding": "xyzrpy"},
+)
+
+time.sleep(0.3)
+
+
+node.send_output(
+    "action",
+    pa.array([place_y, place_x, 0.15, -1.6, -0.0, -3]),
+    metadata={"encoding": "xyzrpy"},
+)
+
+time.sleep(1.0)
+
+node.send_output(
+    "action",
+    pa.array([place_y, place_x, 0.10, -1.6, -0.0, -3]),
+    metadata={"encoding": "xyzrpy"},
+)
+
+time.sleep(0.2)
+
+node.send_output(
+    "action",
+    pa.array([place_y, place_x, 0.10, -1.6, -0.0, -1]),
+    metadata={"encoding": "xyzrpy"},
+)
+time.sleep(1.0)
+
+node.send_output(
+    "action",
+    pa.array([place_y, place_x, 0.15, -1.6, -0.0, -3]),
+    metadata={"encoding": "xyzrpy"},
+)
+
+time.sleep(1.0)
 
-for j in range(0, 100):
-    for i in range(110):
-        time.sleep(0.033)
-        y = 0.0 + (j * 0.1)
-        x = -0.1 - (i * 0.01)
-
-        node.send_output(
-            "action",
-            pa.array([0.0 + (j * 0.05), -0.1 - (i * 0.001), 0.1, -1.6, -0.9, -3.14]),
-            metadata={"encoding": "xyzrpy"},
-        )
+node.send_output(
+    "action",
+    pa.array([place_y, place_x, 0.15, -1.6, -0.0, -3]),
+    metadata={"encoding": "xyzrpy"},
+)

examples/so100-remote/test.yml

@@ -10,6 +10,7 @@ nodes:
       PORT: /dev/ttyACM0
       CONFIG: follower.right.json
       TORQUE: true
+      IDS: 1 2 3 4 5 6
 
   - id: parse_keyboard
     path: parse_keyboard.py
@@ -34,7 +35,7 @@ nodes:
     build: pip install -e ../../node-hub/dora-rerun
     path: dora-rerun
     inputs:
-      # series_so100: so100/pose
+      series_so100: so100/pose
       series_pose: pytorch-kinematics/pose
       series_action: pytorch-kinematics/action
       jointstate_so100: so100/pose

node-hub/dora-pytorch-kinematics/dora_pytorch_kinematics/main.py

@@ -204,6 +204,11 @@ class RobotKinematics:
             pos_tolerance=1e-3,
         )
         solution_angles = ik_solver.solve(target_pose)
+        if solution_angles.err_rot > 1e-4 or solution_angles.err_pos > 1e-3:
+            print(
+                f"IK did not converge: pos_err={solution_angles.err_pos}, rot_err={solution_angles.err_rot}"
+            )
+            return None
         return solution_angles.solutions.detach()
 
 
@@ -246,9 +251,22 @@ def main():
                             ),
                         )
                         solution = robot.compute_ik(target, last_known_state)
+                        if solution is None:
+                            continue
+
+                        solution = solution.numpy().ravel()
+                        delta_angles = solution - last_known_state
 
+                        valid = np.all(
+                            (delta_angles >= -np.pi) & (delta_angles <= np.pi)
+                        )
+                        if not valid:
+                            print(
+                                "IK solution is not valid, as the rotation are too wide. skipping."
+                            )
+                            continue
                         metadata["encoding"] = "jointstate"
-                        last_known_state = solution.numpy().ravel()
+                        last_known_state = solution
                         solution = pa.array(last_known_state)
                         node.send_output(event["id"], solution, metadata=metadata)
                 case "jointstate":