Torque-Controlled Stepping-Strategy Push Recovery: Design and Implementation on the iCub Humanoid Robot

TL;DR

This paper introduces a torque-controlled stepping strategy for push recovery in humanoid robots, specifically implemented on the iCub, enabling it to better react to external disturbances while maintaining environmental interaction capabilities.

Contribution

It presents a novel control strategy based on the Capture Point concept that extends iCub's balancing abilities to include stepping responses using torque control.

Findings

Successful simulation validation in Gazebo

Effective real-world testing on iCub

Enhanced push recovery performance

Abstract

One of the challenges for the robotics community is to deploy robots which can reliably operate in real world scenarios together with humans. A crucial requirement for legged robots is the capability to properly balance on their feet, rejecting external disturbances. iCub is a state-of-the-art humanoid robot which has only recently started to balance on its feet. While the current balancing controller has proved successful in various scenarios, it still misses the capability to properly react to strong pushes by taking steps. This paper goes in this direction. It proposes and implements a control strategy based on the Capture Point concept [1]. Instead of relying on position control, like most of Capture Point related approaches, the proposed strategy generates references for the momentum-based torque controller already implemented on the iCub, thus extending its capabilities to react to external disturbances, while retaining the advantages of torque control when interacting with the environment. Experiments in the Gazebo simulator and on the iCub humanoid robot validate the proposed strategy.

Equations

$${\ddot{x}}_{\text{CoM}}={\omega }_0^2(P_G{x}_{\text{CoM}}-x_{\text{CoP}})$$

$$x_{\text{cp}}=x_{\text{CoM}}+\frac{{\dot{x}}_{\text{CoM}}}{{\omega }_0}$$

$${\dot{x}}_{\text{cp}}={\omega }_0(x_{\text{cp}}-x_{\text{CoP}}).$$

$${\dot{x}}_{\text{cp}}={\omega }_0(x_{\text{cp}}-x_{\text{foot}}).$$

$$\underset{f}{\mathrm{minimize}}$$

$$Af\le b$$

$$\underset{\tau }{\mathrm{minimize}}$$

$$M(q)\dot{\nu }+h(q,\nu )-J^{\top }f=S\tau$$

$$\Vert x_{\text{cp}}-x_{\text{foot}}\Vert \le \Vert x_{\text{edge}}-x_{\text{foot}}\Vert$$

$$x_{\text{cp}}(t_{\text{step}})=e^{{\omega }_0{t}_{\text{step}}}\left(x_{\text{cp}}(0)-x_{\text{foot}}\right)+x_{\text{foot}}.$$

$$x_{\text{cp}}(t_{\text{step}})=e^{{\omega }_0(t_{\text{step}}-\bar{t})}\left(x_{\text{cp}}(\bar{t})-x_{\text{CoP}}(\bar{t})\right)+x_{\text{CoP}}(\bar{t}).$$

Full text

I INTRODUCTION

An open robotics research problem is to endow robots with the capabilities to reliably operate in an unstructured environment, performing a variety of tasks at close contact with humans. Indeed, robots must be able to move in the environment so as to accomplish their objectives.

The walking problem has been faced traditionally by resorting to simple models that approximate the predominant effects of the real systems. One of the most popular models is the Linear Inverted Pendulum, whose 3D version has been introduced in [2]. In particular, it has been applied to walking tasks [2, 3], or to predict when the robot is going to fall [4] and, also, within push recovery strategies.

Push recovery is a fundamental skill for a legged robot if it has to reliably operate in a real world scenario. It embeds the ability to exploit the contacts with the environment in order to maintain stability in an erect position. The robotics community have come up with many efficient controllers to achieve this goal, such as [5, 6, 7]. Sometimes, in case of severe disturbances, it may be necessary to take a step, in order to change the support configuration and reject the disturbance. This is what humans instinctively do.

In literature this problem is tackled by resorting to simple models so as to obtain uncomplicated conditions to decide when to trigger the step and where the foot should be placed. In the context of walking, the Linear Inverted Pendulum has been widely used [8, 9, 10]. The Capture Point framework [1, 11] has been used to stabilize walking patterns against external disturbances. The strategy consists in controlling the dynamics of this particular point by generating a trajectory for the Zero Moment Point (ZMP) [12]. This approach has been followed in [13, 14, 15] and implemented on position controlled robots. In [7] the same approach has been used to stabilize the Atlas humanoid robot walking. In [16] authors introduce a Capture Point control “task” in the stack-of-task control formulation as a low priority objective. The main motivation is to make the stack-of-task controller aware of the possibility to fall while reaching a desired target. Finally in [17] the Capture Point framework has been applied to the M2V2 force-controlled lower body humanoid robot.

This paper presents a control strategy based on the Instantaneous Capture Point concept to endow iCub with a stepping strategy. The iCub humanoid robot is a state-of-the-art $53$ degrees of freedom robot [18]. Recently, a momentum-based whole-body torque controller [5, 19] has been synthesized, allowing the robot to balance on both two feet and one foot while performing complex movements with the limbs. While this controller has proved successful in various balancing scenarios, it still misses the possibility to take a step in order to reject stronger disturbances, essential requirement for reliably operating together with humans. This work proposes a surge in this direction, generating references to be fed to the momentum controller while retaining the advantages of the torque control. Experiments in simulation on Gazebo and on the real robot validate the proposed control architecture.

II BACKGROUND

This section introduces the model used in this work, together with the concept of the Capture Point. In particular we describe a variation of the classic 3D Linear Inverted Pendulum model (LIP), namely the Linear Inverted Pendulum model with Finite-sized foot (FLIP) [11].

II-A Notation

Throughout the paper the term $\mathcal{I}$ denotes an inertial frame, with its $z$ axis pointing against the gravity, the $x$ axis points in front of the robot and with the origin placed on the ground level. We denote with $g$ the gravitational constant.

Given a time function $f(t)\in\mathbb{R}^{n}$ its first and second order time derivatives are denoted as $\dot{f}(t)$ and $\ddot{f}(t)$ respectively.

II-B Linear Inverted Pendulum with Finite-sized foot model

The Linear Inverted Pendulum with Finite-sized foot model is an extension of the more classic LIP where a foot with finite dimension is introduced. The model approximates the lower body of a legged robot as an inverted pendulum with the point mass coinciding with the robot center of mass, connected by a massless rod to a foot in rigid contact with the ground as shown in Figure 1. The foot possesses two degrees of freedom (DoFs), and thus can generate torques along the contact plane axes. By assuming the center of mass to remain at a constant height during the robot motion, the model becomes linear. It is thus possible to obtain the following equations of motion of the simplified model:

[TABLE]

where $P_{G}$ projects on the ground plane the position of the center of mass ${x}_{\text{CoM}}\in\mathbb{R}^{3}$ (w.r.t. $\mathcal{I}$). $\omega_{0}$ is the natural frequency of the pendulum, i.e. $\omega_{0}\mathrel{\mathop{\mathchar 58\relax}}=\sqrt{g/z_{0}}$, where $z_{0}$ is the initial CoM height. Finally $x_{\text{CoP}}$ is the position of the center of pressure [20] of the considered foot w.r.t $\mathcal{I}$.

II-C The Capture Point

The Capture Point is defined as the point on the ground where the foot must be placed in order to stop the mass in the vertical upright position. Mathematically, the Capture Point (CP), which can be found by considering the orbital energy of the pendulum [2], takes the following expression:

[TABLE]

and its dynamics is given by

[TABLE]

The above equation is valid for the FLIP model. If the simple 3D LIP model is used, Eq. (3) becomes:

[TABLE]

II-D Momentum-based whole-body torque control

The push recovery strategy proposed in this paper interfaces with the momentum-based whole-body torque control currently implemented on the iCub humanoid robot. In this section we thus briefly describe its main peculiarities and we refer the reader to [5, 19] for additional details.

The momentum-based balancing controller is a hierarchical controller composed of two control objectives. The first, and most priority objective, is the tracking of a desired robot momentum while the second is the stabilization of the zero dynamics. Denoting with $H=[H_{\text{lin}}^{\top},H_{\text{ang}}^{\top}]^{\top}\in\mathbb{R}^{6}$ the robot (linear and angular) momentum, its rate of change is obtained as $\dot{H}=\sum_{i=1}^{n_{c}}{}^{\text{CoM}}X_{i}f_{i}+m\bar{g}$. Here $f_{i}\in\mathbb{R}^{6}=[F_{i}^{\top},\mu_{i}\top]^{\top}$ is the $i$-th of the $n_{c}$ contact wrenches composed of the 3D force and moment, ${}^{\text{CoM}}X_{i}\in\mathbb{R}^{6\times 6}$ is the matrix transforming the corresponding wrench from the application frame to a frame attached to the center of mass with the same orientation of the inertial frame $\mathcal{I}$, $m$ is the robot total mass and $\bar{g}\in\mathbb{R}^{6}$ is the 6D gravity acceleration vector. By assuming as virtual control inputs the contact wrenches $f=[f_{1}^{\top},\cdots,f_{n_{c}}^{\top}]^{\top}$, it is possible to control the robot momentum by solving the following minimization problem:

[TABLE]

where the inequality constraint $Af\leq b$ represents friction cone, center of pressure and other constraints on the wrenches. The desired momentum rate of change is obtained by mean of a PI control law plus a feed-forward action [19].

The second objective is responsible for constraining the joint variables and avoid internal divergent behaviors. As before, we can specify a minimization problem also for this second task, i.e.

[TABLE]

Eq.(10a) describes the free-floating dynamics of the mechanical system, with $q=(q_{b},q_{j})$, $q_{b}\in\mathbb{R}^{3}\times SO(3)$ denotes the configuration of the floating base w.r.t. $\mathcal{I}$, $q_{j}\in\mathbb{R}^{n}$ denotes the configuration of the internal $n$ DoFs, $\nu$ is the velocity of the system, $J=[J_{1}^{\top},\cdots,J_{n_{c}}^{\top}]$ is the stack of the contact Jacobians and $S\in\mathbb{R}^{n+6\times n}$ is a selector matrix describing the underactuation pattern. Eq.(10a) is the constraint equation describing the kinematic constraints associated with the contacts. Eq.(10a), which resembles a PD plus gravity and contact wrenches compensation, plays the role of a desired joint torque reference where $h_{j}$ and $J^{(j)}$ denotes the joint space bias term and Jacobian respectively. Finally Eq.(10a) is the hierarchical constraint, i.e. it prevents the solution of this second problem from changing the optimum of Eq.(5).

Equations (10a) and (10a) together describes the dynamics of the constrained dynamical system. It is worth noting that when the constraint set changes, e.g. when the robot goes from two feet to one foot or vice versa, the constrained dynamics changes. The overall system is thus a hybrid system and the discrete state transitions should be handled accordingly. In the current implementation particular care has been taken in mitigating torque discontinuities happening during the transition.

Summarizing, from a functional point of view, the momentum-based controller takes as references a desired momentum trajectory, i.e. $\dot{H}^{r},H^{r}$, a desired joint configuration $q_{j}^{r}$ and the set of contact constraints. The generated torques are then applied directly as references for the low-level torque control.

III The Stepping Strategy

This section describes the control architecture necessary to implement the Stepping strategy on the iCub humanoid robot. In particular, the proposed strategy is composed of three main phases:

Step Trigger, 2. 2.

Foot Placement, 3. 3.

Reference Generation.

Peculiar to this implementation, and differently from most of the state-of-the-art approaches, is the fact that the resulting trajectories are fed to the momentum-based torque control currently implemented on the iCub robot (see Section II-D).

Figure 2 depicts a schematic of the control architecture.

III-A Step Trigger

The first element composing the stepping strategy is the so called “Step Trigger”. This component is responsible to detect when the internal torques are no longer sufficient to balance the robot and instead a change in the support configuration must be made, i.e. the robot must take a step.

In particular, we can define a “do not step” condition, i.e. a condition such that as long as it is satisfied there is no need to step, namely:

[TABLE]

where $x_{\text{edge}}\in\mathbb{R}^{3}$ basically defines the intersection on the foot contour of the line passing through $x_{\text{cp}}$ and $x_{\text{foot}}$. In other words, the step is triggered when the Capture Point exits the support polygon. This kind of trigger is derived from Eq.(3). When the CP exits the foot boundary, the CoP cannot follow it, thus leaving the Capture Point to exponentially diverge from the foot.

