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 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_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 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]
