def is_mesh_on_surface(polygon,
world_from_surface,
mesh,
world_from_mesh,
epsilon=1e-2):
surface_from_mesh = multiply(invert(world_from_surface), world_from_mesh)
points_surface = apply_affine(surface_from_mesh, mesh.vertices)
min_z = np.min(points_surface[:, 2])
return (abs(min_z) < epsilon) and \
all(is_point_in_polygon(p, polygon) for p in points_surface)
def approximate_as_prism(body, body_pose=unit_pose(), **kwargs):
"""get the AABB bounding box of a body
Note: the generated AABB is not truly axis-aligned, bounding box under world axis x, y
Parameters
----------
body : int
pb body's index
body_pose : [type], optional
[description], by default unit_pose()
Returns
-------
tuple of (Point, float)
bounding box center and extent length
"""
from pybullet_planning.interfaces.geometry.bounding_box import aabb_from_points, get_aabb_center, get_aabb_extent
# TODO: make it just orientation
vertices = apply_affine(body_pose, vertices_from_rigid(body, **kwargs))
aabb = aabb_from_points(vertices)
return get_aabb_center(aabb), get_aabb_extent(aabb)
def vertices_from_data(data):
from pybullet_planning.interfaces.env_manager.pose_transformation import apply_affine
from pybullet_planning.interfaces.env_manager.shape_creation import get_data_type, get_data_extents, get_data_radius, get_data_height, \
get_data_filename, get_data_scale, get_collision_data, get_data_pose
from pybullet_planning.interfaces.env_manager import get_model_info
from pybullet_planning.interfaces.geometry.bounding_box import AABB, get_aabb_vertices
geometry_type = get_data_type(data)
#if geometry_type == p.GEOM_SPHERE:
# parameters = [get_data_radius(data)]
if geometry_type == p.GEOM_BOX:
extents = np.array(get_data_extents(data))
aabb = AABB(-extents/2., +extents/2.)
vertices = get_aabb_vertices(aabb)
elif geometry_type in (p.GEOM_CYLINDER, p.GEOM_CAPSULE):
# TODO: p.URDF_USE_IMPLICIT_CYLINDER
radius, height = get_data_radius(data), get_data_height(data)
extents = np.array([2*radius, 2*radius, height])
aabb = AABB(-extents/2., +extents/2.)
vertices = get_aabb_vertices(aabb)
elif geometry_type == p.GEOM_SPHERE:
radius = get_data_radius(data)
half_extents = radius*np.ones(3)
aabb = AABB(-half_extents, +half_extents)
vertices = get_aabb_vertices(aabb)
elif geometry_type == p.GEOM_MESH:
from pybullet_planning.interfaces.geometry.mesh import read_obj
filename, scale = get_data_filename(data), get_data_scale(data)
if filename == UNKNOWN_FILE:
raise RuntimeError(filename)
mesh = read_obj(filename, decompose=False)
vertices = [scale*np.array(vertex) for vertex in mesh.vertices]
# TODO: could compute AABB here for improved speed at the cost of being conservative
#elif geometry_type == p.GEOM_PLANE:
# parameters = [get_data_extents(data)]
else:
raise NotImplementedError(geometry_type)
return apply_affine(get_data_pose(data), vertices)
def tform_mesh(affine, mesh):
from pybullet_planning.interfaces.env_manager.pose_transformation import apply_affine
return Mesh(apply_affine(affine, mesh.vertices), mesh.faces)
def approximate_as_prism(body, body_pose=unit_pose(), **kwargs):
from pybullet_planning.interfaces.geometry.bounding_box import aabb_from_points, get_aabb_center, get_aabb_extent
# TODO: make it just orientation
vertices = apply_affine(body_pose, vertices_from_rigid(body, **kwargs))
aabb = aabb_from_points(vertices)
return get_aabb_center(aabb), get_aabb_extent(aabb)
def is_point_on_surface(polygon, world_from_surface, point_world):
[point_surface] = apply_affine(invert(world_from_surface), [point_world])
return is_point_in_polygon(point_surface, polygon[::-1])
