def display_custom_ir(oracle, manip_trans):
if not hasattr(oracle, 'ir_database'):
oracle.ir_database = get_custom_ir(oracle.robot)
handles = []
default_trans = get_trans(oracle.robot)
for transform in oracle.ir_database:
base_trans = ir_base_trans(oracle, manip_trans, transform, default_trans)
handles.append(draw_point(oracle.env, point_from_trans(base_trans)))
return handles
def fn(self, edge, object_name, object_pose, holding):
oracle = self.oracle
preprocess_edge(oracle, edge)
all_trans = [object_trans_from_manip_trans(q.manip_trans, holding.grasp.grasp_trans) for q in edge.configs()] # TODO - cache these transforms
distance = oracle.get_radius2(object_name) + oracle.get_radius2(holding.object_name)
nearby_trans = [trans for trans in all_trans if length2(point_from_trans(trans) - point_from_pose(object_pose.value)) <= distance]
if len(nearby_trans) == 0: return False
with oracle.body_saver(object_name):
oracle.set_pose(object_name, object_pose)
with oracle.body_saver(holding.object_name):
for trans in nearby_trans:
set_trans(oracle.get_body(holding.object_name), trans)
if collision(oracle, object_name, holding.object_name):
return True
return False
def manip_point(q):
robot = get_env().GetRobots()[0]
with robot:
robot.SetActiveDOFValues(q.value)
return point_from_trans(get_trans(robot.GetActiveManipulator()))
