High Level Control for Go1

# Overview
This page describes how we control Go1 with high level control mode using unitree_ros_to_real package.
You need to install necessary packages and setup your environment beforehand. ([Reference](https://qiita.com/coder-penguin/items/6e1e66b36a2fd2fd20c6))
Here is  contents of this page.

- [How to send command](#How-to-send-command)
- [Commands for high level control](#Commands-for-high-level-control)
  - [Available parameters for each mode](##Available-parameters-for-each-mode)

# Environment
- Ubuntu 18.04
- unitree_legged_sdk v3.5.1 (v3.8.0)
- unitree_ros_to_real v3.5.0
- unitree_ros (version is not specified. installed on June 29, 2022)

# How to send command
The image below illustrates how to send a command to Go1 using the example codes in unitree_ros_to_real, which are `ros_udp.cpp` and `example_walk.cpp`.
- You can send a command by running the ROS nodes with the following commands as described in [the official document](https://github.com/unitreerobotics/unitree_ros_to_real):
  In a terminal window, execute
  ```console
  roslaunch unitree_legged_real real.launch ctrl_level:=highlevel
  ```
  This runs `ros_udp` with the high level control mode.
  Then, execute 
  ```console
  rosrun unitree_legged_real example_walk
  ```
  in another terminal window to run `example_walk`.

- In `example_walk.cpp`, parameters for the high level control are set to a ROS message, which is defined as `HighCmd` in unitree_legged_msgs package.
- It is sent to a subscriber in `ros_udp`.
- Then, it is converted into a command formatted for unitree_legged_sdk using `rosMsg2Cmd` function defined in `convert.h`.
- Finally, the command is sent to Go1 via UDP.

![HighCmd.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2699595/40a80b6b-c15e-17ec-5039-ec7e8398c752.png)

[Picture of Go1](https://www.unitree.com/products/go1/)


# Commands for high level control
All of the commands for the high level control are listed below, which are defined in `unitree_legged_sdk/include/unitree_legged_sdk/comm.h`.
- `std::array<uint8_t, 2> head`
  *   Not understood yet
  *   The following parameters are set in `example_walk.cpp`.
  ```c++ :unitree_ros_to_real/unitree_legged_real/src/exe/example_walk.cpp
     high_cmd_ros.head[0] = 0xFE;
     high_cmd_ros.head[1] = 0xEF;
  ```

- `uint8_t levelFlag`
  *   Flag to switch low/high level control.
  *   Its entry can be `HIGHLEVEL` or `LOWLEVEL`.
  *   Here, we assume it is always set to be `HIGHLEVEL` to use high level control.
  *   default: 

- `uint8_t frameReserve`
  *   Not understood yet
  *   default: 

- `std::array<uint32_t, 2> SN`
  *   Not understood yet
  *   default: 0

- `std::array<uint32_t, 2> version`
  *   Not understood yet
  *   default: 0

- `uint16_t bandWidth`
  *   Not understood yet
  *   default: 0

- `uint8_t mode`
  *   Control mode. See the sub-section for detailed explanations.
  *   default: 

- `uint8_t gaitType`
  * How to walk.
     -  0: idle
     -  1: trot
     -  2: trot running
     -  3: climb stair 
     -  4: trot obstacle
  *   default: 

- `uint8_t speedLevel`
  * Walking speed. Available only if MODE = 3.
     -  0: default low speed
     -  1: medium speed
     -  2: high speed
  *   default: 

- `float footRaiseHeight`
  *   Foot height in meter when it is raised up.
  *   default: 0

- `float bodyHeight`
  *   Body height in meter.
  *   default: 0

- `std::array<float, 2> position`
  *   Position in meter to move on from the current position. Available only if MODE = 3.
  *   default: {0, 0}

- `std::array<float, 3> euler`
  *   Rotation angle of the pose in radian.
  *   {yaw, pitch, roll}
  *   default: {0, 0, 0}

- `std::array<float, 2> velocity`
  *   Speed to move on in meter per second ranging from -1 to +1.
  *   {forward-backward, rightward-leftward}
  *   default: {0, 0}

- `float yawSpeed`
  *   Yaw rotation speed in radian per second.
  *   default: 0

- `BmsCmd bms`
  *   Holder of a BmsCmd object including `uint8_t off` and `std::array<uint8_t, 3> reserve`.
  *   default: 

- `std::array<LED, 4> led`
  *   LED settings.
  *   default: {}

- `std::array<uint8_t, 40> wirelessRemote`
  *   Used to store joystick controller commands.
  *   Nothing happened when I tried to control the dog by setting some values equivalent to joystick controller commands though the same parameter in HighState was changed when buttons of the joystick controller were pushed.
  *   Used in `unitree_legged_sdk/examples/example_wirelessHandle.cpp`.
  *   default: {}

- `uint32_t reserve`
  *   Not understood yet.
  *   default: 0

- `uint32_t crc`
  *   Not understood yet.
  *   default: 0


## Available parameters for each mode
In this section, the modes are explained in detail and available parameters for each mode are listed.

### Mode = 0
Idle, default stand.

### Mode = 1
Force stand.
- bodyHeight: height of the body while standing.
- euler: pose of the body while standing.

### Mode = 2
Walk at specified velocity. The dog continues to walk until it is told to stop, i.e. mode = 0, for example.
- gaitType: gait type while walking. Need to be 1-4.
- bodyHeight: height of the body while walking.
- footRaiseHeight: height of the foot. Only for gaitType = 1, 2 or 4.
- velocity: forward and rightward velocity while walking.
- yawSpeed: speed of yaw rotation while walking.

### Mode = 3
Walk toward specified position. Firstly, the dog rotates euler[0] radian, and then moves toward the target position.
The dog may not walk correctly since unitree says this mode is not ready.

- gaitType: gait type while walking. Need to be 2.
- speedLevel: speed while walking. 0-2 for low to high.
- position: forward and rightward target position.
- euler[0]: yaw rotation angle.

### Mode = 4
Path mode walking. This is not implemented yet.

### Mode = 5
Stand down at the current position.

### Mode = 6
Stand up at the current position, which is equivalent to `L + A`.
Sometimes the dog doesn't walk as told possibly due to a failure to change its posture. In such a case, it is useful to correct its posture with this mode.

### Mode = 7
Damping at the current position, which is equivalent to `L + B`. This mode is suitable for software emergency. The dog refuses any commands from UDP as well as the joystick controller unless its mode is turned to mode = 6.

### Mode = 8
Recovery stand at the current position.

### Mode = 9
Back flip.

### Mode = 10
Yaw 90 degree rotation with a jump.

### Mode = 11
Put upward its hands like praying.

### Mode = 12, 13
Dance.