Review Rigid Transformations, Kinematics, and Dynamics Due: Thursday Feb

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:

3 Part a. Let ω ∈ R such that kωk2 = 1. Then   s ω3 −ω2 2 det(sI − ωˆ) = det  −ω3 s ω1  = s(s − 1) ω2 −ω1 s

2 Solving s(s − 1) = 0 yields the eigenvalues s1 = 0, s2 = +i, s3 = −i. 3 Let (x1, x2, ω) be a right-handed orthonormal basis for R . 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 eωθˆ v = (I +ωθ ˆ + (ˆωθ)2 + ...)v i 2! i 1 = v + s θv + (s θ)2v + ... i i i 2! i i siθ = e vi

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(r22r33 − r23r32) + r21(r32r13 − r12r33) + r31(r12r23 − r22r13) = 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

3 3 (a) Let x1, ..., xN ∈ R be a set of observed points in the world and y1, ..., yN ∈ R be a set of target points, where xi corresponds to point yi. Derive a closed-form solution to:

N ∗ ∗ X 2 R , t = argmin kyi − (Rxi + t)k2 3 R∈SO(3),t∈R i=1

Hint: You can use the known solution to the orthogonal Procrustes problem, https://en.wikipedia.org/wiki/Orthogonal Procrustes problem.

3 (b) In real settings we often do not have perfect point matches. Let x1, ..., xN ∈ R be a 3 set of observed points in the world and y1, ..., yN ∈ R 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:

N ∗ ∗ X 2 R , t = argmin min kyj − (Rxi + t)k2 3 j=1,...,N R∈SO(3),t∈R i=1 Justify your answer.

Answer: Part a. N N 1 P 1 P Let x¯ = N xi and y¯ = N yi. i=1 i=1 Then the solution is

N X T H = (xi − x¯)(yi − y¯) i=1 U, S, V = SVD(H) R∗ = VU T t∗ = y¯ − R∗x¯ where SVD is the singular value decomposition. It is possible for the SVD to return a reﬂection matrix, so in practice one must check the determinant of VU T and multiply the third column of V by −1 if det(VU T ) = −1. One can arrive at this solution via coordinate descent. We ﬁrst solve for t0 by setting the ∗ gradient wrt t equal to zero which gives t0 = y¯ − Rx¯, where R is a free variable. Then we ∗ can solve for the optimal rotation with respect to t0 using the solution to the orthogonal Procrustes problem which gives R∗ = VU T . Finally we can solve for t∗ given R∗ by again setting the gradient equal to zero, yielding t∗ = y¯ − 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. Speciﬁcally:

1. R0 = I, t0 = (0, 0, 0)

2 EE106b - Spring 2017 Assignment 1

2. While not converged

2 (a) Solve yi,k = argminjkyj − (Rkxi + tk)k2

(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 1 L = K − V = m x˙ 2 + `2θ˙2 + 2`xθ˙ cos θ + mg` cos θ 2 Substitution to Lagrange’s equations yields:

mx¨ + m`θ¨cos θ − m`θ˙2 sin θ = 0 m`2θ¨ + m`x¨ cos θ + mg` sin θ = 0

Part b. First, we ﬁnd the squared velocities of the masses:

2 2 ˙2 v1 = l1θ1 2 2 ˙2 2 ˙ ˙ 2 ˙ ˙ ˙ v2 = l1θ1 + l2(θ1 + θ2) + 2l1l2θ1(θ1 + θ2) cos θ2 Then, we ﬁnd the kinetic and potential energies: 1 K = m l22 + m (l22 + l2( + )2 + 2l l ˙ ( + ) cos ) 2 1 11 2 11 2 1 2 1 21 1 2 2 V = −m1gl1 cos1 −m2g(l1 cos1 +l2 cos(1+2))

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 deﬁne some shorthand:

c1 = cos1 c12 = cos(1+2)

s1 = sin1 s12 = sin(1+2)

4 EE106b - Spring 2017 Assignment 1

By doing some diﬀerentiation, we get:

2 2 2 L˙1 = m1l1˙1 + m2l1˙1 + m2l2(˙1 +˙2) + m2l1l2(˙1 +˙2)c2 + m2l1l2˙1c2 2 2 2 2 = (m1l1 + m2l1 + m2l2 + 2m2l1l2c2)˙1 + (m2l2 + m2l1l2c2)˙2 2 2 2 2 (L˙1) = (m1l1 + m2l1 + m2l2 + 2m2l1l2c2)¨1 + (m2l2 + m2l1l2c2)¨2 2 − 2m2l1l2s212 − m2l1l2s22 ∂L = − m1gl1s1 − m2g(l1s1 + l2s12) ∂1 2 L2 = m2l2(˙1 +˙2) + m2l1l21c2 2 2 = (m2l2 + m2l1l2c2)1 + m2l22 2 2 (L˙2) = (m2l2 + m2l1l2c2)¨1 + m2l2¨2 − m2l1l2s212

L2 = − m2l1l2˙1(˙1 +˙2)s2 − m2gl2s12

Then, we can rewrite the equations of motion by the following equations:

2 A¨1 + B¨2 =2m2l1l2s212 + m2l1l2s22 − m1gl1s1 − m2g(l1s1 + l2s12) 2 B¨1 + C¨2 =m2l1l2s21 − m2gl2s12 where 2 2 2 A =m1l1 + m2l1 + m2l2 + 2m2l1l2c2 2 B =m2l2 + m2l1l2c2 2 C =m2l2

5 EE106b - Spring 2017 Assignment 1

Figure 1: Solution from MLS.