기능
외부 제어기에서 모터 토크 제어를 하는 함수입니다.
인수
|
인수명 |
자료형 |
기본값 |
설명 |
|
fMotorTor |
float[6] |
- |
타겟 모터 토크 [Nm] |
|
fTargetTime |
float |
- |
타겟 시간 [s] |
-
Asnyc 명령어입니다. 호출된 후 다음 명령줄로 넘어갑니다.
-
fTargetTime 에 fMotorTor 이 되도록 제어합니다.
-
fTargetTime <= controller’s control period(=1ms)인 경우 보간없이 바로 해당 모터 토크로 제어합니다.
-
fMotorTor에 도달할 때까지 다음 명령이 들어오지 않으면, 마지막 들어온 토크를 유지합니다.
-
단, 안전을 위해서 1[s] 동안 다음 명령이 들어오지 않으면 Time-Out으로 에러를 발생시키고 정지합니다.
-
H 시리즈의 경우, 2축에 중력 보상 장치가 있어서 입력한 모터 토크와 중력 보상 장치에서 발생하는 토크가 더해져서 로봇을 움직입니다. 외부 제어기로 출력하는 gravity_torque는 (=link gravity torque – 중력 보상 장치 토크)를 뜻합니다.
리턴
|
값 |
설명 |
|
0 |
오류 |
예제
C++
// 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 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 trq_g[NUMBER_OF_JOINT] = {0.0, };
float trq_d[NUMBER_OF_JOINT] = {0.0, };
float kp[NUMBER_OF_JOINT] = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0}; // have to tune
float kd[NUMBER_OF_JOINT] = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0}; // have to tune
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);
memcpy(trq_g, drfl.read_data_rt()->gravity_torque, sizeof(float)*6);
// make trajectory
TrajectoryGenerator(q_d, q_dot_d); // Custom Trajectory Generation Function
// gravity compensation + pd control
for (int i=0; i<6; i++) {
trq_d[i] = trq_g[i] + kp[i]*(q_d[i] – q[i]) + kd[i]*(q_dot_d[i] – q_dot[i]);
}
drfl.torque_rt(trq_d, 0);
if(time > plan1.time)
{
time=0;
Drfl.stop(STOP_TYPE_SLOW);
break;
}
rt_task_wait_period(NULL); // RTOS function
}