Beispiel #1
0
    def _reset_start_goal_manually(self, start_pos, goal_pos):

        p = self.params.reset_params.start_config

        start_112 = np.array([[start_pos]], dtype=np.float32)
        goal_112 = np.array([[goal_pos]], dtype=np.float32)

        heading_111 = np.zeros((1, 1, 1))
        speed_111 = np.zeros((1, 1, 1))
        ang_speed_111 = np.zeros((1, 1, 1))

        # Initialize the start configuration
        self.start_config = SystemConfig(dt=p.dt, n=1, k=1,
                                         position_nk2=start_112,
                                         heading_nk1=heading_111,
                                         speed_nk1=speed_111,
                                         angular_speed_nk1=ang_speed_111)

        # The system dynamics may need the current starting position for
        # coordinate transforms (i.e. realistic simulation)
        self.system_dynamics.reset_start_state(self.start_config)

        # Initialize the goal configuration
        self.goal_config = SystemConfig(dt=p.dt, n=1, k=1,
                                        position_nk2=goal_112)

        return False
def test_lqr_feedback_coordinate_transform():

    p = create_params()
    n, k = p.n, p.k
    dubins = p.system_dynamics_params.system(p.dt, params=p.simulation_params)

    # # Robot starts from (0, 0, 0)
    # # and does a small spiral
    start_pos_n13 = tf.constant(np.array([[[0.0, 0.0, 0.0]]], dtype=np.float32))
    speed_nk1 = np.ones((n, k - 1, 1), dtype=np.float32) * 2.0
    angular_speed_nk1 = np.linspace(1.5, 1.3, k - 1, dtype=np.float32)[None, :, None]
    u_nk2 = tf.constant(np.concatenate([speed_nk1, angular_speed_nk1], axis=2))
    traj_ref_egocentric = dubins.simulate_T(start_pos_n13, u_nk2, k)

    cost_fn = QuadraticRegulatorRef(traj_ref_egocentric, dubins, p)

    lqr_solver = LQRSolver(T=k - 1, dynamics=dubins, cost=cost_fn)

    start_config0 = SystemConfig(dt=p.dt, n=p.n, k=1,
                                 position_nk2=start_pos_n13[:, :, :2],
                                 heading_nk1=start_pos_n13[:, :, 2:3])
    lqr_res = lqr_solver.lqr(start_config0, traj_ref_egocentric, verbose=False)
    traj_lqr_egocentric = lqr_res['trajectory_opt']
    K_array_egocentric = lqr_res['K_opt_nkfd']
    k_array = lqr_res['k_opt_nkf1']

    # The origin of the egocentric frame in world coordinates
    start_pos_n13 = tf.constant(np.array([[[1.0, 1.0, np.pi / 2.]]], dtype=np.float32))
    ref_config = SystemConfig(dt=p.dt, n=p.n, k=1,
                              position_nk2=start_pos_n13[:, :, :2],
                              heading_nk1=start_pos_n13[:, :, 2:3])

    # Convert to world coordinates
    traj_ref_world = dubins.to_world_coordinates(ref_config, traj_ref_egocentric, mode='new')
    traj_lqr_world = dubins.to_world_coordinates(ref_config, traj_lqr_egocentric, mode='new')
    K_array_world = dubins.convert_K_to_world_coordinates(ref_config, K_array_egocentric, mode='new')

    # Apply K_array_world to the system from ref_config
    traj_test_world = lqr_solver.apply_control(ref_config, traj_ref_world,
                                               k_array, K_array_world)

    # Check that the LQR Trajectory computed using K_array_egocentric
    # then transformed to world (traj_lqr_world) matches the
    # LQR Trajectory computed directly using K_array_world
    assert((tf.norm(traj_lqr_world.position_nk2() - traj_test_world.position_nk2(), axis=2).numpy() < 1e-4).all())
    assert((tf.norm(angle_normalize(traj_lqr_world.heading_nk1() - traj_test_world.heading_nk1()), axis=2).numpy() < 1e-4).all())
    assert((tf.norm(traj_lqr_world.speed_nk1() - traj_test_world.speed_nk1(), axis=2).numpy() < 1e-4).all())
    assert((tf.norm(traj_lqr_world.angular_speed_nk1() - traj_test_world.angular_speed_nk1(), axis=2).numpy() < 1e-4).all())
def visualize_coordinate_transform():
    """Visual sanity check that coordinate transforms
    are working. """
    fig, _, axs = utils.subplot2(plt, (2, 2), (8, 8), (.4, .4))
    axs = axs[::-1]

    n, k = 1, 30
    dt = .1
    dubins_car = DubinsV1(dt=dt)

    traj_egocentric = Trajectory(dt=dt, n=n, k=k, variable=True)
    traj_world = Trajectory(dt=dt, n=n, k=k, variable=True)

    # Form a trajectory in global frame
    # convert to egocentric and back
    start_pos_global_n12 = tf.constant([[[1.0, 1.0]]], dtype=tf.float32)
    start_heading_global_n11 = tf.constant([[[np.pi / 2.]]], dtype=tf.float32)
    start_config_global = SystemConfig(dt=dt, n=n, k=1, position_nk2=start_pos_global_n12,
                                       heading_nk1=start_heading_global_n11)

    start_n13 = tf.concat([start_pos_global_n12, start_heading_global_n11], axis=2)
    u_n12 = np.array([[[.01, .1]]], dtype=np.float32)
    u_nk2 = tf.constant(np.broadcast_to(u_n12, (n, k, 2)), dtype=tf.float32)
    trajectory_world = dubins_car.simulate_T(start_n13, u_nk2, T=k - 1)
    trajectory_world.render([axs[0]], batch_idx=0, freq=4, name='World')

    # Convert to egocentric
    dubins_car.to_egocentric_coordinates(start_config_global, trajectory_world, traj_egocentric)
    traj_egocentric.render([axs[1]], batch_idx=0, freq=4, name='Egocentric')

    dubins_car.to_world_coordinates(start_config_global, traj_egocentric, traj_world)
    traj_world.render([axs[2]], batch_idx=0, freq=4, name='World #2')
    plt.savefig('./tmp/coordinate_transform.png', bbox_inches='tight')
Beispiel #4
0
    def _set_instance_variables(self, data):
        """Set the control pipelines instance variables from a data dictionary."""
        self.start_configs = data['start_configs']
        self.waypt_configs = data['waypt_configs']
        self.start_speeds = data['start_speeds']
        self.spline_trajectories = data['spline_trajectories']
        self.horizons = data['horizons']
        self.lqr_trajectories = data['lqr_trajectories']
        self.K_nkfd = data['K_nkfd']
        self.k_nkf1 = data['k_nkf1']

        # Initialize variable tensor for waypoints in world coordinates
        dt = self.params.system_dynamics_params.dt
        self.waypt_configs_world = [
            SystemConfig(dt=dt,
                         n=config.n,
                         k=1,
                         variable=True,
                         track_trajectory_acceleration=self.params.
                         track_trajectory_acceleration)
            for config in data['start_configs']
        ]

        self.instance_variables_loaded = True

        if self.params.verbose:
            N = self.params.waypoint_params.n
            for v0, start_config in zip(self.start_velocities,
                                        self.start_configs):
                print('Velocity: {:.3f}, {:.3f}% of goals kept({:d}).'.format(
                    v0, 100. * start_config.n / N, start_config.n))
Beispiel #5
0
    def __init__(self, obj_fn, params):
        self.obj_fn = obj_fn
        self.params = params.planner.parse_params(params)

        self.opt_waypt = SystemConfig(dt=params.dt, n=1, k=1, variable=True)
        self.opt_traj = Trajectory(dt=params.dt,
                                   n=1,
                                   k=params.planning_horizon,
                                   variable=True)
        self.control_pipeline = self._init_control_pipeline()
