def custom_base_iterator(oracle, manip_iterator):
if not hasattr(oracle, 'ir_database'):
oracle.ir_database = get_custom_ir(oracle.robot) # TODO - allow multiple IR databases
default_trans = get_trans(oracle.robot)
for manip_trans in cycle(manip_iterator):
yield ir_base_trans(oracle, manip_trans, choice(oracle.ir_database), default_trans), manip_trans
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 plan_base_traj(oracle, start_config, end_config, holding=None, obstacle_poses={}):
with oracle.state_saver():
oracle.set_all_object_poses(obstacle_poses)
oracle.set_robot_config(end_config)
base_trans = get_trans(oracle.robot)
oracle.set_robot_config(start_config)
open_gripper(oracle)
if holding is not None:
oracle.set_pose(holding.object_name, object_pose_from_robot_config(oracle, start_config, holding.grasp))
grab(oracle, holding.object_name)
traj = cspace_traj(oracle, CSpace.robot_base(oracle.robot), base_values_from_trans(base_trans))
if holding is not None:
release(oracle, holding.object_name)
return traj
def manip_point(q):
robot = get_env().GetRobots()[0]
with robot:
robot.SetActiveDOFValues(q.value)
return point_from_trans(get_trans(robot.GetActiveManipulator()))
