최신 버전 정보는 1.33.5에서 확인하세요.
6.6.5 minimal_instruction_sample.cpp
이 예제(5_minimal_instruction_sample.cpp)는 API로 로봇을 제어하는 단계별 명령 흐름을 보여 줍니다.
모니터링 콜백을 등록하고,
컨트롤러 연결을 열고 닫고,
제어권(Control Authority)을 요청하고,
서보 온으로 전환하고 시스템/버전을 확인하고,
로봇 모드/시스템을 전환하고,
간단한 조인트 모션(
movej)을 실행하고,ikin,ikin_norm으로 역기구학(Inverse Kinematics)을 테스트한 다음,DRL 스크립트를 보내고 정지합니다.
각 기능은 숫자 키(1~9)에 대응하므로, 사용자가 작업을 하나씩 실행하면서 API가 어떻게 동작하는지 실시간으로 관찰할 수 있습니다.
이 예제의 경로는 다음과 같습니다.
API-DRFL/example/Linux_64/5_minimal_instruction_sample.cpp
예제는 다음에서 확인하세요. Doosan Robotics API-DRFL GitHub
전체 설명 영상은 다음에서 확인하세요. Doosan Robotics Official YouTube
설정 및 전역 변수
이 예제는 전역 컨트롤러 객체와 제어 상태를 추적하는 여러 플래그를 사용합니다.
#include "../../include/DRFLEx.h"
using namespace DRAFramework;
#undef NDEBUG
#include <assert.h>
CDRFLEx Drfl;
bool g_bHasControlAuthority = FALSE;
bool g_TpInitailizingComplted = FALSE;
bool g_mStat = FALSE;
bool g_Stop = FALSE;
bool moving = FALSE;
DRL 스크립트 문자열과 보조 플래그도 정의합니다.
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";
bool bAlterFlag = FALSE;
이 전역 변수들은 DRFLEx 인스턴스를 저장하고, 제어권이 부여되었는지와 알람이 기록되었는지를 추적하며, DRL 스크립트를 담고 있습니다. 전역 strDrl을 사용하는 메뉴 키는 없으며, 8 키는 자체 지역 변수 strDrl을 선언합니다.
콜백 등록
사용자가 1을 누르면 예제는 여러 모니터링 콜백과 이벤트 콜백을 등록합니다.
case '1':
{
// 콜백 함수 등록
Drfl.set_on_monitoring_data_ex(OnMonitoringDataExCB);
Drfl.set_on_monitoring_ctrl_io_ex(OnMonitoringCtrlIOExCB);
Drfl.set_on_log_alarm(OnLogAlarm);
// Drfl.set_on_disconnected(OnDisConnected);
Drfl.set_on_monitoring_state(OnMonitoringStateCB);
// Drfl.set_on_tp_initializing_completed(OnTpInitializingCompleted);
// 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);
std::cout << "Register Callbacks !! " << std::endl;
}
break;
이 콜백들은 확장 모니터링 데이터와 컨트롤 박스 I/O 데이터를 받고, 알람 정보를 출력하며(OnLogAlarm), 로봇 상태가 바뀔 때마다 출력합니다(OnMonitoringStateCB). OnDisConnected, TP 콜백, RT 모니터링 콜백의 등록 코드는 주석 처리되어 있습니다. OnMonitoringDataExCB와 OnMonitoringCtrlIOExCB는 아무것도 출력하지 않고 반환합니다.
콘솔 출력 예:
input key : 1
Register Callbacks !!
컨트롤러 연결
2를 누르면 컨트롤러와 TCP/IP 연결을 엽니다.
case '2':
{
// 연결 수립
assert(Drfl.open_connection("127.0.0.1"));
std::cout << "Open Connection !! " << std::endl;
}
break;
이 예제는 로컬 에뮬레이터(DART Platform / 가상 컨트롤러)를 가정해
127.0.0.1을 사용합니다.실제 로봇 컨트롤러를 사용할 때는 이 값을 실제 컨트롤러 IP로 바꾸세요.
샘플은
assert.h를 포함하기 전에NDEBUG정의를 해제합니다. 따라서assert()안의open_connection()호출은 모든 빌드에서 실행되며, false를 반환하면 프로그램이 중단됩니다.
콘솔 출력 예:
input key : 2
Open Connection !!
서보 온 및 모니터링 버전
4 키는 모니터링 버전을 설정하고, 컨트롤 박스 디지털 출력을 설정하고, 시스템 정보를 출력한 뒤 서보 온 상태로 전환합니다.
case '4':
{
if (!g_bHasControlAuthority) {
std::cout << "No control authority. Run step 3 first." << std::endl;
break;
}
// 서보 온 및 모니터링 버전 설정
Drfl.setup_monitoring_version(1);
Drfl.set_digital_output(GPIO_CTRLBOX_DIGITAL_INDEX_10, TRUE);
SYSTEM_VERSION tSysVerion = {'\0',};
Drfl.get_system_version(&tSysVerion);
cout << "System version: " << tSysVerion._szController << endl;
cout << "Library version: " << Drfl.get_library_version() << endl;
Drfl.set_robot_control(CONTROL_SERVO_ON);
int nRetryCount = 0;
ROBOT_STATE ePrevState = STATE_LAST;
int nSameStateCount = 0;
const int RETRY_INTERVAL = 10; // 10*500ms = 5초마다 재시도
while ((Drfl.get_robot_state() != STATE_STANDBY) || !g_bHasControlAuthority) {
ROBOT_STATE eState = Drfl.get_robot_state();
bool bSendCommand = false;
if (eState != ePrevState) {
ePrevState = eState;
nSameStateCount = 0;
bSendCommand = true;
} else if (++nSameStateCount >= RETRY_INTERVAL) {
nSameStateCount = 0;
bSendCommand = true;
}
if (bSendCommand) {
switch ((unsigned char)eState) {
case STATE_SAFE_STOP:
cout << ">> Recovery: SAFE_STOP -> resetting..." << endl;
Drfl.SetSafeStopResetType(SAFE_STOP_RESET_TYPE_DEFAULT);
Drfl.SetRobotControl(CONTROL_RESET_SAFET_STOP);
break;
case STATE_SAFE_OFF:
cout << ">> Recovery: SAFE_OFF -> servo on..." << endl;
Drfl.SetRobotControl(CONTROL_SERVO_ON);
break;
case STATE_SAFE_STOP2:
cout << ">> Recovery: SAFE_STOP2 -> recovering..." << endl;
Drfl.SetRobotControl(CONTROL_RECOVERY_SAFE_STOP);
break;
case STATE_SAFE_OFF2:
cout << ">> Recovery: SAFE_OFF2 -> recovering..." << endl;
Drfl.SetRobotControl(CONTROL_RECOVERY_SAFE_OFF);
break;
case STATE_NOT_READY:
cout << ">> Recovery: NOT_READY -> servo on..." << endl;
Drfl.SetRobotControl(CONTROL_SERVO_ON);
break;
default:
cout << ">> Unknown state: " << (int)eState << endl;
break;
}
}
this_thread::sleep_for(std::chrono::milliseconds(500));
if (++nRetryCount > 20) { // 제한 시간 10초
cout << ">> Timeout waiting for STANDBY (state=" << (int)eState << ")" << endl;
break;
}
}
if (Drfl.get_robot_state() == STATE_STANDBY && g_bHasControlAuthority)
std::cout << "Servo On !! " << std::endl;
else
std::cout << "Failed to reach STANDBY state." << std::endl;
}
break;
제어권이 없으면 이 단계는 메시지만 출력하고 아무것도 하지 않습니다.
setup_monitoring_version(1)은 새 모니터링 컨텍스트를 선택합니다.set_digital_output(GPIO_CTRLBOX_DIGITAL_INDEX_10, TRUE)는 컨트롤 박스 디지털 출력 10을 켭니다.set_robot_control(CONTROL_SERVO_ON)은 로봇을 서보 온 상태로 전환합니다.루프는 500 ms마다 로봇 상태를 확인합니다. 상태가 바뀔 때, 그리고 같은 상태가 유지되는 동안 5초마다
STATE_SAFE_STOP,STATE_SAFE_OFF,STATE_SAFE_STOP2,STATE_SAFE_OFF2,STATE_NOT_READY에 맞는 복구 명령을 보냅니다.루프는 로봇 상태 = STANDBY와 제어권 조건이 모두 충족되거나 약 10초가 지나면 끝납니다.
콘솔 출력 예:
input key : 4
System version: GF03020000
Library version: GL013300
Servo On !!
로봇 모드 및 시스템 설정
5 키는 AUTONOMOUS 모드와 REAL 시스템으로 전환합니다.
case '5':
{
//로봇 모드 및 시스템 설정
assert(Drfl.set_robot_mode(ROBOT_MODE_AUTONOMOUS));
assert(Drfl.set_robot_system(ROBOT_SYSTEM_REAL));
std::cout << "Set Auto Mode !!" << std::endl;
}
break;
ROBOT_MODE_AUTONOMOUS는 프로그램으로 모션을 실행할 수 있게 합니다.ROBOT_SYSTEM_REAL은 실제 로봇에서 모션을 실행한다는 뜻입니다. 가상 모드에서 실행하려면ROBOT_SYSTEM_VIRTUAL로 바꾸세요.두 호출 모두
assert()안에서 실행되므로, 하나라도 false를 반환하면 프로그램이 중단됩니다.
콘솔 출력 예:
input key : 5
Set Auto Mode !!
조인트 모션 실행
6 키는 movej로 간단한 조인트 공간(Joint Space) 모션을 실행합니다.
case '6':
{
//모션 실행
std::cout << "Move Start !! " << std::endl;
float p1[6] = {0, 0, -90, 0, 90, 0};
Drfl.movej(p1, 60, 30); // v=60, a=30으로 posj(0,0,10,0,10,0)으로 이동
// float p2[6] = {0, 0, 9, 0, 90, 0};
// Drfl.movej(p2, 30, 30); // v=30, a=30으로 posj(0,0,0,0,0,0)으로 복귀
//일부 매개변수만 골라 지정할 수 없습니다. time을 사용할 때도 속도와 가속도를 먼저 지정해야 합니다.
std::cout << "Move End !! " << std::endl;
LPROBOT_POSE pCur = Drfl.get_current_pose();
std::cout << " Current Pose : "
<< pCur->_fPosition[0] << ", "
<< pCur->_fPosition[1] << ", "
<< pCur->_fPosition[2] << ", "
<< pCur->_fPosition[3] << ", "
<< pCur->_fPosition[4] << ", "
<< pCur->_fPosition[5] << std::endl;
} break;
속도 60, 가속도 30으로 조인트 위치 (0, 0, -90, 0, 90, 0) deg로 이동합니다.
두 번째
movej는 주석 처리되어 있습니다.이동한 뒤 샘플은
get_current_pose()가 반환하는 현재 조인트 위치를 출력합니다.
콘솔 출력 예:
input key : 6
Move Start !!
Move End !!
기구학 테스트
7 키는 역기구학 함수 ikin과 ikin_norm을 보여 줍니다.
case '7':
{
// 기구학 함수를 사용한 모션
float x1[6] = {370.9, 719.7, 651.5, 90, -180, 0}; //ikin 변환에 사용할 작업 공간 포즈
float fIterThreshold[2] = {0.005, 0.01};
LPROBOT_POSE res = Drfl.ikin(x1, 255); //ROBOT_POSE는 float[6] 요소를 가집니다.
//ikin 결과 저장(얕은 복사)
float q1[6] = {0,};
for(int i=0; i<6; i++){
q1[i] = res->_fPosition[i]; //LPROBOT_POSE(반환값)에서 요소 읽기
}
std::cout << " Get ikin Success (Task Space -> Joint Space) !! " << std::endl;
std::cout << " Joint Values [q1]: "
<< q1[0] << ", "
<< q1[1] << ", "
<< q1[2] << ", "
<< q1[3] << ", "
<< q1[4] << ", "
<< q1[5] << std::endl;
this_thread::sleep_for(std::chrono::milliseconds(100));
LPINVERSE_KINEMATIC_RESPONSE res2 = Drfl.ikin_norm(x1, 255, COORDINATE_SYSTEM_BASE, 0); //ROBOT_POSE는 float[6] 요소를 가집니다.
//ikin_norm 결과 저장(얕은 복사)
float q2[6] = {0,};
for(int i=0; i<6; i++){
q2[i] = res2->_fTargetPos[i]; //LPROBOT_POSE(반환값)에서 요소 읽기
}
std::cout << " Get ikin_norm Success (Task Space -> Joint Space) !! " << std::endl;
std::cout << " Joint Values [q2]: "
<< q2[0] << ", "
<< q2[1] << ", "
<< q2[2] << ", "
<< q2[3] << ", "
<< q2[4] << ", "
<< q2[5] << std::endl;
// Drfl.movej(q1, 60, 30);
}
break;
ikin과ikin_norm은 작업 공간(Task Space) 포즈x1로부터 조인트 공간 해를 계산합니다. 샘플은 관절 형상(Solution Space)으로 255를 전달합니다.샘플은 두 결과를 모두 출력합니다.
ikin결과로 이동하는movej는 주석 처리되어 있습니다.
콘솔 출력 예:
input key : 7
Get ikin Success (Task Space -> Joint Space) !!
DRL 스크립트 실행
8 키는 movej 모션 두 개를 실행하는 DRL 스크립트를 보내고 실행합니다.
case '8':
{
// DRL 예제
string strDrl = "movej([0, 0, 10, 0, 10, 0], 60, 30)\nmovej([10, 20, 30, 40, 50, 60], 60, 30)\n";
//movej로 두 위치 사이를 이동하는 DRL 스크립트
Drfl.drl_start(ROBOT_SYSTEM_REAL, strDrl); //가상 로봇에서는 ROBOT_SYSTEM을 VIRTUAL로 설정하고 DRL 스크립트를 전달
Drfl.set_on_program_stopped(OnProgramStopped);
std::cout << "Sent DRL Script !!" << std::endl;
}
break;
등록된 OnProgramStopped 콜백이 프로그램 종료를 처리합니다.
void OnProgramStopped(const PROGRAM_STOP_CAUSE) {
Drfl.drl_stop();
// 50 msec 이내에 끝나는 작업만 하세요.
// assert(Drfl.SetRobotMode(ROBOT_MODE_MANUAL));
cout << "program stopped" << endl;
}
OnProgramStopped는 drl_stop()을 호출합니다. 콜백 안에서 호출하면 안 되는 함수는 콜백 함수 규칙을 참고하세요.
실행 중 DRL 프로그램이 정지하면 알람 로그가 나타날 수 있습니다.
input key : 8
Sent DRL Script !!
input key : Alarm Info: group(1), index(2007), param(), param(), param()
program stopped
이 결과로 DRL 스크립트가 전송되고, 프로그램이 실행되고, 정지 이벤트와 알람이 올바르게 처리되었음을 확인할 수 있습니다.
참고
샘플 위쪽에는 DRL 스크립트 문자열과 보조 플래그도 정의되어 있습니다.
8 키는 자체 지역 변수 strDrl을 사용하며, 아래의 전역 strDrl은 어떤 메뉴 키에서도 사용하지 않습니다.
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";
Xenomai / 실시간 환경 참고 사항
샘플을 __XENO__ 매크로와 함께 컴파일하면 다음 코드 경로가 활성화됩니다.
#ifdef __XENO__
#include <rtdk.h>
#include <native/task.h>
#include <native/timer.h>
#else
//#include "stdafx.h"
//#include <Windows.h>
//#include <process.h>
//#include <conio.h>
#endif // __XENO__
메인 루프에서는 실시간 태스크를 고정 주기로 스케줄링합니다.
#ifdef __XENO__
unsigned long overrun = 0;
const double tick = 1000000; // 1ms 주기
rt_task_set_periodic(nullptr, TM_NOW, tick);
if (rt_task_wait_period(&overrun) == -ETIMEDOUT) {
std::cout << __func__ << ": \x1B[37m\x1B[41mover-runs: " << overrun
<< "\x1B[0m\x1B[0K" << std::endl;
}
#else
std::this_thread::sleep_for(std::chrono::microseconds(1000));
#endif // __XENO__ 끝
비실시간 환경에서는 루프가
std::this_thread::sleep_for로 대기만 합니다.Xenomai 실시간 환경에서는
rt_task_set_periodic과rt_task_wait_period로 루프를 구동해 결정론적 타이밍을 확보하고 오버런을 감지합니다.
이 블록들은 DRFLEx API 로직과는 관계가 없으며, 엄격한 실시간 제어(Real-time Control) 환경에 샘플을 배포할 때만 필요합니다.