1. End Effector 개요

IGRIS-C는 용도에 따라 여러 종류의 end effector를 지원합니다. 어떤 종류든 동일한 DDS 제어
인터페이스(rt/handcmd / rt/handstate / rt/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 채널(rt/handcmd / rt/handstate)에서 ID 기반으로
  함께 제어됩니다.

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

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

노드 개요

igris_c_hand_node는 IGRIS-C end effector를 Igris_c_sdk DDS 채널만으로 제어할 수 있게 해주는 C++ 노드입니다. HandCmd / HandState / HandInitRequest / ServiceResponse 메시지만으로 초기화, 목표 자세 전송, 상태 수신을 수행합니다. 노드는 /dev/igris_c_hand 포트를 기본값으로 사용합니다.

구독 / 발행 인터페이스

노드는 ROS2 표준 토픽이 아닌 Igris_c_sdk DDS 채널로 통신합니다.

노드가 구독하는 채널

1. rt/handcmd (igris_c_sdk::HandCmd) — 목표 위치 명령. motor_cmd는 MotorCmd의 가변 길이
   시퀀스이며, 각 엔트리는 (id, q, …) 형식입니다.

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

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

노드가 발행하는 채널

1. rt/handstate (igris_c_sdk::HandState) — 일정 주기로 실측 모터 상태 배열을 발행.
   각 MotorState 필드:

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

   • dq: 현재 속도

   • tau_est: 추정 전류값 (핸드는 이 필드에 모터 전류를 싣습니다)

   • temperature: 모터 온도 (°C)

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

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

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

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

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

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

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

시퀀스 예시

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

2. 목표 위치는 rt/handcmd에 motor_cmd를 채워 발행합니다. 예) 우측 검지만 0.8 → [{id:12, q:0.8}],
   양손 전체 동일 자세 → 12개 ID를 모두 포함한 시퀀스 발행.

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

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

디버그 팁

• HandState.status_bits는 모터별 fault/limit 비트이므로 GUI/로거에서 경보·상태 표시에 활용.

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

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

4. IGRIS-C Hand Example

실행 가능한 전체 예제: examples/cyclonedds/cyclonedds_hand_example.cpp (C++),
examples/python/hand_example.py (Python). 아래는 핵심 흐름 발췌이며, 토픽은 b.3의
로봇 호기 namespace 아래로 해석됩니다.

// Example: drive the igris_c_hand node entirely through igris_c_sdk DDS channels.
//   * Trigger initialization via rt/service/hand_init/request (HandInitRequest)
//   * Send target vectors via rt/handcmd (MotorCmd.id + q)
//   * React to rt/handstate and rt/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 aliases HandCmd / HandState / MotorCmd / HandInitRequest / ServiceResponse and the
// infra (ChannelFactory / Publisher / Subscriber / QosProfile) into the igris_c_sdk namespace.
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 HandCmd make_targets(const std::array<float, 12>& q) {
    HandCmd cmd;
    for (size_t i = 0; i < kMotorIds.size(); ++i) {
        MotorCmd m;
        m.id(kMotorIds[i]);
        m.q(std::clamp(q[i], 0.0f, 1.0f));
        cmd.motor_cmd().push_back(m);
    }
    return cmd;
}
int main() {
    ChannelFactory::Instance()->Init(0, "igris_c_IG01");  // domain 0, 로봇 호기 namespace (b.3 참고)
    Publisher<HandCmd> cmd_pub("rt/handcmd", QosProfile::SensorData());
    cmd_pub.init();
    // Initialization is a request/response service (no more id=99 in rt/handcmd).
    Publisher<HandInitRequest> init_req_pub("rt/service/hand_init/request", QosProfile::Services());
    init_req_pub.init();
    Subscriber<HandState> state_sub("rt/handstate", QosProfile::SensorData());
    state_sub.init([](const 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());
        }
    });
    Subscriber<ServiceResponse> init_res_sub(
        "rt/service/hand_init/response", QosProfile::Services());
    init_res_sub.init([](const ServiceResponse& 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
    HandInitRequest 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;
}