1. End Effector 개요

IGRIS-C는 용도에 따라 여러 종류의 end effector를 지원합니다. 어떤 종류든 동일한 DDS 제어
인터페이스(handcmd / handstate / service/hand_init)를 공유하며, 종류별로 구동 모터 수·ID·통신 속도만 다릅니다.

end effector 종류는 로봇 구성에 따라 사전 설정되어 출고됩니다. 본 문서의 제어 인터페이스는 종류와 무관하게 동일하게 적용됩니다.

종류별 외형

5지 핸드 (Dexterous Hand)	1자유도 그리퍼 (1-DoF)	전자석 그리퍼 (Magnet)

2. Dexterous Hand

• 엄지손가락은 2개의 독립 구동 자유도를 가지며, 나머지 네 손가락은 각각 1개의 커플링된
  구동 자유도로 동작합니다. 모든 손가락에는 수동 적응을 위한 1 DoF가 추가되어, 물체 형상에
  자연스럽게 대응하는 파지 성능을 제공합니다.

• Hand는 좌/우 손으로 구분되며 좌우 대칭 구조입니다. 좌/우는 서로 다른 모터 ID 대역
  (우 11~16, 좌 21~26)으로 식별되며, 단일 DDS 채널(handcmd / handstate)에서 ID 기반으로
  함께 제어됩니다.

• 1-DoF 그리퍼는 우(11)·좌(21) 2개 모터를, 마그넷은 우(11·12)·좌(21·22) 4개 모터
  (11/21 = 자석 on·off, 12/22 = 위치 서보)를 구동하며, 동일한 handcmd 채널에서 해당 ID로 제어합니다.

• 1-DoF 그리퍼는 q=0이 열림(OPEN) 상태입니다. 2.1.1부터 로봇 모델(URDF)과 시뮬레이터의 그리퍼 관절도 q=0 = 열림 기준으로 맞춰졌습니다.

• 전자석 그리퍼는 2.1.1에서 v2 형상으로 갱신되었습니다. 로봇 모델(URDF)의 질량이 한쪽당 0.324 kg에서 0.427 kg으로 바뀌었습니다.

3. IGRIS-C 핸드 노드 사용 가이드

노드 개요

igris_c_hand_node는 IGRIS-C end effector를 구동하는 로봇 내부 노드입니다.

외부 클라이언트가 사용하는 핸드 토픽·서비스(handcmd / handstate / service/hand_init)는 핸드 노드가 직접 열지 않고, 로봇의 외부 통신 노드(bridge)가 다른 SDK 토픽과 함께 로봇의 전송 모드(Transport)에 맞춰 제공합니다. HandCmd / HandState 메시지와 HandInitRequest 서비스만으로 초기화, 목표 자세 전송, 상태 수신을 수행합니다.

⚠️ 외부 handcmd는 LOW_LEVEL_MODE와 LOW_LEVEL_WALK_MODE에서만 손에 반영됩니다. HIGH_LEVEL_MODE·HIGH_LEVEL_WALK_MODE에서는 모션 프리셋의 손 동작이 우선하고, 그 밖의 모드(WALK_MODE, PASSIVE_MODE, JOINT_HOLD_MODE, DAMPING_MODE)에서는 handcmd가 무시되어 손이 마지막 목표 자세를 유지합니다. 2.1.0은 모든 모드에서 handcmd를 따랐으므로, 손을 직접 제어하는 코드는 로봇이 위 두 모드인지 먼저 확인하십시오(ControlMode 페이지 참고). 초기화 서비스(service/hand_init)와 handstate 수신은 모드와 관계없이 동작합니다.

구독 / 발행 인터페이스

코드에서는 접두어 없는 이름(handcmd)을 사용하며, 실제 토픽 이름과 서비스 방식은 로봇의 전송 모드가 정합니다. 전송 모드에 대한 자세한 내용은 SDK Overview 페이지의 「3. 전송 모드 (Transport)」를 참고하십시오.

로봇이 구독하는 채널

