def test_01_achieves_force_closure(self):
"""Test force closure metric on some random grasps"""
random.seed(42)
np.random.seed(42)
from grasp_metrics import achieves_force_closure
for i in range(100):
random_thetas = [np.random.rand() * np.pi for _ in range(2)]
random_points = [
np.array([np.sin(theta), np.cos(theta)])
for theta in random_thetas
]
normals = [-x / np.linalg.norm(x) for x in random_points]
if not np.allclose(normals[0], -normals[1]):
FC = achieves_force_closure(random_points, normals, 1e-7)
self.assertFalse(
FC,
"This means that you are computing force closure for two points not antipodal "
"(for two points and tiny friction, they need to be opposite to achieve force closure). "
)
for i in range(3):
random_thetas = [np.random.rand() * 2 * np.pi for _ in range(100)]
random_thetas[
-1] = random_thetas[-2] + np.pi # One is directly opposite
random_points = [
np.array([np.sin(theta), np.cos(theta)])
for theta in random_thetas
]
normals = [-x / np.linalg.norm(x) for x in random_points]
FC = achieves_force_closure(random_points, normals, 0.1)
self.assertTrue(
FC,
"These many grasp points should achieve force closure since "
"One of them was chosen to be directly antipoldal. ")
negative_normals = [-normal for normal in normals]
for i in range(3):
random_thetas = [
np.random.rand() * np.pi / 3.0 for _ in range(100)
]
random_points = [
np.array([np.sin(theta), np.cos(theta)])
for theta in random_thetas
]
normals = [-x / np.linalg.norm(x) for x in random_points]
FC = achieves_force_closure(random_points, normals, 0.001)
negative_normals = [-normal for normal in normals]
FC = achieves_force_closure(random_points, negative_normals, 0.1)
self.assertFalse(
FC, "We can't be pulling on the objects, only pushing. "
"Recommend checking your f_{i,z} > 0 constraint.")
def test_00_achieves_force_closure(self):
"""Test force closure metric on some predefined grasps"""
from grasp_metrics import achieves_force_closure
points = [np.asarray([-1.0, 0.]), np.asarray([1.0, 0.])]
normals = [np.asarray([1.0, 0.]), np.asarray([-1.0, 0.])]
mu = 0.2
FC = achieves_force_closure(points, normals, mu)
self.assertTrue(
FC, "This means that one of simple force closure checks failed ")
r1 = np.asarray([0.1, 1])
r2 = np.asarray([0.3, -0.4])
r3 = np.asarray([-0.7, -0.5])
points = [r1, r2, r3]
n1 = np.asarray([-0.1, -1.1])
n1 = n1 / np.linalg.norm(n1)
n2 = np.asarray([-0.4, 1.1])
n2 = n2 / np.linalg.norm(n2)
n3 = np.asarray([0.8, 1.1])
n3 = n3 / np.linalg.norm(n3)
normals = [n1, n2, n3]
mu = 1.5
FC = achieves_force_closure(points, normals, mu)
self.assertTrue(
FC, "This means that one of simple force closure checks failed ")
mu = 1e-7
FC = achieves_force_closure(points, normals, mu)
self.assertFalse(
FC, "Friction cone constraint is probably not properly enforced. ")
points = [np.asarray([-1.0, 0.]), np.asarray([1.0, 0.])]
normals = [np.asarray([-1.0, 0.]), np.asarray([-1.0, 0.])]
mu = 0.2
FC = achieves_force_closure(points, normals, mu)
self.assertTrue(
not FC,
"This means that one of simple force closure checks failed ")
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