CSCI 545: Introduction to Robotics Fall 2019

Lecture #23: Dynamics of 2 DOF Manipulator

Scribe: Jiahui Li, Shuyang Zhao, Saloni Takawale, Chaitrali Kshirsagar, Yogesh Gajjar

13th November 2019

Contents
1 Recap: Dynamics of Robotic Manipulators 2

2 König Theorem 2

3 Dynamics for 2 DOF Manipulator 3

4 Direct and Inverse Dynamics 4

4.1 Direct Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
4.2 Inverse Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

5 Control 6
5.1 Decentralized Approach . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
5.2 Centralized Approach . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
5.3 Example: Gravity Compensation . . . . . . . . . . . . . . . . . . . . . . . . . 7

1
CSCI 545: Introduction to Robotics Fall 2019

1 Recap: Dynamics of Robotic Manipulators

In the previous lecture, we discussed derivation of kinetic energy for manipulator link,
kinematic relation for link is:
ṗ = p˙c + S(ω )r, ṗ = p˙c + r....(1)
where, S(ω ) is skew-symmetric matrix and ω is angular velocity, p is velocity, pc is
the velocity of the center of mass of the rigid body.
And Rwe discussed kinetic energy(T) of link:
T = v 12 ρ ṗ T ṗdv..... (2) R
ρ(r ) is mass distribution ; mass m = v ρ( p)dv

2 König Theorem
In kinetics, König’s theorem or König’s decomposition is a mathematical relation derived
by Johann Samuel König that assists with the calculation of kinetic energy of bodies and
systems of particles. The theorem can be applied to rigid bodies(manipulator link),
stating that the kinetic energy T of a rigid body(manipulator link) can be written as:
Ti = 12 mi v2i + 12 ωiT Ii ωi
where m is the mass of the rigid body; v is the velocity of the center of mass of the
rigid body and is the angular velocity of the rigid body. ωi Ii should be expressed in the
same reference frame, but the product ωiT Ii ωi is invariant w.r.t. any chosen frame.

The above formula can be obtained by the formula (1) and (2) from the recap section.

T = v 21 ρ ṗ T ṗdv where p is the velocity and v is the mass of the rigid body.
R

= 21 v ρ(|| ṗ||2 + 2 p˙c S(ω )r + (S(ω )r )T (S(ω )r ))dv where pc is the velocity of the centre
R
of the mass of the rigid body

• 21 v ρ|| ṗ||2 dv = 21 p2c v which is the translational kinetic energy (point mass of CoM)
R
R R
• V ρ( p˙c S(ω )r )dv = p˙c S(ω ) v rdv = 0

• 12 v ρ((S(ω )r T (S(ω )r )))dv = 12 v ρ((−S(r )ω )T (−S(r )ω ))dv since ωxr = rxω and
R R
S ( ω )r = − S (r ) ω
= 12 v ρω T S(r )T S(r )ωdv
R

= ω T v ρS(r )T (r )dvω v ρS(r )T (r )dv

R R

= 21 ω T Iω rotational kinetic energy (of the whole body)

Then we obtained the formula Ti = 21 mi v2i + 12 ωiT Ii ωi

2
CSCI 545: Introduction to Robotics Fall 2019

3 Dynamics for 2 DOF Manipulator

There is a two-link robot arm shown as below:

Figure 1: Two link Robot Arm

According to König’s theorem:

Ti = 12 mi v2i + 12 ωiT Ii ωi
where mi is the mass of the ith link; vi is the velocity of the center of mass of the ith
link and ωi is the angular velocity of the ith link.
So total kinetic energy T for the above two-link robot arm:
Ti = 21 m1 v21 + 12 ω1T I1 ω1 + 21 m2 v22 + 12 ω2T I2 ω2
= 12 m1 ( x˙1 2 + y˙1 2 ) + 21 p˙1 T I1 p˙1 + 21 m2 ( x˙2 2 + y˙2 2 ) + 12 [0, 0, p˙1 + p˙2 ] I2 [0, 0, p˙1 + p˙2 T ]

