# <center>Advanced Topics for Robotics <br> Homework 1</center>

<center><font size=5>Instructor: Prof. Jianyu Chen</font></center>
<center><font size=5>March 25, 2023</font></center>

<!-- ## Instructions and Announcements
* There is one programming problem in this assignment.
* Before beginning this task, please be aware that you may need to configure a virtual machine and install an operating system to complete this assignment.
* Please record the result as a video and submit it together with your source code.
* **Collaboration policy.** Discussions with others are strongly encouraged. However, you should implement the idea on your own.
* If you have any questions about this assignment, please don't hesitate to reach out to Prof. Chen or the TA : ) -->

## Introduction
In this assignment, you will learn and practice setting up the robotics development environments and using a powerful open-source codebase to run **Model Predictive Control (MPC)** for a quadrupedal robot.

Specifically, you will first set up necessary development tools such as [**R**obot **O**perating **S**ystem (ROS)](https://www.ros.org/). ROS is a set of software libraries and tools that help you build robot applications, from drivers to state-of-the-art algorithms, and with powerful developer tools. For more details, please see [ROS tutorials](http://wiki.ros.org/ROS/Tutorials).

You will then install one of the state-of-the-art MPC solver called [**O**ptimal **C**ontrol for **S**witched **S**ystems (OCS2)](https://leggedrobotics.github.io/ocs2/). OCS2 is a C++ toolbox that provides an efficient implementation of several advanced MPC algorithms such as Sequential Quadratic Programming (SQP) and Differential Dynamic Programming (DDP).

To begin with, you need to follow the step-by-step instructions to install and configure the development environments as well as OCS2. Once you have completed the configuration, you will receive 60% of the homework marks.

After completing the configuration, you will need to modify a bit of the OCS2 code based on the given tips to enable the quadruped robot to execute two new gaits. This section will account for 40% of your mark for this assignment.

Please note:
* Before beginning this task, please be aware that you may need to configure a virtual machine and install an operating system to complete this assignment.
* **Collaboration policy.** Discussions with others are encouraged. However, you should implement the idea on your own.
* If you have any questions about this assignment, please don't hesitate to reach out to Prof. Chen or the TA : )

## Prerequisites
The following are the prerequisites for compiling and running OCS2:

* **Ubuntu 20.04**. We strongly recommend that you use the Ubuntu 20.04 operating system for this assignment. OCS2 is tested under Ubuntu 20.04 with library versions as provided in the package sources. If your computer's operating system is not Ubuntu 20.04, you can also use **[VMware](https://www.vmware.com/in.html)** to build a virtual machine. It is not recommended to use other common virtual machines, such as Parallel Desktop and VirtualBox, because the limitation of graphics memory may lead to poor rendering effects.
* **2-core processor (Intel/AMD CPU)**.
* The minimum requirement for the processor is 2-core, but we **highly recommend using 4-core processor**.
* **6GB RAM**.
* While the minimum requirement for RAM is 6GB, we **strongly suggest using 10GB RAM** for optimal performance.
* **30 GB of free hard disk space**.

You can download the desktop image for [Ubuntu 20.04](https://releases.ubuntu.com/20.04/) from: https://releases.ubuntu.com/20.04/ .
* If your operating system is Windows or Linux, you may consider using [VMware Workstation Player](https://www.vmware.com/in/products/workstation-player/workstation-player-evaluation.html) as your virtual machine.
* If your operating system is macOS, you may consider using [VMware Fusion](https://www.vmware.com/products/fusion.html) as your virtual machine.

## Dependencies
Please run `sudo apt update && sudo apt upgrade` in the command-line interface to ensure the source of each package.
* **C++ compiler with C++11 support** (Default package in Ubuntu 20.04).
* **Eigen (v3.3)** (Default package in Ubuntu 20.04).
* **Boost C++ (v1.71)** (Default package in Ubuntu 20.04).
* **ROS (Noetic)**. [Ubuntu install of ROS Noetic](https://wiki.ros.org/noetic/Installation/Ubuntu).
* **GLPK, URDFDOM, OCTOMAP, ASSIMP** `sudo apt install libglpk-dev liburdfdom-dev liboctomap-dev libassimp-dev`.
* **catkin**. (When you install ROS, it will be installed together.)
* **pybind11_catkin, ROS package**, installable via `sudo apt install ros-noetic-pybind11-catkin ros-noetic-grid-map-rviz-plugin`.
* **catkin-pkg package for python3**. Install with `sudo apt install python3-catkin-tools`.
* **Doxygen for documentation**. Install with `sudo apt install doxygen doxygen-latex`
* **Git**. Install with `sudo apt install git`.

If you find that using the `apt` command is slow, you can refer to https://mirrors.tuna.tsinghua.edu.cn/help/ubuntu/ to change the source of apt. However, this is not a necessary step.

## Installation and setup
1. Create a new catkin workspace:

# Create the directories
# Do not forget to change <...> parts

# You can use any <directory_to_ws> and <catkin_ws_name>. 
# In my personal experiment, 
# I choose `~/workstation`(without quotes) for <directory_to_ws>,
# and `hw1`(without quotes) for <catkin_ws_name>.

mkdir -p <directory_to_ws>/<catkin_ws_name>/src
cd <directory_to_ws>/<catkin_ws_name>/

# Initialize the catkin workspace
catkin init
catkin config --extend /opt/ros/noetic
catkin config -DCMAKE_BUILD_TYPE=RelWithDebInfo

***Catkin** is a build system used primarily within the Robot Operating System (ROS) ecosystem. It is designed to manage the compilation, packaging, and distribution of multiple interdependent software packages, or "packages," that make up a ROS project. Catkin streamlines the build process by handling dependencies, ensuring that packages are built in the correct order, and allowing for easy integration with other tools in the ROS ecosystem.*

2. Clone the OCS2 and other necessary libraries:

# Navigate to the directory of src
# Do not forget to change <...> parts
cd <directory_to_ws>/<catkin_ws_name>/src

# Clone OCS2, pinocchio, hpp-fcl, and ocs2_robotic_assets.
git clone https://github.com/leggedrobotics/ocs2.git
git clone --recurse-submodules https://github.com/leggedrobotics/pinocchio.git
git clone --recurse-submodules https://github.com/leggedrobotics/hpp-fcl.git
git clone https://github.com/leggedrobotics/ocs2_robotic_assets.git

*Software packages used in the project are placed in the `src` folder of the workspace.*

3. Remove packages that are not useful for this homework:

# Navigate to the directory of ocs2
# Do not forget to change <...> parts
cd <directory_to_ws>/<catkin_ws_name>/src/ocs2
rm -rf ocs2_mpcnet
rm -rf ocs2_raisim

4. Build and run the unit tests:

# Navigate to the directory of src
cd <directory_to_ws>/<catkin_ws_name>/src

# Build it
catkin build ocs2

# Source it
source <directory_to_ws>/<catkin_ws_name>/devel/setup.bash

# run tests
catkin run_tests ocs2

*The command `source` would load a configuration file for the bash. Note that each time you open a new terminal for running your code in your ROS workspace, you need to source the corresponding bash file in that workspace.*

If you encounter the "**C++: fatal error: Killed signal terminated program cc1plus**" error in this step, you can solve this issue by creating a swap partition as follows:

# Create partition paths.
sudo mkdir -p /var/cache/swap/

# Set the partition size.
# bs=64M is the block size, count=64 is the number of blocks,
# so the swap space size is bs*count=4096MB=4GB.
sudo dd if=/dev/zero of=/var/cache/swap/swap0 bs=64M count=64

# Set the directory permissions.
sudo chmod 0600 /var/cache/swap/swap0

# create SWAP file.
sudo mkswap /var/cache/swap/swap0

# Activate the SWAP file.
sudo swapon /var/cache/swap/swap0

# Check whether the SWAP information is correct.
sudo swapon -s

<!-- 5. Build the example for legged robots:

# Navigate to the directory of src
cd <directory_to_ws>/<catkin_ws_name>/src
catkin build ocs2_legged_robot_ros

5. Run the example for legged robots:

# Source workspace
# Do not forget to change <...> parts
source <directory_to_ws>/<catkin_ws_name>/devel/setup.bash

# Launch the example for SQP
roslaunch ocs2_legged_robot_ros legged_robot_sqp.launch

*`roslaunch` is a command-line tool in the Robot Operating System (ROS) that allows users to start and manage multiple ROS nodes and their configurations using a launch file. It simplifies the process of initializing complex robotic systems, making it easy to run and manage multiple nodes with their corresponding parameters and configurations in a coordinated manner.*

*The above launch file is placed in `<directory_to_ws>/<catkin_ws_name>/src/ocs2/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_sqp.launch`, which launches the corresponding software packages such as MPC controller, gait scheduler and visualization.*

And then, you can see a legged robot in the RViz window:

***RViz**, short for "ROS Visualization," is a 3D visualization tool for the Robot Operating System (ROS). It provides a user-friendly interface to display sensor data, robot model representations, and other relevant information in a 3D environment. RViz allows users to better understand the data collected by a robot and offers a way to visualize the robot's state, movements, and interactions with its environment.*

You should also see new terminal windows appear after you run the above `roslaunch` command. One terminal is for specifying the gait type, please type `trot` in this terminal. Another terminal is for specifying the destination for the robot to move to, please type `2 0 0 0` in this terminal. Please record a video for your result to be submitted. In this way, you can get 60% points for this homework. Feel free to play with this example by trying to enter any displacements and gaits in the terminals. You can also explore the codebase if you are interested.


## Design new gaits
You can simply modify `<directory_to_ws>/<catkin_ws_name>/src/ocs2/ocs2_robotic_examples/ocs2_legged_robot/config/command/gait.info` to design two new gaits for the quadruped robot.

When executing the **trot** gait, the quadruped robot's four feet will switch between the following two patterns:

* **LF_RH**: The left front (LF) and right hind (RH) feet are on the ground simultaneously, while the right front and left hind feet are not.
* **RF_LH**: The right front (RF) and left hind (LH) feet are on the ground simultaneously, while the left front and right hind feet are not.

You need to now design two new gaits, **galloping** and **jumping**, each also switches between two patterns, but the patterns are different from that of the **trot** gait.

For **galloping**, the two patterns are:
* The left front and right front feet are on the ground simultaneously, while the left hind and right hind feet are not.
* The left hind and right hind feet are on the ground simultaneously, while the left front and right front feet are not.

For **jumping**, the two patterns are:
* All four feet are on the ground simultaneously, which is a stance pattern.
* All four feet are in the air simultaneously, which is a fly pattern.

To complete it, you need to modify part 1 in the figure below and add some code similar to part 2.


Now, you are done with this assignment! Please remember to record the resulting video! Each gait will constitute a 20% score.

## Submit your homework
You need to pack the following four files into a zip file.
* `gait.info`: the file you modified.
* `trot.mp4/.mpg`: the video describes the result of running the example (`roslaunch ocs2_legged_robot_ros legged_robot_sqp.launch`) with the built-in trot gait.
* `gallop.mp4/.mpg`: the video to show the quadrupedal robot can do the galloping.
* `jumping.mp4/.mpg`: the video to show the quadrupedal robot can do the jumping.

Note the videos should be in mp4 or mpg format. Then please name your zip file with your student ID and submit it through Web Learning.