본 페이지는 기존 "Low Level Control" 페이지를 Control Mode 로 개편한 것으로,
IGRIS-C가 지원하는 각 제어 모드의 의미와 전환 방법, 그리고 저수준(Low-Level) 제어에 필요한 메시지 인터페이스를 정리합니다.

⚠️ 이 페이지의 코드는 v2.1.1 SDK 기준입니다. 2.1.1에서 토픽 이름(rt/ 삭제), 타입 이름, 상수 이름(enum → uint8 상수), BMS → PDU 용어가 바뀌었으며, 2.1.0 SDK 클라이언트는 2.1.1 로봇과 통신되지 않습니다(SDK Overview 참고).

• IGRIS-C의 동작 계층은 Control Mode 로 전환합니다. 모드 전환 명령은 SDK의 IgrisC_Client::SendControlModeCommand() (내부적으로 ControlModeCommandRequest Request/Response)로 컨트롤러에 전달되며, 컨트롤러는 이를 내부 RobotControlMode 로 매핑해 제어 루프 동작을 바꿉니다.

struct ControlModeCommandRequest_Request {
  std_msgs/Header header;
  string          request_id;    // 요청 매칭용 ID
  uint8           command_type;  // 전환할 모드 (아래 목록, CONTROL_MODE_CMD_* 상수)
  string          preset_id;     // MOTION_PRESET 전용
  boolean         is_cyclic;     // MOTION_PRESET_CYCLIC_TOGGLE 전용
};
// 응답: ControlModeCommandRequest_Response { header, request_id, success, message, error_code(0=성공) }

1. Control Mode 목록

command_type (uint8 상수, 총 19종, 값 0~18). 컨트롤러 동작은 robotHandler 디스패치 기준입니다.

command_type 값은 igris_c_sdk::srv::dds_::ControlModeCommandRequest_Request_Constants::CONTROL_MODE_CMD_<NAME> 형식의 uint8 상수입니다
(예: CONTROL_MODE_CMD_MOTION_PRESET). Python에서는 igc_sdk.CONTROL_MODE_CMD_<NAME> 입니다.

⚠️ CUSTOM_MODE_1~CUSTOM_MODE_10 (9~18)은 현재 사용자에게 제공되지 않습니다.
enum에 슬롯만 예약되어 있는 내부/확장 전용 항목이며, SDK 사용 시 사용하지 마세요.

아래 두 모드는 명령으로 요청할 수 없고, 컨트롤러가 조건에 따라 스스로 진입합니다(상세는 2절 "자동 안전 모드").

2. 각 모드 설명

저수준 제어 (Low-Level)

• SDK가 lowcmd 로 각 모터의 목표(q·dq·tau·kp·kd)를 직접 지령합니다. 제어 루프 책임이
  외부(SDK)에 있는 모드입니다. (메시지 인터페이스는 5절 참고)

  • LOW_LEVEL_JOINT_CONTROL (5) — LOW_LEVEL_MODE. SDK LowCmd로 관절을 직접 제어.

  • LOW_LEVEL_WALKMODE_ON (6) — LOW_LEVEL_WALK_MODE. 저수준 제어 기반 보행 모드.

• MS(모터 공간) 저수준 명령은 병렬 링크의 모터 공간 안전 영역으로 투영된 뒤 적용됩니다. lowstate로 보고되는 측정값은 투영하지 않으므로, 실제 자세에 따라 관절 한계를 넘는 값이 보고될 수 있습니다.

• 외부 handcmd는 LOW_LEVEL_MODE·LOW_LEVEL_WALK_MODE에서만 반영됩니다(HIGH_LEVEL 계열은 프리셋의 손 동작, 그 외 모드는 무시).

고수준 제어 (High-Level)

• 컨트롤러 내장 로직이 상위 명령을 받아 모션을 생성·유지합니다. 모션 프리셋은 이 계층에서 실행되며,
  다른 모드에서 프리셋을 요청하면 이 계층으로 자동 전환된 뒤 시작됩니다(아래 "모션 프리셋" 참고).

  • HIGH_LEVEL_JOINT_CONTROL (8) — HIGH_LEVEL_MODE. 컨트롤러 고수준 관절 제어.

  • HIGH_LEVEL_WALKMODE_ON (7) — HIGH_LEVEL_WALK_MODE. 고수준 보행 모드.

