def is_placed_on_aabb(body,
bottom_aabb,
above_epsilon=1e-2,
below_epsilon=0.0):
assert (0 <= above_epsilon) and (0 <= below_epsilon)
top_aabb = get_aabb(body) # TODO: approximate_as_prism
top_z_min = top_aabb[0][2]
bottom_z_max = bottom_aabb[1][2]
return ((bottom_z_max - below_epsilon) <= top_z_min <= (bottom_z_max + above_epsilon)) and \
(aabb_contains_aabb(aabb2d_from_aabb(top_aabb), aabb2d_from_aabb(bottom_aabb)))
def add_body_name(body, name=None, **kwargs):
from pybullet_planning.interfaces.env_manager.pose_transformation import set_pose
from pybullet_planning.interfaces.env_manager.savers import PoseSaver
from pybullet_planning.interfaces.robots.body import get_name
if name is None:
name = get_name(body)
with PoseSaver(body):
set_pose(body, unit_pose())
lower, upper = get_aabb(body)
#position = (0, 0, upper[2])
position = upper
return add_text(name, position=position, parent=body, **kwargs) # removeUserDebugItem
def sample_placement(top_body, bottom_body, bottom_link=None, **kwargs):
bottom_aabb = get_aabb(bottom_body, link=bottom_link)
return sample_placement_on_aabb(top_body, bottom_aabb, **kwargs)
def is_center_stable(body, surface, **kwargs):
return is_center_on_aabb(body, get_aabb(surface), **kwargs)
def is_placement(body, surface, **kwargs):
return is_placed_on_aabb(body, get_aabb(surface), **kwargs)
def stable_z(body, surface, surface_link=None):
return stable_z_on_aabb(body, get_aabb(surface, link=surface_link))