1. handcmd (igris_c_sdk::msg::dds_::HandCmd_) — 목표 위치 명령. motor_cmd는 MotorCmd의 12칸 고정 배열이며, 각 칸은 (id, q, …) 형식입니다.

   • 배열 길이가 고정이므로 push_back으로 늘리지 말고 인덱스로 채웁니다(C++ cmd.motor_cmd()[i], Python은 MotorCmd_ 정확히 12개 리스트). 대상 모터는 칸 순서가 아니라 id로 정해지며, 쓰지 않는 칸은 id를 0(기본값)으로 두면 무시됩니다.

   • id가 유효 ID(hand: {11~16, 21~26}, 1-DoF: {11, 21}, magnet: {11, 12, 21, 22}) 중 하나인 엔트리의 q를
     해당 관절의 정규화(0.0~1.0) 목표로 저장한 뒤 즉시 적용합니다. 범위를 벗어난 값은 clamp,
     매핑에 없는 ID는 무시됩니다.

   • ⚠️ 초기화는 더 이상 handcmd로 처리하지 않습니다. (과거 id=99 트리거 방식은 제거됨)
     초기화는 아래 service/hand_init 서비스를 사용하세요.

로봇이 발행하는 채널

1. handstate (igris_c_sdk::msg::dds_::HandState_) — lowstate와 같은 주기로 실측 모터 상태 12칸 고정 배열(motor_state)을 발행합니다(2.1.0보다 발행 주기가 크게 빨라졌습니다). MotorState에는 id 필드가 없으며, motor_state[i]는 ID 11~16, 21~26 순서입니다. 1-DoF는 0·6번 칸, 마그넷은 0·1·6·7번 칸만 사용하고 나머지 칸은 0입니다. 2.1.0의 HandState.imu_state는 삭제되었습니다. 각 MotorState 필드:

   • q: 현재 위치 (0~1)

   • dq: 현재 속도

   • tau_est: 모터 전류 (mA). 메시지 정의의 토크 단위와 달리 핸드는 이 필드에 모터 전류를 싣습니다.

   • temperature: 모터 온도 (°C)

   • status_bits: 모터 에러/상태 비트

초기화(캘리브레이션) 서비스 — service/hand_init

초기화는 별도의 Request/Response 서비스로 분리되어 있습니다.

• 요청: service/hand_init/request에 HandInitRequest_Request_ { std_msgs/Header header; string request_id }를 발행합니다.

• 응답: service/hand_init/response로 HandInitRequest_Response_ { header, request_id, success, message, error_code }가 돌아옵니다 (error_code == 0이면 성공). request_id로 요청과 응답을 매칭합니다.

• ⚠️ 응답은 반드시 HandInitRequest_Response_ 타입으로 구독합니다. 필드가 같은 공용 ServiceResponse_로 구독하면 DDS 타입 이름이 달라 에러 없이 아무것도 수신되지 않습니다.

• ros 전송 모드에서는 토픽 쌍 대신 실제 ROS 2 서비스 /<ns>/service/hand_init(igris_c_sdk/srv/HandInitRequest)를 rclcpp 서비스 클라이언트로 호출합니다.

• 노드는 요청을 받으면 목표를 0으로 초기화하고 effector->initialize()(타입별 캘리브레이션/호밍)를
  수행합니다.

• 고수준 편의 메서드: SDK의 IgrisC_Client::InitHand(timeout_ms) 를 호출하면 위 요청/응답을 내부에서 처리합니다(cyclonedds 전송 모드, Python은 timeout_ms 필수 인자). 아래 4절 예제는 채널을 직접 다루는 저수준 방식입니다.

시퀀스 예시

1. 노드 기동 시 로봇 구성에 따라 자동으로 초기화될 수 있습니다. 런타임에 재초기화가 필요하면
   service/hand_init/request에 HandInitRequest(request_id 채워서)를 발행합니다.

