def get_hand_rot():
"""
Computes the pose of the hand to be perpendicular to the direction of the push
Parameters
----------
start : 3x' :obj:`numpy.ndarray`
end : 3x' :obj:`numpy.ndarray`
Returns
-------
3x3 :obj:`numpy.ndarray`
3D Rotation Matrix
"""
# z = normalize(end - start)
# y = normalize(np.cross(z, -up))
# x = normalize(np.cross(z, -y))
# y = normalize(start - end)
# z = normalize(np.cross(y, up))
# x = normalize(np.cross(y, z))
# x = normalize(start - end)
x = normalize(end - start)
y = normalize(np.cross(x, up))
z = normalize(np.cross(x, y))
return np.hstack((x.reshape((-1,1)), y.reshape((-1,1)), z.reshape((-1,1))))
def tipping_point_rotations(self):
tipping_point_rotations = []
for edge, (edge_point1, edge_point2) in enumerate(self.edge_points):
com_projected_on_edge = self.com_projected_on_edges[edge]
s = normalize(edge_point2 - edge_point1)
x = normalize(com_projected_on_edge - self.com)
y = -normalize(np.cross(x, up))
a = .2 if self.obj.key == "mini_dexnet~yoda" else .01
topple_angle = math.acos(np.dot(x, -up)) + a #.01
tipping_point_rotations.append(
RigidTransform.rotation_from_axis_and_origin(
y, edge_point1, topple_angle))
return tipping_point_rotations
def finger_friction_moment(self, f_z, edge_point1, edge_point2):
"""
maximum increase in torque that can be resisted due to the finger pressing down
on the object. (If you press down harder, you can resist more torques, if you
are lifting up the object, it will resist fewer torques)
Parameters
----------
f_z : float
how much the finger would press in the downward direction
in order to topple the object
edge_point1 : 3x1 :obj:`numpy.ndarray`
edge_point2 : 3x1 :obj:`numpy.ndarray`
Returns
-------
float
"""
v = self.com - edge_point1
s = normalize(edge_point2 - edge_point1)
com_projected_on_edge = v.dot(s) * s + edge_point1
offset_dist = np.linalg.norm(edge_point1 - edge_point2)
offsets = np.linspace(0, offset_dist, self.num_approx)
offsets_relative_to_com = offsets - np.linalg.norm(
com_projected_on_edge - edge_point1)
downward_force_dist = np.array([f_z / float(self.num_approx)] *
self.num_approx)
return self.ground_friction_coeff * downward_force_dist.dot(
np.abs(offsets_relative_to_com))
def induced_torque(self, vertex, push_direction, edge_point1, edge_point2,
com_projected_on_edge, required_force):
"""
how much torque around the z axis (centered around com_projected_on_edge)
does the pushing action exert on the object
Parameters
----------
vertex : 3x1 :obj:`numpy.ndarray`
push_direction : 3x1 :obj:`numpy.ndarray`
edge_point1 : 3x1 :obj:`numpy.ndarray`
edge_point2 : 3x1 :obj:`numpy.ndarray`
com_projected_on_edge : 3x1 :obj:`numpy.ndarray`
Returns
-------
float
"""
if self.baseline:
return 0
r = vertex - com_projected_on_edge
r[2] = 0
max_z_torque_dir = normalize(np.cross(r, up))
cos_push_vertex_z_angle = push_direction.dot(max_z_torque_dir)
r = np.linalg.norm(r)
return required_force * cos_push_vertex_z_angle * r
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 compute_gradients(points, values):
# def grad_helper(points, values):
# n_approx = 10
# grads = []
# for point, value in zip(points, values):
# idxes = np.argsort((np.sqrt((points - point)**2).sum(axis=0)))
# grad = np.zeros(3)
# for idx in idxes[:n_approx]:
# grad += (points[idx] - point) * (values[idx] - value)
# grads.append(grad / n_approx)
# return np.mean(grads, axis=0)
w1 = normalize(np.linalg.lstsq(points, values)[0])
projected_points = np.reshape(points.dot(w1),
(-1, 1)) * np.reshape(w1, (1, -1))
nullspace_points = points - projected_points
w2 = normalize(np.linalg.lstsq(nullspace_points, values)[0])
return w1, w2
def required_force(self, vertex, push_direction, edge_point1, edge_point2,
com_projected_on_edge):
"""
How much to press against the queried point in order to counteract gravity
Parameters
----------
vertex : 3x1 :obj:`numpy.ndarray`
push_direction : 3x1 :obj:`numpy.ndarray`
edge_point1 : 3x1 :obj:`numpy.ndarray`
edge_point2 : 3x1 :obj:`numpy.ndarray`
com_projected_on_edge : 3x1 :obj:`numpy.ndarray`
Returns
-------
float
"""
# s = normalize(edge_point2 - edge_point1)
# vertex_projected_on_edge = (vertex - edge_point1).dot(s)*s + edge_point1
# f_max = normalize(np.cross(edge_point1 - vertex, s))
# if f_max[2] < 0: # is this right??????
# f_max = -f_max
# r_f = np.linalg.norm(vertex - vertex_projected_on_edge)
# cos_push_vertex_edge_angle = push_direction.dot(f_max)
# g_max = normalize(-np.cross(edge_point1 - self.com, s))
# if g_max[2] > 0: # is this right??????
# g_max = -g_max
# r_g = np.linalg.norm(self.com - com_projected_on_edge)
# cos_com_edge_angle = (-up).dot(g_max)
#
# return self.mass * 9.8 * cos_com_edge_angle * r_g / (cos_push_vertex_edge_angle * r_f + 1e-5)
# [0.01282592 0.01404448 0.1]
s = normalize(edge_point2 - edge_point1)
vertex_projected_on_edge = (vertex -
edge_point1).dot(s) * s + edge_point1
push_projected = push_direction.dot(s) * s
p_f = push_direction - push_projected
r_f = vertex_projected_on_edge - vertex
tau_f = np.cross(r_f, p_f)
r_g = com_projected_on_edge - self.com
tau_g = np.cross(r_g, self.mass * 9.8 * -up)
# since the two torque vectors are aligned, the ratio of tau_g / tau_f
# should be the same regardless so we just take the the x value
return -(tau_g / (tau_f + 1e-7))[0]
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)
def load_object(self, obj):
"""
Does a lot of the preprocessing, like calculating the bottom points,
and max_moment / max_tangential_forces for each edge. This way you can
call predict multiple times and not redo too much work
Parameters
----------
obj : :obj:`GraspableObject3D`
object to load
"""
self.obj = obj
self.mesh = obj.mesh.copy().apply_transform(obj.T_obj_world.matrix)
self.mesh.fix_normals()
self.com = self.mesh.center_mass
# self.com = obj.T_obj_world.translation
# self.com = np.mean(self.mesh.vertices, axis=0)
self.mass = 1
# Finding toppling edge
z_components = np.around(self.mesh.vertices[:, 2], 3)
self.lowest_z = np.min(z_components)
cutoff = .02
bottom_points = self.mesh.vertices[z_components ==
self.lowest_z][:, :2]
bottom_points = bottom_points[ConvexHull(bottom_points).vertices]
self.bottom_points = np.append(
bottom_points,
#ensure all points lie on plane
self.lowest_z * np.ones(len(bottom_points)).reshape((-1, 1)),
axis=1)
#print 'bottom', self.bottom_points
#self.bottom_points = self.bottom_points[::5]
#print 'bottom', self.bottom_points
# Turn bottom points into pairs of adjacent bottom_points
self.edge_points = zip(self.bottom_points,
np.roll(self.bottom_points, -1, axis=0))
edge = 1
#self.edge_points = [self.edge_points[edge]]
#self.bottom_points = self.bottom_points[[edge,edge+1]]
# For each edge, calculate the maximum moment
# and maximum tangential force it can resist
self.max_moments, self.max_tangential_forces, self.com_projected_on_edges = [], [], []
for edge_point1, edge_point2 in self.edge_points:
s = normalize(edge_point2 - edge_point1)
com_projected_on_edge = (self.com -
edge_point1).dot(s) * s + edge_point1
offset_dist = np.linalg.norm(edge_point1 - edge_point2)
offsets = np.linspace(0, offset_dist, self.num_approx)
offsets_relative_to_com = offsets - np.linalg.norm(
com_projected_on_edge - edge_point1)
# This is for a non-uniform pressure distribution
#paths = self.mesh.section_multiplane(edge_point1, s, offsets)
#mass_per_unit = np.array([0 if paths[index] == None else paths[index].area for index in range(self.num_approx)])
# Using a uniform pressure distribution along the contact edge (for now)
mass_per_unit = np.array([self.mass / float(self.num_approx)] *
self.num_approx)
# pressure distribution from mass
pressure_dist = mass_per_unit * self.mass * 9.8 / (
np.sum(mass_per_unit) + offset_dist / self.num_approx)
self.max_moments.append(
self.ground_friction_coeff *
pressure_dist.dot(np.abs(offsets_relative_to_com)))
# self.max_moments.append(self.ground_friction_coeff * mass * 9.8 * (y0**2 + y1**2) / 2) # Why doesn't this work ?????
self.max_tangential_forces.append(self.ground_friction_coeff *
(self.mass * 9.8)) # mu F_n
self.com_projected_on_edges.append(com_projected_on_edge)
def add_noise(self, vertices, normals, push_directions, n_trials):
"""
adds noise to the vertex position and friction, and intersects that new position with
where the finger would now press against the object
Parameters
----------
vertices : nx3 :obj:`numpy.ndarray`
normals : nx3 :obj:`numpy.ndarray`
push_direction : nx3 :obj:`numpy.ndarray`
n_trials : int
Returns
nx3 :obj`numpy.ndarray`
nx3 :obj`numpy.ndarray`
nx3 :obj`numpy.ndarray`
nx1 :obj`numpy.ndarray`
"""
vertices = np.repeat(vertices, n_trials, axis=0)
normals = np.repeat(normals, n_trials, axis=0)
push_directions = np.repeat(push_directions, n_trials, axis=0) \
+ np.random.normal(scale=self.push_direction_sigma, size=[len(push_directions)*n_trials, 3])
push_directions = normalize(push_directions, axis=1)
a = time()
# # Add noise and find the new intersection location
# vertices_copied = deepcopy(vertices)
# ray_origins = vertices + .01 * normals
# # ray_origins = vertices + np.random.normal(scale=sigma, size=vertices.shape) + .01 * normals
# vertices, _, face_ind = mesh.ray.intersects_location(ray_origins, -normals, multiple_hits=False)
for i in range(len(vertices)):
# Predicted to hit ground
if push_directions[i].dot(up) > .5: # and vertices[i][2] < .05:
vertices[i] = np.array([0, 0, 0])
normals[i] = np.array([0, 0, 0])
continue
ray_origin = vertices[i] + np.random.normal(
scale=self.finger_sigma, size=3) + .001 * normals[i]
rand_axis = normalize(np.random.rand(3))
obj_rotation = RigidTransform.rotation_from_axis_and_origin(
rand_axis, self.mesh.center_mass,
np.random.normal(scale=self.obj_rot_sigma)).matrix
ray_origin = obj_rotation.dot(np.append(ray_origin, [1]))[:3]
ray_dir = obj_rotation.dot(np.append(-normals[i], [1]))[:3]
# ray_dir = -normals[i]
intersect, _, face_ind = \
self.mesh.ray.intersects_location([ray_origin], [ray_dir], multiple_hits=False)
_, _, back_face_ind = \
self.mesh.ray.intersects_location([ray_origin], [-ray_dir], multiple_hits=False)
if len(face_ind) == 0 or len(back_face_ind) != 0:
vertices[i] = np.array([0, 0, 0])
normals[i] = np.array([0, 0, 0])
else:
vertices[i] = intersect[0]
normals[i] = self.mesh.face_normals[face_ind[0]]
ground_friction_noises = 1 + np.random.normal(
scale=self.ground_friction_sigma,
size=len(vertices)) / self.ground_friction_coeff
finger_friction_noises = 1 + np.random.normal(
scale=self.finger_friction_sigma,
size=len(vertices)) / self.finger_friction_coeff
if self.log:
print 'noise time:', time() - a
return vertices, normals, push_directions, ground_friction_noises, finger_friction_noises
