# Exercises

# Submission

To submit your assignment, please do the following:

- Submit the individual math-related tasks as a typeset PDF to GradeScope.
- Submit the teamβs coding-related tasks to your
`team-submissions`

repository (instructions below).

**Deadline:** the VNAV staff will clone your repository on **October 2nd** at **1 PM** ET.

# π€ Individual

## π¨ Deliverable 1 - Single-segment trajectory optimization (20 pts)

Consider the following minimum velocity ($r=1$) single-segment trajectory optimization problem:

\[\begin{eqnarray} \min_{P(t)} \quad \int_0^1 (P^{(1)}(t))^2 dt, \label{eq:minvel} \\\ s.t. \quad P(0) = 0, \label{eq:initpos} \\\ \quad P(1) = 1, \label{eq:finalpos} \end{eqnarray}\]with $P(t) \in \mathbb{R}[t]$, i.e., $P(t)$ is a polynomial function in $t$ with real coefficients:

\begin{equation} P(t) = p_N t^N + p_{N-1} t^{N-1} + \dots + p_1 t + p_0. \end{equation}

Note that because of constraint (\ref{eq:initpos}), we have $P(0)=p_0=0$, and we can parametrize $P(t)$ without a scalar part $p_0$.

*1.* Suppose we restrict $P(t) = p_1 t$ to be a polynomial of degree 1, what is the optimal solution of problem (\ref{eq:minvel})? What is the value of the cost function at the optimal solution?

*2.* Suppose now we allow $P(t)$ to have degree 2, i.e., $P(t) = p_2t^2 + p_1 t$.

*(a)* Write $\int_0^1 (P^{(1)}(t))^2 dt$, the cost function of problem (\ref{eq:minvel}), as $\boldsymbol{p}^T \boldsymbol{Q} \boldsymbol{p}$, where $\boldsymbol{p} = [p_1,p_2]^T$ and $\boldsymbol{Q} \in \mathcal{S}^2$ is a symmetric $2\times 2$ matrix.

*(b)* Write $P(1) = 1$, constraint (\ref{eq:finalpos}), as $\boldsymbol{A}\boldsymbol{p} = \boldsymbol{b}$, where $\boldsymbol{A} \in \mathbb{R}^{1 \times 2}$ and $\boldsymbol{b} \in \mathbb{R}$.

*(c)* Solve the Quadratic Program (QP): \begin{equation} \min_{\boldsymbol{p}} \boldsymbol{p}^T \boldsymbol{Q} \boldsymbol{p} \quad s.t. \quad \boldsymbol{A} \boldsymbol{p} = \boldsymbol{b}. \label{eq:QPtrajOpt} \end{equation} You can solve it by hand, or you can solve it using numerical QP solvers (e.g., you can easily use the `quadprog`

function in Matlab). What is the optimal solution you get for $P(t)$, and what is the value of the cost function at the optimal solution? Are you able to get a lower cost by allowing $P(t)$ to have degree 2?

*3.* Now suppose we allow $P(t) = p_3t^3 + p_2 t^2 + p_1 t$:

*(a)* Let $\boldsymbol{p} = [p_1,p_2,p_3]^T$, write down $\boldsymbol{Q} \in \mathcal{S}^3, \boldsymbol{A} \in \mathbb{R}^{1\times 3}, \boldsymbol{b} \in \mathbb{R}$ for the QP (\ref{eq:QPtrajOpt}).

*(b)* Solve the QP, what optimal solution do you get? Do this example agree with the result we learned from Euler-Lagrange equation in class?

*4.* Now suppose we are interested in adding one more constraint to problem (\ref{eq:minvel}):

\begin{eqnarray} \min_{P(t)} \quad \int_0^1 (P^{(1)}(t))^2 dt, \label{eq:minveladd} \

s.t. \quad P(0) = 0, \

\quad P(1) = 1, \

\quad P^{(1)}(1) = -2. \end{eqnarray} Using the QP method above, find the optimal solution and optimal cost of problem (\ref{eq:minveladd}) in the case of:

*(a)* $P(t) = p_2t^2 + p_1 t$, and

