HOMEWORK 2 - SOLUTIONS

ROBOTICS 1
Academic Year 2022-2023

Problem 1:
We consider a RP robot shown in Figure 1.

𝐼 0 0
𝑰 = 0 𝐼 0
0 0 𝐼
𝐼 0 0
𝑰 = 0 𝐼 0
0 0 𝐼
𝐴𝐵 = 𝑙 /2
Figure 1
𝐷𝑂 = 𝑙 /2

a) Derive the forward kinematic equations for the above manipulator using the DH-
convention.
DH-Parameters

𝑎 𝑑 𝜃 𝛼
Link 1 0 0 𝜃 + 𝜋/2 𝜋/2
Link 2 0 𝑟 + 𝑙 /2 0 0

− sin 𝜃 0 cos 𝜃 0 1 0 0 0
cos 𝜃 0 sin 𝜃 0 0 1 0 0
𝑨 = 𝑨 =
0 1 0 0 0 0 1 (𝑟 + 𝑙 /2)
0 0 0 1 0 0 0 1
− sin 𝜃 0 cos 𝜃 (𝑟 + 𝑙 /2) cos 𝜃
𝑻 = 𝑨 ∙ 𝑨 = cos 𝜃 0 sin 𝜃 (𝑟 + 𝑙 /2) sin 𝜃
0 1 0 0
0 0 0 1
b) Given a desired position of the end-effector, solve for the inverse kinematics problem.

(𝑟 + 𝑙 /2) cos 𝜃 𝑥 𝑟= 𝑥 + 𝑦 − 𝑙 /2
𝑥 = (𝑟 + 𝑙 /2) cos 𝜃
𝒐 = (𝑟 + 𝑙 /2) sin 𝜃 = 𝑦
𝑦 = (𝑟 + 𝑙 /2) sin 𝜃 𝜃 = tan
0 0
Professor Slim CHOURA Page 1 of 3 ROBOTICS II
 c) Derive the Jacobian of the manipulator.

𝒛 × (𝒐 − 𝒐 ) 𝒛 ) ̇
𝑱(𝒒) = ⇒ 𝑽(𝒐 = 𝑱(𝒒)𝒒̇ 𝒒̇ = 𝜃
𝒛 𝟎 𝝎 𝑟̇
0 (𝑟 + 𝑙 /2) cos 𝜃 0 cos 𝜃
𝒐 =𝒐 = 0 𝒐 = (𝑟 + 𝑙 /2) sin 𝜃 𝒛 = 0 𝒛 = sin 𝜃
0 0 1 0
−(𝑟 + 𝑙 /2) sin 𝜃 cos 𝜃
⎡ ⎤
(𝑟
⎢ + 𝑙 /2) cos 𝜃 sin 𝜃 ⎥
𝑱=⎢ 0 0 ⎥
⎢ 0 0 ⎥
⎢ 0 0 ⎥
⎣ 1 0 ⎦
d) Derive the dynamical equations of the manipulator. The center of mass for each link is
at the center of the link and the moments of inertia are 𝐼 and 𝐼 .

−𝑙 /2 sin 𝜃 0 −𝑟 sin 𝜃 cos 𝜃 0 0

𝑱 = 𝑙 /2 cos 𝜃 0 𝑱 = 𝑟 cos 𝜃 sin 𝜃 𝑱𝝎 = 𝑱𝝎 = 0 0
0 0 0 0 1 0
−sin 𝜃 0 cos 𝜃 𝐼 0 0 𝐼 0 0
𝑹 = 𝑹 = cos 𝜃 0 sin 𝜃 𝑰 = 0 𝐼 0 𝑰 = 0 𝐼 0
0 1 0 0 0 𝐼 0 0 𝐼