보행 (Walk)

• WALKMODE_ON (4) — WALK_MODE. 보행 모드 진입.

• 보행 속도 명령은 cmd_vel 토픽(geometry_msgs/TwistStamped, 모든 레인 공통)으로 보냅니다. 예제: cyclonedds_twist_publisher, twist_publisher.py, ros_twist_publisher.

• ⚠️ 2.1.1부터 WALKMODE_ON 진입 시 팔을 천천히 옮기던 슬로스타트(2.1.0: 0.4 rad/s, 3초)가 없습니다. 진입 즉시 보행 정책이 전신을 제어하므로 로봇 주변, 특히 팔 동작 범위를 비운 뒤 요청하십시오. DAMPING_MODE에서도 WALKMODE_ON 요청은 거부되지 않으므로, 로봇이 바로 선 자세인지 확인한 뒤 요청하십시오.

모션 프리셋 (Motion Preset)

• MOTION_PRESET (0) — preset_id 로 지정한 사전 정의 모션을 재생(startMotionPreset).
  HIGH_LEVEL_MODE 또는 HIGH_LEVEL_WALK_MODE 에서 실행됩니다. 다른 모드에서 요청하면 LOW_LEVEL_WALK_MODE·WALK_MODE에서는 HIGH_LEVEL_WALK_MODE로, 그 외 모드에서는 HIGH_LEVEL_MODE로 자동 전환된 뒤 시작됩니다. 정지 상태(아래)에서는 HOME만 실행할 수 있습니다.
  요청 시 preset_id 인자를 채웁니다.

• MOTION_PRESET_CYCLIC_TOGGLE (1) — 프리셋 반복(cyclic) 재생 여부를 토글(setMotionPresetCyclic).
  요청 시 is_cyclic 인자를 사용합니다.

정지 / 홀드 (Stop / Hold)

• JOINT_POSITION_HOLD (2) — JOINT_HOLD_MODE. 현재 관절 위치를 유지(홀드)합니다. 요청이 성공하면 로봇은 아래의 정지 상태가 됩니다.

• MOTION_STOP (3) — 진행 중인 모션을 멈추고 현재 자세를 유지(holdCurrentPosition).
  HIGH_LEVEL_MODE/HIGH_LEVEL_WALK_MODE 에서만 지원됩니다.

정지 상태 (Motion-Stop 래치)

• MOTION_STOP 또는 JOINT_POSITION_HOLD가 성공하면 로봇은 정지 상태로 고정되며, 이때 control_state는 STOP입니다(2.1.0은 HIGH_MOTION_STANDBY/IDLE).

• 정지 상태에서 허용되는 요청은 MOTION_PRESET "HOME"과, 안전 정지 계열 모드(JOINT_HOLD_MODE·PASSIVE_MODE·DAMPING_MODE)로의 전환뿐입니다(PASSIVE·DAMPING은 자동 진입). 그 밖의 모드 전환과 프리셋은 거부되며 응답 message에 이유가 담깁니다. (2.1.0은 HOME만 허용)

• HOME 프리셋이 시작되면 정지 상태가 해제됩니다. 필요하면 HIGH_LEVEL_MODE(또는 HIGH_LEVEL_WALK_MODE)로 자동 전환된 뒤 실행됩니다.

자동 안전 모드 (DAMPING / PASSIVE)

• DAMPING_MODE — kp=0, kd만 유지하는 안전 정지 모드입니다. SDK나 앱으로 직접 요청할 수 없으며, WALK_MODE·HIGH_LEVEL_WALK_MODE·LOW_LEVEL_WALK_MODE에서 보행 정책이 낙상을 감지하면 자동으로 진입합니다(기본 정책 기준: 골반 기울기 0.8 rad 초과, 또는 0.4 rad 초과 + 각속도 1.57 rad/s 초과). control_state는 STOP입니다.

• ⚠️ DAMPING_MODE 전환은 호이스트 거치 여부와 관계없이 위 기울기 기준으로만 판단합니다. 호이스트에 매단 상태에서도 기울기가 기준을 넘으면 전환되어 로봇이 늘어집니다.