where ( xi , yi ) represents the position of the center of mass of the ith link; qi is the
angular between ith link and (i − 1)th link(q1 is the angular between 1st link and ground).

Based on structure of robot arm, there exist equations below:

x1 = r1 c1
x2 = l1 c1 + r2 c12
y1 = r1 s1
y2 = l1 s1 + r2 s12
x˙1 = −r1 s1 q˙1

3
CSCI 545: Introduction to Robotics Fall 2019

x˙2 = −l1 s1 q˙1 − r2 s12 (q1˙ + q2 ˙)

y˙1 = r1 c1 q˙1
˙ = l1 c1 q˙1 + r2 c12 (q˙1 + q˙2 )
y2
where c1 is cosine value of q1 ; s1 is sine value of q1 ; c12 is the cosine value of q1 + q2 ;
s12 is the sine value of q1 + q2.
According to Lagrangian:
L = T−V
where T is kinetic energy; V is potential energy.
Euler-Lagrange equations for two-link system:
d ∂L
dt ∂q˙i − ∂L
∂qi = τi for i = 1, 2
where i is Torque – generalized force performing work on qi .
Through all equations above, we can get:
B(q1 , q2 )[q¨1 , q¨2 ] T + C (q1 , q2 , q˙1 , q˙2 )[q˙1 , q˙2 ] + G (q1 , q2 ) = [τ1 , τ2 ] T

For general case:

B(q)q̈ + C (q, q̇) + G (q) = τ where B is inertia matrix; C is centrifugal and coriolis
forces; G is gravitational forces.

4 Direct and Inverse Dynamics

4.1 Direct Dynamics
The forward (direct) dynamics problem consists of determining for t > t0 , the acceler-
ation q̈(t), velocity q̇(t) and position q(t) resulting from torque τ (t) and possible end-
effector forces he (t) once the initial position and velocity of the system at time t0 are
known. Solving the forward dynamics equations is useful for manipulator simulation.

Forward dynamics allows the motion of the real physical system to be described
in terms of joint accelerations when a set of assigned joint torques is applied to the
manipulator; joint velocities and positions can be obtained by integrating the system of
non-linear differential equations.

For simulation of manipulator motion, once the state at the time instant tk is known
in terms of acceleration q̈(tk+1 ), then using Euler Integration method with integration
step of ∆t, the velocity q̇(tk+1 ) and position q(tk+1 ) at time instant tk+1 = tk + ∆t can be
computed as follows:
q̇(tk+1 ) = q̈(tk )∆t + q̇(tk ) ....... (a)
q(tk+1 ) = q̇(tk )∆t + q(tk ) ....... (b)

4
CSCI 545: Introduction to Robotics Fall 2019

Figure 2: Direct Dynamics

Runge-Kutta method is used to numerically integrate ordinary differential equations

by using a trial step at the midpoint of an interval to cancel out lower-order error terms.
This method is used quite widely because it is reasonably simple and robust.
If the system’s torque τ, inertia matrix B, gravitational forces G and centrifugal and
Coriolis forces C is known, then we can calculate acceleration q̈(td ) (desired acceleration)
as:
q̈(tk+1 ) = B−1 [(τ − C − G )]........ (c)
However, this may cause problem if the links of the manipulator are too small or too
large.

4.2 Inverse Dynamics

The inverse dynamics problem consists of determining the torque τ (t) which is required
to generate the motion specified by acceleration q̈(t), velocity q̇(t) and position q(t) once
the possible end-effector forces he (t) are known. Solving inverse dynamics is useful for
manipulator trajectory planning and control algorithm implementation.

Once a joint trajectory is specified in terms of positions, velocity and acceleration (typ-
ically as results of inverse kinematics procedure) and if end-effector forces are known,
inverse dynamics allows computation of torques to be applied to joints to obtain desired
motion. This computation is useful both for verifying feasibility of imposed trajectory
and also for compensating non-linear terms in dynamic model of manipulator.

