EE106a - Spring 2016 Assignment 1

Assignment 1: Transformations in Computer Vision and

Robot Dynamics - Solutions
Due: Feb. 2, 2016 at 9:30am
This assignment covers transformations and material from MLS 4.1-4.3 & a bit of 4.5,
focusing on Robot Dynamics and Control. Your homework will be submitted
electronically through GradeScope. No hardcopies will be accepted.

We don’t mind if you work with other students on your homeworks. However, each
student must write up and turn in their own assignment (i.e. no copy & paste). If you
worked with other students, please acknowledge who you worked with at the top
of your homework.
Question 1. (10 points)
Using your knowledge of Lagrangians, derive the equations of motion for a double
pendulum (shown below). Assume that two masses connected together by massless
links and revolute joints. Check out MLS 4.1-4.2 to get started.

Figure 1: Double Pendulum

Answer:
First, we find the squared velocities of the masses:
v12 = l12 θ̇12
v22 = l12 θ̇12 + l22 (θ̇1 + θ̇2 )2 + 2l1 l2 θ̇1 (θ̇1 + θ̇2 ) cos θ2
Then, we find the kinetic and potential energies:
1 1
K = m1 l12 θ̇12 + m2 (l12 θ̇12 + l22 (θ̇1 + θ̇2 )2 + 2l1 l2 θ̇1 (θ̇1 + θ̇2 ) cos θ2 )
2 2
V = −m1 gl1 cos θ1 − m2 g(l1 cos θ1 + l2 cos(θ1 + θ2 ))

1
EE106a - Spring 2016 Assignment 1

Which in turn gives the Lagrangian:

L=K −V

Lagrange’s equation tells us:

d ∂L ∂L
− = 0, i = 1, 2
dt ∂ θ̇i ∂θi

assuming there are no external forces acting on the system.

Before taking the partial derivatives, let’s define some shorthand:

c1 = cos θ1 c12 = cos(θ1 + θ2 )

s1 = sin θ1 s12 = sin(θ1 + θ2 )

By doing some differentiation, we get:

∂L
= m1 l12 θ̇1 + m2 l12 θ̇1 + m2 l22 (θ̇1 + θ̇2 ) + m2 l1 l2 (θ̇1 + θ̇2 )c2 + m2 l1 l2 θ̇1 c2
∂ θ̇1
= (m1 l12 + m2 l12 + m2 l22 + 2m2 l1 l2 c2 )θ̇1 + (m2 l22 + m2 l1 l2 c2 )θ̇2
 
d ∂L
= (m1 l12 + m2 l12 + m2 l22 + 2m2 l1 l2 c2 )θ̈1 + (m2 l22 + m2 l1 l2 c2 )θ̈2
dt ∂ θ̇1
− 2m2 l1 l2 s2 θ̇1 θ̇2 − m2 l1 l2 s2 θ̇22
∂L
= − m1 gl1 s1 − m2 g(l1 s1 + l2 s12 )
∂θ1
∂L
= m2 l22 (θ̇1 + θ̇2 ) + m2 l1 l2 θ̇1 c2
∂ θ̇2
= (m2 l22 + m2 l1 l2 c2 )θ̇1 + m2 l22 θ̇2
 
d ∂L
= (m2 l22 + m2 l1 l2 c2 )θ̈1 + m2 l22 θ̈2 − m2 l1 l2 s2 θ̇1 θ̇2
dt ∂ θ̇2
∂L
= − m2 l1 l2 θ̇1 (θ̇1 + θ̇2 )s2 − m2 gl2 s12
∂θ2
Then, we can rewrite the equations of motion by the following equations:

Aθ̈1 + B θ̈2 =2m2 l1 l2 s2 θ̇1 θ̇2 + m2 l1 l2 s2 θ̇22 − m1 gl1 s1 − m2 g(l1 s1 + l2 s12 )

B θ̈1 + C θ̈2 =m2 l1 l2 s2 θ̇12 − m2 gl2 s12

where
A =m1 l12 + m2 l12 + m2 l22 + 2m2 l1 l2 c2
B =m2 l22 + m2 l1 l2 c2
C =m2 l22

2
EE106a - Spring 2016 Assignment 1

