def create_collision_shape(geometry, pose=unit_pose()):
point, quat = pose
collision_args = {
'collisionFramePosition': point,
'collisionFrameOrientation': quat,
'physicsClientId': CLIENT,
}
collision_args.update(geometry)
if 'length' in collision_args:
# TODO: pybullet bug visual => length, collision => height
collision_args['height'] = collision_args['length']
del collision_args['length']
return p.createCollisionShape(**collision_args)
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 create_visual_shape(geometry, pose=unit_pose(), color=RED, specular=None):
if (color is None): # or not has_gui():
return NULL_ID
point, quat = pose
visual_args = {
'rgbaColor': color,
'visualFramePosition': point,
'visualFrameOrientation': quat,
'physicsClientId': CLIENT,
}
visual_args.update(geometry)
if specular is not None:
visual_args['specularColor'] = specular
return p.createVisualShape(**visual_args)
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 sample_placement_on_aabb(top_body,
bottom_aabb,
top_pose=unit_pose(),
percent=1.0,
max_attempts=50,
epsilon=1e-3):
# TODO: transform into the coordinate system of the bottom
# TODO: maybe I should instead just require that already in correct frame
for _ in range(max_attempts):
theta = np.random.uniform(*CIRCULAR_LIMITS)
rotation = Euler(yaw=theta)
set_pose(top_body, multiply(Pose(euler=rotation), top_pose))
center, extent = get_center_extent(top_body)
lower = (np.array(bottom_aabb[0]) + percent * extent / 2)[:2]
upper = (np.array(bottom_aabb[1]) - percent * extent / 2)[:2]
if np.less(upper, lower).any():
continue
x, y = np.random.uniform(lower, upper)
z = (bottom_aabb[1] + extent / 2.)[2] + epsilon
point = np.array([x, y, z]) + (get_point(top_body) - center)
pose = multiply(Pose(point, rotation), top_pose)
set_pose(top_body, pose)
return pose
return None
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 create_shape(geometry, pose=unit_pose(), collision=True, **kwargs):
collision_id = create_collision_shape(geometry, pose=pose) if collision else NULL_ID
visual_id = create_visual_shape(geometry, pose=pose, **kwargs)
return collision_id, visual_id