$30
Mobile Robotics-Assignment 1 Transformations Solved
1. Getting started with Open3D
1.1 Converting RGBD image into Point Cloud
In your robotics journey, it is common to be given just the depth images along with camera parameters in a generic dataset and you'd want to build a 3D data representation out of it, for example, a point cloud. You will understand the math behind these concepts in detail during Vision classes, for now, you can use the in-built functions as a black box.
In [ ]:
##############################################################################
# TODO: Do tasks described in 1.1 #
##############################################################################
# Replace "pass" statement with your code pass
##############################################################################
# END OF YOUR CODE # ##############################################################################
Question for 1.1
In the next code cell, call the function one_one here showing scene.pcd .
In [ ]:
2. Rotations, Euler angles and Gimbal Lock
2.1 Rotating an object
The objective here is to roughly simulate an object moving on a ground.
Generate a cube at some point on the ground and create another frame 𝐵 at the center of this object.
Combine these both as a single point cloud cube.pcd . (You can pick a point on the ground by using the get_picked_points method of the class open3d.visualization.VisualizerWithEditing .) Now read both the point clouds scene.pcd and cube.pcd in a script. Whatever tasks you do below are on the object cube.pcd (along with the axes 𝐵) with scene.pcd in the background (static). Given a sequence of ZYX Euler angles [30∘, 90∘, 45∘], generate the rotation. In our case, our object (with its respective axis) undergoes rotation with the background being fixed (with its respective axis).
Note: Throughout this assignment, we will be using the standard ZYX Euler angle convention.
Write a function two_one to show the above by animation (cube rotating along each axis one by one).
Hint: Use Open3D's non-blocking visualization and discretize the rotation to simulate the animation. For example, if you want to rotate by 30∘ around a particular axis, do in increments of 5∘ 6 times to make it look like an animation.
In [ ]:
##############################################################################
# TODO: Do tasks described in 2.1 #
##############################################################################
# Replace "pass" statement with your code pass
##############################################################################
# END OF YOUR CODE # ##############################################################################
Question for 2.1
In the next code cell, call the function two_one here showing the animation described in section 2.1.
In [ ]:
2.2 Euler angle & Gimbal lock
Code the following yourself from scratch (Refer Craig book - Section: 𝑍 − 𝑌 − 𝑋 Euler angles - same conventions/notations followed).
Case 1: Given the rotation matrix 𝑀𝑔𝑖𝑣𝑒𝑛 below, extract Euler angles 𝛼, 𝛽, 𝛾. Convert it back to the rotation matrix 𝑀𝑟𝑒𝑐𝑜𝑣𝑒𝑟𝑒𝑑 from Euler angles.
0.26200263 −0.19674724 0.944799
𝑀(𝛼, 𝛽, 𝛾) = 0.21984631 0.96542533 0.14007684
−0.93969262 0.17101007 0.29619813
After coding it from scratch, check your calculations using scipy.spatial.transform.Rotation .
(Mandatory)
Case 2: Given the rotation matrix 𝑁𝑔𝑖𝑣𝑒𝑛, extract Euler angles, and convert back 𝑁𝑟𝑒𝑐𝑜𝑣𝑒𝑟𝑒𝑑.
0 −0.173648178 0.984807753
𝑁(𝛼, 𝛽, 𝛾) = 0 0.984807753 0.173648178
−1 0 0
Again use scipy and check its output. If scipy is showing any warnings on any of the above cases, explain it in "Questions for 2.2" (last question). Write code in the next cell.
(Optional) Case 3: Do the above two for quaternion using scipy functions, i.e. given the rotation matrix, extract quaternion and convert back.
In [ ]:
##############################################################################
# DON'T EDIT
M_given = np.array([[0.26200263, -0.19674724, 0.944799],
[0.21984631, 0.96542533, 0.14007684],
[-0.93969262, 0.17101007, 0.29619813]])
N_given = np.array([[0,-0.173648178,0.984807753],
[0, 0.984807753, 0.173648178],
[-1, 0, 0]])
In [ ]:
# TODO: Do tasks described in 2.2 #
##############################################################################
# Replace "pass" statement with your code pass
##############################################################################
# END OF YOUR CODE # ##############################################################################
Questions for 2.2
Have you used np.arctan or an any equivalent atan function above? Why or why not? Ans: your answer here
For Case 1 above,
What Euler angles 𝛼, 𝛽, 𝛾 did you get? Replace my_array_case1 with your array.
In [ ]:
# Uncomment and replace my_array_case1 with your array.
#print("My Euler angles for case 1 are" + str(my_array_case1))
Were you able to recover back your rotation matrix when you converted it from Euler angles? Why/why not? Replace M_given and M_recovered with your matrices below and explain "why/why not" after this code snippet.
In [ ]:
# Uncomment and Replace M_given and M_recovered with your matrices below.
#error = np.linalg.norm(logm(M_given @ M_recovered.T))
#print("For case 1, it is " + str(error<0.0001) + " I could recover the original ma
Why/why not? Based on your observations here, is there any problem with Euler angle representation for Case 1? If yes, what is it?
Ans: your answer here
Repeat the above for Case 2.
In [ ]:
# Uncomment and Replace N_given and N_recovered with your matrices below.
# print("My Euler angles for case 2 are" + str(my_array_case2))
# error = np.linalg.norm(logm(N_given @ N_recovered.T))
# print("For case 2, it is " + str(error<0.0001) + " I could recover the original m
Why/why not? Based on your observations here, is there any problem with Euler angle representation for Case 2? If yes, what is it?
Ans: your answer here.
Explain any more problems with Euler angle representation. Explain what you understand by Gimbal lock (concisely in your own words). You could revisit this question in the section 2.4.
Ans: your answer here.
When you used scipy.spatial.transform.Rotation for the above 2 cases,
Have you used zyx above in r.as_euler('') argument? Why or why not? Explain the difference between extrinsic and instrinsic rotations with equivalent technical names from Craig book?
Ans: your answer here.
Has scipy shown any warnings on any of the above cases? If yes, explain it.
Ans: your answer here.
(Optional) For Case 3 above (quaternion) which you did using scipy, did you observe any problem with quaternion? Depending on your observations, which is better? Quaternion or Euler angles? And why?
Ans: your answer here.
Consider the frame 𝑋𝑌 𝑍 in the above image. Say you have a vector 𝑥 . Now you want to
rotate it such that you end up at 𝑥2 = [1, 1, 1] through a sequence of Euler angle rotations. Your goal is to find out those 𝛼, 𝛽 & 𝛾 (𝑍𝑌 𝑋). We will follow this order whenever we refer to it below.
First, properly understand the so-called "Rotator-transform equivalence" to figure out what are the terms of rotation matrix. Then, put the math on paper and you will end up with a set of non-linear equations. Write the set of linear equations in LaTeX here:
𝐀𝐧𝐬: 𝐘𝐨𝐮𝐫 𝐬𝐞𝐭 𝐨𝐟 𝐞𝐪𝐮𝐚𝐭𝐢𝐨𝐧𝐬 𝐡𝐞𝐫𝐞
Solve these equations using fsolve from scipy.optimize as follows: (Come back and answer the following questions after coding it in the next block)
case1 : First, solve it with an initialization of (0,0,0). Check if your answer is correct using np.isclose .
What Euler angles did you get? Answer in 𝛼, 𝛽 & 𝛾 format: Ans: 𝑎1, 𝑏1, 𝑐1 = your answer here.
case2 : Now, forget about the solver for a moment: Can you visualize and think of sequence of rotations
one by one to reach the final position (which is different than previous set of rotations)? Now, validate your answer by giving (your answer ±5) as initialization.
What Euler angles did you get? Answer in 𝛼, 𝛽 & 𝛾 format:
Ans: 𝑎2, 𝑏2, 𝑐2 = your answer here.
In [ ]:
##############################################################################
# DON'T EDIT
x_1 = np.array([0,np.sqrt(3),0]) x_2 = np.array([1,1,1])
In [ ]:
##############################################################################
# TODO: Do tasks described in 2.3 #
##############################################################################
# Replace "pass" statement with your code pass
##############################################################################
# END OF YOUR CODE # ##############################################################################
In [ ]:
# From Section 2.2, use the function which takes Euler angles and gives Rotation ma
# Uncomment and replace `r_mat` with the name of the function. (Do NOT edit anythin
#x_2_obtained_case1 = r_mat(a1,b1,c1) @ x_1.T #TODO: replace r_mat #x_2_obtained_case2 = r_mat(a2,b2,c2) @ x_2.T #TODO: replace r_mat test = False #TODO: Set this as True
𝛽
Therefore, when the code is run, there should be a minimum of (2 × (number of values of 𝛽)) animations. 2 for values of 𝛼, you could show it for even more if you wish to.
In [ ]:
##############################################################################
# TODO: Do tasks described in 2.4 #
##############################################################################
# Replace "pass" statement with your code pass
##############################################################################
# END OF YOUR CODE #
##############################################################################
VOILA! You have just animated the famous Gimbal lock problem. If you are curious, read about the Apollo 11 (https://en.wikipedia.org/wiki/Gimbal_lock#On_Apollo_11) Gimbal lock incident.
Questions for 2.4 (Optional)
Mention the value(s) of 𝛽 here:
Ans: your answer here.
Now that you understand gimbal lock through visualization, explain it now in matrix form: For the above values of 𝛽, what does the rotation matrix look like? Can you explain why gimbal lock occurs from the rotation matrix alone? Clue: Use sin/cos formulae.
Ans: your answer here.
Call the function two_four for the visualization of gimbal lock written above.
