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 any_link_pair_collision_info(body1, links1, body2, links2=None, **kwargs):
"""check collision between two bodies' links and return detailed information
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 : int
[description]
links1 : list of int
[description]
body2 : int
[description]
links2 : list of int, 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 pairwise_link_collision_info(body1, link1, body2, link2,
**kwargs)
return False
def any_link_pair_collision(body1, links1, body2, links2=None, **kwargs):
# TODO: this likely isn't needed anymore
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 flatten_links(body, links=None):
"""util fn to get a list (body, link)
TODO: [Q] what's the use case for this one?
Parameters
----------
body : int
[description]
links : list of int, optional
given links, by default None
Returns
-------
set of (body, link)
[description]
"""
if links is None:
links = get_all_links(body)
return {(body, frozenset([link])) for link in links}
def expand_links(body):
"""expand all links of a body
TODO: [REFACTOR] move to body or link modules?
Parameters
----------
body : int
[description]
Returns
-------
body : int
[description]
links : list of int
[description]
"""
body, links = body if isinstance(body, tuple) else (body, None)
if links is None:
links = get_all_links(body)
return body, links
def expand_links(body):
body, links = body if isinstance(body, tuple) else (body, None)
if links is None:
links = get_all_links(body)
return body, links
def flatten_links(body, links=None):
if links is None:
links = get_all_links(body)
return {(body, frozenset([link])) for link in links}