cleanup code, fix up rel hyperlink, add link to gtp paper pdf

microsoft · Oct 31, 2019 · 7ddcca6 · 7ddcca6
1 parent 2401a96
commit 7ddcca6
Show file tree

Hide file tree

Showing 5 changed files with 22 additions and 92 deletions.
diff --git a/README.md b/README.md
@@ -172,7 +172,7 @@ Please read [the race monitoring section](https://github.com/microsoft/AirSim-Ne
 	$ cd baselines;
 	$ python generate_settings_file.py
 	```
-    - Start the AirSim Neurips binary, [as explained above](https://github.com/microsoft/AirSim-NeurIPS2019-Drone-Racing#running-the-executable)
+    - Start the AirSim Neurips binary, [as explained above](https://github.com/microsoft/AirSim-NeurIPS2019-Drone-Racing#running)
     - Run the code!  
       See all the [baseline arguments here](https://github.com/microsoft/AirSim-NeurIPS2019-Drone-Racing/blob/master/baselines/baseline_racer.py#L260-#L265) 
 	```shell
@@ -190,7 +190,7 @@ Please read [the race monitoring section](https://github.com/microsoft/AirSim-Ne
 	$ cd baselines;
 	$ python generate_settings_file.py
 	```
-	- Start the AirSim Neurips binary, [as explained above](https://github.com/microsoft/AirSim-NeurIPS2019-Drone-Racing#running-the-executable)
+	- Start the AirSim Neurips binary, [as explained above](https://github.com/microsoft/AirSim-NeurIPS2019-Drone-Racing#running)
     - Run the GTP code!  
 	```shell
 	$ python baseline_racer_gtp.py \
@@ -199,7 +199,7 @@ Please read [the race monitoring section](https://github.com/microsoft/AirSim-Ne
 		--enable_viz_traj \
 		--level_name Qualifier_Tier_1
 	```
-	- This method is an Iterative Best Response (IBR) trajectory planning technique. In IBR, first the trajectories of both drones are initialized as straight down the track at maximum speed (to win the game!). The opponent trajectory is then held constant while we solve for the ego trajectory via Model Predictive Control (MPC) optimization (details in gtp.py). Then, we hold the ego trajectory constant and solve for a guess of the opponent's trajectory in the same fashion. If after some iterations, the solution convereges (i.e., the resulting trajectories stop changing), we have reached a Nash equilibrium over the space of trajectories. That is to say, either agents can not unilaterally change their trajectory to increase their own performance. This implementation is a heuristic based on the original method proposed in the paper below. 
+	- This method is an Iterative Best Response (IBR) trajectory planning technique. In IBR, first the trajectories of both drones are initialized as straight down the track at maximum speed (to win the game!). The opponent trajectory is then held constant while we solve for the ego trajectory via Model Predictive Control (MPC) optimization (details in [gtp.py](baselines/gtp.py)). Then, we hold the ego trajectory constant and solve for a guess of the opponent's trajectory in the same fashion. If after some iterations, the solution convereges (i.e., the resulting trajectories stop changing), we have reached a Nash equilibrium over the space of trajectories. That is to say, either agents can not unilaterally change their trajectory to increase their own performance. This implementation is a heuristic based on the original method proposed in the paper below ([PDF here](https://arxiv.org/abs/1801.02302)). 
 		- R. Spica, D. Falanga, E. Cristofalo, E. Montijano, D. Scaramuzza, and M. Schwager, "A Real-Time Game Theoretic Planner for Autonomous Two-Player Drone Racing", in the Proccedings of Robotics: Science and Systems (RSS), 2018. 
 
 ## Quick API overview 

diff --git a/baselines/baseline_racer_gtp.py b/baselines/baseline_racer_gtp.py
@@ -1,37 +1,29 @@
-import gtp
 from baseline_racer import BaselineRacer
+from gtp_visualize import *
 from utils import to_airsim_vector, to_airsim_vectors
-from visualize import *
-
+import airsimneurips as airsim
+import argparse
+import gtp
+import numpy as np
+import time
 # Use non interactive matplotlib backend
 import matplotlib
-# matplotlib.use('Agg')
 matplotlib.use("TkAgg")
 import matplotlib.pyplot as plt
-from mpl_toolkits.mplot3d import Axes3D  # 3d plotting
-
-import airsimneurips as airsim
-import time
-import numpy as np
-
-import argparse
-
+from mpl_toolkits.mplot3d import Axes3D #3d plotting
 
 class BaselineRacerGTP(BaselineRacer):
     def __init__(self, traj_params, drone_names, drone_i, drone_params,
                  use_vel_constraints=False,
                  plot_gtp=False):
         super().__init__(drone_name=drone_names[drone_i], 
-          # plot_transform=True, 
-          viz_traj=True)
+            viz_traj=True)
         self.drone_names = drone_names
         self.drone_i = drone_i
         self.drone_params = drone_params
         self.traj_params = traj_params
-
         self.use_vel_constraints = use_vel_constraints
         self.plot_gtp = plot_gtp
-
         self.controller = None
 
         # for plotting: Just some fig, ax and line objects to keep track of
@@ -46,9 +38,6 @@ def __init__(self, traj_params, drone_names, drone_i, drone_params,
         print("baseline_racer_gtp ready!")
         if (self.traj_params.blocking):
             print("   with blocking behavior activated")
-        # self.pause_counter = 0
-        # self.pause_limit = 3
-        # self.send_new_traj = True
         # self.image_num = 0
 
     def update_and_plan(self):
@@ -67,7 +56,6 @@ def update_and_plan(self):
                 replot_state(self.line_state, state)
 
         trajectory = self.controller.iterative_br(self.drone_i, state)
-        # trajectory = self.controller.init_trajectory(0, state[0,:])
 
         # now, let's issue the new trajectory to the trajectory planner
         # fetch the current state first, to see, if our trajectory is still planned for ahead of us
@@ -97,11 +85,6 @@ def update_and_plan(self):
         if not self.use_vel_constraints:
             # this returns a future, that we do not call .join() on, as we want to re-issue a new command   
             # once we compute the next iteration of our high-level planner
-
-            # if self.send_new_traj:
-            #     print('Sending new trajectory!')
-            #     self.send_new_traj = False
-            # print('Final position trajectory: ', trajectory[k_truncate:, :])
 
             self.airsim_client.moveOnSplineAsync(
                 to_airsim_vectors(trajectory[k_truncate:, :]),
@@ -123,11 +106,6 @@ def update_and_plan(self):
                 vel_constraints[0, :] = trajectory[k_truncate, :] - new_state_i
             else:
                 vel_constraints[0, :] = trajectory[k_truncate, :] - trajectory[k_truncate - 1, :]
-
-            # vel_constraints /= self.traj_params.dt
-            # vel_constraints /= 100.0
-            # print('Final position trajectory: ', trajectory[k_truncate:, :])
-            # print('Final velocity trajectory: ', vel_constraints)
 
             self.airsim_client.moveOnSplineVelConstraintsAsync(
                 to_airsim_vectors(trajectory[k_truncate:, :]),
@@ -143,7 +121,7 @@ def update_and_plan(self):
             # refresh the updated plot
             self.fig.canvas.draw()
             self.fig.canvas.flush_events()
-            # # save figure
+            # save figure
             # figure_file_name = '/home/ericcristofalo/Documents/game_of_drones/log/image_' + str(self.image_num) + '.png'
             # print('saving image: ', figure_file_name)
             # self.fig.savefig(figure_file_name)
@@ -152,22 +130,6 @@ def update_and_plan(self):
             # self.fig.xlim(state[self.drone_i][1] - 10, state[self.drone_i][1] + 10)
             # self.fig.ylim(state[self.drone_i][0] - 10, state[self.drone_i][0] + 10)
 
-        # # counter for pause debugging
-        # # self.pause_counter = self.pause_counter + 1
-        # # if (self.pause_counter == 9):
-        # #     self.pause_counter = 0;
-        # #     self.send_new_traj = True
-        # self.pause_counter += 1
-        # if (self.pause_counter == self.pause_limit):
-        #     airsim_pause_check = self.airsim_client.simIsPause()
-        #     if airsim_pause_check:  # if simulation is paused
-        #         self.airsim_client.simPause(False)  # unpause
-        #         self.pause_limit = 3  # run for this many steps
-        #     else:
-        #         self.airsim_client.simPause(True)  # pause
-        #         self.pause_limit = 3  # pause for this many steps
-        #     self.pause_counter = 0
-
     def run(self):
         self.get_ground_truth_gate_poses()
 
@@ -186,11 +148,9 @@ def run(self):
             plot_track3d(self.ax3d, self.controller.track)
             self.fig2.show()
 
-
         while self.airsim_client.isApiControlEnabled(vehicle_name=self.drone_name):
             self.update_and_plan()
 
-
 def main(args):
     drone_names = ["drone_1", "drone_2"]
     drone_params = [

diff --git a/baselines/gtp.py b/baselines/gtp.py
@@ -1,27 +1,22 @@
-#!/usr/bin/env python
-import numpy as np
-import cvxpy as cp
 from scipy.interpolate import CubicSpline, CubicHermiteSpline
 import airsimneurips as airsim
+import cvxpy as cp
+import numpy as np
 import time
 
 gate_dimensions = [1.6, 1.6]
-
 gate_facing_vector = airsim.Vector3r(x_val=0, y_val=1, z_val=0)
 
-
 def rotate_vector(q, v):
     v_quat = v.to_Quaternionr()
     v_rotated_ = q * v_quat * q.inverse()
     return airsim.Vector3r(x_val=v_rotated_.x_val, y_val=v_rotated_.y_val, z_val=v_rotated_.z_val)
 
-
 class SplinedTrack:
     """This class represents a Track defined by Gates.
     A spline is fitted through the Gates with tangential constraints.
     This spline is then sampled at 2048 points.
     """
-
     def __init__(self, gate_poses):
         self.gates = gate_poses
 
@@ -55,11 +50,6 @@ def __init__(self, gate_poses):
         self.track_normals[:, 0] = -self.track_tangents[:, 1]
         self.track_normals[:, 1] = self.track_tangents[:, 0]
         self.track_normals /= np.linalg.norm(self.track_normals, axis=1)[:, np.newaxis]
-        # self.track_verticals = np.zeros_like(self.track_tangents)
-        # self.track_verticals = np.cross(self.track_tangents, self.track_normals)
-        # print('track tangents: ', self.track_tangents)
-        # print('track normals: ', self.track_normals)
-        # print('track verticals: ', self.track_verticals)
 
         self.track_widths = self.track_width_spline(taus)
         self.track_heights = self.track_height_spline(taus)
@@ -73,7 +63,6 @@ def track_frame_at(self, p):
         return i, self.track_centers[i], self.track_tangents[i], self.track_normals[i], \
                self.track_widths[i], self.track_heights[i]
 
-
 class IBRController:
     """
     OVERVIEW:
@@ -109,7 +98,6 @@ class IBRController:
        v_max       Maximal velocity, determines how far waypoints can be apart
        a_max       Maximal acceleration, not used here.
     """
-
     def __init__(self, params, drone_params, gate_poses):
         self.dt = params.dt
         self.n_steps = params.n

diff --git a/baselines/visualize.py → baselines/gtp_visualize.py b/baselines/visualize.py → baselines/gtp_visualize.py
@@ -1,15 +1,10 @@
 import numpy as np
 
-
-# ned_to_xy = np.array([[0, 1, 0], [1, 0, 0], [0, 0, -1]])
-
-
 # Plots gate centers
 def plot_gates_2d(ax, gate_poses):
     gate_pos = np.array([pose.position.to_numpy_array() for pose in gate_poses])
     ax.plot(gate_pos[:, 1], gate_pos[:, 0], 'x')
 
-
 def plot_track_arrows(ax, track):
     # Plot gates
     for i in range(track.n_gates):
@@ -26,7 +21,6 @@ def plot_track_arrows(ax, track):
                  track.track_tangents[i, 0],
                  head_width=0.08, head_length=0.25, fc='k', ec='k')
 
-
 def plot_track(ax, track):
     left_boundary = track.track_centers + track.track_normals*track.track_widths[:, np.newaxis]
     right_boundary = track.track_centers - track.track_normals*track.track_widths[:, np.newaxis]
@@ -50,21 +44,16 @@ def plot_track3d(ax, track):
     ax.plot(upper_boundary[:, 1], upper_boundary[:, 0], -upper_boundary[:, 2], 'g-')
     ax.plot(lower_boundary[:, 1], lower_boundary[:, 0], -lower_boundary[:, 2], 'g-')
 
-
 def plot_state(ax, state):
     return ax.plot(state[:, 1], state[:, 0], 'o')
 
-
 def replot_state(line, state):
     line.set_xdata(state[:, 1])
     line.set_ydata(state[:, 0])
 
-
 def plot_trajectory_2d(ax, trajectory):
     return ax.plot(trajectory[:, 1], trajectory[:, 0])
 
-
 def replot_trajectory_2d(line, trajectory):
     line.set_xdata(trajectory[:, 1])
-    line.set_ydata(trajectory[:, 0])
-
+    line.set_ydata(trajectory[:, 0])
diff --git a/baselines/utils.py b/baselines/utils.py
@@ -1,21 +1,14 @@
+import airsimneurips as airsim
 import json
-import os
-
 import numpy as np
-import airsimneurips as airsim
-
-
-# Maybe we wish to have the following 2 methods in the airsim API itself.
-
-def to_airsim_vector(array):
-    assert np.size(array) == 3
-    # Need to convert this to float
-    return airsim.Vector3r(np.float(array[0]), np.float(array[1]), np.float(array[2]))
-
+import os
 
-def to_airsim_vectors(array):
-    return [to_airsim_vector(array[i, :]) for i in range(np.size(array, 0))]
+def to_airsim_vector(np_arr):
+    assert np.size(np_arr) == 3
+    return airsim.Vector3r(np.float(np_arr[0]), np.float(np_arr[1]), np.float(np_arr[2]))
 
+def to_airsim_vectors(np_arr):
+    return [to_airsim_vector(np_arr[i, :]) for i in range(np.size(np_arr, 0))]
 
 # these clases are only meant to be settings generator.
 # for everything else, there's airsimneurips.Pose()