Beispiel #6
0
def test_coordinate_transform():
    n, k = 1, 30
    dt = .1
    p = create_params()
    dubins_car = DubinsV1(dt=dt, params=p.system_dynamics_params)
    ref_config = dubins_car.init_egocentric_robot_config(dt=dt, n=n)

    pos_nk2 = np.ones((n, k, 2), dtype=np.float32) * np.random.rand()
    traj_global = Trajectory(dt=dt, n=n, k=k, position_nk2=pos_nk2)
    traj_egocentric = Trajectory(dt=dt, n=n, k=k, variable=True)
    traj_global_new = Trajectory(dt=dt, n=n, k=k, variable=True)

    dubins_car.to_egocentric_coordinates(ref_config, traj_global,
                                         traj_egocentric)

    # Test 0 transform
    assert ((pos_nk2 == traj_egocentric.position_nk2()).all())

    ref_config_pos_112 = np.array([[[5.0, 5.0]]], dtype=np.float32)
    ref_config_pos_n12 = np.repeat(ref_config_pos_112, repeats=n, axis=0)
    ref_config = SystemConfig(dt=dt, n=n, k=1, position_nk2=ref_config_pos_n12)
    traj_egocentric = dubins_car.to_egocentric_coordinates(
        ref_config, traj_global, traj_egocentric)
    # Test translation
    assert ((pos_nk2 - 5.0 == traj_egocentric.position_nk2()).all())

    ref_config_heading_111 = np.array([[[3. * np.pi / 4.]]], dtype=np.float32)
    ref_config_heading_nk1 = np.repeat(ref_config_heading_111,
                                       repeats=n,
                                       axis=0)
    ref_config = SystemConfig(dt=dt,
                              n=n,
                              k=1,
                              position_nk2=ref_config_pos_n12,
                              heading_nk1=ref_config_heading_nk1)

    traj_egocentric = dubins_car.to_egocentric_coordinates(
        ref_config, traj_global, traj_egocentric)
    traj_global_new = dubins_car.to_world_coordinates(ref_config,
                                                      traj_egocentric,
                                                      traj_global_new)
    assert (np.allclose(traj_global.position_nk2(),
                        traj_global_new.position_nk2()))
    def optimize(self, start_config):
        """ Optimize the objective over a trajectory
        starting from start_config.
        """
        p = self.params

        model = p.model

        raw_data = self._raw_data(start_config)
        processed_data = model.create_nn_inputs_and_outputs(raw_data)

        # Predict the NN output
        nn_output_113 = model.predict_nn_output_with_postprocessing(
            processed_data['inputs'], is_training=False)[:, None]

        # Transform to World Coordinates
        waypoint_ego_config = SystemConfig(
            dt=self.params.dt,
            n=1,
            k=1,
            position_nk2=nn_output_113[:, :, :2],
            heading_nk1=nn_output_113[:, :, 2:3])
        self.params.system_dynamics.to_world_coordinates(
            start_config, waypoint_ego_config, self.waypoint_world_config)

        # Evaluate the objective and retrieve Control Pipeline data
        obj_vals, data = self.eval_objective(start_config,
                                             self.waypoint_world_config)

        # The batch dimension is length 1 since there is only one waypoint
        min_idx = 0
        min_cost = obj_vals[min_idx]

        waypts, horizons_s, trajectories_lqr, trajectories_spline, controllers = data

        self.opt_waypt.assign_from_config_batch_idx(waypts, min_idx)
        self.opt_traj.assign_from_trajectory_batch_idx(trajectories_lqr,
                                                       min_idx)

        # Convert horizon in seconds to horizon in # of steps
        min_horizon = int(
            tf.ceil(horizons_s[min_idx, 0] / self.params.dt).numpy())

        data = {
            'system_config': SystemConfig.copy(start_config),
            'waypoint_config': SystemConfig.copy(self.opt_waypt),
            'trajectory': Trajectory.copy(self.opt_traj),
            'spline_trajectory': Trajectory.copy(trajectories_spline),
            'planning_horizon': min_horizon,
            'K_nkfd': controllers['K_nkfd'][min_idx:min_idx + 1],
            'k_nkf1': controllers['k_nkf1'][min_idx:min_idx + 1],
            'img_nmkd': raw_data['img_nmkd']
        }

        return data
Beispiel #8
0
 def init_egocentric_robot_config(dt, n, v=0.0, w=0.0, dtype=tf.float32):
     """ A utility function initializing the robot at
     x=0, y=0, theta=0, v=v, w=w, a=0, alpha=0."""
     k = 1
     position_nk2 = tf.zeros((n, k, 2), dtype=dtype)
     heading_nk1 = tf.zeros((n, k, 1), dtype=dtype)
     speed_nk1 = v*tf.ones((n, k, 1), dtype=dtype)
     angular_speed_nk1 = w*tf.ones((n, k, 1), dtype=dtype)
     return SystemConfig(dt=dt, n=n, k=k, position_nk2=position_nk2,
                         heading_nk1=heading_nk1, speed_nk1=speed_nk1,
                         angular_speed_nk1=angular_speed_nk1, variable=False)
Beispiel #9
0
def generate_config_from_pos_3(pos_3, dt=0.1, v=0, w=0):
    pos_n11 = np.array([[[pos_3[0], pos_3[1]]]], dtype=np.float32)
    heading_n11 = np.array([[[pos_3[2]]]], dtype=np.float32)
    speed_nk1 = np.ones((1, 1, 1), dtype=np.float32) * v
    angular_speed_nk1 = np.ones((1, 1, 1), dtype=np.float32) * w
    return SystemConfig(dt,
                        1,
                        1,
                        position_nk2=pos_n11,
                        heading_nk1=heading_n11,
                        speed_nk1=speed_nk1,
                        angular_speed_nk1=angular_speed_nk1,
                        variable=False)
Beispiel #10
0
    def _sample_egocentric_waypoints(self, vf=0.):
        """ Uniformly samples an egocentric waypoint grid over which to plan trajectories."""
        p = self.params.waypoint_params

        waypoints_egocentric = self.waypoint_grid.sample_egocentric_waypoints(
            vf=vf)
        waypoints_egocentric = self._ensure_waypoints_valid(
            waypoints_egocentric)
        wx_n11, wy_n11, wtheta_n11, wv_n11, ww_n11 = waypoints_egocentric
        waypt_pos_n12 = np.concatenate([wx_n11, wy_n11], axis=2)
        waypoint_egocentric_config = SystemConfig(dt=self.params.dt,
                                                  n=self.waypoint_grid.n,
                                                  k=1,
                                                  position_nk2=waypt_pos_n12,
                                                  speed_nk1=wv_n11,
                                                  heading_nk1=wtheta_n11,
                                                  angular_speed_nk1=ww_n11,
                                                  variable=True)
        return waypoint_egocentric_config
