State Machine Overview and Implementation

Base State Machine

The base state machine StateMachineBase is responsible for periodic execution, state transitions, and control thread scheduling.

Run Example

run_thread_ = std::thread(&StateMachineBase::RunThread, this);
rclcpp::spin(ri_ptr_->get_node());

After Run() starts a separate control thread, the main thread blocks on rclcpp::spin to process ROS2 callbacks.

Timed Loop Example

set_timer.time_init(5);   // 5ms cycle period
while (rclcpp::ok()) {
    if (set_timer.time_interrupt()) {
        ri_ptr_->RefreshRobotData();
        current_controller_->Run();

        if (emergency_stop_requested_.exchange(false)) {
            next_state_name_ = StateName::kJointDamping;
        } else if (current_controller_->LoseControlJudge()) {
            next_state_name_ = StateName::kJointDamping;
        } else {
            next_state_name_ = current_controller_->GetNextStateName();
        }

        if (next_state_name_ != current_state_name_) {
            current_controller_->OnExit();
            current_controller_ = GetStateControllerPtr(next_state_name_);
            current_controller_->OnEnter();
            current_state_name_ = next_state_name_;
        }
    }
}

The timer is based on timerfd + epoll, triggering an interrupt every 5ms. Each cycle sequentially executes: refresh sensor data → run current state controller → emergency stop/loss-of-control check → state transition.

State Machine Abstraction

StateBase is the abstract class for all states, providing state enter, exit, run, and transition judgment interfaces.

State Interface

virtual void OnEnter() = 0;           // Executed when entering state
virtual void OnExit() = 0;            // Executed when exiting state
virtual void Run() = 0;               // Executed every control cycle
virtual bool LoseControlJudge() = 0;  // Loss-of-control judgment
virtual StateName GetNextStateName() = 0;  // Get next state

Shared Data

Each state shares the following pointers through 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
};

State Machine Implementation

QStateMachine is the concrete state machine implementation for Lite3, containing 4 control states.

Startup Example

if (robot_name_ == RobotName::Lite3) {
    ri_ptr_ = std::make_shared<Lite3Interface>("Lite3");
    cp_ptr_ = std::make_shared<ControlParameters>(robot_name_);
    joints_data_shadow_subscriber_ =
        std::make_shared<JointsDataShadowSubscriber>(ri_ptr_->get_node());
}

State Construction

idle_controller_ = std::make_shared<IdleState>(robot_name_, "idle_state", data_ptr);
           // Idle state (standby)
standup_controller_ = std::make_shared<StandUpState>(robot_name_, "standup_state", data_ptr);
           // Stand-up state
rl_controller_ = std::make_shared<RLControlState>(robot_name_, "rl_control", data_ptr);
           // RL control state
joint_damping_controller_ = std::make_shared<JointDampingState>(robot_name_, "joint_damping", data_ptr);
           // Joint damping state (safety protection)

State Transition

Buttons in UserCommandInterface directly set the target state:

usr_cmd_->target_mode = uint8_t(RobotMotionState::StandingUp);     // Y button
usr_cmd_->target_mode = uint8_t(RobotMotionState::RLControlMode);  // A button
usr_cmd_->target_mode = uint8_t(RobotMotionState::JointDamping);   // Both joystick buttons

Idle State (IdleState)

IdleState is the robot’s standby state, responsible for sensor validation and waiting for user commands.

Enter Behavior

void OnEnter() {
    msfb_.UpdateCurrentState(RobotMotionState::WaitingForStand);
    enter_state_time_ = ri_ptr_->GetInterfaceTimeStamp();
}

Periodic Execution

void Run() {
    GetProprioceptiveData();        // Read joint position/velocity/torque + IMU
    joint_normal_flag_ = JointDataNormalCheck();  // Joint data validation
    DisplayProprioceptiveInfo();    // Print proprioceptive info every second
    MatXf cmd = MatXf::Zero(12, 5);
    ri_ptr_->SetJointCommand(cmd);  // Send zero-torque commands
}

State Transitions

• Joint data abnormal → Stay in kIdle, print warning
• safe_control_mode != 0 → Stay in kIdle
• target_mode == StandingUp → Transition to kStandUp

Stand-Up State (StandUpState)

StandUpState controls the robot’s smooth transition from sitting to standing posture.

Standing Trajectory Planning

The standing process is divided into two phases, with total duration 2 × stand_duration_:

Phase 1 (0 ~ stand_duration_): Cubic spline interpolation from current joint position to pre-stand target position:

planning_joint_pos(i) = GetCubicSplinePos(init_pos, init_vel, goal_pos, 0, t, T);
planning_joint_vel(i) = GetCubicSplineVel(init_pos, init_vel, goal_pos, 0, t, T);

Phase 2 (stand_duration_ ~ 2×stand_duration_): Interpolation from pre-stand height to final standing height, computing HipY and Knee angles via inverse kinematics:

float hipy_pos = GetHipYPosByHeight(plan_height);
float knee_pos = GetKneePosByHeight(plan_height);

State Transitions

• safe_control_mode != 0 → kJointDamping
• run_time > 2 × stand_duration_ and target_mode == RLControlMode → kRLControl
• Otherwise stay in kStandUp

RL Control State (RLControlState)

RLControlState runs the reinforcement learning policy, executing ONNX inference in a separate thread.

Policy Initialization

auto model_path = fs::canonical(base / ".." / ".." / "policy" / "policy.onnx");
lite3_policy_ = std::make_shared<Lite3PolicyRunner>("lite3_policy", model_path.string());
policy_ptr_ = lite3_policy_;

Inference Thread

void PolicyRunner() {
    while (start_flag_) {
        if (state_run_cnt_ % policy_ptr_->decimation_ == 0) {
            auto ra = policy_ptr_->getRobotAction(rbs_, *(uc_ptr_->GetUserCommand()));
            ri_ptr_->SetJointCommand(ra.ConvertToMat());
        }
        std::this_thread::sleep_for(std::chrono::microseconds(100));
    }
}

The inference thread runs at decimation_ frequency division. Each iteration reads RobotBasicState (RPY, angular velocity, acceleration, joint state) and user commands, outputting 12 joint commands.

Exit Cleanup

void OnExit() {
    start_flag_ = false;
    run_policy_thread_.join();  // Wait for inference thread to exit
    state_run_cnt_ = -1;
}

Damping State (JointDampingState)

JointDampingState is the safety protection state, applying pure damping torque to all joints for safe robot landing.

Damping Command

JointDampingState(...) {
    VecXf kd_ = one_leg_kd.replicate(4, 1);
    joint_cmd_ = MatXf::Zero(12, 5);
    joint_cmd_.col(2) = kd_;   // Only set kd, kp=0 means pure damping
}

State Transition

StateName GetNextStateName() {
    if (run_time_ - time_record_ < 3.) return StateName::kJointDamping;
    return StateName::kIdle;
}

After 3 seconds of damping, automatically returns to kIdle. On transition, resets target_mode to WaitingForStand.

Safety Control Logic

SafeController runs in a separate thread at 1ms period, monitoring robot health status.

Anomaly Detection Items

Detection Item	Method	Trigger Condition	safe_control_mode
Driver error	IsDriverStatusNormal()	status_word != 1	2
Joint data anomaly	IsJointDataNormal()	NaN values	3
Motor over-temperature	IsMotorTempertureNormal()	Temperature > 100°C	2
IMU data anomaly	IsImuDataNormal()	NaN / timeout 0.2s	2
Low battery	IsBatteryNormal()	Level < 10%	—
Gamepad heartbeat timeout	IsUserCommandNormal()	Timeout 1000ms	—

When safe_control_mode != 0, each state’s GetNextStateName() returns kJointDamping, and the state machine automatically enters safety damping.

Error Status Codes

union RobotErrorState {
    struct {
        uint32_t imu_num_error: 1;          // IMU value anomaly
        uint32_t imu_update_overtime: 1;    // IMU update timeout
        uint32_t command_heartbeat_overtime: 1;  // Gamepad heartbeat timeout
        uint32_t driver_error: 1;           // Driver error
        uint32_t motor_heat_warn: 1;        // Motor over-temperature
        uint32_t joint_num_error: 1;        // Joint value anomaly
        uint32_t joint_update_error: 1;     // Joint update timeout
        uint32_t battery_low_warn: 1;       // Low battery
        uint32_t battery_heat_warn: 1;      // Battery over-temperature
        uint32_t cpu_heat_warn: 1;          // CPU over-temperature
        uint32_t cpu_freq_warn: 1;          // CPU frequency throttling
    };
    uint32_t error_code;
};

Cross-Process Emergency Stop

Emergency stop is implemented through a three-layer chain:

1. UDP command layer: lite3_sdk_service receives estop command, calls /EMERGENCY_STOP service
2. Hardware layer: lite3_transfer sends emergency stop command code 0x21010C0E to robot firmware
3. State machine layer: Notifies Lite3_sdk_deploy via /EMERGENCY_STOP_SIGNAL to switch to JointDamping

Emergency Stop Signal Publishing

// Publish emergency stop signal in lite3_transfer
estop_signal_pub_ = create_publisher<drdds::msg::StdMsgInt32>("/EMERGENCY_STOP_SIGNAL", 1);
drdds::msg::StdMsgInt32 sig;
sig.value = 1;
estop_signal_pub_->publish(sig);

Emergency Stop Signal Subscription

// Subscribe to emergency stop signal in Lite3_sdk_deploy
estop_signal_sub_ = ri_ptr_->get_node()->create_subscription<drdds::msg::StdMsgInt32>(
    "/EMERGENCY_STOP_SIGNAL", 1,
    [this](const drdds::msg::StdMsgInt32::SharedPtr msg) {
        emergency_stop_requested_.store(true);
    });