---
# System prepended metadata

title: Multi-Task structure of locomotion control

---

# Multi-Task structure of locomotion control
After completed the hardware of my robot, I could work on locomotion control algorithms of legged-robot. However, the locomotion controller has to deal with not only the algorithms but also the hardware management; not to mention that I have not took any advantage of multi-task programming yet. To make the hardware and the control algorithms properly work together, I designed a control structure for my robot. The control structure is based on the "[Locomotion Controller Library for Legged Robots](https://github.com/ethz-asl/loco)" released by Autonomous Systems Lab, ETH Zurich. But I added some hardware management processes and make all the processes of my control structure work in multi-tasking of FreeRTOS.
## Locomotion control structures of leg robots
Several legged robots have been developed by three laboratories in ETH Zurich, MIT, and OSU. Although the legged robots have different hardware design, their control structures are very similar to each other. The control structures of the legged robots are revealed in the publications [1], [2], and [3]. According to the three publications, the control structures have four important processes related to locomotion control: (i)Gait Generator, (ii)IMU Estimator, (iii)Swing-Leg Control, and (iv)Stance-Leg Control. The Gait Generator process produces a sequence of leg states for a specific leg; the sequence could decide that the corresponding leg should be a swing-leg or a stance-leg. The IMU Estimator process takes advantage of sensor fusion and IMU data to deduce the estimated robot states, such as orientation and velocity. The Swing-Leg Control process is a trajectory generator for the desired foot trajectory of the swing-leg. The Stance-Leg Control process, in contrast with the Swing-Leg Control process, is a force controller for balancing the robot torso through proper leg forces.
## Locomotion control structure released by ETH Zurich
To dig into the locomotion control of legged robots, I studied all of the source code in the "Locomotion Controller Library for Legged Robots" released by ETH Zurich. The source code reveals the locomotion control structure and all the processes that constitute the control structure. I organized the control processes into the flow chart of the locomotion control. The control flow chart is shown below. 

```flow
st=>start: Start
e=>end: End
io1=>inputoutput: Get leg states feedback
op1=>operation: To next leg
c1(align-next=no)=>condition: All legs done?
io2=>inputoutput: Get torso states feedback
io3=>inputoutput: Get foot contact feedback
op2=>operation: To next leg
c2(align-next=no)=>condition: All legs done?
io4=>inputoutput: Get robot locomotion command
op3=>operation: Generate gait pattern
op4=>operation: To next leg
c3(align-next=no)=>condition: All legs done?
op5=>operation: Decide leg TouchDown/LiftOff
op6=>operation: To next leg
c4(align-next=no)=>condition: All legs done?
op7=>operation: Estimate terrain height
op8=>operation: Decide Swing-leg/Stance-leg
op9=>operation: To next leg
c5(align-next=no)=>condition: All legs done?
op10=>operation: Swing-leg control
op11=>operation: To next Swing-leg
c6(align-next=no)=>condition: All Swing-legs done?
op12=>operation: Stance-leg control
io5=>inputoutput: Set leg force/position command
op13=>operation: To next leg
c7(align-next=no)=>condition: All legs done?
c8(align-next=no)=>condition: Stop locomotion control?
op14=>operation: To next control loop

st->io1->c1
c1(yes)->io2->io3->c2
c1(no)->op1(top)->io1
c2(yes)->io4->op3->c3
c2(no)->op2(top)->io3
c3(yes)->op5->c4
c3(no)->op4(top)->op3
c4(yes)->op7->op8->c5
c4(no)->op6(top)->op5
c5(yes)->op10->c6
c5(no)->op9(top)->op8
c6(yes)->op12->io5->c7
c6(no)->op11(top)->op10
c7(yes)->c8
c7(no)->op13(top)->io5
c8(yes)->e
c8(no)->op14(right)->io1
```
The detailed implementation of all the processes shown in the flow chart could be found in the source files listed below.
|Control Process in the flow chart|Source File in the library|Implemented Function in the Source File|
|:--:|:--:|:--:|
|Get leg states feedback|LegStarlETH.cpp|LegStarlETH::advance(double dt)
|Get torso states feedback|TorsoStarlETH.cpp|TorsoStarlETH::advance(double dt)
|Get foot contact feedback|ContactDetectorConstantDuringStance.cpp|ContactDetectorConstantDuringStance::advance(double dt)
|Get robot locomotion command|MissionControlJoystick.cpp|MissionControlJoystick::advance(double dt)
|Generate gait pattern|GaitPatternFlightPhases.cpp|GaitPatternFlightPhases::advance(double dt)
|Decide leg TouchDown/LiftOff|EventDetector.cpp|EventDetector::advance(double dt, loco::LegGroup& legs)
|Estimate terrain height|TerrainPerceptionHorizontalPlane.cpp|TerrainPerceptionHorizontalPlane::advance(double dt)
|Decide Swing-leg/Stance-leg|LimbCoordinatorDynamicGait.cpp|LimbCoordinatorDynamicGait::advance(double dt)
|Swing-leg control|FootPlacementStrategyInvertedPendulum.cpp|FootPlacementStrategyInvertedPendulum::advance(double dt)
|Stance-leg control part 1|TorsoControlDynamicGait.cpp|TorsoControlDynamicGait::advance(double dt)
|Stance-leg control part 2|ContactForceDistribution.cpp|ContactForceDistribution::computeForceDistribution(const Force& virtualForceInBaseFrame, const Torque& virtualTorqueInBaseFrame)