Beispiel #11
0
def test_piecewise_spline():
    #Testing with splines
    np.random.seed(seed=1)
    n = 5  # Batch size (unused now)
    dt = .01  # delta-t: time intervals
    k = 100  # Number of time-steps (should be 1/dt)

    # States represents each individual tangent point that the spline will pass through
    # states[0] = initial state, and states[len(states) - 1] = terminal state
    states = [
        (8, 12, np.pi / 2.0, 0.5),  # Start State (x, y, theta, vel)
        (15, 14.5, np.pi / 2.0, 0.8),  # Middle State (x, y, theta, vel)
        (10, 15, -np.pi / 2.0, 1),  # Middle State (x, y, theta, vel)
        (18, 16.5, np.pi / 2.0, 0.2)  # Goal State (x, y, theta, vel)
    ]

    p = DotMap(spline_params=DotMap(epsilon=1e-5))
    ts_nk = tf.tile(tf.linspace(0., dt * k, k)[None], [n, 1])
    splines = []
    prev_config = None
    next_config = None
    # Generate all two-point splines from the states
    for s in states:
        spline_traj = Spline3rdOrder(dt=dt, k=k, n=n, params=p.spline_params)
        # Keep track of old trajectory
        if (next_config is not None):
            # Rewrite the previous state config with the 'old' next one
            prev_config = next_config
        # Generate position
        s_posx_nk1 = tf.ones(
            (n, 1, 1), dtype=tf.float32) * s[0]  # X position matrix
        s_posy_nk1 = tf.ones(
            (n, 1, 1), dtype=tf.float32) * s[1]  # Y position matrix
        s_pos_nk2 = tf.concat([s_posx_nk1, s_posy_nk1],
                              axis=2)  # combined matrix of (X,Y)
        # Generate speed and heading
        heading_nk1 = tf.ones(
            (n, 1, 1), dtype=tf.float32) * s[2]  # Theta angle matrix
        speed_nk1 = tf.ones((n, 1, 1), dtype=tf.float32) * s[3]  # Speed matrix
        next_config = SystemConfig(dt,
                                   n,
                                   1,
                                   position_nk2=s_pos_nk2,
                                   speed_nk1=speed_nk1,
                                   heading_nk1=heading_nk1,
                                   variable=False)
        # Append to the trajectory if a new trajectory can be constructed
        # Note that any spline needs a 'previous' and 'next' state
        if (prev_config is not None):
            spline_traj.fit(prev_config, next_config, factors=None)
            spline_traj.eval_spline(ts_nk, calculate_speeds=True)
            splines.append(spline_traj)

    # Loop through all calculated splines to combine them together into a singular one
    final_spline = None
    for i, s in enumerate(splines):
        if (final_spline is not None):
            final_spline.append_along_time_axis(s)
        if (i == 0):
            # For first spline
            final_spline = s

    fig = plt.figure()
    fig, ax = plt.subplots(4, 1, figsize=(5, 15), squeeze=False)
    final_spline.render_multi(ax,
                              freq=4,
                              plot_heading=True,
                              plot_velocity=True,
                              label_start_and_end=True)
    # trajectory.render(ax, freq=1, plot_heading=True, plot_velocity=True, label_start_and_end=True)
    fig.savefig('./tests/spline/test_piecewise_spline.png',
                bbox_inches='tight',
                pad_inches=0)
def test_planner():
    # Create parameters

    # Create planner parameters
    planner_params = create_planner_params()
    sim_params = create_sim_params()
    # Define the objective
    # objective = ObstacleAvoidance(params=p.avoid_obstacle_objective,
    #                               obstacle_map=obstacle_map)
    sim = SBPDSimulator(sim_params)
    splanner = SamplingPlanner(sim, planner_params)

    # Spline trajectory params
    n = 1
    dt = 0.1

    # Goal states and initial speeds
    goal_pos_n11 = tf.constant([[[8., 12.5]]
                                ])  # Goal position (must be 1x1x2 array)
    goal_heading_n11 = tf.constant([[[np.pi / 2.]]])
    # Start states and initial speeds
    start_pos_n11 = tf.constant([[[22., 16.5]]
                                 ])  # Goal position (must be 1x1x2 array)
    start_heading_n11 = tf.constant([[[-np.pi]]])
    start_speed_nk1 = tf.ones((1, 1, 1), dtype=tf.float32) * 0.0
    # Define start and goal configurations
    # start_config = SystemConfig(dt, n, 1, speed_nk1=start_speed_nk1, variable=False)
    start_config = SystemConfig(dt,
                                n,
                                k=1,
                                position_nk2=start_pos_n11,
                                heading_nk1=start_heading_n11,
                                speed_nk1=start_speed_nk1,
                                variable=False)
    goal_config = SystemConfig(dt,
                               n,
                               k=1,
                               position_nk2=goal_pos_n11,
                               heading_nk1=goal_heading_n11,
                               variable=True)
    #  waypts, horizons, trajectories_lqr, trajectories_spline, controllers = controller.plan(start_config, goal_config)
    splanner.simulator.reset_with_start_and_goal(start_config, goal_config)
    splanner.optimize(start_config)
    splanner.simulator.simulate()
    # Visualization
    fig = plt.figure()

    # obstacle_map.render(ax)
    # ax.plot(pos_nk2[0, :, 0].numpy(), pos_nk2[0, :, 1].numpy(), 'r.')
    # ax.plot(trajectory._position_nk2[0, :, 0],trajectory._position_nk2[0, :, 1], 'r-')
    ax = fig.add_subplot(1, 3, 1)
    splanner.simulator.render(ax)
    ax = fig.add_subplot(1, 3, 2)
    splanner.simulator.render(ax, zoom=4)
    #ax.plot(objective[0, 0], objective[0, 1], 'k*')
    # ax.set_title('obstacle map')
    ax = fig.add_subplot(1, 3, 3)
    splanner.simulator.vehicle_trajectory.render(ax, plot_quiver=True)
    splanner.simulator._render_waypoints(ax,
                                         plot_quiver=True,
                                         text_offset=(-1.5, 0.1))

    # ax = fig.add_subplot(1,3,3)
    # ax.plot(opt_traj._position_nk2[0, :, 0],opt_traj._position_nk2[0, :, 1], 'r-')
    # ax.set_title('opt_traj')
    # Plotting the "distance map"
    # ax = fig.add_subplot(1,2,2)
    # ax.imshow(distance_map, origin='lower')
    # ax.set_title('distance map')

    fig.savefig('./tests/planner/test_planner.png',
                bbox_inches='tight',
                pad_inches=0)
 def __init__(self, simulator, params):
     super(NNWaypointPlanner, self).__init__(simulator, params)
     self.waypoint_world_config = SystemConfig(dt=self.params.dt, n=1, k=1)
