Computed Torque Method (CTM)
This section implements the Computed Torque Control described in Figure 1 using IndySDK.
Figure 1. 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:
| #include <Indy/DynamicAnalysis6D.h>
typedef NRMKFoundation::IndySDK::DynamicAnalysis6D DynamicAnalysis;
|
2) Let us add the Code 1 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;
|