1 ROBOTICS Lecture

Prof. Hyouk Ryeol Choi 2016

MIDTERM EXAMINATION SOLUTION

Problem 1
Solution

𝑃 𝐴 = 𝑇 𝐵𝐴 𝑃𝐵
Therefore,

𝑃𝐵 = (𝑇 𝐵𝐴 )−1 𝑃 𝐴

Inverse 𝑇 𝐵𝐴 , that is 𝑇 𝐵𝐴 need to be calculated.

𝑇 𝐵𝐴 = [ 𝑅𝑇 −𝑅 𝑇 𝑃]
0 0 0 1
0.9 0.5 0.0
𝑅 𝑇 = [−0.5 0.9 0.0]
0 0 1
0.9 0.5 0.0 10 −7.5
−𝑅 𝑇 𝑃 = − [−0.5 0.9 0.0] [−3] = [ 7.7 ]
0 0 1 9 −9
Therefore, 𝑇 𝐵𝐴 =
0.9 0.5 0 −7.5
𝑇 𝐵𝐴 = [−0.5 0.9 0 7.7 ]
0 0 1 −9
0 0 0 1
0.9 0.5 0 −7.5 1 −5.6
𝑃𝐵 = 𝑇 𝐵𝐴 𝑃 𝐴 = [−0.5 0.9 0 7.7 ] [2] = [ 9.0 ]
0 0 1 −9 3 −6.0
0 0 0 1 1 1
−5.6
∴ 𝑃𝐵 = [ 9.0 ]
−6.0

Problem 2
Solution
-Rotation 30° about z:
cos30° −sin30° 0 0
[ sin30° cos30° 0 0]
0 0 1 0
0 0 0 1
2 ROBOTICS Lecture
Prof. Hyouk Ryeol Choi 2016

-Translation (4, 1, 0):

1 0 0 4
[0 1 0 1]
0 0 1 0
0 0 0 1
Therefore, transformation 𝑇 𝐵𝐴 =
cos30° −sin30° 0 0 1 0 0 4
𝑇 𝐵𝐴 = [ sin30° cos30° 0 0] [0 1 0 1]
0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1
√3 1 4√3−4
cos30° −sin30° 0 4cos30° − sin30° −2 0
2 2

= [ sin30° cos30° 0 4sin30° + cos30° ] = 1 √3

0
4+√3
0 0 1 0 2 2 2
0 0 0 1 0 0 1 0
[0 0 0 1]

Problem 3
a) The rotation matrix is a orthonormal matrix, therefore we have RRT  I . Take time
derivative both side of the equation, we obtain

RRT  RRT  0
RRT   RRT   0
T


 RR 1   RR 1   0
T

 S  ST  0

Hence, S  RR 1 is a skew-symmetric matrix.

b) With the given X-Z-X Euler rotations, we can extract the following information.

 0.5000 0.0000 0.8660  cos  cos  sin  sin  sin  

R 0   0.7500 0.5000 0.4330  cos  sin  * * .

 0.4330 0.8660 0.2500  sin  sin  * * 

First, it is noted that

 cos  sin   0
 cos   0 , thus we have    900 and sin    1 .
sin  sin   0.8660

Second, we will use the following equations

3 ROBOTICS Lecture
Prof. Hyouk Ryeol Choi 2016

cos  0.5
0.866    atan2  0.866, 0.5  600 .
sin    0.866
sin 

Finally, the other two equations will be used as following.

cos  sin   0.7500  0.4330 0.7500   300

   atan2  ,  .
sin  sin   0.4330  sin  sin   1500

In summary, the two pairs of equivalent X-Z-X Euler angles are:

  900 ,   600 ,   300

  900 ,   600 ,   1500.

Problem 4
1) Setup the coordinate frames on each joint and define axis.
→ Note: the origin of 𝑂0 − 𝑥0 𝑦0 𝑧0 is assumed to be located at the home position
of the second prismatic joint.

Fig. 1. Definition of coordinate frame

2) Make a table for the D-H parameters of each link.

Link 𝑎𝑖 𝛼𝑖 𝑑𝑖 𝜃𝑖
1 0 0 0 𝜃1
2 0
𝜋 𝑑2 0
−
2
3 0 0 𝑑2 0
4 ROBOTICS Lecture
Prof. Hyouk Ryeol Choi 2016