Beispiel #14
0
def test_spline_3rd_order(visualize=False):
    np.random.seed(seed=1)
    n = 5
    dt = .01
    k = 100

    target_state = np.random.uniform(-np.pi, np.pi, 3)
    v0 = np.random.uniform(0., 0.5, 1)[0]  # Initial speed
    vf = 0.

    # Initial SystemConfig is [0, 0, 0, v0, 0]
    start_speed_nk1 = tf.ones((n, 1, 1), dtype=tf.float32) * v0

    goal_posx_nk1 = tf.ones((n, 1, 1), dtype=tf.float32) * target_state[0]
    goal_posy_nk1 = tf.ones((n, 1, 1), dtype=tf.float32) * target_state[1]
    goal_pos_nk2 = tf.concat([goal_posx_nk1, goal_posy_nk1], axis=2)
    goal_heading_nk1 = tf.ones((n, 1, 1), dtype=tf.float32) * target_state[2]
    goal_speed_nk1 = tf.ones((n, 1, 1), dtype=tf.float32) * vf

    start_config = SystemConfig(dt,
                                n,
                                1,
                                speed_nk1=start_speed_nk1,
                                variable=False)
    goal_config = SystemConfig(dt,
                               n,
                               1,
                               position_nk2=goal_pos_nk2,
                               speed_nk1=goal_speed_nk1,
                               heading_nk1=goal_heading_nk1,
                               variable=True)

    start_nk5 = start_config.position_heading_speed_and_angular_speed_nk5()
    start_n5 = start_nk5[:, 0]

    goal_nk5 = goal_config.position_heading_speed_and_angular_speed_nk5()
    goal_n5 = goal_nk5[:, 0]

    p = DotMap(spline_params=DotMap(epsilon=1e-5))
    ts_nk = tf.tile(tf.linspace(0., dt * k, k)[None], [n, 1])
    spline_traj = Spline3rdOrder(dt=dt, k=k, n=n, params=p.spline_params)
    spline_traj.fit(start_config, goal_config, factors=None)
    spline_traj.eval_spline(ts_nk, calculate_speeds=True)

    pos_nk3 = spline_traj.position_and_heading_nk3()
    v_nk1 = spline_traj.speed_nk1()
    start_pos_diff = (pos_nk3 - start_n5[:, None, :3])[:, 0]
    goal_pos_diff = (pos_nk3 - goal_n5[:, None, :3])[:, -1]
    assert (np.allclose(start_pos_diff, np.zeros((n, 3)), atol=1e-6))
    assert (np.allclose(goal_pos_diff, np.zeros((n, 3)), atol=1e-6))

    start_vel_diff = (v_nk1 - start_n5[:, None, 3:4])[:, 0]
    goal_vel_diff = (v_nk1 - goal_n5[:, None, 3:4])[:, -1]
    assert (np.allclose(start_vel_diff, np.zeros((n, 1)), atol=1e-6))
    assert (np.allclose(goal_vel_diff, np.zeros((n, 1)), atol=1e-6))

    if visualize:
        fig = plt.figure()
        ax = fig.add_subplot(111)
        spline_traj.render(ax, freq=4)
        plt.show()
