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