EE106b - Spring 2017
Assignment 1
Assignment 1: Review Rigid Transformations,
Kinematics, and Dynamics
Due: Thursday Feb. 2, 2017 at 9:30am
This assignment covers material from MLS Chapters 2-4. 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 homework. 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)
Properties of rotation matrices. MLS Problem 2.3
Answer:
Part a. Let ω ∈ R3 such that kωk2 = 1. Then

s
ω3
s
det(sI − ω̂) = det  −ω3
ω2 −ω1

−ω2
ω1  = s(s2 − 1)
s
Solving s(s2 − 1) = 0 yields the eigenvalues s1 = 0, s2 = +i, s3 = −i.
Let (x1 , x2 , ω) be a right-handed orthonormal basis for R3 . Then the eigenvectors are
v1 = ω, v2 = x1 + ix2 and v3 = x1 − ix2 .
Part b. Let vi be an eigenvector of ω̂. The key idea is to expand out the exponential:
1
(ω̂θ)2 + ...)vi
2!
1
= vi + si θvi + (si θ)2 vi + ...
2!
= esi θ vi
eω̂θ vi = (I + ω̂θ +
Thus the eigenvalues of eω̂θ are 1, eiθ , and e−iθ , and the eigenvector whose eigenvalues are 1
is ω.
Part c. Let


r11 r12 r13
R =  r21 r22 r23 
r31 r32 r33
Using cofactors we get
det(R) = r11 (r22 r33 − r23 r32 ) + r21 (r32 r13 − r12 r33 ) + r31 (r12 r23 − r22 r13 )
= r1 · (r2 × r3 )
Question 2. (10 points)
Rigid point set registration. Robots often need to understand the location of known objects
in 3D space relative to their own body reference frame. The process of determining the best
estimate of the rigid transformation between observed points from a sensor and a set of
known points is called rigid point set registration. Applications of this method include
autonomous driving and 3D scanning.
1
EE106b - Spring 2017
Assignment 1
(a) Let x1 , ..., xN ∈ R3 be a set of observed points in the world and y1 , ..., yN ∈ R3 be a
set of target points, where xi corresponds to point yi .
Derive a closed-form solution to:
R∗ , t∗ =
argmin
N
X
R∈SO(3),t∈R3 i=1
kyi − (Rxi + t)k22
Hint: You can use the known solution to the orthogonal Procrustes problem,
https://en.wikipedia.org/wiki/Orthogonal Procrustes problem.
(b) In real settings we often do not have perfect point matches. Let x1 , ..., xN ∈ R3 be a
set of observed points in the world and y1 , ..., yN ∈ R3 be a set of target points with
unknown correspondences. One idea is to choose correspondences based on the closest
point, given the current transformation.
Propose an algorithm to solve:
R∗ , t∗ =
argmin
N
X
min kyj − (Rxi + t)k22
R∈SO(3),t∈R3 i=1 j=1,...,N
Justify your answer.
Answer:
Part a.
Let x̄ =
1
N
N
P
xi and ȳ =
i=1
1
N
N
P
yi .
i=1
Then the solution is
H=
N
X
(xi − x̄)(yi − ȳ)T
i=1
U, S, V = SV D(H)
R∗ = V U T
t∗ = ȳ − R∗ x̄
where SV D is the singular value decomposition. It is possible for the SV D to return a
reflection matrix, so in practice one must check the determinant of V U T and multiply the
third column of V by −1 if det(V U T ) = −1.
One can arrive at this solution via coordinate descent. We first solve for t0 by setting the
gradient wrt t equal to zero which gives t∗0 = ȳ − Rx̄, where R is a free variable. Then we
can solve for the optimal rotation with respect to t∗0 using the solution to the orthogonal
Procrustes problem which gives R∗ = V U T . Finally we can solve for t∗ given R∗ by again
setting the gradient equal to zero, yielding t∗ = ȳ − R∗ x̄. See this link for details.
Part b.
Multiple answers are acceptable. The most straightforward solution is to iterate between
solving the inner minimization problem and the outer minimization. Specifically:
1. R0 = I, t0 = (0, 0, 0)
2
EE106b - Spring 2017
Assignment 1
2. While not converged
(a) Solve yi,k = argminj kyj − (Rk xi + tk )k22
(b) Solve Rk , tk with respect to y1,k , ..., yN,k using the solution of part a
This algorithm converges to the optimal solution under certain conditions. For more
information, read the paper ”A method for registration of 3-D shapes” by Paul J. Besl and
Neil D. McKay in 1992.
3
EE106b - Spring 2017
Assignment 1
Question 3. (10 points)
Kinematics and Jacobians. MLS Problem 3.3
Answer:
See Figure 1.
Question 4. (10 points)
Equations of Motion. MLS Problem 4.1
Answer:
Part a.
The Lagrangian is given by
L=K −V =
1 2
m ẋ + `2 θ̇2 + 2`xθ̇ cos θ + mg` cos θ
2
Substitution to Lagrange’s equations yields:
mẍ + m`θ̈ cos θ − m`θ̇2 sin θ = 0
m`2 θ̈ + m`ẍ cos θ + mg` sin θ = 0
Part b.
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
m1 l12 21 + m2 (l12 21 + l22 (1 + 2 )2 + 2l1 l2˙1 (1 + 2 ) cos2 )
2
V = −m1 gl1 cos1 −m2 g(l1 cos1 +l2 cos(1 +2 ))
K=
Which in turn gives the Lagrangian:
L=K −V
Lagrange’s equation tells us:
Li − Li = 0,
i = 1, 2
assuming there are no external forces acting on the system.
Before taking the partial derivatives, let’s define some shorthand:
c1 = cos1
c12 = cos(1 +2 )
s1 = sin1
s12 = sin(1 +2 )
4
EE106b - Spring 2017
Assignment 1
By doing some differentiation, we get:
L˙1 = m1 l12˙1 + m2 l12˙1 + m2 l22 (˙1 +˙2 ) + m2 l1 l2 (˙1 +˙2 )c2 + m2 l1 l2˙1 c2
= (m1 l12 + m2 l12 + m2 l22 + 2m2 l1 l2 c2 )˙1 + (m2 l22 + m2 l1 l2 c2 )˙2
(L˙1 ) = (m1 l12 + m2 l12 + m2 l22 + 2m2 l1 l2 c2 )¨1 + (m2 l22 + m2 l1 l2 c2 )¨2
− 2m2 l1 l2 s212 − m2 l1 l2 s2 22
∂L
= − m1 gl1 s1 − m2 g(l1 s1 + l2 s12 )
∂1
L2 = m2 l22 (˙1 +˙2 ) + m2 l1 l21 c2
= (m2 l22 + m2 l1 l2 c2 )1 + m2 l22 2
(L˙2 ) = (m2 l22 + m2 l1 l2 c2 )¨1 + m2 l22¨2 − m2 l1 l2 s212
L2 = − m2 l1 l2˙1 (˙1 +˙2 )s2 − m2 gl2 s12
Then, we can rewrite the equations of motion by the following equations:
A¨1 + B¨2 =2m2 l1 l2 s212 + m2 l1 l2 s2 22 − m1 gl1 s1 − m2 g(l1 s1 + l2 s12 )
B¨1 + C¨2 =m2 l1 l2 s2 21 − 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
5
EE106b - Spring 2017
Assignment 1
Figure 1: Solution from MLS.
6