2. 로봇을 LOW_LEVEL_MODE 또는 LOW_LEVEL_WALK_MODE로 둔 상태에서 handcmd에 motor_cmd를 채워 발행합니다. 예) 우측 검지만 0.8 → 한 칸에 {id:12, q:0.8}을 넣고 나머지 칸은 id 0, 양손 전체 동일 자세 → 12칸에 12개 ID를 모두 채워 발행.

3. 상태 확인: handstate를 구독해 motor_state 배열 모니터링.

4. 초기화 결과 확인: service/hand_init/response를 구독해 success/error_code 확인.

디버그 팁

• HandState.status_bits는 모터별 fault/limit 비트이므로 GUI/로거에서 경보·상태 표시에 활용. 플래그 값은 MotorState의 상수입니다(MOTOR_STALL_ERROR, OVER_CURRENT_ERROR, COMPONENT_OVERTEMPERATURE_ERROR 등). 비트 플래그이므로 (status_bits & FLAG)로 검사하고 등호 비교는 하지 않습니다. C++: igris_c_sdk::msg::dds_::MotorState_Constants::MOTOR_STALL_ERROR, Python: igc_sdk.MOTOR_STALL_ERROR.

• HandCmd.motor_cmd[].q는 0~1 정규화 값입니다. 손 구조에 맞춰 스케일이 필요하면 상위
  애플리케이션에서 선형 변환을 적용하세요.

• 원점 calibration이 틀어졌다고 판단되면 service/hand_init/request로 init을 다시 요청해
  재보정합니다. 응답은 service/hand_init/response에서 확인합니다.

• handcmd를 보내도 손이 움직이지 않으면 먼저 로봇의 제어 모드(LOW_LEVEL_MODE / LOW_LEVEL_WALK_MODE)와 전송 모드·토픽 레이아웃 일치 여부를 확인합니다.

4. IGRIS-C Hand Example

실행 가능한 전체 예제는 아래 표와 같습니다. 아래 코드는 examples/cyclonedds/cyclonedds_hand_example.cpp의 핵심 흐름 발췌이며, 토픽은 SDK Overview의 로봇 호기 namespace 아래로 해석됩니다. 손 동작(2~5단계)은 로봇이 LOW_LEVEL_MODE 또는 LOW_LEVEL_WALK_MODE일 때만 반영됩니다.

cyclonedds_hand_gui_client 실행 화면입니다. 핸드 초기화(hand_init)를 마친 뒤 Closed Pose(0.6)를 보냈고, HandState로 12축 상태가 돌아옵니다(시뮬레이션).

# cyclonedds_native / cyclonedds_ros_compatible
./cyclonedds_hand_example <domain_id> <namespace> [--dds-topic-naming native|ros_compatible]
python3 hand_example.py --domain-id <domain_id> --namespace <namespace> [--dds-topic-naming native|ros_compatible]

# ros
ros2 run igris_c_sdk ros_hand_example --ros-args -p robot_namespace:=<namespace>

// Example: drive the igris_c_hand node entirely through igris_c_sdk DDS channels.
//   * Trigger initialization via service/hand_init/request (HandInitRequest)
//   * Send target vectors via handcmd (MotorCmd.id + q)
//   * React to handstate and service/hand_init/response for logging/verification
//
// Build against igris_c_sdk and run after sourcing the workspace.

#include <algorithm>
#include <array>
#include <chrono>
#include <thread>

#include <igris_c_sdk/channel_factory.hpp>
#include <igris_c_sdk/publisher.hpp>
#include <igris_c_sdk/subscriber.hpp>
#include <igris_c_sdk/types.hpp>

// types.hpp 는 계약 타입 별칭을 의도적으로 두지 않습니다: 전체 이름이 곧 와이어에, ddsls 출력에,
// DDS 타입 불일치 에러에 그대로 찍히므로 사용처마다 풀어서 씁니다.
// 아래 namespace 로 가져오는 것은 인프라(ChannelFactory / Publisher / Subscriber / QosProfile)뿐입니다.
using namespace igris_c_sdk;

static constexpr std::array<uint16_t, 12> kMotorIds = {
    11, 12, 13, 14, 15, 16, 21, 22, 23, 24, 25, 26};

