ROBOTICS

SPRING SEMESTER 2016

 HANYANG
HANYANG
UNIVERSITY
UNIVERSITY

Part I

Evolution of Robotics Research

HANYANG
Classification of Robot Applications

Care Educational and

Robots Pet Robots

For the disabled, For children,

elderly, teachers,
and housekeepers and the elderly

Personal
Service
Robots
Robots Professional For Surgery
Service
Robotics
Heavy Payload Robots 로봇 Robots For the treatment
(more than 1,000 kg) Categories and elimination of
medical problems at
For wider application a macro/micro level
of robots in various Industrial
heavy industries Army
Robots
Robots
Heavy Density Multi-
Robots (slim robots) For saving soldiers
in dangerous
For multitasking situations
and improved
efficiency

HANYANG
Robotics Research

Robotics Research
• Major conferences
ICRA (international conference on robotics and automation)
started in 1984.
IROS (international conference on robots and systems)
started in 1988.
• Major journals
IEEE Transaction on Robotics
International Journal of Robotics Research
IEEE Robotics and Automation Letter

• How has the “Robotics” been evolved?

 Analysis through the theme and trend change of papers pub-
lished in ICRA for last 20 years.
HANYANG
Research Areas

Research Areas
Fundamental topics Special topics
Control Medical Robot
Kinematics/dynamics Space Robot
Motion Planning Mobile Robot
Sensor, sensing, vision Underwater Robot
Design Aerial Robot
Actuator Rehabilitation Robot
Manufacturing Micro Robot
Software Humanoid Robot
intelligence Haptic Interface
Human Robot interaction Biomimetic Robot
Network Robot
Other Applications

HANYANG
Paper distribution for 20 years
(%) 1989 1990 1991 19921993 1994 1995 1996 1997 1998 1999 2000 2001 2002 2003 2004 2005 2006 2007 2008 2009
control 25 29 25 23 22 16 14 23 23 13 18 11 14 7 17 6 12 15 8 8 6
kin/dyn 27 30 17 20 16 24 25 22 18 17 25 22 11 16 13 13 9 15 7 14 12
Motion P 22 15 14 10 12 16 16 17 8 13 14 16 22 16 15 16 21 16 16 14 18

Sensing 10 11 10 14 10 10 15 11 10 10 11 13 10 14 12 13 14 17 17 19 15
design 1 1 9 4 6 2 4 6 8 7 2 2 3 4 5 8 10 6 7 7 2
actuator 4 1 1 1 2 1 2 2 3 2 2 2 3 1 3 3 2 2 2 2

Manuf 6 12 13 13 13 6 6 11 12 7 11 9 9 12 13 4 3 4 1 5

software 4 2 1 5 3 6 5 6 3 2 3 1 3 2 1 6 3
intell 3 4 3 6 2 4 1 2 1 5 3 5 4 3 1 3 8 2
HR inter 1 1 1 1 1 2 1 2 3 3 3 4 7 5
Appli. 4 1 5 3 3 2 5 1 6 3 3 3 6 7 6 5 4 3 6 3 7
Med. R 1 2 1 1 2 2 2 2 3 3 3 2 4 5
Space R 2 1 1 2 2 1 1 2 2 1 2 2 1 1 1 1

Mobile R 4 6 3 2 3 3 3 5 3 3 2 4 5 1 2 5 4

Under R 1 1 1 1 3 1 2 1 1 1 2 1 2 1

Aerial R 1 2 1 2 2 3 3 2

Rehab R 1 1 1 1 1 2 1 2 2 2
Mic R 3 1 1 1 1 2 4 1 2 2 2 2 1 2
Hu-
manoid
1 1 3 1 3 2 3 3 2 2 4 4
Haptic 1 2 1 3 1 3 5 4 3 3 2 1 3 3
Biom R 1 1 1 3 3 6
Network
R
2

HANYANG
Change of distribution
30 30
25 25
20 20
15
1989 15
1994
10 10
5 5
0 0

tro
l P gn uf te
ll li. R rR R id R ro
l P n uf ll li. R rR R id R
tio
n si an in
p
ac
e
de
b o m t
tio
n sig an te p
ac
e
de
b o m
on de M Ap ha an io on de in Ap ha an
Bi
o
c o Sp Un Re m B c o M Sp Un Re m
M 25 Hu M Hu

Basic topics 20 Special topics

15 2000
10

0
ro
l
n
P
ig
n uf el
l
p li. eR rR R oi
d R
t
tio es an in
t
ac de
b m
on d M Ap eh
a an
Bi
o
18 c
M
o Sp
20 Un R u m
H
16 18
14 16
14
12
2004 12 2009
10
10
8
8
6 6
4 4
2 2
0 0
ro
l
n
P
ig
n uf el
l
p li. R r R R id R ro
l
n
P
ig
n uf el
l
p li. eR rR R oi
d R
t
tio es an in
t
ac
e
de
b o
om
t
tio es an t
ac de
b m
on d M Ap ha an
Bi on in Ap eh
a an
Bi
o
c o Sp Un Re m c o d M Sp Un R m
M Hu M Hu

HANYANG
Analysis of four major basic topics
35

30

25

20

control
15
kin/dyn
10 Motion P

5 Sensing

0
89 91 93 95 97 99 01 03 05 07 09
19 19 19 19 19 19 20 20 20 20 20

• Control issue is pretty matured

 Intelligence/learning becomes important
• Mechanics/mechanism issue is pretty matured
 Application-dependent issue is important
• Motion planning and sensor/sensing/vision are associated with
autonomous robotic systems
HANYANG
Analysis of other special topics

8 7

7 6
6
5
5 Appli. Rehab R
Med. R 4 Mic R
4
Space R Humanoid
3
3 Mobile R Haptic
Under R 2 Biom R
2 Aerial R Network R
1 1

0 0
89 91 93 95 97 99 01 03 05 07 09 89 91 93 95 97 99 01 03 05 07 09
19 19 19 19 19 19 20 20 20 20 20 19 19 19 19 19 19 20 20 20 20 20

• Variety of special issues appear after late 90th

 Medical robot and Haptics is so hot research topics.
 Mobile robot gets started around mid 90th
 Humanoid started in 1998
 Biomimetic Robot is popular recently
 Network Robot : start to appear in 2009

HANYANG
Conclusions

• Control and mechanics issues did not die

• Application dependent research should be pursued
Application dependent control method,
Application dependent mechanism design,
Application dependent motion planning,
Sensor and actuator should be task-specific
 Modeling issue is important for new applications
• Society wants interdisciplinary researcher or engineer
SI (system integration) is important
Integration of component technology
(ex. Modeling, design, sensor, control, network,
and software)
 How should we educate graduate students?

HANYANG
Status of Robot Patents

11,674 Controls
12,000 Actuators
Sensors
10,000
HRI

8,000 Mechanisms
Controls 4,128
Mechanisms 8,711
24.5% 6,000
51.7% 7.4%
8.9% 7.5% Actuators 2,407
4,000 1,497
1,248 1,265
Sensors
HRI 2,000
1,255
1,501
0
US JP EU KR

Mechanisms make up 51.7% of all Japan has 69.3% of all robot patents
robot patents

HANYANG
 Major Patent Holders

Major International Companies Major Korean Companies

16 Matsushita 100
Hyundai Heavy
Industries
80

Sales ( ￦ billion)
12
Sales ($ billion)

60
8

Fanuc 40 Robotstar

4
ABB Dasa Robot
Yaskawa 20
iRobot Yujin Robot
Micro Robot

0 100 200 300 400 500 600 700 0 10 20 30 40 50 60 70 80

Number of Patents Number of Patents
Source: 2008 annual reports of respective companies

There is a positive correlation between market sales and the number of patents.

HANYANG
 Robot Modeling
 Kinematic Modeling
• Kinematics in position level
- Forward kinematics (FK) : u  f  
- Inverse kinematics (IK):   f  1 u 
x L1 cos 1  L2 cos 1   2 
FK y L1 sin 1  L2 sin 1   2 
L2 2
y
x 2  y 2  L12  L22 x  L1  L2 cos  2   yL2 sin  2
L1 cos  2  cos 1 
L1  L2 cos  2   L22 sin 2  2
2
2 L1 L2
1
x sin  2  1  cos  2 y  L1  L2 cos  2   xL2 sin  2
IK sin 1 
 2 a tan 2 sin  2 , cos 2  L1  L2 cos  2   L22 sin 2  2
2

1 a tan 2 sin 1 , cos 1 

13
 Modeling
 1st-order Forward Kinematics
d
dt
u  f   u  J 

u Duality f
u  J   J T f
 
u
J : (Jacobian)


14
 Modeling
 2nd-order Forward Kinematics
d
dt
u u  J   T
u J     H 


J
[H ]  : Hessian


15
 Modeling
 Modeling
• Dynamics
- Inverse dynamics F Ma
- Forward dynamics a M  1 F  
 v  
 x
 [ I* ]  T [ P
*
]

u
1 2
1 2

16
 Modeling
 Forward Dynamics

1

2

 ] (   T [ P
* 1
[ I *
] )  
   

17

Simulation of Ro-
bot Mechanisms
Using DAFUL
Software

2009. 9. 2
Professor
Byung-Ju Yi
 Content
 Procedure of Robot Design
 Need of Simulation
 Application Examples
• TV Mounting device
• Omni directional Mobile Robot
• Pipeline Inspection Robot
• Soft Finger
• Soft Robot
• Biomimetic Foot Mechanism
• Biomimetic Walking Mechanism
• 6 DOF parallel Mechanism
 Conclusion
 Procedure of Robot Design
I
 Analysis of Robot Task – DOF, Workspace, Precision, Payload
 Kinematic Modeling
 Kinematics/Dynamics Based Simulation(Virtual Mockup)
 Analysis
• Structural analysis
• Motor Sizing
• Assessment of Dynamic Performance before Production
• Optimal Parameterization
 Development of 3D Model
 Final Assessment through simulator
 Implementation of Prototype Robot
Procedure of Robot Design
II
Task Spec. P1
B1

Kinematic yt
 Y

model P2
zt
 xt
Z
X

P3 B2

Simulator B3

Simulator
Optimal Kinema
design tic
Model
3D Model

Prototype
Prototype 3D
Drawing
 Need of Simulator
 In Robot design, use as a Digital Mock-up.
• Easy robot modeling using a convenient user interface
• Possible to design various type robot structure
• Possible to estimate the performance before production

 Side effects
• Minimize development time and reduce the design cost
• Easiness of design
• Easiness in optimal parameterization and motor sizing
• Provide drawing for robot production
 How to Use Simulator
Impor
Solidwor t Add-on Solve Simulato
NX DAFUL
ks r

- 3D model
program - Set up joints - Motion
- Easy 3D - Exchangible - Set up contacts generation
modeler with - Set up screw - Force or torque
other 3D models - Position/
model
- Provide 2D - Trajectory velocity/
of
drawing joints acceleration
for production

2D 2D Produce
Drawing Drawing robot
 DAFUL Application I
 TV Mounting Device
• Purpose: Plat TV turning device
• Mechanism :
- 3-chains (2-RPS, 1-PSP)
- Joint: 9-R, 4-P, 3-S
- Link : 10
- Motor: 3
• Purpose of simulation
- Check workspace
- Check actuator force
• Simulation time : 2 minutes
• Merits
- DAFUL Based Dynamic Simulation, Motor Sizing
 DAFUL Application I
 Motion generation by DAFUL

Extension Swivel
motion motion

Tilting
motion
 DAFUL Application I
 Comparison of Motor Torques
0
Extension Motion
-50
Swivle motion

-200

-400 -100

-600
Actuator force(N)

Actuator force(N)
-150
-800

-1000

Chain 1 -200
-1200
Chain 2
Chain 3 Chain 1
-1400
DAFULChain 1 Chain 2
-250 Chain 3
DAFULChain 2
-1600
DAFULChain 3 DAFULChain 1
DAFULChain 2
-1800
0 5 10 15 20 25 DAFULChain 3
-300
Time(sec) 0 5 10 15 20 25

Extension Tilting Motion

Time(sec)

motion
-50
Chain 1
Chain 2
Swivel
motion
-100 Chain 3
DAFULChain 1
DAFULChain 2
-150 DAFULChain 3
Actuator force(N)

-200

-250

-300

-350
0 5 10 15 20 25
Tilting
motion
Time(sec)
 DAFUL Application II
 Omni directional Mobile Robot
• Purpose : Utilization of kinematic redundancy for
obstacle avoidance
• Mechanism
- 3-chains (3-RRRR)
- Joint: 12-R
- Link: 10
- Motor: 9
• Purpose of simulation
- Analysis of joint motion
• Simulation time: 10 min
 DAFUL Application II
 Motion generation by DAFUL

 Collision Analysis in
Uneven Terrain
 DAFUL Application III
 Pipeline Inspection Robot
• Mechanism
- 3-chains (four-bar)
- Joint: 48-R, 6-P, 3-springs
- Link: 16
- Motor : 3
• Purpose of Simulation
- Motor sizing and stiffness sizing
• Simulation time : 10 min.
• Merits
- Possible to develop complex environment
- Easiness in parameter optimization
 DAFUL Application III
 Motion generation by DAFUL

 Motion for variable pipe  Motion under gravity

diameter load
 DAFUL Application IV
 Soft Finger
• Purpose: Interaction task between human and robot
• Mechanism
- Backbone :1-spring
- Joint: 3-Pseudo R
- Link: 1-Spring, 3-Cylinders
- Motor: 3
• Purpose of Simulation
- Motion analysis of flexible
finger and motor sizing
• Simulation time: 20 min.
• Merits
- Possible to simulate flexible body
 DAFUL Application IV
1
2
 Motion generation by DAFUL 3

 Motion and stress

analysis due to
pulling wire number 1

 Motion and stress

analysis due to
pulling wire number 2
and 3
 DAFUL Application V
 Soft Arm
• Purpose: Gesture generation for Human-Robot Interaction
• Mechanism
- Body: spring
- Cylinder: 5
- Driving wire: 4
- Motor: 2
• Purpose of simulation
- Estimation of driving force
and motion, motor sizing
• Simulation time: 20 min.
• Merits
- Possible to simulate flexible body
 DAFUL Application V
 Motion generation by DAFUL

 Motion and stress  Motion and stress

analysis for a analysis for a
large mass under small mass under
gravity gravity
 DAFUL Application V
 Motion generation by DAFUL

 Motion and stress  Motion and stress

analysis analysis
for a planar for a vertical
displacement displacement
 DAFUL Application VI
 Biomimetic Foot
• Purpose: stable walking
• Mechanism
- 5-chain (5-bar)
- Joint: 10-R, 5-spring
- Link : 11
• Purpose of simulation
- Estimate the adaptable
characteristic of foot
• Simulation time: 10 min.
• Merits
- Simulate flexible body
- Possible to analyze impulse
 DAFUL Application VI
 Motion generation by DAFUL

Biomimet
ic

Planar

 Landing on  Landing on
uneven terrain simply
 DAFUL Application VI
 Comparison of Impulse

 Biomimetic  Planar foot

foot
 DAFUL Application VII
 Biomimetic Walking Robot
• Purpose: Stable Walking
• Mechanism
- 4-chains (4-bar)
- Joint: 30-R, 2-P
- Link: 12
- Motor: 1
• Simulation Purpose
- Walking pattern analysis

• Simulation time: 15 min.

• Merits
- Possible to develop various environment
- Possible to analyze the walking pattern
 DAFUL Application VII
 Motion generation by DAFUL

Stress analysis on  Walking at

the rear foot uneven terrain
 DAFUL Application VIII
 6DOF Parallel Robot
• Purpose: Needle insertion
• Mechanism
- 6-chains (UPS)
- Joint: 6-P, 6-S, 6-H
- Link: Ring, 2-plate
- Motor: 6
• Purpose of simulation
- Check the RCM characteristic and workspace
• Simulation time: 3 min.
• Merits
- Possible to estimate the workspace and the 3D motion
 DAFUL Application VIII
 Motion generation by DAFUL

 RCM (Remote Center of  Needle Insertion

Motion) Task
 Conclusion
 Good points of using DAFUL
• technically
- Easy to develop 3D robot model
- Easy to analyze the performance of the robot
- Easy to check the feasibility as a usable robot mechanism
• economically
- Reduce the design expense by using digital mock-up
- Minimize the production time
• educationally
- Easy to understand the physics of robot mechanisms
- Enhancement of modeling technique
- Enhancement of robot design technique
 What to do in the future?
• Develop subroutines for robot application
• Use as a control engine
 1.2 Fundamentals
 Position and rotation matrix
Reference coordinate system - Earth coordinate system
Y

Moving rotating coordinate

x
y

44
 Fundamentals
 Position
 ^
ZA X A : unit vector in X A direction
^
A Y A : unit vector in YA direction
 A p
^
Z A : unit vector in Z A direction
py 
0 YA ^ ^ ^
X A 1, Y A 1, Z A 1
px


XA
 px 
Position vector expressed A p  p   p X^  p Y^  p Z^
in A coordination system  y x A y A z A

 pz 
45
 Fundamentals
 Orientation
Not required for particle motion, but indispensable for rigid
body motion 


ZA
ZB


YB


0
YA

  
X B , Y B , Z B : unit vectors of B coordinate system

XA 
XB
46
 Fundamentals
 Note : vector dot (inner) product
 a1     b1   
a   a1 X A  a2 Y A , b   b1 X A  b2Y A
 a2   b2 
a b  a b cos
   
=(a1 X A  a2 Y A )(b1 X A  b2 Y A )
       
=a1b1 X A  X A  a1b2 X A Y A  a2b1Y A  X A  a 2b2Y A Y A
=a1b1  a2b2
b


a

47
 Fundamentals
 Definition of unit vectors
 
A
XB : XB Expressed in A coordinate system
 
A
YB : YB Expressed in A coordinate system
 
A
ZB : ZB Expressed in A coordinate system

ZA A 
XB


0 YA


XA 48
 Fundamentals
 Procedure
• Initially, A and B coordinate systems are coincident.
• B coordinate system rotates with respect to A coordinate
system.

X B can be expressed with respect to A coordinate system
A     

X B cos  x X A  cos  x Y A  cos x Z A 

ZB
ZA

A     

Y B cos  y X A  cos  y Y A  cos y Z A YB


0 YA
A    
Z B cos  z X A  cos  z Y A  cos z Z A

XA
49

XB
 Fundamentals
 Rotation matrix A
B R
rotation of B coordinate system with respect to A coordinate sys-
tem

      

 BX  X A Y B  X A Z B  X A

A        
B R  X
 B A Y Y B  Y A Z B  Y A 
       
 X B Z A Y B Z A Z B Z A 
 
A A

A


= XB YB ZB
 

50
 Fundamentals
 
A
Difference between X B and XB
  

X
 B  X A
 1 
    
 0  ,
A Same magnitude, but expressed
XB XB  X B Y A  in different coordinates
 0     
 X B Z A 
 
<Proof>
 
A
X B 1 X B  cos 2  x  cos 2  x  cos 2 x 1

  
A A A
 Y B  Z B  X B 1
51
 Fundamentals
 Note : Inverse Rotation matrix
B B  B

B


A R  X A YA ZA
 
      
       
 A T
 X A X B Y A X B Z A X B   X B X A X B Y A X B Z A   X B
                  
A T
=  X A Y B Y A Y B Z A Y B   Y B  X A Y B Y A Y B Z A   Y B   BA RT
                  
 X A Z B Y A Z B Z A Z B   Z B  X A Z B Y A Z B Z A   AZT 
     B 
 
A T
 XB
     
1 0 0

  Z B  =  0 0 
B A A T A A A
A R B R  Y B  XB YB 1
    0
 A T   0 1 
 ZB 
 
52
 AB R  BA R  1  BA RT
 Fundamentals
 
X B projected onto A X A projected onto B


 YA YB 

YB YA


Reverse
XB
order

 
XB
XA 

XA

 cos   sin  0  cos(  )  sin(   ) 0 

  sin(  ) cos(   ) 0   A RT
0 
A 1
B R  sin 
A
cos  B R    B
 0 0 1   0 0 1 
53
 Fundamentals
 Frame : 
frame B :  B  BA R, p BORG
A


ZB

 B
ZA A
p p

YB
A
p BORG

YA 
XB

XA A A B
p  p BORG  p (cannot add vectors with different coords)

Then, what should we do ?

B
p has to be expressed in A coordinate system followed by summation
54
 Fundamentals
 Note : vector dot product

 a1   b1  T
a   b   a (a1, a2 )
 a2   b2 
 b1 
a b  a1 a2    a1b1  a2b2 a b
T

 b2 

55
 Fundamentals
 Mapping
  
B
p  px X B  p y Y B  p z Z B
 
B B
p gX A
: component of p along the direction of X A

p Xˆ
B
= A cos 
    B
( p x X B  p y Y B  p z Z B )gX A
p
     
 px X B gX A  p y Y B gX A  p z Z B gX A

B B

B

T B Xˆ A
 pg X A  X A p B
p cos 
   
B B T B B B T B
Similarly, p gY A  Y A p, p gZ A  Z A p

56
 Fundamentals
  
A B T B B T B B T B
pB  X A p YA p ZA p

 T 

B ZA
 X A 
pB
ZB
   
B T B B
 YA A
p R
B p YB

 
 0 YA
 B
Z A 
T
 
  XA 
XB

ZA A
pB


0 YA

 57
XA
 Fundamentals
 Derivation of rotation matrix
B  T
  X A
Ax
pB    
A   B T B A A

A 
B B
p B  Ay
pB   Y A  p = XB XB X B p  BAR p
     

Az
pB   B ZT 
 A  
  ZB

 B
A A B p
Finally, p  p BORG  p ZA A
p 
YB
A B
 p BORG  BA R p A
p BORG

YA 
XB

XA
58
 Fundamentals
 Example
 3/2  1/ 2 0  1  3 / 2
A
    
p  BA R B p A
B R  1/ 2 3 / 2 0   0   1/ 2 
 0 0 1   0   0 
 


 1 YB YA
B
p  0 
 0 


B
XB
p 1

  
6 XA
59
 Fundamentals
 Examples of mapping
1. Translation (planar motion)
  x A  xB  m,  x A   1 0 m   xB 
YA YB
y A  yB  n,  y   0 1 n   y 
yA yB  A    b

z A zB  1   0 0 1   1 
n XB
O xB
 xA  1 0 0 m  xB 
  y A  0 1 0 n   y B 
O m xA XA   
 zA  0 0 1 l   zB 
     
 1  0 0 0 1  1 
60
 Fundamentals
2. Rotation
Yˆ A
Yˆ B
yA

 Xˆ B
xB
yB


Xˆ A
Zˆ B  Zˆ A ⊙ xA

 x A  xB cos   yB sin  
 
y
 A  x B sin   y B cos  
 z  z 
 A B  61
 Fundamentals

Mapping for rotation about ZA axis by 
 x A   cos   sin  0 0  xB 
 y   sin  cos  0 0  y  A
R
 A    B B
 zA   0 0 1 0  zB 
     
 1  0 0 0 1  1

 xA    
 xB    
 y   A X A
YB A   A A A
Z B   y B  xB X B  y B Y B  z B Z B
 A   B
 
 z A   zB 
 cos     sin    0
 xB  sin    y B  cos    z B  0 
 0   0   1 

62
 Fundamentals
 Problem 1
Drive mapping which represents rotation at Xˆ A axis and YˆA
axis, respectively.
Draw picture to derive the relations.

 cos  0 sin  0 1 0 0 0
 0 0   0 cos 
1 0   sin  0 
Rot about Yˆ A  , Rot about X A 
  sin  0 cos  0  0 sin  cos  0
   
 0 0 0 1 0 0 0 1

63
 Fundamentals
 Mapping involving general frames
B A B
p p  A pBORG  p
Zˆ A Zˆ B
A B
 p BORG  BA R p
A
p Xˆ B

Yˆ B
A  A p
p BORG Define A
P  
Yˆ A  1 

B
 B
p
  P  
Xˆ A    1 
A A
R p
P  BORG  B
A B B
P  BAT P
 
 
 0 0 0 1 
where ABT  R 44 : Homogeneous Transformation
64
 Fundamentals
 Operators

 1 0 0 a
 0 1 0 0
Trans ( Xˆ A , a )   translate by a along the x  direction
 0 0 1 0
 
 0 0 0 1 
 cos   sin  0 0
 sin  cos  0 0  
Rot ( Zˆ A ,  )   Rotate about Z A by 
 0 0 1 0
 
 0 0 0 1

65
 Fundamentals
 Relative transformation
Transformation always happens with respect to the new
(moving) coordinate system.
Initially B coordinate system lies on A coordinate sys-
tem.
- Trans ( Xˆ A , a ) happens x A xB  a
Yˆ A Yˆ B y A  yB
A B
z A zB
yB p

 xA   xB   1 0 0 a   xB 
y   y   0 1 0 0   yB  (1)
a xB  A  Trans ( Xˆ A , a)  B    
 zA   zB   0 0 1 0   zB 
Xˆ B  1      
⊙  
ZA Z B Xˆ A
   1  0 0 0 1  1 
66
 Fundamentals
 Relative transformation
- Next, perform Rot ( Zˆ A ,  )
 xB   cos  sin  0 0   xB   xB 
Yˆ A Yˆ B  y   sin  cos 0 0   yB  y 
Yˆ B

 B      Rot ( Z B , )  B 
B
B  zB   0 0 1 0   zB   zB 
p       
1  0 0 0 1  1  1
yB
Xˆ B
xB Substitute this into (1)
 Xˆ B
 
Z B , Z B Xˆ A
 xA   xB 
y   y 
  Trans ( Xˆ A , a ) Rot ( Z B , )  B  BA T B p
A

 zA   zB 
  Left to right  
 1  1
67
 Fundamentals
 Absolute transformation
Transformation always happens with respect to the fixed
(global) coordinate system. For the same initial condition
- Rot( Zˆ A ,  )

Yˆ A Y A
Yˆ B
B B
p  xA   xB 
 y  y 
 A  Rot ( Zˆ A , )  B 
yB Xˆ B
 zA   zB 
   

xB 1
   1

 
Xˆ A  X A Substitute this into (1)
Z A , Z A
68
 Fundamentals
 Absolute transformation
- Next, Trans ( Xˆ A , a)
 xA   xA 
Yˆ A Y Yˆ A

B
y    y 
  Trans ( X A , a )  A  (1)
A

B  zA   zA 
{B} p    
{A}
1
  1
yB 
XB
 xA   xB 
xB y    y 
  A  Trans ( X A , a ) Rot ( Z A , )  B 
a Xˆ A XA  zA   zB 
  Right to left  
1
  1
69
 Fundamentals
 Conclusion
The results of Absolute & Relative Transformations are the same
if the transformation order is reversed

Problem 2
 
A Trans (Y ,2), B Rot ( Z ,60), C Trans ( Xˆ ,2)
ⓐ Relative Transformation according to A→B→C order
ⓑ Absolute Transformation according to C→B→A order
Solve it by drawing and show that they produce the same result.

70
 Fundamentals
 Transformation Arithmatics
Zˆ C {C}
Yˆ C
Zˆ B {B}
Zˆ A
A
{A}
p
C
A
p BORG
B
p CORG p

Yˆ B
Yˆ A
Xˆ C
Xˆ A Xˆ B
A A B C
p  p BORG  p CORG  p
A B A C
p CORG  BA R p CORG , p C  BA R CB R p
A A B C
p  p BORG  BA R p CORG  BA R CB R p
71
 Fundamentals
First, Homogeneous Transformation between A and B

A
 B
A
R
A
p  A  A
p  B  B
p
BORG B
P   P, P   and P  
 000 1   1   1 
second, Homogeneous Transformation between B and C

B
 B
CR
B
p CORG  C C  C p
P   P, P  
 000 1   1 
A A B
Since p CORG  p BORG  BA R p CORG ,
B B B C
p C  p CORG  R p C

72
 Fundamentals

A
 A
BR
A
p BORG   CB R
B
p CORG 
P   
 0 1   0 1 
 BA R CB R A
B R p CORG  p BORG  C
B A

  P
 0 1 
Zˆ C {C Yˆ C
Zˆ B {B }
Zˆ A
A
p
{A } C
A B
p
A
 R A
C
A
p CORG  }
p BORG p CORG

CT   Yˆ B
 0 1  Yˆ A
Xˆ C
Xˆ Xˆ B
where A

A A B A
C R  BA R CB R and p CORG  BAR p CORG  p BORG

73
 Fundamentals
 Inverse BA T Zˆ B {B}

Zˆ A
{A} A
p BORG
Yˆ B
 A pBORG Xˆ B
Yˆ A

A
 A
BR
A
p BORG 
Xˆ A BT  
 0 1 

B
 B
A R
B
p AORG   BA RT  BA RT p BORG 
A

AT    
 0 1   0 1 
74
 Fundamentals
 Transform equation
U A
T
A DT UBT C
B
T C
D T
U
U
known : A T , UBT , CBT , CDT
0
A
A unknown : DT

A 1
B D Pre-multiplyingDT
C to both sides gives
A
D T  UAT  1 UBT CBT CDT
A an identity matrix.
DT
Control is performed in such a way to make

75
 Fundamentals
 Rotation matrix (review)
●
A
B R consists of 9 elements.
expressed as a 3×3 matrix

● Linear algebraically : it is called “proper orthogo-

nal matrix”.
① All columns are mutually orthogonal.
A
② All columns have
B R unit magnitude.
③ Determinant of is 1.
Note : Text book error
orthogonal → incorrect
orthonormal → correct

76

X B can be expressed with respect to A coordinate system
A    
X B cos  x X A  cos  x Y A  cos x Z A
A    
Y B cos  y X A  cos  y Y A  cos y Z A
A    
Z B cos  z X A  cos  z Y A  cos z Z A
Nine angles to define rotation matrix

77
 Fundamentals
 Rotation matrix
Let    

R  X Y Z
 
  
X Y 0 X 1
  
The X Z 0 Y 1 Six constraint equations
n,   
Y Z 0 Z 1

● R has nine components and they have 6 relatives.

● Therefore, only 3 independent elements exists!
- 3 rotational parameters
(1) Roll, Pitch, Yaw angle (Absolute transformation)
(2) Three Euler angle (Relative transformation)
78
 Fundamentals
 Kinds of rotation matrix
XY Z Y Z X Z XY
XY X Y ZY Z X Z
X ZY Y X Z ZY X
X Z X Y XY ZY Z

X  Y  Z implies Rot ( X ,*) Rot (Y ,*) Rot ( Z ,*)

12 Kinds for both Relative and Absolute transforma-

tion.
(Refer to Appendix of Craig’s)
79
 Fundamentals
(1) X-Y-Z fixed angles (roll, pitch, yaw)
≡ Absolute Transformation : reverse order of relative transformation
Initially, A coordinate system and B coordinate system are coincident.
Process : rot ⓐ Xˆ Aby  followed by 
ZA
rot ⓐ Yˆ A by β followed by 
Z B'
rot ⓐ Zˆ A by α


 YB'

Rot ( Xˆ A , )  
YA



XA 
XB
'
80
 Fundamentals


ZA ZA

 ZB 
Z B YB



Z B

Z B YB 
Y B

Y B


YA Yˆ A

Xˆ A

XA  Xˆ B 
X A
Rot (YˆA ,  ) Rot ( Zˆ A ,  )
XB

81
 Fundamentals
• X-Y-Z Absolute transformation

A
B R RZ A ( ) RYA (  ) RX A ( )
 cos   sin  0   cos  0 sin    1 0 0 
 sin  cos  0   0 1 0   0 cos   sin  
 0 0 1    sin  0 cos    0 sin  cos  

 C C C S  S  S C C S  C  S S 
 
 S C S S  S  C C S S  C  C S  (1)
  S C S C C 
 

82
 Fundamentals
 Inverse problem
 r11 r12 r13 
A
Given B RXYZ  r21 r22 r23 
 
 r31 r32 r33 
Find  ,  , by comparing this with (1)

r11 c c , r31  s , r33 c c

r112  r212 cos 
r21 s c , r32 c s

tan  should not be used in robotics since it does not

exactly tell the configuration of robots.

83
 Fundamentals
Instead, we use arctan2( )
arctan 2(  ) arc tan 2(sin  ,cos  )

 a tan 2  r31 , r112  r212 
a tan 2  r , 
31 r112  r212 
Two-  solutions exist!!!
 r21 r11 
 a tan 2  ,  a tan 2 sin  ,cos  
 c c 
 r32 r33 
 a tan 2  ,  a tan 2 sin ,cos  
 c c 
Therefore, there are “Two solution sets!!!”
84
 Fundamentals
(2) Z-Y-X Euler angles : Relative Transformation
Process : rot ⓐ Z axis by  followed by

rot ⓐ moving Y axis by  followed by


ZA

rot ⓐ moving X axis by 

 
Y B
A
B RXYZ RZ ( ) RY (  ) RX ( ) 
YA

 Rot ( Zˆ A ,  )
XA 
X B
85
 Fundamentals



ZA
ZA

Z B 


YB
ZB
 
 Y B , Y B

 
YA YA

Rot (YˆB,  ) Rot ( Xˆ B , )

 
XA   XA 
X B X B XB

86
 Fundamentals
 Equivalent angle-axis

ZA
  
 kˆ k x X A  k y Y A  k z Z A
k
kˆ 1
 
YA


XA
k x , (k y ,
For BA R, 4 parameters are used kz ,  )
with one constraintk x2:  k y2  k z2 1
Once 3 of 4 parameters are known,
A can be
BR
obtained.
87
 Fundamentals

K
A

i C 
 k


J

  
  C-frame : i , j , k , where
I j   
we decide the directionk of first,i , and
j
are decided arbitrarily.
 nx ox kx 
C=  n y oy k y  : known
 
 nz oz k z 
88
 Fundamentals

Looking for Rot ( K ,  )
   
ⅰ) K k x X A  k y Y A  k z Z A

K is expressed in the global (fixed) coordinate system.
 
rotate C about the global K axis Rot ( K ,  )C

ⅱ) k is the z-axis of C frame.

rotate C about the local k axis C Rot ( kˆ, )


ⅰ) = ⅱ)  Rot ( K ,  )C CRot ( C Z , )

Rot ( K ,  ) CRot ( kˆ, )C T (C  1 C T )
89
 Fundamentals
 Classification of vectors
Vector : magnitude + direction, with n component
 x1 
x 
x  2 
 
 
● Line vector : dependent
 xn  on its line of ac-
tion
1N 1N
(ex) Force vector applied to a certain loca-
tion
angular velocity about an axis.

v 1 m / s
● Free vector : independent of line of action
(ex) linear velocity vector, pure couple v 1 m / s

90
 Fundamentals
 Computational issue
A
P  BA R B P
A
P  BA R CB R DC R D P
①
②

AB
① : RBC R → ×63 , +42 (No. of computation)

② : 3 ( ×9 , +6) = ×27 , +18 (No. of computation)

91
 1.3 Problems

1. A vector A P is rotated about Zˆ A by  degrees and is subse-

quently rotate about byXˆ A degrees.
 Give the rotation matrix
which accomplishes these rotations in the given order.

2. A frame {B} is located as follows: initially coincident with a

frame {A} we rotate {B} about Zˆ A by degrees and then we ro-
tate the resulting frame about Xˆby
B
degrees. Give the rotation
matrix which will change the description of vectors from B P to
. AP

92
 Fundamentals
 Example
1. Obtain the position of Q by relative
transformation : Trans(X,2)Trans(Y,1)Rot(z,120)
2. Obtain the position of Q by using using geometric
approach
 
 A A 
 B R p BORG 
 
 
 0 0 0 1 

93
 Problems
3. The following frame definitions are given as known. Draw a
frame diagram, which qualitatively shows their arrangement.
 0.866  0.500 0.000 11.0 
Solve for T
B
C  0.500 0.866 0.000  1.0 
U
AT 

 0.000 0.000 1.000 8.0 
 
0 0 0 1
 1.000 0.000 0.000 0.0 
 0.000 0.866  0.500 10.0 
B
AT 
 ,
 0.000 0.500 0.866  20.0 
 
0 0 0 1 
 0.866  0.500 0.000  3.0 
 0.433 0.750  0.500  3.0 
C
UT 

 0.250 0.433 0.866 3.0 
 
0 0 0 1  94
 Problems
4. A familiar set of three coordinates which can be used to describe a point in
space is cylindrical coordinates. The three coordinates are defined as illus-
r
trated in Figure. The coordinate gives a direction in the plane along which
to translate radially by an amount . Finally, z is given to specify the height
above the plane. Compute the Cartesian coordinates of the point A P in terms of
the cylindrical coordinates,  , r and z. Use the relative transformation method.

95
Problems

Use both geometric approach and

Relative transformation

5. Refering to Fig. 2.25, give the value of ABT .

6. Refering to Fig. 2.25, give the value of CAT .
7. Refering to Fig. 2.25, give the value of CBT .
8. Refering to Fig. 2.25, give the value of CAT .

96
Chapter 2. Forward Kine-
matics
 Chapter 2. Forward Kine-
matics
2.1 Components of Robot
Kinds of joint
Definition of link
2.2 Robot forward kinematics
Denavit-Hartenburg parameters
2.3 Examples
planar model (2, 3 DOF)
spatial model (3, 6 DOF)

98
 2.1 Components of Robots
 Forward Kinematics ( 정방향 기구학 )
• Given the information of input parameters, calcu-
late the position and orientation of the robot end-
effector (task point).
• Unique in serial type manipulators, but not unique
in parallel type manipulators.

: sensor location
99
 Components of Robots
 Kinds of joints d

Revolute J (1 DOF) Prismatic J (sliding) Cylindrical J (2DOF)

d
φ 

X
d  p
Y where p : pitch

Spherical J(3DOF) Planar J (3DOF) Screw J (1DOF)

100
 Components of Robots
 Definition of link
① Link length ( )
a
: distance between joints at both ends
of link

shortest distance between two axis

: mutually perpendicular distance
a

101
 Components of Robots
② Twist angle ( )
angle between two joint axes attached at both ends
of the link

102
 Components of Robots
 Definition of joint
① Joint angle ( )
: an angle of one link relative to the previous link ( )

② Joint displacement (d)

: displacemnt of one link relative to the previous link (d)

d=0
  const d

103
 Components of Robots
 Denavit-Hartenburg parameters
● DH parameters : a,  ,  , d
Axis i  1
Axis i
Axis i  1

Link i
i 1
Link i  1
Zˆ i  1

Yˆ i  1 Zˆ i
Xˆ i
Yˆ i
Xˆ i  1 di
ai  1
i

104
 Components of Robots
 Rules
- Draw actuation axes as z-direction
- Draw Xˆ i along the link direction

 Link parameters
- ai  1: distance from Zˆ i  1 to Zˆ i along Xˆ i  1
-  i  1: angle measured from Zˆ i  1 to Zˆ i about Xˆ i  1

105
 Components of Robots
Zˆ i
i
Xˆ i
Link i Xˆ i
Zˆ i Li
di Xˆ i  1
Link i  1
Þ Zˆ i  1 L
i 1
i
Xˆ i  1

 Joint parameters
-  i : angle from Xˆ i  1 to Xˆ i about Zˆ i
- di : displacement from Xˆ to i 1 Xˆ i along Zˆ i

106
2.2 Manipulator Kinematics
i 1
We want T, which represents the relative
i

transformation of i-th link relative to (i-1)-th

link Zˆ
Zˆ R
i 1

i 1 Xˆ i   
X i 1, Y i 1 , Z i 1
Xˆ R Zˆ Q
Xˆ P fixed to i-th link
Xˆ i  1 di
Zˆ P
ai  1   
i X i, Y i , Zi
Xˆ Q
fixed to i-th link
107
 Manipulator Kinematics
i 1
T  i R1T QRT QPT PiT
i
  
Rot ( Xˆ i  1 ,  i  1 )Trans ( X R , ai  1 ) Rot ( Z Q , i )Trans ( Z P , d i )
ScrewX i 1 ( i  1 , ai  1 ) ScrewZi (i , d i )
1 0 0 0  1 0 0 ai  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
iT   i 1

 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  Zˆ i  1
Zˆ R
 s c c i c i  1  s i  1  s i  1d i 
 i i 1 i 1 Xˆ i

 s i s i  1 c i s i  1 c i  1 c i  1d i  Xˆ R Zˆ Q
  Xˆ P
Xˆ i  1 di
 0 0 0 1  Zˆ P
ai  1
i 108
Xˆ Q
 Manipulator Kinematics
 Interpretation of i 1
i T

 ci  si 0 ai  1 
 s c ci c i  1  s i  1  s i  1di   i  i1R i 1
piORG 
i 1
iT 
 i i 1
 
 si s i  1 c i s i  1 c i  1 c i  1d i   0 1 
 
 0 0 0 1 
Zˆ i  1 Zˆ i
Zˆ R i 1
i 1 Xˆ i
i R : Rotation matrix of i-th link coordinate i 1
piORG Zˆ P
with respect to (i-1)th link coordinate Xˆ P
Xˆ R ˆ di
X i 1 Zˆ Q
i 1
piORG : position vector of the origin of i-th link coordinate ai  1
i
with respect to (i-1)th link coordinate
Xˆ Q
109
 Manipulator Kinematics
 Example 1 : RR Manipulator ( 2DOF)
Yˆ 0 P ( x, y )

X2 • Link & joint parameters
Yˆ 2 1   2
l2 2 Set a0 &  0 as zero initial set
Yˆ 1 l1s1
X̂ 1  

l1 Z 0 Z 1
Ẑ 2
1
X̂ 0

  l1c1 when i 1
Z 0, Z1   
1 : angle from X 0 to X 1 about Z 1
  
d1 : distance from X 0 to X 1 along Z 1
(set d1 0) 110
 Manipulator Kinematics
 Example 1 : RR Manipulator ( 2DOF)
when i=2
     
1 : angle from Z 1 to Z 2 about X 1  2 : angle from X 1 to X 2 about Z 2
 
Z1 = Z 2 , 1 0
     
a1 : distance from Z 1 to Z 2 along X 1 d 2 :distance from X 1 to X 2 along Z 2

a1 l1 along X 1 d 2 0

i i 1 ai  1 di i
DH Table
1 0 0 0 1

2 0 l1 0 2
111
 Manipulator Kinematics
  
T  T T Rot ( Z 1 , 1 )Trans ( X 1 , l1 ) Rot ( Z 2 , 2 )
0
2
0
1
1
2

 c1s2  s1s2  c1s2  s1c2 0 l1c1 

s c c s  s1s2  c1c2 0 l1s1 
 1 2 1 2
 0 0 1 0
 
 0 0 0 1 
 c12  s12 0 l1c1 
s c12 0 l1s1   20 R 0
p2ORG  Yˆ 0
 12   
 0 0 1 0   0 1  X2
  Yˆ 2
 0 0 0 1  l2 2
Yˆ 1
X̂ 1
0 0 l1
Interpretation of 2 R andp2ORG ?
1
X̂ 0
112
 Manipulator Kinematics
 We want to find the location of P as function
of 1 and  2 l  2
 Knowns : 02T & p  0  (Position vector of E-E : end–effector)
2
  E E

 0 
0
0p   2
p E E 
LET P E  E  E E
, 0 0
P E  E  2T  
 1   1 
 c12  s12 0 l1c1   l2   l1c1  l2c12 
s c12 0 l2 s1   0   l2 s1  s12 
 12     
 0 0 1 0   0  0 
    
 0 0 0 1  1  1 
113
 Manipulator Kinematics
 Forward position
0
 20 R 0
p 2ORG   p E  E 
2

PE E   
 0 1   1 
 x
 20 R 2 p 
0
p   y
 E E 2 ORG
  
 1   z
 
 1
x l1c1  l2c12 , y l1s1  l2 s12

114
 Manipulator Kinematics
 Alternative way

Y0
i 1 i


Y3 X3 i ai  1 di

 
l2 1 0 0 0 1
Y2 X2 2

Y1
2 0 l1 0 2
 l1
X1
1  3 0 l2 0 0
X0

 c12  s12 0 l1c1  l2c12 

 0
R 0
p3  s c12 0 l1s1  l2 s12 
 12
0 0 2 3
3T  2T 3T  
 0 1   0 0 1 0 
 
 0 0 0 1  115
 Manipulator Kinematics

 Example 2 : RP Manipulator ( 2DOF)

Yˆ 0  
X 1  X 2 when d 2 0
 

Yˆ 1 X 0  X 1 when 1 0

Ẑ 2 a1: distance between Ẑ 1 and Ẑ 2

X̂ 2
l2
 
d2 X̂ 1 Z1  Z 2
l1

1
  X̂ 1 is along the shortest distance
Z 0 Z1 1 X̂ 0
(path) between Ẑ 1 and Ẑ 2

:1 angle from X̂ 0 to X̂ 1

 : angle from Ẑ 1 to Ẑ 2
d 2 : distance from X̂ 1 to X̂ 2 along Ẑ 2  
1 1  (   ) (1   ) 
2 2
116
 Manipulator Kinematics

 c1  s1 0 0   s(1   ) c(1   ) 0 0  s[(1   )  2 ]
 s  c  0 0    c(1   ) s(1   ) 0 0   
0
1T 
 1 1
  s (1   )c(  )  c(1   ) s (  )
 0 0 1 0  0 0 1 0 2 2
     c(1   )
 0 0 0 1  0 0 0 1 
c[(1   )  ]  s (1   )
   
2
1
T Rot ( X 1 , 
2 )Trans ( X 1 , l1 s )Trans ( Z 2 , d2 )
2
1 0 0 0   1 0 0 l1 s   1 0 0 0
0 0 1 0  0 1 0 0  0 1 0 0
=   
0 1 0 0  0 0 1 0  0 0 1 d2 
   
0 0 0 1  0 0 0 1  0 0 0 1
1 0 0 l1 s 
0 0 1 d2 
= 
0 -1 0 0 
 
0 0 0 1 
117
 Manipulator Kinematics
 s (1   ) 0 c(1   ) l1 s s (1   )  d2 c(1   ) 
  c(   ) 0 s(   )  l s c(   )  d s(   ) 
0 0 1
2T  1T 2T 
 1 1 1 1 2 1 
 0 1 0 0 
 
 0 0 0 1 

0
pE  E  0 p2ORG  2 pE  E  0 p2ORG  20 R 2 p E  E

 0 pE  E   20 R 0
p2ORG   2 pE  E  0  2 pE  E 
      2T  
 1   0 1   1   1 

118
 Manipulator Kinematics
 3DOF spatial type robot
 
Z 0, Z1

1

X2
l1

X1 l2
2

Z2  3
Z3

119
 Manipulator Kinematics
< Top view >
Y1 Y0 X1

1
Z1 X0
Z0 < Side view >

2 3 X2
3 Y4
Z2 Z3 Z0 Z1
l2
1 l1
X3 , X 4
2
X1

120
 Manipulator Kinematics
i  i 1 ai  1 di i
1 0 0 0
1
2  0 0
2
2
3 0 0
l1 3
4 0 0 0
0 0 1 2 l2
TT T T
3 1 2 3

 c1  s1 0 0  1 0 0 0   c2  s2 0 0  1 0 0 l1   c3  s 3 0 0   c1c 23  c1s 23 s1 l1c1c 2 

s c 0 0  0 0  1 0   s2 c2 0 0   0 1 0 0   s3 c3 0 0   s1c 23 s1s 23  c1 l1s1c 2 
 1 1       
0 0 1 0  0 1 0 0  0 0 1 0  0 0 1 0  0 0 1 0   s23 c23 0 l1c2 
       
0 0 0 1  0 0 0 1  0 0 0 1  0 0 0 1  0 0 0 1  0 0 0 1 

121
 Manipulator Kinematics
 l2 
 0
p   3
pE 
① where p  0 
0 3
 E
  3T   E  
 1   1   0
 

0
 4
0
R
0
pE  0 3  l1c1c2  l2c1c23   px 
② 4T    3T 4T
p E  l1s1c2  l2 s1c23   p y 
0
 0 1     
 1 0 0 l2   l1s2  l2 s23   pz 
0 1 0 0
3
4T 
 
0 0 1 0
 
0 0 0 1

122
 Manipulator Kinematics
 6 DOF Puma Robot

i i 1 ai  1 di i
1 0 0 0 1
2  90 0 0 2
3 0 a2 d3 3
4  90 a3 d4 4
5 90 0 0 5
6  90 0 0 6

123
 Manipulator Kinematics
<Top view>

1 X̂ 1
 0
R 0
Ẑ 0 X̂ 0 T  Rot ( Zˆ1 , 1 )  
0
1
1

 0 1 
Ẑ1

<side view>
Ẑ1

2 X̂ 2  21R 1
p2ORG 3 
1
2 T Rot ( X 1 ,  90 ) Rot ( Zˆ 2 ,  2 ) 
o

 X̂ 1
Ẑ 2  0 1 
1
p2ORG 3 0

124
Manipulator Kinematics
2
T Rot ( X 2 , 0o )Trans ( X 2 , a2 )
3

Trans ( Z , d ) Rot ( Zˆ ,  )
3 3 3 3

 32 R 2
p3ORG 
 
 0 1 
2
p3ORG

125
 Manipulator Kinematics

3
p4ORG

 43 R 3
p4ORG 
T Rot ( X 3 ,  90 )Trans ( X 3 , a3 )Trans ( Z 4 , d 4 ) Rot ( Zˆ4 , 4 ) 
3
4
o

 0 1 

 a3   c 4  s 4 0
3
p4ORG   d 4  , 43 R  Rot ( X 3 ,  90o ) Rot ( Zˆ 4 ,  4 )   0 0 1 
 0    s 4  c 4 0 

126
 Manipulator Kinematics
X̂ 5 X̂ 4
6 X̂ 6
5 X̂ 5
Ẑ 6
Ẑ 5
Ẑ 6
Ẑ 5  54 R 4
p5ORG 
Ẑ 4
4
5T Rot ( X 4 ,90 ) Rot ( Zˆ5 , 5 ) 
o

.  0 1 
Xˆ 4 Xˆ 5 Xˆ 6
 5
R 5
p6ORG 
ˆ 6
6T  Rot ( X 5 ,90 ) Rot ( Z 6 , 6 ) 
5 o

 0 1 

127
 Manipulator Kinematics
 r11 r12 r13 px  r11 c1[c23 (c4 c5c6  s4 s6 )  s23 s5c6 ]  s1 ( s4c5c6  c4 s6 )
r r22 r23 p y  r21 s1[c23 (c4 c5c6  s4 s6 )  s23 s5c6 ]  c1 ( s4 c5c6  c4 s6 )
0
T   21
6
 r31 r32 r33 pz  r31  s23 (c4 c5c6  s4 s6 )  c23 s5c6
 
0 0 0 1 r12 c1[c23 ( c4 c5 s6  s4 s6 )  s23 s5 s6 ]  s1 (c4 c6  s4 c5 s6 )
r22 c1[c23 ( c4 c5 s6  s4c6 )  s23 s5 s6 ]  c1 (c4 c6  s4 c5 s6 )
r32  s23 (  c4 c5 s6  s4c6 )  c23 s5 s6
r13  c1[c23c4 s5  s23c6 ]  s1s4 s5
r23  s1[c23c4 s5  s23c6 ]  c1s4 s5
r33 s23c4 s5  c23c5
Only function of px c1 (a2 c2  a3c23  d 4 s23 )  d3 s1
1 ,  2 , 3 p y s1 (a2 c2  a3c23  d 4 s23 )  d3c1
pz  a3 s23  a2 s2  d 4 c23
128
 Manipulator Kinematics
< Homework > 3DOF robot

 Z3
Z1 3
1


X 2 l1


X1
l2
2


Z2
   
Z1  Z 2 , Z 2  Z 3
129
 Manipulator Kinematics
 Express x, y, z in terms of 1 ,  2 , .3
 In case of 6 DOF
Control x, y, z and three orientation parameter such as
Roll, Pitch, Yaw.
or three Euler angles with 6 inputs(1  6).
 Refer to the Puma robot in text page 88.

130
 2.3 Problems
 3DOF (RRR) Manipulator

131
 Problems
 3DOF (RPR) Manipulator

132
 Problems
 3DOF (RRP) Manipulator

133
 Problems
 3DOF (RRR) Manipulator

134
 Problems
 3DOF (RPP) Manipulator

135
 Problems
 3DOF (PRR) Manipulator

136
Chapter 3. Inverse Manip-
ulator Kinematics
 Chapter 3. Robot Inverse
Kinematics
3.1 Workspace
3.2 Sovability
3.3 Planar examples
3.4 Pieper’s solution for 6-DOF robot
3.5 Spatial examples

138
 1. Definition
 Problem:
Given output position and orientation.
Find (calculate) joint angles (displacement).
This is a real control problem in Robotics.
u Inverse +
-
GC Robot 
Kinematics

Sensor

Primitive controller
Design Gc to eliminate  and 
139
 Definition
 Workspace
Volume of space that the end-effector of the ma-
nipulator can reach.
 Reachable workspace (A) : position
 Dexterous workspace (B) : position+orientation
(A > B)

140
 Definition
 Multiple solutions
For the given output, there are multiple configura-
tions that satisfy the given output.

In robot design, only one of those configuration is

chosen and the corresponding inverse solution
should be used in control.

141
 Definition
 Inverse solution of Puma robot
 4 different configurations for the wrist position two
different orientation configurations for each. There-
0
6T
fore, 8 Inverse Solutions for .
 The more nonzero links there are, the more ways
there will be to reach a certain goal (many inverse
solution).
ai 0
 If all , maximum 16 solutions exist.

142
 3.2 Method of Solution
 Method of solution
Can you obtain for the given manipulator po-
sition?
• Unique solution
• Multiple solution
• Numerical solution
 Inverse Kinematics
- Algebraic Method
- Geometric Method

143
Inverse Manipulator Kinemat-
ics
Y

Elbow up  c12  s12 0 l1c1  l2c12 

