Browse Source

Fix CI/CD And improve example

remote-reachy2-improvements
haixuanTao 8 months ago
parent
commit
f6980ebee7
10 changed files with 82 additions and 452 deletions
  1. +1
    -1
      .github/workflows/node-hub-ci-cd.yml
  2. +0
    -21
      Cargo.lock
  3. +2
    -3
      examples/so100-remote/no_torque.yml
  4. +22
    -14
      examples/so100-remote/parse_keyboard.py
  5. +11
    -32
      examples/so100-remote/so100.urdf
  6. +0
    -365
      examples/so100-remote/so100_2.urdf
  7. +20
    -9
      examples/so100-remote/test.yml
  8. +17
    -5
      node-hub/dora-pytorch-kinematics/dora_pytorch_kinematics/main.py
  9. +8
    -1
      node-hub/dora-rerun/src/urdf.rs
  10. +1
    -1
      node-hub/dora-rustypot/Cargo.toml

+ 1
- 1
.github/workflows/node-hub-ci-cd.yml View File

@@ -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



+ 0
- 21
Cargo.lock View File

@@ -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",


+ 2
- 3
examples/so100-remote/no_torque.yml View File

@@ -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


+ 22
- 14
examples/so100-remote/parse_keyboard.py View File

@@ -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"},
)

+ 11
- 32
examples/so100-remote/so100.urdf View File

@@ -95,13 +95,9 @@
<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"
upper="2.1"
effort="0"
velocity="0" />
</joint>
<link
name="Upper_Arm">
@@ -148,18 +144,14 @@
type="revolute">
<origin
xyz="0 0.1025 0.0306"
rpy="1.5708 0 0" />
rpy="0 0 0" />
<parent
link="Rotation_Pitch" />
<child
link="Upper_Arm" />
<axis
xyz="-1 0 0" />
<limit
lower="-0.1"
upper="3.45"
effort="0"
velocity="0" />
xyz="1 0 0" />
</joint>
<link
name="Lower_Arm">
@@ -206,18 +198,13 @@
type="revolute">
<origin
xyz="0 0.11257 0.028"
rpy="-1.5708 0 0" />
rpy="-0.1 0 0" />
<parent
link="Upper_Arm" />
<child
link="Lower_Arm" />
<axis
xyz="1 0 0" />
<limit
lower="-0.2"
upper="3.14159"
effort="0"
velocity="0" />
</joint>
<link
name="Wrist_Pitch_Roll">
@@ -322,7 +309,7 @@
rpy="0 0 0" />
<geometry>
<mesh
filename="Wrist_Roll_Follower_SO101.stl" />
filename="Wrist_Roll_Follower_SO101.STL" />
</geometry>
</collision>
</link>
@@ -337,12 +324,8 @@
<child
link="Fixed_Jaw" />
<axis
xyz="0 -1 0" />
<limit
lower="-3.14159"
upper="3.14159"
effort="0"
velocity="0" />
xyz="0 1 0" />
</joint>
<link
name="Moving Jaw">
@@ -380,7 +363,7 @@
rpy="0 0 0" />
<geometry>
<mesh
filename="Moving_Jaw_SO101.stl" />
filename="Moving_Jaw_SO101.STL" />
</geometry>
</collision>
</link>
@@ -396,10 +379,6 @@
link="Moving Jaw" />
<axis
xyz="0 0 1" />
<limit
lower="0"
upper="1.7"
effort="0"
velocity="0" />
</joint>
</robot>

+ 0
- 365
examples/so100-remote/so100_2.urdf View File

