ROBOT MODELING

Rotation matrix
𝑟𝑟11 𝑟𝑟12 𝑟𝑟13
0
1𝑅𝑅
𝑟𝑟
= � 21 𝑟𝑟22 𝑟𝑟23 �
𝑟𝑟31 𝑟𝑟32 𝑟𝑟33
Columns represent the components of the unit orthogonal vectors of reference system 1 projected
in system 0. Rows represent the components of the unit orthogonal vectors of reference system 0
projected in system 1.
Properties:

• Columns are orthogonal (scalar product is null).

• Rows are orthogonal (scalar product is null).
• Colums have modulus 1 (unit vectors).
• Rows have modulus 1 (unit vectors).

Rotations in Space
A rotation matrix can represent both the relative orientation of one reference system with respect
ot another reference system and also a rotation of one reference system from an initial position to
another orientation.
0
𝑟𝑟 = 01𝑅𝑅 1𝑟𝑟

can be tought of as “vector r1 that was described in components in the reference system {1} is
now projected in the {0} system” or as “vector r1 suffers the same rotation that has transformed
system {0} into system {1}”.

Basic Rotations
Rotation about the X axis:
1 0 0
𝑅𝑅𝑋𝑋 (𝜃𝜃) = �0 cos 𝜃𝜃 − sin 𝜃𝜃 �
0 sin 𝜃𝜃 cos 𝜃𝜃
Rotation about the Y axis:
cos 𝜃𝜃 0 sin 𝜃𝜃
𝑅𝑅𝑌𝑌 (𝜃𝜃) = � 0 1 0 �
−sin 𝜃𝜃 0 cos 𝜃𝜃
Rotation about the Z axis:
cos 𝜃𝜃 − sin 𝜃𝜃 0
𝑅𝑅𝑍𝑍 (𝜃𝜃) = � sin 𝜃𝜃 cos 𝜃𝜃 0�
0 0 1
Composition of Rotations
Composition of rotations in space is not commutative. Post-multiplication corresponds to
succesive rotations about mobile axis, while pre-multiplication corresponds to successive
rotations about fixed axis.
For example:
 1 0 0 0 0 1 0 0 1
𝑅𝑅𝑋𝑋 (90°) · 𝑅𝑅𝑌𝑌 (90°) = �0 0 −1� � 0 1 0� = �1 0 0�
0 1 0 −1 0 0 0 1 0

Rotation about mobile axis

Rotation about fixed axis

0 0 1 1 0 0 0 1 0
𝑅𝑅𝑌𝑌 (90°) · 𝑅𝑅𝑋𝑋 (90°) = � 0 1 0� �0 0 −1� = � 0 0 −1�
−1 0 0 0 1 0 −1 0 0

Rotation about mobile axis

Rotation about fixed axis

The inverse of a rotation matrix is its transpose:

𝑟𝑟11 𝑟𝑟21 𝑟𝑟31
0 −1
1𝑅𝑅 = 0 𝑇𝑇
1𝑅𝑅 = 1
0𝑅𝑅
𝑟𝑟
= � 12 𝑟𝑟22 𝑟𝑟32 �
𝑟𝑟13 𝑟𝑟23 𝑟𝑟33

General Orientation in Space

Any orientation in space can be generated by three successive rotations about different axis.
Several systems exist, some of the most common being:

Euler ZXZ system

A rotation ϕ about Z, a rotation θ about the new X and a rotation ψ about the new Z.
 cos 𝜙𝜙 cos 𝜓𝜓 − sin 𝜙𝜙 cos 𝜃𝜃 sin 𝜓𝜓 −cos 𝜙𝜙 sin 𝜓𝜓 − sin 𝜙𝜙 cos 𝜃𝜃 cos 𝜓𝜓 sin 𝜙𝜙 sin 𝜃𝜃
𝑅𝑅10 = �sin 𝜙𝜙 cos 𝜓𝜓 + cos 𝜙𝜙 cos 𝜃𝜃 sin 𝜓𝜓 −sin 𝜙𝜙 sin 𝜓𝜓 + cos 𝜙𝜙 cos 𝜃𝜃 cos 𝜓𝜓 −cos 𝜙𝜙 sin 𝜃𝜃 �
sin 𝜃𝜃 sin 𝜓𝜓 sin 𝜃𝜃 cos 𝜓𝜓 cos 𝜃𝜃
From the rotation matrix it is possible to retrieve two possible sets of angles θ, ϕ and ψ:

𝜃𝜃 = ±cos−1 𝑟𝑟33
𝑟𝑟13 ⁄sin 𝜃𝜃
𝜙𝜙 = atan2 � �
− 𝑟𝑟23⁄sin 𝜃𝜃

𝑟𝑟31⁄sin 𝜃𝜃
𝜓𝜓 = atan2 � �
𝑟𝑟32⁄sin 𝜃𝜃

Where atan2 is a function that returns the arc tangent taking into account the respective signs of
the sine and the cosine components and, as a consequence, can identify an angle in any of the four
quadrants of the complete trigonometric circle.

Euler ZYZ system

A rotation ϕ about Z, a rotation θ about the new Y and a rotation ψ about the new Z.
cos 𝜙𝜙 cos 𝜃𝜃 cos 𝜓𝜓 − sin 𝜙𝜙 sin 𝜓𝜓 − cos 𝜙𝜙 cos 𝜃𝜃 sin 𝜓𝜓 − sin 𝜙𝜙 cos 𝜓𝜓 cos 𝜙𝜙 sin 𝜃𝜃
𝑅𝑅10 = �sin 𝜙𝜙 cos 𝜃𝜃 cos 𝜓𝜓 + cos 𝜙𝜙 sin 𝜓𝜓 − sin 𝜙𝜙 cos 𝜃𝜃 sin 𝜓𝜓 + cos 𝜙𝜙 cos 𝜓𝜓 sin 𝜙𝜙 sin 𝜃𝜃 �
− sin 𝜃𝜃 cos 𝜓𝜓 sin 𝜃𝜃 sin 𝜓𝜓 cos 𝜃𝜃
From the rotation matrix it is possible to retrieve two possible sets of angles θ, ϕ and ψ:

