From 7391291f689821e487e3e107eece421f55a61876 Mon Sep 17 00:00:00 2001 From: haixuantao Date: Tue, 24 Jun 2025 18:18:25 +0200 Subject: [PATCH] Better configuration for each arm --- examples/keyboard/dataflow.yml | 14 +++++++++++++ examples/urdf/franka.yml | 7 +++++-- examples/urdf/kuka.yml | 3 +++ examples/urdf/piper.yml | 3 +++ examples/urdf/so_arm101.yml | 5 ++++- examples/urdf/ur5.yml | 5 +++++ examples/urdf/z1.yml | 7 ++++++- .../dora_pytorch_kinematics/main.py | 2 +- node-hub/gamepad/gamepad/main.py | 21 ++++++++++--------- 9 files changed, 52 insertions(+), 15 deletions(-) create mode 100644 examples/keyboard/dataflow.yml diff --git a/examples/keyboard/dataflow.yml b/examples/keyboard/dataflow.yml new file mode 100644 index 00000000..34e303b3 --- /dev/null +++ b/examples/keyboard/dataflow.yml @@ -0,0 +1,14 @@ +nodes: + - id: keyboard + build: pip install -e ../../node-hub/dora-keyboard + path: dora-keyboard + outputs: + - char + env: + DISPLAY: $DISPLAY + + - id: rerun + path: dora-rerun + build: pip install -e ../../node-hub/dora-rerun + inputs: + text_input: keyboard/char diff --git a/examples/urdf/franka.yml b/examples/urdf/franka.yml index 4aef1345..59afa7b5 100644 --- a/examples/urdf/franka.yml +++ b/examples/urdf/franka.yml @@ -16,6 +16,9 @@ nodes: - raw_control inputs: tick: dora/timer/millis/10 + env: + MAX_LINEAR_SPEED: 0.01 + MAX_ANGULAR_SPEED: 0.05 - id: pytorch_kinematics build: pip install -e ../../node-hub/dora-pytorch-kinematics @@ -28,5 +31,5 @@ nodes: MODEL_NAME: "panda_description" END_EFFECTOR_LINK: "panda_link8" TRANSFORM: "0. 0. 0. 1. 0. 0. 0." - POSITION_TOLERANCE: 0.01 - ROTATION_TOLERANCE: 0.01 + POSITION_TOLERANCE: 0.001 + ROTATION_TOLERANCE: 0.001 diff --git a/examples/urdf/kuka.yml b/examples/urdf/kuka.yml index a7668be7..0a46f861 100644 --- a/examples/urdf/kuka.yml +++ b/examples/urdf/kuka.yml @@ -16,6 +16,9 @@ nodes: - raw_control inputs: tick: dora/timer/millis/10 + env: + MAX_LINEAR_SPEED: 0.02 + MAX_ANGULAR_SPEED: 0.10 - id: pytorch_kinematics build: pip install -e ../../node-hub/dora-pytorch-kinematics diff --git a/examples/urdf/piper.yml b/examples/urdf/piper.yml index e2ca4385..3055fb93 100644 --- a/examples/urdf/piper.yml +++ b/examples/urdf/piper.yml @@ -16,6 +16,9 @@ nodes: - raw_control inputs: tick: dora/timer/millis/10 + env: + MAX_LINEAR_SPEED: 0.01 + MAX_ANGULAR_SPEED: 0.05 - id: pytorch_kinematics build: pip install -e ../../node-hub/dora-pytorch-kinematics diff --git a/examples/urdf/so_arm101.yml b/examples/urdf/so_arm101.yml index d61e5ef4..395c4591 100644 --- a/examples/urdf/so_arm101.yml +++ b/examples/urdf/so_arm101.yml @@ -16,6 +16,9 @@ nodes: - raw_control inputs: tick: dora/timer/millis/10 + env: + MAX_LINEAR_SPEED: 0.01 + MAX_ANGULAR_SPEED: 0.05 - id: pytorch_kinematics build: pip install -e ../../node-hub/dora-pytorch-kinematics @@ -29,4 +32,4 @@ nodes: END_EFFECTOR_LINK: "gripper" TRANSFORM: "0. 0. 0. 1. 0. 0. 0." POSITION_TOLERANCE: 0.01 - ROTATION_TOLERANCE: 0.01 + ROTATION_TOLERANCE: 0.03 diff --git a/examples/urdf/ur5.yml b/examples/urdf/ur5.yml index cf00a4a4..0f19291a 100644 --- a/examples/urdf/ur5.yml +++ b/examples/urdf/ur5.yml @@ -16,6 +16,9 @@ nodes: - raw_control inputs: tick: dora/timer/millis/10 + env: + MAX_LINEAR_SPEED: 0.01 + MAX_ANGULAR_SPEED: 0.05 - id: pytorch_kinematics build: pip install -e ../../node-hub/dora-pytorch-kinematics @@ -28,3 +31,5 @@ nodes: MODEL_NAME: "ur5_description" END_EFFECTOR_LINK: "tool0" TRANSFORM: "0. 0. 0. 1. 0. 0. 0." + POSITION_TOLERANCE: 0.001 + ROTATION_TOLERANCE: 0.001 diff --git a/examples/urdf/z1.yml b/examples/urdf/z1.yml index bdfd8a0d..bea243d2 100644 --- a/examples/urdf/z1.yml +++ b/examples/urdf/z1.yml @@ -16,6 +16,9 @@ nodes: - raw_control inputs: tick: dora/timer/millis/10 + env: + MAX_LINEAR_SPEED: 0.01 + MAX_ANGULAR_SPEED: 0.05 - id: pytorch_kinematics build: pip install -e ../../node-hub/dora-pytorch-kinematics @@ -26,5 +29,7 @@ nodes: - cmd_vel env: MODEL_NAME: "z1_description" - END_EFFECTOR_LINK: "link05" + END_EFFECTOR_LINK: "link06" TRANSFORM: "0. 0. 0. 1. 0. 0. 0." + POSITION_TOLERANCE: 0.001 + ROTATION_TOLERANCE: 0.001 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 63ae2b7e..d1c795d4 100644 --- a/node-hub/dora-pytorch-kinematics/dora_pytorch_kinematics/main.py +++ b/node-hub/dora-pytorch-kinematics/dora_pytorch_kinematics/main.py @@ -374,7 +374,7 @@ def main(): target = robot.compute_fk(last_known_state) target = ( np.array(get_xyz_rpy_array_from_transform3d(target).detach()) - + target_vel / 10 + + target_vel ) target = pa.array(target.ravel(), type=pa.float32()) target = pk.Transform3d( diff --git a/node-hub/gamepad/gamepad/main.py b/node-hub/gamepad/gamepad/main.py index e65f6d40..6dd61280 100644 --- a/node-hub/gamepad/gamepad/main.py +++ b/node-hub/gamepad/gamepad/main.py @@ -5,7 +5,11 @@ import json import pyarrow as pa import pygame from dora import Node +import os +MAX_LINEAR_SPEED = float(os.getenv("MAX_LINEAR_SPEED", "0.05")) +MAX_ANGULAR_SPEED = float(os.getenv("MAX_ANGULAR_SPEED", "0.8")) +JOYSTICK_DEADZONE = float(os.getenv("JOYSTICK_DEADZONE", "0.2")) class Controller: """Controller mapping.""" @@ -50,9 +54,6 @@ def main(): controller = Controller() - move_speed = 0.05 # Fixed increment for D-pad - max_linear_z_speed = 0.1 # Maximum linear Z speed - max_angular_speed = 0.8 # Maximum angular speed print(f"Detected controller: {joystick.get_name()}") print(f"Number of axes: {joystick.get_numaxes()}") @@ -90,39 +91,39 @@ def main(): # 5. Left stick vertical for rotation around X # 6. Left stick horizontal for rotation around Y - deadzone = 0.05 + deadzone = JOYSTICK_DEADZONE # Linear X velocity from D-pad vertical linear_x = 0.0 if dpad_y != 0: - linear_x = dpad_y * move_speed + linear_x = dpad_y * MAX_LINEAR_SPEED # Linear Y velocity from D-pad horizontal linear_y = 0.0 if dpad_x != 0: - linear_y = dpad_x * move_speed + linear_y = dpad_x * MAX_LINEAR_SPEED # Linear Z velocity from right stick vertical right_y = -joystick.get_axis(controller.axisNames["RIGHT-Y"]) right_y = 0.0 if abs(right_y) < deadzone else right_y - linear_z = right_y * max_linear_z_speed + linear_z = right_y * MAX_LINEAR_SPEED # Angular Z velocity (rotation) from right stick horizontal right_x = -joystick.get_axis(controller.axisNames["RIGHT-X"]) right_x = 0.0 if abs(right_x) < deadzone else right_x angular_z = ( - right_x * max_angular_speed + right_x * MAX_ANGULAR_SPEED ) # TODO: Make z non zero, but on my gamepad the value is never zero # Angular X velocity from left stick vertical left_y = -joystick.get_axis(controller.axisNames["LEFT-Y"]) left_y = 0.0 if abs(left_y) < deadzone else left_y - angular_x = left_y * max_angular_speed + angular_x = left_y * MAX_ANGULAR_SPEED # Angular Y velocity from left stick horizontal left_x = -joystick.get_axis(controller.axisNames["LEFT-X"]) left_x = 0.0 if abs(left_x) < deadzone else left_x - angular_y = left_x * max_angular_speed + angular_y = left_x * MAX_ANGULAR_SPEED cmd_vel = [linear_x, linear_y, linear_z, angular_x, angular_y, angular_z] if any(cmd_vel):