Chapter 4: Upper-Limb Joint Motion Planning

1 Interface Overview

The upper-limb joint motion planning interface is used to batch-execute joint motion planning tasks, supporting queue management and automatic execution. Developers can define multiple motion tasks (such as reaching, handshaking, and resetting) in a YAML configuration file; the program loads and executes each task in sequence while monitoring the execution state.

This interface is suitable for scenarios that require orchestrating multiple upper-limb action sequences. The difference from joint override control (Chapter 5) is: motion planning is "send a request once and wait for execution to complete," whereas override control is "continuously publish commands at high frequency."

2 Message Fields

Communication interface: Upper-limb joint motion planning is accomplished through the following ROS2 topics.

Planning request topic: /joint_motion_plan_request, message type: interface_protocol/msg/JointMotionPlanRequest
Planning state topic: /joint_motion_plan_state, message type: interface_protocol/msg/JointMotionPlanState

The detailed message field definitions are given in the following sections.

JointMotionPlanRequest.msg field definition (from GitHub):

PLAIN

## request_type constants
uint8 REQUEST_PLAN_EXECUTE=0    # Plan and execute
uint8 REQUEST_CANCEL=1          # Cancel specified request_id
uint8 REQUEST_RESET=2           # Reset to default pose

int32 request_id
uint8  request_type
bool      use_gravity_compensation
int32[]   joint_indices
float64[] target_positions
float64[] target_velocities
float64   execution_time
float64[] stiffness
float64[] damping

JointMotionPlanState.msg field definition:

PLAIN

uint8 STATUS_DISABLED = 0
uint8 IDLE = 1
uint8 EXECUTING = 2
uint8 EXITING = 3

int32 request_id
uint8  status
float64 progress          # 0.0~1.0

Communication interface:

Topic name	Message type	Direction	Description
`/motion/joint_motion_plan/request`	`JointMotionPlanRequest.msg`	Client → Planner	Send motion planning request
`/motion/joint_motion_plan/state`	`JointMotionPlanState.msg`	Planner → Client	Receive planner state and execution progress

3 Request Types and State Constants

Request type constants:

Constant	Value	Description
`REQUEST_PLAN_EXECUTE`	0	Normal motion planning, execute to target position
`REQUEST_CANCEL`	1	Cancel the task with the specified request_id
`REQUEST_RESET`	2	Reset to the default pose, ignoring most parameters

State constants:

Constant	Value	Description
`STATUS_DISABLED`	0	Disabled state
`IDLE`	1	Idle state, can accept new requests
`EXECUTING`	2	Executing
`EXITING`	3	Exiting

4 Prerequisites

Must enter the lower_body_balance (lower-body balance) mode, with the upper limb in a low-damping gait state
Gamepad switch: [LB, CROSS_X_DOWN]
The request only needs to be sent once; the planner executes it automatically
request_id must be incremented (current ID + 1); the current ID can be obtained by subscribing to the state topic

5 YAML Configuration File Format

Motion planning tasks are defined via a YAML configuration file. The configuration file consists of two parts: global joint indices and the task queue. The joint indices and parameters differ across robot models. The duration field in the YAML corresponds to the execution_time field in JointMotionPlanRequest.msg, representing the expected execution time (unit: seconds).

PM01 configuration example (handshake motion):

YAML

motion_plan:
  # Global joint indices, upper limb starts from 12
  - joint_indices: [12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23]

  # Task 1: extend right hand
  - motion: extend_right_hand   # Note: this example uses the correct spelling "extend"
    target_positions: [0.0, 0.024, 0.081, -0.001, -0.069, 0.000, -0.47, 0.255, 0.161, -0.731, 0.028, 0.000]
    use_gravity_compensation: true
    duration: 2.0
    stiffness:
      - [200.0]           # Waist joint
      - [10.0, 10.0, 5.0, 10.0, 5.0]  # Left upper limb
      - [10.0, 10.0, 5.0, 10.0, 5.0]  # Right upper limb
      - [100.0]           # Head joint
    damping:
      - [3.0]
      - [1.0, 1.0, 1.0, 1.0, 1.0]
      - [1.0, 1.0, 1.0, 1.0, 1.0]
      - [3.0]

  # Task 2: retract right hand
  - motion: withdraw_right_hand
    target_positions: [0.0, 0.024, 0.081, -0.001, -0.069, 0.000, 0.028, -0.084, 0.001, -0.066, 0.000, 0.000]
    use_gravity_compensation: true
    duration: 2.0
    stiffness:
      - [200.0]
      - [10.0, 10.0, 5.0, 10.0, 5.0]
      - [10.0, 10.0, 5.0, 10.0, 5.0]
      - [100.0]
    damping:
      - [3.0]
      - [1.0, 1.0, 1.0, 1.0, 1.0]
      - [1.0, 1.0, 1.0, 1.0, 1.0]
      - [3.0]

The following are configuration examples for the T800 Development Edition and T800 Pro. The differences from PM01 are mainly in the joint_indices, stiffness, and damping values:

T800 Development Edition** configuration example** (13 upper-limb joints, including head pitch + yaw):

YAML

