def test_03_compute_convex_hull_volume(self):
"""Tests the convex hull volume is correctly computed on a small sample set"""
random.seed(42)
np.random.seed(42)
from grasp_metrics import compute_convex_hull_volume
points = [
np.asarray([-1.0, 0.]),
np.asarray([1.0, 0.]),
np.asarray([0.0001, 0.0001])
]
volume = compute_convex_hull_volume(points)
self.assertTrue(
abs(volume) < 1e-2, "This test should have a tiny volume"
"it just has points: " + str(points))
points = [
np.asarray([0, 0]),
np.asarray([1.0, 0]),
np.asarray([0, 1]),
np.asarray([1, 1])
]
volume = compute_convex_hull_volume(points)
self.assertTrue(
abs(volume - 1) < 1e-3, "This test should have a volume of 1"
"it has points: " + str(points))
for i in range(100):
points.append(np.random.rand(2))
volume = compute_convex_hull_volume(points)
self.assertTrue(
abs(volume - 1) < 1e-3, "This test should still have a volume of 1"
"The points that were added are only on the interior.")
def test_02_compute_convex_hull_volume_two_points(self):
"""Tests the convex hull volume is 0 for two points"""
from grasp_metrics import compute_convex_hull_volume
points = [np.asarray([-1.0,0.]), np.asarray([1.0,0.])]
volume = compute_convex_hull_volume(points)
self.assertTrue(abs(volume) < 1e-9, "The volume of two points"
"should be zero, with no error")
def PlanGraspPoints(self):
# First, extract the bounding geometry of the object.
# Generally, our geometry is coming from 3d models, so
# we have to do some legwork to extract 2D geometry. For
# the examples we'll use in this set, we'll assume
# that extracting the convex hull of the first visual element
# is a good representation of the object geometry. (This is
# not great! But it'll do the job for us, since we're going
# to experiment with only simple objects.)
kinsol = self.hand.doKinematics(
self.x_nom[0:self.hand.get_num_positions()])
self.manipuland_link_index = \
self.hand.FindBody(self.manipuland_link_name).get_body_index()
body = self.hand.get_body(self.manipuland_link_index)
# For projecting into XY plane
Tview = np.array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 0.,
1.]])
all_points = ExtractPlanarObjectGeometryConvexHull(body, Tview)
# For later use: precompute the fingertip positions of the
# robot in the nominal posture.
nominal_fingertip_points = np.empty((2, self.num_fingers))
for i in range(self.num_fingers):
nominal_fingertip_points[:, i] = self.hand.transformPoints(
kinsol, self.fingertip_position, self.finger_link_indices[i],
0)[0:2, 0]
# Search for an optimal grasp with N points
random.seed(42)
np.random.seed(42)
def get_random_point_and_normal_on_surface(all_points):
num_points = all_points.shape[1]
i = random.choice(range(num_points - 1))
first_point = np.asarray([all_points[0][i], all_points[1][i]])
second_point = np.asarray(
[all_points[0][i + 1], all_points[1][i + 1]])
first_to_second = second_point - first_point
# Clip to avoid interpolating close to a corner
interpolate_param = np.clip(np.random.rand(), 0.2, 0.8)
rand_point = first_point + interpolate_param * first_to_second
normal = np.array([-first_to_second[1], first_to_second[0]]) \
/ np.linalg.norm(first_to_second)
return rand_point, normal
best_conv_volume = 0
best_points = []
best_normals = []
best_finger_assignments = []
for i in range(self.n_grasp_search_iters):
grasp_points = []
normals = []
for j in range(self.num_fingers):
point, normal = \
get_random_point_and_normal_on_surface(all_points)
# check for duplicates or close points -- fingertip
# radius is 0.2, so require twice that margin to avoid
# intersection fingertips.
num_rejected = 0
while min([1.0] + [
np.linalg.norm(grasp_point - point, 2)
for grasp_point in grasp_points
]) <= 0.4:
point, normal = \
get_random_point_and_normal_on_surface(all_points)
num_rejected += 1
if num_rejected > 10000:
print "Rejected 10000 points in a row due to crowding." \
" Your object is a bit too small for your number of" \
" fingers."
break
grasp_points.append(point)
normals.append(normal)
if achieves_force_closure(grasp_points, normals, self.mu):
# Test #1: Rank in terms of convex hull volume of grasp points
volume = compute_convex_hull_volume(grasp_points)
if volume < best_conv_volume:
continue
# Test #2: Does IK work for this point?
self.grasp_points = grasp_points
self.grasp_normals = normals
# Pick optimal finger assignment that
# minimizes distance between fingertips in the
# nominal posture, and the chosen grasp points
grasp_points_world = self.transform_grasp_points_manipuland(
self.x_nom)[0:2, :]
prog = MathematicalProgram()
# We'd *really* like these to be binary variables,
# but unfortunately don't have a MIP solver available in the
# course docker container. Instead, we'll solve an LP,
# and round the result to the nearest feasible integer
# solutions. Intuitively, this LP should probably reach its
# optimal value at an extreme point (where the variables
# all take integer value). I've not yet observed this
# not occuring in practice!
assignment_vars = prog.NewContinuousVariables(
self.num_fingers, self.num_fingers, "assignment")
for grasp_i in range(self.num_fingers):
# Every row and column of assignment vars sum to one --
# each finger matches to one grasp position
prog.AddLinearConstraint(
np.sum(assignment_vars[:, grasp_i]) == 1.)
prog.AddLinearConstraint(
np.sum(assignment_vars[grasp_i, :]) == 1.)
for finger_i in range(self.num_fingers):
# If this grasp assignment is active,
# add a cost equal to the distance between
# nominal pose and grasp position
prog.AddLinearCost(
assignment_vars[grasp_i, finger_i] *
np.linalg.norm(grasp_points_world[:, grasp_i] -
nominal_fingertip_points[:,
finger_i]))
prog.AddBoundingBoxConstraint(
0., 1., assignment_vars[grasp_i, finger_i])
result = Solve(prog)
assignments = result.GetSolution(assignment_vars)
# Round assignments to nearest feasible set
claimed = [False] * self.num_fingers
for grasp_i in range(self.num_fingers):
order = np.argsort(assignments[grasp_i, :])
fill_i = self.num_fingers - 1
while claimed[order[fill_i]] is not False:
fill_i -= 1
if fill_i < 0:
raise Exception("Finger association failed. "
"Horrible bug. Tell Greg")
assignments[grasp_i, :] *= 0.
assignments[grasp_i, order[fill_i]] = 1.
claimed[order[fill_i]] = True
# Populate actual assignments
self.grasp_finger_assignments = []
for grasp_i in range(self.num_fingers):
for finger_i in range(self.num_fingers):
if assignments[grasp_i, finger_i] == 1.:
self.grasp_finger_assignments.append(
(finger_i, np.array(self.fingertip_position)))
continue
qinit, info = self.ComputeTargetPosture(
self.x_nom,
self.x_nom[(self.nq - 3):self.nq],
backoff_distance=0.2)
if info != 1:
continue
best_conv_volume = volume
best_points = grasp_points
best_normals = normals
best_finger_assignments = self.grasp_finger_assignments
if len(best_points) == 0:
print "After %d attempts, couldn't find a good grasp "\
"for this object." % self.n_grasp_search_iters
print "Proceeding with a horrible random guess."
best_points = [
np.random.uniform(-1., 1., 2) for i in range(self.num_fingers)
]
best_normals = [
np.random.uniform(-1., 1., 2) for i in range(self.num_fingers)
]
best_finger_assignments = [(i, self.fingertip_position)
for i in range(self.num_fingers)]
self.grasp_points = best_points
self.grasp_normals = best_normals
self.grasp_finger_assignments = best_finger_assignments