2 s c12 0 l1s1  l2 s12 
0
3T 
 12 
 0 0 1 0 
0
pBORG 2  
l2  0 0 0 1 
l1
Elbow down
1
X

144
 Inverse Manipulator Kinemat-
ics
 Given x &y , find 1 &  2

x 2  y 2  l12  l22
x l1c1  l2c12 cos 2  a
2l1l2
y l1s1  l2 s12
(cos , 0    180 )
x 2 l12c12  l22c12
2
 2l1l2c1c12
known : l1 , l2 , x, y
y 2 l12 s12  l22 s12
2
 2l1l2 s1s12
sin  2  1  cos 2  2 b
x 2 +y 2 l12  l22  2l1l2 cos 2
 2  A tan 2(b, a )

145
Inverse Manipulator Kinemat-
ics
x l1c1  l2 (c1c2  s1s2 ) Cramer’s rule

(l1  l2c2 )c1  (l2 s2 ) s1 x  l2 s2

y l1  l2c2
y l1s1  l2 ( s1c2  c1s2 ) c1 
l1  l2c2  l2 s2
(l2 s2 )c1  (l1  l2c2 ) s1 l2 s2 l1  l2c2

l1  l2c2 x
l2 s2 y
1  A tan 2( s1 , c1 ) s1 
l1  l2c2  l2 s2
l2 s2 l1  l2c2

146
Inverse Manipulator Kinemat-
ics
 Another notation
Let, k1 l1  l2c2 r cos  x r cos  cos1  r sin  sin 1

k2 l2 s2 r sin  r cos(r  1 )

where,   A tan 2( k2 , k1 ) y r sin  cos1  r cos  sin 1

2 2 r sin(  1 )
r  k k 0
1 2
y x
sin(  1 )  , cos(   2 ) 
 
y x
  1  A tan 2( , )  A tan 2( y, x)
 
1  A tan 2( y, x)  A tan 2(k 2 , k1) 147
Inverse Manipulator Kinemat-
ics
3 DOF Serial Manipulator x l1c1  l2c12  l3c123
Y
y l1s1  l2 s12  l3s123
 1   2   3
l3
Given x, y, , find 1 , 2 , 3
l2 3
l1
Let x x  l3c l1c1  l2c12
2
X y  y  l3 s l1s1  l2 s12
1 the same
1 , 2 solution
as the 2
DOF case
 3   (1   2 )
148
 Algebraic solution by re-
duction to polynomial
 1  u2 2u
 Let u tan , cos  2
, sin  
2 1 u 1  u2
Convert a transcendental equation given by
a cos  b sin  c

a 1  u 2   2bu c 1  u 2  a  c u 2  2bu  c  a  0

b  b2  a 2  c2  2 2
1 b  b  a  c
2 

u  2 tan 

 ,  180    180

a c  a c 

if a+c=0,  180 Note : polynomials up to degree four

possess closed-form solutions.
149
 3.3 Pieper’s Solutin
 For wrist type robot with all revolute joint
When the last three axes intersect,
the origins of frame {4}, {5}, and {6} are coincident.
 o p4 org  0 1 2  3 p4 org 
   1T 2T 3T  
 1   1 
 c4  s4 0 a3 
 s c c4c 3  s 3  s3 d4 
3
4T 
 4 3 
 s4 s 3 c4 s 3 c 3 c3 d4 
 
 0 0 0 1   a3 
3
p4 org   s 3d 4 
 
 c 3d 4 
150
 Inverse Manipulator Kinemat-
ics
 c3  s3 0 a2   a3   f1 3 
 s c  
 3
p  c3c 2  s 2  s 2 d3    s 3d 4   f 2 3 
2
T
3 
4 org
  3 2  
 1   s3 s 2 c3 s 2 c 2 c 2 d3   c 3d 4   f3 3 
    
 0 0 0 1   1   1 
where

f1 3  a3c3  d 4 s 3s3  a2

f 2 3  a3s3c 2  d 4 s 3c3c 2  d 4c 3s 2  d 3s 2
f3 3  a3s3s 2  d 4 s 3c3s 2  d 4c 3c 2  d 3c 2

151
Inverse Manipulator Kinemat-
ics
 f1 3   c2  s2 0 a1   f1  3   g1 
    
1  2f  
3 
s2c1 c2c1  s1  s1d 2   f 2  3   g 2 
2T    
 f3 3   s2 s1 c2 s1 c1 c1d 2   f 3  3   g 3 
      
 1   0 0 0 1   1   1 
where
g1 c2 f1  s2 f 2  a1
g 2 s2c1 f1  c2c1 f 2  s1 f 3  d 2 s1
g3 s2 s1 f1  c2 s1 f 2  c1 f 3  d 2c1

152
Inverse Manipulator Kinemat-
ics
 g1   c1  s1 0 0   g1   c1 g1  s1 g2 
 o p4 org  0  g 2   s1 c1 0 0   g2   s1 g1  c1 g2 
    
   1T 
 1   g3   0 0 1 0   g3   g3 
      
 1 0 0 0 1  1   1 
o 2
p4 org r g12  g 22  g32
 f12  f 22  f32  a12  d 22  2d 2 f3  2a1 c2 f1  s2 f 2 
r k1c2  k 2 s2 2a1  k3 (1) k1  f1
z k1s2  k2c2 s1  k 4 (2) k2  f 2
k3  f12  f 22  f 32  a12  d 22  2d 2 f 3
where (1) and (2) are not dependent of 1
k4  f 3c1  d 2c1
153
 Inverse Manipulator Kinemat-
ics
a) If a1 0  r k3  f1 3 , then solve for  using
3
3
half tangent angle quadratic equation in tan
2
b) If s1 0  z k4  f 3c1  d 2c1the same.
,
c) Otherwise, eliminate c2 and s2 from (1) and (2).
r  k3   z  k4 
2 2

 k12  k22
4a12 s1
 4th order polynomials of 3
Now, obtain  2 from (1) and (2) and from0 p4org
Obtain 1 from x and y.

154
 Inverse Manipulator Kinemat-
ics
 Now it remains to solve for  4 , 5 , and 6

 r11 r12 r13 

3
R  0 1 0
R R r r r23 
6 3 6  21 22 
 r31 r32 r33 
 c4c5c6  s4 s6  c4c5 s6  s4c6 c4 s5 
 s5c6  s5 s6 c5 
 
  s4c5c6  c4 s6 s4c5 s6  c4c6 s4 s5 

 find ,4 ,5 and 6 (two sets of solution)

singular at  00
5
155
 Inverse Manipulator Kinemat-
ics
 Solve the inverse solution of the following
3DOF manipulator (use Pieper’s method)

 Z3
Z1 3
1


X 2 l1


X1
l2
2


Z2

156
Inverse Manipulator Kinemat-
ics
 Homework 1
( x, y )
Given x and y, find 1 & d 2
d2

1
 Homework 2
( x, y )
x l1c1  l2c12
l2
 1   2 

Given x & , find 1 &  2 l1 2

1
157
Chapter 4. Jacobian
 Chapter 4. Jacobian
4.1 Rotating Axis
4.2 General Rigid-body Motion
4.3 Motion of Robot Link
4.4 Velocity Propagation
4.5 Jacobian Analysis

159
 Level of kinematics
 Zero-order kinematics - position level
(chap 2~4)
 1st-order kinematics - velocity level (chap 5)
 2nd-order kinematics - acceleration level
(chap 6)
 3rd- order kinematics - Jerk
 4th - order kinematics - Jink

160
 Definition of Jacobian
 Jacobian (a form of matrix) relates operational
(task, Cartesian) velocity vector (u ) to the joint
velocity vector  or it relates the joint torque
vector ( ) to the operational force vector(f )
 ( grammer : J relates A to B ⇒ A=J B )

u  J 
 J T f

161
 4.1 Fundamentals
 Vector : a a e
a
e , e 1 Unit vector
a

For a fixed unit vector, d

(e) 0
dt

For a rotating unit vector, d

(e) 0
dt

162
 Fundamentals

j (t ) length of cord=R = = i(t )

j (t  t )
n(n 방향 ) di (t ) i (t )
 lim
i i (t  t )  i (t ) dt t  0 t
i (t  t )
sin ce i (t )  n
 
R 1 = lim n  j
  k i (t ) t  0 t

d 
di (t ) ( = lim )
= j  i ( k ) i dt t  0 t
dt

163
 Fundamentals
 Analogy
d
( j )   i   j
dt
d
(k )  k ( k ) k 0
dt

164
 Fundamentals
Ex 2) p ai  b j
d di dj
dt
 
p a  b a i  b  j
dt dt
 (ai  b j )  p

165
 Fundamentals
A
Given and
B
Q B R
In text
Then, we know
 BQx 
  
A   
Q  BA R BQ  A X B A
YB A
Z B   BQ y  
ZB

ZA
 B 
 Qz  B
Q 
Note  d1   a1 b1   c1   a1   b1 
YB
 d   a      c1    c2
b2   c2   a2 
 2  2  b2 

YA
Q  A Xˆ B BQx  AYˆ B BQy  A Zˆ B BQZ
A

XA 
XB
166
 Fundamentals
 If Q is fixed to the B coordinate system
Yˆ A
Yˆ B

Q
B xˆ B

xˆ A

 BQx 
A  B  B A ˆ A
ˆ A
Q  B R Q  B R  Qy   Qx X B  Qy Y B  Qz Zˆ B
A A B B B

 BQz 
 
167
 Fundamentals
 If Q vector is fixed in B coordinate and rotates
with it by A  B
d A d B
( Q) 0, ( Q) 0
dt dt

d
dt
 
A
Q  B vx A Xˆ B  B
v y AYˆ B  B
vz A Zˆ B  BQx ( B   A Xˆ B ) 
A

Qy (  B  AYˆ B )  BQz (  B  A Zˆ B )
B A A

A A A A
 B v x X B  B v y Y B  B v z Z B   B  AQ
168
 Fundamentals
d B
where B
vx   Qx ,  B vx 
dt
B  
if we define v Q  B v y 
 B vz 
 

d
dt
 
A
Q  BA R v Q   B  BA R BQ 
B A

d
If Q is fixed in B,
dt
 
A
Q   B  BA R BQ 
A

169
 Fundamentals
 Example (2DOF Manipulator)
Yˆ A  r  cos  sin  0
Yˆ B Q  0  ,  sin  0
B A
R  cos 
  B  
 0   0 0 1 
(slider 의 center)
Q A B
Q  BA R Q
r
Xˆ B  cos  sin  0   r   r cos  
 sin  cos  0   0   r sin  
  Ẑ  r sin 
Xˆ A     
 0 0 1   0   0 
r cos 

d
dt
 Q 
A A B A
v Q  BA R vQ   B  Q
A

170
 Fundamentals
 r 
d
B
v Q  0 
  dt
 Q 
A A B A A
v Q  BA R vQ   B  Q
 0 

 cos  sin  0   r 
  
v Q  sin    
A
cos 0 0  ( Z A ) ( r cos X A  r sin  Y A )
  
 0 1   0 
 r cos  r sin  
 r sin   r cos 
 
 0 

171
 Fundamentals
 Second method
Differentiate Q directly and transform the re-
B

sult to A coordinate.
 
B
Q r X B  B  Z B
d d 
 

B
Q r X B  r ( X B )
dt dt
 
r X B  r ( B X B )
B
 r 
 
r X B  rY B   r 
 
 0 
172
 Second method

d A d
dt
  A
Q B R
dt
 Q
B

 cos   sin  0   r   r cos   r sin  

 sin  cos  0   r   r sin   r cos  
    
 0 0 1   0   0 

Identical to the first method

173
 Rigid-body motion
 Translation

Rigid-body 의 모든 점이 똑같은 거리
X
만큼 움직인다 .

X&Y

174
 Rigid-body motion
 Rotation

회전 기준 축에 대하여 Rigid-body 의 모든 점이
같은 회전각 만큼 움직인다 .

175
 Rigid-body motion
 General rigid-body motion
Combination of translation and rotation
x2
2 v2
v1
1
x1 x2  x1

2
v1
 l
= v1 +
1 1
x1
176
 Rigid-body motion
For t , x1 v1t (t : small )
x2 v2t
x2  x1 (v2  v1 )t
v
v v2  v1 l , (  )
l
 v 2 v1   r 2 /1 v1 i  l i (v1  l )i
where r 2 /1 l j ,  (  )k

177
 Motion of robot link

l1 l2 l3 l4

1 2 3 4

i :(angular velocity of i -th motor) creates an

angular velocity of i -th link.
The absolute angular velocity of i -th link :
i
  k
k 1

178
 Motion of robot link
① lock 2, 3, 4 joints & rotate the first joint with 1
The linear velocity of e.e. due to 1
v1 (l1  l2  l3  l4 )1
② lock 1, 3, 4 joints & rotate the second joint with 2.
The linear velocity of e.e. due to 2
v2 (l2  l3  l4 )2
③ lock 1, 2, 4 joints & rotate the third joint with 3
The linear velocity of e.e. due to 3
v3 (l3  l4 )3
179
 Motion of robot link
④ lock 1, 2, 3 joints & rotate the fourth joint with 4
The linear velocity of e.e. due to 4
v4 l44

Since every joint is kinematically independent

and it is a linear configuration, the velocity of the
end-effector can be obtained by linear
combination of the four independent cases.
That is ,v v1  v2  v3  v4
l11  l2 (1  2 )  l3 (1  2  3 )  l4 (1  2  3  4 )
180
 Motion of robot link
where,
1 : absolute angular velocity of 1st link
1  2 : absolute angular velocity of 2st link
1  2  3 : absolute angular velocity of 3st link
1  2  3  4 : absolute angular velocity of 4st link

181
 Motion of robot link

v  0 v1  0 v2  0 v3  0 v4
0
v1 l11
0
v2 l2 (1  2 )
0
v3 l3 (1  2  3 )
0
v4 l4 (1  2  3  4 )
v 1 r1/ 0   2 r 2 /1   3 r 3/ 2   4 r 4 / 3

182
183
4.2 Motion of links of a ro-
bot
i
i
Link i

0 i
i
vi v i  0i R v i
 
Zi Yi

Xi
v i : linear velocity of the origin of link i , expressed in
i

i
i
-th coordinate
 i : angular velocityframe
of the origin of link i ,
expressed in reference coordinate i 184
4.3 Velocity propagation from link to
link

i
i

Link i


Z i 1
i
vi
i 

 P i 1 X i 1
Zi Yi i 1

Xi

i
i th link rotates with  i
(i  1)th joint rotates with &
i 1
i 1 ˆ
Z i 1
i
    i R& i 1Zˆ , expressed in i-th coordinate frame
i 1
i
i i 1 i 1 i 1

185
 Axis i  1 Axis i  2
Axis i

Linki  1
i
Link i
Zˆ i

Yˆ i Zˆ i 1
Xˆ i 1
0 Yˆ i 1
pi
Ẑ 0 ai Xˆ i di 1 Zˆ i 1
0
pi 1 i 1
X̂ 0 Yˆ0

 
d

 
0 0 0 0
0
pi 1  pi  ai Xˆ i  di 1 Zˆ i 1   0 vi 1  0 vi  ai 0i 0 Xˆ i  di 1 Zˆ i 1  di 1 0i  Zˆ i 1
0 dt

 
0
0 vi  0i  0 pi 1  0 pi  di 1 Zˆ i 1

R o vi 1 i 01R 0 vi  0i  0 pi 1  0 pi  
i 1
i 1 
d Zˆ
0 i 1 i 1

vi 1  i 1 vi  i 1i  i 1 pi 1  pi   di 1 Zˆ i 1  i i1R  i vi  ii i p(i 1)ORG   di 1 Zˆ i 1

i 1 i 1 i 1 i 1

 i 1
pi 1  i 1
pi   i i1R i p(i 1)ORG
Velocity propagation from link to link

i 1
i 1 i 1 
 i 1  R  i  i 1 Zˆ i 1 for revolute joint (i+1)
i
i
i 1 i 1 i
 i 1  R  i for prismatic (i+1) joint
i

expressed in (i+1)-th coordinate frame

The velocity vector of the (i  1)th coordinate origin
i i i i
v i 1  v i   i  P i 1
i 1 i i i
v i 1  i i1R ( v i   i  P i 1 ) for revolute joint

v i 1  i i1R ( v i   i  P i 1 )  d i 1 i 1Zˆi 1 for prismatic joint

i 1 i i i

187
 Velocity propagation from link to link

Assume that 0 0,

0 0
v0 0

Y0

 0 0
0

0
v0 0


X0

Except mobile robot


Y0

0
v0 v0 i

X0

188
 Velocity propagation from link to link

 Start from i=0 and go to i=n-1, where n is the

number of links
1 N
1 N
1
 N N
v1 

0
 N  N0 R N  N , 0 N
vN  N0 R v N

189
 Velocity propagation from link to link

 Example 
X3

E

Y3
l2

X2
2


Y0 
Y2

Y1 l1 2


X1
1
1 
X0

Assume 0  0 0 and 0 v 0 0 0 v E ?

190
 Velocity propagation from link to
link
 For, i 0 1

X3

 0

E

1  01R 0  0  1 Z 1   0 

1 1 Y3
l2

 1 

X2
2

 


1 0 0 0 Y0 

v1  R v 0   0  p1 0
1
0

Y1 l1
Y2
2


X1
1
1 
X0

 For, i 1 2
 2  12 R 11  2 2 Z 2
2
R  21R  1  21RT  c1  s1 0  c2  s2 0
1
0
R  s c1 0 , 21R  s2 c2 0
1  1   
 0 0 1   0 0 1 
191
 Velocity propagation from link to
link
1 2
 c2 s2 0  0  0 

 2   s2 0   0   0  X3
2
c2
     E

 0 1   1    2 


0 Y3
l2


2 2 2 0 X2

 0  0  0   0 
2

  0   0   0    0 

Y0 
Y2

2
       
Y1 l1

    2   1   2   1   2 


1 
X1
1
1 
X0
2
 1
v 2  R v1  1  p 2
2
1
1 1

  
  
   

2  1 1 2 1
 1 R   1  Z 1   l1 X 1    1 R  l11 Y 1 
     
192
 Velocity propagation from link to