III-B Foot Placement

This second element is the core part of the control architecture, as it is responsible for choosing where the foot should be placed so as to avoid the robot falling. Indeed, the feet positions are not planned in advance and instead they should be computed online.

We start by considering for simplicity the dynamics of the Capture Point in the LIP model, i.e. Eq. (4). The solution to the differential equation, with initial conditions in $t=0$ is given by $x_{\text{cp}}(t)=e^{\omega_{0}t}\left(x_{\text{cp}}(0)-{x}_{\text{foot}}\right)+{x}_{\text{foot}}$. This equation describes the time evolution of the Capture Point, given the initial condition and considering that $x_{\text{foot}}$ remains constant. In order to reject the disturbance, the foot is placed taking into account the Capture Point dynamic. Hypothesizing that the time needed to perform a step is $t_{\text{step}}$ we want to position the foot in

[TABLE]

If the FLIP model is used instead, the dynamic equation to be solved is the one in Eq. (3). Differently from before, this equation is not trivially integrable as the CoP is a function of time itself. A classic approach, used for example in [11], is to use a constant “equivalent CoP”. Furthermore, a desired Capture Point trajectory is planned, and tracked by means of controlling directly the CoP itself.

Instead of keeping the desired foot location constant and tracking the desired Capture Point, we decide to continuously update the desired foot location by repeatedly initializing the solution at the current state of the robot. At a generic time instant $0\leq\bar{t}\leq t_{\text{step}}$ we integrate Eq. (3) with the further assumption that $x_{\text{CoP}}$ remains constant $\forall t\mathrel{\mathop{\mathchar 58\relax}}\bar{t}\leq t\leq t_{\text{step}}$. The solution to this differential equation computed in $t=t_{\text{step}}$ is given by the following equation

[TABLE]

The adoption of the CoP as a feedback variable, allows to have a hint about the capability of the underneath balancing controller in controlling the CoM dynamics and, consequently, the Capture Point dynamics. Its effects (CoP and CP motion) are taken into account inside Eq. 12, so that it may be possible to perform a step smaller than what predicted in the beginning. It is worth noting that this should be considered as the closest point to which the foot should be placed to restore from the push. After the impact, the momentum based controller is responsible of keeping the robot in an erect position on two feet.

Summarizing our approach, in $t=0$ we compute the desired foot location $x^{r}_{\text{foot}}\in\mathbb{R}^{3}$, corresponding to the position the Capture Point will have at $t=t_{\text{step}}$. At every controller period, i.e. every $10\mathrm{ms}$ in our implementation, we refine the previously computed quantity by reading the actual state of the robot. This allows us to increase the robustness of the strategy to modeling errors.

III-C Reference Generation

Most of the Capture Point-based implementations on real robots generate reference joint trajectories which are then tracked by the position controller. While this approach usually results in well-tracked trajectories, it is usually not sufficiently robust to environment discrepancies or additional external disturbances. Our recovery strategy, instead of applying the obtained control actions directly to the robot by means of position control, follows a different route. As Figure 2 shows, we choose to feed the generated actions to the momentum-based whole-body torque controller briefly described in Section II-D. It is thus necessary to coordinate the actions chosen by the step strategy with the ones taken by the low level balancing controller, which actually commands the robot.

To accomplish the aforementioned coordination objective we feed the momentum-based controller presented in Section II-D with a linear momentum reference $H_{\text{lin}}^{r},\dot{H}_{\text{lin}}^{r}$ and with a reference for the joint configuration $q_{j}^{r}$.

The joint configuration $q_{j}^{r}$ is obtained starting from the desired foot position as predicted by the Capture Point criterion and described in the previous section. The desired foot orientation can be chosen at will, therefore we decide to keep it parallel to the stance foot for the sake of simplicity. The foot pose is then transformed into desired joint positions by inverse kinematics [21]. This algorithm handles the redundancies by considering joint limits and picking the solution closest to current robot configuration. Notice that joint references may not be tracked because of the prioritized structure of the momentum controller presented in Section II-D. The center of mass trajectory, therefore, should be “compatible” with the desired joint configurations. For example, this trajectory may be the generated through forward kinematics given the computed $q_{j}^{r}$ opportunely smoothed [21] to avoid being heavily influenced by a moving foot reference. The desired linear momentum is then obtained by setting $H^{r}_{\text{lin}}=m\dot{x}_{\text{CoM}}^{r}.$ The correct definition of this trajectory is necessary to perform the step movement correctly. Indeed, the lack of “coordination” between the two controller tasks may lead to an erratic motion, causing the robot to fall. Finally, the adoption of a torque controller may allow to recover balance even if the foot positioning is imperfect, as the intrinsic compliance helps adapting to the ground surface.

