COMPUTED TORQUE CONTROL

Lagrange-Euler Dynamics
i) Derive kinetic and potential energy
ii) Use Lagrange's equations of motion
Force, inertia, and Energy
Centripetal force : F cent =

mv 2
mrw 2 mr 2
r

linear velocity : v w r

coriolis force : Fcor 2mwo v

1
2

kinetic energy (k) : k mv 2

1
2

rotational kinetic energy : k rot I 2

potential energy (p) : P mgh
Dynamics of a Two-Link Planar Elbow Arm
1

1) kinetic energy & potential energy

1
2

For link1 : K1 m(a11 ) 2

P1 m1 ga1 s i n
1

For link2 : x2 a1 cos 2 a2 cos(1 2 )

y2 a1 sin 2 a2 sin(1 2 )
x 2 a1 sin 1 a2 (1 2 ) sin(1 2 )
y a cos a ( ) cos( )
2

velocity squared :
v2 x2 y 2 a1 1 a2 (1 2 ) 2 2a1a2 (1 12 ) cos 2
2

1
1
1
2
2
2
P2 m2 g y2 m2 g[a1 sin 1 a2 sin(1 2 )]

K 2 m2 v 2 2 m2 a1 2 1 2 m2 a 2 2 (1 2 ) 2 m2 a1 a 2 (1 2 12 ) cos 2

2) Lagrange's equation

d L L

)
dt q q

q 1 , q 1 , 1

2
2
2

L K P ( K1 K 2 ) ( P1 P2 )

1
1
2
2
2
2
(m1 m2 )a1 1 m2 a2 (1 2 ) 2 m2 a1a2 (1 12 ) cos 2
2
2
(m1 m2 ) ga1 sin 1 m2 ga2 sin(1 2 )

L
2
2
( m1 m2 ) a1 1 m2 a2 (1 2 ) m2 a1a2 ( 21 2 ) c o s 2

d L
2
2
( m1 m2 ) a1 1 m2 a2 (1 2 ) m2 a1a2 ( 21 2 ) c o s 2

dt 1
2
m2 a1a2 (212 1 ) sin 2

L
( m1 m2 ) ga1 cos1 m2 ga2 cos(1 2 )
1

L
2
m2 a2 (1 2 ) m2 a1a21 cos 2

2
d L
2
m2 a2 (1 2 ) m2 a1a21 cos 2 m2 a1a212 sin 2

dt 2
L
2
m2 a1a2 (1 12 ) sin 2 m2 ga2 cos(1 2 )
2

1 [( m1 m2 )a12 m2 a2 2 2m2 a1a2 c o s 2 ]1

2
2
[m2 a2 m2 a1a2 cos 2 ]2 m2 a1a2 (212 1 ) sin 2

(m1 m2 ) ga1 cos1 m2 ga2 cos(1 2 )

2 [m2 a2 2 m2 a1a2 cos 2 ]1 m2 a2 22 m2 a1a212 sin 2

m2 ga2 cos(1 2 )

(m1 m2 )a12 m2 a2 2 2m2 a1a2 c o s 2

2
m2 a2 m2 a1a2 c o s 2

2
m2 a2 m2 a1a2 c o s 2 1

2
m2 a 2
2

m2 a1a2 (212 12 ) sin 2 (m1 m2 ) ga1 cos1 m2 ga2 cos(1 2 )

2 sin
m2 ga2 cos(1 2 )
m
a
a

2 1 2 1
2

1
2

Manipulator in the standard form

Mx

cx

M (q)q
Inertia
matrix

kx

V (q, q )

G (q)

Coriolis/centripetal
vector

Gravity
vector
3

* M(q) is symmetric.

Computed-Torque Control
( feedback linearization of nonlinear system)
Consider robot arm dynamics :
M (q)q N (q, q ) d

,where

d :

---

disturbance/noise, : control torque

Define tracking error as

e(t ) qd (t ) q(t )

then,
e q d q
e qd q
e qd M 1 ( N d )

M (qd e) ( N d )
qd e M 1 ( N d )
e qd M 1 ( N d )

Defining control input u and disturbance function w ,

u qd M 1 ( N )

---

w M 1 d

then e u w
Defining state x :
e
x
e

Then, tracking error dynamics :

0
d e 0 I e 0

u w

dt e 0 0 e I
I

---

linear error system

The dynamic sytem itself is nonlinear, but the tracking error

dynamic is linear.
looks like feedback linearizing transformation
M (qd u) N

---

called "Computed-torque control law."

If we selects u(t) stabilizing so that e(t) goes to zero, then non

trajectory.
(1) Selecting control input u(t) as PD :
u