Question 2. (10 points)

A great number of the tools we’ll learn in this class can be applied to other fields. One
such example is Computer Vision. Cameras are often modeled as Pinhole Cameras (see
Figure 2), that project points of light in the real world onto the image plane. Suppose
we have a point in an image, denoted (x, y), that corresponds to a point in the real
world, denoted (X, Y, Z). After a little geometry, we get the following relations:

X Y
x=f y=f
Z Z
where f is the focal point of the camera. As you can see, if we are just given a point in
an image, solving for the point in the real world is underdetermined.

Figure 2: Example of Pinhole Camera Projection. Credit to Prof. Malik.

Let’s assume we have two cameras that are taking pictures of the same object.
Suppose the change in perspective of camera 1 to camera 2 is given by a rotation R
and translation t.

Show how we can map point x1 = (x1 , y1 ) in the image from camera 1 to the image
from camera 2. Then, show how we can recover the real world coordinates (X, Y, Z).

Answer:
The key to this problem is recognizing that the point in camera 1 x1 = (x1 , y1 ) maps
to X1 = (X, Y, Z), in the coordinate frame of camera 1. That means to map this to a
point in the image plane of camera 2 you have to transform X1 to the coordinate frame
of camera 2 and then project it into the image plane. To do this, you’ll have to assume
you have an idea of what Z is, as was hinted on Piazza.
First, solve for X and Y :
X1 = Zx1 /f
Y1 = Zy1 /f

3
EE106a - Spring 2016 Assignment 1

Then rotate this into the new coordinate frame:

 
Zx1 /f
X2 = R  Zy1 /f  + t
Z

All that is left is to project this new point into the image plane of camera 2, by using
the given relationships x2 = f X2 /Z and y2 = f Y2 /Z.

For the second part of the problem, you can derive a system of equations to solve for
the real-world point, using the relationships given for a single image, the translations
found between the two cameras, and a little algebra. Note that you could also have
solved this problem, using homography notation, as presented in Problem 3.

You may have recognized that this is part was attempting to solve the stereovision
problem (i.e. recovering depth from two cameras/perspectives), which is often used in
computer vision for robotics.
BONUS: (5 points)
You may notice that parallel lines in images converge to the same point in the image
plane. Show the mapping of this line to the image plane and show all parallel lines
converge to the same point.

Figure 3: Comic depicting a vanishing point.

Answer:
You can write a line as a point A and a direction vector D in a three dimensional space
     
X Ax Dx
 Y  =  Ay  + λ  D y 
Z Az Dz

4
EE106a - Spring 2016 Assignment 1

where λ is a paramter that sweeps from −∞ to ∞. If we substitute into our camera

projection equations:
X Ax + λDx
x=f =f
Z Az + λDz
and let λ → ∞, we see that:
λDx Dx
x=f =f
λDz Dz
which converges to a point regardless of point A, meaning that all lines with the same
direction vector (i.e. parallel lines) will converge to this same point.

Question 3. (15 points)

In class, the advantages of homogeneous coordinates were discussed. These are often
commonly used in computer vision (as was also mentioned). To convert image and
world coordinates to homogeneous coordinates, the following conversions are used:

Homogeneous Coordinates Image Coordinates World Coordinates

 
  X
x Y 
Convert TO: (x, y) → y  (X, Y, Z) → 
Z 

1
1
 
  X
x Y 
Convert FROM:  y  → (x/w, y/w)   → (X/W, Y /W, Z/W )
Z 
w
W

From this, we can see that the projection from real world point to image point is given
through a projection matrix:
 
  X  
1 0 0 0   X  
0 1 0 0 Y  → f ,fX Y
Z  = Y
  
Z Z
0 0 1/f 0 Z/f
1

Now let’s consider camera perspectives again. Mapping between different perspectives
is called a homography, assuming the same center of projection. Given a point p from
one perspective, we can map this to another perspective point p0 with homography H.
Formally:
0
 p 0  = Hp
  
wx h11 h12 h13 x
wy 0  = h21 h22 h23  y  (1)
w h31 h32 1 1
Recall from class that the entry h33 affects scaling, so is generally set to 1. Using this
idea, we can do some fun things like rectify images and play with perspective (Fig. 4
and 5).

