Kinematics and Dynamics of Phantom(TM) model 1.

5 Haptic
Interface
Murat Cenk C
avusoglu, David Feygin
June 16, 2001
Abstract
In this report, derivation of the kinematic and dynamic equations of PHANToM(TM)
model 1.5 manipulator are presented.

Introduction

In this document, we will follow the notation of [1] in representing rigid body transformations, and kinematics and dynamics calculations. The units are in MKS system unless
otherwise noted.
Notation

The matrix

H = I33

0
0

(1)

is used to convert from homogeneous coordinates to Euclidean coordinates.

Kinematics

2.1

Forward Kinematics

Using the zero conguration and the naming convention shown in gure 1, the kinematic
conguration of the manipulator is characterized by the following vectors and points:


w1 =
w2 = w3 =
q1 =

0 1 0



T

1 0 0
0 0 l1

(2)

T
T

Phantom is a trademark of SensAble Technologies, Inc. of Cambridge, MA.

MKS: meter-kilogram-second.

(3)
(4)

l1

y
y

x
x
z

Spatial and Tool Frames

Figure 1: Zero conguration on the manipulator



q2 = q3 =

i =

0 l2 l1
wi qi
wi

T

(5)

, i = 1, 2, 3.

(6)

As it can be easily seen from the side and top view illustrations in gure 2, the forward
kinematic map is given as:

gst () =
where

R()
0 0 0

p()
1

(7)

R() = ew1 1 ew3 3 I33

and

p() = H e1 1 e2 2

I33
0 0 0

0
l2
0
1

0
0
0
1

(8)

0
R() l

2
+
.

0
0

(9)

In closed form,

gst () =

cos(1 ) sin(1 ) sin(3 ) cos(3 ) sin(1 )

sin(1 )(l1 cos(2 ) + l2 sin(3 ))
sin(3 )
l2 l2 cos(3 ) + l1 sin(2 )
0
cos(3 )
sin(1 ) cos(1 ) sin(3 ) cos(1 ) cos(3 ) l1 + cos(1 )(l1 cos(2 ) + l2 sin(3 ))
0
0
0
1
(10)

Tool Frame

Four bar linkage

y
x

Spatial Frame

Tool Frame
z
y

z
Spatial Frame

Figure 2: Side and top views

2.2

Inverse Kinematics

As this is a 3 DOF manipulator, the inverse kinematics problem is to nd the set of

(1 , 2 , 3 ) triples which move the manipulator to a desired end-eector position po =


T

po x po y po z
.
1 can be calculated by inspections as
1 = atan2(po x , po z + l1 ).

(11)

For, 2 and 3 consider the gure 3. In this gure



R =
r =

po 2x + (po z + l1 )2

(12)

po 2x + (po y l2 )2 + (po z + l1 )2 .

(13)

By inspection
= atan2(po y l2 , R).

(14)

l2

p -l2
y

