Skip to content

Computed Torque 제어

본 예제에서는 아래 그림에 설명 된 Computed Torque 제어를 IndySDK를 이용하여 구현합니다.


그림 1. Computed Torque Method (CTM)

1) 첫 번째로 로봇의 동역학 모델을 사용하기 위해서 DynamicAnalysis6D.h 를 선언하고 타입 DynamicAnalysis 를 정의합니다.

1
2
#include <Indy/DynamicAnalysis6D.h>
typedef NRMKFoundation::IndySDK::DynamicAnalysis6D DynamicAnalysis;

2) 다음으로 computeControlTorq() 함수에 아래 코드과 같이 작성합니다. 여기서 JointDynamics 함수는 M, C, g에 각각 로봇의 관성행렬, 코리올리 행렬 및 중력벡터를 반환합니다.

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
JointMat M, C;
JointVec g;
DynamicAnalysis::JointDynamics(robot, M, C, g, LieGroup::Vector3D(0,0,-GRAV_ACC));

JointMat Kp, Kv;
JointVec tauCTM;
JointVec qddot_ref;

for (int i=0; i<JOINT_DOF; ++i)
{
  switch(i)
  {
  case 0:
  case 1:
    Kp(i,i) = 70;
    Kv(i,i) = 55;
    break;
  case 2:
    Kp(i,i) = 40;
    Kv(i,i) = 30;
    break;
  case 3:
  case 4:
    Kp(i,i) = 25;
    Kv(i,i) = 15;
    break;
  case 5:
    Kp(i,i) = 18;
    Kv(i,i) = 3;
    break;
  }
}

qddot_ref = qddotDesired + Kv*(qdotDesired - robot.qdot()) + Kp*(qDesired - robot.q());
tauCTM = M*qddot_ref + C*robot.qdot() + g;

torque = tauCTM;