Safety Mode API

Overview

PLEM uses two independent state machines to manage robot safety and control:

1. SafetyMode - Hardware safety constraints (high priority)
2. RobotMode - User-controlled operational mode

SafetyMode constrains RobotMode. When a safety violation occurs, it immediately overrides user commands to protect the system.

State Transition Diagram

SafetyMode

SafetyMode represents hardware-level safety constraints and has higher priority than RobotMode.

Value	Meaning	Allowed Modes
NORMAL	No safety violations	All RobotModes allowed
P_STOP	Protective stop (recoverable)	Only BRAKED, FREEDRIVE allowed
HW_FAULT	Hardware fault (critical)	Only BRAKED allowed

P_STOP (Protective Stop)

Automatically triggered under the following conditions:

• Joint velocity or position limit exceeded
• Control loop timing violation (jitter > 200µs)

Action on occurrence: Immediately forces the robot into BRAKED mode, engaging all motor brakes.

Recovery: Automatically returns to NORMAL state when the violation condition is cleared. No user intervention required.

HW_FAULT (Hardware Fault)

Triggered when critical hardware issues occur:

• Servo drive fault (CiA 402 FAULT state)
• EtherCAT communication loss
• Emergency stop button activated

Recovery: After resolving the hardware issue, you must call the reset_error() function.

