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 visualize(self):
"""Visualize the salient edges of the mesh.
"""
vis3d.figure()
for edge in self.salient_edges:
vis3d.plot3d(self.mesh.vertices[edge],
color=(0.0, 1.0, 0.0),
tube_radius=0.0005)
vis3d.mesh(self.mesh)
vis3d.show()
def visualize_normals(mesh, vertices, normals):
scale = 0.01
normals_scaled = normals * scale
vis3d.pose(RigidTransform(),
alpha=0.01,
tube_radius=0.001,
center_scale=0.002)
vis3d.mesh(mesh, style='wireframe')
vis3d.points(mesh.centroid, color=(0, 0, 0), scale=0.003)
vis3d.points(vertices, color=(1, 0, 0), scale=0.001)
for v, n in zip(vertices, normals_scaled):
vis3d.plot3d([v, v + n], color=(1, 0, 0), tube_radius=0.0005)
vis3d.show()
def grasp(grasp,
T_obj_world=RigidTransform(from_frame='obj', to_frame='world'),
tube_radius=0.0002,
endpoint_color=(0, 1, 0),
endpoint_scale=0.0005,
grasp_axis_color=(0, 1, 0)):
""" Plots a grasp as an axis and center.
Parameters
----------
grasp : :obj:`dexnet.grasping.Grasp`
the grasp to plot
T_obj_world : :obj:`autolab_core.RigidTransform`
the pose of the object that the grasp is referencing in world frame
tube_radius : float
radius of the plotted grasp axis
endpoint_color : 3-tuple
color of the endpoints of the grasp axis
endpoint_scale : 3-tuple
scale of the plotted endpoints
grasp_axis_color : 3-tuple
color of the grasp axis
"""
g1, g2 = grasp.endpoints
center = grasp.center
g1 = Point(g1, 'obj')
g2 = Point(g2, 'obj')
center = Point(center, 'obj')
g1_tf = T_obj_world.apply(g1)
g2_tf = T_obj_world.apply(g2)
center_tf = T_obj_world.apply(center)
grasp_axis_tf = np.array([g1_tf.data, g2_tf.data])
Visualizer3D.points(g1_tf.data,
color=endpoint_color,
scale=endpoint_scale)
Visualizer3D.points(g2_tf.data,
color=endpoint_color,
scale=endpoint_scale)
Visualizer3D.plot3d(grasp_axis_tf,
color=grasp_axis_color,
tube_radius=tube_radius)
Visualizer3D.pose(grasp.T_grasp_obj,
alpha=endpoint_scale * 10,
tube_radius=tube_radius,
center_scale=endpoint_scale)
def visualize_grasps(mesh, vertices, metrics):
vis3d.pose(RigidTransform(),
alpha=0.01,
tube_radius=0.001,
center_scale=0.002)
vis3d.mesh(mesh, style='wireframe')
vis3d.points(mesh.centroid, color=(0, 0, 0), scale=0.003)
min_score = float(np.min(metrics))
max_score = float(np.max(metrics))
metrics_normalized = (metrics.astype(float) - min_score) / (max_score -
min_score)
for v, m in zip(vertices, metrics_normalized):
vis3d.points(v, color=(1 - m, m, 0), scale=0.001)
vis3d.plot3d(v, color=(1 - m, m, 0), tube_radius=0.0003)
vis3d.show()
def gripper(gripper, grasp, T_obj_world, color=(0.5, 0.5, 0.5)):
""" Plots a robotic gripper in a pose specified by a particular grasp object.
Parameters
----------
gripper : :obj:`dexnet.grasping.RobotGripper`
the gripper to plot
grasp : :obj:`dexnet.grasping.Grasp`
the grasp to plot the gripper performing
T_obj_world : :obj:`autolab_core.RigidTransform`
the pose of the object that the grasp is referencing in world frame
color : 3-tuple
color of the gripper mesh
"""
T_gripper_obj = grasp.gripper_pose(gripper)
T_gripper_world = T_obj_world * T_gripper_obj
T_mesh_world = T_gripper_world * gripper.T_mesh_gripper.inverse()
T_mesh_world = T_mesh_world.as_frames('obj', 'world')
Visualizer3D.mesh(gripper.mesh.trimesh,
T_mesh_world,
style='surface',
color=color)
g1, g2 = grasp.endpoints
center = grasp.center
g1 = Point(np.array([0.1, 0.0, 0.0]), 'obj')
g2 = Point(np.array([0.0, 0.0, 0.0]), 'obj')
center = Point(np.array([0.0, 0.0, 0.0]), 'obj')
g1_tf = T_mesh_world.apply(g1)
g2_tf = T_mesh_world.apply(g2)
center_tf = T_mesh_world.apply(center)
grasp_axis_tf = np.array([g1_tf.data, g2_tf.data])
points = [(x[0], x[1], x[2]) for x in grasp_axis_tf]
Visualizer3D.plot3d(points, color=(0, 1, 0), tube_radius=0.004)
g1 = Point(np.array([0.0, 0.0, 0.0]), 'obj')
g2 = Point(np.array([0.0, 0.0, 0.1]), 'obj')
center = Point(np.array([0.0, 0.0, 0.0]), 'obj')
g1_tf = T_mesh_world.apply(g1)
g2_tf = T_mesh_world.apply(g2)
center_tf = T_mesh_world.apply(center)
grasp_axis_tf = np.array([g1_tf.data, g2_tf.data])
points = [(x[0], x[1], x[2]) for x in grasp_axis_tf]
Visualizer3D.plot3d(points, color=(1, 0, 0), tube_radius=0.004)
g1 = Point(np.array([0.0, 0.0, 0.0]), 'obj')
g2 = Point(np.array([0.0, 0.1, 0.0]), 'obj')
center = Point(np.array([0.0, 0.0, 0.0]), 'obj')
g1_tf = T_mesh_world.apply(g1)
g2_tf = T_mesh_world.apply(g2)
center_tf = T_mesh_world.apply(center)
grasp_axis_tf = np.array([g1_tf.data, g2_tf.data])
points = [(x[0], x[1], x[2]) for x in grasp_axis_tf]
Visualizer3D.plot3d(points, color=(0, 0, 1), tube_radius=0.004)
def shownormals(normal,
surface_point,
color=(1, 0, 0),
tube_radius=0.001,
T_obj_world=RigidTransform(from_frame='obj',
to_frame='world')):
""" Plots a grasp as an axis and center.
Parameters
----------
grasp : :obj:`dexnet.grasping.Grasp`
the grasp to plot
T_obj_world : :obj:`autolab_core.RigidTransform`
the pose of the object that the grasp is referencing in world frame
tube_radius : float
radius of the plotted grasp axis
endpoint_color : 3-tuple
color of the endpoints of the grasp axis
endpoint_scale : 3-tuple
scale of the plotted endpoints
grasp_axis_color : 3-tuple
color of the grasp axis
"""
normalpoint = surface_point + 0.02 * normal
normalpoint = Point(normalpoint, 'obj')
surface_point = Point(surface_point, 'obj')
normal_tf = T_obj_world.apply(normalpoint)
surface_point_tf = T_obj_world.apply(surface_point)
normal_axis_tf = np.array([normal_tf.data, surface_point_tf.data])
# print [(x[0], x[1], x[2]) for x in normal_axis_tf]
points = [(x[0], x[1], x[2]) for x in normal_axis_tf]
Visualizer3D.plot3d(points, color=color, tube_radius=tube_radius)
def show_points(points, color=(0,1,0), scale=0.005, frame_size=0.2, frame_radius=0.02):
vis.figure(bgcolor=(1,1,1), size=(500,500))
vis.points(np.array(points), color=color, scale=scale)
vis.plot3d(np.array(([0, 0, 0], [frame_size, 0, 0])).astype(np.float32), color=(1,0,0), tube_radius=frame_radius)
vis.plot3d(np.array(([0, 0, 0], [0, frame_size, 0])).astype(np.float32), color=(0,1,0), tube_radius=frame_radius)
vis.plot3d(np.array(([0, 0, 0], [0, 0, frame_size])).astype(np.float32), color=(0,0,1), tube_radius=frame_radius)
vis.show()
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 vis(self, mesh, grasp_vertices, grasp_qualities, top_n_grasp_vertices,
approach_directions, rs, TG):
"""
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 i, (grasp,
quality) in enumerate(zip(grasp_vertices, grasp_qualities)):
color = [min(1, 2 * (1 - quality)), min(1, 2 * quality), 0, 1]
vis3d.plot3d(grasp, color=color, tube_radius=.001)
blue = [0, 0, 255]
light_blue = [50, 50, 200]
for i, (grasp, approach_direction) in enumerate(
zip(top_n_grasp_vertices, approach_directions)):
midpoint = np.mean(grasp, axis=0)
approach_direction = np.asarray(
[midpoint, midpoint - 0.1 * approach_direction])
if (i == 0):
vis3d.plot3d(approach_direction, color=blue, tube_radius=.005)
vis3d.points(grasp, color=blue, scale=.005)
else:
vis3d.plot3d(approach_direction,
color=light_blue,
tube_radius=.001)
vis3d.points(grasp, color=light_blue, scale=.001)
midpoint = np.mean(top_n_grasp_vertices[0], axis=0)
x_purp = [204, 0, 204]
x_axis = np.asarray([midpoint, midpoint + 0.1 * rs[:, 0]])
y_cyan = [0, 204, 204]
y_axis = np.asarray([midpoint, midpoint + 0.1 * rs[:, 1]])
z_black = [0, 0, 0]
z_axis = np.asarray([midpoint, midpoint + 0.3 * rs[:, 2]])
vis3d.plot3d(x_axis, color=x_purp, tube_radius=.005)
vis3d.plot3d(y_axis, color=y_cyan, tube_radius=.005)
vis3d.plot3d(z_axis, color=z_black, tube_radius=.005)
origin = np.asarray([0, 0, 0])
x_axis = np.asarray([origin, 0.2 * np.array([1, 0, 0])])
y_axis = np.asarray([origin, 0.2 * np.array([0, 1, 0])])
z_axis = np.asarray([origin, 0.2 * np.array([0, 0, 1])])
vis3d.plot3d(x_axis, color=x_purp, tube_radius=.005)
vis3d.plot3d(y_axis, color=y_cyan, tube_radius=.005)
vis3d.plot3d(z_axis, color=z_black, tube_radius=.005)
# eucl_orien = np.asarray(tfs.euler_from_quaternion(TG.quaternion))
intermediate_pos = TG.position - np.reshape(
np.matmul(TG.rotation, np.array([[0], [0], [0.2]])), (1, 3))
print(intermediate_pos, np.shape(intermediate_pos))
red = [255, 0, 0]
vis3d.points(intermediate_pos, color=red, scale=.01)
vis3d.points(TG.position, color=red, scale=.01)
vis3d.show()
if __name__ == '__main__':
depth_im_filename = sys.argv[1]
camera_intr_filename = sys.argv[2]
camera_intr = CameraIntrinsics.load(camera_intr_filename)
depth_im = DepthImage.open(depth_im_filename, frame=camera_intr.frame)
depth_im = depth_im.inpaint()
point_cloud_im = camera_intr.deproject_to_image(depth_im)
normal_cloud_im = point_cloud_im.normal_cloud_im(ksize=KSIZE)
vis3d.figure()
vis3d.points(point_cloud_im.to_point_cloud(), scale=0.0025)
alpha = 0.025
subsample = 20
for i in range(0, point_cloud_im.height, subsample):
for j in range(0, point_cloud_im.width, subsample):
p = point_cloud_im[i, j]
n = normal_cloud_im[i, j]
n2 = normal_cloud_im_s[i, j]
if np.linalg.norm(n) > 0:
points = np.array([p, p + alpha * n])
vis3d.plot3d(points, tube_radius=0.001, color=(1, 0, 0))
points = np.array([p, p + alpha * n2])
vis3d.plot3d(points, tube_radius=0.001, color=(1, 0, 1))
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_transform(self, mesh, G_transform, vertices):
"""
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
"""
L = MAX_HAND_DISTANCE / 2 # gripper half width
# transform from gripper to contact 1
G_gc1 = np.array([[1, 0, 0, 0], [0, 0, 1, -1 * L], [0, -1, 0, 0],
[0, 0, 0, 1]])
# transform from gripper to contact 2
G_gc2 = np.array([[1, 0, 0, 0], [0, 0, -1, L], [0, 1, 0, 0],
[0, 0, 0, 1]])
G = G_transform.matrix
print('G')
print(G)
G_oc1 = np.matmul(G, G_gc1)
G_oc2 = np.matmul(G, G_gc2)
scale = 0.01
o = np.array([0, 0, 0, 1])
x = np.array([scale, 0, 0, 1])
y = np.array([0, scale, 0, 1])
z = np.array([0, 0, scale, 1])
ot = np.matmul(G, o)
xt = np.matmul(G, x)
yt = np.matmul(G, y)
zt = np.matmul(G, z)
o1 = np.matmul(G_oc1, o)
x1 = np.matmul(G_oc1, x)
y1 = np.matmul(G_oc1, y)
z1 = np.matmul(G_oc1, z)
o2 = np.matmul(G_oc2, o)
x2 = np.matmul(G_oc2, x)
y2 = np.matmul(G_oc2, y)
z2 = np.matmul(G_oc2, z)
vis3d.mesh(mesh, style='wireframe')
#Plot origin axes
x_axis = np.array([o, x])[:, :3]
y_axis = np.array([o, y])[:, :3]
z_axis = np.array([o, z])[:, :3]
x_axis_t = np.array([ot, xt])[:, :3]
y_axis_t = np.array([ot, yt])[:, :3]
z_axis_t = np.array([ot, zt])[:, :3]
x_axis_1 = np.array([o1, x1])[:, :3]
y_axis_1 = np.array([o1, y1])[:, :3]
z_axis_1 = np.array([o1, z1])[:, :3]
x_axis_2 = np.array([o2, x2])[:, :3]
y_axis_2 = np.array([o2, y2])[:, :3]
z_axis_2 = np.array([o2, z2])[:, :3]
vis3d.plot3d(x_axis, color=(0.5, 0, 0), tube_radius=0.001)
vis3d.plot3d(y_axis, color=(0, 0.5, 0), tube_radius=0.001)
vis3d.plot3d(z_axis, color=(0, 0, 0.5), tube_radius=0.001)
vis3d.plot3d(x_axis_t, color=(255, 0, 0), tube_radius=0.001)
vis3d.plot3d(y_axis_t, color=(0, 255, 0), tube_radius=0.001)
vis3d.plot3d(z_axis_t, color=(0, 0, 255), tube_radius=0.001)
vis3d.plot3d(x_axis_1, color=(255, 0, 0), tube_radius=0.001)
vis3d.plot3d(y_axis_1, color=(0, 255, 0), tube_radius=0.001)
vis3d.plot3d(z_axis_1, color=(0, 0, 255), tube_radius=0.001)
vis3d.plot3d(x_axis_2, color=(255, 0, 0), tube_radius=0.001)
vis3d.plot3d(y_axis_2, color=(0, 255, 0), tube_radius=0.001)
vis3d.plot3d(z_axis_2, color=(0, 0, 255), tube_radius=0.001)
vis3d.points(vertices[0], scale=0.003)
vis3d.points(vertices[1], scale=0.003)
vis3d.show()
def graspwithapproachvectorusingapproach_point(
grasp,
T_obj_world=RigidTransform(from_frame='obj', to_frame='world'),
tube_radius=0.001,
endpoint_color=(0, 1, 0),
endpoint_scale=0.004,
grasp_axis_color=(0, 1, 0),
sdfcenter=np.array([50., 50., 50.]),
sdforigin=np.array([0, 0, 0])):
""" Plots a grasp as an axis and center.
Parameters
----------
grasp : :obj:`dexnet.grasping.Grasp`
the grasp to plot
T_obj_world : :obj:`autolab_core.RigidTransform`
the pose of the object that the grasp is referencing in world frame
tube_radius : float
radius of the plotted grasp axis
endpoint_color : 3-tuple
color of the endpoints of the grasp axis
endpoint_scale : 3-tuple
scale of the plotted endpoints
grasp_axis_color : 3-tuple
color of the grasp axis
"""
g1, g2 = grasp.endpoints
center = grasp.center #这个center未必是真正的center，所以后面不能用它
approach_point = np.array(grasp.approach_point)
approach_axis = grasp.rotated_full_axis[:, 0]
approach_pointfar = approach_point + approach_axis * 0.1
# print 'approach_angle:',grasp.approach_angle
# print 'approach_point:',grasp.approach_point
g1 = Point(g1, 'obj')
g2 = Point(g2, 'obj')
center = Point(center, 'obj')
approach_pointfar2 = Point(approach_pointfar, 'obj')
approach_point2 = Point(approach_point, 'obj')
g1_tf = T_obj_world.apply(g1)
g2_tf = T_obj_world.apply(g2)
#center_tf = T_obj_world.apply(center)
approach_pointfar_tf = T_obj_world.apply(approach_pointfar2)
approach_point2_tf = T_obj_world.apply(approach_point2)
Visualizer3D.points(approach_point2_tf.data,
color=(1, 0, 0),
scale=0.001)
grasp_axis_tf = np.array([g1_tf.data, g2_tf.data])
approach_axis_tf = np.array(
[approach_point2_tf.data, approach_pointfar_tf.data])
#approach_point2_tf_again = np.array([approach_point2_tf.data, approach_point2_tf.data])
# print [(x[0], x[1], x[2]) for x in grasp_axis_tf]
#print [(x[0], x[1], x[2]) for x in grasp_axis_tf]
#points = [(x[0]+sdfcenter[0]+sdforigin[0] , x[1]+sdfcenter[1]+sdforigin[1] , x[2] +sdfcenter[2]+sdforigin[2]) for x in grasp_axis_tf]
points = [(x[0], x[1], x[2]) for x in grasp_axis_tf]
approaching = [(y[0], y[1], y[2]) for y in approach_axis_tf]
# approachingpoint=[(z[0] , z[1] , z[2] ) for z in approach_point2_tf_again]
#points = [(x[0] , x[1] , x[2] ) for x in grasp_axis_tf]
Visualizer3D.plot3d(points,
color=grasp_axis_color,
tube_radius=tube_radius)
Visualizer3D.plot3d(approaching,
color=(0, 0.5, 0.5),
tube_radius=tube_radius)
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()
