Kinematics

Felix Monasterio-Huelin, Alvaro Gutierrez & Blanca Larraga

September 4, 2019

Contents

Contents 1

List of Figures 1

List of Tables 2

Acronym list 3

1 Degrees of freedom and kinematic chains of rigid body mechanisms 4

2 Six degrees of freedom robot 7

3 Forward kinematics of position and orientation 8

4 Hand orientation vector (a, n, s) 11

5 Kinematic decoupling 12

6 Inverse kinematics of position 12

7 Inverse kinematics of orientation 14

8 Kinematic conditions for q4 = 0 16

A Vector operations and rotation matrices 18

B Solution to the inverse kinematics of position 20

List of Figures

1.1 Two DOFs planar mechanisms. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41.2 Zero DOFs planar mechanism. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41.3 One DOF plan mechanism: simple pendulum. . . . . . . . . . . . . . . . . . . . . . 51.4 3 DOFs three-dimensional mechanism. . . . . . . . . . . . . . . . . . . . . . . . . . 62.1 6 DOFs robot. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73.1 Local reference systems. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 83.2 Local reference systems and kinematic pair (2, 3). . . . . . . . . . . . . . . . . . . . 94.1 Hand orientation and projection over plane XY. . . . . . . . . . . . . . . . . . . . . 115.1 Kinematic decoupling. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 126.1 4 DOFs mechanism. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 136.2 Two different solutions to the same inverse kinematics problem of position. . . . . . 147.1 RI

SHconfiguration with RII

SA. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

7.2 Solutions to the inverse kinematics problem of orientation. . . . . . . . . . . . . . . 158.1 Hand orientation with q4 = 0. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17B.1 4 DOFs robot. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20

1

List of Tables

2

Acronym List

DOF: Degree of freedom

SO: Special Orthogonal Group (also called rotation group)

SE: Special Euclidean

Si: Local Reference System of every i-segment

oi: Origin of every Si

oi: Point oi declared in the reference system Si−1 prior to the rotation

H: Hand

A: Arm

C: Wrist center

Q: Hand’s end

3

1 Degrees of freedom and kinematic chains of rigid body mechanisms

The concept of Degree of Freedom (DOF) has different meanings. In this document, it will beexclusively used in the sense of the mobility of a mechanism. This mechanism is considered to becomposed of a set of rigid bodies connected by any type of joint, that is, creating a kinematic chainof rigid bodies.

l1

l2

(a) Two DOFs planar serial mechanism.

l1

l2

l3 = l1

l4

(b) Five bars Two DOFs planar parallel mechanism.

Figure 1.1: Two DOFs planar mechanisms.

Figure 1.1 shows two planar mechanisms of two DOFs. Figure 1.1a shows a serial mechanism as anopen kinematic chain while Figure 1.1b shows a parallel mechanism as a closed kinematic chain.Serial mechanisms are always composed of open kinematic chains, while in parallel mechanisms thereis, at least, one closed kinematic chain. Mechanisms are defined as parallel when the end effector(l2 in Figure 1.1b) is connected to more than one independent mechanism: l1 is one independentmechanism and l3 and l4 form another one. An advantage of parallel mechanisms is that motorscan be attached to a common surface, while in serial mechanisms motors must be attached to eachjoint. As a consequence, parallel mechanisms are typically lighter than serial ones and reduce theinherent flexibility of a rigid bar.

A kinematic pair is a connection between two rigid bodies that imposes constraints on theirrelative movement. A kinematic pair is defined by its DOFs, but it is also typical to talk about theDOFs of the joint of the kinematic par. Therefore, it is the same to talk about a rotational joint of1 DOF than a kinematic pair with a rotational joint of 1 DOF.

l1

l2

Figure 1.2: Zero DOFs planar mechanism.

The mobility of a mechanism is a kinematic concept that indicates the capacity that the mech-anism has to perform infinitesimal movements. The number of DOFs is the minimum number ofindependent kinematic variables that allows a mechanism to move. For example, the mechanism ofFigure 1.2 consists of a closed planar kinematic chain made up of two bars joined to the ground by3 rotational joints. It has no mobility, so the number of DOFs equals zero.

The number of DOFs of a rigid body that moves freely in the three-dimensional Cartesian spaceequals 6: three for the position in R3 and three for the orientation in SO(3) (creating the SE(3) =

4

R3 × SO(3) space, called group of spatial rigid transformations which dimension is D = 6). Ifmovements are limited in such a way that it remains in the plane, the number of DOFs is reduced to3: two for the position in R2 and one for the orientation in SO(2) (creating the SE(2) = R2×SO(2)space, called group of planar rigid transformations which dimension is D = 3). Independently ofhow the plane is limited, the system can be defined as three independent equations that only dependon the position and orientation. The rotational space has changed from SO(3), with D = 3, toSO(2), with D = 1 and the Cartesian space has changed from R3 to R2. Moreover, if the system islimited to perform rotations around a perpendicular axis to the plane (rotation kinematic pair R) ortranslation motion following a line contained in the plane (translational or prismatic kinematic pairP ), the number of DOFs will be reduced to 1. On the first case, this is because it cannot performany translational movement, so it can only perform rotations on a subspace of D = 1, that is, acircumference. On the second case, this is because it cannot perform any rotational movement so itcan only perform translations on a subspace of D = 1, that is, a line.

a

Q

Figure 1.3: One DOF plan mechanism: simple pendulum.

Figure 1.3 shows the first case, a simple pendulum. It is a 1 DOF mechanism where the center ofmass (point Q) can only be moved in a circumference of constant radio. To change the orientationof a vector, because the movement is limited to the plane, just one rotation is allowed. Therefore,there are two constraints with respect to the position in the Cartesian plane.

In general, it is not easy to know the number of constraints of a three-dimensional mechanism.However, if a mechanical system in R3 has a set of N rigid bodies and m constraints than onlydepend on the position ri ∈ R3, i = 1 . . . N and the orientation Ri ∈ SO(3) defined by integrableand independent vector functions such as gk(r,R) = 0, k = 1 . . .m, then the Implicit FunctionTheorem shows that the system has n = 6N −m DOFs.

n = 6N −m (1.1)

As an example, a mechanism as the one shown in Figure 1.1a, which has 2 segments (withouttaking into account the bar in the ground), N = 2, and two rotational joints (of 1 DOF each), has2 DOFs (n = 2). This is because the number of constraints (in the three-dimensional space R3) ofeach joint is 5, therefore m = 10.

In the literature, several studies have focused on the calculation of the number of DOFs of arigid body attending to the number of elements, number of joints or kinematic pairs and the DOFsof every joint or kinematic pair. The Grubler-Kutzbach mobility criteria is the most known. Howeverit does not calculates correctly the number of DOFs of every mechanism.

The Grubler-Kutzbach equation is written as follows:

5

n = D(N − 1− j) +

j∑i=1

fi (1.2)

where D is the space dimension in which the mechanism is located (D = 6 in a three-dimensionalspace and D = 3 in the plane), N is the number of elements in which the anchor to the ground iscounted, j is the number of joints and fi is the number of DOFs of the i− th joint.

Equation 1.2 is valid for many mechanisms (simple open (j = N − 1) and closed (j = N )kinematic chains included). The closed kinematic chain planar mechanism (D = 3) of Figure 1.1bhas n = 2 DOFs because it has five segments (taking into account the anchor to the ground),N = 5, and five rotational joints, j = 5, of 1 DOF each, fi = 1.

Figure 1.4 shows a 3 DOFs serial mechanism as a simple open kinematic chain which satisfiesthe Grubler-Kutzbach equation: D = 6, N = 4, j = 3 and fi = 1 with i = 1, 2, 3.

x1

z1

x2

z2

x3

z3

x

y

z

q1

P

P⊥

q2

q3

l0

l1

l2

l3

Figure 1.4: 3 DOFs three-dimensional mechanism.

Every DOF is associated to qi, i = 1 . . . n, which will be named in this text as generalizedcoordinate or joint coordinate.

6

2 Six degrees of freedom robot

q4

q6

x

y

z

q1

q2

q3

q5

Figure 2.1: 6 DOFs robot.

Figure 2.1 shows the robot to be studied in this document. It has 6 DOFs {q1, q2, q3, q4, q5, q6}. InSection 8 a specific case is studied, where the movement of q4 is restricted with the objective ofanalyzing a 5 DOFs robot.

The specific generalized coordinates’ values shown in Figure 2.1 are

{π

3,5π

36,π

9,−3π

4,π

4,π

4

}.

Moreover, Figure 2.1 shows which are the rotation axes of each segment relative to the previous one.Section 3 defines more exhaustively the reference system of every segment.

In general, any set of generalized coordinates can be selected as long as they are independent ofeach other. In fact, every generalized coordinate should be an invertible linear combination of theones defined in this document. It is not mandatory that the generalized coordinates are relative anglesbetween adjacent segments. For example, the third generalized coordinate could have been definedas θ3 = q2 + q3, and the set {q1, q2, θ3, q4, q5, q6} will still represent the generalized coordinates ofthe same robot. In practice, the selection of one system or another depends on the type of robotand the study to be done. The selection of Figure 2.1 is done because the actuators will be locatedat every joint, so it is the most convenient representation from the control point view.

In the remainder of this document, the first three generalized coordinates {q1, q2, q3} will becalled the arm or elbow and the last three ones {q4, q5, q6} will be called the hand or spherical wrist.Section 5 will show how a robot composed of these two structures simplifies the inverse kinematicsproblem. That is, the problem of obtaining the generalized coordinates when the position androtation, in the Cartesian space, of the hand’s end are known. In any robotic problem related to atask of grasping, transporting, pushing, etc. and provided that there are no exteroceptive sensors(such as vision, touch, etc., that is, sensors relative to the outside world of the robot), it will bemandatory to solve this problem. That is because the robot will start with a specific configurationSq0 and should move to another one Sqf . Nonetheless, the trajectory generation problem and thecontrol problem should also be solved.

Finally, the problem of obtaining the position and orientation, in the Cartesian space, of the endeffector of the hand (hand’s end) when the generalized coordinates are known is called the forwardkinematics problem. This problem will be studied in Section 3.

7

3 Forward kinematics of position and orientation

Every point P in the three-dimensional Cartesian space can be represented by coordinates relatedto any reference system. Moreover, two reference systems can relate to each other by means of arotation of their coordinates axes and a translation of the coordinates origin. In Appendix A, thoserotation matrices are defined and some properties are demonstrated. Those matrices will be usedalong the document.

Therefore, if the coordinates of a point P related to one reference system S1 are known, it ispossible to obtain the coordinates related to another system, S0, if the rotation R1

0 and translationt10 matrices between both systems are known:

p0 = R10p1 + t10 (3.1)

where p0 represents the coordinates of point P related to S0, p1 represents the coordinates of thesame point P related to S1 and t10 represents the translation vector defined in S0. The transformationgiven by Equation 3.1, is called rigid motion.

q4

q6

x1

z1 x2

z2

x3

z3

x4

z4

x5

z5

x6 z6

x

y

z

q1

q2

q3

q5

O

Figure 3.1: Local reference systems.

Figure 3.1 represents the local reference system Si of every segment of the robot shown inFigure 2.1. Notice that only two axes have been represented. The third one is obtained by the crossproduct of the other two:

yi = zi × xi (3.2)

As already mentioned, the election of the reference system on every segment is arbitrary. However,there exist systematic representations as the Denavit-Hartenberg representation, which will not betreated in this document. Nevertheless, because all joints of the robot are rotational, in this documentthe local reference systems will be defined as a rotation of the kinematic pair by keeping the nameof the axis but modifying the subindex.

Let li be the distance between two joints of the kinematic pair (i− 1, i) and (i, i+ 1). Distancel0 is defined as the distance between the anchor to the ground (point O) and the joint of thekinematic pair (0, 1). Therefore, the reference system S0 is a motionless reference system parallel tothe inertial reference system (x, y, z). If there is not any confusion, S0 will be also used to represent

8

the inertial reference system. On the other hand, because the kinematic pair (n, n + 1) does notexist, a significant point Q in the n-th segment will be defined. This point Q is called the “hand’send”. This point is typically a tool when the robot is a manipulator, e.g. a gripper with two mobilefingers. In this example, the point Q could be the geometric center between the gripping point ofthe fingers. Therefore, distance ln will be the distance between the kinematic pair (n − 1, n) andthe point Q.

Moreover, let the set of rotation axes be defined as U = {u1, u2, u3, u4, u5, u6}, where ui rep-resents the rotation of the segment related to its kinematic pair. Therefore, for the example ofFigure 3.1 the set of rotation axes is:

U = {z0,−y1,−y2, x3, y4, x5} (3.3)

where z0 = z.

x1

z1 x2

z2

o2

o3

x3

z3

x4

z4

x5

z5

x6 z6

x

y

z

q3

O

l3

Figure 3.2: Local reference systems and kinematic pair (2, 3).

The origin oi of every local reference system Si is located at the joint of the kinematic pair(i, i + 1). An exception exists with the origin on that is located at the already defined point Q.Figure 3.2 shows the kinematic pair (2, 3) that allows to define the generalized coordinate q3. Asalready defined, the rotation axis of segment 3 is −y2. Moreover, according to the aforementionednotation, li = |−−−→oi−1oi|.

Let the vector dii−1, declared in the reference system Si−1, be defined as:

dii−1 =−−−→oi−1oi (3.4)

Moreover, let the set O be defined as O = {o0, o1, o2, o3, o4, o5, o6}, where oi refers to the pointoi declared in the reference system Si−1 prior to the rotation, that is for qi = 0. Therefore, for therobot of Figure 3.1

O = {l0z, l1z0, l2x1, l3x2, l4x3, l5x4, l6x5} (3.5)

According to this convention:

dii−1 = Rii−1oi (3.6)

9

For the example of the kinematic pair (2, 3) of Figure 3.2

d32 = R32o3 (3.7)

Segment 3 rotates around u3 = −y2 (see Equation 3.3), o3 = l3x2 (see Equation 3.5) andaccording to Appendix A:

R32 = R−y,q3 =

cos q3 0 − sin q30 1 0

sin q3 0 cos q3

(3.8)

Therefore, according to Equation 3.6:

d32 = R32o3 =

cos q3 0 − sin q30 1 0

sin q3 0 cos q3

l300

= l3

cos q30sin q3

(3.9)

where l3 = |−−→o2o3|.In what follows, some recursive relations for vectors dji are derived. According to Equation 3.1,

for any point P in the space:

p0 = Ri0pi + di0 (3.10a)

p0 = Ri−10 pi−1 + di−1

0 (3.10b)

pi−1 = Rii−1pi + dii−1 (3.10c)

where p0 represents point P in coordinates of S0, pi represents the same point P in coordinates ofSi and pi−1 represents the same point P in coordinates of Si−1.

Because the composition of rotations of local reference systems satisfies the chain rule, that is:

Ri0 = R1

0R21 · · ·Ri

i−1 (3.11)

it will be satisfied, by substituting Equation 3.10c in 3.10b and subtracting Equation 3.10a, that

di0 = di−10 +Ri−1

0 dii−1 (3.12)

Moreover, Equation 3.11 can be recursively expressed as:

Ri0 = Ri−1

0 Rii−1 (3.13)

Equation 3.12 represents the fundamental equation of the forward kinematics of position andEquation 3.13 represents the fundamental equation of the forward kinematics of orientation.

Thanks to these two fundamental equations, the position of the final point of the hand Q = on(−−→OQ = l0k+−−→o0on = l0k+ dn0 ) and its orientation Rn

0 can be calculated in the S0 reference system.

10

4 Hand orientation vector (a, n, s)

It is common to define the orientation of the hand by three orthonormal vectors {a, n, s}, that willbe called approach, normal and sliding respectively, where

a =

axayaz

(4.1a)

n =

nx

ny

nz

(4.1b)

s =

sxsysz

(4.1c)

The approach vector lays in the direction of the gripper towards the object to grasp. The slidingvector describes the direction of the sliding of the fingers of the gripper and the normal vector is thethird vector which completes the reference system of the hand.

Figure 4.1 shows the orientation of the hand in the space.

a

s

n

x

y

z Q

Q⊥

a⊥

Q⊥

1

Q⊥

2

s⊥

Q1

Q2

αa

αs

Figure 4.1: Hand orientation and projection over plane XY.

The orientation matrix RSOreferred to the inertial system, can be represented in different forms,

depending on the definition of the approach vector. For example, if the approach vector matches axisX, RSO

will be represented by Equation 4.2a. On the other hand, if the approach vector matchesaxis Z, RSO

will be represented by Equation 4.2b,

RSO=

ax −nx −sxay −ny −syaz −nz −sz

(4.2a)

RSO=

nx sx axny sy aynz sz az

(4.2b)

Nonetheless, both cases match the following relationships:

n = s× a (4.3a)

s = a× n (4.3b)

a = n× s (4.3c)

a · n = a · s = n · s = 0 (4.3d)

where × represents the cross product and · the dot product.

11

5 Kinematic decoupling

Every 6 DOFs mechanism, in which its last three joints intersect in a point, allows the kinematicdecoupling of position and orientation. The point of intersection of the last three joints is calledwrist center.This point is shown in Figure 5.1 as point C. This means that it is possible to solve theinverse kinematics problems of position and orientation independently. More concretely, it is possibleto obtain the generalized coordinates of the three first joints by solving an inverse kinematics problemof position and the last three ones by solving an inverse kinematics problem of orientation.

The mechanism studied in the previous sections satisfies this condition, because it has an sphericwrist. Therefore, the movement of the last three joints around their rotation axes does not changethe wrist center.

Figure 5.1 shows this mechanism, where Q represents the position and {a, n, s} the orientationof the hand’s end.

C⊥

Q⊥

a

s

n

x

y

z

Q

C

O

l0

l1

l2

l3

l4

l5

l6

Figure 5.1: Kinematic decoupling.

The procedure must be implemented as follows:

1. Obtain the wrist center (C) when the approach vector (a), the desired point of the hand’s end(Q) and the hand dimension (dH = l5 + l6) are known:

−−→OC =

−−→OQ− dH a (5.1)

2. Solve the inverse kinematics problem of position C to obtain {q1, q2, q3}. This will be studiedin Section 6.

3. Calculate the rotation matrix R30 = R1

0R21R

32 that will be called RSA

.

4. Calculate RSHonce the desired orientation of the mechanism RS0 and RSA

are known, as:

RSH= RT

SARS0 (5.2)

5. Solve the inverse kinematics problem of orientation R63 to obtain {q4, q5, q6}. This will be

studied in Section 7.

6 Inverse kinematics of position

In this Section, a mechanism as the one shown in Figure 6.1, where C is the center of the wrist, isconsidered. This figure shows the arm of the robot.

12

x1

z1

x2

z2

x3

z3

x

y

z

q1

P

P⊥

C

q2

q3

l0

l1

l2

l3

l4

Figure 6.1: 4 DOFs mechanism.

The inverse kinematics problem consists of obtaining the generalized coordinates of the centerof the wrist when the coordinates of a point Q, fixed in the hand’s end, are known

Q =

Qx

Qy

Qz

(6.1)

If the arm and hand can be decoupled, the approach axis can be located at a point C (wristcenter) with coordinates

C =

Cx

Cy

Cz

(6.2)

The wrist center C can be known if the dimensions of the segments of the hand (l5 and l6) andthe approach vector a are known,

−−→OC =

−−→OQ− (l5 + l6) a (6.3)

where O is the coordinates origin of the reference system SO.The approach vector can be obtained from the rotation matrix of the fixed system at the hand’s

end (RSO). For example, if this matrix has been defined as in Equation 4.2a, then

a =

ax −nx −sxay −ny −syaz −nz −sz

100

(6.4)

A compact form of writing coordinates of point C is

−−→OC =

−−→OQ− (l5 + l6)RSO

i (6.5)

where

i =

100

(6.6)

13

If this matrix was defined as in Equation 4.2b, that is, the approach axis was axis Z,

−−→OC =

−−→OQ− (l5 + l6)RSO

k (6.7)

where

k =

001

(6.8)

The inverse kinematics problem of position consist of obtaining the generalized coordinates{q1, q2, q3} when the coordinates of point C are known. Figure 6.2 shows the two possible solutionsfor a similar example.

C

C⊥

x1

z1

x2

z2

x3

z3

x4

z4

x

y

z

l0

l1l2 l3

l4

(a) RISA

configuration.

C

C⊥

x1

z1

x2z2

x3

z3

x4

z4

x

y

z

l0

l1

l2

l3

l4

(b) RIISA

configuration.

Figure 6.2: Two different solutions to the same inverse kinematics problem of position.

Appendix B shows how to obtain the different solutions {q1, q2, q3} when the arm rotation axesare {z0,−y1,−y2},

q1 =

{atan2 (Cy, Cx) Cx = 0 ∨ Cy = 0

R Cx = Cy = 0

q2 = −β + atan2(L,±

√(1− L2)

)q3 = −q2 + atan2 (Cz′ − l2 sin q2, Cx′ − l2 cos q2)

(6.9a)

(6.9b)

(6.9c)

where

β = atan2 (Cx′ , Cz′)

L =l22 + C2

x′ + C2z′ − (l3 + l4)

2

2l2

√(C2x′ + C2

z′)

Cx′ = Cx cos q1 + Cy sin q1

Cz′ = Cz − (l0 + l1)

(6.10a)

(6.10b)

(6.10c)

(6.10d)

7 Inverse kinematics of orientation

The inverse kinematics problem of orientation consists of obtaining the generalized coordinates whenthe rotation matrix SO of a fix reference system at the hand’s end is known.

RSO= R3

0R63 (7.1)

If the arm and hand can be decoupled, RSA= R3

0 is known if the inverse kinematics problem ofposition is already solved. As a consequence, RSH

= R63 can be obtained by solving:

RSH= RT

SARSO

(7.2)

14

C⊥

Q⊥

a

s

n

x3

z3

x4

z4

x5

z5

x6

z6

x

y

z

q1

O

Figure 7.1: RISH

configuration with RIISA

.

Figure 7.1 shows one of the possible solutions to the inverse kinematics problem of orientationfor the solution RII

SAof the inverse kinematics problem of position obtained in Section 6.

C⊥

Q⊥

a

s

n

x3

z3

x4

z4

x5

z5

x6

z6

x

y

z

q1

O

(a) RISH

configuration with RISA

C⊥

Q⊥

a

s

n

x3

z3

x4

z4

x5

z5

x6

z6

x

y

z

q1

O

(b) RISH

configuration with RIISA

C⊥

Q⊥

a

s

n

x3

z3

x4

z4

x5

z5

x6

z6

x

y

z

q1

O

(c) RIISH

configuration with RISA

C⊥

Q⊥

a

s

n

x3

z3

x4

z4 x5

z5x6

z6

x

y

z

q1

O

(d) RIISH

configuration with RIISA

Figure 7.2: Solutions to the inverse kinematics problem of orientation.

Figure 7.2 shows the two possible solutions to the inverse kinematics problem of orientation forevery solution to the inverse kinematics problem of position obtained in Section 6.

The rotation matrix R63 is obtained as a composition of three successive rotations. Therefore, it

has three unknowns variables which represent the generalized coordinates {q4, q5, q6} of the hand.That is:

R63 = R4

3R54R

65 (7.3)

The generalized coordinates {q4, q5, q6} chosen represent the rotation angles relatives to the priorsegment. Let the rotation axes be {x3, y4, x5}. As a consequence:

R43 = RX,q4 (7.4a)

R54 = RY,q5 (7.4b)

R65 = RX,q6 (7.4c)

15

where RX,q4 , RY,q5 and RX,q6 represent the elemental rotation matrix around axis X, Y and Xrespectively.

R63 =

1 0 00 cos q4 − sin q40 sin q4 cos q4

cos q5 0 sin q50 1 0

− sin q5 0 cos q5

1 0 00 cos q6 − sin q60 sin q6 cos q6

(7.5)

By developing Equation 7.5 1,

R63 =

c5 s5s6 s5c6s4s5 c4c6 − s4c5s6 −c4s6 − s4c5c6−c4s5 s4c6 + c4c5s6 −s4s6 + c4c5c6

= U =

u11 u12 u13u21 u22 u23u31 u32 u33

(7.6)

A specific case occurs when s5 = 0. In this case, matrix R63 remains as one of the following

equations:

R63 =

1 0 00 c4c6 − s4s6 −c4s6 − s4c60 s4c6 + c4s6 −s4s6 + c4c6

=

1 0 00 cos (q4 + q6) − sin (q4 + q6)0 sin (q4 + q6) cos (q4 + q6)

(7.7a)

R63 =

−1 0 00 c4c6 + s4s6 −c4s6 + s4c60 s4c6 − c4s6 −s4s6 − c4c6

=−1 0 0

0 cos (q4 − q6) sin (q4 − q6)0 sin (q4 − q6) − cos (q4 − q6)

(7.7b)

Therefore, the inverse kinematics problem of orientation consists of solving the nine non-linearequations that can be obtained from identifying the following matrices:

R63 = RSH

(7.8)

However, because they are rotation matrices, it will only be necessary to solve three independentequations of the identification. Therefore, solutions can be written as follows:

q5 = atan2

(±√

1− u211, u11

)

q4 =

atan2 (u21,−u31) , sin q5 > 0

atan2 (−u21, u31) , sin q5 < 0

−u11q6 + atan2 (u32, u22) , |u11| = 1

q6 =

atan2 (u12, u13) , sin q5 > 0

atan2 (−u12,−u13) , sin q5 < 0

u11 (−q4 + atan2 (u32, u22)) , |u11| = 1

(7.9a)

(7.9b)

(7.9c)

It can be observed that two possible solutions are obtained when |u11| = 1. Moreover, infinitesolutions can be found when |u11| = 1, because in this case there is a dependence between q4 andq6.

8 Kinematic conditions for q4 = 0

This Section shows the conditions to be met when q4 = 0, assuming that the arm and hand canbe decoupled. To impose q4 = 0 is equivalent to impose that the approach vector a belongs to themovement plane of the arm.

1For clarity on the equations, sin and cos are replaced by s and c respectivelly

16

a

s

n

x

y

z

x3

z3

x4

z4

q1

Q

Q⊥

a⊥

C⊥

C

αa

αQ

Figure 8.1: Hand orientation with q4 = 0.

Figure 8.1 shows a situation in which q4 = 0.R6

3 given by Equation 7.6 when q4 = 0 is converted to:

R63 =

c5 s5s6 s5c60 c6 −s6

−s5 c5s6 c5c6

= U (8.1)

It can be observed that it is immediate to obtain the generalized coordinates {q5, q6},

q5 = atan2 (−u31, u11) (8.2a)

q6 = atan2 (−u23, u22) (8.2b)

Different from the general case where q4 = 0, now there is a unique solution to the inversekinematics problem of orientation. Moreover, the approach vector a will be limited to the planedefined by q1.

17

A Vector operations and rotation matrices

Definition A.1 (Orthogonal matrix R). An squared orthogonal matrix R is a matrix which rowsand columns form an orthonormal base of an Euclidean space, that is, their module is one and areorthogonal between them.

The inverse of an orthogonal matrix is its transpose matrix,

RTR = I (A.1)

where I is the identity matrix and RT is the transpose matrix of R.The determinant of an orthogonal matrix is det(R) = 1 or det(R) = −1, but not every matrix

with a determinant 1 or −1 is orthogonal.

Definition A.2 (Elemental rotation matrices Rx, Ry, Rz). Rotation matrices R ∈ SO(n), wherethe set SO(n) is a special orthogonal group of dimension n. In general, the dimension of SO(n) isn(n− 1)

2.

The set SO(3) is of dimension 3, that means that it is always possible to transform a referencesystem into another one by a consecutive sequence of three rotations around a single axis (elementaryrotations).

The set SO(2) is of dimension 1, that means that it is always possible to transform a referencesystem into another one by a single rotation around a single axis.

Let the elementary rotation matrices R ∈ SO(3) around axis x, y and z be defined as:

Rx,θ =

1 0 00 cos θ − sin θ0 sin θ cos θ

(A.2a)

Ry,θ =

cos θ 0 sin θ0 1 0

− sin θ 0 cos θ

(A.2b)

Rz,θ =

cos θ − sin θ 0sin θ cos θ 00 0 1

(A.2c)

The elementary rotation matrix R ∈ SO(2) is defined as:

Rθ =

[cos θ − sin θsin θ cos θ

](A.3)

Proposition A.3 (Rotation composition). Let the matrix Rji be defined as the rotation matrix of

the reference system Sj with respect to Si.

Given the rotation matrices Rki and Rj

k and the reference systems Si, Sj y Sk, it is true that

Rji = Rk

iRjk (A.4)

For example, R30 can be rewritten by following the chain rule as:

R30 = R1

0R21R

32 (A.5)

Proposition A.4 (Coordinates of a point in two reference systems). Let point P be defined as afixed point in the space which coordinates in the reference system Si are given by vector pi and letmatrix Rj

i be defined as the rotation matrix of the reference system Sj with respect to Si. Then itis fulfilled that

pi = Rjipj (A.6)

For example, if Rji is the elemental matrix Rθ ∈ SO(2), then if pj = (cos θ,− sin θ)T , it is true

that pi = (1, 0)T . It can be observed that pj = (Rji )

T pi = Rijpi.

18

Proposition A.5 (Antisymmetric matrix S). Given two vectors a, b ∈ R3, there is a single antisym-metric matrix S(a) such that

a× b = S(a)b (A.7)

where × represents the cross product and

S(a) =

0 −az ayaz 0 −ax−ay ax 0

(A.8)

Antisymmetric matrices S satisfy the condition that S + ST = O.

Proposition A.6. Because a× b = −b× a it is fulfilled that

a× b = −S(b)a (A.9)

Proposition A.7. For every rotation matrix R ∈ SO(3)

R(a× b) = (Ra)× (Rb) = S(Ra)Rb = −S(Rb)Ra (A.10)

Proposition A.8. For every rotation matrix R ∈ SO(3)

S(Ra)Rb = RS(a)b (A.11)

Proposition A.9. By substituting c = Rb in A.11, for every rotation matrix R ∈ SO(3), it is truethat

RS(a)RT c = S(Ra)c (A.12)

Proposition A.10. Because RTR = I, the square of the Euclidean norm of vector a can be rewrittenas:

a · a = aTa = aTRTRa = (Ra)T (Ra) = (Ra) · (Ra) (A.13)

where · is the dot product.This means that the norm is invariant to rotations. In general, it can be assured that

a · b = (Ra) · (Rb) (A.14)

where R is an orthogonal matrix.

Proposition A.11.

a · b = tr(abT ) =1

2tr[S(a)TS(b)] (A.15)

where tr[A] represents the trace of matrix A.

The norm of Frobenius of matrix A is defined as ||A||F =√tr[AAT ].

Proposition A.12. Given a rotation matrix R(t) it is true that:

R(t) = S(w(t))R(t) (A.16)

where w(t) represents the angular speed vector and the antisymmetric matrix S(w(t)), defined asEquation A.8, is called angular speed tensor.

Proposition A.12 demonstration. It is known that RTR = RRT = I.It can be said that

R(t) = R(t)RTR (A.17)

It can be demonstrated that S = R(t)RT is an antisymmetric matrix.By deriving expression RRT = I,

RRT +RRT = O (A.18)

that is S + ST = O, hence S is an antisymmetric matrix.

19

B Solution to the inverse kinematics of position

In this Section the equations of the inverse kinematics problem of position are solved by knowing thewrist’s center C of the mechanism of Figure B.1.

x1

z1

x2

z2

x3

z3

x

y

z

q1

P

P⊥

C

q2

q3

l0

l1

l2

l3

l4

Figure B.1: 4 DOFs robot.

Let the rotation axes of the robot be {z0,−y1,−y2, x3}. Therefore, because x3 is the rotationaxis of the fourth segment, the mechanism is equivalent, from the solution of the inverse kinematicsproblem of position perspective, to other mechanism of 3 DOFs in which the length of the lastsegment is l3 + l4. That means that the rotation around x3 does not affect to the solution of theinverse kinematics problem of position.

Because the rotation axis of the first segment is z0, q1 can be immediately obtained as:

q1 = atan2 (Cy, Cx) (B.1)

To obtain q2 and q3 it is mandatory to solve the following trigonometric equations’ system:

Cx′ = l2 cos q2 + (l3 + l4) cos (q2 + q3) (B.2a)

Cz′ = l2 sin q2 + (l3 + l4) sin (q2 + q3) (B.2b)

where

Cx = Cx′ cos q1 (B.3a)

Cy = Cx′ sin q1 (B.3b)

Cz = Cz′ + l0 + l1 (B.3c)

(B.3d)

The solution of Cx′ can be obtained from Equation B.3a and Equation B.3b:

Cx′ = Cx cos q1 + Cy sin q1 (B.4)

The equations’ system of Equation B.2 has two solutions that can be obtained by using:

(Cx′ − l2 cos q2)2 + (Cz′ − l2 sin q2)

2 = (l3 + l4)2 (B.5)

By simplifying Equation B.5:

Cx′ cos q2 + Cz′ sin q2 =l22 + C2

x′ + C2z′ − (l3 + l4)

2

2l2(B.6)

20

To solve Equation B.6, the expression of the sin of the angles sum can be used; therefore:

sin (q2 + β) =l22 + C2

x′ + C2z′ − (l3 + l4)

2

2l2

√(C2x′ + C2

z′) (B.7)

where

sinβ =Cx′√(

C2x′ + C2

z′) (B.8a)

cosβ =Cz′√(

C2x′ + C2

z′) (B.8b)

and in computational form:

q2 = −β + atan2(L,±

√(1− L2)

)(B.9)

where

β = atan2 (Cx′ , Cz′) (B.10a)

L =l22 + C2

x′ + C2z′ − (l3 + l4)

2

2l2

√(C2x′ + C2

z′) (B.10b)

To obtain q3 it is enough to solve the equations’ system given by Equation B.2, when q2 is known;that is:

q3 = −q2 + atan2 (Cz′ − l2 sin q2, Cx′ − l2 cos q2) (B.11)

As shown in Equation B.9, the inverse kinematics problem of position has two possible solutions,although there are singular conditions when Cx = Cy = 0, because Equation B.1 can not be applied.In this situation, q1 could have any value. From Equation B.4 it can be obtained that if Cx = Cy = 0then Cx′ = 0 and vice-versa. Moreover, if Cx′ = 0, then sinβ = 0 so β = 0 or β = π obtaining twopossible solutions for q2 from Equation B.1, that is , sin q2 = ±L. Nonetheless cosβ = ±1, whatimplies that Cz′ = ±1.

It can also be observed that if Cx′ = Cz′ = 0, β cannot be obtained from Equation B.10a. Inthis case, it is mandatory that l2 = l3 + l4, as can be checked from Equation B.5. This singularitycan be easily solved from Equation B.2, as:

0 = l2 cos q2 + (l3 + l4) cos (q2 + q3) (B.12a)

0 = l2 sin q2 + (l3 + l4) sin (q2 + q3) (B.12b)

By multiplying Equation B.12a by sin q2 and Equation B.12b by cos q2 and subtracting one fromthe other, it can be obtained that sin q3 = 0, so q3 = 0 or q3 = π. However, q3 = 0 does not satisfyEquations B.12, but q3 = π does it for any value of q2.

However, when l2 = l3+ l4 it is not necessary that Cx′ = Cz′ = 0. If Cx′ = 0 or Cz′ = 0 there isnot this singularity and β could be obtained uniquely. Therefore, if the first singularity Cx = Cy = 0is not met, neither is the second one. But if the first one is met and Cz = l0 + l1, the secondsingularity will be met. From the robot design point of view, it will be interesting that l2 = l3 + l4,and the singularity will be avoided.

Finally, it can also occur that Cx′ = l2 cos q2 and Cz′ = l2 sin q2, so q2 cannot be obtained form

Equation B.9, because l2 =√(

C2x′ + C2

z′). Moreover, because Equation B.5, l3 + l4 = 0 what is

impossible except for l3 = l4 = 0. Therefore, this singularity cannot be produced because l3 > 0,although it could be considered l4 = 0 without loss of generalization. This implies that it will alwaysbe possible to obtain q3 given q2 and using Equation B.12.

In summary, if l2 = l3 + l4, there is only the possibility of the first singularity.

21

To conclude, and assuming that l2 = l3 + l4, the inverse kinematics problem of position has thefollowing solutions:

q1 =

{atan2 (Cy, Cx) Cx = 0 ∨ Cy = 0

R Cx = Cy = 0

q2 = −β + atan2(L,±

√(1− L2)

)q3 = −q2 + atan2 (Cz′ − l2 sin q2, Cx′ − l2 cos q2)

(B.13a)

(B.13b)

(B.13c)

where

β = atan2 (Cz′ , Cx′)

L =l22 + C2

x′ + C2z′ − (l3 + l4)

2

2l2

√(C2x′ + C2

z′)

Cx′ = Cx cos q1 + Cy sin q1

Cz′ = Cz − (l0 + l1)

(B.14a)

(B.14b)

(B.14c)

(B.14d)

22