link
 For i 2 
X3

  1 0 0
2  
 1 0
E
3 3 3
 3  R   2  3 Z 3 
3
2 2 R  0 

Y3

   0 0 1  l2

X2
3 0 2
 0   0  
Y0 

  0   0 
Y2

Y1 l1 2

    

 
X1

1   2   1   2  1
1 
X0

3
v 3  23 R  2
v 2  22 2 p3 

193
Velocity propagation from link to
link
 l11 s2  
      2 
2

v 3  l11c2    1   2  2
Z 2   l2 X 2 
 0     
 
 l11 s2 
 
 l1 
1c2  l2 1   2 

 
 0 
 1 1 2 l  s   3v 
  3 x
 v 3  2 R v 3  l1
3 3 2
1c2  l2 1   2     v y 
   3v 
 0   z 
194
Velocity propagation from link to
link
3
vx l1s21
v y l1
3
1c2  l2 (1  1 ) (l1c2  l2 )1  l2 2

3
 vx   l1s2 0   1 
 v     
 y   (l1c2  l2 ) l2    2 
0 0 3
v E  R v3
3

195
Velocity propagation from link to
link
0
3 R  10 R 21R 32 R
 c1  s1 0   c2  s2 0  1 0 0
 s1 c1 0   s2 c2 0  0 1 0
   
 0 0 1   0 0 1   0 0 1 
 c12  s12 0 
 s12 c12 0 
 
 0 0 1 
  (l1s1  l2 s12 )  l2 s12  
0    1 
v E  l1c1  l2 c12 l2 c12  
  
 0 0   2 
196
 4.4 Jacobian Analysis
Let’s remove Z-component
0
 vx  0 
 v   J
 y
where
0   (l1s1  l2 s12 )  l2 s12 
J  
(l c 
 1 1 2 12l c ) l c
2 12 

197
 Jacobian Analysis
Remind
3
 vx   l1s2 0   3
 v      J
 y   l2  l1c2 l2 
0
J  30 R 3 J


Why? 
y0

y3
x3

0 3
 vx  0  vx 
 v  3 R  v 
 y  y 
x0

198
 Jacobian Analysis
3
J and 0 J have the same physics,
but they are expressed in different coordinate.
0
 vx 
We want to control   , what should we do?
 vy 

v  J
0 1 0
0
  J 0
v

199
 Jacobian Analysis

1
 a1 a2  1  b2  a2 
※ b 
 1 b2  a1b2  a2b1   b1 a1 

0 1 1  L2 c12 L2 s12 
 J   Lc  L c
L1 L2 s2  11 2 12  L1s1  L2 s12 

Det ( 0 J ) L1 L2 s2 0
sin  2 0
 2 n ( n 0, 1, 2,)

200
 Jacobian Analysis

 2 
2
 2 0

1

vx 1m / s, v y 0.5m / s v  1 ,  2 do not exist!!!

201
 Jacobian Analysis
 At singular point, some of the operational mo-
tion cannot be achieved
  1  0  0
v   J1 J 2     J11  J 2 2
0 0

 
 2

1   2
2 2

1 1

Note : 1 and  2 are independent.

202
 Jacobian Analysis
  (l1s1  l2 s12 )  J11
Lock 2 and apply 1 , 0 0
J1  
l c  l
 1 1 2 12 c
  y   y  
     
2

 x   x  1

   l2 s12  0
J 22
Lock 1 and apply  2 , J 2 
0

l c
 2 12 

203
 Jacobian Analysis
 1 
Let    torque vector(=input) at joints
 2 
 fx 
f   effective force vector at End-effector
 fy 
  J T f  u  0 J  f y

fx

2

1

Actuator at E-E 204

 Jacobian Analysis
 Work
T
  
Input work = 1 1 2 2    (   )
Output work = f x x  f y y  f T u

If no energy loss,  T   f T u

양변을 divide tand take limit in t

 T  T u  T  T
lim   f      f u power equilibrium
t  0
 t t 
205
 Jacobian Analysis
d 
※  lim
dt x 0 t

Here, we know u  J 
 T   f T J 
(  f J ) 0
T T

 0 T T
   f J 0
T T
 f J
  J T f effective force relation
206
 Jacobian Analysis
Offset force
  J T f
 *     T f 0

Total offset torque at joint

f x 1N

1m
1 1N m  2 1N m
1m
207
 Jacobian Analysis
 Easy way to obtain Jacobian

( x, y )
Y0
1   2
l2
2
l1

1 X0

x l1 cos1  l2 cos(1   2 )  x   1 

y l1 sin 1  l2 sin(1   2 )  y   J   
  2 
208
 Jacobian Analysis
x  l1 sin 1
1  l2 sin(1   2 )(1   2 )

  l1 sin 1  l2 sin(1   2 ) 1   l2 sin(1   2 )  2

y l1 cos1
1  l2 cos(1   2 )(1   2 )

 l1 cos1  l2 cos(1   2 ) 1   l2 cos(1   2 )  2

 x     l1 sin 1  l2 sin(1   2 )   l2 sin(1   2 )   1 

      
  1
y l cos  1  l 2 cos(1   2 
) 2
l cos(  1   2   2 
)

209
 Jacobian Analysis
< Homework #5>
1. (5-19)
 a1c1  d 2 s1 
0
p2 BOG  a1s1  d 2c1 
 
 0 

(1) Draw the RP manipulator

(2) Derive Jacobian (easy method)
(3) Identify singularity (draw)

210
 Jacobian Analysis
2. (5-11) Given  0.866  0.500 0.000 10.0 
 0.500 0.866 0.000 0.0 
A
BT 
 
 0.000 0.000 1.000 5.0   0.0 
   2.0 
 0 0 0 1   
  3.0 
If the velocity vector at the origin f {A} is A
V  
 1.414 
 1.414 
 
 0.0 
Find the 6x1 velocity vector with reference point the
origin of {B}.

211
 Jacobian Analysis
3. (5.13) A certain two-link manipulator has the
following Jacobian
  l1s1  l2 s12  l2 s12 
0
J ( ) 
 l1c1  l2 c12 l2 c12 

Ignoring gravity, what are the joint torques re-

quired in order that the manipulator apply a
static force vector 0 F 10 Xˆ o

212
 5. Jacobian
(G&H Formulation)
 Chapter 5. Jacobian
(G&H Formulation)
 5.1 Kinematics
 5.2 Kinematic Influence Coefficient

214
Modeling Approaches in Ro-
botics
 Classical Newton-Euler Approach
Classical vector mechanics approach,
not closed-form
 Lagrangian and Hamiltonian Apprach
Energy-based approach
Analytical dynamics approach
 Lagrange’s form of D’Alembert Principle**
Kinematic influence coefficiants : G & H
Isomorphic formulation in both kinematic & dynam-
ics,
Easy to obtain closed-form models of general robot
systems 215
 Motivation
 Principle
Employ Lagrange’s form D’Alembert Principle
 Advantage
All kinematic and Dynamic models are derived
in terms of Kinematic Influence Coefficient,
which is purely position-dependent function
 Applications
Useful for Robot Design and Model-based Control

216
 Purpose
 [*][ P*] Joint space dynamics

Via transform of coordinate

Tu [uu*] u u[ Puuu*]u Operation space dynamics

 P   [Gp ]   [ G p
]  R 3n

u  jk   jk    Gu   ,  nx

w [G ] [G

jk
]  R 3n
     

 P  
u  jk   Gu  
   Hu    [Gu ]  [Gu ] [ Hu ]  R 6nn
 w 

217
 Definition
P Dependent parameter
Y0 j u
Jacobian  G i Independent parameter
jk
p w
Chain number

Chain 1 Chain 2
X0
 ux  [A]i ; i-th row
u 
[A] ; j j-th row
u  y  u x  f ( )  f (1 , 2 ....n )
 .
  모든 joint
[A]i ; j i x j element
 .

218
 5.1 Kinematics
ux  ux  u
x ux ux 
u x    n 
   [Gu ]
1 1
n    1 1 n 


 g1x g 2x  g nx  
 

d  u x    u x  d1   ux  d2   ux  dn

Note           
dt  1  1   1  dt  2  1  dt n  1  dt
2u x  2u x 2 u x
 2 1  2    n
1 2 1 n 1

219
 Kinematics
 d  u x  d  u x  d  ux  

 ux        G  1; 
u
   
 dt  1  dt  2  dt  n  
 2u x  2u x 2 u x 
 
2 1
2   n  
 1 2 1 n 1 
 2u x 2u x 2 u x 
 1  2   n     G  
u
  1;
 12 2 2 n 2 

  Gu   T
   H  

u
 1;;
1;

220
 Kinematics

where
 2u x 2u x 2 u x    
  2   G u

 1 2 1 n 1   1    1; 
 
 2u x 2u x  ux   
2

u     G u

 H   12  2 2 n 2   2   1; 


  1;;    

 


 2 2
   
  ux  ux    Gu  
   
 n 1  2 n   n 1;


221
 Kinematics

 Hu    H u  1;;  H u   R 6nn

6;;
6-plane

definition :  H u    j, k  Component of i-th plane

i ; j ;k

Def. Hessian   ( Jacobian)



  u x   u x
x
hmn  
m  n
   g nx  g nx 
n
 m

222
 Kinematics
uy  Gu    Hu  
T

2; 2;;

uz  Gu    Hu  

T

3; 3;;

Note
     
 G u

     2;   G u

     3; 
 1   1 
     
 G u
  G u

 2   2;   2   3; 
 
 H u   Hu 
2;;   3;;  
     
     
  G  
u
  G  
u

 n   n 
2; 3;

223
 Kinematics
 ux 
  G p   T  H p    u 
P       y
 uz 
 G   u   Hu  3;;
    1; 
 H p    Hu 
 Gp    Gu   1;;  Hu  2;;
 2; 
 u  3 n n
  G  3; 
3 n

P is any point, i.e. P s not always the position of end-effector.

224
 Kinematics
Note. There is no physical angles corresponding to w jk , where is
an absolute angular vel. vector. Thus, rotational kinematics
starts from the velocity-level.
j -joint k -joint
w  Gjk     Hjk  
jk T

jk link
3 n 3 n n

 Gjk   R 3n ,  Hjk   R 3nn

 Hjk  
 H u 
 H u  6;;
4;;
 H u 
5;;

225
 Kinematics
 H jk    Hu 
2;; 5;;

 H jk    Hu 
3;; 6;;

u  Gu    Hu  

T

where 3 n 3 n n

  Gp     Hp  
   
 Gu   ,  Hu  
  G  
jk
  Hjk  
6 n   6 n n  
3 n 3 n n

226
 Kinematics
ex-1) 2-DOF Planar Manipulator
( x, y )
x  x
l2 x  1  2
2 1 2
 1 
l1 ( x1 , y1 )
  (l1s1  l2 s12 )  (l2 s12 )   

1
 2 
y  y
( x1 , y2 ) (l1c1 , l1s1 ) y  1  2
1 2
x l1c1  l2c12  1 
 (l1c1  l2 c12 ) (l2 c12 )   

y l1s1  l2 s12  2 

227
 Kinematics
  (l1s1  l2 s12 )  (l2 s12 )  
u     
 G  
P

 (l1c1  l2 c12 ) (l2 c12 ) 

  (l1c1  l2 c12 )  (l2c12 )  
 H  
P
 
  (l2 c12 )  (l2c12 ) 
1;;

  (l1s1  l2 s12 )  (l2 s12 )  

 H  
P
 
  (l2 s12 )  (l2 s12 ) 
2;;

u  GP    T  H P  

x  GP    T  H P  

1; 1;;

y  GP    T  H P  

2; 2;;
228
 Kinematics
u [Gu ]
where

  u u u  u
[G ] 
u
   
 1 2 n  

 u x u x u x 
  
2 n  du u d
 1   chain  rule
 u y u y u y   dt  dt
   
 1 2 n 
  
 
 
각 column은 u를 i 로 미 분 한 것 .
229
 Kinematics
u u u
u [Gu ]1  2    n
1 2 n
=g u   g u     g u
1 1 2 2 n n

u is a linear combination of contribution of all joints,

where i is a scaling factor.

230
 Kinematics

u [Gu ]1;   T [ Hu ]1;; 

where
  
 G u

     1;; 
 1 
  
 G u

 Hu   2   2;;  .

1;;  
  
 
 
 Gu  
 n n ;; 


231
 Kinematics
u u u
[Question]  

  
 
Try  ( 
u )  (u )
n n

  d     d 
 (u )   (u )    (u ) 
n n  dt  n   dt 
  u d 
    G    0
n  dt   0
 d   d    u
 u
   G   u
 G      G u
    g u

dt   n  dt     
 n
n n
 1
 
  232
 Kinematics
This is true for all n (1 ~ N )
u u
  
 

This is true for all n  th derivative

u u ( n ) u
    ( n )
  

233
 5.2 Kinematic Influence Coefficient
 j k
link( j )
link(j-1)

link( k )

Let S n : Unit vector along Zˆ n expressed in terms of

global reference frame
0
 jk : angular vel. Vector of j-th link.
j
0
 jk 
1 S1   2 S 2     j S j   n Sn
n 1

 S1 S2  S j 0 0  0    Gjk  

where 3 N N 1
T

 1  2   j   n 
234
 Kinematic Influence Coefficient
 of S j 3 1
 of  Gjk  3 N
 of 0
 jk 3 1
 of   N 1

 jk  Gjk  
3 N N 1
where
 S n ; n  j and n  n
 G  
jk

 o ; n  j or n Sn  prismatic joint 

235
 Kinematic Influence Coefficient
ex 1)  1 
3  
Y0  jk  S1 S2 S3   2 
 3 
 
S3
0 0 0   1 
2  
 0 0 0     
 1   2  3 
  2
 1 1 1   3 
S2
1
Z0  C1  S1 0   0   0 
S1 X0
S1 10 R 1Zˆ1  S1 C1 0   0   0 
    
 0 0 1   1   1 
S 0 R 2 Zˆ
2 2 2

S3 30 R 3 Zˆ3 236

 Kinematic Influence Coefficient
ex 2)
3
34  S1 0 S2  
 0 0 0   1 
 
d2  0 0 0   d 2   1  3 
 
 
 1 0 1   3 

1

237
 Kinematic Influence Coefficient
ex 3)  C1  S1 0 
0
R  S C1 0 
Zˆ 0 , Zˆ1 1  1 
 0 0 1 
1 S1
1 90
X2
 1 0 0   C2  S 2 0   C2  S2 0
Ẑ 2  
2 1
2 R  0 0  1  S2 C2 0   0 0  1
   
 0 1 0   0 0 1   S 2 C2 0 
 C1C2  C1S2 S1 
0
R  SC SS C 
2  1 1 1 2 1

 S 2 C2 0 
0 ˆ 0 ˆ 0 ˆ
X 2 Y 2 Z 2
238
 Kinematic Influence Coefficient
 1  
  S1
23
S2    1S1   2 S2

2 
 0 S1  
 
 0  C1   1 
  
 1 0   2 

Note


  jk dt   jk
  j  
    i Si 
    i 1 
 jk   j  
 G  
jk
;n 
n
  
n  i 1
 i S i  S n n  j, n n 


239
 Kinematic Influence Coefficient
Sl Zˆ j
j
P
S2 Zˆl l
S1 Ẑ 2  2 Xˆ j
Sl  1
Ẑ1 1 X̂ 2 a23 l  1
Sll Xˆ l al  1l
al  1l
X̂ 1 a12
a0 0 al  2 l  1
Ẑ 0 S0  01 0
o 1
X̂ 1
X̂ 0
j
0

P S11 S1   al  1l al l  1  Sll Sl 0j R j P
l 2

240
 Kinematic Influence Coefficient
where
 al  1l  Xˆ l  1 : rotaing vector

 0P : position vector of E.E w.r.t 0.

 Rl : position vector of l  th coordinate origin w.r .t 0.


 l 1  
a l  1l l  1 al  1l    n Sn  al  1l
 n1 
S l l  1 Sl * Sl 은 l  1 로 회전 . 그러나 방향은 변하지 않는다 .
Sl l 은 의 영향을 받지 않는다 .

241
 Kinematic Influence Coefficient
j
0
 
P S11 S1   al  1l al  1l  Sll Sl 0j R j P
l 2

 j
 l 1
  l 1
 
P S
S
11 1  S S
11 1    l  1l   n n  l  1l
a  S a  S S
ll l  Sll   n n 
 S Sl
l 2   n1   n1  
  j 0j R j P 0j R j P
if j P is fixed in j  th coordinate frame
0
j
   j j 
  Sn Sn   n Sn   al  1l al  1l  Sll Sl  j R P  
0

n 1   l n1 
0
P  Rn
j


 Sn S n   Sn  0 P  Rn 
n 1

 g1p g 2p  g np  g Np  
242
 Kinematic Influence Coefficient
 Sn ( o P  Rn ) ; n  j and n  n (revolute joint)

where g np  Sn ; n  j and n Snn (Prismetic joint)
o ; otherwise (n  j )

j

 
P  Sn Sn   Sn  0 P  Rn 
n 1
Note
P P    d     
P    ( P)   ( P)    ( P) 
     dt     


243
 Kinematic Influence Coefficient
P P  j


P      Sn Sn   n Sn  0 P  Rn 
   n1

 1 
  
 P P P   2 
  
 N    
 1 2
  
 N 
P  j

 
n n

n 1

Sn S n   n S n  0 P  Rn 

P
if n  n and n  j   g nP Sn ( o P  Rn )
n
if n Sn and n  j g nP Sn
244
 Kinematic Influence Coefficient
① if n  n
  j  
 G  
P
;n   
 n  n1
 n Sn ( P  Rn ) 

 
   1S1 ( P  R1 )    n Sn ( P  Rn )  
 n
S n ( P  Rn )
② if n Sn
P P   j  
    
 n S n Sn  n1
S n S n  Sn

 0
n

245
 Kinematic Influence Coefficient
ex 4)
 x 
P  y  
S ( P  R1 )   2 S 2 ( P  R2 )  3 S3 ( P  R3 )
o
P  R3   1 1
 z 
 
 1 
o
P  R1 o
P  R2  1 
R3
   
