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')
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))
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()
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
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)
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)
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
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)
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()
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
def __init__(self, simulator, params):
super(NNPlanner, self).__init__(simulator, params)
self.goal_ego_config = SystemConfig(dt=self.params.dt, n=1, k=1)
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)
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
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)