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]