• PASSIVE_MODE — 명령을 내보내지 않는 대기 모드입니다(2.1.0의 TORQUE_OFF_MODE). 모드 전환은 몸체 전원과 토크가 켜져 있고 비상정지가 해제된 상태에서만 받아들여지며, 그렇지 않은 상태에서 요청하면 거부되고 컨트롤러는 PASSIVE_MODE로 전환됩니다.

사용자 정의 (Custom) — 현재 미제공

• CUSTOM_MODE_1~CUSTOM_MODE_10 (9~18) — 현재 사용자에게 제공되지 않는 내부/확장 전용 슬롯입니다.
  enum에는 정의되어 있으나 SDK 사용자가 호출할 수 있는 모드가 아니므로, 일반 제어에는 사용하지 마세요.

• 참고: 2.1.1에서 이 슬롯들의 안전 프로파일이 바뀌었습니다. 허리·발목 병렬 링크는 SATURATE + SOFT, 손목은 CLIP + HARD입니다(2.1.0은 모두 CLIP + HARD).

3. 모드 전환 API (SDK)

모드 전환은 IgrisC_Client::SendControlModeCommand() 한 메서드로 수행합니다.

igris_c_sdk::msg::dds_::ServiceResponse_ SendControlModeCommand(
    uint8_t                command_type,       // ControlModeCommandRequest_Request_Constants::CONTROL_MODE_CMD_*
    const std::string&     preset_id = "",     // MOTION_PRESET 전용
    bool                   is_cyclic = false,  // MOTION_PRESET_CYCLIC_TOGGLE 전용
    int                    timeout_ms = 5000);

사용 예 (cyclonedds/cyclonedds_service.cpp 기준):

using namespace igris_c_sdk::srv::dds_;   // ControlModeCommandRequest_Request_Constants
// 모션 프리셋 "HOME" 실행 (HIGH_LEVEL 계층에서)
res = client.SendControlModeCommand(
    ControlModeCommandRequest_Request_Constants::CONTROL_MODE_CMD_MOTION_PRESET, "HOME", false, 60000);
// 현재 위치 홀드
res = client.SendControlModeCommand(
    ControlModeCommandRequest_Request_Constants::CONTROL_MODE_CMD_JOINT_POSITION_HOLD, "", false, 60000);
// 진행 중 모션 정지
res = client.SendControlModeCommand(
    ControlModeCommandRequest_Request_Constants::CONTROL_MODE_CMD_MOTION_STOP, "", false, 60000);
// 저수준 관절 제어 모드로 전환 (이후 lowcmd 사용)
res = client.SendControlModeCommand(
    ControlModeCommandRequest_Request_Constants::CONTROL_MODE_CMD_LOW_LEVEL_JOINT_CONTROL, "", false, 60000);

리턴 ServiceResponse 의 success / error_code(0=성공) / message 로 결과를 확인합니다.

• 이미 같은 모드인 상태에서 같은 모드를 다시 요청하면 success=false("Control mode is already …")가 반환됩니다.

• 모드가 바뀔 때 이전 명령에서 새 모드의 명령으로 0.5초 동안 부드럽게 넘어갑니다. PASSIVE_MODE·JOINT_HOLD_MODE·DAMPING_MODE로 들어가거나 나올 때는 즉시 전환됩니다.

4. 서비스 토픽 정리

SDK 클라이언트가 사용하는 Request/Response 서비스 채널:

cyclonedds 레인(cyclonedds_native / cyclonedds_ros_compatible)에서 각 서비스는 <토픽>/request(요청 발행) · <토픽>/response(응답 구독) 쌍으로 동작합니다.
예: service/pdu_init/request · service/pdu_init/response. (위 표는 base 토픽)

• 서비스 제공 방식은 로봇의 전송 모드(SDK Overview 3절)에 따라 다릅니다. cyclonedds 레인은 위의 request/response 토픽 쌍이며 IgrisC_Client로 호출합니다. 이 토픽 쌍은 ROS 2 서비스가 아니므로 cyclonedds_ros_compatible에서도 ros2 service list에 보이지 않습니다.

• ros 레인에서는 같은 이름의 실제 rclcpp 서비스(/<ns>/service/pdu_init 등, 타입 igris_c_sdk/srv/PduInitCmd · TorqueCmd · HandInitRequest · ControlModeCommandRequest · MujocoSimCmd)가 제공되며, ros2/의 ros_service 예제로 호출합니다.

