From f6980ebee72629d336214b27136b5beb32f684d3 Mon Sep 17 00:00:00 2001 From: haixuanTao Date: Wed, 30 Apr 2025 20:54:36 +0200 Subject: [PATCH] Fix CI/CD And improve example --- .github/workflows/node-hub-ci-cd.yml | 2 +- Cargo.lock | 21 - examples/so100-remote/no_torque.yml | 5 +- examples/so100-remote/parse_keyboard.py | 36 +- examples/so100-remote/so100.urdf | 43 +-- examples/so100-remote/so100_2.urdf | 365 ------------------ examples/so100-remote/test.yml | 29 +- .../dora_pytorch_kinematics/main.py | 22 +- node-hub/dora-rerun/src/urdf.rs | 9 +- node-hub/dora-rustypot/Cargo.toml | 2 +- 10 files changed, 82 insertions(+), 452 deletions(-) delete mode 100644 examples/so100-remote/so100_2.urdf diff --git a/.github/workflows/node-hub-ci-cd.yml b/.github/workflows/node-hub-ci-cd.yml index eee7ed1e..a6d34027 100644 --- a/.github/workflows/node-hub-ci-cd.yml +++ b/.github/workflows/node-hub-ci-cd.yml @@ -54,7 +54,7 @@ jobs: run: | sudo apt update sudo apt-get install portaudio19-dev - sudo apt-get install libdav1d-dev nasm + sudo apt-get install libdav1d-dev nasm libudev-dev mkdir -p $HOME/.rustup/toolchains/stable-x86_64-unknown-linux-gnu/lib/rustlib/x86_64-unknown-linux-gnu/lib ln -s /lib/x86_64-linux-gnu/libdav1d.so $HOME/.rustup/toolchains/stable-x86_64-unknown-linux-gnu/lib/rustlib/x86_64-unknown-linux-gnu/lib/libdav1d.so diff --git a/Cargo.lock b/Cargo.lock index b405c65d..7f1eb0c7 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -6505,26 +6505,6 @@ dependencies = [ "redox_syscall 0.5.10", ] -[[package]] -name = "libudev" -version = "0.3.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "78b324152da65df7bb95acfcaab55e3097ceaab02fb19b228a9eb74d55f135e0" -dependencies = [ - "libc", - "libudev-sys", -] - -[[package]] -name = "libudev-sys" -version = "0.1.4" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3c8469b4a23b962c1396b9b451dda50ef5b283e8dd309d69033475fa9b334324" -dependencies = [ - "libc", - "pkg-config", -] - [[package]] name = "libz-sys" version = "1.1.22" @@ -12456,7 +12436,6 @@ dependencies = [ "core-foundation 0.10.0", "core-foundation-sys", "io-kit-sys", - "libudev", "mach2", "nix 0.26.4", "scopeguard", diff --git a/examples/so100-remote/no_torque.yml b/examples/so100-remote/no_torque.yml index 05aaf191..2e622176 100644 --- a/examples/so100-remote/no_torque.yml +++ b/examples/so100-remote/no_torque.yml @@ -7,7 +7,7 @@ nodes: outputs: - pose env: - PORT: /dev/ttyACM1 + PORT: /dev/ttyACM0 IDS: 1 2 3 4 5 6 TORQUE: false @@ -31,13 +31,12 @@ nodes: env: URDF_PATH: so100.urdf END_EFFECTOR_LINK: "Moving Jaw" - so100_transform: -0.18 0.02 -0.65 0.7 0 0 0.7 + TRANSFORM: -0.18 0.02 -0.65 0.7 0 0 0.7 - id: plot build: pip install -e ../../node-hub/dora-rerun path: dora-rerun inputs: - series_so100: so100/pose series_fk: pytorch-kinematics/pose jointstate_so100: so100/pose camera/image: camera/image diff --git a/examples/so100-remote/parse_keyboard.py b/examples/so100-remote/parse_keyboard.py index 9670ad7e..ed085c42 100644 --- a/examples/so100-remote/parse_keyboard.py +++ b/examples/so100-remote/parse_keyboard.py @@ -7,18 +7,26 @@ from dora import Node node = Node() -target_x = -0.08 -target_y = -0.20 +target_y = -0.02 +target_x = 0.00 -place_x = -0.08 -place_y = 0.20 +place_x = -0.02 +place_y = -0.1 + +top_z = -0.50 +low_z = -0.57 + +roll = 1.86 +pitch = 1.43 +yaw_closed = 0.8 +yaw_opened = -0.5 now = time.time() time.sleep(1.5) node.send_output( "action", - pa.array([target_y, target_x, 0.15, -1.6, -0.0, -1]), + pa.array([target_x, target_y, top_z, roll, pitch, yaw_closed]), metadata={"encoding": "xyzrpy"}, ) @@ -26,7 +34,7 @@ time.sleep(0.8) node.send_output( "action", - pa.array([target_y, target_x, 0.15, -1.6, -0.0, -1]), + pa.array([target_x, target_y, top_z, roll, pitch, yaw_closed]), metadata={"encoding": "xyzrpy"}, ) @@ -34,7 +42,7 @@ time.sleep(0.5) node.send_output( "action", - pa.array([target_y, target_x, 0.09, -1.6, -0.0, -1]), + pa.array([target_x, target_y, low_z, roll, pitch, yaw_closed]), metadata={"encoding": "xyzrpy"}, ) time.sleep(0.2) @@ -42,7 +50,7 @@ time.sleep(0.2) node.send_output( "action", - pa.array([target_y, target_x, 0.09, -1.6, -0.0, -3]), + pa.array([target_x, target_y, low_z, roll, pitch, yaw_opened]), metadata={"encoding": "xyzrpy"}, ) @@ -51,7 +59,7 @@ time.sleep(1.0) node.send_output( "action", - pa.array([target_y, target_x, 0.15, -1.6, -0.0, -3]), + pa.array([target_x, target_y, top_z, roll, pitch, yaw_opened]), metadata={"encoding": "xyzrpy"}, ) @@ -60,7 +68,7 @@ time.sleep(0.3) node.send_output( "action", - pa.array([place_y, place_x, 0.15, -1.6, -0.0, -3]), + pa.array([place_x, place_y, top_z, roll, pitch, yaw_opened]), metadata={"encoding": "xyzrpy"}, ) @@ -68,7 +76,7 @@ time.sleep(1.0) node.send_output( "action", - pa.array([place_y, place_x, 0.10, -1.6, -0.0, -3]), + pa.array([place_x, place_y, low_z, roll, pitch, yaw_opened]), metadata={"encoding": "xyzrpy"}, ) @@ -76,14 +84,14 @@ time.sleep(0.2) node.send_output( "action", - pa.array([place_y, place_x, 0.10, -1.6, -0.0, -1]), + pa.array([place_x, place_y, low_z, roll, pitch, yaw_closed]), metadata={"encoding": "xyzrpy"}, ) time.sleep(1.0) node.send_output( "action", - pa.array([place_y, place_x, 0.15, -1.6, -0.0, -3]), + pa.array([place_x, place_y, top_z, roll, pitch, yaw_opened]), metadata={"encoding": "xyzrpy"}, ) @@ -91,6 +99,6 @@ time.sleep(1.0) node.send_output( "action", - pa.array([place_y, place_x, 0.15, -1.6, -0.0, -3]), + pa.array([place_x, place_y, top_z, roll, pitch, yaw_opened]), metadata={"encoding": "xyzrpy"}, ) diff --git a/examples/so100-remote/so100.urdf b/examples/so100-remote/so100.urdf index f13689fb..c993f6b5 100644 --- a/examples/so100-remote/so100.urdf +++ b/examples/so100-remote/so100.urdf @@ -95,13 +95,9 @@ + xyz="0 -1 0" /> - + @@ -148,18 +144,14 @@ type="revolute"> + rpy="0 0 0" /> - + xyz="1 0 0" /> + @@ -206,18 +198,13 @@ type="revolute"> + rpy="-0.1 0 0" /> - @@ -322,7 +309,7 @@ rpy="0 0 0" /> + filename="Wrist_Roll_Follower_SO101.STL" /> @@ -337,12 +324,8 @@ - + xyz="0 1 0" /> + @@ -380,7 +363,7 @@ rpy="0 0 0" /> + filename="Moving_Jaw_SO101.STL" /> @@ -396,10 +379,6 @@ link="Moving Jaw" /> - + \ No newline at end of file diff --git a/examples/so100-remote/so100_2.urdf b/examples/so100-remote/so100_2.urdf deleted file mode 100644 index 3c574412..00000000 --- a/examples/so100-remote/so100_2.urdf +++ /dev/null @@ -1,365 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/examples/so100-remote/test.yml b/examples/so100-remote/test.yml index 74061f04..a7bf9740 100644 --- a/examples/so100-remote/test.yml +++ b/examples/so100-remote/test.yml @@ -8,7 +8,6 @@ nodes: - pose env: PORT: /dev/ttyACM0 - CONFIG: follower.right.json TORQUE: true IDS: 1 2 3 4 5 6 @@ -17,6 +16,15 @@ nodes: outputs: - action + - id: camera + build: pip install -e ../../node-hub/dora-pyrealsense + path: dora-pyrealsense + inputs: + tick: dora/timer/millis/33 + outputs: + - image + - depth + - id: pytorch-kinematics build: pip install -e ../../node-hub/dora-pytorch-kinematics path: dora-pytorch-kinematics @@ -27,18 +35,21 @@ nodes: - pose - action env: - URDF_PATH: so100_2.urdf + URDF_PATH: so100.urdf END_EFFECTOR_LINK: "Moving Jaw" - so100_transform: 0 -0.3 0 + TRANSFORM: -0.18 0.02 -0.65 0.7 0 0 0.7 - id: plot build: pip install -e ../../node-hub/dora-rerun path: dora-rerun inputs: - series_so100: so100/pose - series_pose: pytorch-kinematics/pose - series_action: pytorch-kinematics/action - jointstate_so100: so100/pose + #series_so100: so100/pose + # series_pose: pytorch-kinematics/pose + series_so100: pytorch-kinematics/action + jointstate_so100: pytorch-kinematics/action + camera/image: camera/image + camera/depth: camera/depth env: - so100_urdf: so100_2.urdf - so100_transform: 0 -0.3 0 + so100_urdf: so100.urdf + so100_transform: -0.18 0.02 -0.65 0.7 0 0 0.7 + CAMERA_PITCH: -3.1415 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 15f5643f..d5994075 100644 --- a/node-hub/dora-pytorch-kinematics/dora_pytorch_kinematics/main.py +++ b/node-hub/dora-pytorch-kinematics/dora_pytorch_kinematics/main.py @@ -10,6 +10,20 @@ import torch from dora import Node from pytorch_kinematics.transforms.rotation_conversions import matrix_to_euler_angles +TRANSFORM = np.array(os.getenv("TRANSFORM", "0. 0. 0. 0. 0. 0. 1.").split(" ")).astype( + np.float32 +) +pos = torch.tensor([TRANSFORM[0], TRANSFORM[1], TRANSFORM[2]]) +rot = torch.tensor( + [ + TRANSFORM[3], + TRANSFORM[4], + TRANSFORM[5], + TRANSFORM[6], + ], +) +ROB_TF = pk.Transform3d(pos=pos, rot=rot) + def get_xyz_rpy_array_from_transform3d( transform: "pytorch_kinematics.transforms.Transform3d", convention: str = "XYZ" @@ -134,9 +148,6 @@ class RobotKinematics: """ # robot frame - pos = torch.tensor([0.0, 0.0, 0.0]) - rot = torch.tensor([0.0, 0.0, 0.0]) - rob_tf = pk.Transform3d(pos=pos, rot=rot) angles_tensor = self._prepare_joint_tensor( joint_angles, batch_dim_required=False @@ -145,7 +156,7 @@ class RobotKinematics: goal_in_rob_frame_tf = self.chain.forward_kinematics( angles_tensor, end_only=True ) - goal_tf = rob_tf.compose(goal_in_rob_frame_tf) + goal_tf = ROB_TF.compose(goal_in_rob_frame_tf) return goal_tf def compute_ik( @@ -250,6 +261,7 @@ def main(): torch.tensor(target[3:6]), convention="XYZ" ), ) + target = ROB_TF.inverse().compose(target) solution = robot.compute_ik(target, last_known_state) if solution is None: continue @@ -275,7 +287,7 @@ def main(): # Apply Forward Kinematics target = robot.compute_fk(target) target = np.array(get_xyz_rpy_array_from_transform3d(target)) - target = pa.array(target.ravel()) + target = pa.array(target.ravel(), type=pa.float32()) metadata["encoding"] = "xyzrpy" node.send_output(event["id"], target, metadata=metadata) diff --git a/node-hub/dora-rerun/src/urdf.rs b/node-hub/dora-rerun/src/urdf.rs index 62d96f6b..b3de8fc0 100644 --- a/node-hub/dora-rerun/src/urdf.rs +++ b/node-hub/dora-rerun/src/urdf.rs @@ -1,4 +1,4 @@ -use std::collections::HashMap; +use std::{collections::HashMap, path::PathBuf}; use eyre::{Context, ContextCompat, Result}; use k::{nalgebra::Quaternion, Chain, Translation3, UnitQuaternion}; @@ -64,6 +64,12 @@ pub fn init_urdf(rec: &RecordingStream) -> Result>> { let chain = k::Chain::::from_urdf_file(&urdf_path).context("Could not load URDF")?; let transform = key.replace("_urdf", "_transform"); + + if PathBuf::from(&urdf_path).file_name() != PathBuf::from(&path).file_name() { + return Err(eyre::eyre!( + "URDF path should be the same as the key without _urdf or _URDF. Got {} instead of {}", urdf_path, path + )); + } if let Err(err) = rec.log_file_from_path(&urdf_path, None, false) { println!("Could not log file: {}. Errored with {}", urdf_path, err); println!("Make sure to install urdf loader with:"); @@ -71,6 +77,7 @@ pub fn init_urdf(rec: &RecordingStream) -> Result>> { "pip install git+https://github.com/rerun-io/rerun-loader-python-example-urdf.git" ) }; + // Get transform by replacing URDF_ with TRANSFORM_ if let Ok(transform) = std::env::var(transform) { let transform = transform diff --git a/node-hub/dora-rustypot/Cargo.toml b/node-hub/dora-rustypot/Cargo.toml index 0b619bed..26ef058b 100644 --- a/node-hub/dora-rustypot/Cargo.toml +++ b/node-hub/dora-rustypot/Cargo.toml @@ -9,4 +9,4 @@ edition = "2021" dora-node-api = "0.3.11" eyre = "0.6.12" rustypot = { git = "https://github.com/pollen-robotics/rustypot", branch = "next-release-1.0" } -serialport = "4.7.1" +serialport = { version = "4.7.1", default-features = false }