Skip to content

Computed Torque Method (CTM)

As described in below, this section implements the Computed Torque Control using IndySDK.


Computed Torque Method (CTM)

1) First, let us declare DynamicAnalysis6D.h and define the type DynamicAnalysis to use the dynamic model of the robot as follows:

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

2) Let us add the fwolloing code to computeControlTorq(). Here, the JointDynamics returns Inertia matrix, Coriolis matrix, and Gravity vector to M, C, and g, respectively.

 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;