𝜃𝜃 = ±cos−1 𝑟𝑟33
𝑟𝑟23 ⁄sin 𝜃𝜃
𝜙𝜙 = atan2 � �
𝑟𝑟13 ⁄sin 𝜃𝜃

𝑟𝑟32⁄sin 𝜃𝜃
𝜓𝜓 = atan2 � �
−𝑟𝑟31 ⁄sin 𝜃𝜃

Roll-Pitch-Yaw system
A rotation ψ (yaw) about the X, a rotation θ (pitch) about the original Y and a rotation ϕ (roll)
about the original Z.
cos 𝜙𝜙 cos 𝜃𝜃 − sin 𝜙𝜙 cos 𝜓𝜓 + cos 𝜙𝜙 sin 𝜃𝜃 sin 𝜓𝜓 sin 𝜙𝜙 sin 𝜓𝜓 + cos 𝜙𝜙 sin 𝜃𝜃 cos 𝜓𝜓
𝑅𝑅10 = � sin 𝜙𝜙 cos 𝜃𝜃 cos 𝜙𝜙 cos 𝜓𝜓 + sin 𝜙𝜙 sin 𝜃𝜃 sin 𝜓𝜓 − cos 𝜙𝜙 sin 𝜓𝜓 + sin 𝜙𝜙 sin 𝜃𝜃 cos 𝜓𝜓�
− sin 𝜃𝜃 cos 𝜃𝜃 sin 𝜓𝜓 cos 𝜃𝜃 cos 𝜓𝜓
From the rotation matrix it is possible to retrieve two possible sets of angles θ, ϕ and ψ:
𝜃𝜃 = sin−1 (−𝑟𝑟31 ) or 𝜃𝜃 = 𝜋𝜋 − sin−1 (−𝑟𝑟31 )
𝑟𝑟21⁄cos 𝜃𝜃
𝜙𝜙 = atan2 � �
𝑟𝑟11⁄cos 𝜃𝜃

𝑟𝑟32⁄cos 𝜃𝜃
𝜓𝜓 = atan2 � �
𝑟𝑟33⁄cos 𝜃𝜃

Equivalent Angle-Axis
A unique rotation of angle θ about a unit vector kx, ky, kz that represents the axis of rotation.
 𝑘𝑘𝑥𝑥 𝑘𝑘𝑥𝑥 𝑣𝑣 𝜃𝜃 + 𝑐𝑐𝑐𝑐 𝑘𝑘𝑥𝑥 𝑘𝑘𝑦𝑦 𝑣𝑣 𝜃𝜃 − 𝑘𝑘𝑧𝑧 𝑠𝑠𝑠𝑠 𝑘𝑘𝑥𝑥 𝑘𝑘𝑧𝑧 𝑣𝑣 𝜃𝜃 + 𝑘𝑘𝑦𝑦 𝑠𝑠𝑠𝑠
𝑅𝑅10 = �𝑘𝑘𝑥𝑥 𝑘𝑘𝑦𝑦 𝑣𝑣 𝜃𝜃 + 𝑘𝑘𝑧𝑧 𝑠𝑠𝑠𝑠 𝑘𝑘𝑦𝑦 𝑘𝑘𝑦𝑦 𝑣𝑣 𝜃𝜃 + 𝑐𝑐𝑐𝑐 𝑘𝑘𝑦𝑦 𝑘𝑘𝑧𝑧 𝑣𝑣 𝜃𝜃 − 𝑘𝑘𝑥𝑥 𝑠𝑠𝑠𝑠�
𝑘𝑘𝑥𝑥 𝑘𝑘𝑧𝑧 𝑣𝑣 𝜃𝜃 − 𝑘𝑘𝑦𝑦 𝑠𝑠𝑠𝑠 𝑘𝑘𝑦𝑦 𝑘𝑘𝑧𝑧 𝑣𝑣 𝜃𝜃 + 𝑘𝑘𝑥𝑥 𝑠𝑠𝑠𝑠 𝑘𝑘𝑧𝑧 𝑘𝑘𝑧𝑧 𝑣𝑣 𝜃𝜃 + 𝑐𝑐𝑐𝑐

In this equation vθ stands for (1-cosθ). Two possible sets of angle plus vector (both the vector and
the angle are opposed in the two solutions) can generate the rotation matrix above (except when
θ = 0° or θ = 180°):
𝑟𝑟11 + 𝑟𝑟22 + 𝑟𝑟33 − 1
𝜃𝜃 = cos−1 � �
2
𝑘𝑘𝑥𝑥 𝑟𝑟32 − 𝑟𝑟23
1
�𝑘𝑘𝑦𝑦 � = �𝑟𝑟13 − 𝑟𝑟31 �
2 sin 𝜃𝜃 𝑟𝑟 − 𝑟𝑟
𝑘𝑘𝑧𝑧 21 12

When θ = 0° there is no rotation and no rotation axis exists. When θ = 180° the values of kx, ky,
kz can be calculated from the elements in the matrix diagonal.

Homogeneous coordinates
In order to get a compact description of the position of a reference system in the tridimensional
space, we can use homeogenous coordinates. They consist of adding a fourth component to the
components of a position vector. In robotics, the fourth element is a one.
𝑥𝑥
𝑦𝑦
𝑟𝑟 = � �
𝑧𝑧
1
Transformations
Now it is possible to combine the position of the origin with the orientation of the unit vectors of
a reference system to describe its position with respect to another reference system.
𝑟𝑟11 𝑟𝑟12 𝑟𝑟13 𝑝𝑝𝑥𝑥
0 0 𝑟𝑟21 𝑟𝑟22 𝑟𝑟23 𝑝𝑝𝑦𝑦
0 1𝑅𝑅 1𝑝𝑝�
1𝑇𝑇 =� =�
𝑟𝑟31 𝑟𝑟32 𝑟𝑟33 𝑝𝑝𝑧𝑧
�
0,0,0 1
0 0 0 1
The transform is also an operator. Any vector premultiplied by it will be displaced by the position
vector 01𝑝𝑝 and rotated by the rotation matrix 01𝑅𝑅.