3) Find the forward kinematic equation from D-H parameters.

cos𝜃1 −sin𝜃1 0 0
sin𝜃1 cos𝜃1 0 0]
𝐴10 = [
0 0 1 0
0 0 0 1
1 0 0 0
𝐴12 = [ 0 0 1 0]
0 −1 0 𝑑2
0 0 0 1
1 0 0 0
𝐴23 = [0 1 0 0 ]
0 0 1 𝑑3
0 0 0 1

Therefore, forward kinematic equation 𝐴03 =

cos𝜃1 0 −sin𝜃1 −𝑑3 sin𝜃1

sin𝜃1 0 cos𝜃1 𝑑3 cos𝜃1
𝐴03 = 𝐴10 𝐴12 𝐴23 [ ]
0 −1 0 𝑑2
0 0 0 1

4) Find the joint variables 𝜃1 , 𝑑2 , 𝑑3 for the given end effector position 𝑋𝑒 =
[𝑃𝑥 𝑃𝑦 𝑃𝑧 ]𝑇 .

𝑃𝑥 = −𝑑3 sin𝜃1 , 𝑃𝑦 = 𝑑3 cos𝜃1 , 𝑃𝑧 = 𝑑2

∴ 𝜃1 = 𝑎𝑡𝑎𝑛2(−𝑃𝑥 , 𝑃𝑦 )

𝑑2 = 𝑃𝑧

𝑑3 = √𝑃𝑥 2 + 𝑃𝑦 2

5) Find the geometric Jacobian matrix about the base frame.

𝑍0 × (𝑃3 − 𝑃0 ) 𝑍1 𝑍2
J(𝑞) = [ ]
𝑍0 0 0

0 −𝑑3 sin𝜃1 0 −sin𝜃1

𝑃0 = [0] , 𝑃3 = [ 𝑑3 cos𝜃1 ] , 𝑍0 = 𝑍1 = [0] , 𝑍2 = [ cos𝜃1 ]
0 𝑑2 1 1
5 ROBOTICS Lecture
Prof. Hyouk Ryeol Choi 2016

−𝑑3 cos𝜃1 0 −sin𝜃1

−𝑑3 sin𝜃1 0 cos𝜃1 −𝑑3 cos𝜃1 0 −sin𝜃1
J= 0 1 0 = [ −𝑑3 sin𝜃1
0 0 cos𝜃1 ]
0 0
0 0 0 1 1
0
[ 1 0 0 ]

6) Discuss about kinematic singularities of the given robot mechanism

det(J) = 0

∴ 𝑑3 = 0
Singularity, that is the end effector is located along joint 1 axes.

Problem 5
a) For simplicity, the forward kinematics can be calculated directly using geometric
projection, we have
x  L1 c1 L2 c1 2
. (1)
y  L1 s1 L2 s1 2
b) The inverse kinematic can be calculated by squaring up Eqs. (1) as following.

 L2c12    L2 s12    x  L1c1    y  L1s1 

2 2 2 2

x 2  y 2  L12  L22
L22  x  y 2  L12  2 L1  xc1  ys1 
2
where k .
2 L1
xc1  ys1  k

The above equation can be solved by managing as following

x y k
c1  s1 
x y
2 2
x y
2 2
x  y2
2
c  cos  
x
k x  y2
2

c c1  s s1  where .
x2  y 2 s  sin  
y
k x2  y 2
cos 1    
x2  y 2

The solution for 1 is obvious now :

k  y x  k
1    acos  atan2  ,   acos (2)
x2  y 2  x2  y 2 x2  y 2  x2  y 2
 
6 ROBOTICS Lecture
Prof. Hyouk Ryeol Choi 2016

Since we have 1 now, we can calculate 2 by the following equation.

 2  1  atan2  s12 , c12 

 y  L1s1 x  L1c1 
 2  1  atan2  , 
 L2 L2 

Noticeably, the condition for Eq. (2) to be valid is that the function inside acos is less
than or equal to 1. Therefore, the condition for this inverse solution exists is

