최신 버전 정보는 1.33.5에서 확인하세요.
6.6.6 realtime_control_sample.cpp
이 예제(6_realtime_control_sample.cpp)는 다음을 사용해 로봇의 실시간(RT) 제어를 실행하는 최소 절차를 보여 줍니다.
1 ms 주기의 고주파 서보 명령(
servoj_rt)5차 다항식 궤적(Trajectory) 생성
실시간 스레드 스케줄링(Linux/Pthread 또는 Xenomai)
안전을 고려한 RT 모드 활성화
주의
실시간 모니터링과 제어는 실제 로봇 시스템에서만 지원합니다. 이 샘플은 반드시 실제 컨트롤러 IP로 실행해야 합니다.
실시간 UDP 클라이언트는 한 번에 하나만 허용됩니다. 다른 애플리케이션이 이미 실시간 UDP 포트에 연결되어 있으면 로봇이 새 RT 연결을 거부합니다.
이 예제의 경로는 다음과 같습니다.
API-DRFL/example/Linux_64/6_realtime_control_sample.cpp
예제는 다음에서 확인하세요. Doosan Robotics API-DRFL GitHub
전체 설명 영상은 다음에서 확인하세요. Doosan Robotics Official YouTube
주의
이 영상은 튜토리얼을 더 잘 이해하도록 돕기 위해 제공합니다.
실제로 튜토리얼을 실행할 때 실시간 제어(Real-time Control)는 시뮬레이터 모드에서 지원되지 않으며 실제 로봇에서만 실행할 수 있습니다.
실제 로봇에서 실행하는 영상은 다음에서 확인하세요. Doosan Robotics Official YouTube
설정 및 전역 변수
이 예제는 전역 DRFLEx 컨트롤러 인스턴스와 함께 다음을 추적하는 여러 플래그를 정의합니다.
제어권(Control Authority) 부여 상태(GRANT), TP 초기화 상태, 정지/일시 정지/재개 명령 상태, 내부 RT 궤적 매개변수
// ============================================================================
// 전역 변수
// ============================================================================
CDRFLEx Drfl;
bool g_bHasControlAuthority = false;
bool g_TpInitailizingComplted = false;
bool g_mStat = false;
bool g_Stop = false;
bool moving = false;
bool bAlterFlag = false;
float k_ratio = 1.5;
string strDrl =
"\r\n\
loop = 0\r\n\
while loop < 1003:\r\n\
movej(posj(10,10.10,10,10.10), vel=60, acc=60)\r\n\
movej(posj(00,00.00,00,00.00), vel=60, acc=60)\r\n\
loop+=1\r\n";
또한 5차 다항식 궤적 계획에 사용할 구조체도 선언합니다.
struct PlanParam {
float time;
float ps[6], vs[6], as[6];
float pf[6], vf[6], af[6];
float A0[6], A1[6], A2[6], A3[6], A4[6], A5[6];
};
struct TraParam {
float time;
float pos[6], vel[6], acc[6];
};
이 구조체에는 시작/끝 경계 조건과, 매끄러운 RT 모션을 정의하는 다항식 계수가 저장됩니다.
콜백 등록
예제는 컨트롤러 연결을 열기 전에 필요한 콜백을 모두 등록합니다.
// 콜백 함수 설정
Drfl.set_on_homming_completed(OnHommingCompleted);
Drfl.set_on_monitoring_data(OnMonitoringDataCB);
Drfl.set_on_monitoring_data_ex(OnMonitoringDataExCB);
Drfl.set_on_monitoring_ctrl_io_ex(OnMonitoringCtrlIOExCB);
Drfl.set_on_monitoring_access_control(OnMonitroingAccessControlCB);
Drfl.set_on_tp_initializing_completed(OnTpInitializingCompleted);
Drfl.set_on_log_alarm(OnLogAlarm);
Drfl.set_on_tp_popup(OnTpPopup);
Drfl.set_on_tp_log(OnTpLog);
Drfl.set_on_tp_progress(OnTpProgress);
Drfl.set_on_tp_get_user_input(OnTpGetuserInput);
Drfl.set_on_rt_monitoring_data(OnRTMonitoringData);
Drfl.set_on_program_stopped(OnProgramStopped);
Drfl.set_on_disconnected(OnDisConnected);
제어권 모니터링
void OnMonitroingAccessControlCB(const MONITORING_ACCESS_CONTROL eTrasnsitControl) {
switch (eTrasnsitControl) {
case MONITORING_ACCESS_CONTROL_REQUEST:
assert(Drfl.ManageAccessControl(MANAGE_ACCESS_CONTROL_RESPONSE_NO));
break;
case MONITORING_ACCESS_CONTROL_GRANT:
g_bHasControlAuthority = true;
break;
case MONITORING_ACCESS_CONTROL_DENY:
case MONITORING_ACCESS_CONTROL_LOSS:
g_bHasControlAuthority = false;
if (g_TpInitailizingComplted) {
Drfl.ManageAccessControl(MANAGE_ACCESS_CONTROL_FORCE_REQUEST);
}
break;
default:
break;
}
}
이 콜백은 제어권이 바뀌면 GRANT/LOSS 상태를 갱신합니다. 다른 클라이언트의 제어권 요청에는 MANAGE_ACCESS_CONTROL_RESPONSE_NO로 응답하고, TP 초기화가 끝난 상태에서 DENY나 LOSS가 오면 제어권을 다시 요청합니다.
ManageAccessControl은 요청을 보내자마자 반환하므로 콜백 안에서 호출할 수 있습니다(콜백 함수 규칙 참고).
TP 초기화 → 제어권 강제 요청
void OnTpInitializingCompleted() {
g_TpInitailizingComplted = true;
Drfl.ManageAccessControl(MANAGE_ACCESS_CONTROL_FORCE_REQUEST);
}
TP가 초기화되면 프로그램이 자동으로 제어권 강제 요청을 보냅니다.
연결 및 버전 정보
콜백을 등록한 뒤 컨트롤러 연결을 엽니다.
// 로봇에 연결
assert(Drfl.open_connection("192.168.137.100"));
SYSTEM_VERSION tSysVerion = {'\0'};
Drfl.get_system_version(&tSysVerion);
assert(Drfl.setup_monitoring_version(1));
cout << "System version: " << tSysVerion._szController << endl;
cout << "Library version: " << Drfl.get_library_version() << endl;
샘플은 assert.h를 포함하기 전에 NDEBUG 정의를 해제합니다. 따라서 assert() 안의 호출은 모든 빌드에서 실행되며, 그중 하나라도 false를 반환하면 프로그램이 중단됩니다.
콘솔 예:
System version: GF03050000
Library version: GL013300
로봇 모드 설정
실시간 제어에는 AUTONOMOUS 모드, REAL 시스템, AUTONOMOUS 안전 모드가 필요합니다.
assert(Drfl.set_robot_mode(ROBOT_MODE_AUTONOMOUS));
assert(Drfl.set_robot_system(ROBOT_SYSTEM_REAL));
RT 세션 활성화
메인 루프에서 사용자가 s를 누르면 프로그램이 실시간 제어 시퀀스 전체를 한 번에 시작합니다. connect_rt_control의 기본 주소로 RT 채널을 연결하고, RT 출력 매개변수를 설정하고, 로봇을 AUTONOMOUS 모드와 AUTONOMOUS 안전 모드로 전환하고, RT 제어를 시작한 다음, 마지막으로 pthread_create로 realtime_task를 높은 우선순위로 실행합니다.
(연결 → RT 출력 설정 → RT 시작 → realtime_task 시작)
case 's': {
// 실시간 제어 절차 시작(case 1, 2, 3, 4를 합친 것)
cout << "Starting realtime control sequence..." << endl;
// 1단계: RT 제어 연결
Drfl.connect_rt_control();
cout << "RT control connected" << endl;
// 2단계: RT 제어 출력 설정
string version = "v1.0";
float period = 0.001;
int losscount = 4;
Drfl.set_rt_control_output(version, period, losscount);
cout << "RT control output configured" << endl;
std::this_thread::sleep_for(std::chrono::microseconds(1000));
// 3단계: 로봇 모드 설정 후 RT 제어 시작
Drfl.set_robot_mode(ROBOT_MODE_AUTONOMOUS);
Drfl.set_safety_mode(SAFETY_MODE_AUTONOMOUS, SAFETY_MODE_EVENT_MOVE);
Drfl.start_rt_control();
cout << "RT control started" << endl;
std::this_thread::sleep_for(std::chrono::microseconds(1000));
// 4단계: 실시간 스레드 생성
pthread_t rt_thread;
pthread_attr_t attr;
struct sched_param param;
pthread_attr_init(&attr);
pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
param.sched_priority = 99;
pthread_attr_setschedparam(&attr, ¶m);
int v = pthread_create(&rt_thread, &attr, realtime_task, nullptr);
if (v != 0) {
perror("Failed to create real-time thread");
std::cout << "Error: " << v << std::endl;
return 1;
}
cout << "Realtime thread created successfully" << endl;
break;
}
connect_rt_control로 실시간 채널을 엽니다.
set_rt_control_output으로 RT 매개변수(1 ms 주기)를 설정합니다.
로봇을 ROBOT_MODE_AUTONOMOUS와 SAFETY_MODE_AUTONOMOUS로 전환한 뒤 start_rt_control로 RT 세션을 시작합니다.
우선순위 99의 SCHED_FIFO 스케줄링 정책을 설정하고, 1 ms마다 servoj_rt 명령을 보내는
realtime_task를 시작합니다.
참고
우선순위 99의 SCHED_FIFO를 사용하려면 root 권한이나 CAP_SYS_NICE 권한이 필요합니다.
콘솔 예:
Starting realtime control sequence...
RT control connected
RT control output configured
RT control started
Realtime thread created successfully
정지 / 일시 정지 / 재개
ThreadFunc는 키보드 입력을 받아 컨트롤러에 정지 / 일시 정지 / 재개 명령을 보냅니다. 샘플은 __XENO__로 컴파일했을 때만 rt_task_spawn으로 ThreadFunc를 시작합니다.
int linux_kbhit(void) {
struct termios oldt, newt;
int ch;
tcgetattr(STDIN_FILENO, &oldt);
newt = oldt;
newt.c_lflag &= ~(ICANON | ECHO);
tcsetattr(STDIN_FILENO, TCSANOW, &newt);
ch = getchar();
tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
return ch;
}
// ...
uint32_t ThreadFunc(void* arg) {
printf("Start ThreadFunc\n");
while (true) {
if (linux_kbhit()) {
char ch = getch();
switch (ch) {
case 's':
printf("Stop!\n");
g_Stop = true;
Drfl.MoveStop(STOP_TYPE_SLOW);
break;
case 'p':
printf("Pause!\n");
Drfl.MovePause();
break;
case 'r':
printf("Resume!\n");
Drfl.MoveResume();
break;
}
}
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
return 0;
}
#ifdef __XENO__
RT_TASK sub_task;
char sub_task_name[256] = {'\0'};
sprintf(sub_task_name, "drfl_sub_t");
uint32_t stack_size = 1024 * 64;
uint32_t prio = 50;
if (rt_task_spawn(&sub_task, sub_task_name, stack_size, prio,
T_CPU(3) | T_JOINABLE,
(void (*)(void*)) & ThreadFunc, nullptr) != 0) {
cout << "Cannot create sub task" << endl;
}
#endif
__XENO__ 빌드에서는 실시간 실행 중에 사용자가 로봇 모션에 직접 개입(감속 정지, 일시 정지, 재개)할 수 있습니다.
5차 다항식 궤적 계획
RT 루프에 들어가기 전에 샘플은 두 궤적을 준비합니다.
home → target_pose, target_pose → home
TrajectoryPlan은 다항식 계수 A0~A5를 계산합니다.
float posj_s[6] = {0, 0, 0, 0, 0, 0};
float posj_f[6] = {30, 30, 60, -30, -90, 30};
// ...
float target_time = 8.0;
// ...
float vs[6] = {0, 0, 0, 0, 0, 0};
float vf[6] = {0, 0, 0, 0, 0, 0};
float as[6] = {0, 0, 0, 0, 0, 0};
float af[6] = {0, 0, 0, 0, 0, 0};
// ...
TraParam tra1, tra2;
PlanParam plan1, plan2;
plan1.time = target_time;
plan2.time = target_time;
memcpy(plan1.ps, posj_s, NUMBER_OF_JOINT * sizeof(float));
memcpy(plan1.pf, posj_f, NUMBER_OF_JOINT * sizeof(float));
memcpy(plan2.ps, posj_f, NUMBER_OF_JOINT * sizeof(float));
memcpy(plan2.pf, posj_s, NUMBER_OF_JOINT * sizeof(float));
memcpy(plan1.vs, vs, NUMBER_OF_JOINT * sizeof(float));
memcpy(plan1.vf, vf, NUMBER_OF_JOINT * sizeof(float));
memcpy(plan1.as, as, NUMBER_OF_JOINT * sizeof(float));
memcpy(plan1.af, af, NUMBER_OF_JOINT * sizeof(float));
memcpy(plan2.vs, vs, NUMBER_OF_JOINT * sizeof(float));
memcpy(plan2.vf, vf, NUMBER_OF_JOINT * sizeof(float));
memcpy(plan2.as, as, NUMBER_OF_JOINT * sizeof(float));
memcpy(plan2.af, af, NUMBER_OF_JOINT * sizeof(float));
TrajectoryPlan(&plan1);
TrajectoryPlan(&plan2);
이 계수로 매끄러운 5차 위치/속도/가속도 프로파일을 생성할 수 있습니다.
실시간 RT 루프
실시간 제어 예제의 핵심 로직은 SCHED_FIFO 우선순위 99로 실행되는 realtime_task() 함수 안에 구현되어 있습니다.
초기화: 홈 위치와 안전 모드
// 위치 초기화
float home[6] = {0, 0, 0, 0, 0, 0};
float posj_s[6] = {0, 0, 0, 0, 0, 0};
float posj_f[6] = {30, 30, 60, -30, -90, 30};
const float None = -10000;
// 안전 모드를 설정하고 홈 위치로 이동
Drfl.set_safety_mode(SAFETY_MODE_AUTONOMOUS, SAFETY_MODE_EVENT_MOVE);
Drfl.movej(home, 60, 60);
안전 모드를 AUTONOMOUS / EVENT_MOVE로 설정한 다음, movej로 로봇을 홈 위치로 이동합니다.
RT 속도/가속도 설정
float vel[6] = {100, 100, 100, 100, 100, 100};
float acc[6] = {120, 120, 120, 120, 120, 120};
// ...
Drfl.set_velj_rt(vel);
Drfl.set_accj_rt(acc);
DRCF v2에서 servoj_rt가 사용할 조인트 속도와 가속도 값을 설정합니다. 이 값은 servoj_rt 궤적을 제한하지 않습니다. 다음 명령이 제때 도착하지 않으면 로봇은 감속해 정지합니다. 이때 DRCF v2에서는 set_accj_rt 값으로, DRCF v3에서는 안전 설정의 조인트 가속도 한계로 감속합니다.
제어 루프 타이밍 매개변수
// 제어 루프 매개변수
const int loop_period_ms = 1;
float dt = 0.001 * loop_period_ms;
std::chrono::milliseconds loop_period(loop_period_ms);
int repeat = 100000;
float ratio = 15;
double tt = dt * ratio;
loop_period_ms = 1→ 1 ms 제어 주기(1 kHz 루프)tt(dt * ratio= 0.015 s)는 궤적 시간 매개변수로servoj_rt에 전달됩니다. DRCF v2.12.5 이상과 v3.4.0 이상에서는 컨트롤러가 0.025 s 미만의 값을 0.025 s로 올립니다(servoj_rt 참고).
정방향/역방향 궤적 번갈아 실행
바깥 루프는 정방향(plan1) 궤적과 역방향(plan2) 궤적을 번갈아 실행합니다.
for (int i = 0; i < repeat; i++) {
bool flag = true;
int count = 0;
float time = 0;
PlanParam* current_plan = (i % 2 == 0) ? &plan1 : &plan2;
TraParam* current_tra = (i % 2 == 0) ? &tra1 : &tra2;
std::cout << "Loop " << i << ") Start position: ";
for (size_t k = 0; k < 6; k++) {
std::cout << current_plan->ps[k] << ", ";
}
std::cout << std::endl;
while (flag) {
auto loop_start_time = std::chrono::steady_clock::now();
time = (++count) * dt;
current_tra->time = time;
TrajectoryGenerator(current_plan, current_tra);
// ...
}
}
각 루프는 다음과 같습니다.
짝수 인덱스(0, 2, 4, …): home → target(
plan1)홀수 인덱스(1, 3, 5, …): target → home(
plan2)
RT 콘솔 로그 예:
Loop 0) Start position: 0, 0, 0, 0, 0, 0,
Loop 0) End position: 30, 30, 60, -30, -90, 30,
Loop 1) Start position: 30, 30, 60, -30, -90, 30,
Loop 1) End position: 0, 0, 0, 0, 0, 0,
...
실시간 서보 명령(servoj_rt)
안쪽 while 루프에서 궤적 샘플을 생성해 컨트롤러로 보냅니다.
TrajectoryGenerator(current_plan, current_tra);
if (i % 2 != 0) {
for (int j = 0; j < 6; j++) {
current_tra->vel[j] = None;
current_tra->acc[j] = None;
}
}
Drfl.servoj_rt(current_tra->pos, current_tra->vel, current_tra->acc, tt);
정방향 모션(짝수 루프)에서는 위치/속도/가속도를 보냅니다.
복귀 모션(홀수 루프)에서는 속도와 가속도를
None(-10000)으로 바꿔, 사실상 위치만 사용하는 RT 제어로 전환합니다.
현재 시간이 계획한 시간을 넘으면 루프가 끝나고 최종 포즈를 출력합니다.
if (time > current_plan->time) {
flag = false;
std::cout << "Loop " << i << ") End position: ";
for (size_t k = 0; k < 6; k++) {
std::cout << current_plan->pf[k] << ", ";
}
std::cout << std::endl;
break;
}
1 kHz 타이밍 유지
루프는 경과 시간을 측정하고 남은 주기만큼 대기해 1 ms 주기를 일정하게 유지합니다.
auto loop_end_time = std::chrono::steady_clock::now();
auto elapsed_time = std::chrono::duration_cast<std::chrono::milliseconds>(loop_end_time - loop_start_time);
if (elapsed_time < loop_period) {
std::this_thread::sleep_for(loop_period - elapsed_time);
}
이를 통해 RT 제어 루프의 타이밍을 결정론적으로 유지합니다.
RT 제어 정지
e를 누르면 RT 스트리밍이 끝납니다.
case 'e':
Drfl.stop_rt_control();
Drfl.disconnect_rt_control();
break;
사용자가 q를 누르면 샘플이 종료되고 모든 연결이 닫힙니다.
// 정리
Drfl.disconnect_rt_control();
Drfl.CloseConnection();