motion_plan:
  # Global joint indices, T800 Development Edition upper limb has 13 joints including the head
  - joint_indices: [12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24]

  # Task 1: extend right hand
  - motion: extend_right_hand
    target_positions: [0.0, 0.024, 0.081, -0.001, -0.069, 0.000, -0.47, 0.255, 0.161, -0.731, 0.028, 0.000, 0.000]
    use_gravity_compensation: true
    duration: 2.0
    stiffness:
      - [400.0]           # Waist joint
      - [40.0, 40.0, 20.0, 40.0, 20.0]  # Left upper limb
      - [40.0, 40.0, 20.0, 40.0, 20.0]  # Right upper limb
      - [100.0, 100.0]    # Head joint (pitch + yaw)
    damping:
      - [3.0]
      - [1.0, 1.0, 1.0, 1.0, 1.0]
      - [1.0, 1.0, 1.0, 1.0, 1.0]
      - [3.0, 3.0]

  # Task 2: retract right hand
  - motion: withdraw_right_hand
    target_positions: [0.0, 0.024, 0.081, -0.001, -0.069, 0.000, 0.028, -0.084, 0.001, -0.066, 0.000, 0.000, 0.000]
    use_gravity_compensation: true
    duration: 2.0
    stiffness:
      - [400.0]
      - [40.0, 40.0, 20.0, 40.0, 20.0]
      - [40.0, 40.0, 20.0, 40.0, 20.0]
      - [100.0, 100.0]
    damping:
      - [3.0]
      - [1.0, 1.0, 1.0, 1.0, 1.0]
      - [1.0, 1.0, 1.0, 1.0, 1.0]
      - [3.0, 3.0]

T800 Pro configuration example (joint index gaps; the dexterous hand occupies the middle indices):

YAML

motion_plan:
  # T800 Pro joint index gaps (dexterous hand occupies middle indices), 13 upper-limb joints total
  - joint_indices: [12, 13, 14, 15, 16, 17, 27, 28, 29, 30, 31, 41, 42]

  # Task 1: extend right hand
  - motion: extend_right_hand
    target_positions: [0.0, 0.024, 0.081, -0.001, -0.069, 0.000, -0.47, 0.255, 0.161, -0.731, 0.028, 0.000, 0.000]
    use_gravity_compensation: true
    duration: 2.0
    stiffness:
      - [500.0]           # Waist joint
      - [40.0, 40.0, 20.0, 40.0, 20.0]  # Left upper limb
      - [40.0, 40.0, 20.0, 40.0, 20.0]  # Right upper limb
      - [100.0, 100.0]    # Head joint
    damping:
      - [10.0]
      - [1.0, 1.0, 1.0, 1.0, 1.0]
      - [1.0, 1.0, 1.0, 1.0, 1.0]
      - [3.0, 3.0]

  # Task 2: retract right hand
  - motion: withdraw_right_hand
    target_positions: [0.0, 0.024, 0.081, -0.001, -0.069, 0.000, 0.028, -0.084, 0.001, -0.066, 0.000, 0.000, 0.000]
    use_gravity_compensation: true
    duration: 2.0
    stiffness:
      - [500.0]
      - [40.0, 40.0, 20.0, 40.0, 20.0]
      - [40.0, 40.0, 20.0, 40.0, 20.0]
      - [100.0, 100.0]
    damping:
      - [10.0]
      - [1.0, 1.0, 1.0, 1.0, 1.0]
      - [1.0, 1.0, 1.0, 1.0, 1.0]
      - [3.0, 3.0]

Model difference comparison:

Model	Joint indices	Waist stiffness	Waist damping
PM01	[12~23], 12 total	200.0	3.0
T800 Development Edition	[12~24], 13 total	400.0	3.0
T800 Pro	[12, 13, 14, 15, 16, 17, 27, 28, 29, 30, 31, 41, 42], 13 total	500.0	10.0

The first item of the configuration file must contain joint_indices, and the number of target_positions must match the number of joint_indices. A RESET-type request ignores most parameters and resets the joints to the default position.

6 Example: Multi-Task Queue Execution

File name: joint_mutiple_motion_plan_example.py

Run command:

Bash

# PM01
python3 src/interface_example/scripts/joint_mutiple_motion_plan_example.py -f src/interface_example/config/pm01/motion_plan_shake_hand.yaml

# T800 Development Edition
python3 src/interface_example/scripts/joint_mutiple_motion_plan_example.py -f src/interface_example/config/t800/motion_plan_shake_hand.yaml

# T800 Pro
python3 src/interface_example/scripts/joint_mutiple_motion_plan_example.py -f src/interface_example/config/t800pro/motion_plan_shake_hand.yaml

Program flow: Load the task queue from the configuration file → send motion planning requests in order → monitor execution state → handle task completion and error conditions. Tasks are executed in sequence and cannot run in parallel.

7 Notes

Must enter the lower_body_balance mode
request_id must be incremented (current ID + 1); if the state is out of sync, wait for the next state message
The first item of the configuration file must contain joint_indices
The number of target_positions must match the number of joint_indices
A RESET-type request ignores most parameters and resets the joints to the default position
Task execution failures are not automatically retried and must be handled manually
You can interrupt the program at any time using Ctrl+C

📎 Source files in the open-source repository for this chapter (click file name to jump to GitHub): JointMotionPlanRequest.msg · JointMotionPlanState.msg · joint_mutiple_motion_plan_example.py