State Machine Overview and Implementation

1. Architecture Overview

The DR02 Pro SDK includes a complete RL state machine system managed by the HumanoidStateMachine class, with the following core components:

Component	Class Name	Responsibility
State Machine Main Body	HumanoidStateMachine	2ms cycle loop, state scheduling
Robot Interface	RobotInterface / DR2ProInterface	ROS 2 topic subscription and publishing
User Command Interface	UserCommandInterface	Keyboard, gamepad, /STEER input handling
Safety Controller	SafeController	Independent thread monitoring health status
Motion State Storage	MotionStateStore	Thread-safe state sharing
Control Parameters	ControlParameters	PD gains, limits, and other parameters for each body part

1.1 Runtime Pipeline

User Input (Keyboard / Gamepad / /STEER / /REAL_STEER)
    ↓
HumanoidStateMachine (Humanoid state machine, 2ms cycle)
    ↓
DR2ProInterface → RobotInterface (ROS 2 topic communication)
    ↓
ROS 2 Topics: /JOINTS_CMD, /JOINTS_DATA, /IMU_DATA_HEAD, /IMU_DATA_BASE
    ↓
Robot Firmware (Internal controller execution)

2. Startup Sequence

main.cpp

auto fsm = std::make_shared<HumanoidStateMachine>(RobotName::DR2PRO);
fsm->Start();   // Initialize interfaces, create state controllers, start safety monitoring
fsm->Run();     // Enter 2ms control loop
fsm->Stop();    // Stop all threads

Start() executes the following in sequence:

1. Create ROS 2 node sdk_deploy
2. Initialize UserCommandInterface (subscribe to /GAMEPAD_KEY, /STEER, /REAL_STEER)
3. Initialize DR2ProInterface (31 DOF)
4. Create ControlParameters and SafeController
5. Build 4 state controllers
6. Wait 3 seconds for safety initialization
7. Start all interfaces and safety monitoring

3. Control Cycle

loop_timer_.Start(std::chrono::milliseconds(2));  // 2ms cycle
while (rclcpp::ok()) {
    loop_timer_.Wait();
    ri_ptr_->RefreshRobotData();         // Refresh sensor data
    current_controller_->Run();          // Run current state logic
    next_state_name_ = current_controller_->GetNextStateName();  // Determine next state
    if (next_state_name_ != current_state_name_) {
        current_controller_->OnExit();   // Exit current state
        current_controller_ = GetStateControllerPtr(next_state_name_);
        current_controller_->OnEnter();  // Enter new state
        current_state_name_ = next_state_name_;
    }
}

ROS 2 callbacks are handled in a separate thread via rclcpp::spin(node_).

4. State Transitions

Idle → ZeroPos → RLControl
  ↑                  ↓
  └── JointDamping ←─┘

State	Class	Description
Idle	IdleState	Standby, send zero torque commands, wait for stand-up command
ZeroPos	ZeroPosState	3-second cubic spline interpolation, control all 31 joints to return to zero position
RLControl	RLControlState	Run AMP policy inference, receive velocity commands
JointDamping	JointDampingState	Full-body pure damping, auto-return to Idle after 3 seconds

5. State Base Class

StateBase defines the interface for all states:

virtual void OnEnter() = 0;           // Execute on state entry
virtual void OnExit() = 0;            // Execute on state exit
virtual void Run() = 0;               // Execute each control cycle
virtual StateName GetNextStateName() = 0;  // Return next state name

Each state shares the following pointers via ControllerData:

struct ControllerData {
    std::shared_ptr<RobotInterface> ri_ptr;          // Robot interface
    std::shared_ptr<UserCommandInterface> uc_ptr;    // User command interface
    std::shared_ptr<ControlParameters> cp_ptr;       // Control parameters
    std::shared_ptr<MotionStateStore> motion_state_store;  // Motion state storage
};

6. State Details

6.1 Idle State

• Set motion state to Idle on entry
• Send zero torque commands each cycle (joint_cmd_ = 0)
• Switch to ZeroPos when target_mode == ZeroPos
• Remain in Idle when safe_control_mode != 0

