def vis(self, mesh, grasp_vertices, grasp_qualities):
"""
Pass in any grasp and its associated grasp quality. this function will plot
each grasp on the object and plot the grasps as a bar between the points, with
colored dots on the line endpoints representing the grasp quality associated
with each grasp
Parameters
----------
mesh : :obj:`Trimesh`
grasp_vertices : mx2x3 :obj:`numpy.ndarray`
m grasps. Each grasp containts two contact points. Each contact point
is a 3 dimensional vector, hence the shape mx2x3
grasp_qualities : mx' :obj:`numpy.ndarray`
vector of grasp qualities for each grasp
"""
vis3d.mesh(mesh)
dirs = normalize(grasp_vertices[:, 0] - grasp_vertices[:, 1], axis=1)
midpoints = (grasp_vertices[:, 0] + grasp_vertices[:, 1]) / 2
grasp_endpoints = np.zeros(grasp_vertices.shape)
grasp_vertices[:, 0] = midpoints + dirs * MAX_HAND_DISTANCE / 2
grasp_vertices[:, 1] = midpoints - dirs * MAX_HAND_DISTANCE / 2
for grasp, quality in zip(grasp_vertices, grasp_qualities):
color = [min(1, 2 * (1 - quality)), min(1, 2 * quality), 0, 1]
if color != [1, 0, 0, 1]:
good_grasp += 1
# vis3d.plot3d(grasp, color=color, tube_radius=.0005)
vis3d.plot3d(grasp, color=color, tube_radius=.0005)
vis3d.show()
def vertices_to_baxter_hand_pose(self, grasp_vertices, approach_direction,
obj_name):
"""
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
"""
# parameters required to create a autolab_core:RigidTransform:
# - rotation (aka 3x3 rotation matrix)
# - translation (aka 3x1 vector)
# - from_frame (aka str)
# - to_frame (aka str)
midpoint = (grasp_vertices[0] + grasp_vertices[1]) / 2
gripper_half_width = MAX_HAND_DISTANCE / 2
z = normalize(approach_direction)
y = normalize(grasp_vertices[0] - grasp_vertices[1])
x = np.cross(y, z)
rot_mat_opposite = np.array([x, y, z]).T
p_opposite = midpoint
rot_mat = rot_mat_opposite.T
p = -np.matmul(rot_mat_opposite.T, p_opposite)
rigid_trans = RigidTransform(rot_mat_opposite,
p_opposite,
to_frame='right_gripper',
from_frame=obj_name)
return rigid_trans
def vis(self, mesh, grasp_vertices, grasp_qualities, grasp_normals):
"""
Pass in any grasp and its associated grasp quality. this function will plot
each grasp on the object and plot the grasps as a bar between the points, with
colored dots on the line endpoints representing the grasp quality associated
with each grasp
Parameters
----------
mesh : :obj:`Trimesh`
grasp_vertices : mx2x3 :obj:`numpy.ndarray`
m grasps. Each grasp containts two contact points. Each contact point
is a 3 dimensional vector, hence the shape mx2x3
grasp_qualities : mx' :obj:`numpy.ndarray`
vector of grasp qualities for each grasp
"""
vis3d.mesh(mesh)
middle_of_part = np.mean(np.mean(grasp_vertices, axis=1), axis=0)
print(middle_of_part)
vis3d.points(middle_of_part, scale=0.003)
dirs = normalize(grasp_vertices[:, 0] - grasp_vertices[:, 1], axis=1)
midpoints = (grasp_vertices[:, 0] + grasp_vertices[:, 1]) / 2
grasp_endpoints = np.zeros(grasp_vertices.shape)
grasp_endpoints[:, 0] = midpoints + dirs * MAX_HAND_DISTANCE / 2
grasp_endpoints[:, 1] = midpoints - dirs * MAX_HAND_DISTANCE / 2
n0 = np.zeros(grasp_endpoints.shape)
n1 = np.zeros(grasp_endpoints.shape)
normal_scale = 0.01
n0[:, 0] = grasp_vertices[:, 0]
n0[:, 1] = grasp_vertices[:, 0] + normal_scale * grasp_normals[:, 0]
n1[:, 0] = grasp_vertices[:, 1]
n1[:, 1] = grasp_vertices[:, 1] + normal_scale * grasp_normals[:, 1]
for grasp, quality, normal0, normal1 in zip(grasp_endpoints,
grasp_qualities, n0, n1):
color = [min(1, 2 * (1 - quality)), min(1, 2 * quality), 0, 1]
vis3d.plot3d(grasp, color=color, tube_radius=.001)
vis3d.plot3d(normal0, color=(0, 0, 0), tube_radius=.002)
vis3d.plot3d(normal1, color=(0, 0, 0), tube_radius=.002)
vis3d.show()
def compute_approach_direction(self, mesh, grasp_vertices, grasp_quality,
grasp_normals):
## initalizing stuff ##
visualize = True
nb_directions_to_test = 6
normal_scale = 0.01
plane_normal = normalize(grasp_vertices[0] - grasp_vertices[1])
midpoint = (grasp_vertices[0] + grasp_vertices[1]) / 2
## generating a certain number of approach directions ##
theta = np.pi / nb_directions_to_test
rot_mat = rotation_3d(-plane_normal, theta)
horizontal_direction = normalize(
np.cross(plane_normal, np.array([0, 0, 1])))
directions_to_test = [horizontal_direction] #these are vectors
approach_directions = [
np.array(
[midpoint, midpoint + horizontal_direction * normal_scale])
] #these are two points for visualization
for i in range(nb_directions_to_test - 1):
directions_to_test.append(
normalize(np.matmul(rot_mat, directions_to_test[-1])))
approach_directions.append(
np.array([
midpoint, midpoint + directions_to_test[-1] * normal_scale
]))
## computing the palm position for each approach direction ##
palm_positions = []
for i in range(nb_directions_to_test):
palm_positions.append(midpoint +
finger_length * directions_to_test[i])
if visualize:
## plotting the whole mesh ##
vis3d.mesh(mesh, style='wireframe')
## computing and plotting midpoint and gripper position ##
dirs = (grasp_vertices[0] - grasp_vertices[1]
) / np.linalg.norm(grasp_vertices[0] - grasp_vertices[1])
grasp_endpoints = np.zeros(grasp_vertices.shape)
grasp_endpoints[0] = midpoint + dirs * MAX_HAND_DISTANCE / 2
grasp_endpoints[1] = midpoint - dirs * MAX_HAND_DISTANCE / 2
color = [
min(1, 2 * (1 - grasp_quality)),
min(1, 2 * grasp_quality), 0, 1
]
vis3d.plot3d(grasp_endpoints, color=color, tube_radius=.001)
vis3d.points(midpoint, scale=0.003)
## computing and plotting normals at contact points ##
n0 = np.zeros(grasp_endpoints.shape)
n1 = np.zeros(grasp_endpoints.shape)
n0[0] = grasp_vertices[0]
n0[1] = grasp_vertices[0] + normal_scale * grasp_normals[0]
n1[0] = grasp_vertices[1]
n1[1] = grasp_vertices[1] + normal_scale * grasp_normals[1]
vis3d.plot3d(n0, color=(0, 0, 0), tube_radius=.002)
vis3d.plot3d(n1, color=(0, 0, 0), tube_radius=.002)
## plotting normals the palm positions for each potential approach direction ##
for i in range(nb_directions_to_test):
vis3d.points(palm_positions[i], scale=0.003)
vis3d.show()
directions_to_test = [
directions_to_test[3], directions_to_test[2],
directions_to_test[4], directions_to_test[1],
directions_to_test[5], directions_to_test[0]
]
palm_positions = [
palm_positions[3], palm_positions[2], palm_positions[4],
palm_positions[1], palm_positions[5], palm_positions[0]
]
## checking if some approach direction is valid ##
for i in range(nb_directions_to_test):
if len(
trimesh.intersections.mesh_plane(mesh,
directions_to_test[i],
palm_positions[i])) == 0:
# it means the palm won't bump with part
return directions_to_test[i]
# it means all approach directions will bump with part
return -1
def vis(self, mesh, grasp_vertices, grasp_normals, grasp_qualities,
hand_poses):
"""
Pass in any grasp and its associated grasp quality. this function will plot
each grasp on the object and plot the grasps as a bar between the points, with
colored dots on the line endpoints representing the grasp quality associated
with each grasp
Parameters
----------
mesh : :obj:`Trimesh`
grasp_vertices : mx2x3 :obj:`numpy.ndarray`
m grasps. Each grasp containts two contact points. Each contact point
is a 3 dimensional vector, hence the shape mx2x3
grasp_qualities : mx' :obj:`numpy.ndarray`
vector of grasp qualities for each grasp
hand_poses: list of RigidTransform objects
"""
vis3d.mesh(mesh)
dirs = normalize(grasp_vertices[:, 0] - grasp_vertices[:, 1], axis=1)
midpoints = (grasp_vertices[:, 0] + grasp_vertices[:, 1]) / 2
grasp_contacts = np.zeros(grasp_vertices.shape)
grasp_contacts[:, 0] = midpoints + dirs * MAX_HAND_DISTANCE / 2
grasp_contacts[:, 1] = midpoints - dirs * MAX_HAND_DISTANCE / 2
for grasp, quality, normal, contacts, hand_pose in zip(
grasp_vertices, grasp_qualities, grasp_normals, grasp_contacts,
hand_poses):
color = [min(1, 2 * (1 - quality)), min(1, 2 * quality), 0, 1]
vis3d.points(grasp, scale=0.001, color=(1, 0, 0))
n1_endpoints = np.zeros((2, 3))
n1_endpoints[0] = grasp[0]
n1_endpoints[1] = grasp[0] + 0.01 * normal[0]
n2_endpoints = np.zeros((2, 3))
n2_endpoints[0] = grasp[1]
n2_endpoints[1] = grasp[1] + 0.01 * normal[1]
vis3d.plot3d(n1_endpoints, color=color, tube_radius=0.001)
vis3d.plot3d(n2_endpoints, color=color, tube_radius=0.001)
# hand pose
vis3d.points(hand_pose.position,
scale=0.001,
color=(0.5, 0.5, 0.5))
x_axis_endpoints = np.zeros((2, 3))
x_axis_endpoints[0] = hand_pose.position
x_axis_endpoints[1] = hand_pose.position + 0.01 * hand_pose.x_axis
vis3d.plot3d(x_axis_endpoints, color=(1, 0, 0), tube_radius=0.0001)
y_axis_endpoints = np.zeros((2, 3))
y_axis_endpoints[0] = hand_pose.position
y_axis_endpoints[1] = hand_pose.position + 0.01 * hand_pose.y_axis
vis3d.plot3d(y_axis_endpoints, color=(0, 1, 0), tube_radius=0.0001)
z_axis_endpoints = np.zeros((2, 3))
z_axis_endpoints[0] = hand_pose.position
z_axis_endpoints[1] = hand_pose.position + 0.01 * hand_pose.z_axis
vis3d.plot3d(z_axis_endpoints, color=(0, 0, 1), tube_radius=0.0001)
vis3d.plot3d(contacts, color=(0, 0, 1), tube_radius=.0005)
vis3d.show()