Denavit-Hartemberg Convention to Assign Reference Systems to Links

The transformations are useful tools to represent the relative positions of differents links of the
kinematic chain (which constitute the structure of the robot). Specially, two consecutive links.
Assuming that the vast majority of connexions between links are rotation joints, for sure we can
attach a rotation axis to each link. The convention that we apply is that each link has a rotation
axis about which it rotates. With only four parameters it is possible to represent the relative
positionbetween two consecutive links, namely:

• Link length (ai-1): distance between the consecutive axis i-1 and i along the common
normal.
• Link twist (αi-1): angle of twist between the two consecutive axis i-1 and i, measured along
the common normal and positive when right handed from i-1 to i.
• Link offset (di): distance along the i axis between the point of intersection of the common
normal i-1 to i and the common normal i to i+1.
 • Joint angle (θi): angle between the common normals i-1 to i and i to i+1 measured about
axis i and positive when right-handed.
It is worth mentioning that ai-1 and αi-1 are constants that never change during motion, while di or
θi will be a variable. In a rotational joint R θi is a variable and di a constant. Conversely, in a
prismatic joint P, di is a variable and θi a constant.

θi

Link i-1

αi-1
Respective reference systems can be attached to the links as represented in the figure. Then, the
link trasnformation can be written using only the four D-H parameters:
cos 𝜃𝜃𝑖𝑖 − sin 𝜃𝜃𝑖𝑖 0 𝑎𝑎𝑖𝑖−1
𝑖𝑖−1 sin 𝜃𝜃𝑖𝑖 cos 𝛼𝛼𝑖𝑖−1 cos 𝜃𝜃𝑖𝑖 cos 𝛼𝛼𝑖𝑖−1 − sin 𝛼𝛼𝑖𝑖−1 − sin 𝛼𝛼𝑖𝑖−1 𝑑𝑑𝑖𝑖
𝑖𝑖 𝑇𝑇 =� �
sin 𝜃𝜃𝑖𝑖 sin 𝛼𝛼𝑖𝑖−1 cos 𝜃𝜃𝑖𝑖 sin 𝛼𝛼𝑖𝑖−1 cos 𝛼𝛼𝑖𝑖−1 cos 𝛼𝛼𝑖𝑖−1 𝑑𝑑𝑖𝑖
0 0 0 1
Special cases appear for the first and the last links of the kinematic chain. To write 01T there is no
0 axis. Then, it is recommended that Z0 coincides with Z1 and that X0 and Y0 coincide with X1
and Y1 when the joint variable di or θi is zero. Then
cos 𝜃𝜃𝑖𝑖 − sin 𝜃𝜃𝑖𝑖 0 0
0 sin 𝜃𝜃𝑖𝑖 cos 𝜃𝜃𝑖𝑖 0 0
1𝑇𝑇 =� �
0 0 1 𝑑𝑑𝑖𝑖
0 0 0 1
and also either di or θi is also zero.
For the last link, no axis N+1 exists. Then, reference system N is chosen so that XN coincides with
XN-1 when θn = 0º and also with dn = 0 if the last joint is R. If the last joint is P, then XN is parallel
to XN-1 and dn = 0 when the joint has no displacement.
 System
{X,Y,Z}i

Link i-1

System
{X,Y,Z}i-1

Once the successive transformations have been written, they can be composed by matrix product
so that the result is the transformation matrix that relates the position of the tool frame with respect
to the base frame
𝐵𝐵
𝑇𝑇𝑇𝑇 = 𝐵𝐵0𝑇𝑇 01𝑇𝑇 ··· 𝑁𝑁−1 𝑁𝑁
𝑁𝑁𝑇𝑇 𝑇𝑇𝑇𝑇

{W} {N} Wrist

{T} Tool

{G} Goal {B} Base

{S} Station

Filling the joint variable values in the transformation that descrives the position of the kinematic
chain is called solving the direct kinematics. For serial chains it is a straightforward task. Although
every joint variable can be substituted into its respective transformation and, afterwards, the
transformations can be multiplied to find the position of the end link with respect to the base, it is
usually possible to get this transformation as a function of all the joint variables. We are going to
discuss some of them.
Conversely, the inverse kinematics problem consists of solving for all the joint variables if the
position of the end link is given in a numerical trasnformation. We are going to see some examples
of robot models and their direct and inverse kinematics.

Puma 762