P  S1 ( o P  R1 ) S 2 ( o P  R2 ) S3 ( o P  R3 )   2  [GP ]   2 
 3   3 
o
R2    
 g P   g P   g P
R1 0  l C
1 1  1 1 2 2 3 3

R2  l1S1   x    G P  
   1,2 ; 
     
 0  u  y 
    G jk  

   
   
  3; 
 33
 l1C1  l2C12 
R3  l1S1  l2 S12   x 
 P P P
 
    g g g
 0  u  y  34
 
1 2 3
34 

  34
   a1 a2 a3  246
 
 Kinematic Influence Coefficient
ex5)
 1 
 
o P  S1 ( o P  R1 ) S 2 S3 ( o P  R3 )   S 2 
o 
P  R3
 3 
 
o
P  R1 X̂ 2 
1 S1 ( P  R1 )  S 2 S 2   3 S3 ( P  R3 )
o o

Ẑ 2

247
 Kinematic Influence Coefficient
j
w  G    n Sn
jk jk

n 1
where
 S n in n  j n  n
 G  jk

;n
0 otherwise

P  GP  
where
 S n ( P  Rn ) if n  j and n  n
 GP   S n if n  j and n S n
;n 
0 otherwise

 jk  Gjk  
  Gjk    Gjk      Hjk  
  G P    T  H P  
P     
248
Kinematic Influence Coefficient
d
 jk  ( w jk )
dt
d j  
   n S n 
dt  n1 
j j
 
n S n   n S n
n 1 n 1

n 1
 
where Sn   i Si  Sn
 i 1 
j j
  n 1 
  n Sn   n   n Si  Sn
jk

n 1 n 1  i 1 
j j n 1
 
n S n    Si S n in  G      H   
 jk  T
 jk 
n 1 n 1 i 1

249
 Kinematic Influence Coefficient
 Physical meaning of ( Si S n )
note
e (t t )
 n

o e (t )
d
 k
dt

de  d 
 e  k  e  ( k e )  ①
dt  dt 
de de d  de
   ②
dt d dt dt

250
 Kinematic Influence Coefficient
①=②
de
k e Si e
d
dS
Si Sn  n
dSi

[case 1] n  n , i i (all revolute)


Si Si Sn  (Sn )  0
i
i 
n Sn Si  ( Si )  0
Sn i

251
 Kinematic Influence Coefficient
[case 2] n Si , n  n

Si

Si Sn  ( Sn ) 0
di
Si
n
Sn

[case 3] n i , n Sn


Si Sn  ( Sn ) 0
i
i
Si Sn

252
 Kinematic Influence Coefficient
When N joint exist
j .. j n 1 . .
  n S n   ( S i S n )  i  n
jk

n 1 n 1 i 1
j .. j n 1 . .
 g  n   h  i  n
n
jk jk
in
n 1 n 1 i 1

where
 S i S n ; i n  j : n  n , i i

jk
hin  0 ; in : n  n , i i

 0 ; otherwise n S n or i Si

253
 Kinematic Influence Coefficient
.
  dt   ( dt )
 jk
  2
 ( jk )
i
gn 
ik
  
i   n  i n
 . .
n1    i  n
 ( jk )
 . .
 i  n
where

 S i S n ; i n  j : n  n , i i
( ) 
jk
hinjk    0 ; in : n  n , i i
i n 
 0 ; otherwise n S n or i Si

254
 Kinematic Influence Coefficient
.. . .
  G      H  
jk jk T jk

 G jk   g 1jk ...... g njk 

 

Tensor up to N

 g njx   hnjx 
N   .. N N  jy  . .
   g n  n    hn  n i
jk jy

n 1  jz  n 1 i 1  jz 
g
 n   hn 
N .. N N . .
  g n   h n i
jx
n
jx
n
jx
결과가 x 방향으로의 가속도
n 1 n 1 i 1

255
Kinematic Influence Coefficient
 h11jx h12jx  h1jxN 
 jx 
.. . h   .
 G jk     T  21 
1;    
 jx jx 
  hNN   H   1;;
jk
 hN 1

256
 Kinematic Influence Coefficient
.. . .

  G  1;     H  1;; 
jk jk T jk

.. . . 
jk
  G  2;     H  2;; 
jy jk T jk
 

.. . .

  G  3;     H  3;; 
jz jk T jk



 . T  jk  . 
   H   1;;  
.. .  . .  .
 jk  G        H    
jk T T jk

 2;;

 T. .

  H jk

    3;;  

257
 Kinematic Influence Coefficient
 h11jx 
 H   
jk
 jy 
jk
h11  h11 
 jz 
 h11 

hinjk
hinjk
i hinjk 
a  Si a
hin
jk i
n

258
 Kinematic Influence Coefficient

 

. j . .
P  S n S   n S  P  R n 
n n

n 1


 

.. j .. ..
P  S n S   n S  P  R 
n n n
 N N N
 .. . .
  g n  n   hmn  m  n
n 1 p p
j . . j . .
  S n S n    n S n P  R n 
n 1 m 1 n 1

n 1 n 1

j .

d
   n S  P  R n 
n 
n 1 dt 
 GP    T  HP  

259
 Kinematic Influence Coefficient
where
.
. n
S
n . S S n
n
 S m S n ; m n , m  n
S n  n   . 
m 1 m m   0 ; otherwise
m

n  S n
j . 
. n
 S
n .
j
 ..
.
 Sn S  Sn  
n
n   m
n 1 n 1  m1 m 
N N

 S
. .
 S S
m n
n m
n 1 m 1
N N . .
 h n  m
p
mn
n 1 m 1

260
 Kinematic Influence Coefficient
where
 S m S n ; m  n  j , m  m , n Sn
p 
h mn  0 ; n m , m  m , n Sn
 0 ; all m, n  S ,  S
 m m n n

n  n
j j n 1 .
     S 
. .
  n S P  R  P  R n 
n n m n
n m S
n 1 n 1 m 1

j n

 P  R 
. .
  n  m S S
m n n
(1)
n 1 m 1

261
 Kinematic Influence Coefficient
n m

d j

 P R
n

 
.
P  R 
n
m
dt m 1 m
j j n

   
. . .
    m S  P  R
m m m m n
 Sm S  m S  P  R
m n 1 m n 1 m 1

n
P R  P R  m
  R m
 R
n


262
Kinematic Influence Coefficient
 
m

S  P R
n
 
 m
 m
P R  
 m
m n m
 
R  R S  P  R 
m

m

S  P R
n
 m

S  R  R
m n

 

 n
 m
P R  
 m
m n
R  R P  
 P  R 
m m
S

263
 Kinematic Influence Coefficient
m
P R

 P  R di
n
P n om
P R

m
R m n
R  R
n
n
P R
o R on
d
j .
n d
  n S  P  R
n
  dt
 n
 n



 . . n
P  R   P  R   P  R 


n 1 dt
j
 j . .  j . n n .
   
. .
  n S   Sm S   
n m m m n
m
 m S  P  R   n S   m S  P  R
n 1  m n 1  n 1 m 1
264
Kinematic Influence Coefficient
i j
mn    S ( S
n 1 m n 1
n n
n
S m ) (2)
i j
mn   n m S n ( S m ( P  R m )) (3)
n 1 m n 1
i j
m n   n m S n ( S m ( P  R n )) (4)
n 1 m 1

  P 
N N

 P  g 
n n    h P
 
mn m n 
 n 1 m 1 n 1

 P   P    
  P
 hmn        
 m  n  m  n  

265
 Kinematic Influence Coefficient
n  n
First n  n , m Sm Only term 2
P
h mn S n S m ; n  m  j , m Sm , n  n
"θ before S"

[Note] n  n ,m  m terms 1.3.4

 n  m  j only term 3.
P
h mn S n ( S m ( P  R m ))

266
 Kinematic Influence Coefficient

  P 
N N

 P  g 
n n   hmnmn
P

 n 1 m 1 n 1

 P   P    P   
 hmn        
   m    n    m   n  

P
h mn  S m  ( S n ( P  R n )) ; m  n  j m  m , n  n

 S n  ( S m ( P  R m )) ; n  m  j m  m , n  n

 S m S n ; m  n  j m  m , n Sn

 S n S m ; n  m  j m S m , n  n
 0 ;otherwise

267
 Kinematic Influence Coefficient
  P  
P
ex) h 
13    
 S 3
( P  R 3
) 
 S 1

 S 3
( P  R 3
) 
1  3  1
P  R1 ( P  R 3 )  ( R3  R1 )

  P 
P
h  
31   S 3

 S 1
( P  R1
) 
  S 1

 S 3
( P  R 3
) 
3  1 

268
 Kinematic Influence Coefficient
 [G P ] T [ H P ]
P  

Every plane of [ H P ] is symmetric.

Px Px P  0
h h [ H ]1;;
31 13  
0 

 jk [Gjk ]  T [ Hjk ]

 Si Sn
jk
; i nj
h 
in
 0 ; otherwise

269
 Kinematic Influence Coefficient
Inverse Kinematics
Assume u  R 6 ,   R 6 , [Gu ]  R 66
[Gu ] is invertible.
Dependent parameter
 [Gu ] 1 u [Gu ] u
Independent parameter

[Gu ]  u   T [ Hu ] 

[Gu ] u u T [ H uu

]u

270
 Kinematic Influence Coefficient
where generalized scalar dot product

[Huu ]  [Gu ]T [Gu ]  [ Hu ][Gu ]

Inverse known inverse

Let [Gu ]T   g1    g x g y g z g x g y g z 
   
 g2    
g   
 3  
    
 T [ H u ]1;;   plane by plane
 T [ H u ] =
  T 

  [ H  ]2;;  
u multiplication
 
  
 T [ H u ]  
  6;;  271
 Kinematic Influence Coefficient

 g x g y g z g x g y g z   T [ H u ]1;;  

 T 
  [ H  ]2;; 
u

 
  
 T [ H u ]  
scalar   6;; 
x
 g x
T
[ H ]1;;   g y  T [ H ]2;;   

272
 Kinematic Influence Coefficient
  g x  H  1;;  g y  H  2;;   
T

 
 g h g1  h12 
 1 11
g1  H  g 1  h 21 
 
  
g x g y g z   g 1  h ij  66

hi j

h11
H 
i

273
 Kinematic Influence Coefficient
General form
 Gu  
T
 H  u  

 
T
  Gu    H  u   
6 6 6

 g   H  u  
 1  
 Gu     H  u   g 2   H  u  
 
  
 
  Gu   u 이므로

 G 
T
=u  Gu   
   H  u   Gu   u
T
u

274
 Kinematic Influence Coefficient
E A  C

row vector : scaling effect

N
E pmg  Apn Cnmq
plane
n 1

q q
 p  m
m
n  column
i  th plane n  plane

비교 ) u  J 
u  J  J 
  J  1 u  J  1 JJ
 1 u

275
 Summary
Example : Unimate Puma 6-DOF manipulator with 6 revolute joints
u  Gu    Hu  
T

where 3 6 3 6 6

  G  p
  H   p Rn  O pNORG
   
 G  
u
,  H  
u

  G  
jk
  Hjk   sn  0 Zˆ n  n0 R n Zˆ n
6 6   6 6 6  
3 6 3 6 6

PP  jk
g P
g P
g g P
g P
g P P
 g 
i gi jk
 
 G  
u 1
jk
2
jk
3
jk
4
jk
5
jk
6
jk  i i
 g 1 g 2 g g 3 g 4 g 5 6 
 s1 ( P  R1 ) s2 ( P  R2 ) s3 ( P  R3 ) s4 ( P  R4 ) s5 ( P  R5 ) s6 ( P  R6 ) 
 
 s1 s2 s3 s4 s5 s6 

276
 Summary
2 P
 H    s1 { s1 ( P  R1 )} h  2
p p
11
 H p   s2 { s2 ( P  R2 )}
;1;1 1 ;2;2

 H p   s1 { s2 ( P  R2 )}  Hp   H p   s2 { s3 ( P  R3 )}  Hp 

;1;2 ;2;1 ;2;3 ;3;2

 H p   s1 { s3 ( P  R3 )}  Hp   H p   s2 { s4 ( P  R4 )}  H p 

;1;3 ;3;1 ;2;4 ;4;2

 H    s1 { s4 ( P  R4 )}  H  
p p  H p   s2 { s5 ( P  R5 )}  H p 
;2;5 ;5;2
;1;4 ;4;1

 H    s1 { s5 ( P  R5 )}  H  
p p  H p   s2 { s6 ( P  R6 )}  H p 
;2;6 ;6;2
;1;5 ;5;1

 H p   s1 { s6 ( P  R6 )}  H p   H p   s3 { s3 ( P  R3 )}

;1;6 ;6;1 ;3;3

  P   H p   s3 { s4 ( P  R4 )}  H p 

 H   hi j  
p p
 ;3;4 ;4;3
;i ; j i   j 
 H p   s3 { s5 ( P  R5 )}  H p 
;3;5 ;5;3
 si { s j ( P  R j )}
 H p   s3 { s6 ( P  R6 )}  H p 
;3;6 ;6;3
277
 Summary
 H p   s4 { s4 ( P  R4 )}  H jk   s1 s2  H jk   s1 s3
;4;4 ;1;2 ;1;3

 H p   s4 { s5 ( P  R5 )}  H p   H jk   s1 s4  H jk   s1 s5

;4;5 ;5;4 ;1;4 ;1;5

 H p   s4 { s6 ( P  R6 )}  H p   H jk   s1 s6  H jk   s2 s3

;4;6 ;6;4 ;1;6 ;2;3

 H    s5 { s5 ( P  R5 )}  H  
p p 
 H jk

  ;2;4  s2  s 
4  H jk

  s2 s5
;2;5
;5;5 ;5;5

 H p   s5 { s6 ( P  R6 )}  H p   H jk   s2 s6  H jk   s3 s4

;5;6 ;6;5 ;2;6 ;3;4

 H    s6 { s6 ( P  R6 )}
p 
 H jk

  ;3;5  s3 s 
5  H jk

  ;3;6  s3 s6
;6;6

 H 
jk

  s4  s5

 H jk

  s4 s6
   
jk ;4;5 ;4;6
 H  
jk jk
hm n      H jk   s5 s6
;m ;n m  n  ;5;6

 sm sn  H jk   si si 0, (i 1 6)

;i ;i

 H jk   sm sn 0, ( m  n) 278

;m ;n
 Summary


p
h
mn 
m
 
g np  sm g np hmpn  ( sm & g np )

sm
g np
hmpn

279
Chapter 6. 로봇 동역학
Chapter 6. Robot Dynamics
6.1 Newton-Euler Equation
6.2 Recursive Newton-Euler Equation
6.3 Closed-form Dynamic Model
6.4 Example
6.5 G&H Based Dynamic Modeling

281
 Preview of Dynamics
 Newtonian approach
• 1st law :Momentum conservation
• 2nd law : F ma,   I 
• 3rd law :Action/Reaction

282
 Preview of Dynamics
• Particle : No area, No volume
• Planar rigid-body : 3DOF (2T, 1R)
• Spatial rigid-body : 6DOF (3T, 3R)
• Flexible body

Motion 2D 3D

Translation F ma F ma

Rotation   I     I      I   

283
 Preview of Dynamics
 Fundamental theory
• Coordinate system
- Rectangular coordinate (Cartesian coordinate)
- Tangential and normal component coordinate
- Polar coordinate (Central force problem)
• Kinematics
- Position
- Velocity
- Acceleration

284
 Preview of Dynamics
 Rectangular coordinate (Cartesian coordinate)
• Projectile motion

ax 0, a y  g , az 0
Y
g

X
Z

285
 Preview of Dynamics
 Tangential and normal component coordinate
et
en
en
et
v vet d et d
 e n  e n  et
dt dt
dv dv d et dv
a   et  v a  et  v e n
dt dt dt dt
If horizontal motion,  0 then a dv et et
dt
286
en
 Preview of Dynamics
 Polar coordinate (Central force problem)
e
er
r

r re r
 r  re r re r  r e
v re e r  e r  e
re r  re r  r
a   r e  r e
e   e  k e   e r
a  er  2r  r e
r  r
2

centripetal acceleration
287
 Preview of Dynamics
 Planar motion of rigid-body
Translational motion Rotational motion
vA vA
A
vB l
y  rA / B l j
v A // v B x B
v A  vB
d (r A/ B ) d (lj )
vA   l  (  k ) ( j ) l i
dt dt
 rA/ B
288
 Preview of Dynamics
 Planar motion of rigid-body
• General motion
: Translational motion + Rotational motion
A vA
vA
l
y 
vB
x B

vA v A v B  v A / B
vB v B   rA / B
289
 Preview of Dynamics
 Momentum
• Linear momentum (1D, 2D, 3D)
L mv
• Angular momentum (C: center of mass)
' '
mi r i rc  r i  v i v c  v i
'
ri y N

We know that  mi r i 0
'
ri z C i 1
z 
xx
rc N

y H o  r i mi v i  N   r i v i dmi
x i 1

H o r c mv C   H C If particle H C 0
290
 Preview of Dynamics
 Angular momentum
vi mi m
r i r i r c  r i
ri  v i v c  vi
C
z
rc
y
x N N
H o  r i mi v i   r c  r i mi v c  vi 
i 1 i 1
0 0
 mi r c v c  r c  mi vi    mi r i v c   r i vi mi
r c mv c    r i mi vi 
291
 Preview of Dynamics
 Angular momentum
H o r c mv c    r i mi vi  vi  r i
mi  r i  r i 
mi   r i r i  
H o r c mv c    mi r i 
2
 mi  ri2  


 i i  dm J
2
m r   r 2

i 1

H o r c mv c   J 
r c mv c   J  k r c mv c   H G

292
 Preview of Dynamics
 Rate Change of Angular Momentum
dHo
rc mv c   r c mv c   J 
dt
M o r c F  J 
H o r c mv c   J 
dL
L mv G , ma G
dt
Translation : F ma G
Rotation : M o  J   r c F
M o  J  with F 0

293
 6.1 Newton-Euler Equation
 Acceleration of a rigid body