Beispiel #15
0
def test_spline_rescaling():
    # Set the random seed
    np.random.seed(seed=1)

    # Spline trajectory params
    n = 2
    dt = .1
    k = 10
    final_times_n1 = tf.constant([[2.], [1.]])

    # Goal states and initial speeds
    goal_posx_n11 = tf.constant([[[0.4]], [[1.]]])
    goal_posy_n11 = tf.constant([[[0.]], [[1.]]])
    goal_heading_n11 = tf.constant([[[0.]], [[np.pi / 2]]])
    start_speed_nk1 = tf.ones((2, 1, 1), dtype=tf.float32) * 0.5

    # Define the maximum speed, angular speed and maximum horizon
    max_speed = 0.6
    max_angular_speed = 1.1

    # Define start and goal configurations
    start_config = SystemConfig(dt,
                                n,
                                1,
                                speed_nk1=start_speed_nk1,
                                variable=False)
    goal_config = SystemConfig(dt,
                               n,
                               k=1,
                               position_nk2=tf.concat(
                                   [goal_posx_n11, goal_posy_n11], axis=2),
                               heading_nk1=goal_heading_n11,
                               variable=True)

    # Fit the splines
    p = DotMap(spline_params=DotMap(epsilon=1e-5))
    spline_trajs = Spline3rdOrder(dt=dt, k=k, n=n, params=p.spline_params)
    spline_trajs.fit(start_config, goal_config, final_times_n1, factors=None)

    # Evaluate the splines
    ts_nk = tf.stack([
        tf.linspace(0., final_times_n1[0, 0], 100),
        tf.linspace(0., final_times_n1[1, 0], 100)
    ],
                     axis=0)
    spline_trajs.eval_spline(ts_nk, calculate_speeds=True)

    # Compute the required horizon
    required_horizon_n1 = spline_trajs.compute_dynamically_feasible_horizon(
        max_speed, max_angular_speed)
    assert required_horizon_n1[0, 0] < final_times_n1[0, 0]
    assert required_horizon_n1[1, 0] > final_times_n1[1, 0]

    # Compute the maximum speed and angular speed
    max_speed_n1 = tf.reduce_max(spline_trajs.speed_nk1(), axis=1)
    max_angular_speed_n1 = tf.reduce_max(tf.abs(
        spline_trajs.angular_speed_nk1()),
                                         axis=1)
    assert max_speed_n1[0, 0] < max_speed
    assert max_angular_speed_n1[0, 0] < max_angular_speed
    assert max_speed_n1[1, 0] > max_speed
    assert max_angular_speed_n1[1, 0] > max_angular_speed

    # Rescale horizon so that the trajectories are dynamically feasible
    spline_trajs.rescale_spline_horizon_to_dynamically_feasible_horizon(
        max_speed, max_angular_speed)
    assert np.allclose(spline_trajs.final_times_n1.numpy(),
                       required_horizon_n1.numpy(),
                       atol=1e-2)

    # Compute the maximum speed and angular speed
    max_speed_n1 = tf.reduce_max(spline_trajs.speed_nk1(), axis=1)
    max_angular_speed_n1 = tf.reduce_max(tf.abs(
        spline_trajs.angular_speed_nk1()),
                                         axis=1)
    assert max_speed_n1[0, 0] <= max_speed
    assert max_angular_speed_n1[0, 0] <= max_angular_speed
    assert max_speed_n1[1, 0] <= max_speed
    assert max_angular_speed_n1[1, 0] <= max_angular_speed

    # Find the spline trajectories that are valid
    valid_idxs_n = spline_trajs.find_trajectories_within_a_horizon(
        horizon_s=2.)
    assert valid_idxs_n.shape == (1, )
    assert valid_idxs_n.numpy()[0] == 0
Beispiel #16
0
 def __init__(self, simulator, params):
     super(NNPlanner, self).__init__(simulator, params)
     self.goal_ego_config = SystemConfig(dt=self.params.dt, n=1, k=1)