𝑐𝑐1 {𝑐𝑐23 [𝑐𝑐4 𝑐𝑐5 𝑐𝑐6 − 𝑠𝑠4 𝑠𝑠6 ] − 𝑠𝑠23 𝑠𝑠5 𝑠𝑠6 } − 𝑠𝑠1 [𝑠𝑠4 𝑐𝑐5 𝑐𝑐6 + 𝑐𝑐4 𝑠𝑠6 ] 𝑐𝑐1 {−𝑐𝑐23 [𝑐𝑐4 𝑐𝑐5 𝑠𝑠6 + 𝑠𝑠4 𝑐𝑐6 ] + 𝑠𝑠23 𝑠𝑠5 𝑐𝑐6 } + 𝑠𝑠1 [𝑠𝑠4 𝑐𝑐5 𝑠𝑠6 − 𝑐𝑐4 𝑐𝑐6 ]
𝑠𝑠 {𝑐𝑐 [𝑐𝑐 𝑐𝑐 𝑐𝑐 − 𝑠𝑠4 𝑠𝑠6 ] − 𝑠𝑠23 𝑠𝑠5 𝑐𝑐6 } + 𝑐𝑐1 [𝑠𝑠4 𝑐𝑐5 𝑐𝑐6 + 𝑐𝑐4 𝑠𝑠6 ] 𝑠𝑠1 {−𝑐𝑐23 [𝑐𝑐4 𝑐𝑐5 𝑠𝑠6 + 𝑠𝑠4 𝑐𝑐6 ] + 𝑠𝑠23 𝑠𝑠5 𝑠𝑠6 } − 𝑐𝑐1 [𝑠𝑠4 𝑐𝑐5 𝑠𝑠6 + 𝑐𝑐4 𝑐𝑐6 ]
� 1 23 4 5 6
−𝑠𝑠23 [𝑐𝑐4 𝑐𝑐5 𝑐𝑐6 − 𝑠𝑠4 𝑠𝑠6 ] − 𝑐𝑐23 𝑠𝑠5 𝑐𝑐6 𝑠𝑠23 [𝑐𝑐4 𝑐𝑐5 𝑠𝑠6 + 𝑠𝑠4 𝑐𝑐6 ] + 𝑐𝑐23 𝑠𝑠5 𝑠𝑠6
0 0
𝑐𝑐1 [𝑐𝑐23 𝑐𝑐4 𝑠𝑠5 + 𝑠𝑠23 𝑐𝑐5 ] − 𝑠𝑠1 𝑠𝑠4 𝑠𝑠5 𝑐𝑐1 {𝑑𝑑6 [𝑐𝑐23 𝑐𝑐4 𝑠𝑠5 + 𝑠𝑠23 𝑐𝑐5 ] + 𝑠𝑠23 𝑑𝑑4 + 𝑎𝑎2 𝑐𝑐2 } − 𝑠𝑠1 [𝑑𝑑6 𝑠𝑠4 𝑠𝑠5 + 𝑑𝑑2 ]
𝑠𝑠1 [𝑐𝑐23 𝑐𝑐4 𝑠𝑠5 + 𝑠𝑠23 𝑐𝑐5 ] − 𝑐𝑐1 𝑠𝑠4 𝑠𝑠5 𝑠𝑠1 {𝑑𝑑6 [𝑐𝑐23 𝑐𝑐4 𝑠𝑠5 + 𝑠𝑠23 𝑐𝑐5 ] + 𝑠𝑠23 𝑑𝑑4 + 𝑎𝑎2 𝑐𝑐2 } + 𝑐𝑐1 [𝑑𝑑6 𝑠𝑠4 𝑠𝑠5 + 𝑑𝑑2 ]
�
−𝑠𝑠23 𝑐𝑐4 𝑠𝑠5 + 𝑐𝑐23 𝑐𝑐5 𝑑𝑑6 [−𝑠𝑠23 𝑐𝑐4 𝑠𝑠5 + 𝑐𝑐23 𝑐𝑐5 ] + 𝑐𝑐23 𝑑𝑑4 − 𝑎𝑎2 𝑠𝑠2
0 1
Solving the inverse kinematics is quite difficult.

2D 3R serial manipulator
 𝑐𝑐123 −𝑠𝑠123 0 𝐿𝐿1 𝑐𝑐1 + 𝐿𝐿2 𝑐𝑐12 cos 𝜙𝜙 − sin 𝜙𝜙 0 𝑝𝑝𝑥𝑥
0 𝑠𝑠123 𝑐𝑐123 0 𝐿𝐿1 𝑠𝑠1 + 𝐿𝐿1 𝑠𝑠12 sin 𝜙𝜙 cos 𝜙𝜙 0 𝑝𝑝𝑦𝑦
3𝑇𝑇 = � �=� �
0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1
Inverse kinematics:

𝑝𝑝𝑥𝑥2 + 𝑝𝑝𝑦𝑦2 − 𝐿𝐿21 − 𝐿𝐿22

𝜃𝜃2 = ±cos−1
2𝐿𝐿1 𝐿𝐿2
For each θ2, then calculate:
𝑘𝑘1 = 𝐿𝐿1 + 𝐿𝐿2 𝑐𝑐2
𝑘𝑘2 = 𝐿𝐿2 𝑠𝑠2
𝑝𝑝𝑦𝑦 𝑘𝑘2
𝜃𝜃1 = atan2 − atan2
𝑝𝑝𝑥𝑥 𝑘𝑘1
𝑟𝑟21
𝜃𝜃3 = atan2 − 𝜃𝜃1 − 𝜃𝜃2
𝑟𝑟11

2D RRP serial manipulator

sin(𝜃𝜃2 − 𝜃𝜃1 ) 0 cos(𝜃𝜃2 − 𝜃𝜃1 ) 𝑑𝑑3 cos(𝜃𝜃2 − 𝜃𝜃1 ) + 𝐿𝐿1 cos 𝜃𝜃1
0 cos(𝜃𝜃2 − 𝜃𝜃1 ) 0 − sin(𝜃𝜃2 − 𝜃𝜃1 ) −𝑑𝑑3 sin(𝜃𝜃2 − 𝜃𝜃1 ) + 𝐿𝐿1 sin 𝜃𝜃1 �
3𝑇𝑇 = �
0 1 0 0
0 0 0 1
cos 𝜙𝜙 0 sin 𝜙𝜙 𝑝𝑝𝑥𝑥
sin 𝜙𝜙 0 − cos 𝜙𝜙 𝑝𝑝𝑦𝑦
=� �
0 1 0 0
0 0 0 1
Inverse kinematics (two solutions):

2
𝑑𝑑3 = −(𝑝𝑝𝑦𝑦 cos 𝜙𝜙 − 𝑝𝑝𝑥𝑥 sin 𝜙𝜙) ± ��𝑝𝑝𝑦𝑦 cos 𝜙𝜙 − 𝑝𝑝𝑥𝑥 sin 𝜙𝜙� − �𝑝𝑝𝑥𝑥2 + 𝑝𝑝𝑦𝑦2 − 𝐿𝐿21 �

𝑑𝑑3 𝑟𝑟11 + 𝑝𝑝𝑦𝑦

𝜃𝜃1 = atan2
−𝑑𝑑3 𝑟𝑟21 + 𝑝𝑝𝑥𝑥
𝑟𝑟11
𝜃𝜃2 = 𝜃𝜃1 + atan2
𝑟𝑟21
Jacobian
The jacobian is a convenient way to represent the relationship between the velocities of the joints
and the velocities of the end-effector. As an example, suppose a 2R planar robot and the
coordinates x and y of the tip of its end-effector.

