def plot_path(path, path_type):
fig = plt.figure()
ax = Axes3Ds(fig)
world.draw(ax)
if path is not None:
world.draw_line(ax, path, color='blue')
world.draw_points(ax, path, color='blue')
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')
ax.set_title("%s Path through %s" % (path_type, test_name))
return fig
def animate(time,
position,
rotation,
world,
filename=None,
blit=True,
show_axes=True):
"""
Animate a completed simulation result based on the time, position, and
rotation history. The animation may be viewed live or saved to a .mp4 video
(slower, requires additional libraries).
Parameters
time, (N,) with uniform intervals
position, (N,3)
rotation, (N,3,3)
world, a World object
filename, for saved video, or live view if None
blit, if True use blit for faster animation, default is True
show_axes, if True plot axes, default is True
"""
# Visual style.
shade = True
# Temporal style.
rtf = 1.0 # real time factor > 1.0 is faster than real time playback
render_fps = 30
close_on_finish = False
# Decimate data to render interval; always include t=0.
if time[-1] != 0:
sample_time = np.arange(0, time[-1], 1 / render_fps * rtf)
else:
sample_time = np.zeros((1, ))
index = _decimate_index(time, sample_time)
time = time[index]
position = position[index, :]
rotation = rotation[index, :, :]
# Set up axes.
if filename is not None:
fig = plt.figure(filename)
else:
fig = plt.figure('Animation')
ax = Axes3Ds(fig)
if not show_axes:
ax.set_axis_off()
ax.set_xlim(-1, 1)
ax.set_ylim(-1, 1)
ax.set_zlim(-1, 1)
quad = Quadrotor(ax)
world_artists = world.draw(ax)
title_artist = ax.set_title('t = {}'.format(time[0]))
def init():
ax.draw(fig.canvas.get_renderer())
return world_artists + list(quad.artists) + [title_artist]
def update(frame):
title_artist.set_text('t = {:.2f}'.format(time[frame]))
quad.transform(position=position[frame, :],
rotation=rotation[frame, :, :])
[a.do_3d_projection(fig.canvas.get_renderer()) for a in quad.artists]
if close_on_finish and frame == time.size - 1:
plt.close(fig)
return world_artists + list(quad.artists) + [title_artist]
ani = FuncAnimation(fig=fig,
func=update,
frames=time.size,
init_func=init,
interval=1000.0 / render_fps,
repeat=False,
blit=blit)
if filename is not None:
print('Saving Animation')
ani.save(filename, writer='ffmpeg', fps=render_fps, dpi=100)
import inspect
import matplotlib.pyplot as plt
from pathlib import Path
from flightsim.axes3ds import Axes3Ds
from flightsim.crazyflie_params import quad_params
from flightsim.simulate import Quadrotor, simulate, ExitStatus
from flightsim.world import World
from flightsim.world import ExpectTimeout
import numpy as np
quadrotor = Quadrotor(quad_params)
robot_radius = 0.25
fig = plt.figure('A* Path, Waypoints, and Trajectory')
ax = Axes3Ds(fig)
for _ in range(20):
try:
with ExpectTimeout(3):
world = World.fixed_block(lower_bounds=(-2,-2,0),upper_bounds=(3,2,2),block_width=0.5,block_height=1.5,num_blocks=4,robot_radii=robot_radius,margin=0.2)
break
except TimeoutError:
pass
world.draw(ax)
start=world.world['start']
goal=world.world['goal']
ax.plot([start[0]], [start[1]], [start[2]], 'go', markersize=5, markeredgewidth=3, markerfacecolor='none')
ax.plot([goal[0]], [goal[1]], [goal[2]], 'ro', markersize=5, markeredgewidth=3, markerfacecolor='none')
plt.show()
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 plot_mission(world, start, goal, results, metrics, test_name):
"""
Return a figure showing path through trees along with start and end.
"""
from matplotlib.axes import Axes
from matplotlib.lines import Line2D
from matplotlib.projections import register_projection
import matplotlib.pyplot as plt
from flightsim.axes3ds import Axes3Ds
# 3D Paths
flight_fig = plt.figure('3D Path')
ax = Axes3Ds(flight_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, results['flat']['x'], color='black', linewidth=2)
world.draw_points(ax, results['state']['x'], color='blue', markersize=4)
if not metrics['no_collision']:
ax.plot([metrics['collision_point'][0]],
[metrics['collision_point'][1]],
[metrics['collision_point'][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')
ax.set_title("Path through {test_name}", loc='left')
# # Visualize the original dense path from A*, your sparse waypoints, and the
# # smooth trajectory.
path_fig = plt.figure('A* Path, Waypoints, and Trajectory')
ax = Axes3Ds(path_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 results['path'] is not None:
# world.draw_line(ax, results['path'], color='red', linewidth=1)
# if results['points'] is not None:
# world.draw_points(ax, results['points'], color='purple', markersize=8)
# t = np.linspace(0, results['time'][-1], num=100)
# x = np.zeros((t.size,3))
# for i in range(t.size):
# flat = my_world_traj.update(t[i])
# x[i,:] = flat['x']
# world.draw_line(ax, results['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')
return [flight_fig]
