Velocity and acceleration
GENERAL METHOD
( )
( )
( , ... , , , ... , , , ... , ) 0
...
0 ,
...
, , , ...
, , , ...
,
0 ,
...
, , , ...
, , , ...
,
1 1
1
1 1
1 2 2
1 1
1 1 1
=
=
=
=
=
=
m n
k m
m
m n
k
m n
k
x x
q q
w w
f f
x x
q q
w w
f f
x x
q q
w w
f f
( w , q , x ) = 0
f
w – vector of links’ dimensions,
q – vector of known independent variables (drivers’ position), x – vector of unknown dependent variables (links’ position)
( ) t x x ( ) t
q
q = =
x A
f =
=
m m m
m
m
x f x
f x
f
x f x
f x
f
...
...
...
2 1
1 2
1 1
1
Dependent velocities
𝑓 = 0 → 𝑑𝑓
𝑑𝑡 = 0 → 𝜕𝑓
𝜕𝑥 ሶ𝑥 + 𝜕𝑓
𝜕𝑞 ሶ𝑞 = 0
ሶ𝑥 = 𝑥 1 ሶ … . ሶ 𝑥 𝑚 𝑇
q B
f =
−
=
−
n m m
m
n
q f q
f q
f
q f q
f q
f
...
...
...
2 1
1 2
1 1
1
Independent velocities, drivers
ሶ𝑞 = 𝑞 1 ሶ … . ሶ 𝑞 𝑛 𝑇 𝐴 ሶ𝑥 = 𝐵 ሶ𝑞
ሶ𝑥 = 𝐴 −1 𝐵 ሶ𝑞
𝐴 −1 𝐴 = 1
ሷ𝑥 = 𝑥 1 ሷ … . ሷ 𝑥 𝑚 𝑇 ሷ𝑞 = 𝑞 1 ሷ … . ሷ 𝑞 𝑛 𝑇
𝐴 ሶ𝑥 = 𝐵 ሶ𝑞 𝐴 ሷ𝑥 + ሶ 𝐴 ሶ𝑥 = 𝐵 ሷ𝑞 + 𝐵 ሶ𝑞 ሶ
ሷ𝑥 = 𝐴 −1 − ሶ 𝐴 ሶ𝑥 + ሶ 𝐵 ሶ𝑞 + 𝐵 ሷ𝑞
Example of 2 DOF manipulator
M
=
=
→ x
My
Mq
q
1,
20 q
c b
a + − −
2=
0 sin
sin sin
0 cos
cos cos
2 2
1 1
2 2
1 1
=
− +
=
−
− +
x q
x b
q a
x q
c x
b q
a
M
sin 0 sin
sin
cos cos
cos
2 2
1 1
2 2
1 1
2
1
=
− +
−
−
= +
=
x q
x b
q a
x q
c x
b q
a f
f f
q1, q2 – independent (drivers), x1, x2 – unknowns,
−
= −
=
2 2
1
2 2
1
cos cos
sin sin
x q
x b
x q
x b
x A f
sin 0 sin
sin
cos cos
cos
2 2
1 1
2 2
1 1
2
1
=
− +
−
−
= +
=
x q
x b
q a
x q
c x
b q
a f
f f
x A
f =
=
m m m
m
m
x f x
f x
f
x f x
f x
f
...
...
...
2 1
1 2
1 1
1
sin 0 sin
sin
cos cos
cos
2 2
1 1
2 2
1 1
2
1
=
− +
−
−
= +
=
x q
x b
q a
x q
c x
b q
a f
f f
q B
f =
−
=
−
n m m
m
n
q f q
f q
f
q f q
f q
f
...
...
...
2 1
1 2
1 1
1
−
−
− −
=
−
=
2 1
2 1
sin cos
cos sin
x q
a
x q
a q
B f
Velocity equation
ሶ𝒙 = 𝑨 −𝟏 𝑩 ሶ𝒒
ሶ𝑞 = 𝑞 1 ሶ 𝑞 2 ሶ 𝑇 ሶ𝑥 = 𝑥 1 ሶ 𝑥 2 ሶ 𝑇
ሶ
𝑥
1ሶ
𝑥
2= −𝑏 sin 𝑥
1𝑞
2sin 𝑥
2𝑏 cos 𝑥
1−𝑞
2cos 𝑥
2−1
𝑎 sin 𝑞
1cos 𝑥
2−𝑎 cos 𝑞
1sin 𝑥
2ሶ
𝑞
1ሶ
𝑞
2Velocity equation
ሶ𝒙 = 𝑨 −𝟏 𝑩 ሶ𝒒
Acceleration equation
ሷ𝑥 = 𝐴 −1 − ሶ 𝐴 ሶ𝑥 + ሶ 𝐵 ሶ𝑞 + 𝐵 ሷ𝑞
−
= −
=
2 2
1
2 2
1
cos cos
sin sin
x q
x b
x q
x b
x A f
ሶ 𝐴 = 𝑑
𝑑𝑡 𝐴 = − ሶ 𝑥
1𝑏 cos 𝑥
1𝑞
2ሶ sin 𝑥
2− ሶ 𝑥
2𝑞
2cos 𝑥
2− ሶ 𝑥
1𝑏 sin 𝑥
1𝑞
2ሶ cos 𝑥
2+ ሶ 𝑥
2𝑞
2sin 𝑥
2
−
−
− −
=
−
=
2 1
2 1
sin cos
cos sin
x q
a
x q
a q
B f
ሶ 𝐵 = 𝑑
𝑑𝑡 𝐵 = 𝑞
1ሶ 𝑎 cos 𝑞
1− ሶ 𝑥
2sin 𝑥
2ሶ
𝑞
1𝑎 sin 𝑞
1𝑥
2ሶ cos 𝑥
2Acceleration equation
ሷ𝑥 = 𝐴 −1 − ሶ 𝐴 ሶ𝑥 + ሶ 𝐵 ሶ𝑞 + 𝐵 ሷ𝑞
ሷ𝑥 = 𝑥 1 ሷ , ሷ 𝑥 2 𝑇 ሷ𝑞 = 𝑞 1 ሷ , ሷ 𝑞 2 𝑇
ሷ
𝑥1
ሷ
𝑥2 =
= −𝑏 sin 𝑥1 𝑞2 sin 𝑥2 𝑏 cos 𝑥1 −𝑞2 cos 𝑥2
−1
൜
ൠ
− − ሶ𝑥1𝑏 cos 𝑥1 𝑞2ሶ sin 𝑥2 − ሶ𝑥2𝑞2 cos 𝑥2
− ሶ𝑥1𝑏 sin 𝑥1 𝑞2ሶ cos 𝑥2 + ሶ𝑥2𝑞2 sin 𝑥2
ሶ
𝑥1
ሶ
𝑥2 + 𝑞1ሶ𝑎 cos 𝑞1 − ሶ𝑥2 sin 𝑥2
ሶ
𝑞1𝑎 sin 𝑞1 𝑥2ሶ cos 𝑥2
ሶ
𝑞1
ሶ
𝑞2 + 𝑎 sin 𝑞1 − cos 𝑥2 𝑎 cos 𝑞1 − sin 𝑥2
ሷ
𝑞1
ሷ
𝑞2
−
−
− −
=
−
=
2 1
2 1
sin cos
cos sin
x q
a
x q
a q
B f
d a
r
M= +
( )
( )
+ +
+
= +
1 1
1 1
sin sin
cos cos
x d
q a
x d
q a
y x
M M
M
r
Mሶ
𝑥
𝑀ሶ
𝑦
𝑀= −𝑎 ሶ 𝑞
1sin 𝑞
1−𝑑 ሶ 𝑥
1sin 𝑥
1+ 𝛽 𝑎 ሶ 𝑞
1cos 𝑞
1+ 𝑑 ሶ 𝑥
1cos 𝑥
1+ 𝛽
ሷ
𝑥
𝑀ሷ
𝑦
𝑀= −𝑎 ሷ 𝑞
1sin 𝑞
1− 𝑎 ሶ 𝑞
12cos 𝑞
1−𝑑 ሷ 𝑥
1sin 𝑥
1+ 𝛽 − 𝑑 ሶ 𝑥
12cos 𝑥
1+ 𝛽
𝑎 ሷ 𝑞
1cos 𝑞
1− 𝑎 ሶ 𝑞
12sin 𝑞
1+ 𝑑 ሷ 𝑥
1cos 𝑥
1+ 𝛽 − 𝑑 ሶ 𝑥
12sin 𝑥
1+ 𝛽
SERIAL and PARALLEL P L A N A R MANIPULATORS
M A N I P U L A T O R S
(robot mechanisms)
APPLICATIONS of ROBOTS:
Operation in a danger zone:
•RADIATION
•EXPLOSION (POLICE, ARMY)
•HIGH PRESSURE, HIGH TEMPERATURE
Manufacturing
•ASSAMBLY, WELDING, MACHINING, etc
Health care
•REHABILITATION
•OPERATIONS
and many others ...
forearm arm
hand finger
MANIPULATOR
Mechanical device for gripping and the controlled
movement of objects (mechanism having many degrees of freedom)
END-EFFECTOR
Device attached to the robot arm by which
objects can be grasped or
acted upon.
forearm arm
hand finger
ROBOT
Mechanical system under automatic control that
performs operations such as handling and locomotion
mechanical system
automatic control
SENSORS
SENSORS
ENVIRON MENT CONTROL
UNIT DRIVES MANIP. END-
EFFECTOR
MECHANICAL SYSTEMS
Modern robot block diagram
Planar parallel robot – 2 dof
PARALLEL MANIPULATORS
Manipulator that controls the motion of its end effector by means of at least two kinematic chains going from the end-effector towards the frame.
Parallel manipulators (2D)
DOF = 3
circles are 1-st class joints:
R and/or T end-effector (platform)
3 RRR
RRR
Planar serial manipulators DOF = 3
RRT
TRR
RTR
Direct kinematics:
−
→ −
n orientatio
position y
x
Q Q
, , ,
:
known
1 2 3RRR
In general: DIRECT TASK
Computation of the pose, motion and forces at the end-effector of a robot arm from given actuator displacements, velocities, accelerations and forces
Inverse kinematics:
,
1,
2,
3:
→
−
−
n orientatio
position y
known x
Q QRRR
In general: INVERSE TASK
Computation of actuator forces, displacements, velocities and accelerations from given forces, pose and motion of the end-effector of a robot.
vectors
34 23
12
+ +
= a a a r
QDirect kinematics using vector projections
RRR
r
Q( ) ( )
( ) ( )
3 2
1
3 2
1 34
2 1
23 1
12
3 2
1 34
2 1
23 1
12
sin sin
sin
cos cos
cos
+ +
=
+ +
+ +
+
=
+ +
+ +
+
=
a a
a y
a a
a x
Q Q
RRR
r
Qj
k M
M k
r
k j
p
M j
r
e
kye
kxk j
Direct kinematics using Cartesian coordinates (absolute coordinates)
j k M
M
jr M
kr
k jp
eky ekx
k j
k j
M k
k j
M
j
r = R r + p
=
k j
k j kx
j
sin
e cos
= −
k j
k j ky
j
cos e sin
versors
−
=
=
k j k
j
k j k
j ky
j kx j k
j
cos sin
sin e cos
e R
Rotation matrix
+
−
=
k j
k j
M k
M k
k j k
j
k j k
j
M j
M j
y x y
x y
x
cos sin
sin cos
j k M
M
jr M
kr
k jp
eky ekx
k j
−
=
−
=
k j k
j
k j k
j T
k j k j
cos sin
sin
1
cos R R
Properties of rotation matrix
Inversion=transposition!!!
I R
R =
=
−
−
=
−
1 0
0 1 cos
sin
sin cos
cos sin
sin
1 cos
k j k
j
k j k
j
k j k
j
k j k
j k
j k j
identity matrix
=
1 0
0
pk
A R
j k j k
j
−
=
1 0
0
cos sin
sin cos
k j k
j k
j
k j k
j k
j
k
j
y
x A
M k k j M
j
r = A r
Homogeneous transformation
k
Tj k
j k
T j k j T
k j k
j
q = p = x y
Vector of cart. coordinates
k j M
k k j
M
j
r = R r + p
Instead of
we can use
Example
Known:
point Q position on link 3 link dimensions
joint variables
Find:
point Q position in frame coordinate system xoyo
x
0y
0a12
a23
a34 Q
0 1
2
3
x
0y
0a12
a23
a34 Q
1
2
3
0 1
2
3
Example
1. Introduce link
coordinate systems and joint variables
=
1 1
3 3
3 2 2 1 1 0
Q Q Q
Q
y x A
A A
y x
=
1 0 1
34 3
3 a
y x
Q Q
2. equations
−
=
1 0
0
0 cos
sin
0 sin
cos
1 1
1 1
1 0A
−
=
1 0
0
0 cos
sin
sin cos
2 2
12 2
2 2
1
a A
−
=
1 0
0
0 cos
sin
sin cos
3 3
23 3
3 3
2
a A
−
−
−
=
1 1
0 0
0 cos
sin
sin cos
1 0
0
0 cos
sin
sin cos
1 0
0
0 cos
sin
0 sin
cos
1
3 3
3 3
23 3
3 2
2
12 2
2 1
1
1 1
Q Q Q
Q
y x a
a y
x
x
0y
0a12
a23
a34 Q
1
2
3
0 1
2
3
Manipulator 2D
Inverse kinematics
39
x
0y
0a12
a23
a34 Q
1
2
3
0 1
2
3
x
y
Given:
x, y,
Calculate:
3 , 2 , 1
= ?, =
ii
40
x
0y
0a12
a23
a34 Q
1
1
2
3
0 1
2
3
=
1 1
3 3
3 2 2 1 1 0
Q Q Q
Q
y x y
x
A A
A
=
1 0 1
34 3
3 a
y x
Q Q
We already
have:
41
−
=
1 0
0
0 cos
sin
0 sin
cos
1 1
1 1
1 0A
−
=
1 0
0
0 cos
sin
sin cos
2 2
12 2
2 2
1
a A
−
=
1 0
0
0 cos
sin
sin cos
3 3
23 3
3 3
2
a
A
42
3 0
3 2
2 1
1
0
A A A = A
( ) ( ) ( )
( ) ( ) ( )
+
+
+
+
+
+
+
+
+
+
−
+
+
=
1 0
0
sin sin
cos sin
cos cos
sin cos
2 1
2 3 1
1 2 3
2 1
3 2
1
2 1
2 3 1
1 2 3
2 1
3 2
1 3 0
a a
a a
A
43
x
0y
0a3
4
Q
0 1
3 x
y
−
=
=
1 0
0
cos sin
sin cos
3 0
y x
T
44
−
=
=
1 0
0
cos sin
sin cos
3 0 3
0
y
x
T
A
( ) ( ) ( )
( ) ( ) ( )
+
+
+
+
+
+
+
+
+
+
−
+
+
=
1 0
0
sin sin
cos sin
cos cos
sin cos
2 1
2 3 1
1 2 3
2 1
3 2
1
2 1
2 3 1
1 2 3
2 1
3 2
1 3 0
a a
a a
A
Comparison of matrices:
45
It gives 4 equations
( )
( )
( )
( )
+
+
=
+
+
=
+
+
=
+
+
=
) ( sin
sin
) ( cos
cos
) ( sin
sin
) ( cos
cos
2 1
2 3 1
1 2
2 1
2 3 1
1 2
3 2
1
3 2
1
d a
a y
c a
a x
b a
...
...
...
2 31
= = =
46
( )
( )
( )
( )
+
+
=
+
+
=
+
+
=
+
+
=
) ( sin
sin
) ( cos
cos
) ( sin
sin
) ( cos
cos
2 1
2 3 1
1 2
2 1
2 3 1
1 2
3 2
1
3 2
1
d a
a y
c a
a x
b a
• (c)^2 and (d)^2 sum gives:
u a
a a
a y
x
2+
2=
122+
232+ 2
12 23(
1 2)
1(
1 2)
1
cos sin sin
cos + + +
u =
where:
47
(
1 2)
1(
1 2)
1
cos sin sin
cos + + +
= u
( )
(
11 22) sin
11cos
22cos
11sin
22sin
sin sin
cos cos
cos
+
=
+
−
=
+
Using:
48
(
1 2)
1(
1 2)
1
cos sin sin
cos + + +
= u
cos 2
= u
One can obtain
(
12 1 1 2 12)
21 1 2 22 1
1 2
1 1
cos cos
sin cos
sin cos
sin cos
sin sin
sin sin
cos cos
cos cos
=
+
=
=
+
+
+
−
u =
u a a a
a y
x
2+
2=
122+
232+ 2
12 232 23
12 2
23 2
12 2
2
+ y = a + a + 2 a a cos
x
2 3 1 2
2 2 3 2
1 2 2
2
2
2
cos a a
a a
y
x + − −
=
(
2 2 2)
2
= atan2 1 - cos , cos
50
Function atan2(y,x) – (1)
y x
- <= atan2(y,x) <=
atan2(y,x) calculates arctan(y/x) for any y, x
atan2(2,2) = pi/4 atan2(-2,-2)=-3pi/4 but
arctan(2/2) =
arctan[(-2)/(-2)]=pi/4
51
( )
=
= →
=
a b
b
a atan2 ,
cos sin
( ) ( - a, b )
atan2 .
2
a,-b atan2
. 1
0 sin
cos
=
=
=
+
b
a
52
( ad - bc, ac - bd )
atan2
cos sin
sin cos
=
→
=
+
=
−
d b
a
c b
a
53
2
2Two configurations
(
2 2 2)
2
