Review

• Pay attention to basic terminologies and concepts,

basic formulas, and basic skills.
• Understand the examples and homework
• Reading materials: book, lecture notes and homework

1
Chapter 2 Review
• Reading pp. 19-49 (Third Ed.)
• Basic concepts: Coordinate system, vector, orientation, frame,
graphical representation, mapping, operator, homogeneous
transformation, compound and inverse transformation,
transformation equations.
• Basic formulas: Xˆ  Yˆ  Zˆ Ŷ  Ẑ  X̂ Ẑ  X̂  Ŷ R(2.3), T(2.9)

X̂B  X̂ A ŶB  X̂ A ẐB  X̂ A  A

BR
A
PBORG
  BT  
A
A A A A   
B R  X̂B ŶB ẐB   X̂B ŶA ŶB ŶA ẐB ŶA 
 X̂B  ẐA ŶB  ẐA ẐB  ẐA  0 0 0 1 
 ABR |BART APBORG
 A B
B RC R | BA R BPCORG  APBORG   
A A B   BAT 1BAT       |    
CT  T
B CT        |  
 0 0 0 | 
0 0 0| 1 
 1   
• Should be able to establish and solve transformation equation. 2
 Review
Chapter 3
• Readings: pp. 62-83,89-91 (Third Ed.)
• Basic concepts: link parameters, assign frame to links
(origin, X, Z ) forward kinematics, home position
• How to define ( xi-1 // xi ) i  0?
• Basic formula (3.1)-(3.6) and (3.8).
• Should be able to sign the frames correctly and obtain 0N T
with different methods: projection and compound operation.

3
 Review
Chapter 4
• Readings: pp. 101-117, 125-127 (Third Ed.)
• Basic concepts: what is inverse kinematics?
Workspace , subspace how to determine them?
• Understand how to use algebraic and geometric
approaches. Pay attention to geometric approach.
• Basic skills on how to obtain the equation and solve
them.

4
Coordinate System

Xˆ  Yˆ  Zˆ
Ŷ  Ẑ  X̂
Ẑ  X̂  Ŷ
5 5
 Description of a Position
{A} point = position vector
ZA  px 
P   py 
AP
A
 
 pz 
YA

XA
How to obtain {px, py, pz}?
Projection
6
 Orientation Description
In order to describe the orientation of a
Ẑ A
body, we will attach a coordinate system Ẑ B
to the body.

The coordinate system {B} has been

X̂ B A
P YˆB
attached to the body in a known way.
YˆA
X̂ A

Thus, if we can describe the relationship between {B} and {A}, we

can determine the orientation of the body with respect to the {A}.
7
 Orientation Description
• A frame is made up by three-unit vectors.
• The projection of a frame to its reference frame is equal to project
three vectors as a group to its reference frame.
• Stack three-unit vectors to form Rotation Matrix
A
B R   A Xˆ B A
YˆB A
Zˆ B 
Ẑ A Ẑ B
 r11 r12 r13 
 r21 r22 r23 
X̂ B
A
P YˆB
?  r31 r32 r33 
r31  X̂ B  Ẑ A
Xˆ B  Xˆ A YˆB  Xˆ A ZˆB  Xˆ A  r21  X̂ B  Ŷ A
  YˆA
  Xˆ B YˆA YˆB YˆA ˆ ˆ
ZB YA 
ˆ ˆ ˆ ˆ  r11  X̂ B  X̂ A
ˆ ˆ
 XB  ZA YB  ZA ZB  ZA 
X̂ A
cosine matrix 8 8
 Rotation matrix about principal axes
 Xˆ B  Xˆ A YˆB  Xˆ A Zˆ B  Xˆ A 
A  ˆ ˆ 
B R   X B  YA YˆB  YˆA Zˆ B  YˆA 
 Xˆ B  Zˆ A YˆB  Zˆ A Zˆ B  Zˆ A 

A B T
B R  A R
A B A B 1
B R A R  I  B R  A R
1 T 1
B
A R T
 B
A R ( R A
B  A
B R )

1 0 0   cos 0 sin   cos  sin  0

 0 0  Rz ( )   sin  cos 0
Rx ( )  0 cos   sin   R y ( ) 

1
  
   0 0 1 
0 sin  cos     sin  0 cos 

9
 Description of a Frame
The information needed to completely specify where is the
manipulator hand is.
Ẑ A
• Frame: set of four vectors
Ẑ B
giving position + orientation
X̂ B
A
• Description of a frame: P YˆB
position + rotation matrix
Ex.: YˆA
{B}  A

B
A
R, PBORG 
• Position: frame with identity as rotation X̂ A
• Orientation: frame with zero position
10
 Description of a Frame
A graphical representation of frames is convenient in
visualizing frames.
This vector represents Ẑ A
Ẑ B
the position of the origin
at the head of the arrow
in terms of the frame at X̂ B
the tail of the arrow. A
PBORG YˆB
The direction of this locating
arrow tell us, that {B} is YˆA
known relative to {A} and {B} {BA R, APBORG }
not vice versa.
X̂ A
11
Graphical Representation
{A} {Au R,uPAORG}
{B} {Bu R,uPBORG} {BAR, APBORG}
{B} {Bu R,uPBORG}
Ẑ A Ẑ B
{B} { R, PBORG}
A
B
A
{B}

Ẑ u u X̂ B
PBORG A
PBORG YˆB
{U} Yˆu
u
YˆA
PAORG {A}
X̂ u
X̂ A
12
Homogeneous Transform-Summary of Interpretation
• As a general tool to represent a frame we have introduced
the homogeneous transformation, a 4×4 matrix containing
orientation and position information.
 A
BR PBORG
A
A
BT   
0 0 0 1 
• Three interpretations of the homogeneous transformation :
1. It is a description of a frame.
2. It is a transform mapping.
3. It is a transform operator.
R Q A
A
P2 T P1 
A
 P1
0 1  13
Homogeneous Transform Interpretations
 A
BR PBORG
A

BT  
A

0 0 0 1 
Description of a frame
A
BT : {B}  BA R PBORG 
Transform mapping
A
BT : BP  AP A
P  BA T BP

Transform Operator
T : P1  P2 P2  TP1
14
Example A
P  BA R B P  APBORG
B
P

A
P
B
P A
P
=?
A A B A  BAR A
PBORG 
A
PBORG 
P  BT P BT   
 0 1 
( Using projection )
1 0 0 0 0  A
  A Xˆ B Aˆ A
Zˆ B 
0 BR 
YB

Homogeneous
 P0 1 3  1 
P  0 0
A

Transform BT 
A B
1 
BORG

0 1 0 1 1  0  0  1
       
0 0 0 1 1  0  
 2   0  1   0 
A
P  ABT B P  B
P 
2
  15
1 
Transformation Arithmetic –Compound Transformations
Given: Vector c P Frame {C} is known relative to frame {B}  CB T
Frame {B} is known relative to frame {A}  AB T
Calculate: Vector A P
A
P  ABT BP B
P
B
P  CB T CP ?
A
P ABT BP ABT CBT CP
A A B
C T  BT CT

 CAR | APCORG  BA RCBR | BA R BPCORG  APBORG 

A    
CT       |    
        |    
0 0 0| 1   0 0 0 | 1 
  
16
Transformation Arithmetic –Inverse Transformations
Given: Description of frame {B} relative to frame {A}  ABT  ( BAR, APBORG )
Calculate: Description of frame {A} relative to frame {B}  BT  ( B R , B P
A A AORG )
Homogeneous Transform ?
{B}
 ABR | BPAORG
B
AT  
     |   

 
0 0 0| 1 
 
B
B PAORG
PAORG

B
PAORG  ?
{B}

B
PAORG APBORG ??
 A T
R  A T A
BR PBORG 
B
PAORG  R A
B
T A
PBORG   AB R A PBORG AT  
B B

0 0 0 1  17
Inverse Transform

18
Transform Equation

U U D 1 D
C T  AT AT CT

U U B
C T  BT CT

U
AT ?
U U B D 1 D
A T  BT CT CT AT

19
Transform Equation (second approach)

T=?

20
Example
Given BTT , BST and GS T
T
GT ?

B
GT  BST GST

B B T
G T  T T GT

B T B S
T T GT  ST GT
T B 1 B S
G T  TT ST GT
21
 Basic formula
• Know how and when to use those Basic formulas:
Xˆ  Yˆ  Zˆ Yˆ  Zˆ  Xˆ Ẑ  X̂  Ŷ

X̂B  X̂ A ŶB  X̂ A ẐB  X̂ A  A

BR
A
PBORG
BT  
A
A
A
B R  X̂B
A
ŶB A
 
ẐB   X̂B ŶA ŶB ŶA ẐB ŶA 
 
 X̂B  ẐA ŶB  ẐA ẐB  ẐA  0 0 0 1 

 BA RCBR | BA R BPCORG  APBORG   ABR |BART APBORG

A A B   A 1 B  
CT  T
B CT         |     B T  AT       |    
 0 0 0 | 1  0 0 0| 1 
   

• Should be able to establish and solve transformation equation 22

 Assignment

0 0 T
1R 1R  I
120 0
T
Xˆ1  Xˆ 0 Yˆ1  Xˆ 0 Zˆ1  Xˆ 0 Xˆ1  Xˆ 0 Yˆ1  Xˆ 0 Zˆ1  Xˆ 0 
   30 0
 Xˆ1 Yˆ0 Yˆ1 Yˆ0 Zˆ1 Yˆ0  Xˆ1 Yˆ0 Yˆ1 Yˆ0 Zˆ1 Yˆ0   I
ˆ ˆ  ˆ ˆ 
 X1  Z0 Yˆ1  Zˆ0 Zˆ1  Zˆ0  X1  Z0 Yˆ1  Zˆ0 Zˆ1  Zˆ0  600

r112  r212  r312  r212  r222  r232  r312  r322  r332  1

T r11r21  r12 r22  r13 r23  0
r11 r12 r13 r11 r12 r13  1 0 0 
   0 1 0 r11r31  r12 r32  r13 r33  0
r r r r r
 21 22 23 21 22 23 r  I    r21r31  r22 r32  r23 r33  0
r31 r32 r33
r31 r32 r33  0 0 1 
23
Check your answer by drawing the frames accordingly
ˆ ˆ Yˆ1  Xˆ 0 Zˆ1  Xˆ 0 
r11 r12 r13  X1  X0
    0
ˆ ˆ Y1 Y0 Z1 Y0    Xˆ1
ˆ ˆ ˆ ˆ Yˆ1 Zˆ1 
1 R  r21 r22 r23    X1 Y0
0 0 0

r31 r32 r33   Xˆ  Zˆ Yˆ1  Zˆ0 Zˆ1  Zˆ0 


 1 0

Inverse the projection

Xˆ 0  Xˆ1 Yˆ0  Xˆ1 Zˆ0  Xˆ1 

 
   Xˆ 0 Yˆ1 Yˆ0 Yˆ1 Zˆ0 Yˆ1   1Xˆ 0 Yˆ0 Zˆ0 
1 1 T 1 1
0 R 0 R
ˆ ˆ 
 X0  Z1 Yˆ0  Zˆ1 Zˆ0  Zˆ1 

There should be only one that is correct among all the possible solutions.
Assignment

 BAR A
PBORG
A
BT  
{3}
0 0 0 1 

{2} Hint Using projection to obtain the Rotation matrices.

{1}

27
Assignment
3. Referring to Fig. 3, a) draw the graphical representations and
obtain the expressions of CAT , CAT and ABT , accordingly; b) calculate
the values of CAT , CAT and ABT , respectively.

