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 }