*(b)* $P(t) = p_3t^3 + p_2 t^2 + p_1t$.

## π¨ Deliverable 2 - Multi-segment trajectory optimization (15 pts)

*1.* Assume our goal is to compute the minimum snap trajectory ($r= 4$) over $k$ segments. How many and which type of constraints (at the intermediate points and at the start and end of the trajectory) do we need in order to solve this problem? Specify the number of waypoint constraints, free derivative constraints and fixed derivative constraints.

- Hint: According to Euler-Lagrange method, what is the degree of the polynomial of each segment?
- Hint: How many unknown parameters do we need to solve?
- Hint: How many constraints does each waypoint/free derivative/fixed derivative constraint provide?
- Hint: See figure for $k=3$ as described in the lecture notes.

*2.* Can you extend the previous question to the case in which the cost functional minimizes the $r$-th derivative and we have $k$ segments?

# π₯ Team

## π¨ Deliverable 3 - Drone Racing (65 pts)

For this lab we will be racing our simulated our simulated quadcopters in a drone racing course we prepared in our Unity simulator!

### Graded GitHub files checklist

- Implement all the missing parts in the code:
- Part 0 β
`planner_pkg/src/simple_traj_planner.cpp`

- Part 1.1 β
`trajectory_generation_pkg/src/trajectory_generation_node.cpp`

- Part 1.2 β
`trajectory_generation_pkg/src/trajectory_generation_node.cpp`

- Part 1.3 β
`trajectory_generation_pkg/src/trajectory_generation_node.cpp`

- Part 0 β
- Create a file called
`links.txt`

with two**publically links**(i.e., hosted on either Google Drive or Dropbox) to the following deliverables:- A video (i.e., ideally a screen capture) showing your quadrotor completing the race course.
- A ROS2 bag recording of your complete and fastest run. In ROS2 this a bag recording is a
**folder**with a`metadata.yaml`

and`.db3`

file(s) β i.e., it is no longer a single`.bag`

files as in ROS1. Instructions on how to record are below.

### Setting up your codebase

Moving forward, we will be using the following file structure:

```
~/vnav
βββ starter-code β cloned from https://github.com/MIT-SPARK/VNAV-labs
βΒ Β βββ ... β starter code for previous labs
βΒ Β βββ lab4 β start code for this lab
βββ team-submissions β cloned from https://github.mit.edu/VNAV2024-submissions
βΒ Β βββ ... β simulator files for previous labs
βΒ Β βββ lab4 β this lab's graded code files need to be commited/pushed here
βββ personal-submissions β cloned from https://github.mit.edu/VNAV2024-submissions
βΒ Β βββ ... β submission code from previous labs (no personal code for lab4)
βββ tesse β create this folder for downloading/unzipping simulator code
βΒ Β βββ lab3 β download/unzip from https://vnav.mit.edu/material/lab3.zip
βΒ Β βββ lab4 β download/unzip from https://vnav.mit.edu/material/lab4.zip
βββ ws β `colcon` workspaces for each assignment
βββ ... β ros2 workspaces for previous labs
βββ lab4 β ros2 workspace for lab4
βββ build β generated by `colcon build`
βββ install β generated by `colcon build`
βββ log β generated by `colcon build`
βββ src
βββ ... β packages placed/linked here by you
```

Letβs get the new assignment setup!

First, make sure to install these two dependencies:

```
# dependency libraries
sudo apt install libnlopt-dev libgoogle-glog-dev
```

Next, letβs get the new starter code:

```
# make sure you have the new starter code
cd ~/vnav/starter-code/
git pull
```

In `~/vnav/starter-code/lab4`

we now have the `planner_pkg`

and `trajectory_generation_pkg`

, the two packages we will be modifying for this lab.

To prevent merge conflicts, only one team member should do this next code block:

```
# copy lab4 starter code to submissions
mkdir -p ~/vnav/team-submissions/lab4
cp -r ~/vnav/starter-code/lab4/* ~/vnav/team-submissions/lab4
# commit starter code to submissions repo
cd ~/vnav/team-submissions/lab4
git add .
git commit -m "added lab4 starter files"
git push
```