x 𝑥𝑥 = 𝐿𝐿1 𝑐𝑐1 + 𝐿𝐿2 𝑐𝑐12

�
𝑦𝑦 = 𝐿𝐿1 𝑠𝑠1 + 𝐿𝐿2 𝑠𝑠12
𝑥𝑥̇ = −(𝐿𝐿1 𝑠𝑠1 + 𝐿𝐿2 𝑠𝑠12 )𝜃𝜃̇1 − 𝐿𝐿2 𝑠𝑠12 𝜃𝜃̇2
→ �
𝑦𝑦̇ = (𝐿𝐿1 𝑐𝑐1 + 𝐿𝐿2 𝑐𝑐12 )𝜃𝜃̇1 + 𝐿𝐿2 𝑐𝑐12 𝜃𝜃̇2
θ2 𝑥𝑥̇ 𝜃𝜃̇
𝑣𝑣 = 𝐽𝐽 · Ω with 𝑣𝑣 = � � and Ω = � 1�
y 𝑦𝑦̇ 𝜃𝜃̇2
−(𝐿𝐿1 𝑠𝑠1 + 𝐿𝐿2 𝑠𝑠12 ) −𝐿𝐿2 𝑠𝑠12
𝐽𝐽 = � �
θ1 (𝐿𝐿1 𝑐𝑐1 + 𝐿𝐿2 𝑐𝑐12 ) 𝐿𝐿2 𝑐𝑐12
So, if we know the dimensions and the position of the
manipulator and also the velocities of the joints, we can
calculate the velocity of the end-effector.
If the jacobian can be inverted, it can be used, given a desired velocity of the end-effector, to
calculate the necessary joint velocities that can create it.

Ω = 𝐽𝐽−1 𝑣𝑣
For an inverse to exist, the jacobian must be a square matrix and have non-null determinant. In
our example
1 𝐿𝐿2 𝑐𝑐12 𝐿𝐿2 𝑠𝑠12
𝐽𝐽−1 = � �
𝐿𝐿1 𝐿𝐿2 𝑠𝑠2 −(𝐿𝐿 𝑐𝑐
1 1 + 𝐿𝐿 𝑐𝑐
2 12 ) −(𝐿𝐿1 1 + 𝐿𝐿2 𝑠𝑠12 )
𝑠𝑠

−(𝐿𝐿1 𝑠𝑠1 + 𝐿𝐿2 𝑠𝑠12 ) −𝐿𝐿2 𝑠𝑠12

|𝐽𝐽| = � � = 𝐿𝐿1 𝐿𝐿2 𝑠𝑠2
(𝐿𝐿1 𝑐𝑐1 + 𝐿𝐿2 𝑐𝑐12 ) 𝐿𝐿2 𝑐𝑐12
This means that there is no inverse when sinθ2 is zero. This condition is named a singularity and
happens when the arm is fully stretched (θ2 = 0°) or folded (θ2 = 180°). This kind of singularity
is called a boundary singularity and it happens at the limits of the workspace, the area within reach
of the manipulator.
Applying the virtual power principle, if some external forces are applied at the tip of the
manipulator, for the system to be in equilibrium it is necessary that suitable torques are applied
in the joints

𝐹𝐹 𝑇𝑇 · 𝑣𝑣 + Τ 𝑇𝑇 · Ω = 0.
It comes out that the jacobian is also the relationship between these forces and those torques

𝐹𝐹 𝑇𝑇 · 𝐽𝐽Ω = −Τ 𝑇𝑇 Ω → Τ = −𝐽𝐽𝑇𝑇 𝐹𝐹
where the superindex T indicates the matrix transpose. As a consequence, the inverse realtionship
is also valid

𝐹𝐹 = −𝐽𝐽−𝑇𝑇 Τ.
Building the jacobian is rather easy by differentiation if the manipulator is 2D, but when it is 3D,
then it is much better to proceed by vector composition. Two equations are needed when the joints
are rotation joints. The angular velocity of a link with respect to the reference system attached to
it is
𝑖𝑖+1
𝑖𝑖+1𝜔𝜔 = 𝑖𝑖+1
𝑖𝑖 𝑅𝑅 · 𝑖𝑖𝑖𝑖𝜔𝜔 + 𝜃𝜃̇𝑖𝑖+1 𝑖𝑖+1
𝑖𝑖+1𝑍𝑍.

And the velocity of the origin of that reference system is

𝑖𝑖+1 𝑖𝑖+1 𝑖𝑖
𝑖𝑖+1𝑣𝑣 = 𝑖𝑖 𝑅𝑅 � 𝑖𝑖𝑣𝑣 + 𝑖𝑖𝑖𝑖𝜔𝜔 × 𝑖𝑖+1𝑖𝑖𝑃𝑃�.

If the joint are prismatic joints, then

𝑖𝑖+1 𝑖𝑖+1
𝑖𝑖+1𝜔𝜔 = 𝑖𝑖 𝑅𝑅 · 𝑖𝑖𝑖𝑖𝜔𝜔 and
𝑖𝑖+1
𝑖𝑖+1𝑣𝑣 = 𝑖𝑖+1 𝑖𝑖
𝑖𝑖 𝑅𝑅 � 𝑖𝑖𝑣𝑣 + 𝑖𝑖𝑖𝑖𝜔𝜔 × 𝑖𝑖+1𝑖𝑖𝑃𝑃� + 𝑑𝑑̇𝑖𝑖+1 𝑖𝑖+1
𝑖𝑖+1𝑍𝑍.

We can apply this procedure to the 2R planar robot:

0 0
1 1
1𝜔𝜔 = �0� 1𝑣𝑣 = �0�
𝜃𝜃̇1 0
0 𝐿𝐿1 𝑠𝑠2 𝜃𝜃̇1
2 2
2𝜔𝜔 =� 0 � 2𝑣𝑣 = �𝐿𝐿1 𝑐𝑐2 𝜃𝜃̇1 �
𝜃𝜃̇1 + 𝜃𝜃̇2 0
0 𝐿𝐿1 𝑠𝑠2 𝜃𝜃̇1
3 3
3𝜔𝜔 =� 0 � 3𝑣𝑣 = �𝐿𝐿1 𝑐𝑐2 𝜃𝜃̇1 + 𝐿𝐿2 (𝜃𝜃̇1 + 𝜃𝜃̇2 )�
𝜃𝜃̇1 + 𝜃𝜃̇2 0

−𝐿𝐿1 𝑠𝑠1 𝜃𝜃̇1 − 𝐿𝐿2 𝑠𝑠12 (𝜃𝜃̇1 + 𝜃𝜃̇2 )

0 0 3
3𝑣𝑣 = 3𝑅𝑅 3𝑣𝑣 = � 𝐿𝐿1 𝑐𝑐1 𝜃𝜃̇1 + 𝐿𝐿2 𝑐𝑐12 (𝜃𝜃̇1 + 𝜃𝜃̇2 ) �
0

Singularities
If we consider in the previous example a configuration where sinθ2 is zero, we can see that it will
be impossible to solve Ω = 𝐽𝐽−1 𝑣𝑣. Stated in another way, in such a configuration it is impossible
to find a set of values for Ω that enables the tip of the manipulator to move with velocity v. In the
figure below, the forbidden direction of the velocity is indicated with blue arrows. No matter the
values of Ω, v can not move in that direction.
Because the jacobian can be used to solve for Ω along a trajectory, if the determinant is not zero
although very close to that value, the result will be extremely high joint velocities, something that
must be avoided. So, moving with high velocities near a singularity can be dangerous.
On the other hand, from the figure it is also apparent that if a force is applied pushing or pulling
in the direction of the blue arrows, no joint torque will be necessary to balance it. This type of
singularities can be of advantage when supporting static loads.

Statics
If we are interested in the reactions at the connections between the links of the kinematic chain,
then we can use a FBD approach to calculate them. Basically, we can calculate the reaction force
or the reaction moment in one link with respect to the force and moment in the following link
𝑖𝑖 𝑖𝑖 𝑖𝑖+1
𝑖𝑖 𝑓𝑓 = 𝑖𝑖+1𝑅𝑅 𝑖𝑖+1𝑓𝑓

𝑖𝑖 𝑖𝑖 𝑖𝑖+1
𝑖𝑖 𝑛𝑛 = 𝑖𝑖+1𝑅𝑅 𝑖𝑖+1𝑛𝑛 + 𝑖𝑖+1𝑖𝑖𝑃𝑃 × 𝑖𝑖𝑖𝑖𝑓𝑓
If we are only interested in the force or moment applied by the actuator at the joint, then we simply
project the calculated forces or moments onto the Z axis of the reference system

𝑓𝑓𝑖𝑖 = 𝑖𝑖𝑖𝑖𝑓𝑓 𝑇𝑇 𝑍𝑍⃗𝑖𝑖 or 𝑇𝑇𝑖𝑖 = 𝑖𝑖𝑖𝑖𝑛𝑛𝑇𝑇 𝑍𝑍⃗𝑖𝑖

In the 2R planar manipulator of the example
𝐹𝐹𝑥𝑥 𝐹𝐹𝑥𝑥 0
2
2𝑓𝑓 = �𝐹𝐹𝑦𝑦 � 2
2𝑛𝑛 = 𝐿𝐿2 𝑋𝑋⃗2 × �𝐹𝐹𝑦𝑦 � = � 0 �
0 0 𝐿𝐿2 𝐹𝐹𝑦𝑦

𝑐𝑐2 −𝑠𝑠2 0 𝐹𝐹𝑥𝑥 𝑐𝑐2 𝐹𝐹𝑥𝑥 − 𝑠𝑠2 𝐹𝐹𝑦𝑦 0

1
1𝑓𝑓 = �𝑠𝑠2 𝑐𝑐2 0� �𝐹𝐹𝑦𝑦 � = �𝑠𝑠2 𝐹𝐹𝑥𝑥 + 𝑐𝑐2 𝐹𝐹𝑦𝑦 � 1
1𝑛𝑛 = � 0 � + 𝐿𝐿1 𝑋𝑋⃗1 × 11𝑓𝑓
0 0 1 0 0 𝐿𝐿2 𝐹𝐹𝑦𝑦
0
=� 0 �
𝐿𝐿1 𝑠𝑠2 𝐹𝐹𝑥𝑥 + 𝐿𝐿1 𝑐𝑐2 𝐹𝐹𝑦𝑦 + 𝐿𝐿2 𝐹𝐹𝑦𝑦

The blue elements of the vectors are the joint torques. Using the jacobian, we get the same result
 −(𝐿𝐿1 𝑠𝑠1 + 𝐿𝐿2 𝑠𝑠12 ) (𝐿𝐿1 𝑐𝑐1 + 𝐿𝐿2 𝑐𝑐12 ) 𝑐𝑐12 −𝑠𝑠12 𝐹𝐹𝑥𝑥
𝑇𝑇 = −𝐽𝐽𝑇𝑇 𝐹𝐹 = − � � �𝑠𝑠 𝑐𝑐12 � �𝐹𝐹𝑦𝑦 �
−𝐿𝐿2 𝑠𝑠12 𝐿𝐿2 𝑐𝑐12 12
𝐿𝐿2 𝐹𝐹𝑦𝑦
=� �
𝐿𝐿1 𝑠𝑠2 𝐹𝐹𝑥𝑥 + 𝐿𝐿1 𝑐𝑐2 𝐹𝐹𝑦𝑦 + 𝐿𝐿2 𝐹𝐹𝑦𝑦

Dynamics
Following the same 2D example, the dynamics simulation of the robot can be done in a recursive
manner first calculating the velocities
𝑖𝑖+1
𝑖𝑖+1𝜔𝜔 = 𝑖𝑖+1
𝑖𝑖 𝑅𝑅 · 𝑖𝑖𝑖𝑖𝜔𝜔 + 𝜃𝜃̇𝑖𝑖+1 𝑖𝑖+1
𝑖𝑖+1𝑍𝑍.