@@ -1,365 +0,0 @@
<?xml version="1.0" encoding="utf-8"?>
<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com)
Commit Version: 1.6.0-1-g15f4949 Build Version: 1.6.7594.29634
For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
<robot
name="SO_5DOF_ARM100_8j_URDF.SLDASM">
<link
name="base_link">
<inertial>
<origin
xyz="-2.45960666746703E-07 0.0311418169687909 0.0175746661003382"
rpy="0 0 0" />
<mass
value="0.193184127927598" />
<inertia
ixx="0.000137030709467877"
ixy="2.10136126944992E-08"
ixz="4.24087422551286E-09"
iyy="0.000169089551209259"
iyz="2.26514711036514E-05"
izz="0.000145097720857224" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="Base.STL" />
</geometry>
<material
name="">
<color
rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="Base.STL" />
</geometry>
</collision>
</link>
<link
name="Rotation_Pitch">
<inertial>
<origin
xyz="-9.07886224712597E-05 0.0590971820568318 0.031089016892169"
rpy="0 0 0" />
<mass
value="0.119226314127197" />
<inertia
ixx="5.90408775624429E-05"
ixy="4.90800532852998E-07"
ixz="-5.90451772654387E-08"
iyy="3.21498601038881E-05"
iyz="-4.58026206663885E-06"
izz="5.86058514263952E-05" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="Rotation_Pitch.STL" />
</geometry>
<material
name="">
<color
rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="Rotation_Pitch.STL" />
</geometry>
</collision>
</link>
<joint
name="Rotation"
type="continuous">
<origin
xyz="0 -0.0452 0.0165"
rpy="1.5708 0 0" />
<parent
link="base_link" />
<child
link="Rotation_Pitch" />
<axis
xyz="0 -1 0" />
</joint>
<link
name="Upper_Arm">
<inertial>
<origin
xyz="-1.7205170190925E-05 0.0701802156327694 0.00310545118155671"
rpy="0 0 0" />
<mass
value="0.162409284599177" />
<inertia
ixx="0.000167153146617081"
ixy="1.03902689187701E-06"
ixz="-1.20161820645189E-08"
iyy="7.01946992214245E-05"
iyz="2.11884806298698E-06"
izz="0.000213280241160769" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="Upper_Arm.STL" />
</geometry>
<material
name="">
<color
rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="Upper_Arm.STL" />
</geometry>
</collision>
</link>
<joint
name="Pitch"
type="continuous">
<origin
xyz="0 0.1025 0.0306"
rpy="0 0 0" />
<parent
link="Rotation_Pitch" />
<child
link="Upper_Arm" />
<axis
xyz="1 0 0" />
</joint>
<link
name="Lower_Arm">
<inertial>
<origin
xyz="-0.00339603710186651 0.00137796353960074 0.0768006751156044"
rpy="0 0 0" />
<mass
value="0.147967774582291" />
<inertia
ixx="0.000105333995841409"
ixy="1.73059237226499E-07"
ixz="-1.1720305455211E-05"
iyy="0.000138766654485212"
iyz="1.77429964684103E-06"
izz="5.08741652515214E-05" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="Lower_Arm.STL" />
</geometry>
<material
name="">
<color
rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="Lower_Arm.STL" />
</geometry>
</collision>
</link>
<joint
name="Elbow"
type="continuous">
<origin
xyz="0 0.11257 0.028"
rpy="0 0 0" />
<parent
link="Upper_Arm" />
<child
link="Lower_Arm" />
<axis
xyz="1 0 0" />
</joint>
<link
name="Wrist_Pitch_Roll">
<inertial>
<origin
xyz="-0.00852653127372418 -0.0352278997897927 -2.34622481569413E-05"
rpy="0 0 0" />
<mass
value="0.066132067097723" />
<inertia
ixx="1.95717492443445E-05"
ixy="-6.62714374412293E-07"
ixz="5.20089016442066E-09"
iyy="2.38028417569933E-05"
iyz="4.09549055863776E-08"
izz="3.4540143384536E-05" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="Wrist_Pitch_Roll.STL" />
</geometry>
<material
name="">
<color
rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="Wrist_Pitch_Roll.STL" />
</geometry>
</collision>
</link>
<joint
name="Wrist_Pitch"
type="continuous">
<origin
xyz="0 0.0052 0.1349"
rpy="-1.57 0 0" />
<parent
link="Lower_Arm" />
<child
link="Wrist_Pitch_Roll" />
<axis
xyz="1 0 0" />
</joint>
<link
name="Fixed_Jaw">
<inertial>
<origin
xyz="0.00552376906426563 -0.0280167153359021 0.000483582592841092"
rpy="0 0 0" />
<mass
value="0.0929859131176897" />
<inertia
ixx="4.3328249304211E-05"
ixy="7.09654328670947E-06"
ixz="5.99838530879484E-07"
iyy="3.04451747368212E-05"
iyz="-1.58743247545413E-07"
izz="5.02460913506734E-05" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="Wrist_Roll_Follower_SO101.STL" />
</geometry>
<material
name="">
<color
rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="Wrist_Roll_Follower_SO101.STL" />
</geometry>
</collision>
</link>
<joint
name="Wrist_Roll"
type="continuous">
<origin
xyz="0 -0.0601 0"
rpy="0 1.57 0" />
<parent
link="Wrist_Pitch_Roll" />
<child
link="Fixed_Jaw" />
<axis
xyz="0 1 0" />
</joint>
<link
name="Moving Jaw">
<inertial>
<origin
xyz="-0.00161744605468241 -0.0303472584046471 0.000449645961853651"
rpy="0 0 0" />
<mass
value="0.0202443794940372" />
<inertia
ixx="1.10911325081525E-05"
ixy="-5.35076503033314E-07"
ixz="-9.46105662101403E-09"
iyy="3.03576451001973E-06"
iyz="-1.71146075110632E-07"
izz="8.9916083370498E-06" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="Moving_Jaw_SO101.STL" />
</geometry>
<material
name="">
<color
rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="Moving_Jaw_SO101.STL" />
</geometry>
</collision>
</link>
<joint
name="Jaw"
type="continuous">
<origin
xyz="-0.0202 -0.0244 0"
rpy="0 0 0" />
<parent
link="Fixed_Jaw" />
<child
link="Moving Jaw" />
<axis
xyz="0 0 1" />
</joint>
</robot>

+ 20
- 9
examples/so100-remote/test.yml View File

@@ -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

+ 17
- 5
node-hub/dora-pytorch-kinematics/dora_pytorch_kinematics/main.py View File

@@ -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)



+ 8
- 1
node-hub/dora-rerun/src/urdf.rs View File

@@ -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<HashMap<String, Chain<f32>>> {
let chain = k::Chain::<f32>::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<HashMap<String, Chain<f32>>> {
"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


+ 1
- 1
node-hub/dora-rustypot/Cargo.toml View File

@@ -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 }

Loading…
Cancel
Save