IV Simulations and experimental results

This section presents the experimental results performed both in simulation and on the real iCub humanoid robot, that for the purpose of the proposed recovery strategy, is endowed with $23$ degrees of freedom. In both experiments the robot is controlled by the momentum-based torque controller described in Section II-D. The robot starts the experiment balancing on two feet and then moves on one foot balancing. At this point, the robot is pushed.

IV-A Experiments in Gazebo simulator

We first test the proposed step strategy in simulation using the Gazebo simulator [22] and the corresponding Yarp plugins [23].

A push is applied to the robot at $t=1.35\mathrm{s}$. Because the push is strong enough to bring the Capture Point out of the foot, as it can be seen in Figure 3, the condition described in Section III-A triggers. As a consequence, by using the FLIP model (see Section III-B), we generate the reference for the position of the right foot, according to Eq. (12). Figure 4 shows the behaviour of the Capture Point and the prediction given by Eq. (12) during the step, which will be used as a reference for the foot placement. It is worth noting that, close to the end of the step, the desired footstep reaches the Capture Point. The CoM position is represented in Figure 5, proving the capability of the robot to recover from the push.

IV-B Experiments on the iCub robot

The presented step recovery strategy has been implemented also on the iCub humanoid robot. The robot has been pushed at $t=1.6\mathrm{s}$, and, as a result, the Capture Point exits the support polygon at time $t=2.1\mathrm{s}$, as it can be seen from Figure 6. Differently from the simulation experiments, the foot reference position has been obtained using the 3D linear inverted pendulum model. This choice has been dictated by not precise measurements of the CoP. Furthermore, this model usually generates larger step references with respect to the FLIP model, thus making it less likely to underestimate the foot step length. An additional benefit of this simpler model is the constancy of the generated reference. Indeed, the predicted Capture Point at the push instant is given by Eq. (11), which depends only on the initial conditions and on the constant stance foot position. Because of the constancy of the reference, the inverse kinematics function is called only once. Given that it usually requires about $30\mathrm{ms}$ to yield its results, the performance of the controller benefits from this fact. The cartesian reference for the right foot is in Figure 7.

Figure 8 shows the center of mass trajectory. After the robot returns to double support, which occurs at $t=3\mathrm{s}$, the center of mass is properly stabilized in the middle of the two feet, thus proving the effectiveness of the recovery strategy.

When the foot impacts the ground, the force on the foot presents a discontinuity with an amplitude comparable to the weight of the robot itself. It is in this case that the torque control exhibits its advantages helping absorbing the impact through its intrinsic compliance, without the need to add it explicitly in the formulation.

V CONCLUSIONS

In this paper we presented a control architecture to achieve push recovery under strong disturbances by using the concept of the Capture Point. Differently from most of the state-of-the-art approaches, which control the robot in position, the obtained references are then fed to a momentum-based torque controller thus allowing us to retain the advantages of the torque control while interacting with the environment. Simulation results and experiments on the iCub humanoid robots validate the soundness of the proposed approach.

As a future work, multiple and possibly crossing steps will be employed, in order to face an even wider set of perturbations. In addition, as discussed in Section II-D the considered constrained dynamical system is a hybrid system as the dynamics changes depending on the contact configuration. A more thoughtful theoretical analysis about the mitigation of torque discontinuities during the state transitions should be performed. We intend to investigate this subject in the near future. Another important element we will work on regards the model complexity. Most of the current state-of-the-art approaches, including the Capture Point, relies on simplified and approximated models of the full free-floating dynamics of the robot. While these approximations allow us to perform simpler mathematical analysis and obtain control actions without expensive computations, we think that to operate in highly dynamic environments the full whole-body model should be exploited.