Description
Questions
1. (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.
Software
1. Download and unzip HW4files.zip. Run test utils.py in the pointclouds folder. 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. Install scipy using pip3 install scipy
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. 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. The code for this problem is in the ik folder. To simplify this problem, we will ignore endeffector rotation and use only the position variables in the Jacobian. We will also ignore collisions.
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 get translation jacobian
function in ik template.py in the ik folder. You will need to use the get joint axis, get joint position,
and get ee transform functions defined in the template. You may not use any functions to generate Jacobians directly from pybullet.
1
b (10 points) Now write a function to compute the pseudo-inverse of the Jacobian. Do this by filling
in the get jacobian pinv 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 in ik template.py. You will need to tune the parameters. In the template you can select
one of five end-effector 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. Force closure evaluation: The closure template.py script executes grasps using a 3-finger Robotiq
gripper and a 7-DOF Kuka iiwa arm simulated in PyBullet. The code for this problem is in the
grasping folder. You will be implementing friction cone calculation and a method that uses a convex
hull to determine if grasps are in force closure.
(a) (15 points) Implement the calculate friction cone and compare discretization functions
in closure template.py. The friction cone, shown below, can be calculated given a vector fz
which represents the normal force associated with the contact, a vector ft perpendicular to the
normal force vector, and a coefficient of friction µ. ft
is an arbitrary vector lying on the plane
perpendicular to fz.
We will discretize the friction cone to help calculate our set of wrenches.
B. Siciliano, O. Khatib, and T. Kroger, Springer handbook of robotics, vol. 200. Springer, 2008. ¨
To generate a vector f0 on the friction cone, we can rotate fz about ft by β, where β = tan−1
(µ).
To generate the full set of vectors to approximate the frictions code, we will then rotate f0 about
fz. We want n evenly spaced vectors to form the approximation to the cone. We can achieve
this by rotating f0 by the set of angles (2πi)/n, for i = 0…n − 1. Note that n = 4 in the figure
2
above.
To perform rotations, you should use the imported Rotation class from scipy. This class
allows you to define rotations which can then be applied to input vectors. Specifically, you
should use the Rotation.from rotvec method. This method takes in a vector to rotate about,
where the magnitude of the vector specifies the amount to rotate.
The number of vectors you choose (n) to include in your discretization will impact how close
the discretization is to the true friction cone. Implement the compare discretization function
in closure template.py to calculate the volumes of the true friction cone, a 4-vector approximation, and an 8-vector approximation.
In the pdf, include the volume of the true cone, 4-vector discretization, and 8-vector discretization for contact point 1 from Grasp 1. Round the volumes to 4 decimal places. Explain how
changing the number of vectors changes the error of the discretization.
Note that you only need to implement the friction cone and volume calculations. The provided code will run your implementation on a given contact point.
(b) (25 points) Implement the convex hull function in closure template.py to determine if grasps
are in force closure.
To do this, we first calculate wrenches acting on the center of mass of the object using the
friction cone vectors. We will calculate a wrench vector for each force vector in our friction
cone approximation. The wrench at the center of mass associated with a contact force f is:
”
f
(⃗r × f)/rmax#
(1)
where⃗r is the vector pointing from the center of mass to the location where the contact is applied.
The cross product with the force calculates the torque about the center of mass induced
by the contact. We divide the torque by the maximum radius rmax of the object to ensure that
the force and torque are in comparable units (Newtons). The code for converting from a contact force to a wrench on the object is provided.
To check for force closure, we will first construct a convex hull of the wrenches and then analyze its structure to determine if it contains the origin. We will use the Q-Hull algorithm to
compute a convex hull. Scipy provides an implementation in the ConvexHull class (when computing the convex hull, use the ’QJ’ parameter to prevent numerical issues).
The calculated
convex hull can be thought of as an intersection of halfspaces in the 6D wrench space. Q-Hull
provides the equations of these halfspaces, in ConvexHull.equations. Use these equations to
determine if the grasp being tested is in force closure. If the grasp is in force closure, calculate
the radius of the largest hypersphere centered at the origin that fits within the hull.
In the pdf, include an explanation of your method for checking force closure and comput3
ing the radius of the hypersphere. Also include the force closure output for each grasp and
the hypersphere radius if the grasp is in force closure. Explain why knowing the hypersphere
radius is useful.
You can run closure template.py to execute one of two grasps and evaluate your force closure
implementation. Run python3 closure template.py –g1 to evaluate Grasp 1. Run python3
closure template.py –g2 to evaluate Grasp 2. Include closure template.py in your submission zip file. We will be running the submitted closure template.py script on a new grasp to
verify the implementation.
3. Run pca template.py in the pointclouds folder. 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
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.
4. Run ransac template.py in the pointclouds folder. 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.
5. Now we will compare RANSAC and PCA with varying amounts of outliers. Run pca vs ransac.py
in the pointclouds folder 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.
6. Run icp template.py in the pointclouds folder 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.
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?
5
