# 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"