• 응답 타입은 서비스별입니다(PduInitCmd_Response_, TorqueCmd_Response_, HandInitRequest_Response_, ControlModeCommandRequest_Response_, MujocoSimCmd_Response_). 필드는 모두 header, request_id, success, message, error_code로 같으며, IgrisC_Client 메서드는 이를 공통 igris_c_sdk::msg::dds_::ServiceResponse_로 돌려줍니다.

저수준 스트림: lowcmd (LowCmd, 명령) / lowstate (LowState, 상태).

5. 저수준 제어 메시지 인터페이스

IGRIS-C Topic List

• Low-level Cmd & State (high frequency): lowcmd / lowstate

• Service State (low frequency): pdustate / robotstate

Common (igris_c_sdk::msg::dds_ / igris_c_sdk::srv::dds_)

const uint32 N_JOINTS = 31; // Fixed DOF Num
// Kinematic Space (MS=Motor Space/AB, PJS=Parallel Joint Space/PR)
// enum이 아니라 uint8 상수입니다 (LowCmd_Constants)
uint8 KINEMATIC_MODE_MS  = 0;
uint8 KINEMATIC_MODE_PJS = 1;
// 모터 제어는 HYBRID 모드 전용입니다. (이전 MotorMode { CURRENT, HYBRID } enum은 제거됨)

Command Side

struct MotorCmd {            // HYBRID 전용
  uint16 id;   // MS: motor index / PJS: virtual joint index
  float  q;    // Position (rad)
  float  dq;   // Velocity (rad/s)
  float  tau;  // Feedforward Torque (Nm)
  float  kp;   // kP
  float  kd;   // kD
};
struct LowCmd {
  std_msgs/Header header;
  uint8           kinematic_modes[5];  // parallel-link 그룹별: waist / L_ankle / R_ankle / L_wrist / R_wrist
  MotorCmd        motors[31];          // 고정 길이 31 DOF
};

// 사용 예: 전체 그룹을 동일 모드로 설정
cmd.kinematic_modes().fill(igris_c_sdk::msg::dds_::LowCmd_Constants::KINEMATIC_MODE_PJS);

State Side

// imu_state 는 커스텀 구조체가 아니라 표준 sensor_msgs/Imu 입니다:
//   std_msgs/Header header;
//   Quaternion  orientation;                  // 이름 있는 x/y/z/w 필드
//   double      orientation_covariance[9];
//   Vector3     angular_velocity;             // rad/s
//   double      angular_velocity_covariance[9];
//   Vector3     linear_acceleration;          // m/s^2
//   double      linear_acceleration_covariance[9];
// rpy 필드는 없습니다 - 필요하면 orientation 에서 계산하세요.
struct MotorState {
  float  q;             // Position (rad)
  float  dq;            // Velocity (rad/s)
  float  tau_est;       // Estimated Torque (Nm)
  int16  temperature;   // Motor Temperature (°C)
  uint32 status_bits;   // fault/limit 비트 플래그; 값은 MotorState 상수
};
struct JointState {
  float  q;            // PJS Position (rad)
  float  dq;           // PJS Velocity (rad/s)
  float  tau_est;      // PJS Estimated Torque (Nm)
  uint32 status_bits;  // Status Bits
};
struct LowState {
  std_msgs/Header  header;
  sensor_msgs/Imu  imu_state;         // Base IMU state (표준 타입)
  MotorState motor_state[N_JOINTS];   // MS raw state
  JointState joint_state[N_JOINTS];   // PJS derived state
};

• status_bits의 에러 플래그는 MotorState 메시지 상수(MOTOR_STALL_ERROR, OVER_CURRENT_ERROR 등)로 제공되며, C++ igris_c_sdk::msg::dds_::MotorState_Constants::*, Python igc_sdk.MOTOR_STALL_ERROR 등으로 씁니다. 비트 플래그이므로 같음 비교가 아니라 (status_bits & FLAG)로 검사합니다.

• IMU 값의 각도 단위는 radian입니다. 롤·피치·요가 필요하면 orientation 쿼터니언에서 계산합니다.

