Edited by: Antonio Manuel Pascoal, Instituto Superior Técnico, Portugal
Reviewed by: Vincent Padois, Inria Bordeaux-Sud-Ouest Research Centre, France; Francesco Pierri, University of Basilicata, Italy
This article was submitted to Robotic Control Systems, a section of the journal Frontiers in Robotics and AI
This is an open-access article distributed under the terms of the Creative Commons Attribution License (CC BY). The use, distribution or reproduction in other forums is permitted, provided the original author(s) and the copyright owner(s) are credited and that the original publication in this journal is cited, in accordance with accepted academic practice. No use, distribution or reproduction is permitted which does not comply with these terms.
In the context of legged robotics, many criteria based on the control of the Center of Mass (CoM) have been developed to ensure a stable and safe robot locomotion. Defining a whole-body framework with the control of the CoM requires a planning strategy, often based on a specific type of gait and a reliable state-estimation. In a whole-body control approach, if the CoM task is not specified, the consequent redundancy can still be resolved by specifying a postural task that set references for all the joints. Therefore, the postural task can be exploited to keep a well-behaved, stable kinematic configuration. In this work, we propose a generic locomotion framework which is able to generate different kind of gaits, ranging from very dynamic gaits, such as the trot, to more static gaits like the crawl, without the need to plan the CoM trajectory. Consequently, the whole-body controller becomes planner-free and it does not require the estimation of the floating base state, which is often prone to drift. The framework is composed of a priority-based whole-body controller that works in synergy with a walking pattern generator. We show the effectiveness of the framework by presenting simulations on different types of simulated terrains, including rough terrain, using different quadruped platforms.
Over the last few years in the context of legged robots, a lot of effort has been devoted to designing controllers and planners for locomotion. However, most of the time these two elements are considered separately (Kalakrishnan et al.,
Priorities are a strategy to deal with conflicting tasks where some of them are more critical. This strategy ensures the achievement of high priority tasks at the expense of other tasks with lower priority. In robotics, hierarchical approaches based on priorities were originally introduced for inverse kinematics problems with the works of Whitney (
The figure shows the horizontal frame
Our approach builds on top of previous works (Hoffman et al.,
Order of priorities.
Contact task | 1 | |
Trunk orientation task | 2 | |
Postural task | 3 |
Nominal configuration used as reference for the postural task (values shown in radians).
To summarize, the contributions of the present work are:
A
an
The rest of this paper is structured as follows: in section 2 we describe the Hierarchical Whole-Body Operational Space formulation that we use to enforce priorities in our locomotion framework. In section 3 we present the walking pattern generator while section 4 discusses some details useful for the implementation of the whole-body framework on the real robot. In section 5 we present both simulation and preliminary experimental results, and we conclude with section 7.
In this section we introduce the formulation of the
We describe the configuration of our robotic system using
where the values of the first
The dynamic model of the floating-base system in contact with the environment is given by the following:
According to our parameterization,
where
In this work, we aim to simplify the gait generation and to make the whole-body controller independent, as much as possible, from the floating-base state estimation. In order to do so, the world frame
In our whole-body formulation we intend to consider generalized accelerations and contact forces as optimization variables. Therefore, a generic Cartesian (6D) acceleration should be expressed in terms of these variables as:
where
where
Considering the generic Cartesian acceleration task in (6), we can define the
In the contact task we set the reference acceleration to be zero (i.e., the feet in contact do not move). The task is defined w.r.t. the world frame
We consider another Cartesian task
Where
To track posture references, we define a postural task
defined just for the
where
and on the joint accelerations:
Notice that the limits in (11) are chosen to be feasible upper and lower bounds w.r.t. the limits in (10).
With all the
given a generic task
In addition, the regularization of the contact forces can be used to prevent the solver from generating a solution with unnecessarily high forces or to increase robustness. For instance, in Focchi et al. (
The iHQP problem in (13) is used to solve two different
We first introduce the stack
where the “/” symbol implies a null-space relation between the cost functions (
The second stack
where the + operator is used to sum cost functions.
As a matter of fact, strict hierarchies in
It is important to note that the presented approach does not rely on explicit control of the CoM of the robot. For stability purposes we instead rely on the postural task which acts in the final layer of
The outputs of the QP, used to solve the problem described in (14) and (15), are optimal joint accelerations
Block diagram of the locomotion framework. The user gives high-level velocity inputs to the foothold planner, that, on its behalf, is triggered by the gait scheduler. The swing trajectory generator computes the trajectories for the swinging feet, given the step length
It is worth pointing out that the role of the inertia matrix
As stated in the previous section, to be robust w.r.t. state estimation drift, our intention is to drop the dependency from the
However, Equation (7) for the contact, is a constraint
However, if we carefully inspect the structure of
To be able to control the height of the robot it is necessary to obtain an estimation of it. Differently from the
where the rotation matrix
In order to understand which are the active contacts, we rely on contact force estimation based on torque readings extracting the leg equation from (3)
where
The walking pattern generator receives desired twist commands for the base of the robot from an external source, such as an operator device or a high level planner
Each different gait can be simply defined as a timed sequence of footsteps. Therefore, given a type of gait, the role of the scheduler is to trigger the sequence of leg swings (see
The left side of the image shows the schedule of a trot gait, while the right side shows a crawl gait. Given the feet sequence, the scheduler triggers the lift-off events for the legs. The touch-down events instead, are triggered if a real contact is detected (
A state machine is associated to each leg to keep track of its state. The possible states and transitions are depicted in
Each leg starts in the
The foothold planner calculates the desired foothold coordinates (
Frames used by the foothold planner. Desired base twist and foothold positions are expressed in the horizontal frame. Each step is taken with respect to a “virtual foothold” that moves with the base and represents the nominal size of the stance of the gait cycle (e.g., when the desired twist is set to zero).
Top view of the robot with the geometrical explanation of the foot displacements computation: (1) starting from the desired linear
The linear part of mapping of the desired velocity command is:
which represents the displacement of the foot produced by a linear velocity command,
where
To keep the robot close to a preferred stance configuration, and avoid accumulation of errors, we compute the difference between a preferred
which is then summed to (21) to obtain the total foot displacement:
Note that in absence of base command velocities, the effect of the offset (22) is to align the feet to the virtual foothold configuration.
Finally we extract from
The swing is generated in the swing frame
The trajectory for the swing (expressed in the swing frame
where
where
with
While the timing of the lift-off is dictated by the scheduler, the touch-down is triggered haptically (Focchi et al.,
To transform the swing trajectories from Cartesian to joint space we use the Closed Loop Inverse Kinematics (CLIK) algorithm (Siciliano et al.,
where
where
where
In this section we present some implementation details and remarks on the final control scheme that we are employing on the real platform.
The swing task can be implemented as a Cartesian task or a joint task. From a theoretical point of view, a Cartesian space formulation is more sound because it allows us to set the gains in the same space the trajectories are defined. Conversely, a joint space formulation provides an anisotropic and tilted impedance ellipsoid at the feet, making the legs more compliant in a direction than in another depending on the leg configuration, even with a constant joint stiffness. However, in the implementation on the real HyQ platform, we found issues with the Cartesian implementation of the swing task. The reason is that the Jacobian matrix
To handle contact transitions, during stance and swing phases, we impose contact force constraints to switch between a maximum allowed value and zero. This allows to keep the size of the QP problem constant during the whole locomotion phase, which can be useful in a hard real-time implementation. To prevent torque discontinuities it is possible to implement a smooth unloading/loading by setting a time-varying upper bound on the contact force as in Focchi et al. (
The acceleration and the force limits (active only during the stance phase), are summarized in
Parameters used in the controller.
Hessian regularization factor | η | 1 |
Force Max X | 1,000 [N] | |
Force Max Y | 1,000 [N] | |
Force Max Z | 1,000 [N] | |
Force Min X | 1,000 [N] | |
Force Min Y | 1,000 [N] | |
Force Min Z | 20 [N] | |
Accel Max | 500 [ |
|
CLIK gain | 10 [1/ |
|
Swing frequency | 2 [ |
|
Step height | 0.1 [ |
To keep spurious contact estimations from triggering a premature touch-down in the leg's state machine, it is possible to disable the haptic contact detection during the swing up phase (half of the swing time). To detect the touchdown the threshold on the ground reaction forces is set to 50 N (see section 2.4).
The output of the whole-body controller is given as a desired torque to the low-level torque controller, in a cascade loop architecture. Both the whole-body controller and the low-level torque controller run at 1
To solve the stack
If a terrain estimation algorithm (Focchi et al.,
In this section we present some experiments to demonstrate the effectiveness of our whole-body framework for quadruped robots (see the accompanying
First row: snapshots of the locomotion simulations: ANYmal tracking a
We tested our approach on two different quadruped platforms (HyQ and ANYmal) of different sizes and weights. The porting to a different platform required only a slight tuning of the gains of the postural and of the trunk orientation tasks. In a first simulation performed with HyQ we show in the
Simulation—HyQ walking on flat terrain: The upper plots show the tracking of the base orientation (roll, pitch, and yaw) and of the base height. Note that the height does not attain its reference, because it is implemented via the postural task that is in the null-space of the orientation task. In the lower plots instead, the tracking of the left front and right hind foot in the
The ground truth coming from Gazebo is used to obtain the measurements in the world frame. In both cases, good tracking without steady errors is achieved; indeed the swing tasks and the base orientation task are not conflicting with each other because the latter is written in the horizontal frame which is independent from the base orientation. For completeness we present the mean and the standard deviation of the tracking errors during the whole experiment, in
Mean and standard deviation of the errors.
Roll | 0.0050 [rad] | 0.0145 [rad] |
Pitch | 0.0072 [rad] | 0.0187 [rad] |
Yaw | 0.0017 [rad] | 0.0043 [rad] |
Height | 0.0071 [m] | 0.0145 [m] |
RH–X | 0.0303 [m] | 0.0186 [m] |
RH–Y | 0.0203 [m] | 0.0128 [m] |
RH–Z | 0.0016 [m] | 0.0027 [m] |
LF–X | 0.0235 [m] | 0.0152 [m] |
LF–Y | 0.0202 [m] | 0.0201 [m] |
LF–Z | 0.0006 [m] | 0.0022 [m] |
We carried out preliminary experiments on the real platform HyQ showing a 2 Hz trot on flat terrain, in a second moment we control the base orientation to follow some operator desired reference commands given by mean of a joy-pad interface while an external disturbance acts on the robot (see
Real hardware—HyQ changing the base orientation in roll, pitch, and yaw.
Remarks: To achieve a successful implementation on the real robot we had to do some modification on the original formulation of the optimization problem, see section 4.
In this work we present a novel locomotion framework for quadrupedal robots that merges a walking pattern generator, acting only at the foot level, with a prioritized whole-body inverse dynamics controller. One of the advantages of the proposed framework is to avoid estimating the linear position and velocity of the floating base, while maintaining the ability to effectively tackle moderately rough terrain. This has been achieved by leveraging the postural task acting in the whole-body controller as a sort of elastic element. Consequently, the robot's base follows the feet, resulting in a motion of the trunk that adapts naturally to the foot stance configuration while trying to keep a well-behaved kinematic configuration. To increase the robustness of the proposed approach, the foothold selection is done w.r.t. a
The datasets generated for this study are available on request to the corresponding author.
Written informed consent was obtained from the individual(s) for the publication of any potentially identifiable images or data included in this article.
The Scientific contribution is equally distributed among GR, EM, and MF (33% each). CS and NT contributed with funding, lab space and provided the robot platform for experiments. All authors contributed to the article and approved the submitted version.
The authors declare that the research was conducted in the absence of any commercial or financial relationships that could be construed as a potential conflict of interest.
We would like to express our gratitude to Dr. Matteo Parigi Polverini and Dr. Arturo Laurenzi for their help and for their valuable suggestions during the preparation. The research leading to these results has received funding from the INAIL—Teleoperation Project.
The Supplementary Material for this article can be found online at:
Experiments.
This appendix will show how it is possible to relate inverse dynamics to Cartesian impedance control by opportunely selecting tasks gains.
Let us first consider a
with solution:
To neglect the inertia matrix, we just need to choose a particular gain for the
If we plug (A3) in (3), neglecting non-linear terms, we obtain:
obtaining the classic joint impedance control where the inertia matrix does not appear.
We now consider the optimization in (13) for the unconstrained case of a single, full rank, Cartesian task:
with
To solve (A5) we derive the Lagrangian:
With the hypothesis of
In this case, we can neglect the inertia imposing:
where
Notice that (A10) is equivalent to a Cartesian impedance controller (Ott,
Finally, let us consider the final level of a hierarchical controller. Again we consider the optimization in (13):
with
which leads to the following optimal conditions:
The final optimal accelerations are given by:
it is well known that is possible to achieve the dynamically consistent inverted Jacobian (Khatib,
We now plug (A3) and (A9) into (A15):
which plugged into (3) returns:
which again corresponds to a Cartesian impedance controller with dynamically consistent null-space projection (Ott,
1Here the term “stability” is intended in the sense defined by Pang and Trinkle (
2Priorities are achieved by projecting low priority task Jacobians by means of linear operators (i.e., the null-space projector of higher priority task Jacobians).
3The cost functions often quantify the error between a desired and a measured value and present it in the form of a squared euclidean norm.
4In particular we use Euler angles to represent the orientation of the base, so
5We assume our robot establishes
6Note that, in general, the size of the weight matrix
7Even in the case the CoM position is estimated via
8The orientation error can be computed from the quaternion error
9Note that this configuration works well for flat or moderate terrain inclinations and might pose stability issues for bigger inclinations; to improve robustness, it would be possible to make the robot lean forward when climbing up a ramp, as presented in Focchi et al. (
10This is a rule of “thumb” that works well in practice considering the tasks' normalized costs in SI units.
11In the case of the knee joint of HyQ, due to the low inertia of the link, the torque sensor barely measures any torque during the swing (when there is no contact), resulting in a open feedback loop. For further details see section 4.
12For the sake of simplicity, we discard the acceleration terms because their influence on the force computation is very low. However, in case the acceleration terms can not be neglected, they can be incorporated in the equation after filtering. The filtering is necessary due the presence of quantization noise, which would otherwise be amplified by the double differentiation of the encoder measurements.
13In our experiments we used a joy-pad connected to the operator's pc.
14Assuming smooth input changes.
15We remember that we computed ψ
16Both values can be expressed either in the world or horizontal frame since the two differ only for a rotation around the
17Load-cells and torque sensors in legged robots are sized in order to be able to measure big torques during the stance, for this reason they lack accuracy during the swing.
18Experiments:
19The controller can be tested at this repository: