📹 openarmx_teach | Trajectory Recording and Playback¶

This package provides recording and playback tools for OpenArmX dual arms plus dual grippers. Recording samples /joint_states into one YAML file. Playback then splits that YAML by joint name and sends commands to the left and right arms through FollowJointTrajectory and to the grippers through GripperCommand. It also supports synchronized multi-robot playback.

✨ Highlights¶

Continuous recording: record_joint_states_always samples /joint_states at a fixed rate, with interactive start, pause, and save.
Parallel multi-controller playback: play_joint_trajectory automatically groups the left arm, right arm, left gripper, and right gripper and executes them together.
Synchronized multi-robot broadcast: --arm-prefix robot1 robot2 ... drives multiple robots from one command, triggered together through the same asyncio.gather.
Joint filtering: choose a custom joint list, left arm only, right arm only, both arms, or all joints.
Rate scaling: --rate-scale speeds up or slows down the whole motion.
Smart gripper scheduling: finger positions are averaged into a scalar, tiny changes are compressed, and gripper timing can follow arm feedback time with --sync-feedback and --sync-margin.

🔧 Dependencies and Build¶

cd openarmx_ws
colcon build --packages-select openarmx_teach
source install/setup.bash

This package requires rclpy, control_msgs, trajectory_msgs, and PyYAML, which are typically installed through ROS 2 and apt.

⚠️ Build the package and source the workspace first, otherwise the record and playback commands will not be available.

🚀 Typical Workflow¶

Launch the hardware or simulation and make sure the required controllers are running.
Before recording, set KP / KD to 0 so the robot can be moved by hand with low impedance.
Open a new terminal and start recording: move the robot, press SPACE to start, press SPACE again to pause, and press w to save.
Before playback, restore the default or higher KP / KD values so the motors can track the trajectory.
Start with --all-joints to verify playback, then narrow to one arm or the grippers if needed. Add --sync-feedback if gripper timing matters.
If you see Warning: Joint 'xxx' not found, check whether joint_names in the YAML match the controller joint names currently in use.

⚠️ The two most important steps in this workflow are: set KP / KD to 0 before recording, and restore KP / KD before playback.

1️⃣ Step 1: Enable CAN Interfaces¶

Initialize the CAN interfaces after each boot:

📌 In multi-robot setups, make sure each robot uses its own CAN interfaces.

sudo ip link set can0 down
sudo ip link set can0 type can bitrate 1000000
sudo ip link set can0 up

sudo ip link set can1 down
sudo ip link set can1 type can bitrate 1000000
sudo ip link set can1 up

# For multiple robots, continue with can2, can3...
sudo ip link set can2 down
sudo ip link set can2 type can bitrate 1000000
sudo ip link set can2 up

sudo ip link set can3 down
sudo ip link set can3 type can bitrate 1000000
sudo ip link set can3 up

2️⃣ Step 2: Launch the Robot¶

Single robot without gravity compensation¶

ros2 launch openarmx_bringup openarmx.bimanual.launch.py \
    right_can_interface:=can0 left_can_interface:=can1 \
    control_mode:=mit

Single robot with gravity compensation¶

💡 Gravity compensation can remove the roughly 6-degree steady-state error caused by gravity in MIT mode. Enable it when higher accuracy is needed.

ros2 launch openarmx_bringup openarmx.bimanual.launch.py \
    right_can_interface:=can0 left_can_interface:=can1 \
    control_mode:=mit \
    enable_forward_effort:=true

Multiple robots without gravity compensation¶

Launch each robot in a separate terminal and distinguish them with arm_prefix:

📌 Keep the same arm_prefix consistently in launch, recording, parameter tuning, and playback commands.

# Robot 1 (terminal 1)
ros2 launch openarmx_bringup openarmx.bimanual.launch.py \
    right_can_interface:=can0 left_can_interface:=can1 \
    arm_prefix:=robot1 control_mode:=mit

# Robot 2 (terminal 2)
ros2 launch openarmx_bringup openarmx.bimanual.launch.py \
    right_can_interface:=can2 left_can_interface:=can3 \
    arm_prefix:=robot2 control_mode:=mit

Multiple robots with gravity compensation¶

# Robot 1
ros2 launch openarmx_bringup openarmx.bimanual.launch.py \
    right_can_interface:=can0 left_can_interface:=can1 \
    arm_prefix:=robot1 control_mode:=mit enable_forward_effort:=true

# Robot 2
ros2 launch openarmx_bringup openarmx.bimanual.launch.py \
    right_can_interface:=can2 left_can_interface:=can3 \
    arm_prefix:=robot2 control_mode:=mit enable_forward_effort:=true

3️⃣ Step 3: Adjust KP / KD Before Recording¶

⚠️ Before teaching, set KP / KD to 0 so the robot enters a near zero-impedance state and can be dragged by hand.

In RViz, add the panel through Panels -> Add New Panel -> KPKDPanel -> OK.

Single robot¶

Leave the namespace field empty, select both arms, move the KP / KD sliders to the far left, and click Apply KP/KD to all joints.

Required: confirm KP=0 and KD=0 before manually moving the robot.

Multiple robots¶

Enter the namespace prefix for each robot, for example robot1, and repeat the same steps for each robot.

Feedforward compensation during teaching¶

💡 If gravity compensation is enabled with enable_forward_effort:=true, the robot may feel more assisted even when KP / KD are zero. You can temporarily disable feedforward without restarting nodes:

# Single robot
ros2 param set /gravity_comp_node enable_compensation false

# Multi-robot example (robot1)
ros2 param set /robot1/gravity_comp_node enable_compensation false

After teaching, enable it again:

ros2 param set /gravity_comp_node enable_compensation true

# Or
ros2 param set /robot1/gravity_comp_node enable_compensation true

4️⃣ Step 4: Record Motion¶

Single robot¶

ros2 run openarmx_teach record_joint_states_always --rate 10 --outfile demo.yaml

Multi-robot with a namespace prefix¶

# Record robot1
ros2 run openarmx_teach record_joint_states_always --arm-prefix robot1 --rate 10 --outfile demo_robot1.yaml

The default output name is joint_states_stream_YYYYMMDD_HHMMSS.yaml. The sampling rate --rate determines the time_from_start increment as (i+1)*dt.

✅ Recommended recording rate: 10-20 Hz. In most cases, 10 Hz or 20 Hz is enough.

⌨️ Keyboard Controls¶

Key	Function
`SPACE` / `p`	Start or pause recording
`c`	Clear the buffer, with confirmation
`w`	Save and quit, with confirmation
`q`	Quit without saving

🧾 Recording Parameters¶

Parameter	Description
`--rate`	Sampling rate in Hz, default `10`
`--outfile`	Output file path, default is an auto-generated timestamped filename
`--arm-prefix`	Robot namespace prefix for multi-robot use, such as `robot1`
`--topic`	Directly specify the topic name; higher priority than `--arm-prefix`

joint_names follow the order from the first received message, and each recorded snapshot is reordered to that same layout before being written.

📌 If you want to record only one robot, prefer --arm-prefix instead of manually specifying a topic.

5️⃣ Step 5: Restore KP / KD Before Playback¶

⚠️ After recording, restore KP / KD before playback. Otherwise the motors will not have enough torque to follow the trajectory.

In KPKDPanel, click all Restore Default buttons, then click Apply KP/KD to all joints.

Default KP / KD values are intentionally conservative for safety. If you need tighter trajectory tracking, increase KP / KD appropriately or switch to CSP position mode with control_mode:=csp.

⚠️ CSP is more accurate, but payload capacity is around 3 kg and overload can damage the motors.

6️⃣ Step 6: Play Motion¶

Single robot¶

ros2 run openarmx_teach play_joint_trajectory demo.yaml --all-joints

Synchronized multi-robot playback¶

ros2 run openarmx_teach play_joint_trajectory demo.yaml --all-joints --arm-prefix robot1 robot2

All robot trajectories are dispatched together through the same asyncio.gather, which keeps the trigger timing aligned.

✅ For the first playback, start with --all-joints to verify the full pipeline before testing one arm or one gripper.

Default action names:

Left arm: /left_joint_trajectory_controller/follow_joint_trajectory
Right arm: /right_joint_trajectory_controller/follow_joint_trajectory
Left gripper: /left_gripper_controller/gripper_cmd
Right gripper: /right_gripper_controller/gripper_cmd

🎯 Common Options¶

Parameter	Description
`--all-joints`	Auto-group and play all joints, including both arms and both grippers
`--left-arm`	Play the left arm only
`--right-arm`	Play the right arm only
`--both-arms`	Play both arms only, without grippers
`--joints <list>`	Use a custom joint subset
`--arm-prefix`	Namespace prefix, supports multiple values such as `robot1 robot2`
`--rate-scale`	Global speed scaling; `>1` is faster, `<1` is slower, default `1.0`
`--sync-feedback`	Use arm feedback time to schedule gripper actions
`--sync-margin`	Trigger grippers when feedback time plus margin reaches the target time
`--action <name>`	Single-controller mode; names containing `gripper` use `GripperCommand`

▶️ Playback Examples¶

# Slow down to 50% speed and enable gripper synchronization
ros2 run openarmx_teach play_joint_trajectory demo.yaml \
    --all-joints --rate-scale 0.5 --sync-feedback

# Play only the left arm
ros2 run openarmx_teach play_joint_trajectory demo.yaml --left-arm

# Single-controller mode: test only the left gripper
ros2 run openarmx_teach play_joint_trajectory demo.yaml \
    --action /left_gripper_controller/gripper_cmd \
    --joints openarmx_left_finger_joint1 openarmx_left_finger_joint2

# Play the same motion on three robots
ros2 run openarmx_teach play_joint_trajectory demo.yaml \
    --all-joints --arm-prefix robot1 robot2 robot3

🗂️ YAML Format¶

joint_names: [openarmx_left_joint1, openarmx_left_joint2, ..., openarmx_right_joint1, ...]
points:
  - positions: [0.1, 0.2, ...]
    time_from_start: 0.1
  - positions: [0.15, 0.25, ...]
    time_from_start: 0.2

⚠️ Operational Notes¶

time_from_start is derived from the sampling rate, not from real execution timestamps. If controllers start slowly or lag, adjust --rate-scale or --sync-margin.
During multi-robot playback, the order of --arm-prefix values does not affect synchronization because all robots are triggered through the same asyncio.gather.
Too low a recording rate produces sparse trajectories. Too high a rate makes larger files and adds limited value for grippers. 10-20 Hz is usually the best range.
Gripper commands are compressed before sending, so clear open and close motions are easier to reproduce reliably.
Before playback, make sure the action servers are running and their names match the expected configuration, otherwise the command will fail or time out quickly.