## Design of Multi-Task locomotion control structure
Although the "Locomotion Controller Library for Legged Robots" implements all the necessary functions for locomotion control, the library dose not provide any interfaces (APIs or HAL) for robot hardware; users have to create a customized interface for their robot. I could have written a hardware interface for my robot and directly used the library, but I decided to design a multi-task locomotion control structure for my application. The major reasons for the decision are computing power and program efficiency. 

First, the library must be executed in a powerful CPU. An embed MCU, STM32F4 for example, could hardly run the library, because the control processes in the library are fully functional but complex. To run the library in the MCU, I have to simplify the control algorithms in the library. It might be easier for me to design a new control structure and write the corresponding control processes for my control structure.

Second, even if the MCU could run the library, the control structure of the library would restrict the possibility of multi-tasking the locomotion control. Without multi-tasking, the program efficiency of the locomotion control is limited, because the control program is forced to do nothing but wait for hardware communication, such as CAN_Bus communication with motor controllers and SPI communication with IMU.

The flow chart for my multi-task locomotion control structure is shown below. The orange arrows indicate "TaskNotification" in FreeRTOS. A TaskNotification could be sent to a task by another task or a hardware interrupt. The task that has received the TaskNotification would be executed by CPU, if the task has the highest priority. The red loop implies that the "Motor states manage Task" communicates successively with the motors in the robot until all the motors have been served.
```flow
st=>start: Start
e=>end: End
io1=>inputoutput: Get SPI_DMA IMU data
io2=>inputoutput: Set/Get CAN_Bus motor Command/Feedback
op1=>operation: Gait Generator Task
op2=>operation: Motor states manage Task
op3=>operation: IMU Estimator Task
op4=>operation: CAN_Bus_received_INT
op5=>operation: To next motor
op6=>operation: 1st leg Task
op7=>operation: 2nd/3rd/4th leg Task
op8=>operation: Torso Balancing Task(Stance-leg Control)
c1(align-next=no)=>condition: Done a leg?
para1=>parallel: Timer Task
para2=>parallel: SPI_DMA_received_INT
para3(align-next=no)=>parallel: Which leg?
c2=>condition: Stop locomotion control?
op9=>operation: To next control loop

st->para1
para1(path1, bottom)->io1->para2
para1(path2, right)->op1->op8
para2(path1, bottom)->io2->op4->op2->c1
para2(path2, right)->op3->op8
c1(yes)->para3
c1(no)->op5(top)->io2
para3(path1, bottom)->op6->op8
para3(path2, right)->op7->op8->c2
c2(yes, bottom)->e
c2(no)->op9(top)->para1
io2@>op4({"stroke":"Red","stroke-width":5})@>op2({"stroke":"Orange","stroke-width":6})@>c1({"stroke":"Red","stroke-width":5})@>op5({"stroke":"Red","stroke-width":5})@>io2({"stroke":"Red","stroke-width":5})
para3@>op6({"stroke":"Orange","stroke-width":5})
para3@>op7({"stroke":"Orange","stroke-width":5})
para2@>op3({"stroke":"Orange","stroke-width":5})
```
To implement the multi-task control, I divided my locomotion control into nine tasks with different priority. If a high-priority task is waiting for an I/O response, the task will yield the CPU execution to a low-priority task. After the CPU has received the I/O response, the high-priority task could be executed again. Since FreeRTOS is set as a preemptive kernel, the high-priority task can preempt the low-priority task and get the CPU execution immediately. 

