def gripper(gripper, grasp, T_obj_world, color=(0.5, 0.5, 0.5), T_camera_world=None, point=None, point_color=None):
""" 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=T_mesh_world, style='surface', color=color)
Visualizer3D.pose(T_gripper_world)
if T_camera_world is not None:
Visualizer3D.pose(T_camera_world)
if point is not None:
world_points = np.dot(T_obj_world.matrix, point.T).T
Visualizer3D.points(world_points[:, :3], scale=0.002, color=point_color)
def visualize_scene(mesh, T_world_ar, T_ar_cam, T_cam_obj):
T_cam_cam = RigidTransform()
T_obj_cam = T_cam_obj.inverse()
T_cam_ar = T_ar_cam.inverse()
Twc = np.matmul(T_world_ar.matrix, T_ar_cam.matrix)
T_world_cam = RigidTransform(Twc[:3, :3], Twc[:3, 3])
T_cam_world = T_world_cam.inverse()
o = np.array([0., 0., 0.])
centroid_cam = apply_transform(o, T_cam_obj)
tag_cam = apply_transform(o, T_cam_ar)
robot_cam = apply_transform(o, T_cam_world)
# camera
vis3d.points(o, color=(0, 1, 0), scale=0.01)
vis3d.pose(T_cam_cam, alpha=0.05, tube_radius=0.005, center_scale=0.002)
# object
vis3d.mesh(mesh)
vis3d.points(centroid_cam, color=(0, 0, 0), scale=0.01)
vis3d.pose(T_cam_obj, alpha=0.05, tube_radius=0.005, center_scale=0.002)
# AR tag
vis3d.points(tag_cam, color=(1, 0, 1), scale=0.01)
vis3d.pose(T_cam_ar, alpha=0.05, tube_radius=0.005, center_scale=0.002)
vis3d.table(T_cam_ar, dim=0.074)
# robot
vis3d.points(robot_cam, color=(1, 0, 0), scale=0.01)
vis3d.pose(T_cam_world, alpha=0.05, tube_radius=0.005, center_scale=0.002)
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_gripper(mesh, T_obj_grasp, vertices):
o = np.array([0., 0., 0.])
grasp = apply_transform(o, T_obj_grasp)
# object
vis3d.mesh(mesh)
vis3d.points(o, color=(0, 0, 0), scale=0.002)
vis3d.pose(RigidTransform(),
alpha=0.01,
tube_radius=0.001,
center_scale=0.001)
# gripper
vis3d.points(grasp, color=(1, 0, 0), scale=0.002)
vis3d.points(vertices, color=(1, 0, 0), scale=0.002)
vis3d.pose(T_obj_grasp, alpha=0.01, tube_radius=0.001, center_scale=0.001)
vis3d.show()
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 visualize_test():
I = RigidTransform()
g_ab = RigidTransform()
g_ab.translation = np.array([0.05, 0, 0])
g_ab.rotation = np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]])
q_a = np.array([0., 0., 0.])
p_b = np.array([0., 0., 0.])
p_a = apply_transform(p_b, g_ab)
print('g_ab = \n{}'.format(g_ab.matrix))
vis3d.pose(I, alpha=0.01, tube_radius=0.001, center_scale=0.001)
vis3d.points(q_a, color=(1, 0, 0), scale=0.005)
vis3d.pose(g_ab, alpha=0.01, tube_radius=0.001, center_scale=0.001)
vis3d.points(p_a, color=(0, 1, 0), scale=0.005)
vis3d.show()
def visualize_plan(mesh, T_world_obj, T_world_grasp):
# visualize the plan in the world frame
mesh.apply_transform(T_world_obj.matrix)
o = np.array([0., 0., 0.])
obj = apply_transform(o, T_world_obj)
grasp = apply_transform(o, T_world_grasp)
# base frame
vis3d.points(o, color=(1, 0, 0), scale=0.005)
vis3d.pose(RigidTransform(),
alpha=0.03,
tube_radius=0.002,
center_scale=0.001)
# object
vis3d.mesh(mesh)
vis3d.points(obj, color=(0, 0, 0), scale=0.005)
vis3d.pose(T_world_obj, alpha=0.03, tube_radius=0.002, center_scale=0.001)
# grasp
vis3d.points(grasp, color=(0, 1, 1), scale=0.005)
vis3d.pose(T_world_grasp,
alpha=0.03,
tube_radius=0.002,
center_scale=0.001)
vis3d.show()
T_tool_world, cartesian_impedances=[2000, 2000, 1000, 300, 300, 300])
logging.info('Finding closest orthogonal grasp')
T_grasp_world = get_closest_grasp_pose(T_tag_world, T_ready_world)
T_lift = RigidTransform(translation=[0, 0, 0.2],
from_frame=T_ready_world.to_frame,
to_frame=T_ready_world.to_frame)
T_lift_world = T_lift * T_grasp_world
logging.info('Visualizing poses')
_, depth_im, _ = sensor.frames()
points_world = T_camera_world * intr.deproject(depth_im)
if cfg['vis_detect']:
vis3d.figure()
vis3d.pose(RigidTransform())
vis3d.points(subsample(points_world.data.T, 0.01),
color=(0, 1, 0),
scale=0.002)
vis3d.pose(T_ready_world, length=0.05)
vis3d.pose(T_camera_world, length=0.1)
vis3d.pose(T_tag_world)
vis3d.pose(T_grasp_world)
vis3d.pose(T_lift_world)
vis3d.show()
#const_rotation=np.array([[1,0,0],[0,-1,0],[0,0,-1]])
#test = RigidTransform(rotation=const_rotation,translation=T_tag_world.translation, from_frame='franka_tool', to_frame='world')
#import pdb; pdb.set_trace()
rotation = T_tag_world.rotation
output_filename = os.path.join(
output_path, '{0}_to_world.tf'.format(T_world_obj.from_frame))
print T_world_obj
T_world_obj.save(output_filename)
if config['vis'] and VIS_SUPPORTED:
_, depth_im, _ = sensor.frames()
pc_cam = ir_intrinsics.deproject(depth_im)
pc_world = T_world_cam * pc_cam
mesh_file = ObjFile(
os.path.join(object_path, '{0}.obj'.format(args.object_name)))
mesh = mesh_file.read()
vis.figure(bgcolor=(0.7, 0.7, 0.7))
vis.mesh(mesh, T_world_obj.as_frames('obj', 'world'), style='surface')
vis.pose(T_world_obj, alpha=0.04, tube_radius=0.002, center_scale=0.01)
vis.pose(RigidTransform(from_frame='origin'),
alpha=0.04,
tube_radius=0.002,
center_scale=0.01)
vis.pose(T_world_cam, alpha=0.04, tube_radius=0.002, center_scale=0.01)
vis.pose(T_world_cam * T_cb_cam.inverse(),
alpha=0.04,
tube_radius=0.002,
center_scale=0.01)
vis.points(pc_world, subsample=20)
vis.show()
sensor.stop()
points_world = T_camera_world * ir_intrinsics.deproject(depth_im)
true_robot_points_world = PointCloud(np.array([T.translation for T in robot_poses]).T,
frame=ir_intrinsics.frame)
est_robot_points_world = T_camera_world * PointCloud(np.array(robot_points_camera).T,
frame=ir_intrinsics.frame)
mean_est_robot_point = np.mean(est_robot_points_world.data, axis=1).reshape(3,1)
est_robot_points_world._data = est_robot_points_world._data - mean_est_robot_point + mean_true_robot_point
fixed_robot_points_world = T_corrected_cb_world * est_robot_points_world
mean_fixed_robot_point = np.mean(fixed_robot_points_world.data, axis=1).reshape(3,1)
fixed_robot_points_world._data = fixed_robot_points_world._data - mean_fixed_robot_point + mean_true_robot_point
vis3d.figure()
vis3d.points(points_world, color=(0,1,0), subsample=10, random=True, scale=0.001)
vis3d.points(true_robot_points_world, color=(0,0,1), scale=0.001)
vis3d.points(fixed_robot_points_world, color=(1,1,0), scale=0.001)
vis3d.points(est_robot_points_world, color=(1,0,0), scale=0.001)
vis3d.pose(T_camera_world)
vis3d.show()
# save tranformation arrays based on setup
output_dir = os.path.join(config['calib_dir'], sensor_frame)
if not os.path.exists(output_dir):
os.makedirs(output_dir)
pose_filename = os.path.join(output_dir, '%s_to_world.tf' %(sensor_frame))
T_camera_world.save(pose_filename)
intr_filename = os.path.join(output_dir, '%s.intr' %(sensor_frame))
ir_intrinsics.save(intr_filename)
f = open(os.path.join(output_dir, 'corners_cb_%s.npy' %(sensor_frame)), 'w')
np.save(f, reg_result.cb_points_cam.data)
# move the robot to the chessboard center for verification
if config['use_robot']:
from_frame='obj',
to_frame='table')
stable_pose = mesh.resting_pose(T_obj_table)
#print stable_pose.r
table_dim = 0.3
T_obj_table_plot = mesh.get_T_surface_obj(T_obj_table)
T_obj_table_plot.translation[0] += 0.1
vis.figure()
vis.mesh(mesh, T_obj_table_plot, color=(1, 0, 0), style='wireframe')
vis.points(Point(mesh.center_of_mass, 'obj'),
T_obj_table_plot,
color=(1, 0, 1),
scale=0.01)
vis.pose(T_obj_table_plot, alpha=0.1)
vis.mesh_stable_pose(mesh,
stable_pose,
dim=table_dim,
color=(0, 1, 0),
style='surface')
vis.pose(stable_pose.T_obj_table, alpha=0.1)
vis.show()
exit(0)
# compute stable poses
vis.figure()
vis.mesh(mesh, color=(1, 1, 0), style='surface')
vis.mesh(mesh.convex_hull(), color=(1, 0, 0))
stable_poses = mesh.stable_poses()
vertices = mesh.vertices
triangles = mesh.triangles
normals = mesh.normals
print 'Num vertices:', len(vertices)
print 'Num triangles:', len(triangles)
print 'Num normals:', len(normals)
# 1. Generate candidate pairs of contact points
# 2. Check for force closure
# 3. Convert each grasp to a hand pose
contact1 = vertices[500]
contact2 = vertices[2000]
T_obj_gripper = contacts_to_baxter_hand_pose(contact1, contact2)
print 'Translation', T_obj_gripper.translation
print 'Rotation', T_obj_gripper.quaternion
pose_msg = T_obj_gripper.pose_msg
# 3d visualization to help debug
vis.figure()
vis.mesh(mesh)
vis.points(Point(contact1, frame='test'))
vis.points(Point(contact2, frame='test'))
vis.pose(T_obj_gripper, alpha=0.05)
vis.show()
# 4. Execute on the actual robot
vis.imshow(depth_im)
vis.subplot(1, num_plot, 2)
vis.imshow(segmask)
vis.subplot(1, num_plot, 3)
vis.imshow(obj_segmask)
vis.show()
from visualization import Visualizer3D as vis3d
point_cloud = camera_intr.deproject(depth_im)
vis3d.figure()
vis3d.points(point_cloud,
subsample=3,
random=True,
color=(0, 0, 1),
scale=0.001)
vis3d.pose(RigidTransform())
vis3d.pose(T_camera_world.inverse())
vis3d.show()
# Create state.
rgbd_im = RgbdImage.from_color_and_depth(color_im, depth_im)
state = RgbdImageState(rgbd_im, camera_intr, segmask=segmask)
# Init policy.
policy_type = "cem"
if "type" in policy_config:
policy_type = policy_config["type"]
if policy_type == "ranking":
policy = RobustGraspingPolicy(policy_config)
else:
policy = CrossEntropyRobustGraspingPolicy(policy_config)
vis3d.figure()
vis3d.points(points_world,
color=(0, 1, 0),
subsample=10,
random=True,
scale=0.001)
vis3d.points(true_robot_points_world,
color=(0, 0, 1),
scale=0.001)
vis3d.points(fixed_robot_points_world,
color=(1, 1, 0),
scale=0.001)
vis3d.points(est_robot_points_world,
color=(1, 0, 0),
scale=0.001)
vis3d.pose(T_camera_world)
vis3d.show()
# save tranformation arrays based on setup
output_dir = os.path.join(config['calib_dir'], sensor_frame)
if not os.path.exists(output_dir):
os.makedirs(output_dir)
pose_filename = os.path.join(output_dir,
'%s_to_world.tf' % (sensor_frame))
T_camera_world.save(pose_filename)
intr_filename = os.path.join(output_dir, '%s.intr' % (sensor_frame))
ir_intrinsics.save(intr_filename)
f = open(
os.path.join(output_dir, 'corners_cb_%s.npy' % (sensor_frame)),
'w')
np.save(f, reg_result.cb_points_cam.data)
# camera pose
cam_dist = 0.3
T_camera_world = RigidTransform(rotation=np.array([[0, 1,
0], [1, 0, 0],
[0, 0, -1]]),
translation=[0, 0, cam_dist],
from_frame=camera_intr.frame,
to_frame='world')
T_obj_camera = T_camera_world.inverse() * T_obj_world
# show mesh
if False:
vis3d.figure()
vis3d.mesh(mesh, T_obj_camera)
vis3d.pose(RigidTransform(), alpha=0.1)
vis3d.pose(T_obj_camera, alpha=0.1)
vis3d.show()
# render depth image
render_start = time.time()
IPython.embed()
renders = virtual_camera.wrapped_images(mesh, [T_obj_camera],
RenderMode.RGBD_SCENE,
mat_props=mat_props,
light_props=light_props,
debug=False)
render_stop = time.time()
logging.info('Render took %.3f sec' % (render_stop - render_start))
vis.subplot(d, d, k + 1)