모든 타입은 igris_c_sdk::msg::dds_ / igris_c_sdk::srv::dds_ 네임스페이스(IDL의 module)에 정의됩니다. 실제 사용 시
igris_c_sdk::msg::dds_::TypeName_ 으로 참조합니다. 표준 타입은 std_msgs::msg::dds_::Header_, sensor_msgs::msg::dds_::Imu_ 입니다.

6. System & Mode Control API (초기화 / 토크 / 핸드)

#include <igris_c_sdk/channel_factory.hpp>
#include <igris_c_sdk/igris_c_client.hpp>
using namespace igris_c_sdk;
int main() {
    int domain_id = 0;
    ChannelFactory::Instance()->Init(domain_id, "igris_c_IG01");  // domain + 로봇 호기 namespace (SDK Overview 4절 참고)
    if (!ChannelFactory::Instance()->IsInitialized()) { return 1; }
    IgrisC_Client client;
    client.Init();
    client.SetTimeout(10.0f);  // 10초 타임아웃
    // ... 이후 서비스 호출
}

PDU / 모터 초기화

using namespace igris_c_sdk::srv::dds_;   // PduInitCmd_Request_Constants
igris_c_sdk::msg::dds_::ServiceResponse_ res = client.InitPdu(PduInitCmd_Request_Constants::PDU_INIT, 30000);   // PDU 전원 ON
res = client.InitPdu(PduInitCmd_Request_Constants::MOTOR_INIT, 30000);                                          // 모터만 초기화
res = client.InitPdu(PduInitCmd_Request_Constants::PDU_AND_MOTOR_INIT, 30000);                                  // 전원 ON + 모터 (전체 시퀀스)
res = client.InitPdu(PduInitCmd_Request_Constants::PDU_INIT_NONE, 30000);                                       // PDU 전원 OFF

• init_type (uint8, 상수는 PduInitCmd_Request_Constants) — bridge가 실제 처리하는 값:

  • PDU_INIT_NONE (0) — PDU 전원 OFF (setPduPowerAsync(false))

  • PDU_INIT (1) — PDU 전원 ON

  • MOTOR_INIT (2) — 모터 초기화

  • PDU_AND_MOTOR_INIT (3) — 전원 ON + 모터 초기화 (전체 시퀀스)

  • PDU_OFF (4) — PDU 전원 OFF (PDU_INIT_NONE과 동일하게 처리)

• timeout_ms — 타임아웃(ms, 기본 5000)

• 리턴 ServiceResponse_ — request_id, success, message, error_code(0=성공)

토크 제어

using namespace igris_c_sdk::srv::dds_;   // TorqueCmd_Request_Constants
igris_c_sdk::msg::dds_::ServiceResponse_ res = client.SetTorque(TorqueCmd_Request_Constants::TORQUE_ON, 30000);
res = client.SetTorque(TorqueCmd_Request_Constants::TORQUE_OFF, 30000);

• torque (uint8, 상수는 TorqueCmd_Request_Constants) — TORQUE_NONE(0, 예약) / TORQUE_ON(1) / TORQUE_OFF(2)

• timeout_ms — 타임아웃(ms, 기본 5000)

• 이미 토크가 켜져 있을 때 TORQUE_ON을 다시 요청하면 아무 동작 없이 성공(success=true)을 반환합니다. (2.1.0은 경고 후 토크 ON 시퀀스를 다시 실행)

핸드 초기화

igris_c_sdk::msg::dds_::ServiceResponse_ res = client.InitHand(30000);   // end effector 캘리브레이션/호밍

(핸드 제어 상세는 Hand 페이지 참고)

PDU 상태 조회 (pdustate, 저주기)

header, body_power(RELAY_OFF/ON), legs_power, estop, connect, battery(퍼센트, 0–100), pdu_init_state

• pdu_init_state — bit0 = PDU, bit1 = 모터: PDU_NOT_INITIALIZED(0) / PDU_INITIALIZED(1) / MOTOR_INITIALIZED(2) / BOTH_INITIALIZED(3). 상수는 igris_c_sdk::msg::dds_::PduState_Constants(Python igc_sdk.BOTH_INITIALIZED 등)입니다.

제어 상태 조회 (robotstate)

