torque_rt

이 함수는 외부 컨트롤러에서 실시간 모터 토크 제어를 합니다. UDP 기반 실시간 통신으로 각 조인트 모터에 토크를 직접 스트리밍할 수 있습니다. 이 모드는 주로 임피던스, 어드미턴스, 중력 보상 토크 피드백 같은 고급 제어 알고리즘에 사용합니다.

정의
DRFLEx.h, CDRFLEx 클래스의 public 섹션

bool torque_rt(float fMotorTor[NUM_JOINT],
               float fTargetTime) {
    return _torque_rt(_rbtCtrlUDP, fMotorTor, fTargetTime);
};

매개변수

매개변수 이름

자료형

기본값

설명

fMotorTor

float[6]

목표 토크[Nm]입니다. read_data_rt의 gravity_torque와 같은 기준이므로 두 값을 바로 더할 수 있습니다. 모든 값은 유한해야 합니다.

fTargetTime

float

목표 보간 시간[s]입니다.

참고

  • 비동기 명령입니다.

  • 명령을 받을 때마다 그 순간 조인트에서 측정한 토크에서 새 램프를 시작해 fTargetTime 뒤에 fMotorTor에 도달합니다. 새 명령이 오면 램프를 다시 시작하므로, 명령이 fTargetTime보다 자주 도착하면 명령 토크가 fMotorTor에 도달하지 못합니다. 1 ms 루프에서는 fTargetTime을 0으로 설정하세요.

  • fTargetTime이 컨트롤러 주기(약 1 ms) 이하이면 보간 없이 명령을 즉시 실행합니다.

  • 목표 토크에 도달하기 전에 다음 명령을 받지 못하면
    마지막 토크 입력을 유지합니다.

  • 토크 명령은 끊기지 않고 계속 도착해야 합니다. 명령 사이 간격이 0.1 s를 넘으면 컨트롤러가 실시간 모션 경고를 보고하고, 급정지(Quick Stop)로 로봇을 멈춘 뒤 위치를 유지합니다. 서보 전원은 켜진 상태로 유지됩니다. 이 제한은 토크 경로에만 적용되며, set_rt_control_input으로 설정하는 입력 워치독과는 별개이고 더 엄격합니다.

  • 컨트롤러는 fMotorTor에 중력 보상이나 마찰 보상을 더하지 않습니다. 아래 예제처럼 read_data_rt의 gravity_torque 값을 더하세요. 이 값은 컨트롤러에 설정한 툴 무게와 무게 중심, 설치 자세에 따라 달라지므로 이 설정을 정확하게 유지하세요.

  • set_velj_rt, set_accj_rt, set_velx_rt, set_accx_rt로 설정한 한계는 torque_rt에 적용되지 않습니다. torque_rt가 활성화된 동안에도 안전 설정의 조인트 속도 한계와 TCP 속도 한계는 계속 감시됩니다.

  • RT 출력 데이터의 control_mode는 처음으로 수락된 명령에서 CONTROL_MODE_POSITION(3)에서 CONTROL_MODE_TORQUE(4)로 바뀌며, 로봇이 정지하거나 일시 정지하거나 시간 초과가 발생하면 3으로 돌아갑니다.

  • 컨트롤러는 로봇이 STATE_STANDBY 또는 STATE_MOVING 상태일 때 torque_rt를 수락합니다. 다른 모션이 실행 중일 때 명령을 보내면 그 모션을 정지하고 경고를 보고합니다. 조그나 핸드가이딩 중에는 메시지 없이 명령을 무시합니다.

  • NaN 값이나 명령 토크의 급격한 변화는 치명적 오류입니다. 로봇이 정지하고 서보 전원이 꺼집니다.

  • 컨트롤러가 허용하는 최대 토크를 넘는 값(무한대 포함)은 최댓값으로 제한되며, 컨트롤러가 정보 메시지를 보고합니다. 보내기 전에 모든 값이 유한한지 확인하세요.

주의

  • H 시리즈 로봇은 2번 조인트에 중력 보상 장치가 내장되어 있습니다.
    명령한 토크와 보상 장치의 토크가 내부에서 더해져 모션을 만듭니다. 따라서 외부 토크 명령을 보낼 때는 다음 관계식으로 보상해야 합니다.

    \(gravity\_torque = (link\_gravity\_torque - gravity\_compensator\_torque)\)

  • 과도하거나 불안정한 토크 명령은 예기치 않은 동작을 일으킬 수 있습니다.

  • 안전하게 반응하도록 부드러운 토크 피드백이나 순응 제어(Compliance Control)를 사용하는 것을 권장합니다.

반환값

값

설명

1

명령을 보냈습니다. 컨트롤러가 명령을 받았거나 적용했다는 뜻은 아닙니다.

0

명령 데이터그램을 보내지 못했습니다.

참고

이 함수는 비동기로 동작합니다. 컨트롤러가 명령을 수락하거나 거부할 때까지 기다리지 않고 반환하므로, 반환값 1은 로봇이 명령대로 동작했다는 뜻이 아닙니다. 거부된 명령과 모션 경고 및 오류는 set_on_log_alarm으로 보고됩니다. set_on_rt_log_alarm은 RT 세션 메시지만 보고하므로 두 콜백을 모두 등록하세요.

예제

이 예제는 RT 세션을 시작하기 전에 open_connection으로 컨트롤러 연결을 열고, 제어권(Control Authority)을 얻고, 자동 모드를 설정하고, 서보 전원을 켭니다. stop이 그 연결로 전송되고, torque_rt는 STATE_STANDBY 또는 STATE_MOVING 상태에서만 수락되기 때문입니다.