Beispiel #17
0
    def _reset_start_configuration(self, rng):
        """
        Reset the starting configuration of the vehicle.
        """
        p = self.params.reset_params.start_config

        # Reset the position
        if p.position.reset_type == 'random':
            # Select a random position on the map that is at least obstacle margin
            # away from the nearest obstacle
            obs_margin = self.params.avoid_obstacle_objective.obstacle_margin1
            dist_to_obs = 0.
            while dist_to_obs <= obs_margin:
                start_112 = self.obstacle_map.sample_point_112(rng)
                dist_to_obs = tf.squeeze(self.obstacle_map.dist_to_nearest_obs(start_112))
        elif p.position.reset_type == 'custom':
            x, y = p.position.start_pos
            start_112 = np.array([[[x, y]]], dtype=np.float32)
            dist_to_obs = tf.squeeze(self.obstacle_map.dist_to_nearest_obs(start_112))
            assert(dist_to_obs.numpy() > 0.0)
        else:
            raise NotImplementedError('Unknown reset type for the vehicle starting position.')

        # Reset the heading
        if p.heading.reset_type == 'zero':
            heading_111 = np.zeros((1, 1, 1))
        elif p.heading.reset_type == 'random':
            heading_111 = rng.uniform(p.heading.bounds[0], p.heading.bounds[1], (1, 1, 1))
        elif p.position.reset_type == 'custom':
            theta = p.heading.start_heading
            heading_111 = np.array([[[theta]]], dtype=np.float32)
        else:
            raise NotImplementedError('Unknown reset type for the vehicle starting heading.')

        # Reset the speed
        if p.speed.reset_type == 'zero':
            speed_111 = np.zeros((1, 1, 1))
        elif p.speed.reset_type == 'random':
            speed_111 = rng.uniform(p.speed.bounds[0], p.speed.bounds[1], (1, 1, 1))
        elif p.speed.reset_type == 'custom':
            speed = p.speed.start_speed
            speed_111 = np.array([[[speed]]], dtype=np.float32)
        else:
            raise NotImplementedError('Unknown reset type for the vehicle starting speed.')

        # Reset the angular speed
        if p.ang_speed.reset_type == 'zero':
            ang_speed_111 = np.zeros((1, 1, 1))
        elif p.ang_speed.reset_type == 'random':
            ang_speed_111 = rng.uniform(p.ang_speed.bounds[0], p.ang_speed.bounds[1], (1, 1, 1))
        elif p.ang_speed.reset_type == 'gaussian':
            ang_speed_111 = rng.normal(p.ang_speed.gaussian_params[0],
                                       p.ang_speed.gaussian_params[1], (1, 1, 1))
        elif p.ang_speed.reset_type == 'custom':
            ang_speed = p.ang_speed.start_ang_speed
            ang_speed_111 = np.array([[[ang_speed]]], dtype=np.float32)
        else:
            raise NotImplementedError('Unknown reset type for the vehicle starting angular speed.')

        # Initialize the start configuration
        self.start_config = SystemConfig(dt=p.dt, n=1, k=1,
                                         position_nk2=start_112,
                                         heading_nk1=heading_111,
                                         speed_nk1=speed_111,
                                         angular_speed_nk1=ang_speed_111)

        # The system dynamics may need the current starting position for
        # coordinate transforms (i.e. realistic simulation)
        self.system_dynamics.reset_start_state(self.start_config)
Beispiel #18
0
    def _reset_goal_configuration(self, rng):
        p = self.params.reset_params.goal_config
        goal_norm = self.params.goal_dist_norm
        goal_radius = self.params.goal_cutoff_dist
        start_112 = self.start_config.position_nk2()
        obs_margin = self.params.avoid_obstacle_objective.obstacle_margin1

        # Reset the goal position
        if p.position.reset_type == 'random':
            # Select a random position on the map that is at least obstacle margin away from the nearest obstacle, and
            # not within the goal margin of the start position.
            dist_to_obs = 0.
            dist_to_goal = 0.
            while dist_to_obs <= obs_margin or dist_to_goal <= goal_radius:
                goal_112 = self.obstacle_map.sample_point_112(rng)
                dist_to_obs = tf.squeeze(self.obstacle_map.dist_to_nearest_obs(goal_112))
                dist_to_goal = np.linalg.norm((start_112 - goal_112)[0], ord=goal_norm)
        elif p.position.reset_type == 'custom':
            x, y = p.position.goal_pos
            goal_112 = np.array([[[x, y]]], dtype=np.float32)
            dist_to_obs = tf.squeeze(self.obstacle_map.dist_to_nearest_obs(goal_112))
            assert(dist_to_obs.numpy() > 0.0)
        elif p.position.reset_type == 'random_v1':
            assert self.obstacle_map.name == 'SBPDMap'
            # Select a random position on the map that is at least obs_margin away from the
            # nearest obstacle, and not within the goal margin of the start position.
            # Additionaly the goal position must satisfy:
            # fmm_dist(start, goal) - l2_dist(start, goal) > fmm_l2_gap (goal should not be
            # fmm_dist(start, goal) < max_dist (goal should not be too far away)

            # Construct an fmm map where the 0 level set is the start position
            start_fmm_map = self._init_fmm_map(goal_pos_n2=self.start_config.position_nk2()[:, 0]) 
            # enforce fmm_dist(start, goal) < max_fmm_dist
            free_xy = np.where(start_fmm_map.fmm_distance_map.voxel_function_mn <
                               p.position.max_fmm_dist)
            free_xy = np.array(free_xy).T
            free_xy = free_xy[:, ::-1]
            free_xy_pts_m2 = self.obstacle_map._map_to_point(free_xy)
            
            # enforce dist_to_nearest_obstacle > obs_margin
            dist_to_obs = tf.squeeze(self.obstacle_map.dist_to_nearest_obs(free_xy_pts_m2[:, None])).numpy()

            dist_to_obs_valid_mask = dist_to_obs > obs_margin

            # enforce dist_to_goal > goal_radius
            fmm_dist_to_goal = np.squeeze(start_fmm_map.fmm_distance_map.compute_voxel_function(free_xy_pts_m2[:, None]).numpy())
            fmm_dist_to_goal_valid_mask = fmm_dist_to_goal > goal_radius

            # enforce fmm_dist - l2_dist > fmm_l2_gap
            fmm_l2_gap = rng.uniform(0.0, p.position.max_dist_diff)
            l2_dist_to_goal = np.linalg.norm((start_112 - free_xy_pts_m2[:, None]), axis=2)[:, 0]
            fmm_dist_to_goal = np.squeeze(start_fmm_map.fmm_distance_map.compute_voxel_function(free_xy_pts_m2[:, None]).numpy())
            fmm_l2_gap_valid_mask = fmm_dist_to_goal - l2_dist_to_goal > fmm_l2_gap

            valid_mask = np.logical_and.reduce((dist_to_obs_valid_mask,
                                                fmm_dist_to_goal_valid_mask,
                                                fmm_l2_gap_valid_mask))
            free_xy = free_xy[valid_mask]
            if len(free_xy) == 0:
                # there are no goal points within the max_fmm_dist of start
                # return True so the start is reset
                return True

            goal_112 = self.obstacle_map.sample_point_112(rng, free_xy_map_m2=free_xy)
        else:
            raise NotImplementedError('Unknown reset type for the vehicle goal position.')

        # Initialize the goal configuration
        self.goal_config = SystemConfig(dt=p.dt, n=1, k=1,
                                        position_nk2=goal_112)
        return False
