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