5
EE106a - Spring 2016 Assignment 1

Figure 4: Example of Image Rectification. Credit to Prof. Malik.

Figure 5: Example of using a homography to generate virtual camera rotations.

Use MATLAB or your favorite programming language to play around with changing
perspectives of images. Provide some of the original images, how you chose your
homographies, the resulting images, and code snippets that you wrote. Briefly discuss
your implementation and observations you made. Have a little fun. Here’s a link for
some fun images to play with: www.julianbeever.net
For help getting started, see Lab 4 from EE106a on bCourses.

Answer:
Gold star for effort!

Question 4. (15 points)

Consider the passivity of robot dynamics of the form:

M (θ)θ̈ + C(θ, θ̇)θ̇ + N (θ, θ̇) = τ

as presented in MLS 4.3. Let H = T + V to be the total energy for a rigid robot. Show
that if Ṁ − 2C is skew-symmetric, then energy is conserved, meaning that Ḣ = θ̇> τ .

6
EE106a - Spring 2016 Assignment 1

In a sentence or two, briefly discuss the importance of the passivity property for
robotic manipulators.

Answer:
Using the derivations for T and V on page 168 of MLS, we take the derivative of H:
1 1 1 ∂V
Ḣ = θ̈> M (θ)θ̇ + θ̇> Ṁ (θ)θ̇ + θ̇> Ṁ (θ)θ̈ + θ̇(θ)
2 2 2 ∂θ
1 ∂V
= θ̇> M (θ)θ̈ + θ̇> Ṁ θ̇ + θ̇> (θ)
2 ∂θ

Substituting in for M (θ)θ̈ :

∂V 1 ∂V
Ḣ = θ̇> (τ − C(θ, θ̇)θ̇ − (θ)) + θ̇> Ṁ θ̇ + θ̇> (θ)
 ∂θ  2 ∂θ
1
= θ̇> τ + θ̇> Ṁ − C(θ, θ̇) θ̇
2
= θ̇> τ

Passivity is a very important concept in controls and robotics. In robots (and generally
in mechanical systems), passivity relates to the energy dissipation of the system,
meaning that the system is not generating energy, and therefore ties into stability and
tracking, as we talked about in discussion.

Question 5. Show that the workspace version of the PD control law results in
exponential stability in terms of trajectory tracking. First, derive the workspace
dynamics and associated closed loop control. Then, prove stability.

Answer:
Following the transformations given in the book, we can represent the workspace
dynamics in the following form:

M̃ (θ)θ̈ + C̃(θ, θ̇)θ̇ + Ñ (θ, θ̇) = τ

Using this, we can use our computed torque law or the augmented PD control law to
control the system.
The augmented PD law is given by:

τd = M̃ (θ)ẍd + C̃(θ, θ̇)ẋd + Ñ (θ, θ̇) − kv ė − kp e

We then find the closed loop dynamics:

M̃ (θ)ëd + (C̃(θ, θ̇) + kv )ė + kp e = 0

7
EE106a - Spring 2016 Assignment 1

The book gives a proof of stability using Lyapunov arguments, which were not covered
in this class. In discussion, we showed how to find stability of the system by analyzing
the dynamics. We can apply similar techniques, but there is one trick piece here:
dealing with M̃ (θ) and C̃(θ, θ̇). If you recall, when using the computed torque law this
term was cancelled out and results in nice linear error dynamics. Since we’re not
canceling this out, we can do a few things: (1) drop dependence on θ (see MLS pg.
197) and continue solving as we would normally; or (2) linearize M̃ (.) and C̃(.) about
some operating point θ0 and proceed.
We find the following error dynamics:
    
d e −1 0 I e
= M̃
dt ė kp C̃ + kv ė
By analyzing how this system evolves (i.e. checking the eigenvalues of the transition
matrix), we’ll find that the errors decay away and our system is stable. In practice, the
eigenvalues of such a matrix would be computed using numerical methods such as the
Power Method or QR Algorithm described in Numerical Computation of Eigenvalues:
http://www.math.umn.edu/˜olver/num /lnqr.pdf.