RobotState의 state(control_state)는 현재 컨트롤러 모드와 프리셋 재생 상태로부터 계산된 요약 값입니다. 상수는 igris_c_sdk::msg::dds_::RobotState_Constants::ROBOT_STATE_*(Python igc_sdk.ROBOT_STATE_*)이며, environment는 ROBOT_ENV_REAL(0) / ROBOT_ENV_SIM(1)입니다.

7. Python binding

igris_c_sdk_public 저장소의 dist/*.whl 을 Python 환경에 설치하면 C++ 과 동일한
getter/setter 방식으로 사용할 수 있습니다. 예제는
python 폴더에 있습니다.

• 메시지 클래스 이름은 C++과 같이 _로 끝납니다(igc_sdk.LowCmd_() 등). 상수는 모듈 상수(igc_sdk.PDU_INIT, igc_sdk.TORQUE_ON, igc_sdk.CONTROL_MODE_CMD_*, igc_sdk.KINEMATIC_MODE_PJS 등)입니다.

• Python에서는 InitPdu / SetTorque / InitHand의 timeout_ms가 필수 인자입니다(기본값 없음).

import igris_c_sdk as igc_sdk
channel_instance = igc_sdk.ChannelFactory.Instance()
channel_instance.Init(0, "igris_c_IG01")   # domain_id + 로봇 호기 namespace (SDK Overview 4절 참고)
# 로봇이 cyclonedds_ros_compatible 이면:
# channel_instance.Init(0, "igris_c_IG01", "", igc_sdk.parse_topic_naming("ros_compatible"))
client = igc_sdk.IgrisC_Client()
client.Init()
client.SetTimeout(20.0)

lowstate 실시간 구독

lowstate_subscriber = igc_sdk.LowStateSubscriber("lowstate", igc_sdk.QosProfile.SensorData())
def lowstate_callback(msg):
    imu = msg.imu_state()
    q    = imu.orientation()            # q.x() q.y() q.z() q.w()
    gyro = imu.angular_velocity()
    acc  = imu.linear_acceleration()    # rpy 없음: 필요하면 q 에서 계산
    m0  = msg.motor_state()[0]
    print(m0.q(), m0.dq(), m0.tau_est(), m0.temperature(), m0.status_bits())
lowstate_subscriber.init(lowstate_callback)

서비스 호출 (init / torque / control mode)

# PDU / 모터
client.InitPdu(igc_sdk.PDU_INIT, 5000)
client.InitPdu(igc_sdk.PDU_AND_MOTOR_INIT, 30000)
# 토크
client.SetTorque(igc_sdk.TORQUE_ON, 5000)
# 제어 모드 (C++ SendControlModeCommand 와 동일)
client.SendControlModeCommand(
    igc_sdk.CONTROL_MODE_CMD_MOTION_PRESET, "HOME", False, 60000)
client.SendControlModeCommand(
    igc_sdk.CONTROL_MODE_CMD_JOINT_POSITION_HOLD, "", False, 60000)
# 핸드
client.InitHand(5000)

LowCmd 전송 (예: 목 yaw/pitch 모터 제어)

lowcmd_publisher = igc_sdk.LowCmdPublisher("lowcmd", igc_sdk.QosProfile.SensorData())
lowcmd_publisher.init()
low_cmd_msg = igc_sdk.LowCmd_()
for i in range(0, 31):
    motor_cmd = low_cmd_msg.motors()[i]
    motor_cmd.id(i); motor_cmd.q(0.0); motor_cmd.dq(0.0); motor_cmd.tau(0.0)  # 기본값
    if i == 30:          # Neck Pitch
        motor_cmd.q(0.3); motor_cmd.kp(5); motor_cmd.kd(0.1)
    elif i == 29:
        motor_cmd.kp(2);  motor_cmd.kd(0.05)
    else:
        motor_cmd.kp(0.0); motor_cmd.kd(0.0)
# kinematic_modes는 parallel-link 그룹별 5개 배열(waist/L_ankle/R_ankle/L_wrist/R_wrist)입니다.
# 값을 바꿀 때는 5개 목록 전체를 setter로 넘깁니다 (getter가 돌려준 목록을 고쳐도 반영되지 않음).
low_cmd_msg.kinematic_modes([igc_sdk.KINEMATIC_MODE_PJS] * 5)
lowcmd_publisher.write(low_cmd_msg)

Github Repository에서 전체 예제를 확인할 수 있습니다.