본문으로 건너뛰기

안전 모드 API

개요​

PLEM은 두 개의 독립적인 상태 머신을 사용하여 로봇의 안전성과 제어를 관리합니다:

  1. SafetyMode - 하드웨어 안전 제약 (우선순위 높음)
  2. RobotMode - 사용자 제어 작동 모드

SafetyMode는 RobotMode를 제한합니다. 안전 위반 발생 시 사용자 명령을 즉시 재정의하여 시스템을 보호합니다.

상태 전이 다이어그램​

SafetyMode​

SafetyMode는 하드웨어 수준의 안전 제약을 나타내며, RobotMode보다 높은 우선순위를 가집니다.

값의미허용 모드
NORMAL안전 위반 없음모든 RobotMode 허용
P_STOP보호 정지 (복구 가능)BRAKED, FREEDRIVE만 허용
HW_FAULT하드웨어 폴트 (심각)BRAKED만 허용

P_STOP (보호 정지)​

다음 조건에서 자동으로 트리거됩니다:

  • 조인트 속도 또는 위치 제한 초과
  • 제어 루프 타이밍 위반 (지터 > 200µs)

발생 시 동작: 로봇을 즉시 BRAKED 모드로 강제 전환하여 모든 모터 브레이크를 체결합니다.

복구: 위반 조건이 해제되면 자동으로 NORMAL 상태로 복귀합니다. 사용자 개입 불필요.

HW_FAULT (하드웨어 폴트)​

다음과 같은 심각한 하드웨어 문제 발생 시 트리거됩니다:

  • 서보 드라이브 폴트 (CiA 402 FAULT 상태)
  • EtherCAT 통신 손실
  • 비상 정지 버튼 활성화

복구: 하드웨어 문제를 해결한 후 반드시 reset_error() 함수를 호출해야 합니다.

