Skip to main content
Skip table of contents

CDRFLEx.servoj_rt

기능

외부 제어기에서 조인트 위치 제어를 하는 함수입니다.

주의

  • 현재 servoj_rt 명령어는 안정화가 되지 않았습니다. 통신 상황에 따라서 Jerky한 모션이 나올 수 있어서, 타겟 시간(fTargetTime)을 튜닝하여 느린 반응 속도로 사용하거나, speedj_rt 명령어를 이용하여 위치 제어하는 것을 추천합니다.

인수

인수명

자료형

기본값

설명

fTargetPos

float[6]

-

타겟 조인트 위치 [deg]

fTargetVel

float[6]

-

타겟 조인트 속도 [deg/s]. null값을 입력하면, 입력했던 타겟 조인트 위치 기반으로 자동 계산됩니다.

fTargetAcc

float[6]

-

타겟 조인트 가속도 [deg/s2]. null값을 입력하면, 입력했던 타겟 조인트 위치 기반으로 자동 계산됩니다.

fTargetTime

float

-

타겟 시간 [s]

알아두기

  • Asnyc 명령어입니다. 호출된 후 다음 명령줄로 넘어갑니다.
  • 내부 프로파일은 fTargetTime (fTargetPos, fTargetVel, fTargetAcc) 에 도달하도록 보간됩니다.
  • fTargetTime <= controller’s control period(=1ms) 입력하면 보간없이 바로 해당 위치로 제어합니다.
  • 모션 중 다음 명령이 들어오지 않으면, 설정된 전역 가속도 제한값 기준으로 감속합니다.

주의

  • 현재 servoj_rt 명령어가 안정화되지 않아서 Jerky 한 모션이 나올 수 있습니다. 시스템 상황에 따라서 fTargetTime을 통신 주기보다 크게 설정하여 사용하십시오. 현재 버전에서는 1 [ms] 주기로 servoj_rt 명령어를 호출하는 기준으로 fTargetTime >= 20 [ms] 보다 크게 설정하여 사용하거나 speedj_rt를 이용하여 위치 제어하는 것을 권장합니다.
  • 현재 버전에서는 Operation Speed [%] 와 연동되지 않습니다.

리턴

설명

0

오류

1

성공

예제

CPP
// main.cpp
CDRFLEx drfl;
drfl.connect_rt_control();
string version = “v1.0”;
float period = 0.001; // 1 msec
int losscount = 4;
Drfl.set_on_rt_monitoring_data(OnRTMonitoringData);
drfl.set_rt_control_output(version, period, losscount);
drfl.start_rt_control();

float time = 0.0;
int count = 0;
float ratio = 20.0;
float q[NUMBER_OF_JOINT] = {0.0, };
float q_dot[NUMBER_OF_JOINT] = {0.0, };
float q_d[NUMBER_OF_JOINT] = {0.0, };
float q_dot_d[NUMBER_OF_JOINT] = {0.0, };
float integral_v_error[NUMBER_OF_JOINT] = {0.0, };
while (1)
{
    time=(++count)*period;
    // get current state
    memcpy(q, drfl.read_data_rt()->actual_joint_position, sizeof(float)*6);
    memcpy(q_dot, drfl.read_data_rt()->actual_joint_velocity, sizeof(float)*6);

    // make trajectory
    TrajectoryGenerator(q_d, q_dot_d, q_ddot_d); // Custom Trajectory Generation Function

    // or make target position and target velocity, acceleration can be omitted
    TrajectoryGenerator(q_d);
    for (int i=0; i<NUMBER_OF_JOINT; i++) {
        q_dot_d[i] = -10000;
        q_ddot_d[i] = -10000;
    }

    drfl.servoj_rt(q_d, q_dot_d, q_ddot_d, ratio*period); // Currently tuning ratio

    if(time > plan1.time)
    {
        time=0;
        Drfl.stop(STOP_TYPE_SLOW);
        break;
    }

    rt_task_wait_period(NULL); // RTOS function
}      


JavaScript errors detected

Please note, these errors can depend on your browser setup.

If this problem persists, please contact our support.