def load_tests(cls, files):
"""
Add one test for each input file. For each input file named
"test_XXX.json" creates a new test member function that will generate
output files "result_XXX.json" and "result_XXX.pdf".
"""
for file in files:
if file.stem.startswith('test_'):
test_name = file.stem[5:]
cls.test_names.append(test_name)
world = World.from_file(file)
# Dynamically add member function for this test.
def fn(self,
test_name=test_name,
world=world,
start=world.world['start'],
goal=world.world['goal'],
resolution=world.world['resolution'],
margin=world.world['margin'],
expected_path_length=world.world.get(
'expected_path_length', None)):
self.helper_test(test_name, world, start, goal, resolution,
margin, expected_path_length)
setattr(cls, 'test_' + test_name, fn)
# Remove any pre-existing output files for this test.
# TODO: The 'missing_ok' argument becomes available in Python
# 3.8, at which time contextlib is no longer needed.
with contextlib.suppress(FileNotFoundError):
(cls.outpath / ('result_' + test_name + '.json')).unlink()
with contextlib.suppress(FileNotFoundError):
(cls.outpath / ('result_' + test_name + '.pdf')).unlink()
def get_samples_from_new_map():
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=0.25,
margin=0.2)
break
except TimeoutError:
pass
resolution = (.25, .25, .25)
margin = .2
occ_map = OccupancyMap(world, resolution, margin)
start = world.world['start'] # Start point, shape=(3,)
goal = world.world['goal'] # Goal point, shape=(3,)
my_path = graph_search(world, resolution, margin, start, goal, False)[1:-1]
start = my_path[0]
goal = my_path[-1]
args = get_args()
args.go = goal
args.st = start
args.world = world
args.occ_map = occ_map
action_List = np.zeros((my_path.shape))
discretized_path = np.zeros((my_path.shape))
for i in range(discretized_path.shape[0]):
discretized_path[i, :] = args.occ_map.metric_to_index(my_path[i, :])
for i in range(discretized_path.shape[0]):
try:
action_List[i, :] = discretized_path[i + 1] - discretized_path[i]
except:
action_List[i, :] = np.zeros(3)
discretized_path = discretized_path.astype(int)
action_List = action_List.astype(int)
for i in range(len(action_List)):
args.optimal_path_length += np.linalg.norm(action_List[i])
start_index = discretized_path[0]
goal_index = discretized_path[-1]
args.start = start_index
args.goal = goal_index
args.search_range = 20
warm_up_set, warm_up_labels = get_all_warm_up(args, discretized_path,
action_List)
return args, warm_up_set, warm_up_labels
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 load_tests(cls, files, enable_timeouts=False, redirect_stdout=True):
"""
Add one test for each input file. For each input file named
"test_XXX.json" creates a new test member function that will generate
output files "result_XXX.json" and "result_XXX.pdf".
"""
std_target = None if redirect_stdout else sys.stdout
for file in files:
if file.stem.startswith('test_'):
test_name = file.stem[5:]
cls.test_names.append(test_name)
world = World.from_file(file)
# Timeouts must be enabled and have value in test_*.json file.
timeout = None
if enable_timeouts:
with open(file) as f:
test_dict = json.load(f)
if 'timeout' in test_dict.keys():
timeout = test_dict['timeout']
# Dynamically add member function for this test.
@timeout_decorator.timeout(
timeout,
exception_message="Test reached time limit of {} seconds".
format(timeout))
def fn(self,
test_name=test_name,
world=world,
start=world.world['start'],
goal=world.world['goal']):
self.helper_test(test_name, world, start, goal, std_target)
setattr(cls, 'test_' + test_name, fn)
# Remove any pre-existing output files for this test.
# TODO: The 'missing_ok' argument becomes available in Python
# 3.8, at which time contextlib is no longer needed.
with contextlib.suppress(FileNotFoundError):
(cls.outpath / ('result_' + test_name + '.json')).unlink()
with contextlib.suppress(FileNotFoundError):
(cls.outpath / ('result_' + test_name + '.pdf')).unlink()
position = np.zeros((t.shape[0], 3))
position[:, 0] = np.cos(t)
position[:, 1] = np.sin(t)
theta = t % (2 * np.pi)
rotvec = theta * np.array([[0], [0], [1]])
rotation = Rotation.from_rotvec(rotvec.T).as_dcm()
return (t, position, rotation)
(time, position, rotation) = dummy_sim_result()
wall_start = datetime.now()
animate(time, position, rotation, world, filename)
wall_elapsed = (datetime.now() - wall_start).total_seconds()
wall_fps = time.size / wall_elapsed
print('render time = {:.0f} s'.format(wall_elapsed))
print('render fps = {:.0f}'.format(wall_fps))
if __name__ == '__main__':
from flightsim.world import World
world = World.from_file('worlds/grid_forest.json')
print('Test .mp4 rendering.')
test(world=world, filename='data_out/quadrotor.mp4')
plt.show()
print('Test live view.')
test(world=world, filename=None)
plt.show()
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 is_occupied_metric(self, voxel_metric):
"""
Test if a metric point is within an occupied voxel.
Returns True if occupied, False otherwise.
"""
ind = self.metric_to_index(voxel_metric)
return self.is_occupied_index(ind)
if __name__ == "__main__":
from flightsim.axes3ds import Axes3Ds
import matplotlib.pyplot as plt
# Create a world object first
world = World.random_forest(world_dims=(5, 5, 5), tree_width=.1, tree_height=5, num_trees=10)
# Create a figure
fig = plt.figure()
ax = Axes3Ds(fig)
# Draw the world
# world.draw(ax)
# Create an Occupancy map
oc = OccupancyMap(world, (.2, .2, .5), .1)
# Draw the occupancy map (may be slow for many voxels; will look weird if plotted on top of a world.draw)
oc.draw(ax)
plt.show()
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')
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
# Improve figure display on high DPI screens.
# mpl.rcParams['figure.dpi'] = 200
# Choose a test example file. You should write your own example files too!
# filename = '../util/test_window.json'
# filename = '../util/test_maze.json'
# filename = '../util/test_lab_1.json'
# filename = '../util/test_lab_2.json'
filename = '../util/test_lab_3.json'
# filename = '../util/mymap.json'
# 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)
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
if __name__ == '__main__':
world = World.random_block(lower_bounds=(-2, -2, 0),
upper_bounds=(3, 2, 2),
block_width=0.5,
block_height=1.5,
num_blocks=4,
robot_radii=0.25,
margin=0.2) # World boundary and obstacles.
start = world.world['start'] # Start point, shape=(3,)
goal = world.world['goal'] # Goal point, shape=(3,)
df, init_state = sandbox(world, start, goal)
# print(df.head())
# 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.')
"""
Simple test maps should created by directly editing the .json text file,
following any of the existing examples.
This script directly generates some test scenario files and saves them as .json
files. It works, but this is probably not what you want to do.
"""
from flightsim.world import World
from proj1_2.code_soln.occupancy_map import OccupancyMap
if __name__ == "__main__":
# A grid of trees.
world = World.grid_forest(n_rows=4, n_cols=3, width=0.5, height=3.0, spacing=2.0)
world.world['start'] = (1, 1, 1)
world.world['goal'] = (3, 6, 2)
world.world['resolution'] = (0.5, 0.5, 0.5)
world.world['margin'] = 0.1
world.to_file('example_grid_forest.json')
# Some random trees.
world = World.random_forest(world_dims=(5, 5, 3), tree_width=0.2, tree_height=3.0, num_trees=10)
world.world['start'] = (0.5, 0.5, 0.5)
world.world['goal'] = (3, 4, 2.5)
world.world['resolution'] = (0.5, 0.5, 0.5)
world.world['margin'] = 0.1
world.to_file('example_random_forest.json')
