def collision_fn(q, diagnosis=False):
# * joint limit check
if not all_between(lower_limits, q, upper_limits):
if diagnosis:
warnings.warn('joint limit violation!', UserWarning)
cr = np.less_equal(q, lower_limits), np.less_equal(
upper_limits, q)
print('joint limit violation : {} / {}'.format(cr[0], cr[1]))
for i, (cr_l, cr_u) in enumerate(zip(cr[0], cr[1])):
if cr_l:
print('J{}: {} < lower limit {}'.format(
i, q[i], lower_limits[i]))
if cr_u:
print('J{}: {} > upper limit {}'.format(
i, q[i], upper_limits[i]))
return True
# dummy joints
if all(joint < -1 for joint in joints):
cube_pos = q[:3]
cube_ori = q[3:]
cube_quat = p.getQuaternionFromEuler(cube_ori)
set_pose(body, (cube_pos, cube_quat))
else:
# * set body & attachment positions
set_joint_positions(body, joints, q)
for attachment in attachments:
attachment.assign()
# * self-collision link check
for link1, link2 in self_check_link_pairs:
if pairwise_link_collision(body, link1, body, link2, **kwargs):
if diagnosis:
warnings.warn(
'moving body link - moving body link collision!',
UserWarning)
cr = pairwise_link_collision_info(body, link1, body, link2)
draw_collision_diagnosis(cr)
return True
# * self link - attachment check
for body_check_links, attached_body in attach_check_pairs:
if any_link_pair_collision(body, body_check_links, attached_body,
**kwargs):
if diagnosis:
warnings.warn('moving body link - attachement collision!',
UserWarning)
cr = any_link_pair_collision_info(body, body_check_links,
attached_body, **kwargs)
draw_collision_diagnosis(cr)
return True
# * body - body check
for (body1, link1), (body2, link2) in check_body_link_pairs:
if pairwise_link_collision(body1, link1, body2, link2, **kwargs):
if diagnosis:
warnings.warn('moving body - body collision!', UserWarning)
cr = pairwise_link_collision_info(body1, link1, body2,
link2)
draw_collision_diagnosis(cr)
return True
return False
def load_model(rel_path, pose=None, **kwargs):
from pybullet_planning.interfaces.env_manager.simulation import get_model_path, add_data_path, load_pybullet
# TODO: error with loadURDF when loading MESH visual and CYLINDER collision
abs_path = get_model_path(rel_path)
add_data_path()
#with LockRenderer():
body = load_pybullet(abs_path, **kwargs)
if pose is not None:
set_pose(body, pose)
return body
def collision_fn(pose, diagnosis=diagnosis):
set_pose(body, pose)
# * body - body check
for (body1, link1), (body2, link2) in check_body_link_pairs:
if pairwise_link_collision(body1, link1, body2, link2, **kwargs):
if diagnosis:
warnings.warn('moving body - body collision!', UserWarning)
cr = pairwise_link_collision_info(body1, link1, body2,
link2)
draw_collision_diagnosis(cr)
return True
return False
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_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 assign(self):
parent_link_pose = get_link_pose(self.parent, self.parent_link)
child_pose = body_from_end_effector(parent_link_pose, self.grasp_pose)
set_pose(self.child, child_pose)
return child_pose
def set_quat(body, quat):
set_pose(body, (get_point(body), quat))
def set_point(body, point):
set_pose(body, (point, get_quat(body)))
