Computed Torque Method(CTM)

In this example, the computed torque control method (described below) is implemented using IndySDK and it will be executed through the simulation (please note that this example has been verified only in simulations, not real robots)

Summary of Controller

To begin with, the CTM-based position control is briefly summarized. The robot dynamics is given as follows: \begin{align} \tau + J^{T}F_{ext} = M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q), \end{align} Here, $M(q)$, $C(q,\dot{q})$, and $g(q)$ are respectively the inertia matrix, Coriolis matrix, and gravity vector. $F_{ext}$ denotes the external force and $\tau$ is a control input.

As shown in below, the control input is calculated based on the robot model so as to cope with the nonlinear and coupled characteristics of the robot dynamics. This enables to configure the high performance position controller.

$$\begin{align} \tau = M(q)\ddot{q}_{ref} + C(q,\dot{q})\dot{q} + g(q) - J^{T}F_{ext} \end{align}$$

Consequently, this make the robot behave as the following linear system.

$$\begin{align} \ddot{q} = \ddot{q}_{ref} \end{align}$$

Defining the reference acceleration $\ddot{q}_{ref}$ given as

$$\begin{align} \ddot{q}_{ref} = \ddot{q}_{des} + K_{v}\dot{e} + K_{p}e, \end{align}$$

here $e = q_{des} - q$, eventually, the robot is controlled to satisfy the following second order ordinary differential equation:

$$\begin{align} \ddot{e} + K_{v}\dot{e} + K_{p}e = 0. \end{align}$$

It is clear that this satisfies the exponential stability, and consequently the error converges to the equilibrium point (i.e., $q_{des}$ $\rightarrow$ $q$).

Example Code

To begin with, let's generate the joint control component. Then, include "DynamicAnalysis6D.h" and define the type "DynamicAnalysis" to use the robot dynamic models like follows:

#include <Indy/DynamicAnalysis6D.h>
//type definition
typedef typename ROBOT::JointMat JointMat;
typedef NRMKFoundation::IndySDK::DynamicAnalysis6D DynamicAnalysis;

Next, fill in the computeControlTorq() function like follows. Here, JointDynamics function returns inertia matrix, Coriolis matrix, and gravity vector to $M$, $C$, $g$ arguments respectively.

JointMat M, C;
JointVec g;


robot.computeDynamicsParams(LieGroup::Vector3D(0,0,-GRAV_ACC),M,C,g);

//possible to use DynanmicAnalysis's library: JointDynamics
//instead of using AbstractRobot6D's library: computeDynamicsParams
//DynamicAnalysis::JointDynamics(robot, M, C, g, LieGroup::Vector3D(0,0,-GRAV_ACC));

JointMat Kp, Kv;
JointVec tauCTM;
JointVec qddot_ref;

Kp.setZero();
Kv.setZero();

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;