6.2 ZeroPos State

• Record current joint positions and velocities on entry
• Smoothly transition to zero position via cubic spline interpolation over 3 seconds
• Different PD gains for each body part:

Waist kp: [600, 2800, 2300]    kd: [6, 15, 20]
Arms kp: [600, 600, 600, 600, 90, 90, 90]  kd: [6, 6, 6, 6, 2, 2, 2]
Legs kp: [1800, 600, 600, 1800, 600, 90]   kd: [24, 6, 6, 24, 6, 2]
Neck kp: [0, 0]              kd: [0, 0]

• Switch to RLControl when target_mode == RLControl
• Switch to JointDamping when safe_control_mode != 0 or target_mode == JointDamping

6.3 RLControl State

• Start an independent inference thread on entry
• Inference thread runs at decimation_ (default 10) decimation, i.e., 2ms × 10 = 20ms per inference
• Each inference collects RobotBasicState (base RPY, angular velocity, acceleration, joint state) and user commands
• Policy outputs 21 joint actions (12 legs + 1 waist + 8 arms), neck maintains zero torque
  • Waist: WaistZ single DOF
  • Dual arms, 4 DOF each: ShoulderY, ShoulderX, ShoulderZ, Elbow (excluding WristZ/Y/X)
  • Dual legs, 6 DOF each: HipY, HipX, HipZ, Knee, AnkleY, AnkleX
• First 100 steps apply smooth transition for arm joints to avoid jumps
• Wait for inference thread to finish on exit

Policy Inference Details:

• Uses ONNX Runtime CPU inference
• Observation dimensions: 730 (10 frames × 73 dims/frame)
  • Single observation 73 dims: angular velocity(3) + gravity projection(3) + velocity command(3) + command flag(1) + joint position deviation(21) + joint velocity(21) + previous action(21)
• Action output: 21 dims (legs 12 + waist 1 + dual arms 8)

Joint Ordering Mapping:

The policy and robot use different joint orderings, with automatic permutation index calculation via name mapping:

Policy Output Order (rearranged to control order):

Waist(1, WaistZ) → Left Arm(4: ShoulderY/X/Z + Elbow) → Right Arm(4: ShoulderY/X/Z + Elbow) → Left Leg(6) → Right Leg(6)

Default Pose (in output_order, 21 dims):

dof_pos_default_ = [0, 0, 0.15, 0, 1.35, 0, -0.15, 0, 1.35,
                   -0.1, 0, 0, 0.2, -0.1, 0, -0.1, 0, 0, 0.2, -0.1, 0]

Policy Output Processing:

1. Clip to [-10, 10] range
2. Multiply by action_scale_ to get position delta
3. Add dof_pos_default_ to get target position
4. First 100 steps smooth arm joint transition (max 0.01 rad increment per step) to avoid jumps from sudden switching near zero position

Default Parameters:

Max command velocity (max_cmd_vel_): [1.0, 0.5, 1.2] (forward m/s, lateral m/s, rotation rad/s)
Velocity delta limit (vel_delta_const_): [0.02, 0.02, 0.08] (smooth transition)
Angular velocity scale (ang_vel_scale_): 0.2
Linear velocity scale (lin_vel_scale_): 2.0
Joint velocity scale (dof_vel_scale_): 0.1
Observation history frames (obs_history_num_): 10
Decision decimation (decimation_): 10 (2ms × 10 = 20ms per inference)

[!NOTE]

Velocity Command Nonlinearity: The backward velocity response is scaled to 0.6×

if (uc.forward_vel_scale < 0) user_cmd_vel[0] = 0.6f * uc.forward_vel_scale;

This means backward movement is more conservative than forward movement, with actual backward speed approximately 60% of forward speed.

6.4 JointDamping State

• Apply pure damping torque to all 31 joints (only set kd, kp = 0)
• Automatically return to Idle after 3 seconds
• Reset target_mode on exit