then the accelerations of the frames and of the centers of mass

𝑖𝑖+1
𝑖𝑖+1𝜔𝜔̇ = 𝑖𝑖+1
𝑖𝑖 𝑅𝑅 · 𝑖𝑖𝑖𝑖𝜔𝜔̇ + 𝑖𝑖+1𝑖𝑖𝑅𝑅 𝑖𝑖𝑖𝑖𝜔𝜔 × 𝜃𝜃̇𝑖𝑖+1 𝑖𝑖+1 ̈ 𝑖𝑖+1
𝑖𝑖+1𝑍𝑍 + 𝜃𝜃𝑖𝑖+1 𝑖𝑖+1𝑍𝑍.

𝑖𝑖+1 𝑖𝑖+1 𝑖𝑖
𝑖𝑖+1𝑣𝑣̇ = 𝑖𝑖 𝑅𝑅 ( 𝑖𝑖 𝜔𝜔̇ × 𝑖𝑖+1𝑖𝑖𝑃𝑃 + 𝑖𝑖𝑖𝑖𝜔𝜔 × ( 𝑖𝑖𝑖𝑖𝜔𝜔 × 𝑖𝑖+1𝑖𝑖𝑃𝑃) + 𝑖𝑖𝑖𝑖𝑣𝑣̇ )
𝑖𝑖+1 𝑖𝑖+1
𝑖𝑖+1𝑣𝑣̇ 𝐶𝐶 = 𝑖𝑖+1𝜔𝜔̇ × 𝑖𝑖+1 𝑖𝑖+1 𝑖𝑖+1 𝑖𝑖+1 𝑖𝑖+1
𝑖𝑖+1𝑃𝑃𝑃𝑃 + 𝑖𝑖+1𝜔𝜔 × � 𝑖𝑖+1𝜔𝜔 × 𝑖𝑖+1𝑃𝑃𝑃𝑃 � + 𝑖𝑖+1𝑣𝑣̇

The inertial forces and moments

𝑖𝑖+1
𝑖𝑖+1𝐹𝐹 = 𝑚𝑚𝑖𝑖+1 𝑖𝑖+1
𝑖𝑖+1𝑣𝑣̇ 𝐶𝐶

𝑖𝑖+1 𝑖𝑖+1 𝑖𝑖+1

𝑖𝑖+1𝑁𝑁 = 𝑖𝑖+1𝐼𝐼𝐼𝐼 𝑖𝑖+1𝜔𝜔̇ + 𝑖𝑖+1 𝑖𝑖+1 𝑖𝑖+1
𝑖𝑖+1𝜔𝜔 × 𝑖𝑖+1𝐼𝐼𝐼𝐼 𝑖𝑖+1𝜔𝜔 .

Then, the reaction forces are obtained by

𝑖𝑖 𝑖𝑖 𝑖𝑖+1
𝑖𝑖 𝑓𝑓 = 𝑖𝑖+1𝑅𝑅 𝑖𝑖+1𝑓𝑓 + 𝑖𝑖𝑖𝑖𝐹𝐹
𝑖𝑖
𝑖𝑖 𝑛𝑛 = 𝑖𝑖𝑖𝑖𝑁𝑁 + 𝑖𝑖+1𝑖𝑖𝑅𝑅 𝑖𝑖+1 𝑖𝑖 𝑖𝑖 𝑖𝑖 𝑖𝑖 𝑖𝑖+1
𝑖𝑖+1𝑛𝑛 + 𝑖𝑖 𝑃𝑃𝑃𝑃 × 𝑖𝑖 𝐹𝐹 + 𝑖𝑖+1𝑃𝑃 × 𝑖𝑖+1𝑅𝑅 𝑖𝑖+1𝑓𝑓

If gravity has to be included in the calculations, it suffices with using 00𝑣𝑣̇ = −𝑔𝑔⃗ i.e. making the
base accelerating upwards.
Back to the example, suppose that we do not have any forces at the end-effector (they have already
been studied in statics). We include gravity with
0
0𝑣𝑣̇
�⃗0
= 𝑔𝑔𝑌𝑌
and we suppose that the inertias are just punctual masses at the ends of each link.
The steps described above are now
0 0
1 1
1𝜔𝜔 = �0� 1𝜔𝜔̇ = �0�
𝜃𝜃̇1 𝜃𝜃̈1

𝑔𝑔𝑠𝑠1 −𝐿𝐿1 𝜃𝜃̇12 + 𝑔𝑔𝑠𝑠1

1 1
1𝑣𝑣̇ = �𝑔𝑔𝑐𝑐1 � 1𝑣𝑣̇ 𝑐𝑐 = � 𝐿𝐿1 𝜃𝜃̈1 + 𝑔𝑔𝑐𝑐1 �
0 0

−𝑚𝑚1 𝐿𝐿1 𝜃𝜃̇12 + 𝑚𝑚1 𝑔𝑔𝑠𝑠1 0

1 1
1𝐹𝐹 = � 𝑚𝑚1 𝐿𝐿1 𝜃𝜃̈1 + 𝑚𝑚1 𝑔𝑔𝑐𝑐1 � 1𝑁𝑁 = �0�
0 0

and
 0 0
2 2
2𝜔𝜔 =� 0 � 2𝜔𝜔̇ =� 0 �
𝜃𝜃̇1 + 𝜃𝜃̇2 𝜃𝜃̈1 + 𝜃𝜃̈2

−𝐿𝐿1 𝜃𝜃̇12 + 𝑔𝑔𝑠𝑠1 𝐿𝐿1 𝜃𝜃̈1 𝑠𝑠2 − 𝐿𝐿1 𝜃𝜃̇12 𝑐𝑐2 + 𝑔𝑔𝑠𝑠12
2 2
2𝑣𝑣̇ = 1𝑅𝑅 · � 𝐿𝐿1 𝜃𝜃1̈ + 𝑔𝑔𝑐𝑐1 � = � 𝐿𝐿1 𝜃𝜃̈1 𝑐𝑐 + 𝐿𝐿1 𝜃𝜃̇12 𝑠𝑠2 + 𝑔𝑔𝑐𝑐12 �
0 0

