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 link_pairs_collision_info(body1, links1, body2, links2=None, **kwargs):
"""[summary]
Note: for now, this is simply a copy of the original `any_link_pair_collision` function
to return closest point query info. This function has duplicated computation and should
not be used in a planning process.
Parameters
----------
body1 : [type]
[description]
links1 : [type]
[description]
body2 : [type]
[description]
links2 : [type], optional
[description], by default None
Returns
-------
[type]
[description]
"""
if links2 is None:
links2 = get_all_links(body2)
for link1, link2 in product(links1, links2):
if (body1 == body2) and (link1 == link2):
continue
if pairwise_link_collision(body1, link1, body2, link2, **kwargs):
return pairwise_link_collision_info(body1, link1, body2, link2,
**kwargs)
return False
def any_link_pair_collision(body1, links1, body2, links2=None, **kwargs):
"""check collision between two bodies' links
TODO: Caelan : this likely isn't needed anymore
Parameters
----------
body1 : [type]
[description]
links1 : [type]
[description]
body2 : [type]
[description]
links2 : [type], optional
[description], by default None
Returns
-------
[type]
[description]
"""
if links1 is None:
links1 = get_all_links(body1)
if links2 is None:
links2 = get_all_links(body2)
for link1, link2 in product(links1, links2):
if (body1 == body2) and (link1 == link2):
continue
if pairwise_link_collision(body1, link1, body2, link2, **kwargs):
return True
return False
def link_pairs_collision(body1, links1, body2, links2=None, **kwargs):
if links2 is None:
links2 = get_all_links(body2)
for link1, link2 in product(links1, links2):
if (body1 == body2) and (link1 == link2):
continue
if pairwise_link_collision(body1, link1, body2, link2, **kwargs):
return True
return False
def collision_fn(pose, diagnosis=False):
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 collision_fn(q):
if not all_between(lower_limits, q, upper_limits):
#print('Joint limits violated')
return True
set_joint_positions(body, joints, q)
for attachment in attachments:
attachment.assign()
for link1, link2 in check_link_pairs:
# Self-collisions should not have the max_distance parameter
if pairwise_link_collision(body, link1, body,
link2): #, **kwargs):
#print(get_body_name(body), get_link_name(body, link1), get_link_name(body, link2))
return True
for body1, body2 in check_body_pairs:
if pairwise_collision(body1, body2, **kwargs):
#print(get_body_name(body1), get_body_name(body2))
return True
return False