Hence, the translational and rotational parts of the kinetic energy are
1 𝑙 1
𝒒̇ 𝑚 𝑱𝑻𝑽 (𝒒)𝑱𝑽 (𝒒) + 𝑚 𝑱𝑻𝑽 (𝒒)𝑱𝑽 (𝒒) 𝒒̇ = 𝑚 𝜃̇ + 𝑚 𝑟 𝜃̇ + 𝑟̇
2 8 2
1 1
𝒒̇ 𝑱𝑻𝝎 (𝒒)𝑹 (𝒒)𝑰 𝑹𝑻 (𝒒)𝑱𝝎 (𝒒) + 𝑱𝑻𝝎 (𝒒)𝑹 (𝒒)𝑰 𝑹𝑻 (𝒒)𝑱𝝎 (𝒒) 𝒒̇ = 𝐼 + 𝐼 𝜃̇
2 2
Thus, the total kinetic energy reduces to:
𝑙 1 1
𝐾=𝑚 𝜃̇ + 𝑚 𝑟 𝜃̇ + 𝑟̇ + 𝐼 +𝐼 𝜃̇
8 2 2
The potential energy due to gravitation is:
𝑙
𝑉 = 𝑚 𝑔 sin 𝜃 + 𝑚 𝑔𝑟 sin 𝜃
2
𝑙 1 1 𝑙
𝐿 =𝐾−𝑉 =𝑚 𝜃̇ + 𝑚 𝑟 𝜃̇ + 𝑟̇ + 𝐼 +𝐼 𝜃̇ − 𝑚 𝑔 + 𝑚 𝑔𝑟 sin 𝜃
8 2 2 2

Professor Slim CHOURA Page 2 of 3 ROBOTICS II

Equations of motion

𝑑 𝜕𝐿 𝜕𝐿 𝑙 𝑙
− =𝑇 𝑚 +𝑚 𝑟 +𝐼 +𝐼 𝜃̈ + 2𝑚 𝑟𝑟̇ 𝜃̇ − 𝑚 𝑔 + 𝑚 𝑔𝑟 cos 𝜃 = 𝑇
𝑑𝑡 𝜕𝜃̇ 𝜕𝜃 4 2
𝑑 𝜕𝐿 𝜕𝐿
− =𝐹 𝑚 𝑟̈ − 𝑚 𝑟𝜃̇ − 𝑚 𝑔 sin 𝜃 = 𝐹
𝑑𝑡 𝜕𝑟̇ 𝜕𝑟
Problem 2:
(𝑚 + 2𝑚 cos 𝑞 )𝑞̈ + (𝑚 + 𝑚 cos 𝑞 )𝑞̈ − 𝑚 sin 𝑞 𝑞̇ 𝑞̇ − 𝑚 sin 𝑞 (𝑞̇ + 𝑞̇ )𝑞̇ = 𝜏

(𝑚 + 𝑚 cos 𝑞 )𝑞̈ + 𝑚 𝑞̈ − 𝑚 sin 𝑞 𝑞̇ = 𝜏

where 𝑚 (𝑖 = 1, 2) are constants, and 𝑞 and 𝜏 are the generalized coordinates and
generalized forces/torques for 𝑖 = 1, 2.

(i) Determine the inertia matrix, the Coriolis force and centrifugal force vectors, the
gravitational force vector, and the generalized external force vector.

𝑴(𝒒)𝒒̈ + 𝑪(𝒒 , 𝒒̇ )𝒒̇ + 𝒈(𝒒) = 𝝉

𝑚 + 2𝑚 cos 𝑞 𝑚 + 𝑚 cos 𝑞
𝑴(𝒒) =
𝑚 + 𝑚 cos 𝑞 𝑚
Inertia matrix

−2𝑚 sin 𝑞 𝑞̇ 𝑞̇ −𝑚 sin 𝑞 𝑞̇

𝑪(𝒒 , 𝒒̇ )𝒒̇ = +
0 ̇ 𝑞 𝑞̇
−𝑚 sın
Coriolis force vector Centrifugal force vector
0
𝒈(𝒒) =
0

Professor Slim CHOURA Page 3 of 3 ROBOTICS II