For a n-joint manipulator, the number of operations required for computing inverse
dynamics is: O(n)

5
CSCI 545: Introduction to Robotics Fall 2019

Figure 3: Inverse Dynamics

5 Control
5.1 Decentralized Approach
Decentralized Approch is basically used to calculate τ ( InverseDynamics).
In decentralized approach, we consider few things such as

• If we have a common term, we treat them as disturbances.

• We assume that the link moves independently

• Has feedback and feedforward loop

The equation is given as :

τ = K P (qd − q) − K D (q˙d − q̇).....(d)

In this approach, we apply a PD control separately for each joint. A better approach
however is the Centralized Approach which takes into account the dynamics of the sys-
tem.

5.2 Centralized Approach

The centralize approach takes care of the dynamics. The equation is given as :
B(q)q̈ + C (q, q̇)q̇ + G (q) = B(qd )q¨d + C (qd , q˙d )q˙d + G (qd ) + K P (qd − q) + K D (q˙d − q̇)

To take into account error between desired and current, consider q̃= qd − q Then,
B(q)q̃¨ + C (q, q̇)q̃˙ = −K p q̃ − Kd q̃˙

If qd , q are very close then B(q) ≈ B(qd ).

B(q)q̃¨ + [C (q, q̇) + K D ]q̃˙ + K p q̃ = 0

6
CSCI 545: Introduction to Robotics Fall 2019

5.3 Example: Gravity Compensation

The gravity terms will tend to cause static positioning errors, so some robot manufactur-
ers include a gravity model, G(q), in the control law. The complete control law takes the
following form:
τ = G (q) + K p (qd − q) − K D q̇

Lyapunov’s method is used for examining the stability of a system. Lyapunov func-
tion is a function of energy of the system. Lyapunov’s method is concerned with de-
termining the stability of a system. To prove a system stable by Lyapunov’s method,
one is required to propose a generalized energy function V ( x ) that has the following
properties:

• V ( x ) = 0 if and only if x = 0.

• Energy of the system should be positive i.e. V ( x ) > 0 for all x 6= 0

• Derivative of energy of the system should be negative V̇ ( x ) ≤ 0. Here, V̇ ( x ) means

change in V ( x ) along all system trajectories.

For a function of two variables x1 , x2 , example of V ( x ) is schematically shown below:

Consider candidate Lyapunov Function:
V (q̇, q) = 12 q̇ T Bq̇ + 12 q̃Vp q̃
dV
dt = 12 q̇T Bq̇ + 11 q̇ Ḃq̇ + 12 q̇T Bq̈ + q̃T KD (−q̇)
To find acceleration: Bq̈ = τ − G − C
dV
dt = q̇T (τ − G − C ) + 21 q̇T Bq̇ − q̇K p q̃T
If A is symmetric matrix, x = x T Ax
∂α
∂x = ∂x T A
Ḃ − 2C = 0 (Holds true for any rigid body mechanics by preservation of energy)
Solving the equations further
dV
dt = q̇τ − q̇G − q̇K p q̃
For stability to make the above equation negative, only torque can be controlled thus
show that
q̇τ − q̇G − q̇K p q̃ < 0
τ − G − K p q̃ < 0

7
CSCI 545: Introduction to Robotics Fall 2019

h!

Figure 4:

References
[1] Robotics − Modelling, Planning and Control by B. Sicilliano

[2] Konig’s Theorem on

https://ipfs.io/ipfs/QmXoypizjW3WknFiJnKLwHCnL72vedxjQkDDP1mXWo6uco/
/KC3B6nig’stheorem(kinetics).html

[3] Lagrangian Dynamics on

http://www.diag.uniroma1.it/ deluca/rob2en/03LagrangianDynamics1.pdf