legged_control is an NMPC-WBC legged robot control stack and framework based on OCS2 and ros-control.
The advantage shows below:
I believe this framework can provide a high-performance and easy-to-use model-based baseline for the legged robot community.
The source code is hosted on GitHub: qiayuanliao/legged_control.
# Clone legged_control
git clone git@github.com:qiayuanliao/legged_control.git
OCS2 is a huge monorepo; DO NOT try to compile the whole repo. You only need to compile ocs2_legged_robot_ros
and
its dependencies following the step below.
# Clone OCS2
git clone git@github.com:leggedrobotics/ocs2.git
# Clone pinocchio
git clone --recurse-submodules https://github.com/leggedrobotics/pinocchio.git
# Clone hpp-fcl
git clone --recurse-submodules https://github.com/leggedrobotics/hpp-fcl.git
# Clone ocs2_robotic_assets
git clone https://github.com/leggedrobotics/ocs2_robotic_assets.git
# Install dependencies
sudo apt install liburdfdom-dev liboctomap-dev libassimp-dev
ocs2_legged_robot_ros
package with catkin tools
instead of catkin_make
. It will take you about ten minutes.
catkin config -DCMAKE_BUILD_TYPE=RelWithDebInfo
catkin build ocs2_legged_robot_ros ocs2_self_collision_visualization
Ensure you can command the ANYmal as shown in
the document and below.
Build the source code of legged_control
by:
catkin build legged_controllers legged_unitree_description
Build the simulation (DO NOT run on the onboard computer)
catkin build legged_gazebo
Build the hardware interface real robot. If you use your computer only for simulation, you DO NOT need to
compile legged_unitree_hw
(TODO: add a legged prefix to the package name)
catkin build legged_unitree_hw
export ROBOT_TYPE=a1
roslaunch legged_unitree_description empty_world.launch
Or on the robot hardware:
roslaunch legged_unitree_hw legged_unitree_hw.launch
roslaunch legged_controllers load_controller.launch cheater:=false
legged_controller
or legged_cheater_controller
, NOTE that you are not allowed to start
the legged_cheater_controller
in real hardware!rosservice call /controller_manager/switch_controller "start_controllers: ['controllers/legged_controller']
stop_controllers: ['']
strictness: 0
start_asap: false
timeout: 0.0"
Or, you can start the controller using rqt_controller_manager
GUI:
sudo apt install ros-noetic-rqt-controller-manager
rosrun rqt_controller_manager rqt_controller_manager
load_controller.launch
, then use RViz (you need to add what you want to display by
yourself) and control the robot by cmd_vel
and move_base_simple/goal
:The system framework diagram is shown below.
The main module of the entire control framework is NMPC and WBC, and the following is only a very brief introduction.
The NMPC part solves the following optimization problems at each cycle through the formulation and solving interfaces provided by OCS2:
$$ \begin{split} \begin{cases} \underset{\mathbf u(.)}{\min} \ \ \phi(\mathbf x(tI)) + \displaystyle \int{t_0}^{t_I} l(\mathbf x(t), \mathbf u(t), t) \, dt \ \text{s.t.} \ \ \mathbf x(t_0) = \mathbf x_0 \,\hspace{11.5em} \text{initial state} \ \ \ \ \ \ \dot{\mathbf x}(t) = \mathbf f(\mathbf x(t), \mathbf u(t), t) \hspace{7.5em} \text{system flow map} \ \ \ \ \ \ \mathbf g_1(\mathbf x(t), \mathbf u(t), t) = \mathbf{0} \hspace{8.5em} \text{state-input equality constraints} \ \ \ \ \ \ \mathbf g_2(\mathbf x(t), t) = \mathbf{0} \hspace{10.5em} \text{state-only equality constraints} \ \ \ \ \ \ \mathbf h(\mathbf x(t), \mathbf u(t), t) \geq \mathbf{0} \hspace{8.5em} \text{inequality constraints} \end{cases}\end{split} $$
For this framework, we defined system state $\mathbf{x}$ and input $\mathbf{u}$ as:
$$ \begin{equation} \mathbf{x}= [\mathbf{h}_{com}^T, \mathbf{q}_b^T, \mathbf{q}_j^T]^T, \mathbf{u} = [\mathbf{f}_c^T, \mathbf{v}_j^T]^T \end{equation} $$
where $\mathbf{h}_{com} \in \mathbb{R}^6$ is the collection of the normalized centroidal momentum, $\mathbf{q}=[\mathbf{q}_b^T, \mathbf{q}_j^T]^T$ is the generalized coordinate. $\mathbf{f}_c \in \mathbb{R}^{12}$ consists of contact forces at four contact points, i.e., four ground reaction forces of the foot. $\mathbf{q}_j$ and $\mathbf{v}_j$ are the joint positions and velocities. While the cost function is simply the quadratic cost of tracking the error of all states and the input, the system dynamics uses centroidal dynamics with the following constraints:
To solve this optimal control problem, a multiple shooting is formulated to transcribe the optimal control problem to a nonlinear program (NLP) problem, and the NLP problem is solved using Sequential Quadratic Programming (SQP). The QP subproblem is solved using HPIPM. For more details [2, 3]
WBC only considers the current moment. Several tasks are defined in the table above. Each task is the equality constraints or inequality constraints on decision variables. The decision variables of WBC are:
$$ \mathbf{x}_{wbc} = [\ddot{\mathbf{q}}^T, \mathbf{f}_c^T, \mathbf{\tau}^T]^T $$
where $\ddot{\mathbf{q}}$ is acceleration of generalized coordinate, $\mathbf{\tau}$ is the joint torque. The WBC solves the QP problem in the null space of the higher priority tasks' linear constraints and tries to minimize the slacking variables of inequality constraints. This approach can consider the full nonlinear rigid body dynamics and ensure strict hierarchy results. For more details [4].
People with ROS foundation should be able to run through simulation and real machine deployment within a few hours. The following shows some known laboratories that have run through this framework on their own A1 objects. and the time spent The table below shows the labs successfully deploying this repo in their real A1; feel free to open a PR to update it. (because the code they got at the time was not stable, so the spend time cannot represent the their level).
Lab | XPeng Robotics | Unitree | Hybrid Robotics |
---|---|---|---|
Spend Time | 1 day | - | 2 hours |
I recommended to use an external computing device such as NUC to run this control framework. The author uses the 11th generation of NUC, and the computing frequency of NMPC can be close to 200Hz.
Deploying this framework to your robot is very simple, the steps are as follows:
UnitreeHW
class in legged_examples/legged_unitree/legged_unitree_hw
, inherit LeggedHW
and implement the read()
and write()
functions of the hardware interface;[1] T. Flayols, A. Del Prete, P. Wensing, A. Mifsud, M. Benallegue, and O. Stasse, “Experimental evaluation of simple estimators for humanoid robots,” IEEE-RAS Int. Conf. Humanoid Robot., pp. 889–895, 2017, doi: 10.1109/HUMANOIDS.2017.8246977.
[2] J. P. Sleiman, F. Farshidian, M. V. Minniti, and M. Hutter, “A Unified MPC Framework for Whole-Body Dynamic Locomotion and Manipulation,” IEEE Robot. Autom. Lett., vol. 6, no. 3, pp. 4688–4695, 2021, doi: 10.1109/LRA.2021.3068908.
[3] R. Grandia, F. Jenelten, S. Yang, F. Farshidian, and M. Hutter, “Perceptive Locomotion through Nonlinear Model Predictive Control,” (submitted to) IEEE Trans. Robot., no. August, 2022, doi: 10.48550/arXiv.2208.08373.
[4] C. Dario Bellicoso, C. Gehring, J. Hwangbo, P. Fankhauser, and M. Hutter, “Perception-less terrain adaptation through whole body control and hierarchical optimization,” in IEEE-RAS International Conference on Humanoid Robots, 2016, pp. 558–564, doi: 10.1109/HUMANOIDS.2016.7803330.