static igris_c_sdk::msg::dds_::HandCmd_ make_targets(const std::array<float, 12>& q) {
    igris_c_sdk::msg::dds_::HandCmd_ cmd;
    // motor_cmd is a fixed std::array<MotorCmd_, 12> - index it, do not push_back.
    for (size_t i = 0; i < kMotorIds.size(); ++i) {
        auto& m = cmd.motor_cmd()[i];
        m.id(kMotorIds[i]);
        m.q(std::clamp(q[i], 0.0f, 1.0f));
    }
    return cmd;
}

int main() {
    ChannelFactory::Instance()->Init(0, "igris_c_IG01");  // domain 0, 로봇 호기 namespace (SDK Overview 참고)

    Publisher<igris_c_sdk::msg::dds_::HandCmd_> cmd_pub("handcmd", QosProfile::SensorData());
    cmd_pub.init();

    // Initialization is a request/response service (no more id=99 in handcmd).
    Publisher<igris_c_sdk::srv::dds_::HandInitRequest_Request_> init_req_pub("service/hand_init/request",
                                                                            QosProfile::Services());
    init_req_pub.init();

    Subscriber<igris_c_sdk::msg::dds_::HandState_> state_sub("handstate", QosProfile::SensorData());
    state_sub.init([](const igris_c_sdk::msg::dds_::HandState_& s) {
        if (s.motor_state().size() >= 3) {
            printf("q[0..2]=%.3f %.3f %.3f\n",
                   s.motor_state()[0].q(),
                   s.motor_state()[1].q(),
                   s.motor_state()[2].q());
        }
    });

    // The response type is the service's own _Response_, NOT the standalone ServiceResponse -
    // they are field-identical but register different DDS type names, so the wrong one here
    // subscribes successfully and then never receives anything.
    Subscriber<igris_c_sdk::srv::dds_::HandInitRequest_Response_> init_res_sub(
        "service/hand_init/response", QosProfile::Services());
    init_res_sub.init([](const igris_c_sdk::srv::dds_::HandInitRequest_Response_& r) {
        printf("[hand_init] success=%d code=%d msg=%s\n",
               r.success(), r.error_code(), r.message().c_str());
    });

    // 1) Trigger initialization via the hand_init service
    igris_c_sdk::srv::dds_::HandInitRequest_Request_ init_req;
    init_req.request_id() = "hand_init";
    init_req_pub.write(init_req);
    std::this_thread::sleep_for(std::chrono::milliseconds(1500));

    // 2) Fully open
    cmd_pub.write(make_targets({0.05f, 0.05f, 0.05f, 0.05f, 0.05f, 0.05f,
                                0.05f, 0.05f, 0.05f, 0.05f, 0.05f, 0.05f}));
    std::this_thread::sleep_for(std::chrono::seconds(2));

    // 3) Close all joints
    cmd_pub.write(make_targets({0.85f, 0.85f, 0.85f, 0.85f, 0.85f, 0.85f,
                                0.85f, 0.85f, 0.85f, 0.85f, 0.85f, 0.85f}));
    std::this_thread::sleep_for(std::chrono::milliseconds(2500));

    // 4) Basic pinch (right thumb + index + middle), others relaxed
    std::array<float, 12> pinch = {};
    pinch[0] = 0.80f;  // right thumb  (id=11)
    pinch[1] = 0.75f;  // right index  (id=12)
    pinch[2] = 0.60f;  // right middle (id=13)
    cmd_pub.write(make_targets(pinch));
    std::this_thread::sleep_for(std::chrono::seconds(2));

    // 5) Back to open
    cmd_pub.write(make_targets({0.05f, 0.05f, 0.05f, 0.05f, 0.05f, 0.05f,
                                0.05f, 0.05f, 0.05f, 0.05f, 0.05f, 0.05f}));
    std::this_thread::sleep_for(std::chrono::milliseconds(1500));

    cmd_pub.stop();
    state_sub.stop();
    init_res_sub.stop();
    init_req_pub.stop();
    return 0;
}