Now, the other team member(s) should pull the updated submission code with the following code block:

```
# pull starter code
cd ~/vnav/team-submissions
git pull
# confirm ~/vnav/team-submissions/lab4 exists and has the starter code in it
```

Everyone will now setup their colcon package for this assignment and link their solution code:

```
# create folder
mkdir -p ~/vnav/ws/lab4/src
cd ~/vnav/ws/lab4/src
# link lab4 solution packages
ln -s ../../../team-submissions/lab4/* .
# link needed packages from lab3
ln -s ../../../team-submissions/lab3/controller_pkg .
# link needed unmodified packages from lab3
ln -s ../../../starter-code/lab3/tesse_msgs .
ln -s ../../../starter-code/lab3/tesse_ros_bridge .
# clone needed packages
git clone https://github.com/fishberg/mav_trajectory_generation.git
git clone https://github.com/fishberg/mav_comm.git
# build the colcon package
cd ..
colcon build --symlink-install
```

Now update your `~/.bashrc`

:

```
# make sure these are in your ~/.bashrc
source /opt/ros/humble/setup.bash
source ~/vnav/ws/lab4/install/setup.bash
# remove ANY other VNAV workspaces you have been previously sourcing
```

and then source your updated `~/.bashrc`

with:

```
source ~/.bashrc
```

## mav_comm

Note we are using a different `mav_comm`

repo than from the previous assignment. We needed to correct an incompatibility with the previous repo. If you followed the above instructions closely, it should all work.

To sanity check, your `~/vnav/ws/lab4/src`

folder should look like this:

