Algorithmic Robotics Assignment #4

EECS 498: Introduction to Algorithmic Robotics

Homework Assignment #4

Rules:
1. All homework must be done individually, but you are encouraged to post questions on Piazza.
2. No late homework will be accepted.
3. The goal of this homework is to develop your understanding of the algorithms presented in class.
You should use python to implement solutions. You may not use any other language, only python
will be accepted.
4. Submit your zipped python files along with a pdf of your answers to Gradescope. Do not paste
your code into your pdf.
5. Remember that copying-and-pasting code from other sources is not allowed.
Questions
1. (10 points) LaValle Book, Chapter 14 Question 3
2. (10 points) Explain why it is more difficult to define a distance metric that leads to good performance
for a non-holonomic motion planning problem than a holonomic one. Use a car as an example that
illustrates this difficulty and include diagrams.
3. (10 points) Consider a robot finger making a point contact on a plane made of homogeneous material. The friction coefficient between the finger and the plane is unknown and we would like to
determine it. Assume that the robot can apply any force at the contact point and can sense the position of the contact point perfectly. Assuming the standard Coulomb friction model, write down
an algorithm in pseudo-code for applying a series of forces at the contact that would determine the
friction coefficient.
Software
1. Download and unzip HW4files.zip. Run test utils.py. You should see a cloud of points in blue,
that same cloud translated by (1, 1, 1) in red, and a plane in green. If you do not see this, make
sure matplotlib is installed. Refer to this script when you need to plot points and planes for the
implementation problems involving point clouds.
2. You will need to consult the openrave.py documentation to complete this assignment.
1
Implementation
The following implementation problems should be done in python starting from the provided templates.
Only edit what is inside the ### YOUR CODE/IMPORTS HERE ### block. Feel free to look around the
internet and the openrave installation for example code for reference, especially in the openrave examples,
but you should implement your own code from scratch unless explicitly stated otherwise. Include all
code you write in your zip file.
1. We will now implement a Jacobian-based Inverse Kinematics Solver for the seven joints of the PR2’s
left arm. To simplify this problem, we will ignore end-effector rotation and use only the position
variables in the Jacobian. We will also ignore collision constraints. Put your code in ik template.py.
a (15 points) Assume the robot has been placed in a certain configuration and you would like to
compute the Jacobian at that configuration. Write a function to compute the Jacobian numerically,
using the method described in class. Do this by filling in the code in the GetTranslationJacobian
function. You will need to use the GetJointAxis, GetJointPosition, and GetEETransform functions defined in the template. Do NOT use openrave’s built-in Jacobian functions.
b (10 points) Now write a function to compute the pseudo-inverse of the Jacobian. Do this by
filling in the GetJpinv function in the template. Because the robot may get into singularities, use
the damped least-squares method with a small λ.
c (25 points) Implement the Iterative Jacobian Pseudo-Inverse Inverse Kinematics algorithm from
lecture. You will need to tune the parameters. In the template you can select one of five endeffector position targets. Try to make your algorithm achieve all the targets when starting from
q = {0, 0, 0, 0, 0, 0, 0}. Do not allow q to exceed joint limits; if the algorithm goes past a joint limit,
set that joint’s value to be the limit. For each target you can achieve, save the configuration that
places the end-effector at that position and a screenshot of the robot in that configuration in your
pdf.
d (15 points) Make a copy of your code called ik template nullspace.py. Modify your algorithm
in this file to use the left null-space of the Jacobian pseudo-inverse to repel the configuration away
from joint limits while achieving the desired end-effector position. You will need to tune β so that
the secondary task of repeling from joint limits does not interfere with task of reaching the target.
For each target you can achieve, save the configuration that places the end-effector at that position
and a screenshot of the robot in that configuration in your pdf.
2. Run pca template.py. Here a point cloud of a randomly-sampled planar surface has been rotated
and noise has been added to it. Rotate the plot to get a sense of how the points are distributed. Edit
the template to accomplish the following.
a (15 points) Use the Principle Component Analysis (PCA) algorithm to compute a rotation for the
point cloud that aligns the surface in the point cloud with the XY plane as best as possible. Save
a picture of the rotated point cloud in your pdf along with the V
T matrix you applied at the last
step. Adjust the view in the plot so it is clear that the point cloud aligns with the XY plane.
b (10 points) Use PCA to do the same rotation while also eliminating the noise in the data. The
resulting point cloud should be two-dimensional. You will need to set the threshold for variances
2
to properly filter out the noise. Save a picture of the point cloud after PCA is applied in your pdf
along with the matrix V
T
s you applied at the last step. Adjust the view in the plot so it is clear that
the point cloud aligns with the XY plane and all the z values are 0.
c (10 points) Use PCA to fit a plane to the cloud and draw that plane in green in a plot with the
point cloud. Include the plot in your pdf.
3. Run ransac template.py. Here a point cloud of a randomly-sampled planar surface has been rotated and noise has been added to it. We have also added some outlier points to the cloud. Rotate the
plot to get a sense of how the points are distributed. Edit the template to accomplish the following.
a (30 points) Use the RANSAC algorithm from lecture to find the equations of a plane that best fits
the surface in the point cloud. You should select three points at each iteration to fit the model
of the plane. The Error function for inliers should encode least-squares error, i.e. “the sum
of squared residuals (a residual being the difference between an observed value, and the fitted
value provided by a model). (Wikipedia)” You will need to set the number of iterations and the
other parameters.
Once you have computed the plane that best fits the data, include the equations for that plane in
your pdf. Also include an image showing the plane in green fitting to the data, the inliers for that
plane in red, and all other points in blue. Rotate the plot to clearly show the plane fitting to the
data.
4. Now we will compare RANSAC and PCA with varying amounts of outliers. Run pca vs ransac.py
to see a series of point clouds with added outliers. At each iteration, we add 10 outliers to the cloud.
At each iterations where outliers are added, run your PCA and RANSAC algorithms from above so
that each produces a plane that fits to the cloud. Use the Error function from RANSAC to compute
the error of each algorithm’s plane to its inliers. Determine the inliers for PCA the same way as was
done in the RANSAC algorithm.
a (10 points) For the last iteration, generate two plots: 1) Showing PCA’s plane fitting to the data
in green, the inliers for that plane in red, and all other points in blue; 2) Likewise for RANSAC.
Clearly label the plots so it is clear which one corresponds to which algorithm.
b (15 points) Generate a plot of Error vs. Number of Outliers for both algorithms and include it in
your pdf. Also record the computation times of PCA and RANSAC at each iteration.
c (15 points) Discuss the performance difference between the algorithms. Was there a clear performance difference in terms of computation time and error? If so, which algorithm performed
better and why? If not, explain why not.
5. Run icp template.py to see a source and target point cloud of a mug. Implement the ICP algorithm to align a source point cloud to a target point cloud. Use Euclidean distance to determine
correspondences at every iteration. You will need to tune the parameters and determine a reasonable termination condition.
a (40 points) In the template you will see a way to load in one of four target point clouds. Run ICP
on each target cloud separately. You may tune parameters separately for each target. You may
not be able to produce a good alignment for all the targets, but try to make the source align to as
many targets as you can.
3
b (10 points) For each target, generate 1) A plot of Error vs. Iteration of ICP; 2) A plot showing the
target point cloud and the source point cloud at the final iteration (in different colors). Include
these in your pdf.
c (15 points) Discuss the results; which targets were more difficult for ICP and why?
4