k
 1
x2  y 2
 x 2  y 2  L2  L2  2 L x 2  y 2
 
1 2 1

 x  y  L1  L2  2 L1 x  y
2 2 2 2 2 2

 L1  L2  x 2  y 2  L1  L2

The above condition means that the end-effector position should be inside a round disc
bounded by two circles with the radius of L1+L2 and L1  L2 .

c) The Jacobian matrix is calculated by computing the following partial derivatives

 x x 
   2    L1s1  L2 s12  L2 s12 
J 
O 1

y   L1c1  L2c12 L2c12 

.
 y
   2 
 1

d) The Jacobian matrix is calculated by computing the following

J J n   J i   zˆi  i re 
J (q)    1  J    ˆ 
Jn 
, J i
 J 1  i   zi 

0   L1c1  L2c12 
 J 1   zˆ1  1re   
zˆ1  0 
1
J1       , , re  L1c1  L2c12
 
 J1   zˆ1  1   0 

iˆ ˆj kˆ
zˆ1  1re  0 0 1
L1c1  L2c12 L1s1  L2 s12 0

 iˆ( L1s1  L2 s12 )  ˆj ( L1c1  L2c12 )  0  kˆ

 ( L1s1  L2 s12 )iˆ  ( L1c1  L2c12 ) ˆj
7 ROBOTICS Lecture
Prof. Hyouk Ryeol Choi 2016

 ( L1s1  L2 s12 ) 
 (L c  L c ) 
 11 2 12 

 0 
J1   
 0 
 0 
 
 1 

0   L2 c12 
 J   zˆ  2 re 
zˆ2  0   
2
J2    2    2  , , re  L2 c12
 
 J 2   zˆ2  1   0 

iˆ ˆj kˆ
zˆ2  2 re  0 0 1
L2 c12 L2 s12 0

 iˆ( L2 s12 )  ˆj ( L2c12 )  0  kˆ

  L2 s12iˆ  L2 c12 ˆj

  L2 s12 
Lc 
 2 12 
 0 
J2   
 0 
 0 
 
 1 

 ( L1s1  L2 s12 )  L2 s12 

 (L c  L c ) L2 c12 
 11 2 12

 0 0 
J   J1 J2    
 0 0 
 0 0 
 
 1 1 

e) The singular configuration is form where the Jacobian matrix is singular or we have:

det  O J     L1s1  L2 s12  L2c12   L1c1  L2c12  L2 s12  L1L2 s2  0. .

Hence, the system is singular at  2  0 or 2  1800 . At those points, we will have

x   L1  L2  c1 x   L1  L2 c1
y   L1  L2  s1 y   L1  L2 s1
and .

By squaring the above equations, we will get the two boundary curves of the robot
workspace as
8 ROBOTICS Lecture
Prof. Hyouk Ryeol Choi 2016

x 2  y 2   L1  L2  x 2  y 2   L1  L2  .
2 2
and

Obviously, the above boundaries form two concentric circles that bound the workspace of
the end-effector. The position of the end-effector should be inside of the workspace, hence

 L1  L2   x 2  y 2   L1  L2 
2 2

Those are the same with the conditions of existence of inverse kinematics given in question
b.

f) The end-effector velocity has the form p  1 0 . For calculating the angular
T

velocity of the joints, we use the inverse jacobian as following.

θ  J 1p
1  L2 c12 L2 s12  1 
θ
L1 L2 s2   L1c1  L2 c12  L1s1  L2 s12  0 
1  L2 c12 
θ 
L1 L2 s2   L1c1  L2 c12 
.

At singular positions (s2 = 0), the above relationship is undefined.

g) The manipulator is statically balanced if the applied torque can generate exactly
opposite force at the end-effector, which means F  mg   0 98.1 N . Using the
force/torque equation, we have

τ O J T F
* L1c1  L2 c12   0 
τ
* L2 c12  98.1
* 1.366   0  134.00 
τ  98.1   49.05  Nm.
* 0.5    
h) The Jacobian matrix written in frame {C} can be calculated as
C
J CO R O J OC RT  O J
J  Rot z    O J
C T
.
 c s  O
C
J  J
  s c 

==========The End==========