−𝐿𝐿1 𝜃𝜃̇12 + 𝑔𝑔𝑠𝑠1 𝐿𝐿1 𝜃𝜃̈1 𝑠𝑠2 − 𝐿𝐿1 𝜃𝜃̇12 𝑐𝑐2 + 𝑔𝑔𝑠𝑠12
2 2
2𝑣𝑣𝑣𝑣̇ = 1𝑅𝑅 · � 𝐿𝐿1 𝜃𝜃̈1 + 𝑔𝑔𝑐𝑐1 � = �𝐿𝐿1 𝜃𝜃̈1 𝑐𝑐2 + 𝐿𝐿1 𝜃𝜃̇12 𝑠𝑠2 + 𝑔𝑔𝑐𝑐12 �
0 0

𝑚𝑚2 𝐿𝐿1 𝜃𝜃̈1 𝑠𝑠2 − 𝑚𝑚2 𝐿𝐿1 𝜃𝜃̇12 𝑐𝑐2 + 𝑚𝑚2 𝑔𝑔𝑠𝑠12 − 𝑚𝑚2 𝐿𝐿2 (𝜃𝜃̇1 + 𝜃𝜃̇2 )2 0
2 2
2𝐹𝐹 = � 𝑚𝑚2 𝐿𝐿1 𝜃𝜃1̈ 𝑐𝑐2 + 𝑚𝑚2 𝐿𝐿1 𝜃𝜃̇12 𝑠𝑠2 + 𝑚𝑚2 𝑔𝑔𝑐𝑐12 + 𝑚𝑚2 𝐿𝐿2 (𝜃𝜃̈1 + 𝜃𝜃̈2 ) � 2𝑁𝑁 = �0�
0 0

Now, the reactions can be calculated

2
2𝑓𝑓 = 22𝐹𝐹
0
2 0
2𝑛𝑛 =� �
𝑚𝑚2 𝐿𝐿1 𝐿𝐿2 𝜃𝜃̈1 𝑐𝑐2 + 𝑚𝑚2 𝐿𝐿1 𝐿𝐿2 𝜃𝜃̇12 𝑠𝑠2 + 𝑚𝑚2 𝐿𝐿2 𝑔𝑔𝑐𝑐12 + 𝑚𝑚2 𝐿𝐿22 (𝜃𝜃̈1 + 𝜃𝜃̈2 )

𝑚𝑚2 𝐿𝐿1 𝜃𝜃̈1 𝑠𝑠2 − 𝑚𝑚2 𝐿𝐿1 𝜃𝜃1̇ 2 𝑐𝑐2 + 𝑚𝑚2 𝑔𝑔𝑠𝑠12 − 𝑚𝑚2 𝐿𝐿2 (𝜃𝜃̇1 + 𝜃𝜃̇2 )2 −𝐿𝐿1 𝜃𝜃̇12 + 𝑔𝑔𝑠𝑠1
1 1
1𝑓𝑓 = 2𝑅𝑅 � 𝑚𝑚2 𝐿𝐿1 𝜃𝜃̈1 𝑐𝑐2 + 𝑚𝑚2 𝐿𝐿1 𝜃𝜃̇12 𝑠𝑠2 + 𝑚𝑚2 𝑔𝑔𝑐𝑐12 + 𝑚𝑚2 𝐿𝐿2 (𝜃𝜃̈1 + 𝜃𝜃̈2 ) � + 𝑚𝑚1 � 𝐿𝐿1 𝜃𝜃̈1 + 𝑔𝑔𝑐𝑐1 �
0 0
1
1𝑛𝑛
0
2 0
= 2𝑛𝑛 + 𝑚𝑚1 � �
𝐿𝐿21 𝜃𝜃̈1 + 𝐿𝐿1 𝑔𝑔𝑐𝑐1
0
+ 𝑚𝑚2 � 0 �
2
𝐿𝐿1 𝜃𝜃1 − 𝐿𝐿1 𝐿𝐿2 𝑠𝑠2 �𝜃𝜃1̇ + 𝜃𝜃̇2 � + 𝐿𝐿1 𝑔𝑔𝑠𝑠2 𝑠𝑠12 + 𝐿𝐿1 𝐿𝐿2 𝑐𝑐2 �𝜃𝜃̈1 + 𝜃𝜃̈2 � + 𝐿𝐿1 𝑔𝑔𝑐𝑐2 𝑐𝑐12
2 ̈

The joint torques are

𝑇𝑇1 = 𝑚𝑚2 𝐿𝐿22 �𝜃𝜃̈1 + 𝜃𝜃̈2 � + 𝑚𝑚2 𝐿𝐿1 𝐿𝐿2 𝑐𝑐2 �2𝜃𝜃̈1 + 𝜃𝜃̈2 � + (𝑚𝑚1 + 𝑚𝑚2 )𝐿𝐿21 𝜃𝜃̈1 − 𝑚𝑚2 𝐿𝐿1 𝐿𝐿2 𝑠𝑠2 𝜃𝜃̇22 + 𝑚𝑚2 𝐿𝐿2 𝑔𝑔𝑐𝑐12
+ (𝑚𝑚1 + 𝑚𝑚2 )𝐿𝐿1 𝑔𝑔𝑐𝑐1

𝑇𝑇2 = 𝑚𝑚2 𝐿𝐿1 𝐿𝐿2 𝑐𝑐2 𝜃𝜃̈1 + 𝑚𝑚2 𝐿𝐿1 𝐿𝐿2 𝑠𝑠2 𝜃𝜃̇12 + 𝑚𝑚2 𝐿𝐿2 𝑔𝑔𝑐𝑐12 + 𝑚𝑚2 𝐿𝐿2 𝑔𝑔𝑐𝑐12 + 𝑚𝑚2 𝐿𝐿22 �𝜃𝜃̈1 + 𝜃𝜃̈2 