def process_scan_room(robot):
# TODO: could do this whole thing symbolically using point head
path = get_scan_path(robot, math.pi / 6)
if path is None:
raise RuntimeError()
cspace = CSpace.robot_manipulator(robot, 'head')
h_traj = PathTrajectory(cspace, path)
h_traj.traj() # Precompute the traj
return [h_traj]
def sample_trajectory(robot, q1, q2, name, **kwargs):
cspace = CSpace.robot_manipulator(robot, name)
cspace.name = name
with robot:
cspace.set_active()
base_path = mp_birrt(robot, q1, q2, **kwargs)
if base_path is None:
return None
return PathTrajectory(cspace, base_path)
def birrt_wrapper(env, cspace, goal, restarts,iterations,smooth,self_collisions=False):
robot = cspace.body
with robot:
cspace.set_active()
start = robot.GetActiveDOFValues()
with collision_saver(env, openravepy_int.CollisionOptions.ActiveDOFs):
path = birrt(start, goal, distance_fn(robot), sample_fn(robot), extend_fn(robot), collision_fn(env, robot, check_self=self_collisions),restarts=restarts,iterations=iterations,smooth=smooth)
if path is None: return None
return PathTrajectory(cspace, path)
def motion_plan(env, cspace, goal, planner=birrt, self_collisions=False):
robot = cspace.body
with robot:
cspace.set_active()
start = robot.GetActiveDOFValues()
with collision_saver(env, openravepy_int.CollisionOptions.ActiveDOFs):
path = planner(start, goal, distance_fn(robot), sample_fn(robot), extend_fn(robot), collision_fn(env, robot, check_self=self_collisions))
if path is None: return None
return PathTrajectory(cspace, path)
def linear_arm_traj_helper(env, robot, end_config, manip_name=None):
with robot:
if manip_name is not None:
robot.SetActiveManipulator(manip_name)
robot.SetActiveDOFs(get_active_arm_indices(robot))
with collision_saver(env, openravepy_int.CollisionOptions.ActiveDOFs):
start_config = get_active_config(robot)
extend = extend_fn(robot)
collision = collision_fn(env, robot)
path = [start_config] + list(extend(start_config, end_config))
if any(collision(q) for q in path): return None
return PathTrajectory(CSpace.robot_arm(robot.GetActiveManipulator()), path)
def sample_base_trajectory(robot, q1, q2, **kwargs):
# TODO: ActiveDOFs doesn't work here for FCL?
env = robot.GetEnv()
cspace = CSpace.robot_manipulator(robot, 'base')
cspace.name = 'base'
with robot:
cspace.set_active()
collision_fn = get_collision_fn(env,
robot,
self_collisions=False,
limits=True)
base_path = birrt(q1, q2, get_distance_fn(robot),
get_sample_fn(env, robot), get_extend_fn(robot),
collision_fn, **kwargs)
if base_path is None:
return None
return PathTrajectory(cspace, base_path)
def open_gripper_trajectory(arm):
cspace = CSpace.robot_gripper(arm)
cspace.name = '{}_gripper'.format(arm.GetName()[0])
return PathTrajectory(cspace, [get_close_conf(arm), get_open_conf(arm)])
def process_move_head(robot, q1, q2):
cspace = CSpace.robot_manipulator(robot, 'head')
h_traj = PathTrajectory(cspace, [q1.value, q2.value])
h_traj.traj()
return [h_traj]