Manipulator Inverse Dynamics

Problem statement: Given joints angular positions, velocities and

accelerations, i.e. (q1,w1,a1), (q2,w2,a2), (q3,w3,a3), find the
resultant forces and torques on the manipulator links and joints
Manipulator Inverse Dynamics
Problem statement: Given joints angular positions, velocities and
accelerations, i.e. (q1,w1,a1), (q2,w2,a2), (q3,w3,a3), find the
resultant forces and torques on the manipulator links and joints:
mag3

IG3a3 G3

mag2 G2
G1 IG1a1
IG2a2 mag1
Manipulator Dynamics
Solution steps:

Step 1: Find the resultant acceleration vector at the center of

gravity for every link.

Step 2: Draw a free body diagram for the external forces and
moments applied to every link

Step 3: Equate the external forces and moments to the inertial

forces and moments at every link.

Step 4: Solve the system of equations for the torques and forces
in the x,y,z direction.
Manipulator Dynamics
aG1 = aO + a1 x rG1/O + w1 x (w1 x rG1/O)
..
a1 = q1
.
w1 = q1 .
Manipulator Dynamics
aO1 =..aO + a1 x rO1/O + w1 x (w1 x rO1/O)
a1 = q1
.
w1 = q1 .
Manipulator Dynamics
aG2 = aO1 + a2 x rG2/O1 + w2 x (w2 x rG2/O1)
.. ..
a2 = q1 + q2
. .
w2 = q1 + q2 .
Manipulator Dynamics
aO2 =..aO1 +.. a2 x rO2/O1 + w2 x (w2 x rO2/O1)
a2 = q1 + q2
. .
w2 = q1 + q2 .
Manipulator Dynamics
aG3 = aO2 + a3 x rG3/O2 + w3 x (w3 x rG3/O2)
.. ..
a3 = q1 + q2 + q3
. . .
w3 = q1 + q2 + q3 .
Manipulator Dynamics

LINK 1 O1y
O1x
T12

m1g
= mag1
Ox
IG1a1
T01
Oy FIG 2: INERTIAL FORCES & TORQUES
FIG 1: EXTERNAL FORCES & TORQUES
Manipulator Dynamics

LINK 2 O2y

O2x
O1y T23

m2g
= m2ag2

O1x IG2a2
T12
FIG 2: INERTIAL FORCES & TORQUES
FIG 1: EXTERNAL FORCES & TORQUES
Manipulator Dynamics

Payload, P
LINK 3

m3ag3
O2y

m3g
=
O2x IG3a3
T23
FIG 2: INERTIAL FORCES & TORQUES
FIG 1: EXTERNAL FORCES & TORQUES
Manipulator Dynamics
There are 9 unknowns forces / joint torques:
Ox , Oy, O1x, O1y, O2x, O2y, T01, T12, T23
And there are 9 equations, 3 for every link:
Link 1: Sum forces in X-direction
Link 1: Sum forces in Y-direction
Link 1: Sum torques in rotation motion

Link 2: Sum forces in X-direction

Link 2: Sum forces in Y-direction
Link 2: Sum torques in rotation motion

Link 3: Sum forces in X-direction

Link 3: Sum forces in Y-direction
Link 3: Sum torques in rotation motion
IN MATLAB: