See discussions, stats, and author profiles for this publication at: https://www.researchgate.

net/publication/329144030

Inverse kinematics for a Humanoid Robot : a mix between closed form and
geometric solutions

Technical Report · May 2017

DOI: 10.13140/RG.2.2.17317.55520

CITATIONS READS
3 15,419

1 author:

Fabrice Noreils
iFollow
24 PUBLICATIONS 572 CITATIONS

SEE PROFILE

All content following this page was uploaded by Fabrice Noreils on 23 November 2018.

The user has requested enhancement of the downloaded file.

 Inverse kinematics for a Humanoid Robot : a mix between closed form
and geometric solutions

Fabrice R. Noreils
[email protected]

May 1, 2017

1 Introduction
In this paper we present a derivation of the forward kinematics (FK) and inverse kinematics (IK) of a humanoid
robot with 32 degrees of freedom, specifically the ODOI platform. A picture of ODOI is shown in Fig. 1. The FK
and IK are not solved for the entire 32 joints but instead divided into six parts:the two arms(seven joints each), the
two legs (seven joints each), the torso (5 joints), and the head (2 joints).

Figure 1: ODOI humanoid robot.

Forward kinematics (FK) and inverse kinematics (IK) are mandatory in any robot manipulator especially for a
humanoid robot, because usually it is desirable to control the end-effector of the robot (e.g. the hand of one arm)
in its workspace, not in joint space. In other words, tell the hand of the humanoid to go to point (x, y, z) not tell
the joints to go to values θi . However, the joint values are what can be directly controlled, therefore, one needs to
know how to convert from workspace to joint space and vice versa.

1
Pieper [6] outlined two conditions for finding a closed-form joint solution of a robot manipulator in which either
three adjacent joint axes are parallel to one another or they intersect at a single point. A joint solution is said to
be ”closed-form” if the unknown joint angles can be solved for symbolically in terms of the arc-tangent function.
Although a robot manipulator may satisfy one of these two conditions for finding the closed-form joint solution,
it is difficult to develop a consistent procedure for finding a closed-form joint solution for a humanoid robot and
selecting one desirable solution from multiple solutions.
However Ali et. al. [5] [1] presented a closed-form solution for the inverse kinematics (IK) of the limbs of the
HUBO2+ robot platform . They used a reverse decoupling mechanism method by viewing the kinematic chain of
a limb in reverse order and decoupling the position and orientation. The authors then used the inverse transform
method to compute eight possible solutions for each limb. The correct solution is selected based on joint limits and
constraints. Later, O’Flaherty et al. [4] improved the method.
In this paper a mix approach is introduced: a closed-form solution is proposed for the arms and the torso based on
the aforementioned works and a geometric solution is proposed for the legs.

2 Forward and Inverse kinematics for the Arm

2.1 Forward kinematics

The forward kinematics problem is that of solving for the end-effector orientation and position given the joint angles.
This is easily solved using the robot geometry and coordinate frames, which are specified in the Denavit-Hartenberg
(DH) parameters [3]. The general homogeneous transformation from one link to the next given the DH parameters
is represented in matrix form as:
 
cos(θi ) − sin(θi ) cos(αi ) sin(θi ) sin(αi ) ai cos(θi )
i−1
 sin(θi ) cos(θi ) cos(αi ) − cos(θi ) sin(αi ) ai sin(θi ) 
Ti = 
 0
 (1)
sin(αi ) cos(αi ) di 
0 0 0 1
i−1
where Ti is the transformation from coordinate frame i − 1 to frame i.
Create a DH representation of a mechanical structure composed of motorized joints can be a tedious task and it is
really helpful to have the possibility to visualize it to be sure that the parameters of each joint corresponds to what
you have in mind. Please refer to appendix A for some examples of software available for that purpose.
The six i−1 Ti transformation matrices for the right arm of the ODOI robot can be found on the basis of the
coordinate systems established in Fig.2. These i−1 Ti matrices are listed below.
     
C1 0 S1 0 C2 0 −S2 0 C3 0 S3 0
0 S1 0 −C1 0 S2 0 C2 0 S3 0 −C3 0 
, 1 T2 =  , 2 T3 = 
  
T1 =  
 0 −1
 ,
0 1 0 0 0 0 0 1 0 −L2 
0 0 0 1 0 0 0 1 0 0 0 1
     
C4 0 S4 0 C5 0 −S5 0 C6 −S6 0 L4 C6
3
 S4 0 −C4 0 4
 S5 0 C5 0 5
 S6 C6 0 L4 S6 
T4 = 
0
 , T5 =   , T6 =  
1 0 0 0 1 0 L3  0 0 1 0 
0 0 0 1 0 0 0 1 0 0 0 1

The i Ti−1 matrices that will be needed to compute the joints are listed below.
     
C1 S1 0 0 C2 −S2 0 0 C3 S3 0 0
1 0 0 1 0 0 0 −1 0 0 0 0 L2 
, 2 T1 =  , 3 T2 = 
   
T0 = 
 S1 −C1 0 0

−S2
 ,
C2 0 0  S3 −C3 0 0
0 0 0 1 0 0 0 1 0 0 0 1

2
 Figure 2: Denavit Hartenberg parameters for the right arm

   
C4 S4 0 0 C5 S5 0 0
4
0 0 1 0 5
0 0 1 −L3 
T3 =   , T4 =  
 S4 −C4 0 0  S5 −C5 0 0 
0 0 0 1 0 0 0 1

where Si ≡ sin θi and Ci ≡ cos θi

In order to calculate the forward kinematics (FK), the six transformation matrices from each joint are pre-multiplied
to obtain the position and orientation of the end-effector relative to the shoulder. We define the transformation
from the shoulder to the hand as:

6
Y
0 i−1
T6 = Ti = 0 T1 1 T2 2 T3 3 T4 4 T5 5 T6 (2)
i=1

3 Inverse kinematic Joint Position solution for the Arm

The inverse kinematics is the problem of solving for the joint angles given the end-effector orientation and position.
This is a much harder problem because there are multiple solutions. As mentioned before, when solving the inverse
kinematics of a manipulator, Pieper [6] indicates that a closed-form solution exists if three consecutive joint axes
of the manipulator are parallel to one another, or intersect at a single point. The three shoulder joint axes on the
ODOI intersect at a single point for the arms therefore a closed-form kinematic solution exists for the arms.
Let us write 0 T6 as:
   
0 x6 y6 z6 p6 n s a p
T6 = = (3)
0 0 0 1 0 0 0 1