Beispiel #19
0
def test_spline_3rd_order(visualize=False):
    """
    Create a start and goal states, fit a spline from the two points
    assert tests ensure that the difference between the computed points
    and their manually computed points is very small (they are close)
    """
    np.random.seed(seed=1)
    n = 5
    dt = .01
    k = 100

    target_state = np.random.uniform(-np.pi, np.pi, 3)
    v0 = np.random.uniform(0., 0.5, 1)[0]  # Initial speed
    vf = 0.

    # Initial SystemConfig is [0, 0, 0, v0, 0]
    start_speed_nk1 = tf.ones((n, 1, 1), dtype=tf.float32) * v0

    goal_posx_nk1 = tf.ones((n, 1, 1), dtype=tf.float32) * target_state[0]
    goal_posy_nk1 = tf.ones((n, 1, 1), dtype=tf.float32) * target_state[1]
    goal_pos_nk2 = tf.concat([goal_posx_nk1, goal_posy_nk1], axis=2)
    goal_heading_nk1 = tf.ones((n, 1, 1), dtype=tf.float32) * target_state[2]
    goal_speed_nk1 = tf.ones((n, 1, 1), dtype=tf.float32) * vf

    start_config = SystemConfig(dt,
                                n,
                                1,
                                speed_nk1=start_speed_nk1,
                                variable=False)
    goal_config = SystemConfig(dt,
                               n,
                               1,
                               position_nk2=goal_pos_nk2,
                               speed_nk1=goal_speed_nk1,
                               heading_nk1=goal_heading_nk1,
                               variable=True)

    start_nk5 = start_config.position_heading_speed_and_angular_speed_nk5()
    start_n5 = start_nk5[:, 0]

    goal_nk5 = goal_config.position_heading_speed_and_angular_speed_nk5()
    goal_n5 = goal_nk5[:, 0]

    p = DotMap(spline_params=DotMap(epsilon=1e-5))
    ts_nk = tf.tile(tf.linspace(0., dt * k, k)[None], [n, 1])
    spline_traj = Spline3rdOrder(dt=dt, k=k, n=n, params=p.spline_params)
    spline_traj.fit(start_config, goal_config, factors=None)
    spline_traj.eval_spline(ts_nk, calculate_speeds=True)

    pos_nk3 = spline_traj.position_and_heading_nk3()
    v_nk1 = spline_traj.speed_nk1()
    start_pos_diff = (pos_nk3 - start_n5[:, None, :3])[:, 0]
    goal_pos_diff = (pos_nk3 - goal_n5[:, None, :3])[:, -1]
    assert (np.allclose(start_pos_diff, np.zeros((n, 3)), atol=1e-6))
    assert (np.allclose(goal_pos_diff, np.zeros((n, 3)), atol=1e-6))

    start_vel_diff = (v_nk1 - start_n5[:, None, 3:4])[:, 0]
    goal_vel_diff = (v_nk1 - goal_n5[:, None, 3:4])[:, -1]
    assert (np.allclose(start_vel_diff, np.zeros((n, 1)), atol=1e-6))
    assert (np.allclose(goal_vel_diff, np.zeros((n, 1)), atol=1e-6))

    if visualize:
        fig = plt.figure()
        # ax = fig.add_subplot(111)
        fig, ax = plt.subplots(1, 1, squeeze=False)

        spline_traj.render(ax, freq=4)
        # plt.show()
        fig.savefig('./tests/spline/test_spline.png',
                    bbox_inches='tight',
                    pad_inches=0)