def __init__(self, world=World.empty((0, 2, 0, 2, 0, 2)), resolution=(.1, .1, .1), margin=.2):
"""
This class creates a 3D voxel occupancy map of the configuration space from a flightsim World object.
Parameters:
world, a flightsim World object
resolution, the discretization of the occupancy grid in x,y,z
margin, the inflation radius used to create the configuration space (assuming a spherical drone)
"""
self.world = world
self.resolution = np.array(resolution)
self.margin = margin
self.map = np.array
self.create_map_from_world()
def plot_mission(points, results, title):
"""
Return a figure showing simulation results along with a set of waypoints,
formatting for printing as a letter size .pdf.
"""
import matplotlib.pyplot as plt
from matplotlib.axes import Axes
from matplotlib.projections import register_projection
from flightsim.axes3ds import Axes3Ds
register_projection(Axes3Ds)
def _decimate_to_freq(time, freq):
"""
Given sorted lists of source times and a target sampling frequency in Hz,
return indices of source times to approximate frequency.
"""
if time[-1] != 0:
sample_time = np.arange(0, time[-1], 1 / freq)
else:
sample_time = np.zeros((1, ))
index = np.arange(time.size)
sample_index = np.round(np.interp(sample_time, time,
index)).astype(int)
sample_index = np.unique(sample_index)
return sample_index
idx = _decimate_to_freq(results['time'], 100)
fig = plt.figure(num=str(title), figsize=(8.5, 11.0), clear=True)
fig.suptitle(str(title))
# Build world that fits path results.
margin = 0.1
pts = np.concatenate((results['state']['x'], results['flat']['x']), axis=0)
a = np.min(pts, axis=0) - margin
b = np.max(pts, axis=0) + margin
b = a + np.max(b - a)
world = World.empty((a[0], b[0], a[1], b[1], a[2], b[2]))
# 3D Position
x = results['state']['x'][idx, :]
x_des = results['flat']['x'][idx, :]
ax = fig.add_subplot(2, 2, 1, projection='3d')
world.draw(ax)
ax.plot3D(x[:, 0], x[:, 1], x[:, 2], 'b.')
ax.plot3D(x_des[:, 0], x_des[:, 1], x_des[:, 2], 'k')
# Position vs. Time
x = results['state']['x'][idx, :]
x_des = results['flat']['x'][idx, :]
time = results['time'][idx]
ax = fig.add_subplot(2, 2, 2)
ax.plot(time, x_des[:, 0], 'r', time, x_des[:, 1], 'g', time, x_des[:, 2],
'b')
ax.plot(time, x[:, 0], 'r.', time, x[:, 1], 'g.', time, x[:, 2], 'b.')
ax.legend(('x', 'y', 'z'))
ax.set_xlabel('time, s')
ax.set_ylabel('position, m')
ax.grid('major')
ax.set_title('Position')
# Motor speeds.
ax = fig.add_subplot(2, 2, 3)
s = results['control']['cmd_motor_speeds'][idx, :]
ax.plot(time, s[:, 0], 'r.', time, s[:, 1], 'g.', time, s[:, 2], 'b.',
time, s[:, 3], 'k.')
ax.legend(('1', '2', '3', '4'))
ax.set_xlabel('time, s')
ax.set_ylabel('motor speeds, rad/s')
ax.grid('major')
ax.set_title('Commands')
# Orientation.
ax = fig.add_subplot(2, 2, 4)
q_des = results['control']['cmd_q'][idx, :]
q = results['state']['q'][idx, :]
ax.plot(time, q_des[:, 0], 'r', time, q_des[:, 1], 'g', time, q_des[:, 2],
'b', time, q_des[:, 3], 'k')
ax.plot(time, q[:, 0], 'r.', time, q[:, 1], 'g.', time, q[:, 2], 'b.',
time, q[:, 3], 'k.')
ax.legend(('i', 'j', 'k', 'w'))
ax.set_xlabel('time, s')
ax.set_ylabel('quaternion')
ax.grid('major')
ax.set_title('Orientation')
return fig
ax.grid('major')
ax.set_title('Position')
v = state['v']
ax = axes[1]
ax.plot(vicon_time, v[:, 0], 'r.', vicon_time, v[:, 1], 'g.', vicon_time,
v[:, 2], 'b.')
ax.legend(('x', 'y', 'z'), loc='upper right')
ax.set_ylabel('velocity, m/s')
ax.set_xlabel('vicon_time, s')
ax.grid('major')
# 3D Position
# Either load a world definition file with boundaries and obstacles like this:
# world = World.from_file('test_lab_1.json')
# Or define an empty world and set the boundary size like this:
world = World.empty([-2, 2, -2, 2, 0, 4])
fig = plt.figure('3D Path')
ax = Axes3Ds(fig)
world.draw(ax)
world.draw_points(ax, state['x'], color='blue', markersize=4)
ax.legend(handles=[
Line2D([], [],
color='blue',
linestyle='',
marker='.',
markersize=4,
label='Flight')
],
loc='upper right')
# my_traj = hover_traj.HoverTraj()
# You will complete the implementation of the WaypointTraj object. It should
# work for any list of 3D coordinates, such as this example:
points = np.array([[0.0, 0.0, 0.0], [2.0, 0.0, 0.0], [2.0, 2.0, 0.0],
[2.0, 2.0, 2.0], [0.0, 2.0, 2.0], [0.0, 0.0, 2.0]])
my_traj = waypoint_traj.WaypointTraj(points)
# Set simulation parameters.
#
# You may use the initial condition and a simple hover trajectory to examine the
# step response of your controller to an initial disturbance in position or
# orientation.
w = 2
world = World.empty((-w, w, -w, w, -w, w))
t_final = 50
initial_state = {
'x': np.array([0, 0, 0]),
'v': np.array([0, 0, 0]),
'q': np.array([0, 0, 0, 1]), # [i,j,k,w]
'w': np.array([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('Simulate.')
