def is_surface_pose(surface, mesh, pose):
world_from_surface = trans_from_pose(surface.pose)
world_from_mesh = trans_from_pose(pose)
surface_from_mesh = np.linalg.inv(world_from_surface).dot(world_from_mesh)
points_surface = apply_affine(surface_from_mesh, mesh.vertices)
min_z = np.min(points_surface[:, 2])
return (abs(min_z) < 0.01) and all(
is_point_in_polygon(p, surface.convex_hull) for p in points_surface)
def is_edge_pose(surface, pose):
# Pose is assumed to be the center of mass
world_from_surface = trans_from_pose(surface.pose)
world_from_mesh = trans_from_pose(pose)
surface_from_mesh = np.linalg.inv(world_from_surface).dot(world_from_mesh)
points_surface = apply_affine(surface_from_mesh, [point_from_pose(pose)])
min_z = np.min(points_surface[:, 2])
return (abs(min_z) < 0.01) and all(
is_point_in_polygon(p, surface.convex_hull) for p in points_surface)
def sample_surface_pose(surface, mesh):
world_from_surface = trans_from_pose(surface.pose)
for surface_from_origin in sample_polygon_tform(surface.convex_hull,
mesh.vertices):
pose = pose_from_trans(world_from_surface.dot(surface_from_origin))
if is_surface_pose(surface, mesh, pose):
yield pose
def sample_edge_pose(surface, mesh):
world_from_surface = trans_from_pose(surface.pose)
radius = max(length(v[:2]) for v in mesh.vertices)
origin_from_base = trans_from_point(0, 0, np.min(mesh.vertices[:, 2]))
for point in sample_edge_point(surface.convex_hull, radius):
theta = random.uniform(0, 2 * np.pi)
surface_from_origin = trans_from_quat_point(quat_from_z_rot(theta),
point)
yield pose_from_trans(
world_from_surface.dot(surface_from_origin).dot(origin_from_base))
def pose_generator(obj, surface):
surface_pose, convex_hull = surfaces[surface].get_surface()
world_from_surface = trans_from_pose(surface_pose)
body = bodies[obj]
aabb = body.ComputeAABB()
radius = 0 # TODO: compute from aabb
translation = np.array([0, 0, aabb.extents()[2]
]) + (get_point(body) - aabb.pos())
for point in sample_edge_point(convex_hull, radius):
theta = random.uniform(0, 2 * np.pi)
surface_from_origin = trans_from_quat_point(
quat_from_z_rot(theta), translation + point)
#body.Enable(True)
pose = pose_from_trans(world_from_surface.dot(surface_from_origin))
if pose is None:
break
yield [(Pose(pose), )]
def is_point_on_surface(surface, point_world):
surface_from_world = np.linalg.inv(trans_from_pose(surface.pose))
point_surface = surface_from_world.dot(np.append(point_world, 1))[:3]
return is_point_in_polygon(point_surface, surface.convex_hull[::-1])
def manip_from_pose_grasp(pose, grasp):
# base_from_gripper * gripper_from_obj = base_from_obj
# base_from_obj * (gripper_from_obj)^-1 = gripper_from_obj
# arm.GetLocalToolDirection() == [ 0. 0. 1.] (z is the approach direction)
return manip_trans_from_object_trans(trans_from_pose(pose), grasp)
def manip_from_pose_grasp(pose, grasp):
return manip_trans_from_object_trans(trans_from_pose(pose), grasp)