def compute_topple(self, state):
# raise NotImplementedError
mesh = state.mesh.copy().apply_transform(state.T_obj_world.matrix)
mesh.fix_normals()
vertices, face_ind = sample.sample_surface_even(mesh, self.num_samples)
# Cut out vertices that are too close to the ground
z_comp = vertices[:,2]
valid_vertex_ind = z_comp > (1-self.thresh)*np.min(z_comp) + self.thresh*np.max(z_comp)
vertices, face_ind = vertices[valid_vertex_ind], face_ind[valid_vertex_ind]
normals = mesh.face_normals[face_ind]
push_directions = -deepcopy(normals)
self.toppling_model.load_object(state)
poses, vertex_probs, min_required_forces = self.toppling_model.predict(
vertices,
normals,
push_directions,
use_sensitivity=self.use_sensitivity
)
from dexnet.visualization import DexNetVisualizer3D as vis3d
from autolab_core import Point
# j = 6 # gearbox
j = 50 # vase
a = j*self.toppling_model.n_trials
b = (j+1)*self.toppling_model.n_trials
for i in range(a,b):
start = self.toppling_model.vertices[i]
end = start - .01 * self.toppling_model.push_directions[i]
vis3d.plot3d([start, end], color=[0,1,0], tube_radius=.0002)
vis3d.mesh(state.mesh, state.T_obj_world)
vis3d.show()
return vertices, normals, poses, vertex_probs, min_required_forces
def visualize(env, datasets, obj_id_to_keys, models, model_names,
use_sensitivities):
for dataset, obj_id_to_key in zip(datasets, obj_id_to_keys):
for i in range(dataset.num_datapoints):
datapoint = dataset.datapoint(i)
dataset_name, key = obj_id_to_key[str(
datapoint['obj_id'])].split(KEY_SEP_TOKEN)
obj = env._state_space._database.dataset(dataset_name)[key]
orig_pose = to_rigid(datapoint['obj_pose'])
obj.T_obj_world = orig_pose
env.state.obj = obj
actual = 0
for i in range(NUM_PER_DATAPOINT):
mat = datapoint['actual_poses'][i * 4:(i + 1) * 4]
actual_pose = to_rigid(mat)
if not is_equivalent_pose(actual_pose, orig_pose):
actual += 1
actual /= float(NUM_PER_DATAPOINT)
probs = []
for model, model_name, use_sensitivity in zip(
models, model_names, use_sensitivities):
model.load_object(env.state)
_, vertex_probs, _ = model.predict(
[datapoint['vertex']],
[datapoint['normal']],
[-datapoint['normal']], # push dir
use_sensitivity=use_sensitivity)
probs.append(1 - vertex_probs[0, 0])
if -(abs(probs[3] - actual) - abs(probs[2] - actual)) > .1:
print 'actual {} {} {} {} {} {} {} {} {}'.format(
actual, model_names[0], probs[0], model_names[1], probs[1],
model_names[2], probs[2], model_names[3], probs[3])
env.render_3d_scene()
start_point = datapoint['vertex'] + .06 * datapoint['normal']
end_point = datapoint['vertex']
shaft_points = [start_point, end_point]
h1 = np.array([[0.7071, -0.7071, 0], [0.7071, 0.7071, 0],
[0, 0, 1]]).dot(-datapoint['normal'])
h2 = np.array([[0.7071, 0.7071, 0], [-0.7071, 0.7071, 0],
[0, 0, 1]]).dot(-datapoint['normal'])
head_points = [
end_point - 0.02 * h2, end_point, end_point - 0.02 * h1
]
vis3d.plot3d(shaft_points, color=[1, 0, 0], tube_radius=.002)
vis3d.plot3d(head_points, color=[1, 0, 0], tube_radius=.002)
vis3d.show()
def vis_axes(origin, y_dir):
y = [origin, origin + .0075 * y_dir]
z = [origin, origin + .0075 * up]
x = [origin, origin + .0075 * np.cross(y_dir, up)]
vis3d.plot3d(x, color=[1, 0, 0], tube_radius=.0006)
vis3d.plot3d(y, color=[0, 1, 0], tube_radius=.0006)
vis3d.plot3d(z, color=[0, 0, 1], tube_radius=.0006)
def figure_0():
action = policy.action(env.state)
env.render_3d_scene()
bottom_points = policy.toppling_model.bottom_points
vis3d.plot3d(bottom_points[:2], color=[0, 0, 0], tube_radius=.001)
mesh = env.state.mesh.copy().apply_transform(env.state.T_obj_world.matrix)
mesh.fix_normals()
direction = normalize([0, -.04, 0])
origin = mesh.center_mass + np.array([0, .04, .09])
intersect, _, face_ind = mesh.ray.intersects_location([origin],
[direction],
multiple_hits=False)
normal = mesh.face_normals[face_ind[0]]
start_point = intersect[0] + .06 * normal
end_point = intersect[0]
shaft_points = [start_point, end_point]
h1 = np.array([[1, 0, 0], [0, 0.7071, -0.7071], [0, 0.7071,
0.7071]]).dot(-normal)
h2 = np.array([[1, 0, 0], [0, 0.7071, 0.7071], [0, -0.7071,
0.7071]]).dot(-normal)
head_points = [end_point - 0.02 * h2, end_point, end_point - 0.02 * h1]
vis3d.plot3d(shaft_points, color=[1, 0, 0], tube_radius=.002)
vis3d.plot3d(head_points, color=[1, 0, 0], tube_radius=.002)
vis3d.points(Point(end_point), scale=.004, color=[0, 0, 0])
hand_pose = RigidTransform(rotation=policy.get_hand_pose(
start_point, end_point)[0].rotation,
translation=start_point,
from_frame='grasp',
to_frame='world')
orig_pose = env.state.obj.T_obj_world.copy()
env.state.obj.T_obj_world = policy.toppling_model.final_poses[1]
action = policy.grasping_policy.action(env.state)
env.state.obj.T_obj_world = orig_pose
gripper = env.gripper(action)
#vis3d.mesh(gripper.mesh, hand_pose * gripper.T_mesh_grasp, color=light_blue)
# mesh = trimesh.load('~/Downloads/chris.stl')
rot = np.array([[0, -1, 0], [1, 0, 0],
[0, 0,
1]]).dot(np.array([[0, 0, 1], [0, 1, 0], [-1, 0, 0]]))
T = RigidTransform(rotation=rot,
translation=np.array([0, -.09, .1]),
to_frame='mesh',
from_frame='mesh')
# vis3d.mesh(mesh, hand_pose * gripper.T_mesh_grasp * T, color=light_blue)
vis3d.mesh(gripper.mesh,
hand_pose * gripper.T_mesh_grasp * T,
color=light_blue)
vis3d.show(starting_camera_pose=CAMERA_POSE)
env.state.obj.T_obj_world = policy.toppling_model.final_poses[1]
action = policy.grasping_policy.action(env.state)
print 'q:', action.q_value
env.render_3d_scene()
vis3d.gripper(env.gripper(action),
action.grasp(env.gripper(action)),
color=light_blue)
vis3d.show(starting_camera_pose=CAMERA_POSE)
def figure_1():
env.state.obj.T_obj_world.translation += np.array([-.01, -.05, .001])
action = policy.action(env.state)
env.render_3d_scene()
bottom_points = policy.toppling_model.bottom_points
vis3d.plot3d(bottom_points[:2], color=[0, 0, 0], tube_radius=.0005)
vis3d.points(Point(bottom_points[0]), color=[0, 0, 0], scale=.001)
vis3d.points(Point(bottom_points[1]), color=[0, 0, 0], scale=.001)
y_dir = normalize(bottom_points[1] - bottom_points[0])
origin = policy.toppling_model.com_projected_on_edges[0] - .005 * y_dir
vis_axes(origin, y_dir)
#mesh = env.state.mesh.copy().apply_transform(env.state.T_obj_world.matrix)
#mesh.fix_normals()
#direction = normalize([-.03, -.07, 0])
#intersect, _, face_ind = mesh.ray.intersects_location([[.02, -.005, .09]], [direction], multiple_hits=False)
#normal = mesh.face_normals[face_ind[0]]
#start_point = intersect[0] + .03*normal
#end_point = intersect[0]
#shaft_points = [start_point, end_point]
#h1 = np.array([[0.7071,-0.7071,0],[0.7071,0.7071,0],[0,0,1]]).dot(-normal)
#h2 = np.array([[0.7071,0.7071,0],[-0.7071,0.7071,0],[0,0,1]]).dot(-normal)
#head_points = [end_point - 0.01*h2, end_point, end_point - 0.01*h1]
#vis3d.plot3d(shaft_points, color=[1,0,0], tube_radius=.001)
#vis3d.plot3d(head_points, color=[1,0,0], tube_radius=.001)
#vis3d.points(Point(end_point), scale=.002, color=[0,0,0])
# Center of Mass
#start_point = env.state.T_obj_world.translation - .0025*y_dir - np.array([0,0,.005])
start_point = env.state.T_obj_world.translation
end_point = start_point - np.array([0, 0, .03])
vis3d.points(Point(start_point), scale=.002, color=[0, 0, 0])
shaft_points = [start_point, end_point]
h1 = np.array([[1, 0, 0], [0, 0.7071, -0.7071], [0, 0.7071,
0.7071]]).dot(-up)
h2 = np.array([[1, 0, 0], [0, 0.7071, 0.7071], [0, -0.7071,
0.7071]]).dot(-up)
head_points = [end_point - 0.01 * h2, end_point, end_point - 0.01 * h1]
vis3d.plot3d(shaft_points, color=[1, 0, 0], tube_radius=.001)
vis3d.plot3d(head_points, color=[1, 0, 0], tube_radius=.001)
vis3d.show(starting_camera_pose=CAMERA_POSE)
def failure_modes():
# dataset = env._state_space._database.dataset('mini_dexnet')
# obj = dataset['pawn']
# obj.T_obj_world = dataset.stable_poses('pawn')[8].T_obj_table
# env.state.obj = obj
# mesh = obj.mesh.copy().apply_transform(env.state.T_obj_world.matrix)
# mesh.fix_normals()
# direction = normalize([1, -.005, 0])
# intersect, _, face_ind = mesh.ray.intersects_location([[-1, 0, .09]], [direction], multiple_hits=False)
# direction = normalize([1,-0.25,0])
# start_point = intersect[0] - .03*direction
# end_point = intersect[0]
# shaft_points = [start_point, end_point]
# h1 = np.array([[0.7071,-0.7071,0],[0.7071,0.7071,0],[0,0,1]]).dot(direction)
# h2 = np.array([[0.7071,0.7071,0],[-0.7071,0.7071,0],[0,0,1]]).dot(direction)
# head_points = [end_point - 0.01*h2, end_point, end_point - 0.01*h1]
# vis3d.plot3d(shaft_points, color=red, tube_radius=.001)
# vis3d.plot3d(head_points, color=red, tube_radius=.001)
# env.render_3d_scene()
# vis3d.show(starting_camera_pose=CAMERA_POSE)
# env.render_3d_scene()
# vis3d.show(starting_camera_pose=CAMERA_POSE)
# env.state.obj.T_obj_world = dataset.stable_poses('pawn')[0].T_obj_table
# env.render_3d_scene()
# vis3d.show(starting_camera_pose=CAMERA_POSE)
# dataset = env._state_space._database.dataset('mini_dexnet')
# obj = dataset['yoda']
# obj.T_obj_world = dataset.stable_poses('yoda')[2].T_obj_table
# env.state.obj = obj
# mesh = obj.mesh.copy().apply_transform(env.state.T_obj_world.matrix)
# mesh.fix_normals()
# direction = normalize([0, .1, 0])
# intersect = mesh.center_mass + np.array([0,-.015,.037])
# start_point = intersect - .03*direction
# end_point = intersect
# shaft_points = [start_point, end_point]
# h1 = np.array([[0.7071,-0.7071,0],[0.7071,0.7071,0],[0,0,1]]).dot(direction)
# h2 = np.array([[0.7071,0.7071,0],[-0.7071,0.7071,0],[0,0,1]]).dot(direction)
# head_points = [end_point - 0.01*h2, end_point, end_point - 0.01*h1]
# vis3d.plot3d(shaft_points, color=red, tube_radius=.001)
# vis3d.plot3d(head_points, color=red, tube_radius=.001)
# env.render_3d_scene()
# vis3d.show(starting_camera_pose=CAMERA_POSE)
# env.state.obj.T_obj_world = dataset.stable_poses('yoda')[4].T_obj_table
# env.render_3d_scene()
# vis3d.show(starting_camera_pose=CAMERA_POSE)
dataset = env._state_space._database.dataset('mini_dexnet')
obj = dataset['vase']
obj.T_obj_world = dataset.stable_poses('vase')[5].T_obj_table
env.state.obj = obj
mesh = obj.mesh.copy().apply_transform(env.state.T_obj_world.matrix)
mesh.fix_normals()
direction = normalize([.03, .1, 0])
intersect = mesh.center_mass + np.array([-.019, -.02, .02])
start_point = intersect - .03 * direction
end_point = intersect
shaft_points = [start_point, end_point]
h1 = np.array([[0.7071, -0.7071, 0], [0.7071, 0.7071, 0],
[0, 0, 1]]).dot(direction)
h2 = np.array([[0.7071, 0.7071, 0], [-0.7071, 0.7071, 0],
[0, 0, 1]]).dot(direction)
head_points = [end_point - 0.01 * h2, end_point, end_point - 0.01 * h1]
vis3d.plot3d(shaft_points, color=red, tube_radius=.001)
vis3d.plot3d(head_points, color=red, tube_radius=.001)
env.render_3d_scene()
vis3d.show(starting_camera_pose=CAMERA_POSE)
env.state.obj.T_obj_world = dataset.stable_poses('vase')[4].T_obj_table
env.render_3d_scene()
vis3d.show(starting_camera_pose=CAMERA_POSE)
def figure_3():
#env.state.obj.T_obj_world.translation += np.array([-.01,-.05,.01])
action = policy.action(env.state)
mesh = env.state.obj.mesh.copy().apply_transform(
env.state.T_obj_world.matrix)
mesh = mesh.slice_plane([0, 0, .0005], -up)
from dexnet.grasping import GraspableObject3D
env.state.obj = GraspableObject3D(mesh)
env.render_3d_scene()
bottom_points = policy.toppling_model.bottom_points
vis3d.plot3d(bottom_points[:2], color=[0, 0, 0], tube_radius=.0005)
vis3d.points(Point(bottom_points[0]), color=[0, 0, 0], scale=.001)
vis3d.points(Point(bottom_points[1]), color=[0, 0, 0], scale=.001)
y_dir = normalize(bottom_points[1] - bottom_points[0])
origin = policy.toppling_model.com_projected_on_edges[0] - .0025 * y_dir
vis3d.points(Point(origin), color=[0, 0, 1], scale=.001)
# t = .002
# x = np.cross(y_dir, up)
# while t < np.linalg.norm(origin - bottom_points[0]):
# start_point = origin - t*y_dir
# end_point = start_point + .0075*x
# shaft_points = [start_point, end_point]
# h1 = np.array([[0.7071,-0.7071,0],[0.7071,0.7071,0],[0,0,1]]).dot(x)
# h2 = np.array([[0.7071,0.7071,0],[-0.7071,0.7071,0],[0,0,1]]).dot(x)
# head_points = [end_point - 0.001*h2, end_point, end_point - 0.001*h1]
# vis3d.plot3d(shaft_points, color=purple, tube_radius=.0002)
# vis3d.plot3d(head_points, color=purple, tube_radius=.0002)
# t += .002
## try 2
x = np.cross(y_dir, up)
t = .004
arrow_dir = x
start_point = origin - t * y_dir
end_point = start_point + .0075 * arrow_dir
shaft_points = [start_point, end_point]
h1 = np.array([[0.7071, -0.7071, 0], [0.7071, 0.7071, 0], [0, 0,
1]]).dot(x)
h2 = np.array([[0.7071, 0.7071, 0], [-0.7071, 0.7071, 0], [0, 0,
1]]).dot(x)
head_points = [end_point - 0.001 * h2, end_point, end_point - 0.001 * h1]
vis3d.plot3d(shaft_points, color=purple, tube_radius=.0002)
vis3d.plot3d(head_points, color=purple, tube_radius=.0002)
# t = .000
# while t < np.linalg.norm(origin - bottom_points[1]):
# arrow_dir = x
# start_point = origin + t*y_dir
# end_point = start_point + .0075*arrow_dir
# shaft_points = [start_point, end_point]
# h1 = np.array([[0.7071,-0.7071,0],[0.7071,0.7071,0],[0,0,1]]).dot(arrow_dir)
# h2 = np.array([[0.7071,0.7071,0],[-0.7071,0.7071,0],[0,0,1]]).dot(arrow_dir)
# head_points = [end_point - 0.001*h2, end_point, end_point - 0.001*h1]
# vis3d.plot3d(shaft_points, color=purple, tube_radius=.0002)
# vis3d.plot3d(head_points, color=purple, tube_radius=.0002)
# t += .002
## try 2
t = .01
arrow_dir = -x
start_point = origin + t * y_dir
end_point = start_point + .0075 * arrow_dir
shaft_points = [start_point, end_point]
h1 = np.array([[0.7071, -0.7071, 0], [0.7071, 0.7071, 0],
[0, 0, 1]]).dot(arrow_dir)
h2 = np.array([[0.7071, 0.7071, 0], [-0.7071, 0.7071, 0],
[0, 0, 1]]).dot(arrow_dir)
head_points = [end_point - 0.001 * h2, end_point, end_point - 0.001 * h1]
vis3d.plot3d(shaft_points, color=purple, tube_radius=.0002)
vis3d.plot3d(head_points, color=purple, tube_radius=.0002)
#arrow_dir = np.cross(y_dir, up)
#start_point = origin - .005*y_dir
#end_point = start_point + .0075*arrow_dir
#shaft_points = [start_point, end_point]
#h1 = np.array([[0.7071,-0.7071,0],[0.7071,0.7071,0],[0,0,1]]).dot(arrow_dir)
#h2 = np.array([[0.7071,0.7071,0],[-0.7071,0.7071,0],[0,0,1]]).dot(arrow_dir)
#head_points = [end_point - 0.001*h2, end_point, end_point - 0.001*h1]
#vis3d.plot3d(shaft_points, color=purple, tube_radius=.0002)
#vis3d.plot3d(head_points, color=purple, tube_radius=.0002)
#arrow_dir = -up
#end_point = start_point + .0075*arrow_dir
#shaft_points = [start_point, end_point]
#h1 = np.array([[1,0,0],[0,0.7071,-0.7071],[0,0.7071,0.7071]]).dot(arrow_dir)
#h2 = np.array([[1,0,0],[0,0.7071,0.7071],[0,-0.7071,0.7071]]).dot(arrow_dir)
#head_points = [end_point - 0.001*h2, end_point, end_point - 0.001*h1]
#vis3d.plot3d(shaft_points, color=blue, tube_radius=.0002)
#vis3d.plot3d(head_points, color=blue, tube_radius=.0002)
#
#vis3d.points(Point(start_point), color=[0,1,0], scale=.001)
#vis_axes(origin, y_dir)
vis3d.show(starting_camera_pose=CAMERA_POSE)
sys.exit()
def figure_2():
env.state.material_props._color = np.array([0.75] * 3)
# env.state.obj.T_obj_world.translation += np.array([-.095,.025,.001])
env.state.obj.T_obj_world.translation += np.array([-.01, -.04, .001])
action = policy.action(env.state)
env.render_3d_scene()
bottom_points = policy.toppling_model.bottom_points
edge = 1
vis3d.plot3d(bottom_points[edge:edge + 2],
color=[0, 0, 0],
tube_radius=.0005)
vis3d.points(Point(bottom_points[edge]), color=[0, 0, 0], scale=.001)
vis3d.points(Point(bottom_points[edge + 1]), color=[0, 0, 0], scale=.001)
y_dir = normalize(bottom_points[edge + 1] - bottom_points[edge])
origin = policy.toppling_model.com_projected_on_edges[
edge] # - .0025*y_dir
vis_axes(origin, y_dir)
mesh = env.state.mesh.copy().apply_transform(env.state.T_obj_world.matrix)
mesh.fix_normals()
#direction = normalize([-.03, -.07, 0])
#intersect, _, face_ind = mesh.ray.intersects_location([[.02, -.005, .09]], [direction], multiple_hits=False)
ray_origin = mesh.center_mass + np.array([.1, -.1, .035])
direction = normalize([-.1, .1, 0])
intersect, _, face_ind = mesh.ray.intersects_location([ray_origin],
[direction],
multiple_hits=False)
normal = mesh.face_normals[face_ind[0]]
start_point = intersect[0] + .03 * normal
end_point = intersect[0]
shaft_points = [start_point, end_point]
h1 = np.array([[0.7071, 0, -0.7071], [0, 1, 0], [0.7071, 0,
0.7071]]).dot(-normal)
h2 = np.array([[0.7071, 0, 0.7071], [0, 1, 0], [-0.7071, 0,
0.7071]]).dot(-normal)
head_points = [end_point - 0.01 * h2, end_point, end_point - 0.01 * h1]
vis3d.plot3d(shaft_points, color=red, tube_radius=.001)
vis3d.plot3d(head_points, color=red, tube_radius=.001)
vis3d.points(Point(end_point), scale=.002, color=[0, 0, 0])
# Center of Mass
#start_point = env.state.T_obj_world.translation# - .0025*y_dir - np.array([0,0,.005])
start_point = mesh.center_mass
end_point = start_point - np.array([0, 0, .03])
vis3d.points(Point(start_point), scale=.002, color=[0, 0, 0])
shaft_points = [start_point, end_point]
h1 = np.array([[1, 0, 0], [0, 0.7071, -0.7071], [0, 0.7071,
0.7071]]).dot(-up)
h2 = np.array([[1, 0, 0], [0, 0.7071, 0.7071], [0, -0.7071,
0.7071]]).dot(-up)
head_points = [end_point - 0.01 * h2, end_point, end_point - 0.01 * h1]
vis3d.plot3d(shaft_points, color=red, tube_radius=.001)
vis3d.plot3d(head_points, color=red, tube_radius=.001)
# Dotted lines
r_gs = dotted_line(start_point, origin)
for r_g in r_gs:
vis3d.plot3d(r_g, color=orange, tube_radius=.0006)
s = normalize(bottom_points[edge + 1] - bottom_points[edge])
vertex_projected_on_edge = (
intersect[0] - bottom_points[edge]).dot(s) * s + bottom_points[edge]
r_fs = dotted_line(intersect[0], vertex_projected_on_edge)
for r_f in r_fs:
vis3d.plot3d(r_f, color=orange, tube_radius=.0006)
vis3d.show(starting_camera_pose=CAMERA_POSE)
usr_input = utils.keyboard_input('Did the pose registration work? [y/n]:')
env.state.obj.T_obj_world = s2r*orig_pose
# Plan topple action
action = policy.action(env.state, push_idx)
if args.topple_probs:# and push_idx == None:
vis3d.figure()
env.render_3d_scene()
for vertex, prob in zip(action.metadata['vertices'], action.metadata['topple_probs']):
color = [min(1, 2*(1-prob)), min(2*prob, 1), 0]
vis3d.points(Point(vertex, 'world'), scale=.001, color=color)
gripper = env.gripper(action)
# vis3d.plot3d([action.T_begin_world.translation, action.T_end_world.translation], color=(0,1,0), tube_radius=.0006)
pose = action.T_begin_world
vis3d.plot3d([pose.translation, pose.translation + .01*pose.x_axis], color=[1,0,0], tube_radius=.0006)
vis3d.plot3d([pose.translation, pose.translation + .01*pose.y_axis], color=[0,1,0], tube_radius=.0006)
vis3d.plot3d([pose.translation, pose.translation + .01*pose.z_axis], color=[0,0,1], tube_radius=.0006)
try:
vis3d.show(starting_camera_pose=CAMERA_POSE)
except KeyboardInterrupt:
print 'trying different push'
push_idx = None
sample_id = 0
num_toppled, pose_angles, actual_poses = [], [], []
continue
push_idx = action.metadata['best_ind']
final_poses = action.metadata['final_poses']
vertex_probs = action.metadata['vertex_probs'][push_idx]