// HW_FAULT 복구 예제
if (robot->safety_mode() == SafetyMode::HW_FAULT) {
    // 1. 하드웨어 문제 해결 (비상 정지 해제 등)
    // 2. 에러 리셋
    robot->reset_error();

    // 3. NORMAL 복귀 대기
    while (robot->safety_mode() != SafetyMode::NORMAL) {
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
}

RobotMode​

RobotMode는 사용자가 제어하는 로봇의 작동 모드입니다.

값의미브레이크 상태
BRAKED브레이크 체결 상태체결
STOPPING전이 상태 (내부)해제
TRAJECTORY자율 궤적 실행해제
FREEDRIVE수동 가이드 모드해제

BRAKED​

모든 조인트의 기계식 브레이크가 체결된 상태입니다. 로봇은 정지 상태를 유지하며 외력에 저항합니다.

  • 초기 부팅 시 기본 모드
  • 안전 위반 시 강제 전환 대상

STOPPING​

모드 전이 중간 상태로, 사용자가 직접 설정할 수 없는 내부 전이 모드입니다.

  • set_mode() 호출 시 자동 진입
  • complete_stopping() 호출로 목표 모드 완료

TRAJECTORY​

자율 궤적 실행 모드입니다. 제어 튜닝 가이드와 Action Server를 참고하세요.

FREEDRIVE​

수동 가이드 모드로, 사용자가 로봇을 직접 움직일 수 있습니다. 중력 보상이 활성화되어 가벼운 힘으로 조작 가능합니다.

핵심 API (5개 함수)​

1. safety_mode()​

현재 SafetyMode를 조회합니다.

SafetyMode mode = robot->safety_mode();

2. set_mode()​

RobotMode 전이를 시작합니다. 호출 즉시 STOPPING 모드로 진입합니다.

robot->set_mode(RobotMode::TRAJECTORY);

3. is_moving()​

로봇이 모드 전이 중(STOPPING 상태)인지 확인합니다.

bool moving = robot->is_moving();

4. complete_stopping()​

STOPPING 상태를 완료하고 목표 모드로 전환합니다.

robot->complete_stopping();
반드시 호출해야 합니다

set_mode() 호출 후 complete_stopping()을 호출하지 않으면 로봇이 STOPPING 상태에서 무한정 대기합니다. 이는 가장 흔한 실수입니다.

5. reset_error()​

HW_FAULT 상태를 리셋하고 NORMAL로 복귀를 시도합니다.

robot->reset_error();

안전한 모드 전이 패턴​

모든 모드 전이는 다음 3단계 패턴을 따라야 합니다:

// 1. 모드 전이 시작
robot->set_mode(target_mode);

// 2. STOPPING 완료 대기
while (robot->is_moving()) {
    std::this_thread::sleep_for(std::chrono::milliseconds(10));
}

// 3. 전이 완료
robot->complete_stopping();
폴링 루프에서 sleep 필수

is_moving() 폴링 시 반드시 짧은 sleep을 포함해야 CPU 사용률을 낮출 수 있습니다. 10ms 권장.

전이 규칙과 안티패턴​

시작 모드목표 모드전이 가능 여부비고
BRAKEDTRAJECTORY✅ 가능SafetyMode가 NORMAL이어야 함
BRAKEDFREEDRIVE✅ 가능SafetyMode가 P_STOP 이하여야 함
TRAJECTORYBRAKED✅ 가능항상 가능
TRAJECTORYFREEDRIVE✅ 가능먼저 STOPPING을 거쳐야 함
FREEDRIVETRAJECTORY✅ 가능먼저 STOPPING을 거쳐야 함
안티패턴: complete_stopping() 누락
// ❌ 잘못된 코드
robot->set_mode(RobotMode::TRAJECTORY);
// complete_stopping() 없음 → 영원히 STOPPING 상태

// ✅ 올바른 코드
robot->set_mode(RobotMode::TRAJECTORY);
while (robot->is_moving()) {
    std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
robot->complete_stopping();
안티패턴: SafetyMode 사전 확인 누락
// ❌ 잘못된 코드
robot->set_mode(RobotMode::TRAJECTORY);  // P_STOP 상태에서 실패

// ✅ 올바른 코드
if (robot->safety_mode() != SafetyMode::NORMAL) {
    throw std::runtime_error("SafetyMode가 NORMAL이 아님");
}
robot->set_mode(RobotMode::TRAJECTORY);
안티패턴: 폴링 루프에서 sleep 누락
// ❌ 잘못된 코드 (CPU 100% 사용)
while (robot->is_moving()) {
    // sleep 없음
}

// ✅ 올바른 코드
while (robot->is_moving()) {
    std::this_thread::sleep_for(std::chrono::milliseconds(10));
}

완전한 예제​

다음은 모든 안전 체크를 포함한 완전한 모드 전이 예제입니다:

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

void safe_mode_transition(plem::Robot* robot, plem::RobotMode target_mode) {
    // 1. SafetyMode 확인
    if (robot->safety_mode() == plem::SafetyMode::HW_FAULT) {
        throw std::runtime_error("하드웨어 폴트 상태: reset_error() 필요");
    }

    if (target_mode == plem::RobotMode::TRAJECTORY) {
        if (robot->safety_mode() != plem::SafetyMode::NORMAL) {
            throw std::runtime_error("TRAJECTORY 모드는 NORMAL 상태 필요");
        }
    }

    // 2. 모드 전이 시작
    robot->set_mode(target_mode);

    // 3. STOPPING 완료 대기
    while (robot->is_moving()) {
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }

    // 4. 전이 완료
    robot->complete_stopping();

    // 5. 최종 검증
    if (robot->mode() != target_mode) {
        throw std::runtime_error("모드 전이 실패");
    }
}

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

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

        // 궤적 실행...

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

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

        // 에러 발생 시 안전하게 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;
}

트러블슈팅​

문제: set_mode() 호출 후 모드가 변경되지 않음​

원인: complete_stopping()을 호출하지 않아 STOPPING 상태에서 멈춤

해결:

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

문제: TRAJECTORY 모드 전환 시 즉시 BRAKED로 복귀​

원인: SafetyMode가 P_STOP 상태여서 TRAJECTORY 모드가 허용되지 않음

해결:

// SafetyMode 확인 후 전환
if (robot->safety_mode() != SafetyMode::NORMAL) {
    std::cerr << "경고: SafetyMode가 NORMAL이 아님" << std::endl;
    return;
}
robot->set_mode(RobotMode::TRAJECTORY);

문제: HW_FAULT 상태에서 복구 불가​

원인: 하드웨어 문제를 해결하지 않고 reset_error() 호출

해결:

  1. 비상 정지 버튼 해제
  2. EtherCAT 연결 확인
  3. 서보 드라이브 상태 확인
  4. reset_error() 호출
// 하드웨어 문제 해결 후
robot->reset_error();
while (robot->safety_mode() == SafetyMode::HW_FAULT) {
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
}

문제: 폴링 루프에서 CPU 사용률 100%​

원인: is_moving() 폴링 시 sleep 없이 busy-wait

해결:

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

다음 단계​

참고

모든 모드 전이는 실시간 제어 루프에서 발생하므로 타이밍이 중요합니다. 폴링 간격은 10ms를 권장하며, 1ms 미만으로 설정하면 불필요한 CPU 부하가 발생할 수 있습니다.