Take the "Motor states manage Task" for example. When the Motor Task is waiting for the CAN_Bus feedback response from [the motor controller](https://hackmd.io/CW4pr80PTOm-1ogPz8uy_g), the CPU would execute another task, such as the "IMU Estimator Task" or the 1st~4th "leg Task". After the CPU has received the CAN_Bus response, the "CAN_Bus_received_INT" would send a TaskNotification to the Motor Task. Then, the Motor Task will preempt the executing task.

The nine tasks of the multi-task locomotion control structure are listed below.
|Task|Priority|Function|
|:--:|:--:|:--:|
|Timer Task|45 (high)|Change states and variables|
|Motor states manage Task (Motor Task)|40|Implement motor state machine|
|Gait Generator Task|36|Produce the sequences of leg states|
|IMU Estimator Task (IMU Task)|32|Sensor fusion for IMU|
|1st leg Task|29|Swing-Leg trajectory for the first leg|
|2nd leg Task|28|Swing-Leg trajectory for the second leg|
|3rd leg Task|27|Swing-Leg trajectory for the third leg|
|4th leg Task|26|Swing-Leg trajectory for the fourth leg|
|Torso Balancing Task|16 (low)|Balance the robot torso|
### Execution order of the multi-task locomotion control
The expected execution order of the multi-task locomotion control is shown below.

The "Timer Task" executes periodically. The period of the timer would depend on the complexity of the locomotion control; it should be long enough for the CPU to complete the locomotion control loop. In the Timer Task, the robot states and the command variables, which are set by [FreeRTOS+CLI](https://hackmd.io/@YehChiTing/SkqdLKv1I#1-Define-the-robot-states), must be refreshed to the latest values. Then, the values should remain unchanged during this control period. After refreshing the states and the variables, the Timer Task would start the SPI_DMA communication with IMU. Since the SPI_DMA communication might take some time and the Timer Task has finished execution, the CPU could start executing the Gait Generator Task.

The "Gait Generator Task" in my locomotion control structure would be similar to the "Generate gait pattern" in the "Locomotion Controller Library for Legged Robots". As I have mentioned above, the Gait Generator Task would produce a sequence of leg states for a specific leg; the sequence could decide that the corresponding leg should be a swing-leg or a stance-leg.

If the SPI_DMA communication has been completed and the "Gait Generator Task" has finished execution, the "IMU Estimator Task" will be executed. The sensor fusion algorithm in the IMU Task is based on Madgwick's algorithm.

The SPI_DMA_received_INT ISR not only sends a TaskNotification to the IMU Task but also starts the CAN_Bus communication with the motor controllers. As I have mentioned about the red loop in the contol flow chart, once the CAN_Bus communication has been completed, the "Motor states manage Task" would take place and serve all the motors successively. In the Motor Task, I will implement a state machine for the motors; such state machine could manage the motor states depending on the [feedback states](https://hackmd.io/1BK2hWV2TuWpaBAKtNuvgg#Feedback-interface) of the motor controllers. At the end of the Motor Task, if the feedback states of the served motors could fully describe a whole leg, the task will send a TaskNotification to the 1st/2nd/3rd/4th "leg Tasks". After that, the leg Task could be executed.

The 1st to 4th "leg Tasks" in my multi-task control structure will be made up of some control processes in the control structure released by ETH Zurich, such as (i)"Get leg states feedback", (ii)"Decide leg TouchDown/LiftOff", (iii)"Decide Swing-leg/Stance-leg", and (iv)"Swing-leg control". I will rewrite those processes to make the "leg Task" more efficient and compatible with STM32F4.

After all the "leg Tasks" have finished execution, the "Torso Balancing Task" with the lowest priority would be executed. I will use impedance control for the "Torso Balancing Task" because of the limited computing power of STM32F4.

## Reference
[1] Gehring, Christian "Planning and Control for Agile Quadruped Robots"
[2] Gerardo Bledt, Matthew J. Powell, Benjamin Katz, Jared Di Carlo, Patrick M. Wensing, and Sangbae Kim "MIT Cheetah 3: Design and Control of a Robust, Dynamic Quadruped Robot"
[3] Christian Hubicki, Andy Abate, Patrick Clary, Siavash Rezazadeh, Mikhail Jones, Andrew Peekema, Johnathan Van Why, Ryan Domres, Albert Wu, William Martin, Hartmut Geyer, and Jonathan Hurst "Walking and Running with Passive Compliance: Lessons from Engineering a Live Demonstration of the ATRIAS Biped"