#include <atomic>
#include <chrono>
#include <cstdio>
#include <cstring>
#include <thread>

std::atomic<bool> g_bHasControlAuthority(false);

void OnMonitoringAccessControlCB(const MONITORING_ACCESS_CONTROL eAccess)
{
    if (eAccess == MONITORING_ACCESS_CONTROL_GRANT)
        g_bHasControlAuthority = true;
}

// 컨트롤러 연결: 아래의 stop()은 이 연결로 전송됩니다
drfl.set_on_monitoring_access_control(OnMonitoringAccessControlCB);
if (!drfl.open_connection("192.168.137.100")) return 1;
drfl.manage_access_control(MANAGE_ACCESS_CONTROL_FORCE_REQUEST);
while (!g_bHasControlAuthority)
    std::this_thread::sleep_for(std::chrono::milliseconds(100));

if (!drfl.set_robot_mode(ROBOT_MODE_AUTONOMOUS)) return 1;
if (!drfl.set_robot_control(CONTROL_SERVO_ON)) return 1;
while (drfl.get_robot_state() != STATE_STANDBY)
    std::this_thread::sleep_for(std::chrono::milliseconds(100));

drfl.connect_rt_control("192.168.137.100", 12347);

std::string version = "v1.0";
float period = 0.001f;  // 1 ms 주기
int lossCount = 4;

// 출력 스트리밍을 먼저 시작합니다. 입력 워치독은 아래 대기 후에 설정하므로,
// 예제가 첫 RT 데이터를 기다리는 동안에는 워치독 시간이 흐르지 않습니다.
drfl.set_rt_control_output(version, period, lossCount);
drfl.start_rt_control();

float fMotorTor[6] = {0.f, 0.f, 0.f, 0.f, 0.f, 0.f};
float fTargetTime = 0.0f;  // 각 명령을 즉시 적용합니다. 램프를 쓰면 1 ms마다 다시 시작됩니다

float q[6] = {0.f};
float q_dot[6] = {0.f};
float gravity_torque[6] = {0.f};
float trg_q[6] = {0.f};
float trg_q_dot[6] = {0.f};
float kp[6] = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
float kd[6] = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0};

// RT 데이터가 도착하고 time_stamp가 증가할 때까지 대기합니다. 그 전까지
// read_data_rt()는 gravity_torque를 포함해 모두 0을 반환합니다.
while (drfl.read_data_rt()->time_stamp == 0.0)
    std::this_thread::sleep_for(std::chrono::milliseconds(1));
const double first_stamp = drfl.read_data_rt()->time_stamp;
while (drfl.read_data_rt()->time_stamp == first_stamp)
    std::this_thread::sleep_for(std::chrono::milliseconds(1));

// 현재 자세 유지: PD 목표는 측정한 조인트 위치입니다.
memcpy(trg_q, drfl.read_data_rt()->actual_joint_position, sizeof(float) * 6);

// 1 ms 명령 루프를 시작하기 직전에 입력 워치독을 설정합니다.
drfl.set_rt_control_input(version, period, lossCount);

const float loop_period = 0.001f;  // 아래의 대기 시간과 일치
float elapsed = 0.0f;

while (true) {
    // 프레임 전체를 한 번에 복사합니다. 포인터를 보관하는 것은 스냅숏이 아닙니다.
    // 버퍼는 계속 덮어써지므로 필드별로 복사하면 서로 다른 패킷의 값이
    // 섞일 수 있습니다.
    RT_OUTPUT_DATA_LIST frame;
    memcpy(&frame, drfl.read_data_rt(), sizeof(frame));

    memcpy(q,              frame.actual_joint_position, sizeof(float) * 6);
    memcpy(q_dot,          frame.actual_joint_velocity, sizeof(float) * 6);
    memcpy(gravity_torque, frame.gravity_torque,        sizeof(float) * 6);

    // 간단한 중력 보상 + PD 토크 컨트롤러
    for (int i = 0; i < 6; i++) {
        fMotorTor[i] = gravity_torque[i]
                     + kp[i] * (trg_q[i] - q[i])
                     + kd[i] * (trg_q_dot[i] - q_dot[i]);
    }

    drfl.torque_rt(fMotorTor, fTargetTime);

    // 종료 조건
    elapsed += loop_period;
    if (elapsed > 5.0f) {
        break;
    }

    std::this_thread::sleep_for(std::chrono::milliseconds(1)); // 1 ms RT 루프
}

// 종료: 중력에 맞서 암을 유지하고, 위치 제어로 돌아간 뒤 RT 세션을 종료합니다.
// stop_rt_control()만 호출하면 0.1 s 제한 시간이 로봇을 정지시킬 때까지 마지막 토크가 유지됩니다.
drfl.torque_rt(gravity_torque, 0.0f);
if (!drfl.stop(STOP_TYPE_SLOW))
    printf("stop failed\n");
drfl.stop_rt_control();
drfl.disconnect_rt_control();

이 예제는 비례-미분(PD) 피드백과 중력 보상을 결합한 조인트 공간(Joint Space) 토크 제어 루프를 보여 줍니다.

팁

  • 고급 토크 수준 제어나 순응 기반 애플리케이션에는 torque_rt를 사용하세요.

  • 피드백을 검증하려면 항상 read_data_rt로 현재 조인트 토크를 모니터링하세요.

  • 기계적 충격을 피하려면 토크를 부드럽게 전환하세요.