def get_grasp_map(vertices, normals, num_facets, mu, gamma):
"""
defined in the book on page 219. Compute the grasp map given the contact
points and their surface normals
Parameters
----------
vertices : 2x3 :obj:`numpy.ndarray`
obj mesh vertices on which the fingers will be placed
normals : 2x3 :obj:`numpy.ndarray`
obj mesh normals at the contact points
num_facets : int
number of vectors to use to approximate the friction cone. these vectors
will be along the friction cone boundary
mu : float
coefficient of friction
gamma : float
torsional friction coefficient
Returns
-------
:obj:`numpy.ndarray` grasp map
"""
# YOUR CODE HERE
# Ryan code, uses utils for most of the work
finger = np.matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 0],
[0, 0, 0, 0], [0, 0, 0, 1]])
rot1 = look_at_general(vertices[0], normals[0])
adj1 = adj(rot1)
rot2 = look_at_general(vertices[1], normals[1])
adj2 = adj(rot2)
G1 = np.dot(adj1, finger)
G2 = np.dot(adj2, finger)
G = np.hstack((G1, G2))
return G
def get_grasp_map(vertices, normals, num_facets, mu, gamma):
"""
defined in the book on page 219. Compute the grasp map given the contact
points and their surface normals
Parameters
----------
vertices : 2x3 :obj:`numpy.ndarray`
obj mesh vertices on which the fingers will be placed
normals : 2x3 :obj:`numpy.ndarray`
obj mesh normals at the contact points
num_facets : int
number of vectors to use to approximate the friction cone. these vectors
will be along the friction cone boundary
mu : float
coefficient of friction
gamma : float
torsional friction coefficient
Returns
-------
:obj:`numpy.ndarray` grasp map
"""
# YOUR CODE HERE
v1, v2 = vertices
n1, n2 = normals
g1 = look_at_general(v1, n1)
g2 = look_at_general(v2, n2)
adj_g1 = adj(g1)
adj_g2 = adj(g2)
G1 = adj_g1.dot(wrench_basis) # 6x4
G2 = adj_g2.dot(wrench_basis) # 6x4
return np.hstack([G1, G2]) # 6x8
def compute_force_closure(vertices, normals, num_facets, mu, gamma,
object_mass, mesh, unused):
"""
Compute the force closure of some object at contacts, with normal vectors
stored in normals You can use the line method described in HW2. if you do you
will not need num_facets
Parameters
----------
vertices : 2x3 :obj:`numpy.ndarray`
obj mesh vertices on which the fingers will be placed
normals : 2x3 :obj:`numpy.ndarray`
obj mesh normals at the contact points
num_facets : int
number of vectors to use to approximate the friction cone. these vectors
will be along the friction cone boundary
mu : float
coefficient of friction
gamma : float
torsional friction coefficient
object_mass : float
mass of the object
Returns
-------
float : quality of the grasp
"""
# YOUR CODE HERE
"""
First get the line between two grasp points
calculate the friction cones
check if the line passes through both
"""
# Find tangents and store them in a rotation matrix.
R1 = look_at_general(vertices[0], normals[0])
R2 = look_at_general(vertices[1], normals[1])
theta = 0.2 # We should be able to choose this independently, but it changes the results..
f1 = normals[0] + np.cos(theta) * R1[0:3, 0] + np.sin(theta) * R1[0:3, 1]
f2 = normals[1] + np.cos(theta) * R2[0:3, 0] + np.sin(theta) * R2[0:3, 1]
line = vertices[1, :] - vertices[0, :]
# we compare the angles between the line and the normal with the angle between fc vector and the normal
line_in_FC1 = abs(np.matmul(line, normals[0]) / (norm(line) * norm(normals[0]))) > \
abs(np.matmul(line, f1) / (norm(line) * norm(f1)))
line_in_FC2 = abs(np.matmul(-line, normals[1]) / (norm(-line) * norm(normals[1]))) > \
abs(np.matmul(-line, f2) / (norm(-line) * norm(f2)))
return line_in_FC1 and line_in_FC2
def is_in_cone(p1, p2, norm1, mu):
"""
Author: Ryan
Determines if p2 is in the friction cone of p1
Parameters
----------
p1 : coordinate of point 1 in world frame
p2 : coordinate of point 2 in world frame
norm1 : normal direction of force 1 in world frame
mu : coefficient of friction
Returns
-------
bool : whether p2 is in the friction cone of p1
"""
Gaw = look_at_general(p1, norm1)
Gwa = np.linalg.inv(Gaw)
pt = np.array([p2[0], p2[1], p2[2], 1])
pa = np.dot(Gwa, pt)
if math.sqrt(pa[0]**2 + pa[1]**2) <= mu * pa[2]:
return True
else:
return False
def get_grasp_map(vertices, normals, num_facets, mu, gamma):
"""
defined in the book on page 219. Compute the grasp map given the contact
points and their surface normals
Parameters
----------
vertices : 2x3 :obj:`numpy.ndarray`
obj mesh vertices on which the fingers will be placed
normals : 2x3 :obj:`numpy.ndarray`
obj mesh normals at the contact points
num_facets : int
number of vectors to use to approximate the friction cone. these vectors
will be along the friction cone boundary
mu : float
coefficient of friction
gamma : float
torsional friction coefficient
Returns
-------
:obj:`numpy.ndarray` grasp map
"""
# YOUR CODE HERE
if mu == 0:
print("Use point contact without friction")
if gamma == 0:
print("Do not use soft-finger bases")
B = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 0],
[0, 0, 0, 0], [0, 0, 0, 1]])
g1 = look_at_general(vertices[0], normals[0])
A1 = np.linalg.inv(adj(g1))
g2 = look_at_general(vertices[1], normals[1])
A2 = np.linalg.inv(adj(g2))
# as everything is in world coordinates we don't need to transform coordinate systems with the adjoint.
G = np.hstack((np.matmul(A1, B), np.matmul(A2, B)))
return G, np.matmul(A1, B), np.matmul(A2, B)
def vertices_to_baxter_hand_pose(self,
grasp_vertices,
approach_direction,
Rot,
obj_name='gearbox'):
"""
takes the contacts positions in the object frame and returns the hand pose T_obj_gripper
BE CAREFUL ABOUT THE FROM FRAME AND TO FRAME. the RigidTransform class' frames are
weird.
Parameters
----------
grasp_vertices : 2x3 :obj:`numpy.ndarray`
position of the fingers in object frame
approach_direction : 3x' :obj:`numpy.ndarray`
there are multiple grasps that go through contact1 and contact2. This describes which
orientation the hand should be in
Returns
-------
:obj:`autolab_core:RigidTransform` Hand pose in the object frame Note: in object frame
"""
# YOUR CODE HERE
v1, v2 = grasp_vertices
center = (v1 + v2) / 2
#print("v1,v2,center", v1, v2, center)
#print("App_dir:", approach_direction)
rot = look_at_general(center, approach_direction)
wrist_position = center #- approach_direction*GRIPPER_LENGTH # note that this only works b/c I made sure approach_direction is normalized
# print("wrist_position ", wrist_position)
print("approach direction: ", approach_direction)
print("vertices", v1, v2)
# time.sleep(10)
# wrist_position assumes is the position of the wrist (duh) and not the grippers
#print("rotation", rot)
# rot = rot[:3, :3]
modified_identity = np.array([[1, 0, 0], [0, -1, 0], [0, 0, -1]])
rot = np.dot(modified_identity, np.asarray(Rot[:3, :3]))
rpy = tf.transformations.euler_from_matrix(rot)
print("!!!!!!!!!!RPY!!!!!!!!!!")
print("rot in rpy", rpy)
g = RigidTransform(rot,
wrist_position,
from_frame='grasp',
to_frame=obj_name)
print(" ## g ##### ", g)
# I quite not sure here
return RigidTransform(rot,
wrist_position,
from_frame='grasp',
to_frame=obj_name)
def get_friction_cone_basis(vertex, normal, num_facets, mu):
axes_rot_3d = look_at_general(vertex, normal)
axes_rt = RigidTransform(rotation=axes_rot_3d, translation=vertex)
y_axis = axes_rt.y_axis
angle = np.arctan(mu)
rot = quaternion_matrix(quaternion_about_axis(angle, y_axis))
cone_vector = rot.dot(normal)
cone_basis_vecs = []
for angle in np.linspace(0, 2. * np.pi - 1e-8, num_facets):
cone_basis_vec = quaternion_matrix(quaternion_about_axis(
angle, normal)).dot(cone_vector)
cone_basis_vecs.append(cone_basis_vec)
return np.array(cone_basis_vecs).reshape((3, num_facets))