A Real-Time Planning and Control Framework for Robust and Dynamic Quadrupedal Locomotion

Legged locomotion poses significant challenges due to its nonlinear, underactuated and hybrid dynamic properties. These challenges are exacerbated by the high-speed motion and presence of aerial phases in dynamic legged locomotion, which highlights the requirement for online planning based on current states to cope with uncertainty and disturbances. This article proposes a real-time planning and control framework integrating motion planning and whole-body control. In the framework, the designed motion planner allows a wider body rotation range and fast reactive behaviors based on the 3-D single rigid body model. In addition, the combination of a Bézier curve based trajectory interpolator and a heuristic-based foothold planner helps generate continuous and smooth foot trajectories. The developed whole-body controller uses hierarchical quadratic optimization coupled with the full system dynamics, which ensures tasks are prioritized based on importance and joint commands are physically feasible. The performance of the framework is successfully validated in experiments with a torque-controlled quadrupedal robot for generating dynamic motions.


Introduction
Legged robots offer significant advantages over wheeled or tracked robots, as these advantages help traverse challenging terrain and unstructured environments. The ability to perform dynamic locomotion over unstructured terrain in unpredictable environments is extremely essential in real-world deployments. However, control of dynamic legged locomotion remains a challenging problem due to the difficulty in dynamical stabilization and the presence of unknown external disturbances. To achieve robust planning and control of dynamic locomotion, it is critical and necessary to adjust the motion plan according to the current robot state.
In recent years, some progresses have been achieved in dynamic legged locomotion with bipedal [1][2][3][4][5] and quadrupedal [6][7][8][9][10][11][12][13][14][15] robots. These solutions can be classified into optimization-based approaches [1,2,7,10,11] and learning-based approaches [4,12,13]. The optimization-based approach has been widely used, but it often needs to solve computationally expensive nonlinear optimization and suffers convergence issues, which makes them challenging to be implemented in real-time. The learning-based approach builds a neural network directly mapping sensor measurements to desired motor control signals but often requires high-quality training data and can not be adjusted partially. Compared with learning-based approaches that lack safety guarantees, the optimization-based approach is more suitable for the motion control of robots because it has the potential to take full-body dynamics and all physical constraints into consideration.
The optimization-based approach can be divided into trajectory optimization approaches [3,16,17] and predictive control approaches [7,10,11] according to whether they can be run online. The trajectory optimization approach solves the locomotion problem offline over the long-time horizon of a specific motion while optimizing over a large number of variables consisting of contact sequences, contact timings, contact locations and the whole-body trajectory. These variables can be obtained automatically while taking into account physical constraints such as full-body dynamics, joint motion limits, unilateral contact constraints, and friction cone constraints. Conversely, the predictive control approach emphasizes solving the locomotion problem online over the short-time horizon and correcting motions to cope with model mismatches and unpredictable disturbances.
In the last few years, the trajectory optimization approach has been widely used in legged locomotion to generate complex trajectories with whole-body dynamics to explore more possible behaviors, solving the locomotion problem over a long-time horizon at once [3,[16][17][18][19]. For example, a method has been developed for whole-body trajectory generation of multi-limbed robotic systems with unilateral constraints introduced by contact with the environment, which can eliminate the requirement for a priori contact mode ordering and satisfy all dynamic and contact constraints [16]. Although many impressive results are presented, the trajectory optimization approach is too slow to be applied online on the real robot because of the high computational cost of nonlinear optimization problems.
Instead, some works in the predictive control approach focus on the online motion generation and execution on the real robot [6,10,11,20]. For instance, The locomotion problem is formulated as a convex optimization by simplifying the robot dynamics and expressing the robot's orientation as Euler angles. While realizing the online planning of robot motions, some limitations are also introduced, such as the base cannot have a large range of roll and pitch motions, and the joint torque limits cannot be considered. A representation-free model predictive control framework is proposed to control various 3-D motions of a quadrupedal robot with the rotation matrix representation [11]. Since the rotation matrix is the natural representation of the special orthogonal group SO(3), representing orientation with the rotation matrix instead of Euler angles and quaternions can avoid the singularity and ambiguity issues [21,22]. Although the orientation limitation is removed, the joint torque limit still can not be imposed. The use of simplified models and the omission of important constraints may make the generated trajectories unrealizable in the real robot.
To combine the advantages of the trajectory optimization approach and predictive control approach, there are some methods [7,23,24] trying to solve the locomotion problem online with differential dynamic programming (DDP), considering the whole-body dynamics and other physical constraints. These DDP-like methods only optimize a local control policy rather than control-state trajectories by iteratively solving a low-order Taylor approximation of the nonlinear locomotion problem, which makes them computationally efficient, but also limits their applicability as state constraints cannot be easily incorporated.
Most of the above works use only one model, some use a reduced-order model [6,10,11,25], and some use a fullorder model [7,16,23,24,26]. Actually, the whole locomotion control problem can be decomposed into a motion planning module and a motion tracking module [27][28][29][30]. The latter serves as a trajectory stabilizer calculating joint commands to track the task-space and joint-space trajectory references generated by the motion planner compliantly. The decomposition offers the possibility to use different models in motion generation and execution.
Thanks to the decomposition, this paper proposes a realtime planning and control framework shown in Fig. 1 for robust, dynamic quadrupedal locomotion containing two main modules: motion planning and whole-body control. The motion planner generates task-space references such as base motions, center-of-mass (CoM) motions, feet contact positions and forces, as well as limb motion reference trajectories. The tracking controller can execute various tasks and accomplish multiple control objectives, accounting for whole-body dynamics, manipulating contact forces to Fig. 1 Planning and control framework. Body velocity commands are sent to the motion planning module, which computes desired body pose, foot positions, and ground reaction forces. The whole-body control module generates desired generalized accelerations and exter-nal forces by solving a hierarchical quadratic programming problem. Then the joint torque, velocity and position commands of each joint are computed from the acceleration and force control the floating base, and transforming task space references into joint-space commands. Although the gait scheduler and the state estimator are also included in the proposed framework, they will not be explained in detail here since they are out of the scope of this paper. Actually, many gait generation approaches [31,32] and body pose estimation methods [33][34][35][36] can be used to schedule gait and estimate the base state of quadrupedal robots.
The contributions of this research can be summarized below: 1. A real-time planning and control framework is developed for quadrupedal robots capable of performing robust and dynamic locomotion, mainly consisting of a motion planner and a whole-body controller. 2. The 3-D single rigid body model (3D-SRBM) achieves a balance of expressivity and simplicity in the planning phase, allowing to conduct the planning and tracking of base orientation on SO(3) manifold to realize a wider body rotation range while allowing the planner to run in a receding horizon fashion to cope with disturbances.

A trajectory interpolator based on the Bézier curve
is integrated with a heuristic-based foothold planner, which provides continuous and smooth Cartesian velocity and position of foot-ends simultaneously. 4. The developed whole-body controller uses hierarchical quadratic optimization coupled with the full system dynamics and physical constraints, which ensures tasks are prioritized based on importance, and joint commands sent to actuators are physically feasible. 5. The effectiveness of the framework is validated in realworld experiments on the A1 robot.
The rest of this paper is organized as follows. The following section presents the motion planner generating body trajectory commands, desired foot reference trajectories, and contact forces. Section 3 presents the whole-body controller formulating the control problem as a series of quadratic programs. Section 4 describes the control framework's use on the physical robot. Finally, Sect. 5 gives discussions and concludes the paper.

Motion Planning
The motion planner receives operator input and generates body motion reference, foot motion trajectories, and optional reaction force profiles. Planning is achieved via three components, detailed in the following sections and illustrated in Fig. 2. The body motion reference sub-module is responsible for generating desired body velocity and pose that are used in the foot placement planning and contact force optimization sub-modules with the scheduled gait and estimated robot state to plan foot positions and reaction forces.

3D-SRBM
The 3D-SRBM shown in Fig. 3 especially fits those robots with lightweight legs in which the robot base makes the largest contribution to the total mass of the whole robot. The underlying assumptions are: (1) their negligible limb masses do not contribute significantly to the momentum of the whole robotic system, and (2) the leg configuration does not influence the body inertia. This slightly more complex model approximates the robot as a single rigid body subject to external forces at the contact patches, thus it is still possible to consider the angular momentum in the planning phase even though it ignores limb dynamics.
The state of the 3D-SRBM can be collected into a tuple as Fig. 2 A block diagram illustrates the interaction between the body motion reference, foot placement planning, and contact force optimization submodules Fig. 3 3D-SRBM. Its CoM coincides with the CoM of the robot generated based on the mass and state of the whole robot. However, its orientation equals the base orientation of the robot, and its inertia is generated from a simplified robot body, where each leg is lumped to a point on the corresponding hip w h e r e p B ∈ ℝ 3 a n d ̇p B ∈ ℝ 3 a r e t h e C o M p o s i t i o n a n d ve l o c i t y, r e s p e c t i ve ly. R B ∈ SO(3) = R ∈ ℝ 3×3 |R T R = , det(R) = +1 is the body orientation evolving on the SO(3) manifold. det(⋅) calculates the determinant of a rotation matrix and indicates the 3 × 3 identity matrix. B B ∈ ℝ 3 represents the angular velocity of the robot body expressed in the body frame {B}.
Equations of motion of the 3D-SRBM are given by where m is the body mass; f i and a g are three dimensional vectors representing the contact force, and gravitational acceleration; B I ∈ ℝ 3×3 is the equivalent rotational inertia tensor expressed in the body frame {B} ; n is the number of contacts; r i is the location of i-th foot p i relative to the CoM of the robot p B , which is equivalent to the moment arm of the contact force; f i is the contact force of the i-th foot. By default, and unless otherwise stated, all of the above quantities are relative to the inertial frame {W}.
In the dynamics shown in Eqs. (2) and (3), r i is known when the foothold location p i is planned in advance. The contact force f i ∈ ℝ 3 is chosen as the system input. These contact forces create an external wrench where i ∈ {1, 2, 3, 4} is the foot index for the front left (FL), front right (FR), rear left (RL), and rear right (RR), respectively. f and B are the total force and torque, respectively; the hat operator ( ⋅) represents the cross-product operation.
Therefore, The dynamics of the rigid body Eqs. (2) and (3) can be rewritten as

Body Motion Reference
The body pose trajectory evolves on the special Euclidean SE(3) group. Although its motion can be divided as a translational motion in vector space and a rotational motion in special orthogonal group SO(3), it is still impossible to directly apply vector operations to the trajectory due to the special structure of SO(3) manifold. Unlike other representations such as the Euler angles and the unit quaternions, the natural nine parameters representation of the body orientation as an element of SO(3) is unique and non-singular. Therefore, it is more appropriate to use the rotation matrix representation.
The desired velocity V B,k = W B,k , W v B,k and pose X B,k = R B,k , p B,k are updated at every time step k. The above values are calculated with a reference velocity input V = cmd k+1 , v cmd k+1 expressed in the body coordinate from a complex path planner or a user-operated joystick. The desired body velocity and pose are updated as where Δt is the timestep, and the mapping function

Foot Placement Planning
The desired foothold location is determined by a foot placement planner designed based on the Raibert Heuristic [37] and Capture Point [38]. The main idea is to force the leg's landing angle to be the same as its leaving angle if the robot moves at the commanded speed. the location of foot i is calculated with the following equation: where p B is the body position, R z ( ) is the body orientation expressed as a rotation matrix, only the rotation along z-axis is considered. B l i is the position of hip i relative to the body position p B , T st is the stance time for each foot, v i is the velocity of hip i, v cmd i is the desired velocity of hip i which can be calculated by the body linear and angular velocity commands, and K represents a gain, usually taking the value 0.03.
Although the expected foothold location is given by Eq. (10), the foot still needs to lift off and touch down periodically to make the robot move. During the swing phase, when the foot lifts off in the air, the planned velocity and position of the foot should be smooth enough for the whole-body controller to track. The Bézier curve is chosen to generate the swing foot trajectory, for it just needs a set of discrete control points to define a smooth and continuous curve.
Given the start foot-end position p s i,k recorded when the foot lifts off, the desired foot-end position p e i,k calculated by Eq. (10), the phase signal i,k from the gait scheduler, and the maximum foot clearance h f , the quadratic Bézier curve that requires only three control points is naturally suitable for interpolating out the reference velocity and position of foot i in swing-phase at the k-th timestep. Figure 4 illustrates the diagram of generating swing foot trajectory with the quadratic Bézier curve. The conventional procedure to use the quadratic Bézier curve is to first determine the coordinates of the control point

Contact Force Optimization
The desired contact forces are calculated by an optimization problem making the robot follow the given body trajectory updated by equations from (6) to (9). To make the optimization convex, some approximations [6] can be made. To be specific, the system dynamics Eq. (5) and the contact friction cone constraint should be linearized.
The body orientation can be parametrized as Z-Y-X Euler angles = T , where , and are the roll, pitch and yaw angles, respectively. Therefore, the transformation from the body frame to the inertial frame can be decomposed as where R x ( ) , R y ( ) and R z ( ) are rotations of , and about the X-, Y-, and Z-axis, respectively.
With this parametrization, the relationship between the body angular velocity and the rate of change of Euler angles can be expressed as If roll and pitch angles are small and the body is not erected or inverted, the rate of Euler angles is (15)  It is worth noting that the rotation order is critical, with other order, the above approximation will not hold. If the angular velocity is small, the term × (I ) will not contribute significantly to the body orientation dynamics. Then, the orientation dynamics can be approximated with With small roll and pitch angles assumption, the inertia tensor and the orientation dynamics become After combining Eqs. (2), (19) and (20), the linearized robot dynamics can be expressed as where When the foot is in stance phase, a friction cone constraint must be imposed on the contact force f ∈ ℝ 3 as follows Since the friction cone constraint poses some computational challenges, a better option is to consider its conservative pyramid approximation an acceleration error cost, contact selection constraints and linearized friction cone constraints.
where Q and R represent diagonal weight matrices, and H is the friction pyramid matrix corresponding to friction cone constraints Eq. (26), and D is the contact selection matrix for swing feet.

Whole-Body Control
After the motion planner generates reference motions and reaction forces, the whole-body controller computes joint torque, velocity and position commands. Motion tracking is achieved through three submodules and the interaction between them is illustrated in Fig. 5. First, the task formulation sub-module formulates task-space tasks as linear functions of generalized accelerations and external forces and puts them in a task hierarchy. Then, the hierarchy is solved by the hierarchical optimization sub-module to get the optimal generalized accelerations and external forces. Finally, the desired joint torques is calculated from the optimal accelerations and forces in the hybrid joint controller sub-module.

System Underactuation
The quadruped robot shown in Fig. 6 belongs to the multibody dynamic system, and its equations of motion is relatively complicated, written as where q ∈ ℝ n q and v ∈ ℝ n v are generalized coordinates and velocities, and for those robotic systems with floating-bases, Fig. 5 A block diagram illustrates the interaction between the task formulation, hierarchical optimization, and hybrid joint controller submodules n q is different with n v . M(q) ∈ ℝ n v ×n v stands for the generalized inertia matrix of the robot, and h(q, v) ∈ ℝ n v is the nonlinear term (including Coriolis, centrifugal and gravity forces). S = 0 n j ×(n v −n j ) I n j ×n j is the selection matrix, representing the system under-actuation that the floating-base is not directly actuated by joint torques ∈ ℝ n j .
This decomposition largely reduces the number of optimization variables. As a result, it reduces solution time and allows the controller to run at a frequency of 500 Hz in real test.

Task Formulation
All robot tasks are often modelled in the form of a general task T expressed as constraints on the optimization variable z , including linear equality and/or inequality: where A ∈ ℝ m×(n v +3n c ) is the equality constraint matrix, b ∈ ℝ m is the equality constraint vector, C ∈ ℝ k×(n v +3n c ) is the inequality constraint matrix, and d ∈ ℝ k is the inequality constraint vector. s eq ∈ ℝ m and s in ∈ ℝ k are slack variables. It should be noted that, for a specific task, the above equality constraint and inequality constraint may exist together or independently.
(1) Dynamic consistency To ensure physical consistency, the underactuated part Eq. (30) can be expressed in the general task form Eq. (34) with which serves as the dynamic consistency constraint.
(2) Torque saturation limits To ensure the solved joint torques are physically consistent, the joint torque limitations should be considered. The torque limitation constraint can be formulated in the general task form Eq. (34) with where min and max are the lower and upper torque limits, respectively.
(3) Contact force limits When the point feet are in contact with the ground, friction cone constraints on the resulting reaction forces ∈ ℝ 3n c should be enforced. All contact force constraints for active contacts have been derived from Eq. (25) to Eq. (26), so the contact force limits constraint is expressed as (4) Contact motion task When the point feet are in stance phase, three constraint equations are introduced   6 Full body dynamic model. Blue represents task-space motion and force references, red indicates the full-body dynamics, and brown represents joint commands p i (t) = const . Then, the constraint is differentiated twice to generate the following: where J c i is the corresponding end-effector Jacobian. By stacking the above constraints for all active contacts together, the contact motion task can be formulated as (5) Contact force regularization task Given reference contact forces ref generated by Eq. (28), we can also directly penalize the optimization variable z from reference values, with This contact force regularization task is helpful for directly controlling specific contact forces. It also provides regularization on the optimization variable z to solve the QP problem faster. (7) Cartesian space orientation control task For the body with orientation R ∈ SO(3) and angular velocity ∈ ℝ 3 , the relationship between angular velocity in Cartesian space and the generalized velocity is which can be differentiated as (38)

Hierarchical Optimization
The general task Eq. (34) can be formulated as solving a QP problem: The equivalent QP problem is where Z = z T s T in T is the equivalent optimization variable, the Hessian matrix P and the gradient vector c can be calculated from P =Ā TĀ and c = −Ā Tb . The expressions of the augmented equality constraint matrix Ā , augmented equality (45) = J ori (q)̇v +J ori (q, v)v. (46) (47) min z,s eq ,s in constraint vector b , augmented inequality constraint matrix C , augmented inequality constraint vector d are For a set of tasks with priorities T 1 , … , T n , they can be solved under strict prioritization. For the task with priority p, it is defined as: The null space projector of this task is defined as: where {⋅} † is the singular value decomposition (SVD) based pseudo-inverse. The augmented task Jacobian Ā p is defined as: The null space projector of the augmented task Jacobian Ā p is An efficient recursive implementation is Considering tasks T 1 , T 2 , … , T p+1 , the augmented optional solution is where N 1 = N 1 and z 1 = z 1 . z * p+1 is solved from the optimization problem: After collecting z , s in together Z = z T s T in T and stacking inequality constraints to make the notation shorter, similar to the process from Eq. (48) to Eq. (49), the QP problem Eq. (56) becomes: where the Hessian matrix P and the gradient c can be calculated from P =Ā TĀ and c = −Ā T b with:

Hybrid Joint Controller
After solving a set of QP problems, the optimal generalized accelerations ̇v * can be obtained from Eqs. (56) and (33), and the optimal joint torques * can be calculated with Eq. (32). The desired joint velocity v des j and position q des j can be computed by adding the joint position and velocity portions to the measured joint velocity v j and position q j as The joint action q des j , K p,j , v des j , K d,j , * is sent the hybrid joint controller to compute motor torque commands as

Experiment
The proposed planning and control framework is validated through experiments on a quadrupedal robot A1 developed by Unitree Robotics. The robot has 18 degrees of freedom, a height of 0.4 m, and a weight of 14 kg. Four experiments were performed, namely body twisting, sharp turning, multi-gaited locomotion, and disturbance response. A video recording of the experiments is supported by the video submission.
The whole framework including gait generator, motion planner, whole-body controller and state estimator is executed on a dedicated computer with a frequency of 500 Hz. For modelling of kinematics and dynamics, the open-source library Pinocchio [39] is used, which is a C++ implementation of the modern rigid body algorithms for multibody systems [40]. To solve each QP problem numerically, a numerical optimization package OSQP [41] and its C++ interface OSQP-Eigen are used.
(61) = K p,j q des j − q j + K d,j q des j −q j + * .
All experiments use the same parameters, feedback gains, and task arrangements. The equivalent body property in Eq. (23) and feedback gains in Eq. (27) used in the motion planning module are collected in Tables 1 and 2, respectively. The task hierarchy used in the whole-body control module is summarized in Table 3. The hierarchy first maintains the stability of the robot and then plans movements to achieve task requirements. In detail, satisfying the dynamic constraints and hardware limitations has the highest priority. Then, satisfying the task enforces kinematic contact constraints as a second priority. After that, tasks tracking task-space motion and force references have the third priority. Last, the task of avoiding internal drift and keeping a better-looking posture has the lowest priority.

Body Twisting
To examine the body orientation tracking performance of the proposed framework, the body twisting experiment is conducted. Figure 7 presents video snapshots of the robot performing a body twisting motion. The robot is commanded to rotate its body in x-, y-and z-directions respectively while standing with all feet and its CoM position unchanged. The operator sends angular velocity commands, and then the motion planner calculates the reference body velocity and pose trajectories, which are tracked by a whole-body controller with equations from (6) to (9). Figure 8 shows the body orientation reference tracking data. Comparing Fig. 8a, c, e, it can be found that   Torque saturation limits 1 0 Contact force limits 1 1 Contact motion task 1 2 CoM position control task 1 2 Base orientation control task 0.50 2 Swing foot position control task 0.75 2 Contact force regularization task 0. 15 3 Minimum motion task 0.20 tracking errors in the x-direction are larger than those in the other two directions due to the asymmetry of the robot in the three directions. Also, the robot's internal frictional resistance in the x-direction is greater, making it more difficult to control. From Fig. 8b, d, f, it can be seen that the angular velocity tracking is more accurate, even when the angular velocity changes rapidly from -2 rad/s to +2 rad/s within 1 s. The current maximum angle tracking error is about 0.12 rad, which causes by the robot prioritising the stability and limiting the Base orientation control task. It can be improved by adjusting the weight of the Base orientation control task in Table 3 and feedback gains K ori p and K ori d in Eq. (46). However, we believe that prioritising the stability of the robot itself is more important in a mission. It is not cost-effective to sacrifice stability and safety for better performance. Also, although the base orientations are presented as Euler angles for intuitive understanding, they are presented as rotation matrices in the planning and control processes, which won't cause singularity and ambiguity issues.

Sharp Turning
A sharp turning experiment demonstrates the framework's ability to make the robot turn at a high angular velocity, which is essential when the robot traverses confined and crowded environments. Figure 9 presents video snapshots of the robot performing a sharp turning motion. In the experiment, the robot executes manoeuvres including moving forward, performing an S-Turn, and then moving straight again. During the whole process, the robot runs at a constant forward velocity of 0.8 m/s in the trot-run gait. Figure 10 presents the sharp turning experiment data. Figure 10a shows a comparison between the commanded yaw velocity and the actual yaw velocity, Fig. 10b shows the reference yaw angle generated by the motion planner and the actual yaw angle extracted by the state estimator. As can be seen from these two sub-pictures, the reference is nicely tracked and the robot can still withstand

Multi-gaited Locomotion
To demonstrate that the planning and control framework can stabilize basic locomotion gaits, the multi-gaited locomotion   Figure 11 shows the snapshots of all gaits during the experiment. The description of the gaits are shown below: Static-Walk only one leg is in the swing, and the robot is statically stable at all times. Trot-Walk the robot lifts diagonal legs at the same time at low speed, and exist time all four feet make contact with the ground. Trot the robot lifts diagonal legs at the same time at a higher speed, and only two legs make contact with the ground at any time. Trot-Run the robot lifts diagonal legs at the same time, and exist time when all four feet are in the air.
The difference between trot-walk, trot and trot-run gaits is the longer moment of suspension between each beat.
From the top and middle subplots in Fig. 12, different gaits are selected as the speed changes. This gait selection is consistent with the gait-changing behaviour of quadruped animals. From the bottom subplot in Fig. 12, the height trajectory shows a more dramatic height change as the speed of the gait increase because the longer aerial phase of all feet makes it harder to maintain the height of the body. For the same reason, in the trot-run gait, the aerial phase becomes the longest, where exists time none of the feet is making contact with the ground. This makes the robot unable to maintain body height and causes a lower mean value in the graph. Figure 13 shows the trajectory of the front left foot in x-z plane. From the left subplot, the step length increases and then decreases as its speed increases and decreases, which is enforced by the rule illustrated in Eq. (10). The right subplot shows the process of lifting foot height illustrated in Eqs. (15) and (16).
This experiment shows that the proposed framework is independent of gait type, and can be applied to different gaits. It also shows the stability of the framework between gait changes.

Disturbance Response
To evaluate the disturbance rejection capability of the control framework, robot A1 gets kicked from the side when it is trotting in place. Figure 14 presents the snapshots of this experiment. It can be seen that the robot can still remain balanced after strong kicks. Figure 15 shows the changes in CoM velocity and position over time in the presence of external disturbance. These disturbance forces cause a sudden change in the CoM velocity, especially in the y direction, and the increase in y direction velocity results in a side-stepping motion. Since the CoM position control task penalizes deviations from the desired and actual CoM positions, the robot will walk back to its original standing position. Figure 16 presents the y reaction forces and the y motion for a specific foot (front-left). Two recovery strategies are used when the robot reacts to the unpredictable external disturbance resulting from the first kicking. As  Fig. 16a, in the stance phase, the foot adjusts the ground reaction forces to resist the disturbance while satisfying in the constraint of the friction cone. This is realized by optimizing the desired contact forces in the motion planning module illustrated in Sect. 2.4, and tracking these forces via the Contact force regularization task in the whole-body control module. As shown in Fig. 16b, in the swing phase, it reacts to the external disturbance through a side-stepping motion which is realized by adjusting the foothold position according to the actual CoM velocity illustrated in Sect. 2.3. The difference between the reaction force generated by the motion planner and the whole body controller is due to the modelling of the robot being different, where the motion planner uses the 3D-SRBM and the whole body controller uses full-body dynamic model. For the same reason, the foot position planned by the motion Fig. 12 Graph showing the gait type, contact state, velocity and height of the real robot during the multi-gaited locomotion experiment. In the top plot, a coloured region indicates when the foot is in stance phase, while a white space shows that it is in the swing phase.
The middle and bottom plots compare the reference values generated by the motion planner and the actual values extracted from the state estimator  ting. In order to keep its balance, the robot step to the side. After the robot regain balance, it moves back to its initial position eventually

Conclusion
A real-time planning and control framework is proposed and deployed in a real robot, which enables the robot to perform various dynamic locomotion and be robust to external disturbance. The existing methods using the same model in the planning and control phases, where simplified models can not ensure physical consistency and full-order models make computationally expensive. To take advantage of both methods, this research employs both the 3D-SRBM and the full-body dynamic model to build a more comprehensive framework suitable for different tasks. The proposed control framework does not have a limitation in gait types and gait transitions. The experiment result proved the practicability of quadruped robots with the proposed framework in open environments and under disturbances. The above advantages are very beneficial to deploy quadruped robots in complex real environment to complete tasks. The immediate next step will be focused on extending the framework to generate base and foot trajectories that guide the motion of the robot over obstacles with a sequence of suitable footholds. To fully explore coupling effects between the base and limbs and provide sufficient flexibility to generate more complex whole-body motions, future work will replace the 3D-SRBM used in the motion planner with a mixed model combining the centroidal dynamics model and the whole-body kinematics model. Fig. 16 The front-left foot's contact force and position in the y direction, during the first kicking and its recovery period. The gray area indicates the process of kicking, and the orange area shows the process of walking back to its initial position. The red line shows the ref-erence contact force and foot position from the motion planner, while the cyan line shows the reference force from the whole-body controller, and the black line shows actual position from the state estimator