Fig. 3 Frames at the corners of a wedge.

 Assignment
4. Derive equation (2.82). Describe an algorithm that extracts the equivalent
angle and axis of a rotation matrix. Equation (2.82) is a good start, but
make sure that your algorithm handles the special case 𝜃 0 and θ  180 .
0

 k x k x vθ  cθ k x k y vθ  k z sθ k x k z vθ  k y sθ 
 
Rk (θ )   k x k y vθ  k z sθ k y k y vθ  cθ k y k z vθ  k x sθ  (2.80)
z
 k x k z vθ  k y sθ k y k z vθ  k x sθ k z k z vθ  cθ 


Hint: The inverse

algorithm consists of
three sub-algorithms
descript using piece a) If θ =00 , let θ =00 , calcualte (2.80) and solving for A K  [ K x K y K z ]
wise function as: b) If θ =1800 , let θ =1800 , calcualte (2.80) and solving for A K  [ K x K y K z ]
c) Otherwise, using (2.82) 33
 Review
Chapter 3
• Readings: pp. 62-83,89-91 (Third Ed.)
• Basic concepts: link parameters, assign frame to links
(origin, X, Z ) forward kinematics, home position
• How to define ( xi-1 // xi ) i  0?
• Basic formula (3.1)-(3.6) and (3.8).

• Should be able to sign the frames correctly, and obtain 0N T

with different methods, projection and compound operation.

37
 Central Topic
• Problem
–Given: The manipulator ?
geometrical parameters
–Specify: The position and
orientation of manipulator
• Solution

– Coordinate system or “Frames”

are attached to the manipulator
and objects in the environment
following the Denenvit-Hartenberg
notation. 38
 Joint/Link Description
Links
• A robotic manipulator may be thought of
as a set of bodies connected in chain by joints. Higher pair ?

• These bodies are called links. (point

Linkand
Joints
line contact)
• Joints for a connection between a neighboring
pair of links.

39
 Joint Description
There are only two types of joints are used in robotics due to the limitation
of the existing actuator which is normally one DOF only.

Prismatic Joint
Revolute Joint

40
 Joint/Link Description
There exists a well-defined measure of minimum
distance between the two lines in 3D-space.
?
Line 2
This distance is measured along a line which
Line 1
is mutually perpendicular to both axes.
Also there exists a plane which is
perpendicular to the distance.
αi-1
On this plane, it should be an angle
between the projections of those two axes.

Those two parameters can be used to uniquely

define the two lines in space or two axes in space.
41
 Link Length and Twist
• Joint Axis - A line in space (or a Axis i
vector direction) about which link i
• Link Length –
rotates relative to link i-1
– The distance
Axis i-1
i 1
between axis i and
axis i-1
Notes
– Sign ai 1 ≥ 0
• Link Twist –
–The angle  i 1
measured from • Note : Sign i1 by right hand rule
axis i-1 to axis i (counter-clockwise is positive, clockwise is negative)
i-1 42
 Joint Variables (Denevit-Hartenberg) –Angle & Offset
Link Offset – di
• 

– The signed distance measured

along the axis of joint i from the
point where ai1 intersects the axis to
the point where ai intersects the
axis.
• The link offset di is variable if joint i
is prismatic
• Sign of di ?
• Joint Angle – i
– The signed angle made between an
extension of ai1 and ai measured about
the axis of the joint i
• Note:
– The joint angle i is variable if the joint i is revolute
• Sign -  i Right hand rule 43
Denavit-Hartenberg Parameters
Axis i

Axis i-1 Link i+1

i
di

Link i

i-1 44
Affixing Frames to-Intermediate Links in the Chain
• Origin of Frame {i}
– The origin of frame {i} is located
where the ai perpendicular intersects
the joint i axis ai
Yˆi 1  Zˆi 1  Xˆ i 1
• Z Axis
– The Ẑ i axis of frame {i} is coincident
with the joint axis i
• X Axis Frame {i-1}

– The X̂ i axis points along the distance ai

in the direction from joint i to joint i+1
• Y Axis
– The Yˆi axis completes frame {i} following the right hand rule
Yˆi  Zˆi  Xˆ i 45
 Link Frame Attachment Procedure- Summary
1. Identify the joint axes and imagine (or draw) infinite lines along them. For
step 2 through step 5 below, consider two of these neighboring lines (at axes i
and i+1 )
2. Identify the common perpendicular between them or point of intersection. At
the point of intersection, or at the point where the common perpendicular
meets the ith axis, assign the link frame origin.
3. Assign the Zˆ i axis pointing along the ith joint axis.
4. Assign the Xˆ i axis pointing along the common perpendicular, or if two joint
axes intersect, assign Xˆ i to be normal to the plane containing the two
axes ( Ẑi and Ẑi 1 ) (the intersection will be the frame origin).
5. Assign Yˆi the axis to the complete a right-hand coordinate system.
( Yˆ i  Ẑ i  X̂ i )
6. Assign {0} to match {1} when the first joint veritable is zero. For {N}, choose
an origin location and Xˆ n direction freely, but generally so as to cause as
many linkage parameters as possible to be zero. 46
 DH Parameters - Summary
If the link frame have been attached to the links
according to our convention, the following definitions
of the DH parameters are valid:
ai  the distance from Ẑ i to Ẑ i 1measured along X̂ i ;
 i  the angle from Ẑ i to Ẑ i 1measured along X̂ i ;
d i  the distance from X̂ i 1 to X̂ i measured along Ẑ i ;
 i  the angle from X̂ i 1 to X̂ i measured along Ẑ i .
Furthermore, θi is the angle between Xˆ i 1 to Xˆ i measured along
Zˆ , when θ  0, Xˆ // Xˆ . Hence, the determine the direction of
i i i 1 i

Xˆ i also depends on the Xˆ i 1.

• Note: ai  0 , and  i ,d i ,and  i are signed quatities. 47

 Intersecting Joint Axes
– For ai  0, Xˆ i is normal to the plane of Zˆi 1 and Zˆi .

Zˆi 1 the link frame origin

Zˆi 1
Xˆ i Zˆi
Zˆi
The sense of αi is determined
by the diretion of Xˆ .
i

Xˆ i

Since θi is the angle between Xˆ i1 to Xˆ i measured along Zˆi ; when θi  0,

Xˆ i1 // Xˆ i . Hence, the determine the direction of Xˆ i also depends on the Xˆ i1.

Only one choice Chose Xˆ i such that when Xˆ i1 // Xˆ i  θi  0. 48

 Derivation Homogeneous Transformation
Solution for i 1iT
– The problem is further broken into 4 sub problems such that each
of the transformations will be a function of one link parameter only
• Define three intermediate frames:
–{R} , {Q}, and {P}
Then , we have
{i-1} {R} {Q} {P} {i}
– Frame {R} is different from {i-1} only by a ?
rotation of  i 1
– Frame {Q} is different from {R} only by a
translation ai 1
– Frame {P} is different from {Q} only by a
rotation  i
– Frame {i} is different from {P} only by a By mapping or operator ?
translation di 49
 Derivation Homogeneous Transformation
Solution: A vector define in frame {i} is expressed in {i-1} as
follows: i 1 i 1 i
P  iT P
i
P
𝑃 𝑇 𝑇 𝑇 𝑇 𝑃

The transformation from frame

{i-1} to {i} is defined as follows

𝑇 𝑇 𝑇 𝑇 𝑇

𝑇 𝑅 𝛼i 1 𝐷 𝑎 i 1 𝑅 𝜃 𝐷 𝑑
By mapping
50
 Derivation of link Homogeneous Transformation

1 0 0 0  1 0 0 a i 1   c  i  s i 0 0  1 0 0 0
0 c  s i 1 0 0 1 0 0   s i c i 0 0 0 1 0 0
i 1  i 1    
iT 
 0 s i 1 c  i 1 0 0 0 1 0  0 0 1 0 0 0 1 di 
0 0 0 1  0 0 0 1   0 0 0 1  0 0 0 1 

 c i  s i 0 ai 1 
 s c  c i c i 1  s i 1  s i 1d i  i 1  i 1i R i 1i P
i 1
iT 
 i i 1 
iT  
 s i s i 1 c i s i 1 c i 1 c i 1d i 
 0 0 0 1   0 1  51
 
DH Parameters – RPR –Example

T  01T 21T 23T

0
3 52
HW Assignment No. 2
1. The arm with three degrees of freedom shown in Fig.
3.29 is like the one in Example 3.3,except that joint 1’s
axis is not parallel to the other two. Instead, there is a
twist of 90 degrees in magnitude between axes 1 and 2.
Derive link parameters and the kinematics.

53
HW
2. Figure 3.33 shows the schematic of a wrist which has three
intersecting axes that are not orthogonal. Assign link frames to this
wrist (as if it were a 3-DOF manipulator) and give the link
parameters.

3. Show the attachment of link frame for the 5-DOF manipulator

shown schematically in Fig. 3.34 and give the link parameters.

Origin of the frame

Why?

Origin of the frame

55
 HW
6. Consider a RRR robot with two possible frame assignments as shown
in Figure 3.13, obtain the two values of 30 Tθ θ θ  0 and 03T 0.
1 2 3 θ1  θ3  0,θ2 90

 30 R 0
P3ORG 
T 
0
3 
0 1 

Figure 3.13 Two possible frame assignments.

Hint: Draw the configuration of frame {3} first and then using projection to
obtain rotation matrix and positon of frame {3} origin in frame {1}.
60
PUMA 560 – DH Parameters

Using projection to calculate

 60 R 0
P6ORG 
0
6T (0, 0, 0, 0, 0, 0)   ?
0 1 
 Ẑ 4

Do it without using matrix

Multiplication.


65
 Review
Chapter 4
• Readings: pp. 101-117, 125-127 (Third Ed.)
• Basic concepts: what is inverse kinematics? Workspace ,
subspace how to determine them?
• Understand how to use algebraic and geometric
approaches.
• Pay attention to geometric approach.
• Basic skills on how to obtain the equation and solve them.

66
 Direct vs. Inverse Kinematics
• Direct (Forward) Kinematics
– Given: Joint angles and links geometry
?
– Compute: Position and orientation of the
end effector relative to the base frame
• Inverse Kinematics
– Given: Position and orientation of the end
effector relative to the base frame
– Compute: All possible sets of joint angles
which could be used to attain the given
position and orientation of the end effector.
67
Solvability

68
 Algebraic vs. geometric
•We will consider two different approaches to the solution
of simple planar three-link manipulator.
-algebraic approach, we start solving the kinematic

equations based on the forward kinematic transformation.

In other words, the equation to be solved is taken from
the 12 possible equations of the general equations of :
r11 r12 r13 px 
r r r p 
0
T ( , ) B
T   21 22 23 y
N 1 N W
r31 r32 r33 pz 
 
 0 0 0 1 
•Of course, all the possible solutions must lie within the
subspace of the robot. 69
 Algebraic vs. geometric
-Geometric approach, instead of solving the equations from the transformation,
we try to decompose the spatial geometry of the arm into several 2-D plane
geometry problems.
• For many robots when or , this can be done quite easily. Joint angles can
then be solved for using the tools of plane geometry.
-The difference between the approaches is mainly in the way of obtaining
the equations of kinematics.
• Even though the two approaches are different, there are
common techniques in solving those equations.
• Furthermore, both approaches require the physical and geometric
intuitions about what type of equations can be solved, and how to
solve it.
70
 Inverse Kinematics - Planar (3R) -Algebraic Solution
• Given:
- Direct Kinematics: The homogenous transformation from
the base to the wrist.
- Goal Point Definition: For a planar manipulator, specifying
the goal can be accomplished by specifying three parameters.
The position of the wrist in space (x,y), and ( ) the orientation
of link 3 in the plane relative to the X̂ 0 axis

Ŷ0
y
What are the joint angles 
(1 ,  2 , 3 ) as a function of L2
the wrist position and
orientation ( x, y,  ) ? L1
71
x X̂ 0
Inverse Kinematics - Planar (3R) -Algebraic Solution

Subspace:
 c  s 0 x
s c 0 y 
B
T   
W
 0 0 1 0
 
 0 0 0 1

72
Inverse Kinematics - Planar (3R) -Algebraic Solution

0
3T T T T
0
1
1
2
2
3
c123 s123 0 l1c1  l2c12 
s c123 0 l1s1  l2s12 
  123

0 0 1 0 
 
0 0 0 1 

73
Inverse Kinematics - Planar (3R) -Algebraic Solution
• Problem:
What are the joint angles (1 , 2 , 3 ) as a function of the
wrist position and orientation ( x, y,  ) ?
• Solution:
The goal in terms of position and orientation of the wrist in terms
of the homogeneous transformation is defined as follows
 s c123  s123 0 l1c1  l2c12
c 0 x
s
s 0 l1s1  l2s12 
y   c123
c 0 WT 3T 
B 0 123
B
T   0 0 1 0 
W Goal
0 0 1 0  
  0 0 0 1 
0 0 0 1
74
Two-argument arc tangent function: Atan2
In terms of the standard arctan (y/x) function, whose
range is (−π/2, π/2), it can be expressed as follows:
 y
 arctan( ) x  0 (I,V)
x

arctan( y )  π y  0, x  0 (II)
 x Ŷ

arctan( y )  π y  0, x  0 (III)
atan2( y, x)   x
 X̂
 
π
y  0, x  0 ?
 2
 π
  y , x  0
 2
 undefined y  0, x  0 75
Inverse Kinematics – Planar (3R) -Geometric Solution
• Solution 1 , 2 , 3  ?
• Identify joint variables in the 2D plane 2
• We can apply the law of cosine to solve for θ2 y
x 2  y 2  l12  l22  2l1l2 cos(  2 ). L2
L1
• Since 2
cos(    2 )   cos( 2 ),
• We have
x 2  y 2  l 21 l 22 x
c2  . (  1  c 2  1)
2l1l 2
•The other possible solution (the one indicated by the dashed-line
triangle) is found by symmetry to be  2'   2 .

s2   1  c22 , θ2  Atan2(s2 , c2 ). 76
Inverse Kinematics - Planar(3R) -Geometric Solution
• Defining β as a function of x,y as
θ3 𝜙
β  Atan2( y , x ) y
L2
• Defining ψ as a function of x,y and L1
applying low of cosine as: ψ
θ2

x 2  y 2 l 21l 22
cos  . ( 1  cos  1)
2 2
2l1 x  y β x
θ1
•Then we have
θ2<0
1    θ3  𝜙 (θ1 θ2)

0 θ2>0
•Note: 0    180
77
Inverse Kinematics - Planar(3R) -Geometric Solution
Solutions: θ3 𝜙
θ2<0 y
1    β  Atan2( y , x ) L2

θ2>0 L1
2 2
θ2
x y l 21l 22 ψ
cos  . ( 1  cos  1)
2 2
2l1 x  y
θ2  Atan2(s2 , c2 ) β x
θ1
x 2  y 2  l 21 l 22
s2   1  c , c 2 
2
2 . (  1  c 2  1)
2l1l 2

θ3  𝜙 (θ1 θ2)
78
Inverse Kinematics – 3D (RRR) Geometric Solution
-Example • Given
- Manipulator Geometry
- Goal Point Definition:
The position xd, yd, zd, of the writs in space

• Problem:
What are the joint angles (θ1,θ2,θ3) as a function of the
goal (xd, yd, zd, )? 79
Inverse Kinematics – 3D (RRR) Geometric Solution
-Example
Step 1: Assign frame to each joint
Step 2: Identify joint variables
Step 3: Project the 3D onto 2D plane top
(along the different z axis) view

Step 4: Solve 2D geometric problems

80
 Link Frame Attachment Procedure- Summary
1. Identify the joint axes and imagine (or draw) infinite lines along them.
For step 2 through step 5 below, consider two of these neighboring lines
(at axes i and i+1 )
2. Identify the common perpendicular between them, or point of
intersection. At the point of intersection, or at the point where the
common perpendicular meets the ith axis, assign the link frame origin.
3. Assign the Zˆ i axis pointing along the ith joint axis.
4. Assign the Xˆ i axis pointing along the common perpendicular, or if two
axes intersect, assign Xˆ i to be normal to the plane containing the two
axes ( Ẑi and Ẑi 1 ) (the intersection will be the frame origin).
5. Assign Yˆi the axis to the complete a right hand coordinate system.
( Yˆ i  Ẑ i  X̂ i )
6. Assign {0} to match {1} when the first joint veritable is zero. For {N}, choose
an origin location and Xˆ n direction freely, but generally so as to cause as
many linkage parameters as possible to be zero. 81
 Intersecting Joint Axes
– For ai  0, Xˆ i is normal to the plane of Zˆi 1 and Zˆi .

Zˆi 1 the link frame origin

Zˆi 1
Xˆ i Zˆi
Zˆi
The sense of αi is determined
by the diretion of Xˆ .
i

Xˆ i

Since θi is the angle between Xˆ i1 to Xˆ i measured along Zˆi ; when θi  0,

Xˆ i1 // Xˆ i . Hence, the determine the direction of Xˆ i also depends on the Xˆ i1.

Only one choice Chose Xˆ i such that when Xˆ i1 // Xˆ i  θi  0. 82

Inverse Kinematics – 3D (RRR) Geometric Solution
-Example

Top view
top
(along Z1)
view

Step 3: Project the 3D onto 2D plane

(along the different z axis)

83
Inverse Kinematics – 3D (RRR) Geometric Solution
-Example
• The planar geometry-top view of the robot
• Identify joint variables in the 2D plane
√ r1
yd
θ1  Atan2( yd , xd ). X 3 L ’4
L 3 Z3
Y0 X2 θ1
r1  x y
2
d
2
d Z2
X0 xd
√

84
Inverse Kinematics –3D (RRR) Geometric Solution
-Example
• Projection 3D to 2D continued:

• Side view of the robot

(along Z2 and Z3 axes)
85
Inverse Kinematics –3D (RRR) Geometric Solution
-Example
• The planar geometry-side view of the robot
Zˆ ,
• Identify joint variables in the 2D plane 0 1

-θ3
r1
θ2
yd r2 ẑ
X3 L 4 ?
top view L3 Z3 r1 X̂ 1
X2 L1+L2 zd
Y0 θ1
xd
Z 2 X0

r2  r12  ẑ2  xd2  yd2  ẑ2 2 xd2  yd2  (zd  (L1  L2 ))2
where ẑ  zd  (L1  L2 ) 86
Inverse Kinematics – 3D (RRR) Geometric Solution
-Example
• By applying the law of cosines, we get
r22  L 23  L 24 2 L3 L4 cos(1800  θ3 )  L 23  L 24 2 L3 L4 cos(θ3 )
• Rearranging gives Zˆ 0 ,1
?
r 22  L 23  L 24 -θ3
c3  . ( 1  c3  1)
2 L3 L4 θ2
and s3   1  c32
r2 ẑ
X̂ 1
r1
• Solving for  3 we get L1+L2 zd
 3  Atan2(  1  c32 ,c3 )
where c3 is defined above in terms of known parameters
L3 , L4 , x d , y d , and z d . 87
Inverse Kinematics – 3D (RRR) Geometric Solution
-Example
• Finally solve for θ2 Zˆ 0 ,1

θ3
2     L2
L4
β
  Atan2( zˆ , r1 ) θ2
 ẑ
X̂ 1

• where r1

r1  xd2  yd2 zˆ  zd  (L1  L2 )

88
Inverse Kinematics – 3D (RRR) Geometric Solution
-Example
• Based on the law of cosines we can solve for β
Zˆ 0 ,1 L24  r22  L23  2r2 L3cos(β )
θ3
L4 r22  L23  L24
L2 cβ 
β r2 2r2 L3
θ2  ẑ
X̂ 1 β  Atan2(  1  c β2 , c β )
L1+L2 zd
r1 2    

2  Atan2(zd  ( L1  L2 ), xd2  yd2 )  Atan2( 1  c2 , c )

89
 ( xd , yd , z d )

Trying geometric approach (top and side views)

Understanding what you can see from the top view and side view. 90
First, to sketch graphically the
set of the all-possible tip
locations (workspace), then find
the analytical expression of the
set.
91
  0 p4ORG  x 
 
0
P4ORG   0 p4ORG  y   ?
θ3 900 ,θ1 θ2 θ4  0
 0 p4ORG  z 
 

Trying geometric approach: find the suitable 2D plane.

(hint: know where is the initial position and where is the
final positon by using the definitions of link parameters) 92