right_gripper.calibrate()
listener = tf.TransformListener()
from_frame = 'base'
time.sleep(1)
# Main Code
br = tf.TransformBroadcaster()
# SETUP
mesh = trimesh.load(MESH_FILENAME)
vertices = mesh.vertices
triangles = mesh.triangles
normals = mesh.vertex_normals
of = ObjFile(MESH_FILENAME)
mesh = of.read()
ar_tag = lookup_tag(TAG)
# find the transformation from the object coordinates to world coordinates... somehow
grasp_indices, best_metric_indices = sorted_contacts(
vertices, normals, T_ar_object)
for indices in best_metric_indices[0:5]:
a = grasp_indices[indices][0]
b = grasp_indices[indices][1]
normal1, normal2 = normals[a], normals[b]
contact1, contact2 = vertices[a], vertices[b]
# visualize the mesh and contacts
vis.figure()
vis.mesh(mesh)
vis.normals(
os.makedirs(output_path)
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()
from perception import CameraIntrinsics
from visualization import Visualizer2D
from core import RigidTransform
import numpy as np
from matplotlib import pyplot as plt
from perception.object_render import RenderMode
dexnet_path = "/home/chris/optimal_manipulation_simulator/dex-net-new-api"
if __name__ == "__main__":
print "\n\n\n\n"
camera_intr = CameraIntrinsics.load("../config/primesense_overhead.intr")
camera = VirtualCamera(camera_intr)
of = ObjFile(dexnet_path + "/data/meshes/chess_pieces/WizRook.obj")
rook1 = of.read()
T_world_camera = RigidTransform(rotation=np.array([[-1, 0, 0], [0, 1, 0],
[0, 0, -1]]),
translation=np.array([0, 0, .2]).T,
from_frame="world",
to_frame="primesense_overhead")
# T_world_camera = RigidTransform(rotation = np.array([[1,0,0],[0,1,0],[0,0,1]]),
# translation=np.array([-.2,0,0]).T,
# from_frame="world",
# to_frame="primesense_overhead")
# squares = [(-1,-1), (-1,0), (-1,1), (0,-1), (0,1), (1,-1), (1,0), (1,1)]
# for i, square in enumerate(squares):
T_rook2_world = RigidTransform(rotation=np.eye(3),
y_axis = y_axis / np.linalg.norm(y_axis)
z_axis = np.array([y_axis[1], -y_axis[0], 0]) # the z axis will always be in the table plane for now
z_axis = z_axis / np.linalg.norm(z_axis)
x_axis = np.cross(y_axis, z_axis)
# convert to hand pose
R_obj_gripper = np.array([x_axis, y_axis, z_axis]).T
t_obj_gripper = center
return RigidTransform(rotation=R_obj_gripper,
translation=t_obj_gripper,
from_frame='gripper',
to_frame='obj')
if __name__ == '__main__':
of = ObjFile(SPRAY_BOTTLE_MESH_FILENAME)
mesh = of.read()
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]
