From e8395df1bd1a37da4cff0c72b1ca0289366183cf Mon Sep 17 00:00:00 2001
From: haixuanTao <tao.xavier@outlook.com>
Date: Fri, 2 May 2025 12:03:24 +0200
Subject: [PATCH] Minor improvement

---
 examples/so100-remote/no_torque.yml           |  1 -
 examples/so100-remote/parse_pose.py           | 28 ++++++++++++-------
 examples/so100-remote/qwenvl.yml              |  2 +-
 .../dora_pytorch_kinematics/main.py           |  2 +-
 node-hub/dora-rustypot/src/main.rs            | 13 +++++----
 5 files changed, 28 insertions(+), 18 deletions(-)

diff --git a/examples/so100-remote/no_torque.yml b/examples/so100-remote/no_torque.yml
index 2e622176..608ab774 100644
--- a/examples/so100-remote/no_torque.yml
+++ b/examples/so100-remote/no_torque.yml
@@ -9,7 +9,6 @@ nodes:
     env:
       PORT: /dev/ttyACM0
       IDS: 1 2 3 4 5 6
-      TORQUE: false
 
   - id: camera
     build: pip install -e ../../node-hub/dora-pyrealsense
diff --git a/examples/so100-remote/parse_pose.py b/examples/so100-remote/parse_pose.py
index 5e3d702f..24bad2f9 100644
--- a/examples/so100-remote/parse_pose.py
+++ b/examples/so100-remote/parse_pose.py
@@ -6,7 +6,7 @@ import pyarrow as pa
 from dora import Node
 
 node = Node()
-top_z = -0.48
+top_z = -0.5
 low_z = -0.57
 
 roll = 1.86
@@ -30,7 +30,7 @@ def grab(target_x, target_y, low_z, top_z, roll, pitch, yaw_open, yaw_close, las
         pa.array([target_x, target_y, low_z, roll, pitch, yaw_open]),
         metadata={"encoding": "xyzrpy"},
     )
-    time.sleep(0.5)
+    time.sleep(0.2)
 
 
     node.send_output(
@@ -39,7 +39,7 @@ def grab(target_x, target_y, low_z, top_z, roll, pitch, yaw_open, yaw_close, las
         metadata={"encoding": "xyzrpy"},
     )
 
-    time.sleep(0.5)
+    time.sleep(0.4)
 
     node.send_output(
         "action",
@@ -47,6 +47,8 @@ def grab(target_x, target_y, low_z, top_z, roll, pitch, yaw_open, yaw_close, las
         metadata={"encoding": "xyzrpy"},
     )
 
+    time.sleep(0.5)
+
     node.send_output(
         "action",
         pa.array([0.05, 0.04, top_z, roll, pitch, yaw_close]),
@@ -71,17 +73,25 @@ def place(place_x, place_y, place_z, top_z, roll, pitch, yaw_open, yaw_close, la
         metadata={"encoding": "xyzrpy"},
     )
 
-    time.sleep(0.5)
+    time.sleep(0.2)
 
 
     node.send_output(
         "action",
-        pa.array([place_x, place_y, top_z, roll, pitch, yaw_open]),
+        pa.array([place_x, place_y, place_z, roll, pitch, yaw_open]),
         metadata={"encoding": "xyzrpy"},
     )    
     
-    time.sleep(0.7)
+    time.sleep(0.3)
+
 
+    node.send_output(
+        "action",
+        pa.array([place_x, place_y, top_z, roll, pitch, yaw_open]),
+        metadata={"encoding": "xyzrpy"},
+    )    
+    
+    time.sleep(0.3)
 
     node.send_output(
         "action",
@@ -89,6 +99,7 @@ def place(place_x, place_y, place_z, top_z, roll, pitch, yaw_open, yaw_close, la
         metadata={"encoding": "xyzrpy"},
     )
         
+time.sleep(0.6)
 
 
 node.send_output(
@@ -116,9 +127,7 @@ for event in node:
             
 
             # Adjust z with the size of the gripper
-            z = z + 0.073
-            # y = y - 0.01
-            x = x - 0.01
+            z = z + 0.063
             match action:
                 case "grab":
                     grab(
@@ -134,7 +143,6 @@ for event in node:
                         last_y
                     )
                 case "release":
-                    y = y - 0.02
                     place(
                         x,
                         y,
diff --git a/examples/so100-remote/qwenvl.yml b/examples/so100-remote/qwenvl.yml
index efa0cd97..c29d0c5a 100644
--- a/examples/so100-remote/qwenvl.yml
+++ b/examples/so100-remote/qwenvl.yml
@@ -10,7 +10,7 @@ nodes:
       - pose
     env:
       PORT: /dev/ttyACM0
-      TORQUE: true
+      TORQUE: 2000
       IDS: 1 2 3 4 5 6
 
   - id: camera
diff --git a/node-hub/dora-pytorch-kinematics/dora_pytorch_kinematics/main.py b/node-hub/dora-pytorch-kinematics/dora_pytorch_kinematics/main.py
index b6f20e2a..ca362cfd 100644
--- a/node-hub/dora-pytorch-kinematics/dora_pytorch_kinematics/main.py
+++ b/node-hub/dora-pytorch-kinematics/dora_pytorch_kinematics/main.py
@@ -204,7 +204,7 @@ class RobotKinematics:
         # Instantiate and run the IK solver (core pytorch_kinematics objects/methods)
         ik_solver = pk.PseudoInverseIK(
             self.chain,
-            max_iterations=5_000,
+            max_iterations=1_000,
             retry_configs=q_init,
             joint_limits=torch.tensor(self.chain.get_joint_limits()),
             early_stopping_any_converged=True,
diff --git a/node-hub/dora-rustypot/src/main.rs b/node-hub/dora-rustypot/src/main.rs
index dfdfc2e6..650e263c 100644
--- a/node-hub/dora-rustypot/src/main.rs
+++ b/node-hub/dora-rustypot/src/main.rs
@@ -17,10 +17,6 @@ fn main() -> Result<()> {
         .split(&[',', ' '][..])
         .map(|s| s.parse::<u8>().unwrap())
         .collect::<Vec<u8>>();
-    let torque = std::env::var("TORQUE")
-        .unwrap_or_else(|_| "false".to_string())
-        .parse::<bool>()
-        .context("Invalid torque")?;
 
     let serial_port = serialport::new(serialportname, baudrate)
         .timeout(Duration::from_millis(1000))
@@ -30,10 +26,17 @@ fn main() -> Result<()> {
         .with_protocol_v1()
         .with_serial_port(serial_port);
 
-    if torque {
+    if let Ok(torque) = std::env::var("TORQUE") {
         let truthies = vec![true; ids.len()];
+
         c.write_torque_enable(&ids, &truthies)
             .expect("could not enable torque");
+
+        if let Ok(torque_limit) = torque.parse::<u16>() {
+            let limits = vec![torque_limit; ids.len()];
+            c.write_torque_limit(&ids, &limits)
+                .expect("could not enable torque");
+        }
     } else {
         let falsies = vec![false; ids.len()];
         c.write_torque_enable(&ids, &falsies)