B B
B  d B
VQ (t   t )  VQ (t )
VQ  VQ lim ; linear acceleration vector
dt t  0 t
A A
A  d A  (t   t )   B (t ) ;angular acceleration vector
B   B lim B
dt t  0 t
v A  UVAORG ( w.r.t. reference coordinate )


 A  U  ( w.r.t. reference coordinate )
A

294
 Newton-Euler Equation
d A
 Linear Acceleration dt  
Q  BA R v Q   B  BA R BQ 
B A

• When the origin of {A} is coincident to that of {B} :

A d A B
VQ  ( B R Q )  BA R BVQ  A  B BA R BQ
dt
d d
VQ  ( BA R BVQ )  A  B BA R BQ  A  B  ( BA R BQ)
A

dt dt
 BA R BVQ  A  B BA R BVQ  A  B BA R BQ  A  B  BA R BVQ  A  B BA R BQ 
 BA R BVQ  2 A  B BA R BVQ  A  B BA R BQ  A  B  A  B BA R BQ 

a  er  2r  r e

r  r
2

295
 Review
 Example (2DOF Manipulator)
Yˆ A  r  cos  sin  0
Yˆ B Q  0  ,  sin  0
B A
R  cos 
  B  
 0   0 0 1 
(slider 의 center)
Q A B
Q  BA R Q
r
Xˆ B  cos  sin  0   r   r cos  
 sin  cos  0   0   r sin  
  Ẑ  r sin 
Xˆ A     
 0 0 1   0   0 
r cos 

d
dt
 Q 
A A B A
v Q  BA R vQ   B  Q
A

296
 Review
 r   
d d 
 
B 
Q r X B  B  Z B B
Q r X B  r ( X B )
v Q  0 
B
  dt dt
 0   
r X B  r ( B X B )
B
 r 
 
2  
r X  r Y   r 
d B d
 
 
 B B
 
Q   
r X  r Y 
dt 2 dt 
B B
  0 
 d    d 
r X B  r  X B   r  r Y B  r  Y B 

dt   dt  
B
r  r 2 
 
 
 

r  r  X B  (r  2r )Y B   r  2r 
2

 0 
 
297
 Example
Yˆ A
Yˆ B

Q (slider 의 center)

r
Xˆ B
  Ẑ  r sin 
Xˆ A
r cos 

V
A
Q  A B
B R VQ  2 A
 B  A B
B R VQ  A
 B  A B
B R Q  A
 B  A
 B  B R Q
A B

 
r
 BA R  0   2
 
 0
 
298
 Newton-Euler Equation
 Linear Acceleration
• If the linear acceleration of the origin of {B} exists

V
A
Q  A B
B R VQ  2 A
 B  A B
B R VQ  A
 B  A B
B R Q  A
 B  A
 B  B R Q
A B

B
Q is constant
B
VQ  BVQ 0

A
V
Q  A
VQ  A
VBORG

299
 Newton-Euler Equation
 Angular acceleration
• {B} is rotating relative to {A} with A  B
• {C} is rotating relative to {B} with B C

A
C  A  B  BA R B C
A   A   A R B   A   A R B 
 C B B C B B C

300
 Newton-Euler Equation
 Angular momentum
vi mi m
r i

ri
G H G  (r i v imi )
z
rc  (r i ( r i)mi )
y
x
Consider G as a revolute joint

H x  [ yi ( r i) z  zi ( r i) y ]mi

 [ yi ( x yi   y xi )  zi (z xi  x zi )]mi
301
 Newton-Euler Equation
H x  [ yi ( x yi   y xi )  zi (z xi  x zi )]mi
 Angular momentum
H x  x ( y 2  z 2 ) dm   y xydm  z zx dm
I x x  I xy y  I zxz (1)

H y   x xy dm   y ( z 2  x 2 )dm  z yz dm

 I yx x  I y y  I yzz (2)

H z  I zx x  I zy y  I zz (3)

combining (1), (2), and (3) result in

H G [ I ]  Ix  I xy  I xz 
 
[ I ]   I yx Iy  I yz 
  I zx  I zy I z  302
 Newton-Euler Equation
 Angular momentum
I  0 0 
 Ix  I xy  I xz   x 
  [ I ]  0 I y  0 
[ I ]   I yx Iy  I yz 
 
  I zx  I zy I z   0 0 I z 
 
Symmetric, positive definite Principal moment of inertia

H G [ I ] H G  I x x i  I y  y j  I z  z k

303
 Newton-Euler Equation
 Mass distribution
mass moment of inertia
I xx ( y 2  z 2 )  dv
V

I yy ( x 2  z 2 )  dv
V

I zz ( x 2  y 2 )  dv
V

mass product of inertia

 I xx  I xy  I xz 
A   I xy  xy  dv
I   I xy I yy  I yz  V

  I xz  I yz I zz  I yz  yz  dv
V

Inertia tensor I zx  zx  dv 304

V
 Newton-Euler Equation
 Example
h l w
I xx  ( y 2  z 2 ) dxdydz
0 0 0
h l
( y 2  z 2 ) w dydz
0 0

m 2
 (l  h 2 ), m  lwh
3
h l w
I xy  xy dxdydz
0 0 0

w2 h m l
 y dydz  wl
0 0 2 4
305
 Newton-Euler Equation
 Inertia Tensor
m 2 m 2 m 2
I xx  (l  h ), I yy  ( w  h ), I zz  (l  w2 )
2 2

3 3 3
m m m
I xy  wl , I xz  hw, I yz  hl
4 4 4

m 2 2 m m 
3 (l  h )  wl  hw 
4 4
 
 m m 2 m
A
I   wl (w  h2 )  hl 
 4 3 4 
 m m m 2 
  hw  hl (l  w ) 
2

 4 4 3 
306
 Newton-Euler Equation
 Parallel axiom theorem
 r2 0 0
A  
I zz  C I zz  m( xc2  yc2 ) PcT Pc I 3  0 r2 0  , r 2 xc2  yc2  zc2
0 0 r 2 
A 
I xy  C I xy  mxc yc  xc2 xc yc xc zc 
 xc 
 
A Pc PcT  yc   xc yc zc   yc xc yc2 yc zc 
I  C I  m[ PcT Pc I 3  Pc PcT ]  
 zc   zc xc zc yc zc2 
 

 I xx  I xy  I xz   I xx 0 0
A    0 I 
I   I xy I yy  I yz  0
 yy 
  I xz  I yz I zz   0 0 I zz 
* Principle moment of inertia
307
 Newton-Euler Equation
 Example
C m m
 xc   w I xx  (h 2  l 2 ), C
I yy  ( w2  h 2 ),
 y  1  l  12 12
 c 2  C m 2
 zc   h  I zz  (l  w2 ), C
I xy 0
12
m 2 2 
 12 ( h  l ) 0 0 
 
C  m 2 
I 0 (w  h2 ) 0
 12 
 m 2 
2
 0 0 (l  w ) 
 12 
308
 Newton-Euler Equation
 Moment of inertia
• Thin
Slender
rectangular
rod plate
1 1
I y I z  mL2 I x  m b 2  c 2 
12 12
1 1
I y  mc , I z  mb 2
2

12 12

• ISphere
 I  I 
2 2
ma
x y z
5

309
 Mass Moment of Inertia
 Slender bar 2
dI y dI z dm x
x
dx  dVx  r x dx
2 2 2

l
dm  r 2 dx 2

 r
2 2
  I y I z  x dx
0 0 0 
   l
2
ml 2
C
I  0 0 
 12  1 1

0

ml 2    r l  ml
2 3 2


0
12  12 12
310
 Mass Moment of Inertia
 Disk
y dm  dV  trd dr

dr
d 2
r dI z dm r
R 
x  tr d dr
3

R 2

z I z  tr d dr   tR
3 3

0 0
2
1
 mR 2 311
2
 Mass Moment of Inertia
 Disk
y dI x dm (r sin  ) 2
 tr 3 sin 2  d dr
dr
d
r
R 
x R 
r sin  I x I y  tr 3 sin 2  d dr
0 
z 1 2
 mR
4
312
 Newton-Euler Equation
• Thin
Rectangular
disk prism
1
I x  m b 2  c 2  1
12 I x  mR 2
1
2
I y  m a 2  c 2  1
12 I y I z  mR 2
1 4
I z  m a 2  b 2 
12

313
 Newton-Euler Equation
 Circular cone
cylinder
1 3
I x  ma 2 I x  ma 2
2 10
1 3 1 
I y I z  m 3a 2  L2  I y I y  m  a 2  h 2 
12 5 4 

314
 Newton-Euler Equation
 Euler equation
H G [ I ] : angular momentum about the center of mass

HG ( H G )Gxyz   H G  M G
M CX I x x  ( I y  I z ) y z
M CY I y y  ( I z  I x ) z x
M CZ I z z  ( I x  I y )x y
In text book,
N i  Ci I  i  i Ci I i ( M Ci )

( H G )Gxyz  HG 315
 Newton-Euler Equation
 Newton’s Equation
N
Fi mv Ci ( Fn i ) : Newton' 2nd Law
n 1
F2
v Ci

F1
m Ci

FN

F3

316
 Newton-Euler Equation
 Newton’s equation, Euler’s equation

F m C
N  C I    C I 
: w.r.t moving coordinate
C
I inertia tensor of the body written in a
frame {C} whose origin is located at
the center of mass.
N i  Ci I  i  i Ci I i ( M Ci ) 317
 6.2 Recursive Newton-Euler
Equation
 Known: , , 
 Outward iterations to compute velocities and
accelerations
i 1  ii1R ii  i 1 i 1Zˆi 1
i 1

i 1  ii1R ii  i i1R ii 

i 1
i 1
i 1 ˆ
Z i 1   i 1
i 1 ˆ
Z i 1

If joint i+1 prismatic, i 1i 1  i i1R ii

The linear acceleration of each link frame origin
i 1
vi 1  i i1R  i vi  i i i Pi 1  ii  ii i Pi 1 

318
Recursive Newton-Euler Equa-
tion
for prismatic joint i+1
i 1
vi 1  i i1R  i vi  i i i Pi 1  ii  ii i Pi 1 

 2 i 1i 1 di 1 i 1Zˆi 1  di 1 i 1Zˆi 1

The linear acceleration of the center of mass
i
vCi  i vi  i i i PCi  ii ( ii i PCi )

0
0  0 0 0

319
Recursive Newton-Euler Equa-
tion
 Inward iteration to compute forces and torques

Force and torque acting on a link

 ni 1

Fi  f i 1

i
Pi 1  i PCi
i
PCi

Fi mvCi
N i  Ci I  i  i Ci I i 320
Recursive Newton-Euler Equa-
tion
For i-th link,
i
Fi  i fi  i 1i R i 1 fi 1
i
N i  i ni  i ni 1  ( i PCi ) i fi  ( i Pi 1  i PCi ) ( i fi 1 )
 i ni  i 1
i
R i 1ni 1  i PCi i Fi  i Pi 1 i 1i R i 1 fi 1
i
fi  i 1i R i 1 fi 1  i Fi
i
ni  i N i  i 1i R i 1ni 1  i PCi i Fi  i Pi 1 i 1i R i 1 fi 1  ni 1

Fi  f i 1

i
Pi 1  i PCi
i
PCi 321
Recursive Newton-Euler Equa-
tion
 i  i niT i Zˆi for revolute joint
 i  i fiT i Zˆi for prismatic joint

N 1
f N 1 & N 1nN 1 exist only when the robot is contacting
the environment.

322
Recursive Newton-Euler Equa-
tion
 Level 1:Outward iteration: i=0→5
i 1  ii1R ii  i 1 i 1Zˆi 1
i 1

i 1  ii1R ii  i i1R ii 

i 1
i 1
i 1 ˆ
Z i 1   i 1
i 1 ˆ
Z i 1
vi 1  i i1R  i i i Pi 1  ii ( ii i Pi 1 )  i vi 
i 1

vCi 1  i 1 i 1 i 1 PCi 1  i 1i 1 ( i 1i 1 i 1 PCi 1 )  i 1 vi 1

i 1

i 1
Fi 1 mi 1 i 1vCi 1
i 1
N i 1  Ci 1 I i 1 i 1 i 1  i 1i 1 Ci 1 Ii 1 i 1i 1

323
Recursive Newton-Euler Equa-
tion
 Level 2 : Inward iterations : i=6→1
i
f i  i 1i R i 1 fi 1  i Fi
i
ni  i N i  i 1i R i 1ni 1  i PCi i Fi  i Pi 1 i 1i R i 1 fi 1

 i  i niT i Zˆi or  i  i fiT i Zˆi

Inclusion of gravity forces in the dynamic algorithm

Let v 0 = G , where G = 9.81 m/s2
0

fictitious upward acceleration ≡ the gravity effect

324
 Gravity Effect
 Elevator

a
ma y  N  mg
g  mg j N m(a  g )
m m a
This implies that
“m” gets acceleration
N j
upward.

325
 6.3 Closed-form Dynamic
Model
 Usage  V (, )  G ()
 M (  ) 
• Obtain some insight to the structure of the equa-
tions
• It will usually be more efficient than iterative form
• Can be simply applied to various algorithm with
inertia weighting

326
Closed-form Dynamic Model
 Two link model with point mass
1
PC1 L1 Xˆ 1
2
P L Xˆ
C2 2 2
C1
I1 0
Yˆ0 C2
I 2 0
f 3 0
n3 0
0 0
X̂ 0  0 0
0
v  gYˆ
0 0

327
Closed-form Dynamic Model
 Outward iteration for link 1
 0  C1 S1 0   0   gS1 
1
1 1 1Zˆ1  0  1
v1  01R ( 0 v0 )   S1 C1 0   g   gC1 
      
 1   0 0 1   0   0 
i 1
i 1  ii1R ii  i 1 i 1Zˆi 1
i 1  ii1R ii  i i1R ii 
i 1
i 1
i 1 ˆ
Z i 1   i 1
i 1 ˆ
Z i 1
vi 1  i i1R  i i i Pi 1  ii ( ii i Pi 1 )  i vi 
i 1

vCi 1  i 1 i 1 i 1 PCi 1  i 1i 1 ( i 1i 1 i 1 PCi 1 )  i 1 vi 1

i 1

328
  0    L112   gS1    L112  gS1 
 0 1  L    0    gC   L   gC 

v 
1
 1 1 1Zˆ1  0  C1  1 1    1  1 1 1 
   0   0   0   0 

 1  
  m1 L112  m1 gS1   0
  1  
1
F1  m1 L11  m1 gC1  N1  0 
 0   0 
 
i 1
i 1  ii1R ii  i i1R ii i 1
i 1 ˆ
Z i 1   i 1
i 1 ˆ
Z i 1
vCi 1  i 1 i 1 i 1 PCi 1  i 1i 1 ( i 1i 1 i 1 PCi 1 )  i 1 vi 1
i 1

i 1
Fi 1 mi 1 i 1vCi 1
i 1
N i 1  Ci 1 I i 1 i 1 i 1  i 1i 1 Ci 1 Ii 1 i 1i 1 329
Closed-form Dynamic Model
 Outward iteration for link 2
    2

 0   C2 S2 0  L S
1 1 2 L 1 1 C2  gS12
  2
v2   S 2 C 0  1v  L C  L  2 S  gC 
2
2  0  2  C1  1 1 2 1 1 2 12 
 
 1   2   0 0 1  
 0 

 0     L2 (1   2 ) 
2
 0
2
 2  0  2 vC2  L2 (    2
   1 2 )   0   v2
 
1   2   0   0 

 m2 L1S
1 2  m L 
2 1 1
2
C 2  m 2 gS 12  m L
2 2 (1   2 ) 2
  0
   2 N  0 
2
F2  m2 L11C2  m2 L11 S 2  m2 gC12  m2 L2 (1   2 ) 
2
2  
 0   0 
 
330
Closed-form Dynamic Model
 Inward iteration for link 2
2
f 2  2 F2  32 R 3 f3  2 F2
2
n2  2 N 2  32 R 3 n3  2 PC2 2 F2  2 P3 32 R 3 f 3  2 PC2 2 F2
 0 
 0 
 
 m2 L1 L2C2
1  m L L S 
2 1 2 2 1
2
 m L
2 2 gC12  m2 2 (1   2 ) 
L 2

331
Closed-form Dynamic Model
 Inward iteration for link 1
1
f1  21R 2 f 2  1F1
 C2  S2 0   m2 L1
S
1 2  m L 
2 1 1
2
C 2  m 2 gS12  m L
2 2 ( 1   2 ) 2

 
 S 2 C2 0   m2 L1
C  m L  2
S  m gC  m L (   )
2 
  1 2 2 1 1 2 2 12 2 2 1

 0 0 1   0 

  m1 L112  m1 gS1 
 
  m1 L11  m1 gC1 
 0 
 

332
 Closed-form Dynamic Model
 Inward iteration for link 1
1
n1 1 N1  21R 2 n2  1PC1 1 F1  1 P2 21 R 2 f 2
 0 
 0 
 

 m2 L1 L2C21  m2 L1 L2 S21  m2 L2 gC12  m2 L2 (1  2 ) 
2 2

 0 
 0 
 
2 
 m1 L1 1  m1 L1 gC1 
 0 
 0 
 
 m2 L1 1  m2 L1 L2 S2 (1   2 )  m2 L1 gS2 S12  m2 L1 L2 C2 (1  2 )  m2 L1 gC2C12 
2  2

333
Closed-form Dynamic Model
 Joint torque for closed-form model
 1 1n1T 1Zˆ1
m2 L2 2 (
1   2 )  m L L C
2 1 2 2 (21   2 )  ( m1  m2 ) L1 1
2

 m L L S 2  2m L L S    m L gC  (m  m ) L gC
2 1 2 2 2 2 1 2 2 1 2 2 2 12 1 2 1 1

 2 m2 L1 L2C2
1  m L L S 
2 1 2 2 1
2
 m L
2 2 gC12  m2 2 (1  2 )
L 2

334
 Closed-form Dynamic Model
 Structure of manipulator dynamic equation
 M (  )   V (, )  G ()
M () : n n mass matrix
 ) : n 1 vector of centrifugal and Coriolis terms
