def test_sbpd_map(visualize=False):
np.random.seed(seed=1)
p = create_params()
r, dx_cm, traversible = load_building(p)
trajectory = Trajectory(dt=0.1, n=1, k=3, position_nk2=p.pos_nk2)
obstacle_map = SBPDMap(p.obstacle_map_params,
renderer=0,
res=dx_cm,
map_trav=traversible)
obs_dists_nk = obstacle_map.dist_to_nearest_obs(trajectory.position_nk2())
assert (np.allclose(obs_dists_nk[0], p.test_obst_map_ans))
if visualize:
#occupancy_grid_nn = obstacle_map.create_occupancy_grid(trajectory.position_nk2())
fig = plt.figure()
ax = fig.add_subplot(121)
obstacle_map.render(ax)
ax = fig.add_subplot(122)
# ax.imshow(occupancy_grid_nn, cmap='gray', origin='lower')
ax.set_axis_off()
# plt.show()
fig.savefig('./tests/obstacles/test_obstacle_map.png',
bbox_inches='tight',
pad_inches=0)
def test_sbpd_map(visualize=False):
np.random.seed(seed=1)
# Define a set of positions and evaluate objective
pos_nk2 = tf.constant([[[8., 16.], [8., 12.5], [18., 16.5]]],
dtype=tf.float32)
trajectory = Trajectory(dt=0.1, n=1, k=3, position_nk2=pos_nk2)
p = create_params()
# Create an SBPD Map
obstacle_map = SBPDMap(p.obstacle_map_params)
obs_dists_nk = obstacle_map.dist_to_nearest_obs(trajectory.position_nk2())
assert (np.allclose(obs_dists_nk, [0.59727454, 1.3223624, 0.47055122]))
if visualize:
occupancy_grid_nn = obstacle_map.create_occupancy_grid()
fig = plt.figure()
ax = fig.add_subplot(121)
obstacle_map.render(ax)
ax = fig.add_subplot(122)
ax.imshow(occupancy_grid_nn, cmap='gray', origin='lower')
ax.set_axis_off()
plt.show()
def simulation_init(self, sim_map, with_planner: bool = True,
with_system_dynamics: bool = True,
with_objectives: bool = True,
keep_episode_running: bool = False):
""" Initializes important fields for the Simulator"""
if self.params is None:
self.params = create_agent_params(with_planner=with_planner)
self.obstacle_map = sim_map
if with_objectives:
# Initialize Fast-Marching-Method map for agent's pathfinding
self.obj_fn = Agent._init_obj_fn(self)
Agent._init_fmm_map(self)
if with_planner:
# Initialize planner and vehicle data
self.planned_next_config = copy.deepcopy(self.current_config)
self.planner = Agent._init_planner(self)
self.vehicle_data = self.planner.empty_data_dict()
if with_system_dynamics:
# Initialize system dynamics and planner fields
self.system_dynamics = Agent._init_system_dynamics(self)
# the point in the trajectory where the agent collided
self.collision_point_k = np.inf
# whether or not to end the episode upon robot collision or continue
self.keep_episode_running = keep_episode_running
# default trajectory
self.trajectory = Trajectory(dt=self.params.dt, n=1, k=0)
def reset(self, seed=-1):
"""Reset the simulator. Optionally takes a seed to reset
the simulator's random state."""
if seed != -1:
self.rng.seed(seed)
# Note: Obstacle map must be reset independently of the fmm map.
# Sampling start and goal may depend on the updated state of the
# obstacle map. Updating the fmm map depends on the newly sampled goal.
reset_start = True
while reset_start:
self._reset_obstacle_map(self.rng) # Do nothing here
self._reset_start_configuration(self.rng) # Reset self.start_config
# Reset self.goal_config. If there is no available goals, reset_start = True, then reset the start again.
reset_start = self._reset_goal_configuration(self.rng)
# Manually restart the start and goal (only work for single goal)
# reset_start = self._reset_start_goal_manually(start_pos=[8.65, 50.25], goal_pos=[8.60, 47.15])
self._update_fmm_map() # Compute fmm_angle and fmm_goal, wrap it into voxel func
# Initiate and update a reachability map (reach_avoid or avoid)
if self.params.cost == 'reachability':
self._get_reachability_map()
# Update objective functions, may include reachability cost
self._update_obj_fn()
self.vehicle_trajectory = Trajectory(dt=self.params.dt, n=1, k=0)
self.obj_val = np.inf
self.vehicle_data = {}
def _ensure_world_coordinate_tensors_exist(self, goal_config=None):
"""
Creates tensors to hold lqr and spline trajectories as well as lqr feedback matrices in world coordinates.
"""
def _need_to_instantiate_tensors():
"""
Check whether placeholders for lqr and spline trajectories in the world coordiante frame have been
instantiated. If they haven't been, or they are the wrong dimension then re-instantiate them.
"""
if not (hasattr(self, 'trajectories_world')
and hasattr(self, 'spline_trajectories_world')):
return True
if goal_config is None:
return not (len(self.trajectories_world) == len(
self.start_configs))
return not (len(self.trajectories_world) == 1)
dt = self.params.system_dynamics_params.dt
if _need_to_instantiate_tensors():
if goal_config is None:
self.trajectories_world = [
Trajectory(dt=dt,
n=config.n,
k=self.params.planning_horizon,
variable=True,
track_trajectory_acceleration=self.params.
track_trajectory_acceleration)
for config in self.start_configs
]
# There usually is not enough memory to instantiate a placeholder for both the lqr and spline
# trajectories in the world frame
self.spline_trajectories_world = self.spline_trajectories
if self.params.convert_K_to_world_coordinates:
self.Ks_world_nkfd = [
tfe.Variable(tf.zeros_like(K)) for K in self.K_nkfd
]
else:
self.trajectories_world = [
Trajectory(dt=dt,
n=goal_config.n,
k=self.params.planning_horizon,
variable=True,
track_trajectory_acceleration=self.params.
track_trajectory_acceleration)
]
self.spline_trajectories_world = [
Trajectory(dt=dt,
n=goal_config.n,
k=self.params.planning_horizon,
variable=True,
track_trajectory_acceleration=self.params.
track_trajectory_acceleration)
]
if self.params.convert_K_to_world_coordinates:
self.Ks_world_nkfd = [
tfe.Variable(tf.zeros_like(self.K_nkfd[0][0:1]))
]
def load_and_process_data(self,
filename,
discard_lqr_controller_data=False,
discard_precomputed_lqr_trajectories=False,
track_trajectory_acceleration=False):
"""Load control pipeline data from a pickle file and process it so that it can be used by the pipeline."""
with open(filename, 'rb') as f:
data = pickle.load(f)
# To save memory, when discard_precomputed_lqr_trajectories is true the lqr_trajectories variables can be
# discarded.
dt = data['lqr_trajectories']['dt']
n = data['lqr_trajectories']['n']
if discard_precomputed_lqr_trajectories:
lqr_trajectories = Trajectory(dt=dt, n=n, k=0)
else:
lqr_trajectories = Trajectory.init_from_numpy_repr(
track_trajectory_acceleration=track_trajectory_acceleration,
**data['lqr_trajectories'])
# To save memory the LQR controllers and reference trajectories (spline trajectories) can be discarded when not
# needed (i.e. in simulation when the saved lqr_trajectory is the exact result of applying the saved LQR
# controllers.
n = data['spline_trajectories']['n']
if discard_lqr_controller_data:
spline_trajectories = Trajectory(dt=dt, n=n, k=0)
K_nkfd = tf.zeros((2, 1, 1, 1), dtype=np.float32)
k_nkf1 = tf.zeros((2, 1, 1, 1), dtype=np.float32)
else:
spline_trajectories = Trajectory.init_from_numpy_repr(
track_trajectory_acceleration=track_trajectory_acceleration,
**data['spline_trajectories'])
K_nkfd = tf.constant(data['K_nkfd'])
k_nkf1 = tf.constant(data['k_nkf1'])
# Load remaining variables
start_speeds = tf.constant(data['start_speeds'])
start_configs = SystemConfig.init_from_numpy_repr(
track_trajectory_acceleration=track_trajectory_acceleration,
**data['start_configs'])
waypt_configs = SystemConfig.init_from_numpy_repr(
track_trajectory_acceleration=track_trajectory_acceleration,
**data['waypt_configs'])
horizons = tf.constant(data['horizons'])
data_processed = {
'start_speeds': start_speeds,
'start_configs': start_configs,
'waypt_configs': waypt_configs,
'spline_trajectories': spline_trajectories,
'horizons': horizons,
'lqr_trajectories': lqr_trajectories,
'K_nkfd': K_nkfd,
'k_nkf1': k_nkf1
}
return data_processed
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 clip_data_along_time_axis(data, horizon):
"""Clips a data dictionary produced by this planner
to length horizon."""
data['trajectory'] = Trajectory.new_traj_clip_along_time_axis(
data['trajectory'], horizon)
data['spline_trajectory'] = Trajectory.new_traj_clip_along_time_axis(
data['spline_trajectory'], horizon)
data['K_nkfd'] = data['K_nkfd'][:, :horizon]
data['k_nkf1'] = data['k_nkf1'][:, :horizon]
return data
def gather_across_batch_dim(self, data, idxs):
""" For each key in data gather idxs across the batch dimension creating a new data dictionary."""
data_bin = {}
data_bin['waypt_configs'] = SystemConfig.gather_across_batch_dim_and_create(data['waypt_configs'], idxs)
data_bin['start_configs'] = SystemConfig.gather_across_batch_dim_and_create(data['start_configs'], idxs)
data_bin['start_speeds'] = tf.gather(data['start_speeds'], idxs, axis=0)
data_bin['spline_trajectories'] = Trajectory.gather_across_batch_dim_and_create(data['spline_trajectories'], idxs)
data_bin['horizons'] = tf.gather(data['horizons'], idxs, axis=0)
data_bin['lqr_trajectories'] = Trajectory.gather_across_batch_dim_and_create(data['lqr_trajectories'], idxs)
data_bin['K_nkfd'] = tf.gather(data['K_nkfd'], idxs, axis=0)
data_bin['k_nkf1'] = tf.gather(data['k_nkf1'], idxs, axis=0)
return data_bin
def mask_and_concat_data_along_batch_dim(data, k):
"""Keeps the elements in data which were produced
before time index k. Concatenates each list in data
along the batch dim after masking. Also returns data
from the first segment not in the valid mask."""
# Extract the Index of the Last Data Segment
data_times = np.cumsum([traj.k for traj in data['trajectory']])
valid_mask = (data_times <= k)
data_last = {}
last_data_idxs = np.where(np.logical_not(valid_mask))[0]
# Take the first last_data_idx
if len(last_data_idxs) > 0:
last_data_idx = last_data_idxs[0]
last_data_valid = True
else:
# Take the last element as it is not valid anyway
last_data_idx = len(valid_mask) - 1
last_data_valid = False
# Get the last segment data
data_last['system_config'] = data['system_config'][last_data_idx]
data_last['waypoint_config'] = data['waypoint_config'][last_data_idx]
data_last['trajectory'] = data['trajectory'][last_data_idx]
data_last['spline_trajectory'] = data['spline_trajectory'][
last_data_idx]
data_last['planning_horizon_n1'] = [
data['planning_horizon'][last_data_idx]
]
data_last['K_nkfd'] = data['K_nkfd'][last_data_idx]
data_last['k_nkf1'] = data['k_nkf1'][last_data_idx]
data_last['img_nmkd'] = data['img_nmkd'][last_data_idx]
# Get the main planner data
data['system_config'] = SystemConfig.concat_across_batch_dim(
np.array(data['system_config'])[valid_mask])
data['waypoint_config'] = SystemConfig.concat_across_batch_dim(
np.array(data['waypoint_config'])[valid_mask])
data['trajectory'] = Trajectory.concat_across_batch_dim(
np.array(data['trajectory'])[valid_mask])
data['spline_trajectory'] = Trajectory.concat_across_batch_dim(
np.array(data['spline_trajectory'])[valid_mask])
data['planning_horizon_n1'] = np.array(
data['planning_horizon'])[valid_mask][:, None]
data['K_nkfd'] = tf.boolean_mask(tf.concat(data['K_nkfd'], axis=0),
valid_mask)
data['k_nkf1'] = tf.boolean_mask(tf.concat(data['k_nkf1'], axis=0),
valid_mask)
data['img_nmkd'] = np.array(np.concatenate(data['img_nmkd'],
axis=0))[valid_mask]
return data, data_last, last_data_valid
def concat_data_across_binning_dim(self, data):
"""Concatenate across the binning dimension. It is asummed that data is a dictionary where each key maps to a
list of tensors, Trajectory, or System Config objects. The concatenated results are stored in lists of length 1
for each key (i.e. only one bin)."""
data['start_speeds'] = [tf.concat(data['start_speeds'], axis=0)]
data['start_configs'] = [SystemConfig.concat_across_batch_dim(data['start_configs'])]
data['waypt_configs'] = [SystemConfig.concat_across_batch_dim(data['waypt_configs'])]
data['spline_trajectories'] = [Trajectory.concat_across_batch_dim(data['spline_trajectories'])]
data['horizons'] = [tf.concat(data['horizons'], axis=0)]
data['lqr_trajectories'] = [Trajectory.concat_across_batch_dim(data['lqr_trajectories'])]
data['K_nkfd'] = [tf.concat(data['K_nkfd'], axis=0)]
data['k_nkf1'] = [tf.concat(data['k_nkf1'], axis=0)]
return data
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 reset_with_start_and_goal(self, start_config, goal_config):
"""Reset the simulator. Given the start and
end states."""
# Note: Obstacle map must be reset independently of the fmm map.
# Sampling start and goal may depend on the updated state of the
# obstacle map. Updating the fmm map depends on the newly sampled goal.
self._reset_obstacle_map(self.rng)
self.start_config = start_config
self.goal_config = goal_config
self._update_fmm_map()
self.vehicle_trajectory = Trajectory(dt=self.params.dt, n=1, k=0)
self.obj_val = np.inf
self.vehicle_data = {}
def optimize(self, start_config, goal_config=None, sim_state_hist=None):
""" Optimize the objective over a trajectory
starting from start_config.
1. Uses a control pipeline to plan paths from start_config
2. Evaluates the objective function on the resulting trajectories
3. Return the minimum cost waypoint, trajectory, and cost
"""
# TODO:
# changing the code so that sim_state is all history
obj_vals, data = self.eval_objective(start_config,
goal_config,
sim_state_hist=sim_state_hist)
min_idx = np.argmin(obj_vals)
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
try:
min_horizon = int(np.ceil(horizons_s[min_idx] / self.params.dt))
except:
min_horizon = int(np.ceil(horizons_s[min_idx, 0] / self.params.dt))
# If the real LQR data has been discarded just take the first element
# since it will be all zeros
if self.params.control_pipeline_params.discard_LQR_controller_data:
K_nkfd = controllers['K_nkfd'][0:1]
k_nkf1 = controllers['k_nkf1'][0:1]
else:
K_nkfd = controllers['K_nkfd'][min_idx:min_idx + 1]
k_nkf1 = controllers['k_nkf1'][min_idx:min_idx + 1]
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': K_nkfd,
'k_nkf1': k_nkf1,
'img_nmkd': []
} # Dont think we need for our purposes
return data
def plan(self):
"""
Runs the planner for one step from config to generate a
subtrajectory, the resulting robot config after the robot executes
the subtrajectory, and generates relevant planner data
NOTE: this planner only considers obstacles in the environment, not
collisions with other agents/personal space
"""
assert(hasattr(self, 'planner'))
assert(self.sim_dt is not None)
# Generate the next trajectory segment, update next config, update actions/data
if self.end_acting:
return
self.planner_data = self.planner.optimize(self.planned_next_config,
self.goal_config)
traj_segment = \
Trajectory.new_traj_clip_along_time_axis(self.planner_data['trajectory'],
self.params.control_horizon,
repeat_second_to_last_speed=True)
self.planned_next_config = \
SystemConfig.init_config_from_trajectory_time_index(
traj_segment, t=-1)
tr_acc = self.params.planner_params.track_accel
self.trajectory.append_along_time_axis(traj_segment,
track_trajectory_acceleration=tr_acc)
self.enforce_termination_conditions()
def _process_planner_data(self, start_config, planner_data):
"""
Process the planners current plan. This could mean applying
open loop control or LQR feedback control on a system.
"""
# The 'plan' is open loop control
if 'trajectory' not in planner_data.keys():
trajectory, commanded_actions_nkf = self.apply_control_open_loop(start_config,
planner_data['optimal_control_nk2'],
T=self.params.control_horizon-1,
sim_mode=self.system_dynamics.simulation_params.simulation_mode)
# The 'plan' is LQR feedback control
else:
# If we are using ideal system dynamics the planned trajectory
# is already dynamically feasible. Clip it to the control horizon
if self.system_dynamics.simulation_params.simulation_mode == 'ideal':
trajectory = Trajectory.new_traj_clip_along_time_axis(planner_data['trajectory'],
self.params.control_horizon,
repeat_second_to_last_speed=True)
_, commanded_actions_nkf = self.system_dynamics.parse_trajectory(trajectory)
elif self.system_dynamics.simulation_params.simulation_mode == 'realistic':
trajectory, commanded_actions_nkf = self.apply_control_closed_loop(start_config,
planner_data['spline_trajectory'],
planner_data['k_nkf1'],
planner_data['K_nkfd'],
T=self.params.control_horizon-1,
sim_mode='realistic')
else:
assert(False)
self.planner.clip_data_along_time_axis(planner_data, self.params.control_horizon)
return trajectory, planner_data, commanded_actions_nkf
def test_cost_function(plot=False):
# Create parameters
p = create_params()
obstacle_map = SBPDMap(p.obstacle_map_params)
obstacle_occupancy_grid = obstacle_map.create_occupancy_grid_for_map()
map_size_2 = obstacle_occupancy_grid.shape[::-1]
# Define a goal position and compute the corresponding fmm map
goal_pos_n2 = np.array([[20., 16.5]])
fmm_map = FmmMap.create_fmm_map_based_on_goal_position(goal_positions_n2=goal_pos_n2,
map_size_2=map_size_2,
dx=0.05,
map_origin_2=[0., 0.],
mask_grid_mn=obstacle_occupancy_grid)
# Define the cost function
objective_function = ObjectiveFunction(p.objective_fn_params)
objective_function.add_objective(ObstacleAvoidance(params=p.avoid_obstacle_objective, obstacle_map=obstacle_map))
objective_function.add_objective(GoalDistance(params=p.goal_distance_objective, fmm_map=fmm_map))
objective_function.add_objective(AngleDistance(params=p.goal_angle_objective, fmm_map=fmm_map))
# Define each objective separately
objective1 = ObstacleAvoidance(params=p.avoid_obstacle_objective, obstacle_map=obstacle_map)
objective2 = GoalDistance(params=p.goal_distance_objective, fmm_map=fmm_map)
objective3 = AngleDistance(params=p.goal_angle_objective, fmm_map=fmm_map)
# Define a set of positions and evaluate objective
pos_nk2 = tf.constant([[[8., 16.], [8., 12.5], [18., 16.5]]], dtype=tf.float32)
trajectory = Trajectory(dt=0.1, n=1, k=3, position_nk2=pos_nk2)
# Compute the objective function
values_by_objective = objective_function.evaluate_function_by_objective(trajectory)
overall_objective = objective_function.evaluate_function(trajectory)
# Expected objective values
expected_objective1 = objective1.evaluate_objective(trajectory)
expected_objective2 = objective2.evaluate_objective(trajectory)
expected_objective3 = objective3.evaluate_objective(trajectory)
expected_overall_objective = tf.reduce_mean(expected_objective1 + expected_objective2 + expected_objective3, axis=1)
# assert len(values_by_objective) == 3
# assert values_by_objective[0][1].shape == (1, 3)
# assert overall_objective.shape == (1,)
# assert np.allclose(overall_objective.numpy(), expected_overall_objective.numpy(), atol=1e-2)
# Optionally visualize the traversable and the points on which
# we compute the objective function
if plot:
import matplotlib.pyplot as plt
fig = plt.figure()
ax = fig.add_subplot(111)
obstacle_map.render(ax)
ax.plot(pos_nk2[0, :, 0].numpy(), pos_nk2[0, :, 1].numpy(), 'r.')
ax.plot(goal_pos_n2[0, 0], goal_pos_n2[0, 1], 'k*')
fig.savefig('./tmp/test_cost_function.png', bbox_inches='tight', pad_inches=0)
def optimize(self, start_config):
""" Optimize the objective over a trajectory
starting from start_config.
1. Uses a control pipeline to plan paths from start_config
2. Evaluates the objective function on the resulting trajectories
3. Return the minimum cost waypoint, trajectory, and cost
"""
obj_vals, data = self.eval_objective(start_config)
min_idx = tf.argmin(obj_vals)
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())
# If the real LQR data has been discarded just take the first element
# since it will be all zeros
if self.params.control_pipeline_params.discard_LQR_controller_data:
K_nkfd = controllers['K_nkfd'][0:1]
k_nkf1 = controllers['k_nkf1'][0:1]
else:
K_nkfd = controllers['K_nkfd'][min_idx:min_idx + 1]
k_nkf1 = controllers['k_nkf1'][min_idx:min_idx + 1]
img_nmkd = self.simulator.get_observation(config=start_config)
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': K_nkfd,
'k_nkf1': k_nkf1,
'img_nmkd': img_nmkd
}
return data
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 reset(self, seed=-1):
"""Reset the simulator. Optionally takes a seed to reset
the simulator's random state."""
if seed != -1:
self.rng.seed(seed)
# Note: Obstacle map must be reset independently of the fmm map.
# Sampling start and goal may depend on the updated state of the
# obstacle map. Updating the fmm map depends on the newly sampled goal.
reset_start = True
while reset_start:
self._reset_obstacle_map(self.rng)
self._reset_start_configuration(self.rng)
reset_start = self._reset_goal_configuration(self.rng)
self._update_fmm_map()
self.vehicle_trajectory = Trajectory(dt=self.params.dt, n=1, k=0)
self.obj_val = np.inf
self.vehicle_data = {}
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_goal_distance():
# Create parameters
p = create_params()
r, dx_cm, traversible = load_building(p)
obstacle_map = SBPDMap(p.obstacle_map_params,
renderer=0,
res=dx_cm,
map_trav=traversible)
# obstacle_map = SBPDMap(p.obstacle_map_params)
obstacle_occupancy_grid = obstacle_map.create_occupancy_grid_for_map()
map_size_2 = obstacle_occupancy_grid.shape[::-1]
# Define a goal position and compute the corresponding fmm map
goal_pos_n2 = p.goal_pos_n2
fmm_map = FmmMap.create_fmm_map_based_on_goal_position(
goal_positions_n2=goal_pos_n2,
map_size_2=map_size_2,
dx=0.05,
map_origin_2=[0., 0.],
mask_grid_mn=obstacle_occupancy_grid)
# Define the objective
objective = GoalDistance(params=p.goal_distance_objective, fmm_map=fmm_map)
# Define a set of positions and evaluate objective
pos_nk2 = p.pos_nk2
trajectory = Trajectory(dt=0.1, n=1, k=3, position_nk2=pos_nk2)
# Compute the objective
objective_values_13 = objective.evaluate_objective(trajectory)
assert objective_values_13.shape == (1, 3)
# Expected objective values
distance_map = fmm_map.fmm_distance_map.voxel_function_mn
idxs_xy_n2 = pos_nk2[0] / .05
idxs_yx_n2 = idxs_xy_n2[:, ::-1].astype(np.int32)
expected_distance = np.array([
distance_map[idxs_yx_n2[0][0], idxs_yx_n2[0][1]],
distance_map[idxs_yx_n2[1][0], idxs_yx_n2[1][1]],
distance_map[idxs_yx_n2[2][0], idxs_yx_n2[2][1]]
],
dtype=np.float32)
cost_at_margin = 25. * p.goal_distance_objective.goal_margin**2
expected_objective = 25. * expected_distance * expected_distance - cost_at_margin
# Error in objectives
# We have to allow a little bit of leeway in this test because the computation of FMM distance is not exact.
objetive_error = abs(expected_objective -
objective_values_13[0]) / (expected_objective + 1e-6)
assert max(objetive_error) <= 0.1
numerical_error = max(abs(objective_values_13[0] - p.test_goal_dist_ans))
assert numerical_error <= .01
def generate_control_pipeline(self, params=None):
p = self.params
# Initialize spline, cost function, lqr solver
waypoints_egocentric = self._sample_egocentric_waypoints(vf=0.)
self._init_pipeline()
pipeline_data = self.helper.empty_data_dictionary()
with tf.name_scope('generate_control_pipeline'):
if not self._incorrectly_binned_data_exists():
for v0 in self.start_velocities:
if p.verbose:
print('Initial Bin: v0={:.3f}'.format(v0))
start_config = self.system_dynamics.init_egocentric_robot_config(
dt=p.system_dynamics_params.dt,
n=self.waypoint_grid.n,
v=v0)
goal_config = SystemConfig.copy(waypoints_egocentric)
start_config, goal_config, horizons_n1 = self._dynamically_fit_spline(
start_config, goal_config)
lqr_trajectory, K_nkfd, k_nkf1 = self._lqr(start_config)
# TODO: Put the initial bin information in here too. This will make debugging much easier.
data_bin = {
'start_configs':
start_config,
'waypt_configs':
goal_config,
'start_speeds':
self.spline_trajectory.speed_nk1()[:, 0],
'spline_trajectories':
Trajectory.copy(self.spline_trajectory),
'horizons':
horizons_n1,
'lqr_trajectories':
lqr_trajectory,
'K_nkfd':
K_nkfd,
'k_nkf1':
k_nkf1
}
self.helper.append_data_bin_to_pipeline_data(
pipeline_data, data_bin)
# This data is incorrectly binned by velocity so collapse it all into one bin before saving it.
pipeline_data = self.helper.concat_data_across_binning_dim(
pipeline_data)
self._save_incorrectly_binned_data(pipeline_data)
else:
pipeline_data = self._load_incorrectly_binned_data()
pipeline_data = self._rebin_data_by_initial_velocity(pipeline_data)
self._set_instance_variables(pipeline_data)
for i, v0 in enumerate(self.start_velocities):
filename = self._data_file_name(v0=v0)
data_bin = self.helper.prepare_data_for_saving(pipeline_data, i)
self.save_control_pipeline(data_bin, filename)
def test_goal_angle_distance():
# Create parameters
p = create_params()
r, dx_cm, traversible = load_building(p)
obstacle_map = SBPDMap(p.obstacle_map_params,
renderer=0,
res=dx_cm,
map_trav=traversible)
# obstacle_map = SBPDMap(p.obstacle_map_params)
obstacle_occupancy_grid = obstacle_map.create_occupancy_grid_for_map()
map_size_2 = obstacle_occupancy_grid.shape[::-1]
# Define a goal position and compute the corresponding fmm map
# goal_pos_n2 = np.array([[9., 15.]])
goal_pos_n2 = p.goal_pos_n2
fmm_map = FmmMap.create_fmm_map_based_on_goal_position(
goal_positions_n2=goal_pos_n2,
map_size_2=map_size_2,
dx=0.05,
map_origin_2=[0., 0.],
mask_grid_mn=obstacle_occupancy_grid)
# Define the objective
objective = AngleDistance(params=p.goal_angle_objective, fmm_map=fmm_map)
# Define a set of positions and evaluate objective
# pos_nk2 = np.array([[[8., 16.], [8., 12.5], [18., 16.5]]], dtype=np.float32)
pos_nk2 = p.pos_nk2
trajectory = Trajectory(dt=0.1, n=1, k=3, position_nk2=pos_nk2)
# Compute the objective
objective_values_13 = objective.evaluate_objective(trajectory)
assert objective_values_13.shape == (1, 3)
# Expected objective values
angle_map = fmm_map.fmm_angle_map.voxel_function_mn
idxs_xy_n2 = pos_nk2[0] / .05
idxs_yx_n2 = idxs_xy_n2[:, ::-1].astype(np.int32)
expected_angles = np.array([
angle_map[idxs_yx_n2[0][0], idxs_yx_n2[0][1]],
angle_map[idxs_yx_n2[1][0], idxs_yx_n2[1][1]],
angle_map[idxs_yx_n2[2][0], idxs_yx_n2[2][1]]
],
dtype=np.float32)
expected_objective = 25. * abs(expected_angles)
assert np.allclose(objective_values_13[0], expected_objective, atol=1e-2)
# hardcoded results to match the given inputs
assert np.allclose(objective_values_13[0],
p.test_goal_ang_obj_ans,
atol=1e-2)
def test_goal_distance():
# Create parameters
p = create_params()
# Create an SBPD Map
obstacle_map = SBPDMap(p.obstacle_map_params)
obstacle_occupancy_grid = obstacle_map.create_occupancy_grid_for_map()
map_size_2 = obstacle_occupancy_grid.shape[::-1]
# Define a goal position and compute the corresponding fmm map
goal_pos_n2 = np.array([[20, 16.5]])
fmm_map = FmmMap.create_fmm_map_based_on_goal_position(
goal_positions_n2=goal_pos_n2,
map_size_2=map_size_2,
dx=0.05,
map_origin_2=[0., 0.],
mask_grid_mn=obstacle_occupancy_grid)
# Define the objective
objective = GoalDistance(params=p.goal_distance_objective, fmm_map=fmm_map)
# Define a set of positions and evaluate objective
pos_nk2 = tf.constant([[[8., 16.], [8., 12.5], [18., 16.5]]],
dtype=tf.float32)
trajectory = Trajectory(dt=0.1, n=1, k=3, position_nk2=pos_nk2)
# Compute the objective
objective_values_13 = objective.evaluate_objective(trajectory)
assert objective_values_13.shape == (1, 3)
# Expected objective values
distance_map = fmm_map.fmm_distance_map.voxel_function_mn
idxs_xy_n2 = pos_nk2[0] / .05
idxs_yx_n2 = idxs_xy_n2[:, ::-1].numpy().astype(np.int32)
expected_distance = np.array([
distance_map[idxs_yx_n2[0][0], idxs_yx_n2[0][1]],
distance_map[idxs_yx_n2[1][0], idxs_yx_n2[1][1]],
distance_map[idxs_yx_n2[2][0], idxs_yx_n2[2][1]]
],
dtype=np.float32)
cost_at_margin = 25. * p.goal_distance_objective.goal_margin**2
expected_objective = 25. * expected_distance * expected_distance - cost_at_margin
# Error in objectives
# We have to allow a little bit of leeway in this test because the computation of FMM distance is not exact.
objetive_error = abs(expected_objective - objective_values_13.numpy()[0]
) / (expected_objective + 1e-6)
assert max(objetive_error) <= 0.1
numerical_error = max(
abs(objective_values_13[0].numpy() - [3590.4614, 4975.554, 97.15442]))
assert numerical_error <= .01
def assemble_trajectory(self, x_nkd, u_nkf, pad_mode=None):
""" A utility function for assembling a trajectory object
from x_nkd, u_nkf, a list of states and actions for the system.
Here d=5=state dimension and u=2=action dimension. """
n = x_nkd.shape[0].value
k = x_nkd.shape[1].value
u_nkf = self._pad_control_vector(u_nkf, k, pad_mode=pad_mode)
position_nk2, heading_nk1 = x_nkd[:, :, :2], x_nkd[:, :, 2:3]
speed_nk1, angular_speed_nk1 = x_nkd[:, :, 3:4], x_nkd[:, :, 4:]
acceleration_nk1, angular_acceleration_nk1 = u_nkf[:, :, 0:1], u_nkf[:, :, 1:2]
return Trajectory(dt=self._dt, n=n, k=k, position_nk2=position_nk2,
heading_nk1=heading_nk1, speed_nk1=speed_nk1,
angular_speed_nk1=angular_speed_nk1, acceleration_nk1=acceleration_nk1,
angular_acceleration_nk1=angular_acceleration_nk1, variable=False)
def test_goal_angle_distance():
# Create parameters
p = create_params()
# Create an SBPD Map
obstacle_map = SBPDMap(p.obstacle_map_params)
obstacle_occupancy_grid = obstacle_map.create_occupancy_grid_for_map()
map_size_2 = obstacle_occupancy_grid.shape[::-1]
# Define a goal position and compute the corresponding fmm map
goal_pos_n2 = np.array([[20, 16.5]])
fmm_map = FmmMap.create_fmm_map_based_on_goal_position(
goal_positions_n2=goal_pos_n2,
map_size_2=map_size_2,
dx=0.05,
map_origin_2=[0., 0.],
mask_grid_mn=obstacle_occupancy_grid)
# Define the objective
objective = AngleDistance(params=p.goal_angle_objective, fmm_map=fmm_map)
# Define a set of positions and evaluate objective
pos_nk2 = tf.constant([[[8., 16.], [8., 12.5], [18., 16.5]]],
dtype=tf.float32)
trajectory = Trajectory(dt=0.1, n=1, k=3, position_nk2=pos_nk2)
# Compute the objective
objective_values_13 = objective.evaluate_objective(trajectory)
assert objective_values_13.shape == (1, 3)
# Expected objective values
angle_map = fmm_map.fmm_angle_map.voxel_function_mn
idxs_xy_n2 = pos_nk2[0] / .05
idxs_yx_n2 = idxs_xy_n2[:, ::-1].numpy().astype(np.int32)
expected_angles = np.array([
angle_map[idxs_yx_n2[0][0], idxs_yx_n2[0][1]],
angle_map[idxs_yx_n2[1][0], idxs_yx_n2[1][1]],
angle_map[idxs_yx_n2[2][0], idxs_yx_n2[2][1]]
],
dtype=np.float32)
expected_objective = 25. * abs(expected_angles)
assert np.allclose(objective_values_13.numpy()[0],
expected_objective,
atol=1e-2)
assert np.allclose(objective_values_13.numpy()[0],
[1.2449384, 29.137403, 0.],
atol=1e-2)
def query_next_ckpt(self):
robot_v_norm = np.sqrt(self.robot_v[0]**2 + self.robot_v[1]**2)
robot_w = self.robot_v[2]
robot_config = generate_config_from_pos_3(self.robot,
dt=self.agent_params.dt,
v=robot_v_norm,
w=robot_w)
planner_data = self.planner.optimize(robot_config,
self.goal_config,
sim_state_hist=self.sim_states)
tmp_goal = Trajectory.new_traj_clip_along_time_axis(
planner_data['trajectory'],
self.agent_params.control_horizon,
repeat_second_to_last_speed=True)
robot_goal = self.from_conf(tmp_goal, -1)
return robot_goal
def test_avoid_obstacle():
# Create parameters
p = create_params()
# Create an SBPD Map
obstacle_map = SBPDMap(p.obstacle_map_params)
# Define the objective
objective = ObstacleAvoidance(params=p.avoid_obstacle_objective,
obstacle_map=obstacle_map)
# Define a set of positions and evaluate objective
pos_nk2 = tf.constant([[[8., 16.], [8., 12.5], [18., 16.5]]],
dtype=tf.float32)
trajectory = Trajectory(dt=0.1, n=1, k=3, position_nk2=pos_nk2)
# Compute the objective
objective_values_13 = objective.evaluate_objective(trajectory)
assert objective_values_13.shape == (1, 3)
# Expected objective values
distance_map = obstacle_map.fmm_map.fmm_distance_map.voxel_function_mn
idxs_xy_n2 = pos_nk2[0] / .05
idxs_yx_n2 = idxs_xy_n2[:, ::-1].numpy().astype(np.int32)
expected_min_dist_to_obs = np.array([
distance_map[idxs_yx_n2[0][0], idxs_yx_n2[0][1]],
distance_map[idxs_yx_n2[1][0], idxs_yx_n2[1][1]],
distance_map[idxs_yx_n2[2][0], idxs_yx_n2[2][1]]
],
dtype=np.float32)
m0 = p.avoid_obstacle_objective.obstacle_margin0
m1 = p.avoid_obstacle_objective.obstacle_margin1
expected_infringement = m1 - expected_min_dist_to_obs
expected_infringement = np.clip(expected_infringement, a_min=0,
a_max=None) # ReLU
expected_infringement /= (m1 - m0)
expected_objective = 25. * expected_infringement * expected_infringement
assert np.allclose(objective_values_13.numpy()[0],
expected_objective,
atol=1e-4)
assert np.allclose(objective_values_13.numpy()[0], [0., 0., 0.54201907],
atol=1e-4)
def joystick_plan(self):
""" Runs the planner for one step from config to generate a
subtrajectory, the resulting robot config after the robot executes
the subtrajectory, and relevant planner data
- Access to sim_states from the self.current_world
"""
# get information about robot by its "current position" which was updated in sense()
[x, y, th] = self.robot_current
v = self.robot_v
# can also try:
# # assumes the robot has executed all the previous commands in self.commands
# (x, y, th, v) = self.from_conf(self.commands, -1)
robot_config = generate_config_from_pos_3(pos_3=(x, y, th), v=v)
self.planner_data = self.planner.optimize(
robot_config, self.goal_config, sim_state_hist=self.sim_states)
# TODO: make sure the planning control horizon is greater than the
# simulator_joystick_update_ratio else it will not plan far enough
# LQR feedback control loop
self.commands = Trajectory.new_traj_clip_along_time_axis(
self.planner_data['trajectory'],
self.agent_params.control_horizon,
repeat_second_to_last_speed=True)