def test_mission(waypoint_traj_cls, se3_control_cls, points, initial_state):
"""
Test the provided trajectory class and control class against a set of
waypoints and a specific initial state. Return the simulation results and
the performance metrics.
"""
# Student code to test.
my_traj = waypoint_traj_cls(points)
my_se3_control = se3_control_cls(quad_params)
# Simulation options.
quadrotor = Quadrotor(quad_params)
t_final = 60
# Run simulation.
(time, state, control, flat, exit) = simulate(initial_state, quadrotor,
my_se3_control, my_traj,
t_final)
results = {
'time': time,
'state': state,
'control': control,
'flat': flat,
'exit': exit
}
# Evaluate results.
metrics = {}
# Must come to rest at goal.
metrics['stopped_at_goal'] = (results['exit'] == ExitStatus.COMPLETE)
metrics['time'] = time[-1]
# Must pass within minimum distance of each waypoint.
dist = cdist(points, results['state']['x'])
min_dist = np.min(dist, axis=1)
metrics['reached_waypoints'] = bool(np.max(min_dist) < 1.0)
# Details about why the simulation ended (success, failure, timeout).
metrics['sim_exit'] = results['exit'].value
return (results, metrics)
def test_mission(traj_cls, se3_control_cls, world, start, goal):
"""
Test the provided graph_search function against a world, start, and goal.
Return the simulation results and the performance metrics.
"""
# Student code to test.
start_time = time.time()
my_traj = traj_cls(world, start, goal)
planning_time = round(time.time() - start_time, 2)
my_se3_control = se3_control_cls(quad_params)
# Simulation options.
quadrotor = Quadrotor(quad_params)
t_final = 60
# Run simulation and collect results.
initial_state = {
'x': start,
'v': (0, 0, 0),
'q': (0, 0, 0, 1), # [i,j,k,w]
'w': (0, 0, 0)
}
(sim_time, state, control, flat, exit) = simulate(initial_state, quadrotor,
my_se3_control, my_traj,
t_final)
results = {
'time': sim_time,
'state': state,
'control': control,
'flat': flat,
'exit': exit,
'planning_time': planning_time
}
# Evaluate metrics.
metrics = calculate_metrics(results, world, goal)
return results, metrics
'x': start,
'v': (0, 0, 0),
'q': (0, 0, 0, 1), # [i,j,k,w]
'w': (0, 0, 0)
}
# Perform simulation.
#
# This function performs the numerical simulation. It returns arrays reporting
# the quadrotor state, the control outputs calculated by your controller, and
# the flat outputs calculated by you trajectory.
print()
print('Simulate.')
(sim_time, state, control, flat, exit) = simulate(initial_state, quadrotor,
my_se3_control,
my_world_traj, t_final)
print(exit.value)
# Print results.
#
# Only goal reached, collision test, and flight time are used for grading.
collision_pts = world.path_collisions(state['x'], robot_radius)
stopped_at_goal = (exit == ExitStatus.COMPLETE
) and np.linalg.norm(state['x'][-1] - goal) <= 0.05
no_collision = collision_pts.size == 0
flight_time = sim_time[-1]
flight_distance = np.sum(np.linalg.norm(np.diff(state['x'], axis=0), axis=1))
planning_time = planning_end_time - planning_start_time
def sandbox(world, start, goal):
# Load the test example.
# file = Path(inspect.getsourcefile(lambda:0)).parent.resolve() / '..' / 'util' / filename
# world = World.from_file(file) # World boundary and obstacles.
# start = world.world['start'] # Start point, shape=(3,)
# goal = world.world['goal'] # Goal point, shape=(3,)
# This object defines the quadrotor dynamical model and should not be changed.
quadrotor = Quadrotor(quad_params)
robot_radius = 0.25
# Your SE3Control object (from project 1-1).
my_se3_control = SE3Control(quad_params)
# Your MapTraj object. This behaves like the trajectory function you wrote in
# project 1-1, except instead of giving it waypoints you give it the world,
# start, and goal.
planning_start_time = time.time()
my_world_traj = WorldTraj(world, start, goal)
planning_end_time = time.time()
# Help debug issues you may encounter with your choice of resolution and margin
# by plotting the occupancy grid after inflation by margin. THIS IS VERY SLOW!!
# fig = plt.figure('world')
# ax = Axes3Ds(fig)
# world.draw(ax)
# fig = plt.figure('occupancy grid')
# ax = Axes3Ds(fig)
# resolution = SET YOUR RESOLUTION HERE
# margin = SET YOUR MARGIN HERE
# oc = OccupancyMap(world, resolution, margin)
# oc.draw(ax)
# ax.plot([start[0]], [start[1]], [start[2]], 'go', markersize=10, markeredgewidth=3, markerfacecolor='none')
# ax.plot( [goal[0]], [goal[1]], [goal[2]], 'ro', markersize=10, markeredgewidth=3, markerfacecolor='none')
# plt.show()
# Set simulation parameters.
t_final = 60
initial_state = {
'x': start,
'v': (0, 0, 0),
'q': (0, 0, 0, 1), # [i,j,k,w]
'w': (0, 0, 0)
}
# Perform simulation.
#
# This function performs the numerical simulation. It returns arrays reporting
# the quadrotor state, the control outputs calculated by your controller, and
# the flat outputs calculated by you trajectory.
print()
print('Simulate.')
(sim_time, state, control, flat, exit) = simulate(initial_state, quadrotor,
my_se3_control,
my_world_traj, t_final)
print(exit.value)
# Print results.
#
# Only goal reached, collision test, and flight time are used for grading.
collision_pts = world.path_collisions(state['x'], robot_radius)
stopped_at_goal = (exit == ExitStatus.COMPLETE
) and np.linalg.norm(state['x'][-1] - goal) <= 0.05
no_collision = collision_pts.size == 0
flight_time = sim_time[-1]
flight_distance = np.sum(
np.linalg.norm(np.diff(state['x'], axis=0), axis=1))
planning_time = planning_end_time - planning_start_time
print()
print(f"Results:")
print(f" No Collision: {'pass' if no_collision else 'FAIL'}")
print(f" Stopped at Goal: {'pass' if stopped_at_goal else 'FAIL'}")
print(f" Flight time: {flight_time:.1f} seconds")
print(f" Flight distance: {flight_distance:.1f} meters")
print(f" Planning time: {planning_time:.1f} seconds")
if not no_collision:
print()
print(f" The robot collided at location {collision_pts[0]}!")
# Plot Results
#
# You will need to make plots to debug your quadrotor.
# Here are some example of plots that may be useful.
# Visualize the original dense path from A*, your sparse waypoints, and the
# smooth trajectory.
fig = plt.figure('A* Path, Waypoints, and Trajectory')
ax = Axes3Ds(fig)
world.draw(ax)
ax.plot([start[0]], [start[1]], [start[2]],
'go',
markersize=16,
markeredgewidth=3,
markerfacecolor='none')
ax.plot([goal[0]], [goal[1]], [goal[2]],
'ro',
markersize=16,
markeredgewidth=3,
markerfacecolor='none')
if hasattr(my_world_traj, 'path'):
if my_world_traj.path is not None:
world.draw_line(ax, my_world_traj.path, color='red', linewidth=1)
else:
print("Have you set \'self.path\' in WorldTraj.__init__?")
if hasattr(my_world_traj, 'points'):
if my_world_traj.points is not None:
world.draw_points(ax,
my_world_traj.points,
color='purple',
markersize=8)
else:
print("Have you set \'self.points\' in WorldTraj.__init__?")
world.draw_line(ax, flat['x'], color='black', linewidth=2)
ax.legend(handles=[
Line2D([], [], color='red', linewidth=1, label='Dense A* Path'),
Line2D([], [],
color='purple',
linestyle='',
marker='.',
markersize=8,
label='Sparse Waypoints'),
Line2D([], [], color='black', linewidth=2, label='Trajectory')
],
loc='upper right')
# Position and Velocity vs. Time
(fig, axes) = plt.subplots(nrows=2,
ncols=1,
sharex=True,
num='Position vs Time')
x = state['x']
x_des = flat['x']
ax = axes[0]
ax.plot(sim_time, x_des[:, 0], 'r', sim_time, x_des[:, 1], 'g', sim_time,
x_des[:, 2], 'b')
ax.plot(sim_time, x[:, 0], 'r.', sim_time, x[:, 1], 'g.', sim_time,
x[:, 2], 'b.')
ax.legend(('x', 'y', 'z'), loc='upper right')
ax.set_ylabel('position, m')
ax.grid('major')
ax.set_title('Position')
v = state['v']
v_des = flat['x_dot']
ax = axes[1]
ax.plot(sim_time, v_des[:, 0], 'r', sim_time, v_des[:, 1], 'g', sim_time,
v_des[:, 2], 'b')
ax.plot(sim_time, v[:, 0], 'r.', sim_time, v[:, 1], 'g.', sim_time,
v[:, 2], 'b.')
ax.legend(('x', 'y', 'z'), loc='upper right')
ax.set_ylabel('velocity, m/s')
ax.set_xlabel('time, s')
ax.grid('major')
# Orientation and Angular Velocity vs. Time
(fig, axes) = plt.subplots(nrows=2,
ncols=1,
sharex=True,
num='Orientation vs Time')
q_des = control['cmd_q']
q = state['q']
ax = axes[0]
ax.plot(sim_time, q_des[:, 0], 'r', sim_time, q_des[:, 1], 'g', sim_time,
q_des[:, 2], 'b', sim_time, q_des[:, 3], 'k')
ax.plot(sim_time, q[:, 0], 'r.', sim_time, q[:, 1], 'g.', sim_time,
q[:, 2], 'b.', sim_time, q[:, 3], 'k.')
ax.legend(('i', 'j', 'k', 'w'), loc='upper right')
ax.set_ylabel('quaternion')
ax.set_xlabel('time, s')
ax.grid('major')
w = state['w']
ax = axes[1]
ax.plot(sim_time, w[:, 0], 'r.', sim_time, w[:, 1], 'g.', sim_time,
w[:, 2], 'b.')
ax.legend(('x', 'y', 'z'), loc='upper right')
ax.set_ylabel('angular velocity, rad/s')
ax.set_xlabel('time, s')
ax.grid('major')
# Commands vs. Time
(fig, axes) = plt.subplots(nrows=3,
ncols=1,
sharex=True,
num='Commands vs Time')
s = control['cmd_motor_speeds']
ax = axes[0]
ax.plot(sim_time, s[:, 0], 'r.', sim_time, s[:, 1], 'g.', sim_time,
s[:, 2], 'b.', sim_time, s[:, 3], 'k.')
ax.legend(('1', '2', '3', '4'), loc='upper right')
ax.set_ylabel('motor speeds, rad/s')
ax.grid('major')
ax.set_title('Commands')
M = control['cmd_moment']
ax = axes[1]
ax.plot(sim_time, M[:, 0], 'r.', sim_time, M[:, 1], 'g.', sim_time,
M[:, 2], 'b.')
ax.legend(('x', 'y', 'z'), loc='upper right')
ax.set_ylabel('moment, N*m')
ax.grid('major')
T = control['cmd_thrust']
ax = axes[2]
ax.plot(sim_time, T, 'k.')
ax.set_ylabel('thrust, N')
ax.set_xlabel('time, s')
ax.grid('major')
# # 3D Paths
# fig = plt.figure('3D Path')
# ax = Axes3Ds(fig)
# world.draw(ax)
# ax.plot([start[0]], [start[1]], [start[2]], 'go', markersize=16, markeredgewidth=3, markerfacecolor='none')
# ax.plot( [goal[0]], [goal[1]], [goal[2]], 'ro', markersize=16, markeredgewidth=3, markerfacecolor='none')
# world.draw_line(ax, flat['x'], color='black', linewidth=2)
# world.draw_points(ax, state['x'], color='blue', markersize=4)
# if collision_pts.size > 0:
# ax.plot(collision_pts[0,[0]], collision_pts[0,[1]], collision_pts[0,[2]], 'rx', markersize=36, markeredgewidth=4)
# ax.legend(handles=[
# Line2D([], [], color='black', linewidth=2, label='Trajectory'),
# Line2D([], [], color='blue', linestyle='', marker='.', markersize=4, label='Flight')],
# loc='upper right')
# Animation (Slow)
#
# Instead of viewing the animation live, you may provide a .mp4 filename to save.
# R = Rotation.from_quat(state['q']).as_dcm()
# animate(sim_time, state['x'], R, world=world, filename=None, show_axes=True)
# plt.show()
data = np.hstack(
(sim_time.reshape(-1, 1), x, x_des, v, v_des, q, q_des, w, s, M, T))
columns = [
'time', 'x[1]', 'x[2]', 'x[3]', 'x_des[1]', 'x_des[2]', 'x_des[3]',
'v[1]', 'v[2]', 'v[3]', 'v_des[1]', 'v_des[2]', 'v_des[3]', 'q[1]',
'q[2]', 'q[3]', 'q[4]', 'q_des[1]', 'q_des[2]', 'q_des[3]', 'q_des[4]',
'w[1]', 'w[2]', 'w[3]', 's[1]', 's[2]', 's[3]', 's[4]', 'M[1]', 'M[2]',
'M[3]', 'T[1]', 'T[2]', 'T[3]', 'T[4]'
]
df = pd.DataFrame(data, columns=columns)
init_state = {'x': x[0], 'v': v[0], 'q': q[0], 'w': w[0]}
return df, init_state
def forward_simulate(self, x0, J0, tau, D, return_Rs, return_cc):
# TODO
'''
Given the desired trajectory run a forward simulation to identify
if the desired trajectory can be followed and identify the dynamic
feasibilty based on the ability of the controller to follow the trajectory
Input:
x0 = 1 X 17 array of initial state [x0, v0, a0, q0, w0]
p0 = 1 X 3 -- postion in m
v0 = 1 X 3 -- velocity in m/s
a0 = 1 X 3 -- acceleration in m/s^2
q0 = 1 X 4 -- quaternion
w0 = 1 X 3 -- angular velocity in 1/s^2
tau = Duration of planner
Output:
Rs = 3 X 3 np array of state at the end of the period after the simulation
err_cs = M X 1 np array of the cost of the trajectory based on the controller's
ability to follow the trajectory
'''
t_final = tau
initial_state = {
'x': tuple(x0[0:3]),
'v': tuple(x0[3:6]),
'q': tuple(x0[9:13]),
'w': tuple(x0[13:16])
}
quadrotor = Quadrotor(quad_params)
my_se3_controller = SE3Control(quad_params)
# Traj object has been created to main tain consistency with the simulator which
# needs the trajectory to be an ibject with an update function
traj = trajectory(D)
(sim_time, state, control, flat, exit,
col_c) = simulate(initial_state, quadrotor, my_se3_controller, traj,
t_final, self.occ_map)
# TODO: Exit state check. Taking flat outputs
if True:
err = np.array(
flat['x'][-1].shape
) # TODO check if order is stored in the same order in both dictionary
err_cs = np.sum(np.absolute(err))
Rsx = flat['x'][-1]
Rsv = flat['x_dot'][-1]
# Rsx=state['x'][-1]
# Rsv=state['v'][-1]
Rsa = flat['x_ddot'][-1].reshape(729, 3)
Rsq = np.zeros((729, 4))
Rsw = np.zeros((729, 3))
# pdb.set_trace()
Rs = np.hstack((Rsx, Rsv, Rsa, Rsq, Rsw))
# pdb.set_trace()
else:
err_cs = np.inf
Rs = None
return_Rs = Rs
return_cc = return_cc + err_cs + col_c.reshape(729, 1)
return return_Rs, return_cc