V (, 
G () : n 1 vector of gravity terms

 L2 2 m2  2 L1 L2 m2C2  L12 (m1  m2 ) L2 2 m2  L1 L2 m2C2 

M (  )  2 2 
 L2 m2  L L m C
1 2 2 2 L2 m2 
  m L L S 2  2m L L S     m2 L2 gC12  (m1  m2 ) L1 gC1 

 G (  ) 
2 1 2 2 2 2 1 2 2 1 2
V (, )  
m L L S  2
 m L gC 
 2 1 2 2 1  2 2 12

335
Closed-form Dynamic Model
 Home work #1
• Derive the dynamic equation for the two links ma-
nipulator having two revolute joints
• Each link is modeled as a slender bar of homoge-
neous density and has dimensions li , mi
• Employ iterative Newton-Euler method

336
Closed-form Dynamic Model
 Example 1 1 ˆ 2 1 ˆ
PC1  l1 X 1 PC2  l2 X 2
2 2
l2 f 3 0 n3 0 0 0  0 0
m2 2 0
0v  gYˆ
0

0 0 0 
C1
I1  0 m1l12 /12 0 
m1 l1  
2
 0 0 m1l1 /12 
1
0 0 0 
C2
I 2  0 m2l22 /12 0 
 
2
 0 0 m2l2 /12 
337
Closed-form Dynamic Model
 Solution
1 2 
1 1
 1  m1l1 1  m2l2 1   2   m2 l1l2 c2 1  2   m2 l121
2

3 3 2
1 1 1
 m2l1l2 c21  m2l1l2 s21  m2l1l2 s1 1  2 
2
 2

2 2 2
1 1 
 m2l2 gc12   m1  m2  l1 gc1
2 2 
1 1 1 1
 2  m2l2 1   2   m2l1l2 s21  m2 l1l2 c21  m2l2 gc12
2  2

3 2 2 2

338
Closed-form Dynamic Model
 Inverse Dynamic Model

 V (, )  G ()

 M (  ) 
1 2 2 1 1 2 1 
l m  l l m c
3 2 2 12 2 2 1 3 1  l ( m  m2 ) l2 m2  l l m c
1 2 2 2
3 2
M (  )  
 1 1 1 
l2 2 m2  l1l2 m2 c2 l2 2 m2
 3 2 3 

 1 2  m l l s    1 1 
 m l l s  m l gc 
 2 2 2 12 2 1( m  m )l gc1
 2 212 2 2 2 1 2 2 1 2 2 1
 ) 
V (,   G (  )  
 1   1 
m2 L1 L2 s212 m2l2 gc12
 2   2 
339
Closed-form Dynamic Model
 Lagrangian formulation
• Newton-Euler : force balance approach
• Lagrangian : energy-based approach
• Kinetic energy
1 1 i T Ci i
For i link : ki  mi vCi vCi  i I i i
T
th
2 n 2
Total kinetic energy : k  ki
i 1
1 T
kinetic energy for manipulator: k ,     M  

2

340
Closed-form Dynamic Model
• Potential energy
0 T 0
For ith link : ui  mi g PCi  urefi
n
Total potential energy : u  ui u  
i 1

• Lagrangian of a manipulator
L , 
  k ,    u  
• Motion equation for the manipulator
d L L d k k u
    

dt   dt   

341
Closed-form Dynamic Model
 Example for Lagrangian formulation
• RP manipulator
 I xx1 0 0 
C1
I1  0 I yy1 0 
 
 0 0 I zz1 
 I xx 2 0 0 
C2
I 2  0 I yy 2 0 
 
 0 0 I zz 2 

342
Closed-form Dynamic Model
• Kinetic and potential energy
1 2 2 1
k1  m1l1 1  I zz112
2 2
1 1
k2  m2 d 2212  d 22   I zz 2 22
2 2
  1 m1l12  I zz1  I zz 2  m2 d 22 12  1 m2 d 22
k , 
2 2
u1 m1l1 g sin 1   m1l1 g
u2 m2 gd 2 sin 1   m2 gd 2 max
u    g m1l1  m2 d 2 sin 1   m1l1 g  m2 gd 2max
343
Closed-form Dynamic Model

k  m1l1  I zz1  I zz 2  m2 d 2 1  k  0 

2 2

  ,  2
 
 m2 d 2   m d 
 2 2 1

u  g m1l1  m2 d 2 cos 1 
 
  gm2 sin 1  
d k k u
  

dt   

 1 m1l12  I zz1  I zz 2  m2 d 22 1  2m2 d21d 2

 m1l1  m2 d 2  g cos 1 
 2 m2 d2  m2 d 212  m2 g sin 1 
344
Closed-form Dynamic Model
 Inverse Dynamic Model
 V ,    G  
 M  

 m1l12  I zz1  I zz 2  m2 d 22 0
M    
 0 m2 
 2 m d 1d 2 
V , 
 

2 2
2 
  m d  
2 2 1 

 m1l1  m2 d 2  g cos 1 

G    
 m 2 g sin  1  
345
Closed-form Dynamic Model
 Cartesian space equation
 )  G ()
F M x ()   Vx (,  x

M x () : n n Cartesian mass matrix

 ): n 1 vector of velocity terms in Cartesian space
Vx (, 
Gx () : n 1 vector of gravity terms in Cartesian space

  J T () F : Virtual work principle

 J  T V (, )  J  T G ()
J  T   J  T M ( ) 
substitution
, d  J  1   J  1 J 
  J    J
  J 
dt
Cartesian accelera- Joint acceleration
tion 346
Closed-form Dynamic Model
 Summary of Cartesian space equation

 )  G ()
F M x ()   Vx (,  x

where

M x ()  J  T () M ( ) J  1 ( )

V (,  )  J  T (V (, )  M () J  1 () J ())
x

Gx ()  J  T ()G ()

347
 Dynamic simulation
 Inverse dynamics
 M   V  ,   G  
 Forward dynamics
  
M  1     V  ,   G  
 Solving ordinary differential equation
• Euler’s method
• 4th order Runge-Kutta method

  

348
 6.4 Dynamic simulation
 Block Diagram
uact u act u
act
Trajectory
Trajectory 생성
생성 Compare
Compare

udes u des u
des
Forward
Forward
Inverse
Inverse Kinematics
Kinematics
Kinematics
 act act
Kinematics

 des 
des 
des

act
Real System
Inverse
Inverse
Dynamics
Dynamics Forward
Forward
Computing tf Dynamics
Dynamics
Integration
Integration

Torque
349
 Dynamic simulation
 Numerical method
• Solving method for ordinary differential equation
• New value=old value +slopeⅹstep size
• yi1  yi   h
y

yi 1  yi   h
Slope= 

xi xi 1 x
Step size = h 350
 Dynamic simulation
 Euler’s method
  f  xi , yi : slope
yi 1  yi  f  xi , yi h

Predicted
error
True

xi xi 1 x
351
 Dynamic simulation
 4th order Runge-Kutta method
1
yi 1  yi  (k1  2k 2  2k3  k 4 )h
6
k1  f ( xi , yi )
h h
k2  f ( xi  , yi  k1 )
2 2
h h
k3  f ( xi  , yi  k2 )
2 2
k4  f ( xi  h, yi  k3 h)

352
 Dynamic simulation
 Simple example 1

y ( x) sin( x) k1  f ( xi ) sin( xi )
y ( x)  cos( x) h
k2  f ( xi  ) sin( xi  )
h
2 2
h h
k3  f ( xi  ) sin( xi  )
2 2
k4  f ( xi  h) sin( xi  h)

353
 Dynamic simulation
 Simple example 2
y ( x) 3 y ( x), y (0) 2
y ( x) 2e3 x
k1  f ( xi , yi ) 3 yi
h h h
k2  f ( xi  , yi  k1 ) 3( yi  k1 )
2 2 2
h h h
k3  f ( xi  , yi  k 2 ) 3( yi  k 2 )
2 2 2
k4  f ( xi  h, yi  k3h) 3( yi  k3h)

354
 Dynamic simulation
 Pendulum example
c J
 Given Value
Mass = 5 Kg.
Damping Coefficient = 0.8
L
Length : 0.5 m
Sampling time = 0.01 sec
Initial time = 0 sec
m
Final time = 10 sec
mg  Find Value
mg sin  [Case 1]  0 40 , 0 0rad / sec
 Free Body Diagram [Case 2]  0 0 , 0 3rad / sec
355
 Dynamic simulation
 Steady-State Form
Motion of Equation. Runge Kutta Method
k11  f1 ( x1( n ) , x2( n ) )
ml 2 mgl sin   c k21  f 2 ( x1( n ) , x2( n ) )
k12  f1 ( x1( n )  0.5hk11 , x2( n )  0.5hk21 )
Make First order Equation
k22  f 2 ( x1( n )  0.5hk11 , x2( n )  0.5hk21 )
x 
1 x   x
1 2 k13  f1 ( x1( n )  0.5hk12 , x2( n )  0.5hk22 )
k23  f 2 ( x1( n )  0.5hk12 , x2( n )  0.5hk22 )
g c
x2  x
1 x2  x1   sin x1  x
2 2
k14  f1 ( x1( n )  hk13 , x2( n )  hk23 )
l ml k24  f 2 ( x1( n )  hk13 , x2( n )  hk23 )

 x2  h
 x1   f1 ( x1 , x2 )   
x1( n 1) x1( n )  (k12  2k12  2k13  k14 )
6
 x   f ( x , x )    g
sin x1 
c
x 
h
x2( n 1) x2( n )  (k21  2k22  2k23  k24 )
 2  2 1 2  2 2 6
 l ml 
356
 Dynamic simulation
 [Case 1] Initial Position = 40 deg,  [Case 2] Initial Position = 0 deg,
Initial Velocity = 0 rad/sec Initial Velocity = 3 rad/sec

357
 Dynamic simulation
 Robot dynamic simulation
• Inverse dynamics
• Forward dynamics
l2
• Given trajectory
m2 2
- Radius 0.2m circle
- Each link is slender bar
l1 l2 0.5m
m1 l1
m1 m2 1kg
1

358
 Dynamic simulation
 2DOF Example

359
 Dynamic simulation
 Trajectory

360
 Dynamic simulation
 Circle trajectory.
u (t )  x(t ) r sin  (t )  a
y (t ) r cos  (t )  b
u (t )  x (t ) r cos  (t )  (t )
y (t )  r sin  (t )  (t )
x(t )  r sin  (t ) 2 (t )  r cos  (t )  (t )
u(t )  
y (t )  r cos  (t ) 2 (t )  r sin  (t )  (t )

361
 Dynamic simulation

• Conditions
(h=0.01)
 (0)  0 =0  (t f )  f 2
(0) 0  (t f ) 0
• Coefficient (t ) a0  a1t  a2t 2  a3t 3
a0  0 a1 0
3 2
a2  2 ( f   0 ) a3  3 ( f   0 )
tf tf
362
Dynamic simulation
( x, y )

363
Dynamic simulation

x(t ) y (t )

364
Dynamic simulation

x (t ) y (t )

365
Dynamic simulation


x(t ) y (t )

366
 Dynamic simulation
 Inverse kinematics : position
• Give : output x 2  y 2  L12  L22
cos 2 
• Find : joint angle 2 L1 L2
sin  2  1  cos  2
 2 a tan 2 sin  2 ,cos  2 
x  L1  L2 cos 2   yL2 sin  2
cos1 
L1  L2 cos 2   L22 sin 2  2
2

y  L1  L2 cos 2   xL2 sin  2

sin 1 
L1  L2 cos 2   L22 sin 2  2
2

1 a tan 2 sin 1 ,cos 1 

367
Dynamic simulation

1 (t )  2 (t )

368
 Dynamic simulation
 Inverse kinematics : velocity
• Given : output velocity
• Find : joint velocity
1
 (t )  G  U (t )
 u
 
  l1 sin1  l2 sin12  l2 sin 12 
 G  
u

 l1 cos1  l2 cos12 l2 cos12 

369
 Dynamic simulation
 History of joint velocity
1 (t ) 2 (t )

370
 Dynamic simulation
 Inverse kinematics : acceleration
• Given : output acceleration
• Find : joint acceleration
  Gu  
U (t )  Gu  
T
(t )   (t )  H u   (t )  H u  


 u  1 

T
 (t )  G  U   (t )  H u   (t ) 
371
 Dynamic simulation
 History of joint acceleration
1 (t ) 2 (t )

372
 Dynamic simulation
 Result of inverse dynamics
  I   V  ,   G  
*

373
 Dynamic simulation
 Forward dynamics
  V  ,   G  
1
 I 
*

374
 Dynamic simulation
 History of joint velocity
t
for 1

(t )  ( )d   (0)
0

375
 Dynamic simulation
 History of joint velocity for  2

376
 Dynamic simulation
 History of joint angle
t for 1
 (t )  ( )d   (0)
0

377
 Dynamic simulation
 History of joint angle for  2

378
 Dynamic simulation
 Forward kinematics for x
x l1 cos1  l2 cos12

379
 Dynamic simulation
 Forward kinematics for y
y l1 sin1  l2 sin12

380
 Dynamic simulation
 Comparison desired output with actual output

381
6.5 G&H Based Dynamic Modeling
aj

Link j

f  f i
p

Newton’s 2nd law  T

a j  G      j H  c  
j c

p
f M j a j
p

f M j  jG c     j H c  
T
 Inertia force vector
①
모션에 의해
힘에 의한 모션
느끼는 관성힘

382
 Dynamic Modeling
j j  Hc
z

Link j y

m j : inertia moment x

Euler eq. H c  I  
j j j j
m  I j      I j   ②
Angular Momentum
dHc
 c I j  : inertia tensor expressed in mc  I      I   
local coordinate dt
 I j  : inertia tensor expressed in   x i   y j  z k
global coordinate  x i   y j  z k
Energy is coordinate independent.
with respect to local coor-
dinate 383
 Dynamic Modeling
In the rotational kinetic energy
 1 2 
1 j T j j  12 ml 0 0 
( ) [I ]  
2 1
1 [ c I ]  0 ml 2 0 
 ( c j )T [ c I j ] c j  12 
2 0 0 0
local c j   j global로  
 

 j  j0 R c j 이 므 로
( c j )T j0 RT [ c I j ] j0 R c j
c [ I j ]  0j R[ c I j ] 0j RT
(  ) [ I ] 
j T c j c j

Inertia tensor expressed

in terms of the global reference coordinate
384
 Dynamic Modeling
앞의 Euler Equation ② 에서
 j [G jk ] 


 j [Gjk ]  T [ Hjk ]

Note r [ P ]   0  z y  where

 z 0  x  x
  r  y 
  y x 0   
 z 
Skew-Symmetric Matrix

385
 Dynamic Modeling
 j [I j ] j
 0  z  y    I 
jx    z I jy   y I jz 
  jy 
  j
  z 0   x    I    z I  x I  
j jx jz

   y  x 0    I jz   jy 
   
 y I jx
  x I 
( j )T [ P j ] j
Row column
vector vector

 0    I jz   I jy 
     jx 
 p j   I jz   p j   0   p j    I 
1;; 2;; 3;;
  I jy   I jx   0 
     

386
 Dynamic Modeling

 
j j T
m  I j    
j
 p j   j


 I j   Gjk  
T

T T
   Hjk      Gjk   p j   Gjk  

 T

 I j   Gjk     I j    Hjk    Gjk   p j   Gjk  
T

① 과 ②로 부터

m, f :motion
387
 Dynamic Modeling

 *
  T
  I      P
*
  Inverse dynamic equation

Using Virtual Work and d’Alembert Principle

c c
a  ma
F   ma  0
 W F x   ma  x 0
m 
Dynamic equilibrium
u (t )
F
 
  dt F 2 c 2 c
T v, a

 
M
 W  11   22    f   u   m  
j T
T jc jc 2
 0 m  f
2

1 2
 f
j 1 1 c 1
v, a
c

2 , 2
=

=
=

1dt 2 dt jc
v dt  jc dt 1
388

1 , 1 1
m
 Dynamic Modeling
양변을  t 로 나누면
v  jGc    jc  Gjk  
jc


  f j  jGc    m j  Gjk  
T

j 1

 
M T T
   jGc  f j   Gjk  m j
j 1

 
M
j c T j c T  T
   G  M j  G      j Hc  
  
j 1

   
M T
 T T jk T
  G  jk
 I   G      H      G   p jk   Gjk  
j jk jk

j 1

 I     P  

* T *  j
 j T j

389
 Dynamic Modeling

 *
  T
  I      P
*
 

where

 
M T T
 I   M j  jGc   j Gc    Gjk   I j   Gjk 
*

j 1

 
M

 
T T
 P   M j  G    H    G    G   p j   Gjk 
* j c j c jk jk

j 1

 I 
T
  G     Hjk 
 G 
jk j T
 G 
jk

jk
   p j   Gjk 

 T

 Gjk   I j    Hjk 

390
 Dynamic Modeling
 W  T  T Tu  u Virtual work
 u  Gu   u  Gu  

 T  T Tu  Gu  

 
 T  T Tu  Gu   0
T
Since  0,   G  T u
u
Effective force relation between input
torque and output wrench
f
T u    6
 m

391
 Dynamic Modeling
Operational Space Dynamic Model

T u  I  u u  P  u
* T *
 *
  T
  I      P
*
 
uu uuu

T
T u  Gu  

 Gu 
T
  I*     P
T *
  
 T   T T
 Gu   I     Gu    P
   *
  *
 

 T

T u  Gu   I   Gu  u
*  T *
uu  T  T
 u  H  u  u  Gu    Gu    P
*
   Gu  u

T
  
 Gu   I*   Gu  u u   Gu   I*    H uu
T T  T
 T
   Gu   Gu    P
*

  Gu   u


392
 Dynamic Modeling
where
T
 I uu
*
  Gu   I*   Gu 

 
 Puu*    Gu   I*   H uu

   Gu 
T
 T
 Gu    P
*

  Gu 

Relationship between the operational kinematics to the input torque

T
  Gu  T u where
 T
 T  T T  I   Gu   I uu
* *

 Gu   I  u  Gu  u  P  u
*
uu
*
uuu
u

T
* T
 I  u u  P  u*  P*uu   Gu    Puuu
*

u  uu

393