3
where x6 , y6 , and z6 are the unit vectors along the principal axes of the hand frame and p6 is the position vector
describing the location of the hand relative to the shoulder. These three unit vectors describe the orientation of the
hand coordinate frame relative to the shoulder coordinate frame. The vectors n, s, a, and p represent the normal
vector, sliding vector, approach vector, and position vector of the hand, respectively [2].
Using this knowledge, the arm can be viewed in reverse so that the last three joints make up the shoulder, thus
the position and orientation of the shoulder frame can be described relative to the hand frame. This new position
0
vector, p , is only a function of θ4 , θ5 and θ6 , and thus decouples the arm into position and orientation components.
The IK problem is solved in this reverse method by taking the inverse of both sides of Eq. (3). A detailed discussion
(and argumentation) about this method can be found in [5].

 0  0 0 0 0 
0
0
n s a p n s a p
T6 = = = 6 T0 (4)
0 0 0 1 0 0 0 1

We can use the inverse transform method to solve for the last three joint angles by multiplying both sides of Eq.
(4) by 5 T6 . This results in an equation where the left side of the equation is:

0 0 0 0 0 0 0
C6 nx 0 − S6 ny
 
C6 sx − S6 sy C6 ax − S6 ay C6 px − S6 py + C6 L4
0 0 0 0
S6 nx 0 + C6 n0 0 0 0 0 0 0
 
(LHS) n s a p S6 sx + C6 sy S6 ax + C6 ay S6 px + C6 py + S6 L4 
G5−arm == 5 T6 = 0
y
0 0 0
 (5)
0 0 0 1  nz sz az pz 
0 0 0 1

The right side of the equation is:

 
g511 g521 g531 S4 C5 L2
(RHS) 5 5 4 3 2 1 5
g512 g522 g532 −C4 L2 − L3 
G5−arm = T0 = T4 T3 T2 T1 T0 = T6 
  (6)
g513 g523 g533 S4 S5 L2 
0 0 0 1

with:

g511 = C1 (C2 (C3 C4 C5 + S3 S5 ) − S2 S3 C5 ) − S1 (S3 C4 C5 − C3 S5 )

g512 = C1 (C2 C3 S4 + S2 C4 ) + S1 (−S3 S4 )
g513 = C1 (C2 (C3 C4 S5 − S3 C5 ) − S2 S4 C5 ) − S1 (S3 C4 S5 + C3 C5 )
g521 = S1 (C2 (C3 C4 C5 + S3 S5 ) − S2 S4 C5 ) + C1 (S3 C4 C5 − C3 S5 )
g522 = S1 (C2 C3 S4 + S2 C4 ) + C1 (S3 S4 )
g523 = S1 (C2 (C3 C4 S5 − C3 C5 ) − S2 S4 C5 ) + C1 (S3 C4 S5 + C3 S5 )
g531 = S2 (C3 C4 C5 + S3 S5 ) + C2 S4 C5
g532 = S2 C3 S4 − C2 C4
g533 = S2 (C3 C4 S5 − S3 C5 ) + C2 S4 S5

We can solve for the last three joint angles by equating the position terms (last column) of Eq. (5) and Eq. (6) to
get:

0 0
C6 (px + L4 ) − S6 py = S4 C5 L2 (7)
0 0
S6 (px + L4 ) + C6 py = −C4 L2 − L3 (8)
0
pz = S4 S5 L2 (9)

4
 0 0
Let px + L4 = rCψ and py = rSψ , and substituting them into Eqs. (7) and (8), we get:

rC6ψ = S4 C5 L2 (10)
rS6ψ = −C4 L2 − L3 (11)
0
pz = S4 S5 L2 (12)

q 0 0
where r = (p0x + L4 )2 + (p0y )2 and ψ = atan2(py , px + L4 ). By squaring Eqs. (10), (11) and (12) and adding
them, we can obtain an equation for C4 :

0 0 0
(px + L4 )2 + (py )2 + (pz )2 − L22 − L23
C4 = (13)
2L2 L3
q
θ4 = atan2(± 1 − C42 , C4 ) (14)

From Eq. (12), we can find S5 :

0
pz
q
S5 = ; θ5 = atan2(S5 , ± 1 − S52 ) (15)
S5 L2

By dividing Eqs (10) and (11), we get:

S6ψ −C4 L2 − L3
tan6ψ = = (16)
C6ψ S4 C5 L2
θ6 = wrapToPI(atan2(−C4 L2 − L3 , S4 C5 L2 ) − ψ)1 (17)

For the solution of joint angles θ1 , θ2 and θ3 , multiplying both sides of Eq. (4) by 3 T4 , 4 T5 and 5 T6 . This results
in an equation where the left side of the equation is:
 
 0 g411 g421 g431 g441
0 0 0 
(LHS) 3 4 5 n s a p g412 g422 g432 g442 
G4−arm == T4 T5 T6 =
g413
 (18)
0 0 0 1 g423 g433 g443 
0 0 0 1

5
with:
0 0 0
g411 = nx (S4 S6 + C4 C5 C6 ) + ny (S4 C6 − C4 C5 S6 ) + nz C4 S5
0 0 0
g412 = nx (S4 C5 C6 − C4 S6 ) − ny (C4 C6 + S4 C5 6S6 ) + nz S4 S5
0 0 0
g413 = nx S5 C6 − ny S5 S6 − nz C5
0 0 0
g421 = sx (S4 S6 + C4 C5 C6 ) + sy (S4 C6 − C4 C5 S6 ) + sz C4 S5
0 0 0
g422 = sx (S4 C5 C6 − C4 S6 ) − sy (C4 C6 + S4 C5 6S6 ) + sz S4 S5
0 0 0
g423 = sx S5 C6 − sy S5 S6 − sz C5
0 0 0
g431 = ax (S4 S6 + C4 C5 C6 ) + ay (S4 C6 − C4 C5 S6 ) + az C4 S5
0 0 0
g432 = ax (S4 C5 C6 − C4 S6 ) − ay (C4 C6 + S4 C5 6S6 ) + az S4 S5
0 0 0
g433 = ax S5 C6 − ay S5 S6 − az C5
0 0 0
g441 = px (C4 C5 C6 + S4 S6 ) + py (C6 S4 − C4 C5 S6 ) + pz (C4 S5 + S4 L3 ) + L4 (C4 C6 + S4 S6 )
0 0 0
g442 = px (S4 C5 C6 − C4 S6 ) − py (C6 C4 + S4 C5 S6 ) + pz (S4 S5 − C4 L3 ) + L4 (S4 C6 − C4 S6 )
0 0 0
g443 = px S5 C6 − py S5 S6 − pz C5 + L4 S5 C6

and the right side of the equation is:

 
C1 C2 C3 − S1 S3 C1 S3 + S1 C2 C3 S2 C3 0
(RHS) 3 2 1 5
 −C1 S2 −S1 S2 C2 L2 
G4−arm = T2 T1 T0 = T6   (19)
S1 C3 + C1 C2 S3 S1 C2 S3 − C1 C3 S2 S3 0
0 0 0 1

By comparing the element (2,3)2 of LHS and RHS of G4−arm , we can obtain C2 :

0 0 0
C2 = g432 = ax (S4 C5 C6 − C4 S6 ) − ay (C4 C6 + S4 C5 6S6 ) + az S4 S5 (20)
q
θ2 = atan2(± 1 − C22 , C2 ) (21)

By comparing the elements (1,3) and (3,3) of LHS and RHS of G4−arm , we obtain two equations. By dividing these
two equations, we can determine the joint solution θ3 :

S2 S3 g433
= (22)
S2 C3 g431
0 0 0
S3 ax S5 C6 − ay S5 S6 − az C5
= 0 (23)
C3 ax (S4 S6 + C4 C5 C6 ) + a0y (S4 C6 − C4 C5 S6 ) + a0z C4 S5
θ3 = atan2(g433 , g431 ) (24)

by comparing the elements (2,1) and (2,2) of LHS and RHS of G4−arm , we obtain two equations relating to θ1 . By
dividing these two equations, we can determine the joint solution θ1 :

−S1 S2 g422
= (25)
−C1 S2 g412
0 0 0
−S1 sx (S4 C5 C6 − C4 S6 ) − sy (C4 C6 + S4 C5 6S6 ) + sz S4 S5
= 0 (26)
−C1 nx (S4 C5 C6 − C4 S6 ) − n0y (C4 C6 + S4 C5 6S6 ) + n0z S4 S5
θ1 = atan2(−g422 , −g412 ) (27)
2 The notation (row, column) is used in this document.

6
 θ1
Input -130 ... -95 ... -85 ... -20 ... 20 ... 85 ... 95 ... 130
Computed 140 ... 175 ... -175 ... -110 ... -70 ... 70 ... 5 ... 40
sin(θ2 ) < 0 -40 ... -5 ... 5 ... 70 ... 110 ... 175 ... -175 ... -140
Objective -220 ... -185 ... -175 ... -110 ... -70 ... -5 ... 5 ... 40
θ2
Input -130 ... -95 ... -85 ... -20 ... 20 ... 85 ... 95 ... 130
Computed -40 ... -5 ... 5 ... 70 ... 110 ... 175 ... -175 ... -120
Objective -40 ... -5 ... 5 ... 70 ... 110 ... 175 ... 185 ... 220
θ3
Input -130 ... -95 ... -85 ... -20 ... 20 ... 85 ... 95 ... 130
Computed -130 ... -95 ... -85 ... -20 ... 20 ... 85 ... 95 ... 130
sin(θ2 ) < 0 50 ... 85 ... 95 ... 160 ... -160 ... -95 ... -85 ... -50
Objective -130 ... -95 ... -85 ... -110 ... 20 ... 85 ... 95 ... 130

Table 1: (Right arm) Angle values of θ1 and θ3 depend on the sign of sin(θ2 )

At this point, we have a solution for all joints, but the value of θ1 and θ3 depend on the sign of sin(θ2 ). Table 1
shows three tables, one for each joint, namely θ1 , θ2 and θ3 . Each table contain four rows:
• Input: which is the angle provided as a parameter;
• Computed: which is the angle computed based on the above equations;
• sin(θ2 ) < 0: which contains the angle computed when sin(θ2 ) < 0;
• Objective: which is the expected angle value (remember that for θ1 and θ2 there is ±90 + θ in the DH
parameters).
In some cases, one can see that in order to match the objective it is necessary to shift the computed angles by
±πor2π.

3.1 Singularities

Three singularities exist for the arm: Elbow, Shoulder and Shoulder-Elbow. Let us detail each of them.

3.1.1 Case 1: Elbow singularity or θ4 = 0

It means that θ3 and θ5 are collinear. In this case we need to re-calculate all the angles because the equations
introduced in Section 3 were based on the value of C4 . It is necessary to go back to Eqs. (5) and (6) with θ4 = 0
or C4 = 1andS4 = 0.
Using (3,1) from LHS and RHS of G5−arm gives:
0 0
S2 (C3 C5 + S2 S5 ) = S2 (C3,5 ) = C6 ax − S6 ay
Using (3,3) from LHS and RHS of G5−arm gives:
0
S2 (C3 S5 − S2 S5 ) = −S2 (S3,5 ) = az
Thus:

0
−S3,5 az
= (28)
C3,5 C6 a0x − S6 a0y
0 0 0
θT (3−5) = atan2(−az , C6 ax − S6 ay (29)

7
Using (2,1) from LHS and RHS of G5−arm gives:
0
C1 S2 C4 = S6 nx 0 + C6 ny

Using (2,3) from LHS and RHS of G5−arm gives:

0 0
−C2 C4 = S6 ax + C6 ay

thus:

0
S2 S6 nx 0 + C6 ny
= (30)
−C2 C1 (S6 a0x + C6 a0y )
0 0 0
θ2 = atan2(S6 nx 0 + C6 ny , −C1 (S6 ax + C6 ay ) (31)

Please note that in this case the value of θ2 will be affected by the sign of C1 and the values of both θ1 and θT are
also affected by the sign of S2 .
Using (1,4) from LHS and RHS of G5−arm gives:

0 0
0 = C6 px − S6 py + C6 L4 (32)
0 0
S6 py = C6 (px + L4 ) (33)
0
S6 p + L4
= x 0 (34)
C6 py
0 0
θ6 = atan2(px + L4 , py ); (35)

Using (2,1) and (2,2) from LHS and RHS of G5−arm gives:

0 0 0
θ1 = atan2(S6 sx + C6 sy , S6 nx 0 + C6 ny ) (36)

θ1
Input -130 ... -95 ... -85 ... -20 ... 20 ... 85 ... 95 ... 130
Computed 140 ... 175 ... -175 ... -110 ... -70 ... 70 ... 5 ... 40
sin(θ2 ) < 0 -40 ... -5 ... 5 ... 70 ... 110 ... 175 ... -175 ... -140
Objective -220 ... -185 ... -175 ... -110 ... -70 ... -5 ... 5 ... 40
θ2
Input -130 ... -95 ... -85 ... -20 ... 20 ... 85 ... 95 ... 130
Computed -140 ... -175 ... 5 ... 70 ... 110 ... 175 ... -5 ... -40
theta1 < 0 40 ... 5 ... -175 ... -110 ... -70 ... -5 ... 175 ... 140
Objective -40 ... -5 ... 5 ... 70 ... 110 ... 175 ... 185 ... 220
θT
Computed -130 ... -95 ... -85 ... -20 ... 20 ... 85 ... 95 ... 130
sin(θ2 ) < 0 50 ... 85 ... 95 ... 160 ... -160 ... -95 ... -85 ... -50
Objective -130 ... -95 ... -85 ... -20 ... 20 ... 85 ... 95 ... 130

Table 2: (Right arm) θ4 = 0: Angles are shifted according to both the sign of θ1 and the sign of sin(θ2 )

The value of θ3 will be fixed to 0 and thus θ5 = −θT . Again angles are shifted according to both the sign of θ1 and
the sign of sin(θ2 ) as it is shown on Table 2.

8
3.1.2 Case 2: Shoulder singularity or θ2 = 0

It means that θ1 and θ3 are collinear. In this case, it is necessary to go back to Eqs. (5) and (6) with θ2 = 0 or
C2 = 1 and S2 = 0.
In this specific case, we need to re-calculate θ1 and θ3 only. As C4 is not equal to 1 the values of θ4 , θ5 and θ6 with
equations described in Section 3 still hold.
Using (2,1) from LHS and RHS of G5−arm gives:
0
S4 (C1 C3 − S1 S3 ) = S6 nx 0 + C6 ny

Using (2,2) from LHS and RHS of G5−arm gives:

0 0
S4 (S1 C3 + C1 S3 ) = S6 sx + C6 sy

thus:

0 0
S3+1 S6 sx + C6 sy
= (37)
C3+1 S6 nx 0 + C6 n0y
0 0 0
θT = atan2(S6 sx + C6 sy , S6 nx 0 + C6 ny ) (38)

By fixing θ1 = 0 then θ3 = wrapToPI( π2 − θT ). Moreover, in this case there is no need to shift angles.

3.1.3 Case 3 Elbow-shoulder singularity or θ4 and θ2 = 0

It means that θ1 , θ2 and θ3 are collinear. Using (2,1) from LHS and RHS of G5−arm gives:
0
C1 (C3 S5 − S3 C5 ) − S1 (S3 S5 + C3 C5 ) = nz

Using (2,2) from LHS and RHS of G5−arm gives:

0
S1 (C3 S5 − S3 C5 ) + S1 (S3 S5 + C3 C5 ) = sz

thus:
0 0
θT (1+5−3) = atan2(−nz , sz ) (39)

By fixing θ1 = 0 and θ3 = 0, then θ5 = wrapToPI(θT(1+5−3) + π2 )

θ6 is processed as in Section 3.1.1:

0 0
θ6 = atan2(px + L4 , py ); (40)

3.2 Decision table and Metric

The above equations give us 8 solutions, as it is shown in Fig. 3, each one define a Pose: θ1 ...θ6 that brings the
arm to the right position and orientation of the end-effector.
The question is: how to select the right one? This is here that we can select the pose that will meet specific
constraints such as torque limitation or the arm must move along a specific geometric constraint for instance.
For our purpose, I choose to limit the variation of the angles between the previous pose and the next one:

9
 Figure 3: Decision Table for the right/left arm

minimize cost(j)
j
6
X (41)
subject to cost(j) = (θicurrent − θij )2 .
i=1

where θicurrent represents the the current value of the ith joint and θij is the computed value of the ith joint in the
j th row in the decision table.

3.3 Algorithm

Once we have identified all the cases, the algorithm is the following:

10
Algorithm 1 Calculate Inverse kinematic for the Arm
0 0 0
(p +L4 )2 +(p )2 +(p )2 −L2 −L2
C4 = x y
2L2 L3
z 2 3

if C4 = 1 then
Elbow Singularity
θ4 = 0
0 0 0
θT (3−5) = atan2(−az , C6 ax − S6 ay
0 0 0
θ1 = atan2(S6 sx + C6 sy , S6 nx 0 + C6 ny )
θ3 = 0
θ5 = −θT (3−5)
0 0
θ6 = atan2(px + L4 , py )
0 0 0
θ2 = atan2(S6 nx 0 + C6 ny , −C1 (S6 ax + C6 ay )
if θ2 = π then
Elbow Shoulder” Singularity
0 0
θT (1+5−3) = atan2(−nz , sz )
θ1 = 0, θ3 = 0
if right arm then
θ5 = wrapToPI(θT(1+5−3) + π2 )
else
θ5 = wrapToPI( π2 − θT )
end if
end if
else
(End of C4 = 1) Shift angle according to Fig. ?? (right arm) or Fig. ?? (left arm)
else p
θ4 = atan2(± 1 − C42 , C4 )
θ6 = wrapT oP I(atan2(−C
p 4 L2 − L3 , S4 C5 L2 ) − ψ)
θ5 = atan2(S5p , ± 1 − S52 )
θ2 = atan2(± 1 − C22 , C2 )
if θ2 = π then
Shoulder singularity
0 0 0
θT = atan2(S6 sx + C6 sy , S6 nx 0 + C6 ny )
π
θ3 = −wrapToPI( 2 − θT )
θ1 = 0
if right arm then
θ3 = wrapToPI( π2 − θT )
else
θ3 = −wrapToPI( π2 − θT )
end if
else
θ3 = atan2(−g422 , −g412 )
θ1 = atan2(−g422 , −g412 )
shift angles according to Fig. ??(right arm) or Fig.?? (left arm)
Create the decision table and apply the cost function to find the relevant Pose
end if
end if

11
4 Forward and Inverse kinematic for the left arm

4.1 Inverse kinematic joint position for the left arm

The DH representation for the left arm is given on Fig. 4. All the equations to process θ1 ...θ6 associated with the
right arms, which have been defined in Section 3 are still valid.

Figure 4: DH representation for the left arm

However, the shifting of angles for the left arm is different from the right arm and the values are given on Table 3.
This table is used to shift θ1 , θ2 and θ3 angles.

12
 θ1
Input -130 ... -95 ... -85 ... -20 ... 20 ... 85 ... 95 ... 130
Computed 140 ... 175 ... -175 ... -110 ... -70 ... 70 ... 5 ... 40
sin(θ2 ) < 0 -40 ... -5 ... 5 ... 70 ... 110 ... 175 ... -175 ... -140
Objective -220 ... -185 ... -175 ... -110 ... -70 ... -5 ... 5 ... 40
θ2
Input -130 ... -95 ... -85 ... -20 ... 20 ... 85 ... 95 ... 130
Computed -140 ... -175 ... 175 ... 110 ... 70 ... 5 ... -5 ... -40
Objective -40 ... -5 ... 5 ... 70 ... 110 ... 175 ... 185 ... 220
θ3
Input -130 ... -95 ... -85 ... -20 ... 20 ... 85 ... 95 ... 130
Computed 50 ... 85 ... 95 ... 160 ... -160 ... -95 ... -85 ... -50
sin(θ2 ) < 0 -130 ... -95 ... -85 ... -20 ... 20 ... 85 ... 95 ... 130
Objective -130 ... -95 ... -85 ... -110 ... 20 ... 85 ... 95 ... 130

Table 3: (Left arm) Angle values of θ1 and θ3 depend on the sign of sin(θ2 )

4.2 Singularities

4.2.1 Case 1: Elbow singularity or θ4 = 0

Equations which have been defined for the right arm in Section 3.1.1 are valid for the left arm. However the shifting
of angles, as expected, is different and is detailed on Table 4.
It is interesting to notice that the correct values for θT are computed when sin(θ1 ) < 0

θ1
Input -130 ... -95 ... -85 ... -20 ... 20 ... 85 ... 95 ... 130
Computed 140 ... 175 ... -175 ... -110 ... -70 ... 70 ... 5 ... 40
sin(θ2 ) < 0 -40 ... -5 ... 5 ... 70 ... 110 ... 175 ... -175 ... -140
Objective -220 ... -185 ... -175 ... -110 ... -70 ... -5 ... 5 ... 40
θ2
Input -130 ... -95 ... -85 ... -20 ... 20 ... 85 ... 95 ... 130
Computed -40 ... -5 ... 175 ... 110 ... 70 ... 5 ... -5 ... -40
theta1 < 0 140 ... 175 ... -5 ... -70 ... -110 ... -175 ... 5 ... 40
Objective -40 ... -5 ... 5 ... 70 ... 110 ... 175 ... 185 ... 220
θT
Computed 50 ... 85 ... 95 ... 160 ... -160 ... -95 ... -85 ... -50
sin(θ2 ) < 0 -130 ... -95 ... -85 ... -20 ... 20 ... 85 ... 95 ... 130
Objective -130 ... -95 ... -85 ... -20 ... 20 ... 85 ... 95 ... 130

Table 4: (Left arm) θ4 = 0: Angles are shifted according to both the sign of θ1 and the sign of sin(θ2 )

4.2.2 Case 2: Shoulder singularity or θ2 = 0

Equations which have been defined for the right arm in Section 3.1.2 are valid for the left arm. However the value
for θ3 is:
π
θ3 = −wrapToPI( − θT ) (42)
2

13
4.2.3 Case 3 Elbow-shoulder singularity or θ4 and θ2 = 0

Equations which have been defined for the right arm in Section 3.1.3 are valid for the left arm. However the value
for θ5 is:

π
θ5 = wrapToPI( − θT ) (43)
2

5 Forward and Inverse kinematic for the Torso

The five i−1 Ti transformation matrices for the Torso of the ODOI robot can be found on the basis of the coordinate
systems established in Fig.5. These i−1 Ti matrices are listed below.

Figure 5: Denavit Hartenberg parameters for the Torso

     
C1 0 S1 0 C2 0 −S2 0 C3 0 S3 0
0
 S1 0 −C1 0 1
 S2 0 C2 0 2
 S3 0 −C3 0 
T1 =   , T2 =   , T3 =  ,
0 1 0 0 0 −1 0 0 0 1 0 LT 1 
0 0 0 1 0 0 0 1 0 0 0 1
   
C4 0 S4 0 C5 −S5 0 LT 2 C5
3 S4 0 −C4 0 S5 C5 0 LT 2 S5 
, 4 T5 = 
 
T4 = 
0
 
1 0 0 0 0 1 0 
0 0 0 1 0 0 0 1

The i Ti−1 matrices that will be needed to compute the joints are listed below.
     
C1 S1 0 0 C2 −S2 0 0 C3 S3 0 0
1
0 0 1 0 2
 0 0 −1 0 3
0 0 0 −LT 1 
T0 = 
 S1 −C1
 , T1 = −S2 C2
 , T2 =  ,
0 0 0 0  S3 −C3 0 0 
0 0 0 1 0 0 0 1 0 0 0 1

14
  
C4 S4 0 0
4
 0 0 1 0
T3 =  
 S4 −C4 0 0
0 0 0 1

In order calculate the forward kinematics (FK), the five transformation matrices from each joint are pre-multiplied
to obtain the position and orientation of the end-effector relative to main frame:

5
Y
0 i−1
T5 = Ti = 0 T1 1 T2 2 T3 3 T4 4 T5 (44)
i=1

5.1 Inverse kinematics

The same method as for the right arm is applied. Let us write 0 T5 as:
   
0 x5 y5 z5 p5 n s a p
T5 = = (45)
0 0 0 1 0 0 0 1

where x5 , y5 , and z5 are the unit vectors along the principal axes of the hand frame and p5 is the position vector
describing the location of the hand relative to the shoulder. These three unit vectors describe the orientation of the
hand coordinate frame relative to the shoulder coordinate frame. The vectors n, s, a, and p represent the normal
vector, sliding vector, approach vector, and position vector of the hand, respectively [2].

 0  0 0 0 0 
0
0
n s a p n s a p
T5 = = = 5 T0 (46)
0 0 0 1 0 0 0 1

We use the inverse transform method to solve for the last two joint angles by multiplying both sides of Eq. (4) by
4
T5 . This results in an equation where the left side of the equation is:

0 0 0 0 0 0 0
C5 nx 0 − S5 ny
 
C5 sx − S5 sy C5 ax − S5 ay C5 px − S5 py + C5 LT 2
0 0 0 0
S5 nx 0 + C5 n0 0 0 0 0 0 0
 
(LHS) n s a p S5 sx + C5 sy S5 ax + C5 ay S5 px + C5 py + S5 LT 2 
G4−torso == 4 T5 = 0
y
0 0 0

0 0 0 1  nz sz az pz 
0 0 0 1
(47)
The right side of the equation is:
 
g411 g421 g431 −LT 1 S4
(RHS) 4 4 3 2 1
g412 g422 g432 0 
G4−torso = T0 = T3 T2 T1 T0 = 
g413
 (48)
g423 g433 LT 1 C4 
0 0 0 1

15
with:

g411 = C1 (C2 C3C4 − S2 S4 ) − S1 S3 C4

g412 = C1 C2 S3 + S1 C3
g413 = C1 (C2 C3C4 + S2 C4 ) + S1 S3 S4
g421 = S1 (C2 C3C4 − S2 S4 ) + C1 S3 C4
g422 = S1 C2 S3 − C1 C3
g433 = S1 (C2 C3 S4 + S2 C4 ) − C1 S3 S4
g431 = S2 C3 C4 + C2 S4
g432 = S2 S3
g433 = C3

We can solve for the last two (for the torso) joint angles by equating the position terms (last column) of Eq.(47)
and Eq.(48) to get:

0 0
C5 (px + LT 2 ) − S5 py = −LT 1 S4 (49)
0
pz = LT 1 C4 (50)
(51)

From Eq. (50) we can find the value of C4 and thus θ4 :

0
(p
C4 = z (52)
LT 1
q
θ4 = atan2(± 1 − C42 , C4 ) (53)

0 0
Let px + LT 2 = rCψ and py = rSψ , and substituting them into Eq. (49), we get:

rC5 Cψ − rS5 Sψ = −LT 1 S4 (54)

−LT 1 S4
C5ψ = (55)
r s
−LT 1 S4 2 −LT 1 S4
θ5 = wrapT oP I(atan2(± 1 − , ) − ψ) (56)
r r

q 0 0
where r = (p0x + LT 2 )2 + (p0y )2 and ψ = atan2(py , px + LT 2 )

For the solution of joint angles θ1 , θ2 and θ3 , multiplying both sides of Eq. (46) by 3 T4 and 4 T5 . This results in
an equation where the left side of the equation is:
 
g311 g321 g331 g341
 0 0 0 0 
(LHS) n s a p g312 g322 g332 g342 
G3−torso == 3 T4 4 T5 =
g313
 (57)
0 0 0 1 g323 g333 g343 
0 0 0 1

16
with:
0 0 0
g311 = nx C4 C5 − ny C4 S5 + nz S4
0 0 0
g312 = nx S4 C5 − ny S4 S5 − nz C4
0 0
g313 = nx S5 + ny C5
0 0 0
g321 = sx C5 C5 − sy C4 S5 + sz S4
0 0 0
g322 = sx SS C5 − sy S4 S5 − sz C4
0 0
g323 = sx S5 + sy C5
0 0 0
g331 = ax C4 C5 − ay C4 S5 + az S4
0 0 0
g332 = ax S4 C5 − ay S4 S5 − az C4
0 0
g333 = ax S5 + ay C5
0 0 0
g341 = px C4 C5 − py C4 S5 + pz S4 + LT 2 C4 C5
0 0 0
g342 = px S4 C5 − py S4 S5 − pz C4 + LT 2 S4C5
0 0
g343 = px S5 + py C5 + LT 2 S5

and the right side of the equation is:

 
C1 C2 C3 − S1 S3 C1 S3 + S1 C2 C3 S2 C3 0
(RHS) 3 2 1
 −C1 S2 −S1 S2 C2 −LT 1 
G3−torso = T2 T1 T0 =   (58)
S1 C3 + C1 C2 S3 S1 C2 S3 − C1 C3 S2 S3 0 
0 0 0 1

By comparing the element (2,3) of LHS and RHS of G3−torso , we can obtain C2 :

0 0 0
C2 = g332 = ax S4 C5 − ay S4 S5 − az C4 (59)
q
θ2 = atan2(± 1 − C22 , C2 ) (60)

By comparing the elements (1,3) and (3,3) of LHS and RHS of G3−torso , we obtain two equations. By dividing
these two equations, we can determine the joint solution θ3 :

S2 S3 g333
= (61)
S2 C3 g331
0 0
S3 ax S5 + ay C5
= 0 (62)
C3 ax C4 C5 − a0y C4 S5 + a0z S4
θ3 = atan2(g333 , g331 ) (63)

by comparing the elements (2,1) and (2,2) of LHS and RHS of G3−torso , we obtain two equations relating to θ1 . By
dividing these two equations, we can determine the joint solution θ1 :

−S1 S2 g322
= (64)
−C1 S2 g312
0 0 0
−S1 sx SS C5 − sy S4 S5 − sz C4
= 0 (65)
−C1 nx S4 C5 − n0y S4 S5 − n0z C4
θ3 = atan2(−g322 , −g312 ) (66)

17
In the case of the Torso, there is no need to shift angle values and there is no singularity as well.

5.2 Decision table and Metrics

The above equations give us 8 solutions, as it is shown in Fig. 6, each one define a Pose that brings the torso to
the right position and orientation .

Figure 6: Decision Table for the torso

The question is: how to select the right one? This is here that we can select the pose that will meet specific
constraints such as torque limitation or the arm must move along a specific geometric constraint for instance.
For our purpose, like for the arms, I choose to limit the variation of the angles between the previous pose and the
next one:

minimize cost(j)
j
5
X (67)
subject to cost(j) = (θicurrent − θij )2 .
i=1

where θicurrent represents the the current value of the ith joint and θij is the computed value of the ith joint in the
j th row in the decision table.

5.3 Algorithm

In order to compute all the joints the process is quite straightforward because on one hand there is no singularity
and on the other hand not need to shift angle:
• Apply the above equations;

18
 • Create the decision table;
• Apply a given metrics to choose the appropriate pose for the torso.

6 Inverse kinematic Joint Position solution for the legs

For information the DH representation of the right leg is pictured on Fig. 7. However, because of the fact that:
• The leg and thigh are not aligned (parameters L3 and D3);
• There is an additional segment (L5) between the rotation of the foot in the frontal plane (θ5 and the rotation
of the leg in the saggital plane (θ4 ).
The methodology we applied successfully for the Arms and the Torso leads us to complex equations which cannot
be solved. This is why for the leg a solution based on geometry is proposed.

Figure 7: DH parameters for the right leg

19
6.1 let us start with a simple observation

Let us start with an observation based on the mechanical structure of the leg and the thigh. As mentioned before,
the thigh is not aligned with the leg but it makes an angle which is referred to as α.
As it is shown on Fig. 8 if the thigh is rotating along the point K, the trajectory of the perpendicular projection
(represented by a blue dotted line) along the knee point of the point H, which is referred to as HP , is making a
circle, like H.
Moreover:
• The distance D between H and HP is constant and equal to LT sin(α);
• The rotation angle θ between H(θ1 ) and H(θ2 ) is the same as the one between HP (θ1 ) and HP (θ2 ).

Figure 8: Decision Table for the torso

6.2 Construction of the tangent plane - resolve θ6

The input is the position (given by v and orientation of the foot (nf~oot , sf~oot andaf~oot ), as the foot is articulated
(active articulation of the forefoot and passive articulation of the heel), we may have an angle between the ground
and the foot.
In order to compute θ6 , which is referred to as the lateral angle of the ankle, we will compute the plane, referred to
as P laneP which is (see Fig. 9.(A)):
• Tangent to a sphere whose radius is D (as defined in ??) and the center is one extremity of the Pelvic;
• which is passing through the foot articulation F .
In order to compute this plane, we need to compute the points of tangency between the sphere and P laneP The
math to compute these points is discussed in Appendix B.
Actually, two points are computed: T1 and T2 . In order to select to correct one, the dot products P~ • T~1 and P~ • T~2
are computed and the selection will be based on the sign value which must be positive (see Fig. 9.(C)).
Once the point T is known, we are able to compute the vector normal to P laneP , referred to as n~P :

n~P = F~T × nf~oot (68)

where × represents the cross product and nf~oot represents the orientation of the foot with respect of the ground -
remember that the foot is articulated!

20
 Figure 9: Computation of the tangent plane

Then we compute the intersection of P laneP and the plane which represents the orientation of the foot with respect
to the vertical. The intersection is a line which is defined by its vector director ~v .
Now we know all the information to compute θ6 (see Fig. 9.(B)):

~v • sf~oot
θ6 = arccos( (69)
kvkksfoot k)

6.3 Projection in the tangent plane: resolution of ankle θ5 and knee θ4 angles

In order to compute θ5 and θ4 , we compute the point (xH , yH ) which is the projection of the vector F~T on P laneP
(see Fig. 10):

xH = F~T • ~uyH = F~T • ~v (70)

where ~u = nf~oot .
Then we compute the intersection of two circles, one with a radius R which is equal to the length of the leg LL and
one with radius Ra which is equal to LT sin(α) (refer to 6.1.
~ × K1,
There are two solutions K1 and K2 and the sign of the cross product OH ~ 2 = xk yK1,2 − yk xK1,2 allow us to
select the correct point, referred to as K.
Once K is known we are able to compute the ankle (θ4 ) and knee (θ5 ) saggital angles:

21
 Figure 10: Computation of θ5 and θ4 .

D12 − R2 − Ra2
φ = arccos( )
−2RRa
θ4 = 180 − φ (71)
xK
θ5 = arctan( )
yK

This is here also where we take case of the singularity regarding the alignment of the leg and the thigh, i.e.θ5 = 0

6.4 And finally the last three angles θ1 , θ2 and θ3

Let us go back to the sphere and the tangent point. If we take a closer look at the sphere, as it is shown on Fig.
11, the tangent point allows us two to compute θ1 and θ2 .
However, we need to know T in the Pelvic frame, which is computed as follow:
 p  
Tx Tx
Typ  −1
Ty 
 p  = (Tpelvic )  
Tz  Tz 
1 1

Where Tpelvic is the transformation matrix associated with the Pelvic. Knowing T in the pelvic frame, θ1 and θ2
are computed as follow:

Tx
θ1 = arctan( )
Tz
Ty (72)
θ2 = arcsin( )
q r
r= Tx 2 + Ty 2 + Tz 2

Once θ1 and θ2 are known, it is possible to get θ3 . We have all the information to set the Thigh at a position where
θ3 = 0. This is done by applying ROTx and ROTy to the Pelvic transformation:

22
 Figure 11: Computation of θ1 and θ2 .

Tthigh (θ3 = 0) = Tpelvic ROTy (θ2 )ROTx (θ1 )

By providing this transformation to the thigh, we are able to know the position of the knee K0 when θ3 = 0 (and
K0 belongs to P laneP ).

~ • CK
CK ~ 0
θ6 = arccos( (73)
kCKkkCK0 k)

7 Put things together

In this Section, we will bring together the torso and the arms. Transformations related to the torso will be
represented by a T and transformations related to the arms with A
We define two new ”end points” for the torso, the right shoulder (rs) and the left shoulder (ls), these two end points
are computed based on the transformations below:
   
1 0 0 0 1 0 0 0
5
0 1 0 0  0 1 0 0 
TRS =  , 5 TLS = 
0 0 1 −LT 3  0 0 1 LT 3 
0 0 0 1 0 0 0 1

The right shoulder is defined by:

0
TRS = 0 T1 1 T2 2 T3 3 T4 4 T5 5 TRS (74)

23
 Figure 12: Decision Table for the torso

and the left shoulder by:

0
TLS = 0 T1 1 T2 2 T3 3 T4 4 T5 5 TLS (75)

In order to connect the arms, two other matrices are required: S TB which rotates the frame (XET , Y ET , ZET )
related to Torso and B T0a rm which rotates the frame (XB , YB , ZB ) to match the root frame (X0 , Y0 , Z0 ) or the arm.
   
0 0 1 0 0 0 1 0
RS 0 1 0 0 1 0 0 0
TB = LS TB =  , B T0 = 
 
 
−1 0 0 0 0 1 0 0
0 0 0 1 0 0 0 1

Finally, we defined two transformations :

0
A0rarm = 0T TRS RS TB B T0arm
0
(76)
A0larm = 0T TLS LS TB B T0arm

The arms will then be connected to the torso by pre-multiplying 0 A6rarm (as defined by Eq.2) by 0 T0rarm and
0
A6larm by 0 T0larm .

24
The complete 3D representation of the robot is displayed on Fig. 13

Figure 13: 3D representation of the robot once all the ”pieces” are assembled

The Inverse kinematics equations work if the objective is defined in the shoulder frame. As the objective is defined
in the main frame, we need to write the equation to compute it in the right/left shoulder frame. If we look at Fig.
12, we can see in red color an objective defined by its coordinates (XR0 , YR0 , ZR0 and its orientation (~nR0 , ~sR0 , ~aR0 )
in the main frame. If we consider the transformations which link the objective in the main frame to the right
shoulder we have:

Obj
TR0 = Obj TRS RS TR0 (77)

Therefore the transformation which brings the objective in the right shoulder frame is:

Obj
TRS = (Obj TR0 )−1RS TR0
(78)
Obj
TRS = (0 TRS rS TB B T0 )−1RS TR0

The objective in the Shoulder frame is defined as:.

   
nRS sRS aRS pRS Obj nR0 sR0 aR0 pR0
= TRS (79)
0 0 0 1 0 0 0 1

25
A Useful package to visualize DH-based representation of an articu-
lated body
I found one very useful package developed in Python by Rufus Fraanje from the Hague University of Applied
Sciences. It is straightforward to install and easy to use. It is based on Python-visual and can be downloaded from
github: https://github.com/prfraanje/python-robotics
The picture below gives you an example of how a manipulator is displayed.

Figure 14: 3D graphic display of a manipulator based on a DH-based representation

The code is pretty straighworward as one can see:

from links import ∗

scene = display( title =’UR 5’,center=(0,0,0),forward=(−1,−1,−1),up=(0,0,1),background=visual.color.background,range=5)

scene.fov = 0.01 # mimic orthographic projection

scale = 1;
F0 = frame ([0,0,0], label=’O’,scale=scale)

# Denavit−Hartenberg parameters of the UR5 robot, see e.g.

# http://www.universal−robots.com/how−tos−and−faqs/faq/ur−faq/actual−center−of−mass−for−robot−17264/
# note that the a’s and the d’s here are in meters, and the

26
# alpha’s and the theta’s are in degrees
# you may use radians as well, but then change the unit in links below to
# unit=’rad’
a1 = 0 # link length
alpha1 = 90 # link twist
d1 = .089159 # link offset ( controllable )
theta1 = 0 # joint angle ( controllable )
joint1 = ’r ’

a2 = −0.42500 # link length

alpha2 = 0 # link twist
d2 = 0 # link offset ( controllable )
theta2 = −90 # joint angle (controllable)
joint2 = ’r ’

a3 = −0.39225 # link length

alpha3 = 0 # link twist
d3 = 0.0 # link offset ( controllable )
theta3 = 0 # joint angle ( controllable )
joint3 = ’r ’

a4 = 0 # link length
alpha4 = 90 # link twist
d4 = 0.20915 # link offset ( controllable )
theta4 = −90 # joint angle (controllable)
joint4 = ’r ’

a5 = 0 # link length
alpha5 = −90 # link twist
d5 = −0.19465 # link offset ( controllable )
theta5 = 0 # joint angle ( controllable )
joint5 = ’r ’

a6 = 0 # link length
alpha6 = 0 # link twist
d6 = 0.1823 # link offset ( controllable )
theta6 = 0 # joint angle ( controllable )
joint6 = ’r ’

# these are the joint variables

# in these case they are all angles (in degrees)
# that come on top of the offsets defined by the
# theta angles above, so use q as the joint variable
# and theta just as the offset
q1 = 30
q2 = 30
q3 = 30
q4 = 90
q5 = 30
q6 = −30

scale = 0.5

L1 = link(F0,a=a1,alpha=alpha1,d=d1,theta=theta1,q=q1,joint=joint1,label=’L1’,scale=scale,unit=’deg’)
L2 = link(L1.frame,a=a2,alpha=alpha2,d=d2,theta=theta2,q=q2,joint=joint2,label=’L2’,scale=scale,unit=’deg’)
L3 = link(L2.frame,a=a3,alpha=alpha3,d=d3,theta=theta3,q=q3,joint=joint3,label=’L3’,scale=scale,unit=’deg’)
L4 = link(L3.frame,a=a4,alpha=alpha4,d=d4,theta=theta4,q=q4,joint=joint4,label=’L4’,scale=scale,unit=’deg’)
L5 = link(L4.frame,a=a5,alpha=alpha5,d=d5,theta=theta5,q=q5,joint=joint5,label=’L5’,scale=scale,unit=’deg’)
L6 = link(L5.frame,a=a6,alpha=alpha6,d=d6,theta=theta6,q=q6,joint=joint6,label=’L6’,scale=scale,unit=’deg’)

scene.camera(F0,forward=[−1,−1,−1],up=[0,0,1])

27
B Find a plan tangent to a sphere
So we want the plane that passes both P and Q and touches the sphere (radius r). Let that point, where the sphere
and the plane meet be X(α, β, γ).
Since X is on the sphere, it satisfies α2 + β 2 + γ 2 = r2 .
~ will be the normal vector to the tangent plane. The plane passes X so, the equation of the
Also, the vector OX
plane is
α(x − α) + β(y − β) + γ(z − γ) = 0
Thus,
αx + βy + γz = r2

This plane passes through P and Q, so plugging the values in gives us,

αx1 + βy1 + γz1 = r2

αx2 + βy2 + γz = r22

Overall, solve these three equations,

αx1 + βy1 + γz1 = r2 (80)

αx2 + βy2 + γz2 = r2 (81)

α2 + β 2 + γ 2 = r 2 (82)

to get the values of α, β, γ.

From Eq. (80) we have:

r2 − βy1 − γz1
α= (83)
x1

input α in Eq. (81):

x2 2
(r − βy1 − γz1 ) + βy2 + γz2 = r2
x1

thus
y1 x2 z1 x2 x2
β(y2 − ) − γ( − z2 ) = r2 (1 − )
x1 x1 x1
or

β = Aγ + B (84)
with: z 1 x2
x1 − z2
A= y1 x2
y2 − x1
r2 (1 − xx12 )
B=
y2 − yx1 x1 2

28
Reintroducing Eq. (84) in Eq. (83):
r2 − (Aγ + B)y1 − γz1
α=
x1
γ(−Ay1 − z1 ) + r2 − By1
α=
x1
α = Cγ + D (85)
with:
−Ay1 − z1
C=
x1
r2 − By1
D=
x1

Input Eqs (84) and (85) in Eq. (82):

(Cγ + D)2 + (Aγ + B2 + γ 2 = r2

γ 2 (C2 + A2 + 1) + γ(2CD + 2AB) + D2 + B2 − r2 = 0

by solving this second order equation we have two values for γ and we can compute two values for β with Eq. (84)
and two values for α with Eq. (85).

29
Contents
1 Introduction 1

2 Forward and Inverse kinematics for the Arm 2

2.1 Forward kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2

3 Inverse kinematic Joint Position solution for the Arm 3

3.1 Singularities . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
3.1.1 Case 1: Elbow singularity or θ4 = 0 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
3.1.2 Case 2: Shoulder singularity or θ2 = 0 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
3.1.3 Case 3 Elbow-shoulder singularity or θ4 and θ2 = 0 . . . . . . . . . . . . . . . . . . . . . . . 9
3.2 Decision table and Metric . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
3.3 Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

4 Forward and Inverse kinematic for the left arm 12

4.1 Inverse kinematic joint position for the left arm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
4.2 Singularities . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
4.2.1 Case 1: Elbow singularity or θ4 = 0 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
4.2.2 Case 2: Shoulder singularity or θ2 = 0 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
4.2.3 Case 3 Elbow-shoulder singularity or θ4 and θ2 = 0 . . . . . . . . . . . . . . . . . . . . . . . . 14

5 Forward and Inverse kinematic for the Torso 14

5.1 Inverse kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
5.2 Decision table and Metrics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
5.3 Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18

6 Inverse kinematic Joint Position solution for the legs 19

6.1 let us start with a simple observation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
6.2 Construction of the tangent plane - resolve θ6 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
6.3 Projection in the tangent plane: resolution of ankle θ5 and knee θ4 angles . . . . . . . . . . . . . . . 21
6.4 And finally the last three angles θ1 , θ2 and θ3 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

7 Put things together 23

A Useful package to visualize DH-based representation of an articulated body 26

B Find a plan tangent to a sphere 28

References 31

30
 References
[1] Muhammad A Ali, H Andy Park, and CS George Lee. Closed-form inverse kinematic joint solution for humanoid
robots. In Intelligent Robots and Systems (IROS), 2010 IEEE/RSJ International Conference on, pages 704–709.
IEEE, 2010.
[2] King Sun Fu, Ralph Gonzalez, and CS George Lee. Robotics: Control Sensing. Vis. Tata McGraw-Hill Education,
1988.
[3] Richard S Hartenberg and Jacques Denavit. A kinematic notation for lower pair mechanisms based on matrices.
Journal of applied mechanics, 77(2):215–221, 1955.
[4] Rowland O’Flaherty, Pete Vieira, M.X. Grey, Paul Oh, Aaron Bobick, Magnus Egerstedt, and Mike Stilman.
Kinematics and inverse kinematics for the humanoid robot hubo2+. Technical Report GT-GOLEM-2013-001,
Georgia Institute of Technology, Atlanta, GA, 2013.
[5] Hyungju Andy Park, Muhammad Ahmad Ali, and CS George Lee. Closed-form inverse kinematic position
solution for humanoid robots. International Journal of Humanoid Robotics, 9(03):1250022, 2012.
[6] DL Pieper. The kinematics of manipulators under computer control. aim 72, 1968.

31

View publication stats