R
Figure 3: Side view for inverse kinematics calculations
If we write the law of cosines in the upper small triangle
l12 + r 2 2l1 r cos() = l22
l2 + r 2 l22
).
= cos1 ( 1
2l1 r

(15)
(16)

In the physical workspace of the manipulator, > 0. Then,

2 = + .

(17)

To calculate 3 , we write the law of cosines for the same triangle, but this time for the angle
:
l12 + l22 2l1 l2 cos() = r 2
l2 + l22 r 2
)
= cos1 ( 1
2l1 l2

(18)
(19)

This angle is also positive in the physical workspace of the manipulator. Then,
3 = 2 +

2.3

.
2

(20)

Manipulator Jacobian

Spatial Jacobian of the manipulator is given by

J s () =

st 1
( g

i gst )

l1 l1 sin(1 ) sin(2 ) sin(1 )(l2 + l1 sin(2 ))

l1 (cos(1 ) cos(2 ))
0
l1 cos(2 )
0 l1 cos(1 ) sin(2 ) cos(1 )(l2 + l1 sin(2 ))
0
0
cos(1 )
1
0
0
0
0
sin(1 )
4

(21)

(22)

D
Spatial Frame
x

G
A

Figure 4: Segments used in dynamics analysis

Body Jacobian of the manipulator is calculated as
J b () =

1 gst
(gst

i )

(23)

l1 cos(2 ) + l2 sin(3 )
0
0
0
0
l1 cos(2 3 )

0
l1 sin(2 3 ) l2

.
0
0
1

0
0
cos(3 )
0
0
sin(3 )

(24)

Dynamics

Lagrangian formulation will be used to derive the dynamic equations of the manipulator.
For ease of analysis, we have identied the segments A through G shown on gure 4. For
each of the segments, or combinations of segments where appropriate, we will determine the
rotation and position vectors of the rigid body transformation between the body and spatial
frames, calculate b and v s , derive the kinetic and potential energies of the segment(s) and
nally calculate the inertial parameters of interest.
Note that the spatial frame used in dynamics calculations is centered at a dierent point
than the one used in kinematic analysis. This is to simplify calculations, and does not eect
the results.
Due to this change of coordinate frames, the vectors, points and the twists which characterize the conguration are changed as follows:


w1 =

0 1 0

T

(25)

x
Spatial Frame
z

l2 / 2

A Frame

l1

Figure 5: Segment A


w2 = w3 =
q1 = q2 = q3 =

i =

1 0 0
0 0 0

T

(26)

T

wi qi
wi

(27)

, i = 1, 2, 3

(28)

Also, the following parameters of the manipulator are measured as:

l1 = 0.215

(29)

l2 = 0.170

(30)

l3 = 0.0325

(31)

The material used in the manipulator is assumed to be aluminum , except where noted,
with a density of 1750kg/m3 . Also, the motors used are estimated to be Maxon RE-025055035.

3.1

Segment A

The rigid body rotation and the translation between the body frame of segment A and the
spatial frame (Fig. 5) are given by
Ra () = ew1 1 ew3 3 I33

pa () =

H e1 1 e2 2

0
0
l1
1

I33
0 0 0

(32)

0 R () l /2

a
2

+
. (33)
0

Then the potential and kinetic energy of segment A are

Va () = ma gpa y ()
1 sT
T
(va Ma vas + wab Ia wab ),
Ta () =
2
6

(34)
(35)

Spatial Frame

l3
z

C Frame
x
y

C
z

2
l1 / 2

Figure 6: Segment C
where Ma = ma I33 and Ia are respectively the translational and rotational inertia matrices
and
vas = pa
wab
3.1.1

(36)

(RaT R a ) .

(37)

Inertial Parameters

Segment A is approximated as a hollow aluminum cylinder with internal diameter 3 mm,

and outer diameter 8 mm. Then, the mass of segment A and its rotational inertia matrix
are:
ma = 0.0202

Ia

(38)

Iaxx
0
0

0
0
Iayy
0
0
Iazz

(39)

Iaxx = 0.4864 104

(40)
4

Iayy = 0.001843 10

(41)

Iazz = 0.4864 10

3.2

(42)

Segment C

The rigid body rotation and the translation between the body frame of segment C and the
spatial frame (Fig. 6) are given by
Rc () = ew1 1 ew2 2 I33

pc () =

H e1 1 e3 3

I33
0 0 0

0
l3
0
1
7

(43)

0 R ()
0

+
. (44)
0

l1 /2

Spatial Frame

BE Frame
x
x

Center of
Mass

l5

Figure 7: Segments B and E

Then, the potential and kinetic energy of segment C are

where Mc = mc I33
and

(45)
Vc () = mc gpc y ()
1 sT
T
Tc () =
(vc Mc vcs + wcb Ic wcb ),
(46)
2
and Ic are respectively the translational and rotational inertia matrices
vcs = pc
wcb

3.2.1

(47)

(RcT R c ) .

(48)

Inertial Parameters

Segment C is also approximated as a hollow aluminum cylinder with internal diameter 7.35
mm, and outer diameter 10.4 mm. Then, the mass of segment C and its rotational inertia
matrix are:
mc = 0.0249

Ic =

Icxx
0
0

0
0
Icyy
0
0
Iczz

(50)

Icxx = 0.959 104

(51)

(52)

Icyy = 0.959 10

Iczz = 0.0051 10

3.3

(49)

(53)

Segments B and E

The rigid body rotation between the body frame of segment BE and the spatial frame (Fig.
7) is given by
Rbe () = ew1 1 ew2 2 I33 .
8

(54)

The translation, however, is equal to zero. Then, the potential and kinetic energy of segment
BE are
Vbe () = mbe g sin(2 )l5
1 bT
b
(w Ibe wbe
),
Tbe () =
2 be

(55)
(56)

where mbe is the total mass of segment BE, Ibe is its rotational inertia matrix and
b
T
= (Rbe
wbe
Rbe ) .

3.3.1

(57)

Inertial Parameters

Segment BE is the combination of segment B, which is approximated as a hollow aluminum

cylinder with internal diameter 7.35 mm, and outer diameter 10.5 mm, and an aluminum
plate of dimensions 491331 mm oset from the center of the coordinate frame by 20 mm
in the z direction, and segment E, which is the electric motor actuating axis 2. Then, the
total mass of segment BE and its rotational inertia matrix are:
mbe = 0.2359

Ibe =

Ibexx
0
0

0
0
Ibeyy
0
0
Ibezz

Ibexx = 11.09 104

(58)
(59)
(60)

(61)

(62)

Ibeyy = 10.06 10
Ibezz = 0.591 10

The location of the center of mass of the segment BE is calculated as

l5 = 36.8mm

3.4

(63)

Segments D and F

The rigid body rotation between the body frame of segment DF and the spatial frame (Fig.
8) is given by
Rdf () = ew1 1 ew3 3 I33 .

(64)

and the translation is equal to zero. Then, the potential and kinetic energy of segment DF
are
Vdf () = mdf g cos(3 )l6
1 b T
b
(w Idf wdf
),
Tdf () =
2 df

(65)
(66)

where mdf is the total mass of segment DF, Idf is its rotational inertia matrix and
b
wdf

T
= (Rdf
Rdf ) .

(67)

Motor inertia has been added to the inertia around the x-axis with a transmission ratio of 11.6:1.0 .

DF Frame
y

z
x

Center of
Mass

l6

Spatial Frame

F
2

Figure 8: Segments D and F

3.4.1

Inertial Parameters

Segment DF is the combination of segment D, which is the electric motor actuating axis 2,
and segment F, which is approximated as the combination of two stainless steel rods lying
parallel to the x-axis of length 63 mm, diameter 5 mm and oset from the origin by 32.5
mm in the + and y directions, and two rectangular tubes lying parallel to the y-axis of
dimensions 739.59 mm, oset from the origin by 28 mm along the x-axis. Then, the
total mass of segment DF and its rotational inertia matrix are:
mdf
Idf

= 0.1906

Idf xx
0
0
0
0
Idf yy

0
0
Idf zz

(68)
(69)

Idf xx = 7.11 104

(70)

Idf yy = 0.629 10

(71)

Idf zz = 6.246 104

(72)

The location of the center of mass of the segment DF is calculated as

l6 = 52.7mm

3.5

(73)

Segment G - the Base

The base can only rotate around the y-axis of the spatial frame. Then, its potential energy
is zero, and its kinetic energy is given by
Vbase =

1
1 Ibaseyy 1 .
2

(74)

Motor inertia has been added to the inertia around the x-axis with a transmission ratio of 11.6:1.0 .

10

3.5.1

Inertial Parameters

The base is composed of a at semicircular plate of radius 88 mm and thickness 2.1 mm,
an aluminum ring of radius 88 mm and thickness 2.1 mm, a rectangular plate centered at
the point of rotation that is 154213.3 mm, a similar rectangular plate of dimensions
12089 mm, a cylinder centered at the point of rotation of height 21.3 mm and radius13
mm, two vertical shafts of dimensions 8899 mm oset from the point of rotation by
55 mm in the x direction, a steel bar parallel to the x-axis of length 120 mm and diameter
6.5 mm, a circular plate lying in the y-z plane oset from the point of rotation by 51 mm
and thickness 2.7 mm, a ring lying in the y-z plane oset from the point of rotation by
41 mm of radius 57 mm, thickness 2.2 mm and width (x dimension) of 18 mm, and the
motor rotor inertia 0.11 104 kg.m2 through a transmission ratio of 13.3:1.0. Then, the
only component of rotational inertia matrix of the base that is of interest is calculated as
Ibaseyy = 11.87 104

3.6

(75)

Equations of Motion

Lagrangian of the manipulator is given by

L = T V

(76)

= (Ta + Tc + Tbe + Tdf + Tbase ) (Va + Vc + Vbe + Vdf + Vbase )

(77)

(78)

Then, the dynamic equations are calculated as

L
d L

= i , i = 1, 2, 3.

dt i i

(79)

1
1
M11
0
0
C11 C12 C13
0
1

M22 M23 2 + C21 0 C23 2 + N2 = 2

0
0
M32 M33
C31 C32 0
N3
3
3
3

(80)

where
M11 =

 
1
+
+

M22 =

1
4

M23 =
M32 =
M33 =
C11 =

1
4
1
8

1
8
1
8




4Ia yy 4Ia zz + 4Idf yy 4Idf zz l2 ma 4l3 mc

2

4Ibe yy 4Ibe zz + 4Icyy 4Ic zz + l1 (4ma + mc ) cos(22 )

cos(23 ) + l1 (l2 ma + l3 mc ) cos(2 ) sin(3 )

(81)

(4(Ibe xx + Ic xx + l1 ma ) + l1 mc )
2
1

4Ia yy + 4Ia zz + 8Ibase yy + 4Ibe yy + 4Ibe zz + 4Ic yy + 4Ic zz + 4Idf yy + 4Idf zz + 4l1 ma + l2 ma + l1 mc + 4l3 mc

(82)

l1 (l2 ma + l3 mc ) sin(2 3 )

(83)

l1 (l2 ma + l3 mc ) sin(2 3 )

(84)

(4Ia xx + 4Idf xx + l2 ma + 4l3 mc )

(85)

2
2
2 sin(2 ) (4Ibe yy 4Ibe zz + 4Icyy 4Ic zz + 4l1 ma + l1 mc ) cos(2 ) + 2l1 (l2 ma + l3 mc ) sin(3 ) 2

 

2
2
+ 2 cos(3 ) 2l1 (l2 ma + l3 mc ) cos(2 ) + (4Ia yy + 4Ia zz 4Idf yy + 4Idf zz + l2 ma + 4l3 mc ) sin(3 ) 3

11

(86)

C12 =

1
8
1

2
(4Ibe yy 4Ibe zz + 4Icyy 4Ic zz + l1 (4ma + mc )) sin(22 ) + 4l1 (l2 ma + l3 mc ) sin(2 ) sin(3 ) 1

2
2
(4l1 (l2 ma + l3 mc ) cos(2 ) cos(3 ) (4Ia yy + 4Ia zz 4Idf yy + 4Idf zz + l2 ma + 4l3 mc ) sin(23 ))1
8
C21 = C12

C13 =

C23 =

l1 (l2 ma + l3 mc ) cos(2 3)3

C32 =
N2 =
N3 =

2
1
2
1
2

(88)
(89)
(90)

2
C31 = C13
1

(87)

(91)

l1 (l2 ma + l3 mc ) cos(2 3 )2

(92)

g(2l1 ma + 2l5 mbe + l1 mc ) cos(2 )

(93)

g(l2 ma + 2l3 mc 2l6 mdf ) sin(3 ).

(94)

is skew symmetric.
Note that M () is positive denite and (M () 2C(, ))

12

Appendix - Mathematica Code

13

References
[1] R. M. Murray and Z. Li and S. S. Sastry, A Mathematical Introduction to Robotic
Manipulation, CRC Press, Inc., 1994.

14

phdyn.nb

Initialization
We need to load variational methods, and matrix manipulation packages.


CalculusVariationalMethods


LinearAlgebraMatrixManipulation;

We also need to load the screw calculus library of Murray, Li, Sastry (MLS) [1].


~/surgery/phantom/PhantomDynamics/Screws.m


~/surgery/phantom/PhantomDynamics/RobotLinks.m

And some definitions...

I33  IdentityMatrix
3; I44  IdentityMatrix
4;
Origin  0, 0, 0, 1;
Vedge
what_ : what

2

3, what

1

3, what

1

2;
TopPart
a_ : a

1, 2, 3, 1;
SE3inv
g_ : BlockMatrix
Transpose
SubMatrix
g, 1, 1, 3, 3,
Transpose
SubMatrix
g, 1, 1, 3, 3 . SubMatrix
g, 1, 4, 3, 1,
 0, 0, 0, 1 ;

Lets try the example on pp. 1645 of MLS to test the EulerEquations[] function.
f
u_, v_, t_ : u
t, v
t . alpha  2 beta Cos
v
t, delta  beta Cos
v
t,
delta  beta Cos
v
t, delta . u
t, v
t 
2
EulerEquations
f
u, v, t, u
t, v
t, t
1








4 beta Sinvt u t v t  2 beta Sinvt v t2 
2
2 alpha  2 beta Cosvt u t  2 delta  beta Cosvt v t 
0,
beta Sinvt u t2  delta  beta Cosvt u t  delta v t  0
uv
u_, v_, t_ : u
t, v
t;
ff
u_, v_, t_ :
D
Transpose
uv
u, v, t, t . alpha  2 beta Cos
v
t, delta  beta Cos
v
t,
delta  beta Cos
v
t, delta . D
uv
u, v, t, t  2
EulerEquations
ff
u, v, t, u
t, v
t, t
1








4 beta Sinvt u t v t  2 beta Sinvt v t2 
2
2 alpha  2 beta Cosvt u t  2 delta  beta Cosvt v t 
0,
beta Sinvt u t2  delta  beta Cosvt u t  delta v t  0

Phantom Kinematics

Common Definitions
First, some common definitions:

phdyn.nb

wfk1  0, 1, 0; wfk1h  AxisToSkew
wfk1; qfk1  0, 0, l1;

wfk2  1, 0, 0; wfk2h  AxisToSkew
wfk2; qfk2  0, l2, l1;
wfk3  1, 0, 0; wfk3h  AxisToSkew
wfk3; qfk3  0, l2, l1;

Forward Kinematics
Rforkin
th1_, th2_, th3_ : SkewExp
wfk1h, th1 . SkewExp
wfk3h, th3 . I33;
pforkin
th1_, th2_, th3_ : Rforkin
th1, th2, th3 . 0, l2, 0 
TopPart
TwistExp
RevoluteTwist
qfk1, wfk1, th1 .
TwistExp
RevoluteTwist
qfk2, wfk2, th2 . RPToHomogeneous
I33, 0, l2, 0 . Origin;
gst
th1_, th2_, th3_ :
Simplify
RPToHomogeneous
Rforkin
th1, th2, th3, Flatten
pforkin
th1, th2, th3;
gst
ee1, ee2, ee3
Cosee1, Sinee1 Sinee3, Cosee3 Sinee1, Sinee1 l1 Cosee2  l2 Sinee3,
0, Cosee3, Sinee3, l2  l2 Cosee3  l1 Sinee2, Sinee1,
Cosee1 Sinee3, Cosee1 Cosee3, l1  Cosee1 l1 Cosee2  l2 Sinee3,
0, 0, 0, 1

Jacobian
Spatial Jacbian is given by
Js1  Transpose
HomogeneousToTwist

Simplify
D
gst
th1, th2, th3, th1 . SE3inv
gst
th1, th2, th3;
Js2  Transpose
HomogeneousToTwist

Simplify
D
gst
th1, th2, th3, th2 . SE3inv
gst
th1, th2, th3;
Js3  Transpose
HomogeneousToTwist

Simplify
D
gst
th1, th2, th3, th3 . SE3inv
gst
th1, th2, th3;
Js  BlockMatrix
Js1, Js2, Js3 
l1, l1 Sinth1 Sinth2, Sinth1 l2  l1 Sinth2,
0, l1 Costh2, l1 Costh1  Costh2,
0, l1 Costh1 Sinth2, Costh1 l2  l1 Sinth2, 0, 0, Costh1,
1, 0, 0, 0, 0, Sinth1
MatrixForm
Js
l1 l1 Sinth1 Sinth2 Sinth1 l2  l1 Sinth2 







0
l1 Costh2
l1 Costh1  Costh2 











0
l1
Costh1
Sinth2
Costh1
l2

l1
Sinth2










0
0
Costh1










1
0
0






0
Sinth1

Body Jacbian is calculated as

phdyn.nb

Jb1  Transpose
HomogeneousToTwist

Simplify
SE3inv
gst
th1, th2, th3 . D
gst
th1, th2, th3, th1;
Jb2  Transpose
HomogeneousToTwist

Simplify
SE3inv
gst
th1, th2, th3 . D
gst
th1, th2, th3, th2;
Jb3  Transpose
HomogeneousToTwist

Simplify
SE3inv
gst
th1, th2, th3 . D
gst
th1, th2, th3, th3;
Jb  BlockMatrix
Jb1, Jb2, Jb3 
l1 Costh2  l2 Sinth3, 0, 0, 0, l1 Costh2  th3, 0, 0, l1 Sinth2  th3, l2,
0, 0, 1, Costh3, 0, 0, Sinth3, 0, 0
MatrixForm
Jb
0
0 
l1 Costh2  l2 Sinth3






0
l1
Costh2

th3
0 











0
l1
Sinth2

th3
l2










0
0
1









Costh3
0
0 






Sinth3
0
0

Lets define the adjoint of gst. We will use it to check if the spatial and body Jacobians of the manipulator are consistent.
Ad
th1_, th2_, th3_ : BlockMatrix
 Rforkin
th1, th2, th3,
AxisToSkew
Flatten
pforkin
th1, th2, th3 . Rforkin
th1, th2, th3,
 0  I33, Rforkin
th1, th2, th3 ;
Simplify
Js  Simplify
Ad
th1, th2, th3 . Jb
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0

Yes, they check out.

Phantom Dynamics
Now, lets start calculating the equations governing the dynamics of the PHANToM.

Common Definitions
First, some common definitions:
w1  0, 1, 0; w1h  AxisToSkew
w1;
w2  1, 0, 0; w2h  AxisToSkew
w2;
w3  1, 0, 0; w3h  AxisToSkew
w3;

phdyn.nb

Segment A
Ra
th1_, th2_, th3_, t_ : SkewExp
w1h, th1
t . SkewExp
w3h, th3
t . I33;
wab
th1_, th2_, th3_, t_ :
Vedge
Transpose
Ra
th1, th2, th3, t . D
Ra
th1, th2, th3, t, t;
pa
th1_, th2_, th3_, t_ : Ra
th1, th2, th3, t . 0, l2  2, 0 
TopPart
TwistExp
RevoluteTwist
0, 0, 0, w1, th1
t .
TwistExp
RevoluteTwist
0, h, 0, w2, th2
t .
RPToHomogeneous
I33, 0, h, l1 . Origin;
vas
th1_, th2_, th3_, t_ : D
pa
th1, th2, th3, t, t;
Ma  ma  I33; Ia  DiagonalMatrix
Iaxx, Iayy, Iazz;
Va
th1_, th2_, th3_, t_ : ma  g  pa
th1, th2, th3, t

2

1;
Ta
th1_, th2_, th3_, t_ :
Simplify
1  2  Transpose
vas
th1, th2, th3, t . Ma . vas
th1, th2, th3, t 
Transpose
wab
th1, th2, th3, t . Ia . wab
th1, th2, th3, t;
Va
ee1, ee2, ee3, t
1
g ma h 1  Cosee2t  h Cosee2t 




l2 Cosee3t  l1 Sinee2t
2
Ta
ee1, ee2, ee3, t
1 1












4 Iayy  4 Iazz  4 l12 ma  l22 ma  4 l12 ma Cos2 ee2t 
8 2
4 Iayy Cos2 ee3t  4 Iazz Cos2 ee3t  l22 ma Cos2 ee3t 
4 l1 l2 ma Sinee2t  ee3t  4 l1 l2 ma Sinee2t  ee3t ee1 t2 
4 l12 ma ee2 t2  4 l1 l2 ma Sinee2t  ee3t ee2 t ee3 t 
4 Iaxx  l22 ma ee3 t2 

Segment C
Rc
th1_, th2_, th3_, t_ : SkewExp
w1h, th1
t . SkewExp
w2h, th2
t . I33;
wcb
th1_, th2_, th3_, t_ :
Vedge
Transpose
Rc
th1, th2, th3, t . D
Rc
th1, th2, th3, t, t;
pc
th1_, th2_, th3_, t_ : Rc
th1, th2, th3, t . 0, 0, l1  2 
TopPart
TwistExp
RevoluteTwist
0, 0, 0, w1, th1
t .
TwistExp
RevoluteTwist
0, h, 0, w3, th3
t .
RPToHomogeneous
I33, 0, h  l3, 0 . Origin;
vcs
th1_, th2_, th3_, t_ : D
pc
th1, th2, th3, t, t;
Mc  mc  I33; Ic  DiagonalMatrix
Icxx, Icyy, Iczz;
Vc
th1_, th2_, th3_, t_ : mc  g  pc
th1, th2, th3, t

2

1;
Tc
th1_, th2_, th3_, t_ :
Simplify
1  2  Transpose
vcs
th1, th2, th3, t . Mc . vcs
th1, th2, th3, t 
Transpose
wcb
th1, th2, th3, t . Ic . wcb
th1, th2, th3, t;
Vc
ee1, ee2, ee3, t
1
g mc h 1  Cosee3t  h  l3 Cosee3t 




l1 Sinee2t
2

phdyn.nb

Tc
ee1, ee2, ee3, t

1 1












4 Icyy  4 Iczz  l12 mc  4 l32 mc 
8 2
4 Icyy  4 Iczz  l12 mc Cos2 ee2t  4 l32 mc Cos2 ee3t 
4 l1 l3 mc Sinee2t  ee3t  4 l1 l3 mc Sinee2t  ee3t ee1 t2 
4 Icxx  l12 mc ee2 t2  4 l1 l3 mc Sinee2t  ee3t ee2 t ee3 t 
4 l32 mc ee3 t2 

Segments B and E
Rbe
th1_, th2_, th3_, t_ : SkewExp
w1h, th1
t . SkewExp
w2h, th2
t . I33;
wbeb
th1_, th2_, th3_, t_ :
Vedge
Transpose
Rbe
th1, th2, th3, t . D
Rbe
th1, th2, th3, t, t;
vbes
th1_, th2_, th3_, t_ : 0, 0, 0;
Mbe  mbe  I33; Ibe  DiagonalMatrix
Ibexx, Ibeyy, Ibezz;
Vbe
th1_, th2_, th3_, t_ : mbe  g  Sin
th2
t  l5;
Tbe
th1_, th2_, th3_, t_ :
Simplify
1  2  Transpose
vbes
th1, th2, th3, t . Mbe . vbes
th1, th2, th3, t 
Transpose
wbeb
th1, th2, th3, t . Ibe . wbeb
th1, th2, th3, t;
Vbe
ee1, ee2, ee3, t
g l5 mbe Sinee2t
Tbe
ee1, ee2, ee3, t
1







Ibeyy  Ibezz  Ibeyy  Ibezz Cos2 ee2t ee1 t2  2 Ibexx ee2 t2 
4

Segments D and F
Rdf
th1_, th2_, th3_, t_ : SkewExp
w1h, th1
t . SkewExp
w3h, th3
t . I33;
wdfb
th1_, th2_, th3_, t_ :
Vedge
Transpose
Rdf
th1, th2, th3, t . D
Rdf
th1, th2, th3, t, t;
vdfs
th1_, th2_, th3_, t_ : 0, 0, 0;
Mdf  mdf  I33; Idf  DiagonalMatrix
Idfxx, Idfyy, Idfzz;
Vdf
th1_, th2_, th3_, t_ : mdf  g  Cos
th3
t  l6;
Tdf
th1_, th2_, th3_, t_ :
Simplify
1  2  Transpose
vdfs
th1, th2, th3, t . Mdf . vdfs
th1, th2, th3, t 
Transpose
wdfb
th1, th2, th3, t . Idf . wdfb
th1, th2, th3, t;
Vdf
ee1, ee2, ee3, t
g l6 mdf Cosee3t
Tdf
ee1, ee2, ee3, t
1







Idfyy  Idfzz  Idfyy  Idfzz Cos2 ee3t ee1 t2  2 Idfxx ee3 t2 
4

Segment Base
Vbase
th1_, th2_, th3_, t_ : 0;
Tbase
th1_, th2_, th3_, t_ : Simplify
1  2  D
th1
t, t  Ibaseyy  D
th1
t, t;

phdyn.nb

Vbase
ee1, ee2, ee3, t

0
Tbase
ee1, ee2, ee3, t
1





Ibaseyy ee1 t2
2

Equations of Motion of the Whole Structure

The Lagrangian of the manipulator is
Simplify
Ta
th1, th2, th3, t  Tc
th1, th2, th3, t 
Tbe
th1, th2, th3, t  Tdf
th1, th2, th3, t  Tbase
th1, th2, th3, t 
Va
th1, th2, th3, t  Vc
th1, th2, th3, t  Vbe
th1, th2, th3, t 
Vdf
th1, th2, th3, t  Vbase
th1, th2, th3, t
1











4 Iayy  4 Iazz  8 Ibaseyy  4 Ibeyy  4 Ibezz 
16
4 Icyy  4 Iczz  4 Idfyy  4 Idfzz  4 l12 ma  l22 ma  l12 mc  4 l32 mc 
4 Ibeyy  4 Ibezz  4 Icyy  4 Iczz  4 l12 ma  l12 mc Cos2 th2t 
4 Iayy Cos2 th3t  4 Iazz Cos2 th3t  4 Idfyy Cos2 th3t 
4 Idfzz Cos2 th3t  l22 ma Cos2 th3t  4 l32 mc Cos2 th3t 
4 l1 l2 ma Sinth2t  th3t  4 l1 l3 mc Sinth2t  th3t 
4 l1 l2 ma Sinth2t  th3t  4 l1 l3 mc Sinth2t  th3t th1 t2 
2 8 g h ma  8 g h mc  4 g l2 ma Costh3t  8 g l3 mc Costh3t 
8 g l6 mdf Costh3t  8 g l1 ma Sinth2t  8 g l5 mbe Sinth2t 
4 g l1 mc Sinth2t  4 Ibexx  4 Icxx  4 l12 ma  l12 mc th2 t2 
4 l1 l2 ma  l3 mc Sinth2t  th3t th2 t th3 t  4 Iaxx th3 t2 
4 Idfxx th3 t2  l22 ma th3 t2  4 l32 mc th3 t2 

Lets write the Lagrangian as a function

L
th1_, th2_, th3_, t_ : Simplify
Ta
th1, th2, th3, t  Tc
th1, th2, th3, t 
Tbe
th1, th2, th3, t  Tdf
th1, th2, th3, t  Tbase
th1, th2, th3, t 
Va
th1, th2, th3, t  Vc
th1, th2, th3, t  Vbe
th1, th2, th3, t 
Vdf
th1, th2, th3, t  Vbase
th1, th2, th3, t

1

1;

This is the dynamics equaitons

phdyn.nb

DyEq  FullSimplify
EulerEquations
L
th1, th2, th3, t, th1
t, th2
t, th3
t, t

1






2 th1 t 4 Ibeyy  4 Ibezz  4 Icyy  4 Iczz  l12 4 ma  mc Sin2 th2t 
8
4 l1 l2 ma  l3 mc Sinth2t Sinth3t th2 t 
4 l1 l2 ma  l3 mc Costh2t Costh3t 
4 Iayy  4 Iazz  4 Idfyy  4 Idfzz  l22 ma  4 l32 mc Sin2 th3t th3 t 
4 Iayy  4 Iazz  8 Ibaseyy  4 Ibeyy  4 Ibezz  4 Icyy 
4 Iczz  4 Idfyy  4 Idfzz  4 l12 ma  l22 ma  l12 mc  4 l32 mc th1 t 
4 Ibeyy  4 Ibezz  4 Icyy  4 Iczz  l12 4 ma  mc Cos2 th2t th1 t 
4 Iayy  4 Iazz  4 Idfyy  4 Idfzz  l22 ma  4 l32 mc Cos2 th3t th1 t 
8 l1 l2 ma  l3 mc Costh2t Sinth3t th1 t  0,
1





4 g 2 l1 ma  2 l5 mbe  l1 mc Costh2t 
8
4 Ibeyy  4 Ibezz  4 Icyy  4 Iczz  l12 4 ma  mc Sin2 th2t 
4 l1 l2 ma  l3 mc Sinth2t Sinth3t th1 t2 
4 l1 l2 ma  l3 mc Costh2t  th3t th3 t2 
2 4 Ibexx  Icxx  l12 ma  l12 mc th2 t 
4 l1 l2 ma  l3 mc Sinth2t  th3t th3 t  0,
1






8
4 g l2 ma  2 l3 mc  2 l6 mdf Sinth3t  4 l1 l2 ma  l3 mc Costh2t Costh3t 
4 Iayy  4 Iazz  4 Idfyy  4 Idfzz  l22 ma  4 l32 mc Sin2 th3t th1 t2 
4 l1 l2 ma  l3 mc Costh2t  th3t th2 t2  Sinth2t  th3t th2 t 
2 4 Iaxx  4 Idfxx  l22 ma  4 l32 mc th3 t 
0

With signs corrected to follow the convention in MLS, the motor torques at each axis are equal to:
Tau1 
Collect
DyEq

1

1, th1
t, th2
t, th3
t, th1
t, th2
t, th3
t
1
th1 t 




4 Ibeyy  4 Ibezz  4 Icyy  4 Iczz  l12 4 ma  mc Sin2 th2t 
4
4 l1 l2 ma  l3 mc Sinth2t Sinth3t th2 t 
1





4 l1 l2 ma  l3 mc Costh2t Costh3t 
4
4 Iayy  4 Iazz  4 Idfyy  4 Idfzz  l22 ma  4 l32 mc Sin2 th3t th3 t 
1





4 Iayy  4 Iazz  8 Ibaseyy  4 Ibeyy  4 Ibezz 
8
4 Icyy  4 Iczz  4 Idfyy  4 Idfzz  4 l12 ma  l22 ma  l12 mc  4 l32 mc 
1





4 Ibeyy  4 Ibezz  4 Icyy  4 Iczz  l12 4 ma  mc Cos2 th2t 
8
1





4 Iayy  4 Iazz  4 Idfyy  4 Idfzz  l22 ma  4 l32 mc Cos2 th3t 
8
l1 l2 ma  l3 mc Costh2t Sinth3t
th1 t

phdyn.nb

Tau2 
Collect
DyEq

2

1, th1
t, th2
t, th3
t, th1
t, th2
t, th3
t
1





g 2 l1 ma  2 l5 mbe  l1 mc Costh2t 
2
1





4 Ibeyy  4 Ibezz  4 Icyy  4 Iczz  l12 4 ma  mc Sin2 th2t 
8
4 l1 l2 ma  l3 mc Sinth2t Sinth3t th1 t2 
1





l1 l2 ma  l3 mc Costh2t  th3t th3 t2 
2
1





4 Ibexx  Icxx  l12 ma  l12 mc th2 t 
4
1





l1 l2 ma  l3 mc Sinth2t  th3t th3 t
2
Tau3 
Collect
DyEq

3

1, th1
t, th2
t, th3
t, th1
t, th2
t, th3
t
1
1





g l2 ma  2 l3 mc  2 l6 mdf Sinth3t 




4 l1 l2 ma  l3 mc Costh2t Costh3t 
2
8
4 Iayy  4 Iazz  4 Idfyy  4 Idfzz  l22 ma  4 l32 mc Sin2 th3t th1 t2 
1





l1 l2 ma  l3 mc Costh2t  th3t th2 t2 
2
1





l1 l2 ma  l3 mc Sinth2t  th3t th2 t 
2
1





4 Iaxx  4 Idfxx  l22 ma  4 l32 mc th3 t
4

Then these are the elements of the M, C and N matrices

phdyn.nb

M11  

 4 Iayy  4 Iazz  8 Ibaseyy  4 Ibeyy  4 Ibezz 

8
4 Icyy  4 Iczz  4 Idfyy  4 Idfzz  4 l12 ma  l22 ma  l12 mc  4 l32 mc 
1
 4 Ibeyy  4 Ibezz  4 Icyy  4 Iczz  l12 4 ma  mc Cos
2 th2
t 
8
1
 4 Iayy  4 Iazz  4 Idfyy  4 Idfzz  l22 ma  4 l32 mc Cos
2 th3
t 
8

l1 l2 ma  l3 mc Cos
th2
t Sin
th3
t
;

M12  0;
M13  0;
M21  0;
1
M22   4 Ibexx  Icxx  l12 ma  l12 mc th2
t;
4
1
M23    l1 l2 ma  l3 mc Sin
th2
t  th3
t;
2
M31  0;
1
M32    l1 l2 ma  l3 mc Sin
th2
t  th3
t th2
t;
2
1
M33   4 Iaxx  4 Idfxx  l22 ma  4 l32 mc;
4
1
C21   4 Ibeyy  4 Ibezz  4 Icyy  4 Iczz  l12 4 ma  mc Sin
2 th2
t 
8
4 l1 l2 ma  l3 mc Sin
th2
t Sin
th3
t th1
t;
C22  0;
1
C23   l1 l2 ma  l3 mc Cos
th2
t  th3
t th3
t;
2
1
C31   4 l1 l2 ma  l3 mc Cos
th2
t Cos
th3
t 
8
4 Iayy  4 Iazz  4 Idfyy  4 Idfzz  l22 ma  4 l32 mc Sin
2 th3
t th1
t;
1
C32   l1 l2 ma  l3 mc Cos
th2
t  th3
t th2
t;
2
C33  0;
N1  0;
1
N2   g 2 l1 ma  2 l5 mbe  l1 mc Cos
th2
t;
2
1
N3   g l2 ma  2 l3 mc  2 l6 mdf Sin
th3
t;
2

We will choose the first row of C so that (\dot{M}2C) is skew symmetric.

C12  C21;
C13  C31;

10

phdyn.nb

C11x  Collect
1

th1
t 
Simplify

 4 Ibeyy  4 Ibezz  4 Icyy  4 Iczz  l12 4 ma  mc Sin
2 th2
t 

4

4 l1 l2 ma  l3 mc Sin
th2
t Sin
th3
t th2
t 
1
 4 l1 l2 ma  l3 mc Cos
th2
t Cos
th3
t 
4


4 Iayy  4 Iazz  4 Idfyy  4 Idfzz  l22 ma  4 l32 mc Sin
2 th3
t th3
t


C12 th2
t  C13 th3
t,
th1
t
1





th1 t 2 Sinth2t 4 Ibeyy  4 Ibezz  4 Icyy  4 Iczz  4 l12 ma  l12 mc Costh2t 
8
2 l1 l2 ma  l3 mc Sinth3t th2 t 
2 Costh3t 2 l1 l2 ma  l3 mc Costh2t 
4 Iayy  4 Iazz  4 Idfyy  4 Idfzz  l22 ma  4 l32 mc Sinth3t
th3 t

Then
1
C11   2 Sin
th2
t 4 Ibeyy  4 Ibezz  4 Icyy  4 Iczz  4 l12 ma  l12 mc Cos
th2
t 
8
2 l1 l2 ma  l3 mc Sin
th3
t th2
t 
2 Cos
th3
t 2 l1 l2 ma  l3 mc Cos
th2
t 
4 Iayy  4 Iazz  4 Idfyy  4 Idfzz  l22 ma  4 l32 mc Sin
th3
t
th3
t;

Lets check if (\dot{M}2C) is skew symmetric :

Simplify
D
M11, t  2  C11
0

Yes, it holds!

Numerical Values
h  0;
l1  0.215; l2  0.170; l3  0.0325; l5  0.0368; l6  0.0527;
ma  0.0202;
Iaxx  0.4864  10 ^ 4; Iayy  0.0018  10 ^ 4; Iazz  0.4864  10 ^ 4;
mc  0.0249;
Icxx  0.959  10 ^ 4; Icyy  0.959  10 ^ 4; Iczz  0.0051  10 ^ 4;
mbe  0.2359;
Ibexx  11.09  10 ^ 4; Ibeyy  10.06  10 ^ 4; Ibezz  0.591  10 ^ 4;
mdf  0.1906;
Idfxx  7.11  10 ^ 4; Idfyy  0.629  10 ^ 4; Idfzz  6.246  10 ^ 4;
Ibaseyy  11.87  10 ^ 4;
g  9.81;

11

phdyn.nb

Tau1
1
th1 t 




0.00905514 Sin2 th2t  0.00364919 Sinth2t Sinth3t th2 t 
4
1





0.00364919 Costh2t Costh3t  0.00312962 Sin2 th3t th3 t 
4
0.00283279  0.00113189 Cos2 th2t 
0.000391203 Cos2 th3t  0.000912299 Costh2t Sinth3t
th1 t
Tau2
0.016298 Costh2t 
1





0.00905514 Sin2 th2t  0.00364919 Sinth2t Sinth3t th1 t2 
8
0.000456149 Costh2t  th3t th3 t2  0.0024264 th2 t 
0.000456149 Sinth2t  th3t th3 t
Tau3
0.0737552 Sinth3t 
1





0.00364919 Costh2t Costh3t  0.00312962 Sin2 th3t th1 t2 
8
0.000456149 Costh2t  th3t th2 t2  0.000456149 Sinth2t  th3t th2 t 
0.000931886 th3 t