Kd e

Kp e

Then, equation becomes

M ( qd

Kd e

K p e)

Kp e

Now, we want to find gains for u.

Closed-loop error dynamics
e

Kd e

Kp e

Kp e

Kd e

e u w
():

Kd e

Kd e

Kp e

in state-space form :
d e
dt e

Kp

Kd e

Closed-loop characteristic equation :

Using | sI A | 0
c (s) S 2 K v S K p 0

Where, Kv diag{Kvi } ,

K p diag{K pi }

Error system is asymptotically stable if K vi and K pi are all positive

(using Routh-Hurwitz table)

[Ex] For the robot dynamic equation derived before, desired

trajectory qd (t ) has the components:
1d g1 sin(2t / T )
2d g2 cos(2t / T )

,where T=2s , amplitudes g1=g2=0.1 rad, kp=100, kv=20,

m1=1, m2=1, a1=1, a2=1,
initial condition, x=[0 0 0 0]:theta1, theta2, dtheta1,dtheta2
Simulate PD computed torque control.

[Example of MATLAB code]

%FILE NAME=ct_pd.m
%Computed torque method for 2-D robot arm
global m_inv;
global n;
global torq;
g=9.806;
g1=0.1;
g2=0.1;
fact=3.14;
kp=100;
kv=20;
m1=1;m2=1;a1=1;a2=1;
x=[0 0.1 0 0]; %theta1, dtheta1, theta2,dtheta2
temp=[0];
for i=0:500
ts=i*0.01;
tf=(i+1)*0.01;
6

rowx_out=size(x,1);
x_end=x(rowx_out,:);
th_d=[g1*sin(fact*ts) g2*cos(fact*ts)];
dth_d=[g1*fact*cos(fact*ts) -g2*fact*sin(fact*ts)];
ddth_d=[-g1*fact^2*sin(fact*ts) -g2*fact^2*cos(fact*ts)];
e=[th_d(1)-x_end(1) th_d(2)-x_end(2)];
de=[dth_d(1)-x_end(3) dth_d(2)-x_end(4)];
m=[(m1+m2)*a1^2+m2*a2^2+2*m2*a1*a2*cos(x_end(2))
m2*a2^2+m2*a1*a2*cos(x_end(2))
m2*a2^2+m2*a1*a2*cos(x_end(2))
m2*a2^2];
m_inv=inv(m);
temp=-m2*a1*a2*(2*x_end(3)*x_end(4)+x_end(4)^2)*sin(x_end(2));

n=[temp+(m1+m2)*g*a1*cos(x_end(1))+m2*g*a2*cos(x_end(1)+x_end(2
))
m2*a1*a2*x_end(3)^2*sin(x_end(2))+m2*g*a2*cos(x_end(1)+x_end(2))]'
;
s=[ddth_d(1)+kv*de(1)+kp*e(1) ddth_d(2)+kv*de(2)+kp*e(2)];
torq=[m(1,1)*s(1)+m(1,2)*s(2)+n(1) m(1,2)*s(1)+m(2,2)*s(2)+n(2)];
[t,x_o]=ode45('xdot',[ts tf],x_end);
rowx_o=size(x_o,1);
x(rowx_out+1,:)=x_o(rowx_o,:);
theta(i+1,:)=th_d;
ee(i+1,:)=e;
tq(i+1,:)=torq;
subplot(3,1,1);
plot(theta), title('position and velocity');
subplot(3,1,2);
plot(ee), title('position error');
subplot(3,1,3);

plot(tq), title('control torque');

end

%% FILE NAME=xdot.m
function x_dot=xdot(t,x)
global m_inv;
global n;
global torq;
x_dot=[
x(3)
x(4)
m_inv(1,1)*(-n(1)+torq(1))+m_inv(1,2)*(-n(2)+torq(2))
m_inv(1,1)*(-n(1)+torq(1))+m_inv(2,2)*(-n(2)+torq(2))
];

## Desired trajectories/Errors for angle and rate angle/Control

torques
(2) Selecting control input u(t) as PID :
u

Kp e

Ki

Kd e

edt )

Substituting u into computed-torque control ( M (qd u) N ),

M ( qd

Kp e

Ki

K d e)

Now, in order to find Kp, Ki and Kd, we consider closed-loop error

dynamic system.
e
K p e Ki
Kd e w
d
e
dt
e

Ki

Kp

Kd e

0w
1

Closed-loop error dynamics characteristic equation :

c (s)

By Routh test

S3

Kd S 2

Kp S

Ki

K ii K vi K pi
9

 Gain selection: integral gain should not be too large.

10

11

12