```
src
βββ controller_pkg -> linked from lab3 team-submissions
βββ mav_comm -> cloned from https://github.com/fishberg/mav_comm
βββ mav_trajectory_generation -> cloned from https://github.com/MIT-SPARK/mav_trajectory_generation`
βββ planner_pkg -> linked from lab4 team-submissions
βββ tesse_msgs -> linked from lab3 starter-code
βββ tesse_ros_bridge -> linked from lab3 starter-code
βββ trajectory_generation_pkg -> linked from lab4 team-submissions
```

Next we will unpack the new simulator code. Please download the zip archive here.

```
# create folder for simulator
mkdir -p ~/vnav/tesse/lab4
# unzip contents of lab4.zip into ~/vnav/tesse/lab4
# make simulator executable
chmod +x ~/vnav/tesse/lab4/lab4.x86_64
```

Try launch the simulator. It should look like this:

Since we linked your `controller_pkg`

from your lab3 team-submissions, your controller should be running with the gains you found in lab3. Feel free to further tune the gains in `~/vnav/ws/lab4/src/controller_pkg/config/params.yaml`

.

## Controller gains

It's possible that you may need to adjust your controller gains from lab3 to achieve good performance in lab4.

Before you start coding, letβs take a look at the big picture. This is the system weβre trying to construct:

### Coding β Part 0

As a warm up exercise, letβs just fly and hover at the first gate! Follow the instructions for Part 0 inside `planner_pkg/src/simple_traj_planner.cpp`

.

To run your code, use the following commands:

```
# terminal 1
# run the simulator
cd ~/vnav/tesse/lab4
./lab4.x86_64
# terminal 2
# launches the bridge between tesse and ros2
# NOTE: make sure you always start the bridge AFTER the simulator
ros2 launch tesse_ros_bridge tesse_quadrotor_bridge.launch.yaml
# terminal 3
# launches the controller_node, traj_vertices_publisher, simple_traj_planner
ros2 launch planner_pkg static_point_test.launch.py
```

- Hint: you can press
`r`

to respawn your quadcopter - Hint: review the handout from lab 3 if you have trouble running the simulator.

## Waypoint publishing

We wrote the waypoint publishing node for you in `planner_pkg/src/traj_vertices_publisher.cpp`

. Although you don't need to modify this code, it is important to understand what it is doing. This node reads the position and orientation of the gates in the racing course and publishs them as waypoints for trajectory optimization. The topic to note here is `/desired_traj_vertices`

, which should contain a `geometry_msgs::PoseArray`

type storing the position and orientation of the gates on the racing course.

### Coding β Parts 1.1, 1.2, and 1.3

Next, letβs implement the rest of the stack. Follow the instructions for Parts 1.1, 1.2, and 1.3 in `trajectory_generation_pkg/src/trajectory_generation_node.cpp`

to get your quadcopter ready for drone racing!

This node will subscribe to the waypoints published and use them for trajectory optimization using the mav_trajectory_generation library, and then based on the trajectory, publish the desired state at time t to your controller.

- Hint: use
`vertex.addConstraint(POSITION, position)`

where position is of type`Eigen::Vector3d`

to enforce a waypoint position. - Hint: use
`vertex.addConstraint(ORIENTATION, yaw)`

where yaw is a double to enforce a waypoint yaw. - Hint: remember angle wraps around 2$\pi$. Be careful!
- Hint: for the ending waypoint for position use
`end_vertex.makeStartOrEnd`

as seen with the starting waypoint instead of`vertex.addConstraint`

as you would do for the other waypoints.

To run your code, use the following commands:

```
# terminal 1
# run the simulator
cd ~/vnav/tesse/lab4
./lab4.x86_64
# terminal 2
# launches the bridge between tesse and ros2
# NOTE: make sure you always start the bridge AFTER the simulator
ros2 launch tesse_ros_bridge tesse_quadrotor_bridge.launch.yaml
# terminal 3
# launches the controller_node and trajectory_generation_node (i.e., trajectory follower)
ros2 launch trajectory_generation traj_following.launch.py
# terminal 4
# launches traj_vertices_publisher node (i.e., waypoint publisher)
ros2 launch planner_pkg traj_gen.launch.py
```

If everything is working well, your drone should be gracefully going through each gate like this

**Note**: If the quadcopter flips over after launching the trajectory follower, press `r`

in the Unity window to respwan, the quadcopter should just go back to the starting position.

### Recording a Video and ROS2 Bag

Once you have your drone racing, you will need to submit a video and a ROS2 bag recording of your best trial.

To take a video, ideally use a screen recorder of your choice. If this isnβt working with your setup, you make take a **high quality video** of your screen with a phone.

To collect a ROS2 bag recording, use the following command (i.e., it only records the `/current_state`

and `/desired_state`

topics):

```
ros2 bag record /current_state /desired_state
```

Note that a ROS2 bag recording is a **folder** with a `metadata.yaml`

and `.db3`

file(s) β i.e., it is no longer a single `.bag`

files as in ROS1.

Upload the both the video and ROS2 bag folder to Google Drive or Dropbox, generate **two publicly viewable links**, and place them in a textfile called `links.txt`

within your `team-submissions/lab4`

folder.

### Submitting your code

Navigate to your submisison directory:

```
cd ~/vnav/team-submissions/lab4
```

Sanity check the correct files/folders are in the lab4 submission directory:

```
$ ls
links.txt planner_pkg trajectory_generation_pkg
```

Add these files to your submission repo, commit, and push:

```
git add links.txt
git add planner_pkg
git add trajectory_generation
git commit -m "YOUR MESSAGE HERE"
git push
```

Confirm the submitted files appear correctly in your teamβs submission repo.

### [Optional] Faster, faster (Extra credit: 15 pts)

How can you make the drone go faster? We will award extra credit to the fastest 3 teams! Current record is 10.3s, achieved by Nikhil Singhal, Fernando Herrera and Chris Chang in 2020.

### [Optional] Provide feedback (Extra credit: 3 pts)

This assignment involved a lot of code rewriting, so it is possible something is confusing or unclear. We want your feedback so we can improve this assignment (and the class overall) for future offerings. If you provide **actionable feedback** to the Google Form link, you can get up to 3 pts of extra credit. Feedback can be positive, negative, or something in between β the important thing is your feedback is **actionable** (i.e., βI didβt like Xβ is vague, hard to address, and thus not **actionable**).

The Google Form is pinned in the course Piazza (we do not want to post the link publically, which is why it is not provided here).

*Note that we cannot give extra points for anonymous feedback while maintaining your anonymity. That being said, you are still encouraged to provide anonymous feedback if youβd like!*