// HW_FAULT recovery example
if (robot->safety_mode() == SafetyMode::HW_FAULT) {
    // 1. Resolve hardware issue (release emergency stop, etc.)
    // 2. Reset error
    robot->reset_error();

    // 3. Wait for NORMAL return
    while (robot->safety_mode() != SafetyMode::NORMAL) {
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
}

RobotMode

RobotMode is the robot's operational mode controlled by the user.

Value	Meaning	Brake State
BRAKED	Brakes engaged	Engaged
STOPPING	Transition state (internal)	Released
TRAJECTORY	Autonomous trajectory execution	Released
FREEDRIVE	Manual guide mode	Released

BRAKED

All joints' mechanical brakes are engaged. The robot maintains a stopped state and resists external forces.

• Default mode on initial boot
• Target mode for forced transition on safety violation

STOPPING

An intermediate state during mode transitions, an internal transition mode that users cannot directly set.

• Automatically entered when set_mode() is called
• Completed to target mode by calling complete_stopping()

TRAJECTORY

Autonomous trajectory execution mode. See Control Tuning Guide and Action Server.

FREEDRIVE

Manual guide mode where users can directly move the robot. Gravity compensation is activated, allowing manipulation with light force.

Core API (5 Functions)

1. safety_mode()

Queries the current SafetyMode.

SafetyMode mode = robot->safety_mode();

2. set_mode()

Initiates a RobotMode transition. Immediately enters STOPPING mode upon call.

robot->set_mode(RobotMode::TRAJECTORY);

3. is_moving()

Checks if the robot is in mode transition (STOPPING state).

bool moving = robot->is_moving();

4. complete_stopping()

Completes the STOPPING state and transitions to the target mode.

robot->complete_stopping();

Must be called

If you don't call complete_stopping() after calling set_mode(), the robot will wait indefinitely in the STOPPING state. This is the most common mistake.

5. reset_error()

Resets the HW_FAULT state and attempts to return to NORMAL.

robot->reset_error();

Safe Mode Transition Pattern

All mode transitions should follow this 3-step pattern:

// 1. Start mode transition
robot->set_mode(target_mode);

// 2. Wait for STOPPING completion
while (robot->is_moving()) {
    std::this_thread::sleep_for(std::chrono::milliseconds(10));
}

// 3. Complete transition
robot->complete_stopping();

Sleep required in polling loop

When polling is_moving(), you must include a short sleep to reduce CPU usage. 10ms recommended.

Transition Rules and Anti-patterns

Starting Mode	Target Mode	Transition Allowed	Notes
BRAKED	TRAJECTORY	✅ Allowed	SafetyMode must be NORMAL
BRAKED	FREEDRIVE	✅ Allowed	SafetyMode must be P_STOP or better
TRAJECTORY	BRAKED	✅ Allowed	Always allowed
TRAJECTORY	FREEDRIVE	✅ Allowed	Must go through STOPPING first
FREEDRIVE	TRAJECTORY	✅ Allowed	Must go through STOPPING first

Anti-pattern: Missing complete_stopping()

// ❌ Incorrect code
robot->set_mode(RobotMode::TRAJECTORY);
// No complete_stopping() → stuck in STOPPING state forever

// ✅ Correct code
robot->set_mode(RobotMode::TRAJECTORY);
while (robot->is_moving()) {
    std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
robot->complete_stopping();

Anti-pattern: Missing SafetyMode pre-check

// ❌ Incorrect code
robot->set_mode(RobotMode::TRAJECTORY);  // Fails in P_STOP state

// ✅ Correct code
if (robot->safety_mode() != SafetyMode::NORMAL) {
    throw std::runtime_error("SafetyMode is not NORMAL");
}
robot->set_mode(RobotMode::TRAJECTORY);

Anti-pattern: Missing sleep in polling loop

// ❌ Incorrect code (100% CPU usage)
while (robot->is_moving()) {
    // No sleep
}

// ✅ Correct code
while (robot->is_moving()) {
    std::this_thread::sleep_for(std::chrono::milliseconds(10));
}

Complete Example

Here's a complete mode transition example with all safety checks:

#include <plem/robot.hpp>
#include <chrono>
#include <thread>
#include <stdexcept>

void safe_mode_transition(plem::Robot* robot, plem::RobotMode target_mode) {
    // 1. Check SafetyMode
    if (robot->safety_mode() == plem::SafetyMode::HW_FAULT) {
        throw std::runtime_error("Hardware fault state: reset_error() required");
    }

    if (target_mode == plem::RobotMode::TRAJECTORY) {
        if (robot->safety_mode() != plem::SafetyMode::NORMAL) {
            throw std::runtime_error("TRAJECTORY mode requires NORMAL state");
        }
    }

    // 2. Start mode transition
    robot->set_mode(target_mode);

    // 3. Wait for STOPPING completion
    while (robot->is_moving()) {
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }

    // 4. Complete transition
    robot->complete_stopping();

    // 5. Final verification
    if (robot->mode() != target_mode) {
        throw std::runtime_error("Mode transition failed");
    }
}

int main() {
    auto robot = plem::Robot::create("robot_config.yaml");

    try {
        // BRAKED → TRAJECTORY transition
        safe_mode_transition(robot.get(), plem::RobotMode::TRAJECTORY);

        // Execute trajectory...

        // TRAJECTORY → BRAKED return
        safe_mode_transition(robot.get(), plem::RobotMode::BRAKED);

    } catch (const std::exception& e) {
        std::cerr << "Error: " << e.what() << std::endl;

        // On error, safely return to BRAKED
        if (robot->mode() != plem::RobotMode::BRAKED) {
            robot->set_mode(plem::RobotMode::BRAKED);
            while (robot->is_moving()) {
                std::this_thread::sleep_for(std::chrono::milliseconds(10));
            }
            robot->complete_stopping();
        }

        return 1;
    }

    return 0;
}

Troubleshooting

Problem: Mode doesn't change after calling set_mode()

Cause: Stuck in STOPPING state without calling complete_stopping()

Solution:

robot->set_mode(target_mode);
while (robot->is_moving()) {
    std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
robot->complete_stopping();  // Required!

Problem: Immediately returns to BRAKED when transitioning to TRAJECTORY mode

Cause: SafetyMode is in P_STOP state, TRAJECTORY mode not allowed

Solution:

// Check SafetyMode before transition
if (robot->safety_mode() != SafetyMode::NORMAL) {
    std::cerr << "Warning: SafetyMode is not NORMAL" << std::endl;
    return;
}
robot->set_mode(RobotMode::TRAJECTORY);

Problem: Cannot recover from HW_FAULT state

Cause: Called reset_error() without resolving the hardware issue

Solution:

1. Release emergency stop button
2. Check EtherCAT connection
3. Check servo drive status
4. Call reset_error()

// After resolving hardware issue
robot->reset_error();
while (robot->safety_mode() == SafetyMode::HW_FAULT) {
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
}

Problem: 100% CPU usage in polling loop

Cause: Busy-wait in is_moving() polling without sleep

Solution:

while (robot->is_moving()) {
    std::this_thread::sleep_for(std::chrono::milliseconds(10));
}

Next Steps

• Learn PID control tuning methods for TRAJECTORY mode in the Control Tuning Guide
• Learn ROS 2 Action-based trajectory execution patterns in Action Server

Note

All mode transitions occur in the real-time control loop, so timing is critical. A polling interval of 10ms is recommended; setting it below 1ms can cause unnecessary CPU load.