Log In Sign Up

Jerk Control of Floating Base Systems with Contact-Stable Parametrised Force Feedback

by   Ahmad Gazar, et al.

Nonlinear controllers for floating base systems in contact with the environment are often framed as quadratic programming (QP) optimization problems. Common drawbacks of such QP based controllers are: the friction cone constraints are approximated with a set of linear inequalities; the control input often experiences discontinuities; no force feedback from Force/Torque (FT) sensors installed on the robot is taken into account. This paper attempts at addressing these limitations through the design of jerk controllers. These controllers assume the rate-of-change of the joint torques as control input, and exploit the system position, velocity, accelerations, and contact wrenches as measurable quantities. The key ingredient of the presented approach is a one-to-one correspondence between free variables and the manifold defined by the contact stability constraints. This parametrisation allows us to transform the underlying constrained optimisation problems into one that is unconstrained. Then, we propose a jerk control framework that exploits the proposed parametrisation and uses FT measurements in the control loop. Furthermore, we present Lyapunov stable controllers for the system momentum in the jerk control framework. The approach is validated with simulations and experiments using the iCub humanoid robot.


page 1

page 3

page 9


Computationally-Robust and Efficient Prioritized Whole-Body Controller with Contact Constraints

In this paper, we devise methods for the multi- objective control of hum...

Stabilization of Complementarity Systems via Contact-Aware Controllers

We propose a framework for provably stable local control of multi-contac...

A Learning from Demonstration Approach fusing Torque Controllers

Torque controllers have become commonplace in the new generation of robo...

CASCLIK: CasADi-Based Closed-Loop Inverse Kinematics

A Python module for rapid prototyping of constraint-based closed-loop in...

Impact-Aware Task-Space Quadratic-Programming Control

Generating on-purpose impacts with rigid robots is challenging as they m...

Contact-Aware Controller Design for Complementarity Systems

While many robotic tasks, like manipulation and locomotion, are fundamen...

I Introduction

Nonlinear feedback control of fixed-based (e.g. manipulators) and floating-base (e.g. humanoids) robots is not new to the Control community [1, 2, 3, 4]. Feedback-linearisation, robust control, and adaptive laws are only few examples of the large variety of control methods developed for steering these systems towards desired quantities. When fixed and floating-base systems make contact with the environment, the robot control has to deal with also regulating the forces and torques towards values that ensure a desired interaction. This paper contributes towards the stabilisation of floating-base systems in contact with the environment by proposing controllers that ensure contact stability (see, e.g., [5]) while including feedback force measurements into the control laws. The proposed approach exploits the rate-of-change of the joint torques as control input, and for this reason it is here referred to as jerk control.

Force control strategies for fixed-base systems can be roughly divided into two categories: direct and indirect force control methods [1, 6]. Indirect methods achieve compliance without explicitly closing a feedback loop on the measured contact forces. In particular, stiffness and impedance control are two common objectives for indirect techniques. Stiffness control generates a desired dynamic behavior of the robot end-effector while interacting with an unknown external wrench. Impedance control makes use of the force/torque measurements as a feedforward term to achieve full feedback linearization of the end-effector dynamics [1]. On the other hand, direct force control methods include explicit feedback from the measured interaction wrenches, usually related to the force error [6, 7]. An example of these techniques is the hybrid position/force control, which is often applied when the environment is rigid and the end-effector has to continuously maintain a contact. The rigid environment assumption enables the decomposition of the end-effector dynamics into a constrained and a free direction [8]. Along the constrained directions, feasible desired forces are exerted. Here, additional feedback terms are added to ensure convergence in presence of external disturbances and unmodeled dynamics. Although the desired force satisfies the contact stability constraints, the commanded force, which includes the feedback terms from Force/Torque (FT) measurements, may instantaneously violate the contact constraints.

The recent research effort on humanoid robots gave impetus to the force control of floating-base systems [9, 10, 11, 12]

. These systems are often underactuated, namely, the number of control inputs is fewer than the system’s degrees of freedom 

[13]. The lack of actuation is usually circumvented by means of rigid contacts between the robot and the environment, but this requires close attention to the forces the robot exerts at the contact locations. If not regulated appropriately, uncontrolled contact forces may break the contact, and the robot control becomes critical [10, 11]. Contacts between floating-base systems and the environment are often assumed to be rigid and planar, e.g. the constraints applied to a humanoid robot feet during balancing and/or walking. Furthermore, all contacts are also unilateral, being the robot not physically attached to ground and in general able to activate/deactivate contacts. Contact activation and deactivation occur continuously, e.g. in case of humanoid robot walking, and can be addressed with the design of a proper state machine that plans references for the balancing/walking controller [14, 15].

From the control design perspective, a common strategy for floating-base systems is based on the so called stack-of-task approach [16]. These strategies usually consider several control objectives organized in a hierarchical or weighted prioritization. Often, the high-priority task is the stabilization of the robot momentum [17] : the objective of this task is the control of the robot center-of-mass and angular momentum by means of contact forces, while guaranteeing stable zero-dynamics [18] . Quadratic programming (QP) solvers can be used to monitor contact forces for ensuring both robot and contact stability [19].

Analogously to the aforementioned direct and indirect force controllers, QP based force control of floating base systems usually suffers from the following limitations, which are listed below in a crescendo of complexity:  friction cones for ensuring contact stability are approximated (and shrunk) to a set of linear inequalities, thus reducing the search space of feasible control inputs; optimal control inputs may be discontinuous in certain cases. Although further (and often numerous, which complexifies the associated optimisation problem) constraints can be added to the QP solver to enforce continuity, the effectiveness of this approach is not always satisfactory in practice; force feedback from FT measurements is missing in the control action.

This paper proposes a control approach that attempts at addressing these three main limitations of QP based controllers for floating base systems. The key ingredients of our approach are: propose an invertible one-to-one mapping between a set of constraint-free variables and contact wrenches that always satisfy the contact stability constraints; propose controllers that use this wrench parametrisation and exploit the rate-of-change of the joint torques as control input. We also propose extensions of the approach to deal with the cases where the joint torques (and not their rate-of-change) is the available control input. The proposed controllers exploit a relative degree augmentation of the underlying nonlinear system, i.e., the system state is composed of the system position, velocity, and acceleration. For this reason, we refer to the proposed approach as jerk control, which also contains force feedback information from FT measurements. Furthermore, we also present control laws that aim at stabilising a desired robot momentum and for which Lyapunov based stability properties can be shown. The proposed approach is validated with simulations and experiments on the humanoid robot iCub.

The paper is organized as follows. Section II recalls the notation, the robot modeling, the contact stability constraints, and introduces the problem statement. Section III presents a novel contact-stable parametrization of the contact wrenches. Section IV introduces the main idea behind jerk control using the contact-stable parametrisation. Section V presents Lyapunov stable jerk controllers when the control objective is the stabilisation of the robot momentum. Section VI presents validations of the approach on the humanoid robot iCub. Conclusions and perspectives conclude the paper.

Ii Background

Ii-a Notation

  • denotes an inertial frame, with its axis pointing against the gravity. indicates a frame attached to the robot’s base link.

  • The constant represents the total mass of the robot, and is the norm of the gravitational acceleration.

  • Given a matrix , we denote with its Moore-Penrose pseudoinverse.

  • is the canonical vector, consisting of all zeros but the

    -th component that is equal to one.

  • Let

    be the skew-symmetric matrix such that

    , where is the cross product operator in .

Ii-B Robot Modeling

The robot is modeled as a multi-body system composed of rigid bodies, called links, connected by joints with one degree of freedom each. We also assume that none of the links has an a priori constant pose with respect to an inertial frame, i.e. the system is free floating.

The robot configuration space is the Lie group and it is characterized by the pose (position and orientation) of a base frame attached to the robot’s base link, and the joint positions. An element can be defined as the following triplet: where denotes the origin of the base frame with respect to the inertial frame, is the rotation matrix representing the orientation of the base frame, and is the joint configuration characterizing the shape of the robot. The velocity of the multi-body system can be characterized by the algebra of . We here choose to represent the velocity of the multi-body system by the set , where an element of is a triplet , and is the angular velocity of the base frame expressed w.r.t. the inertial frame, i.e. . A more detailed description of the configuration space and its algebra is provided in [20].

We assume that the robot interacts with the environment by exchanging distinct wrenches. The equations of motion of the multi-body system can be described by applying the Euler-Poincaré formalism [21]:


where is the mass matrix, is the matrix accounting for Coriolis and centrifugal effects, is the gravity term, is a selector of the actuated DoFs, is a vector representing the internal actuation torques, and denotes an external wrench applied by the environment to the link of the -th contact. The Jacobian is the map between the robot’s velocity and the linear and angular velocity at the -th contact link.

Lastly, it is assumed that a set of holonomic constraints acts on Eq. (1). These holonomic constraints are of the form , and may represent, for instance, a frame having a constant pose w.r.t. the inertial frame. In the case where this frame corresponds to the location at which a rigid contact occurs on a link, we represent the holonomic constraint as . The holonomic constraints associated with all the rigid contacts can be represented as:


By differentiating the kinematic constraints Eq. (2), one has:


Combining the system dynamics (1) and the constraint equations Eq. (3) leads to the following set of equations:


where we defined , while are the set of contact wrenches - i.e. Lagrange multipliers - making Eq. (3) satisfied.

Ii-C Contact Stability Constraints

Fig. 1: Contact surface. The picture highlights the rectangle’s dimensions w.r.t. the contact frame .

QP based controllers usually require that the forces and moments at the

-th active contact preserve the contact stability conditions. In case of planar unilateral contacts, the contact stability conditions can be formulated as follows:


Being the constraints unilateral, condition (6a) imposes the positivity of the force normal to the contact surface in order to keep the contact constraint active. Eq. (7a) limits the magnitude of the forces parallel to the contact surface in order not to overcome static friction, and is the static friction coefficient. Conditions (8a)-(9a) constrain the local Center of Pressure to remain inside the contact surface, which is assumed to be rectangular with dimensions , calculated w.r.t. the contact reference frame and defined as in Fig. 1. Eq. (10a) imposes no foot rotation along the axis normal to the contact surface, and is the torsional friction coefficient.

Ii-D Problem Statement

A common control approach for system (4a) is the stack-of-task approach, which usually considers several control objectives organized in a hierarchical or weighted prioritization. More precisely, let be a desired acceleration that the system should achieve. Then, a single priority111When several priorities are defined into the optimisation problem, higher priority tasks can be defined as constraints of (11). stack-of-task can be represented by the following optimisation problem:

subject to:

with a proper projection matrix, and the manifold given by the constraints (6a). The above optimisation problem is usually framed as a Quadratic Programming (QP) one, and its solutions may suffer from the following limitations:

  1. The friction cone manifold (7a) is approximated with a set of linear inequalities in order to frame the optimisation problem (11) as a QP;

  2. The optimal solution may be discontinuous, e.g during contact switching or after sharp variations of the reference trajectory;

  3. The closed-loop dynamics does not include any feedback term from the measured contact wrenches .

Limitation does not usually have a strong impact on experimental activities, although it does remain an approximation of the static friction properties. Limitation is often addressed by approximating the continuity property with a set of inequality constraints to be added to (11) but the effectiveness of this approach is often unsatisfactory from the experimental standpoint [22]. Limitation is the most critical one, since FT sensor information are not used in the optimal control law that solves (11), thus potentially wasting important feedback information at the control level.

Let us observe that Limitation may be attenuated when desired force tasks are added to the problem (11). For instance, if we assume to achieve a desired force , then the force task can be achieved by adding equality constraints in the form to the problem (11). At this point, one may attempt at using the FT measurements by replacing with

where is the measured force, and being the equality constraint. The main limitation of this approach is that this equality constraint may require to violate the constraint . Putting the desired force as part of the cost function of (11) may be an option, but this alters the priorities that the force task has over the acceleration one.

What follows presents an alternative, theoretically sound approach that aims at addressing the above limitations , , and of classical stack-of-task approaches for the control of floating base systems in contact with the environment.

Iii A Contact-Stable Wrench Parametrization

Parametrisations can be an effective way to transform constrained optimisation problems into unconstrained ones [23]. Consider, for instance, the following optimisation problem:

subject to

with , and . If there exists a solution to (12), the process of seeking this solution is equivalent to solving the following problem:


with . For this specific case, it is trivial to find a parametrisation ensuring . Note, however, that the mapping is one-to-one, and its gradient is always invertible, namely . These two additional properties are of particular importance for the numerical stability of solvers addressing the problem (13).

Next Lemma proposes a wrench parametrisation that may be used to remove the constraint into the optimisation problem (11). The parametrisation catches completely the friction cones (7a), thus avoiding the approximation of these cones as a set of inequality constraints.

Lemma 1.

Consider the parametrization , with


the minimum magnitude of the vertical force ,

and and the contact surface dimensions as described in Fig. 1.

Then, the following properties hold:

  1. The contact constraints (6a) are always satisfied, namely, .

  2. The function is a bijection, namely, a one-to-one correspondence from to .

  3. Let


    be the gradient of the function . Then,

    is an invertible matrix


The proof is in the Appendix A. Lemma 1 shows that there exists a one-to-one correspondence between the manifold , i.e. the set defined by (6a), and a set of free parameters . Clearly, one may find other functions for which the contact constraints (6a) are always satisfied. But the proposed function in (14) has an image that corresponds to the set  uniquely and completely. Let us also observe that the gradient of this function is invertible for any value of the parameter . This property will be of pivotal importance in Sections IV and V when designing stable controllers for system (4a).

The parametrization (14) can be easily extended in case of distinct contact wrenches. In this case, define:


where and . It is then straightforward to verify that the properties described in Lemma 1 are retained even in case of multiple contact wrenches.

Iv Jerk Control

This section proposes control laws that exploit the contact wrench parametrisation (14) and attempt to address the limitations – listed in Section II-D – of the classical torque-based controllers framed as stack-of-tasks optimisation problems.

Iv-a Jerk control with parametrised contact wrenches

The wrench parametrisation (14) can be used to remove the constraint into the optimisation problem (11). This process would lead to the following formulation:

subject to:

with . The main drawbacks of the above approach are: the optimisation problem (18) can no longer be casted in a QP being the parametrisation nonlinear; we would then need nonlinear – and often slower than QPs – optimisers to solve (18); the limitations and listed in Section II-D are not addressed.

To include feedback terms into the control laws, the contact wrenches, or accelerations, shall become part of the system state. In the language of Automatic Control, we shall then proceed with augmenting the relative degree of the output (or task) that one wants to stabilise [24].

More precisely, assume that: the control objective is the stabilisation of a desired jerk ; the joint torque rate-of-change can be considered as a control input; both the joint torques and the contact forces are measurable quantities, so the robot acceleration – if not measurable – can be obtained from (4a). Now, define


As a consequence of , one has a measurement of the vector , while the variable can be used as a search variable. Then, control laws for that contain feedback information from FT sensor are obtained as an outcome of the following optimisation problem:

subject to: (22a)

The solutions to the above problem are continuous in (even if is discontinuous) and contain FT measurement feedback from the vector . One of the main difficulties when solving (21a) is given by the constraint (22a). Since the variable no longer is a search variable, in fact, one cannot instantaneously choose values of the contact wrenches such that . One may attempt at making (22a) satisfied by regulating appropriately the variable , which influences the wrench at the next time stamp.

A possibility to make (22a) satisfied is to use the parametrisation in Lemma 1: the gradient of the parametrisation automatically enforces the fact that . More precisely, in view of (15), one has , which leads to the following optimisation problem

subject to: (24a)


In order to be solved at each time instant, the optimisation problem (23a) requires the variable . This variable may be retrieved from either time integration of , or by inverting the relationship : being the parametrisation a one-to-one correspondence (see Lemma 1), there exists a unique for any value of the contact wrench provided that the contact constraints (6a) are satisfied. This latter way allows us to inject further information from the FT sensor measurements into the optimal control laws solving (23a).

Note also that the matrix is invertible thanks to the property of Lemma 1. The invertibility of clearly plays a pivotal role when solving the optimisation problem (23a).

Iv-B On the robot actuation for imposing a desired

The optimal value solving (23a) may be sent directly to the robot low-level control system if it allows to set desired rate-of-changes of joint torques. This may be feasible, for instance, when the low-level robot control exploits the model between the joint torques and the motor currents , e.g. . More precisely, the motor currents are usually subject to electrical dynamics of the kind , where is often the motor voltages to be applied to the motors – namely, the real control input. Then, it is straightforward to express the optimisation problem (23a) so as the search variable contains . Let us observe, however, that this architecture in general requires high-frequency control loops (e.g. KHz) for generating the motor voltages : these loops have to compute inverse dynamics within the short control cycle. If the control loops are not fast enough, sampling effects may be preponderant phenomena that render the assumption not representative of the underlying physical dynamics. In this case, the associated control strategy resulting from (23a) may prove to be ineffective.

Now, assume that the joint torques (and not their rate-of-change ) can be considered as control input. Then, a strategy for achieving a desired rate-of-change of the joint torques is that of performing time integration of and obtain references for low-level joint torque controllers. In these cases, time integration may add drifts that have to be properly dealt with.

Iv-C On the dynamic computations for jerk control

The optimisation problem (23a) requires the terms , and to be solved. These terms in general depend on the robot configuration space , velocity , and accelerations , and need the derivatives of the system inverse dynamics. Besides numerical approximations for computing these terms, existing libraries nowadays provide users with the support of automatic differentiation and/or directly derivatives of inverse dynamics [25, 26]. If some of the terms in (23a) are not available, one may attempt at setting them equal to zero and tune the feedback control gains so as to achieve robustness against them. Alternately, one can focus on specific control objectives for which it is possible to conceive architectures that do not require additional inverse dynamics derivatives. An example of such an architecture is presented in V-C.

V Momentum-based Jerk control

This section proposes control laws that can be obtained from (23a) when explicitly solved and extended for a two layer stack-of-task. These laws can also be shown to possess stability properties. Interestingly, the architecture presented below does not require the feedforward terms that depend on the inverse dynamics derivatives required by (23a). This is achieved by loosing the continuity property of but retaining the continuity of the contact wrenches .

More precisely, we assume that: the highest priority task is the stabilisation of a desired robot centroidal momentum [20, 27]; the lower priority task aims at stabilising the robot posture to regulate the system zero dynamics [24].

Let us recall that the momentum rate-of-change equals the summation of all the external wrenches acting on the robot. In a multi-contact scenario, the external wrenches reduce to the contact wrenches plus the gravity force:


where is the robot’s momentum, is the matrix mapping the -th contact wrench to the momentum dynamics, is the origin of the frame associated with the -th contact, and is the CoM position.

Recall that the rate-of-change of the robot momentum (25) is related to the system accelerations (e.g. acceleration of the system center of mass). So, to derive jerk-based control laws, we need to differentiatate (25) w.r.t. time, which writes:


Note that Eq. (26) is linear w.r.t. . Thus, optimisation problems similar to (23a) may be laid down. In particular, to obtain stabilisation of the robot momentum, one may:  consider as control input – or search variable – of the momentum acceleration (26);  apply feedback linearization to (26) in order to impose a momentum acceleration of the form:


where are symmetric and positive definite matrices, is the reference momentum and is the momentum error.

Observe that it is always possible to find  such that


because of the item of Lemma 1. More precisely, the gradient being always invertible ensures that the matrix in (26) is full rank . Consequently, has full control authority on the momentum acceleration for any value of . Clearly, one can impose (28) as long as remains bounded.

The third order system (28), however, is in general very sensitive to gain tuning, as not all the possible combinations of the gain matrices guarantee stability of the associated closed-loop the system. This limitation affects the controller’s performances when applied to the real robot, where phenomena as modeling errors, measurements noise and external disturbances further limit the control gain choice.

V-a Momentum-based jerk control laws

We propose a control algorithm alternative to pure feedback linearization with the goal of facilitating the gain tuning of the closed-loop dynamics. In particular, consider as control objective the stabilization of towards the reference values . Consequently, one has

whose dynamics writes:


Then, the following result holds.

Lemma 2.

Assume that the robot makes at least one rigid contact with the environment, i.e. , and that can be chosen at will. In particular, choose:


with symmetric and positive definite matrices,

the projector in the null space of , and a free variable of proper dimension. Then:

  • the equilibrium point is locally (globally) asymptotically stable if is locally (globally) bounded, respectively.

The proof is in the Appendix B. Lemma 2 shows that there exist control laws for the robot momentum that possess stability properties despite the constraints (6a) on the generated contact wrenches . In fact, these constraints remain satisfied while ensuring stability properties of the associated closed-loop system, and such a claim cannot usually be made in classical stack-of-tack approaches (11).

The control law (32) contains both feedforward and feedback terms that depend on the measured contact wrenches. It makes use, in fact, of (25) for computing , which depends on the measured contact wrenches. In the case of a single contact, there exists a unique control input that satisfies (32), and consequently one has that the null space of the matrix is empty, i.e. . In the case of multiple contacts (), instead, infinite control inputs satisfy (32). We solve the associated redundancy using the free variable to minimize the norm of the robot joint torques. The computation of is detailed in Appendix B.

Let us remark again the importance of the the invertibility of the gradient – see Lemma 1. This property guarantees that the matrix in (32) is full rank, so has full control authority on the momentum acceleration for any value of .

V-B Computation of , and

The control input (32) requires: the contact wrenches ; the momentum derivative ; and the associated variable such that

. The contact wrenches can be measured/estimated using the measurements from 6-axis FT sensors installed on the robot. Once the wrenches

are retrieved, we can compute the momentum rate of change via (25). The associated variable can be computed by applying the parametrisation inverse, namely . The inverse mapping exists provided that the measured contact wrenches remain inside the set defined by (6a). If the measured wrenches do not belong to (because, e.g., measurements noise, external unmodeled disturbances, etc.), a saturation shall be applied in the calculation of the inverse mapping so that the control input always remains finite.

V-C Computation of the joint torques to realize

To realize a , e.g. the law in (32), we have to choose the real control input of the system properly. We assume in this section that the control input is the joint torque , so we cannot impose a desired instantaneously.

As mentioned in IV-B, a possibility for finding the joint torques is that of finding realising , and then perform time-integration of to obtain . This procedure, however, requires some derivatives of the inverse dynamics, which may not be always be available in practice.

For this reason, we follow here another route for finding the joint torques attempting to realise . First, we find the instantaneous relationship between the joint torques and the contact wrenches . This relationship can be found, for instance, by substituting the state accelerations from (4a) into the constraints (5a), which leads to:


with . Then, we proceed as follows:

  • Integrate the control input to obtain . The initial conditions for the integrator can be calculated by measuring the initial contact forces and by applying the parametrization inverse mapping, i.e. ;

  • Apply the parametrization direct mapping to evaluate the wrenches from , i.e. . By doing so, note that always satisfy the contact stability constraints;

  • Retrieve the input torques from (33), which write


where is the projector in the null space of , and is a free variable, that can be chosen to guarantee the stability of the system’s zero-dynamics [18]:

where we partitioned , and ; , and , and the mass matrix is partitioned as:

with and .

Vi Simulations and Experimental Results

Vi-a Simulation Environment

The modeling and control framework presented in Sec. III-IV is tested on the 23-DoFs iCub humanoid robot [28], both on the real robot and on simulations using Gazebo [29]. The controller is implemented in Simulink, and runs at a frequency of . An advantage of using the Simulink-Gazebo simulator consists in the possibility to test directly on the real robot the same control software used in simulation.

Gazebo offers different physic engines to integrate the system’s dynamics. Among all the possibilities, we chose the Open Dynamics Engine (ODE), that uses a fixed step semi-implicit Euler integration scheme, with a maximum simulation time step of [ms].

On the real iCub, the Simulink controller runs on an external PC and provides reference joint torques to an inner joint torque control loop, that runs on board at . At the moment, iCub is not endowed with joint torques sensors. The measured joint torques are achieved by combining the FT sensors information, the joint encoders, IMU information and the robot model. The robot is endowed with 6 FT sensors, two of them located in the robot’s upper body, two of them in the legs and two in the robot’s feet.

Vi-B Differences Between Simulation and Real Robot

When mounted on the robot, the FT sensors accuracy is affected by several phenomena such as temperature, internal stresses and vibrations. More specifically, we observed that even after FT sensor fine calibration the linear forces measurements still have an offset of . Low sensors accuracy combined with a limited range for gain tuning on the real robot may in practice impair the convergence of the control input to a stable value. To this purpose, we modified Eq. (32) by adding a regularization term as follows:


where is a positive scalar, the integral of and is obtained by applying the parametrization inverse mapping on the set of wrenches satisfying the desired momentum rate of change, i.e. . In case of multiple solutions, the one ensuring minimum norm of is chosen.

(a) Linear momentum error norm
(b) Angular momentum error norm
Fig. 2: Linear and angular momentum error norm during two feet balancing. On the real robot the additional regularization term is required, while in simulation it is not required. Adding noise on the FT sensors measurements in simulation generates a response similar to the one on the real iCub.
(a) Force norm at left foot
(b) Moment norm at left foot
Fig. 3: Norm of the left foot input forces and moments (). The momentum-based jerk controller (red line) helps in providing smoother references to the torque controller while switching from one foot to two feet balancing and vice-versa (blue background).

It is important to point out that the regularization term only accounts for limitations on the FT measurements, and it is not a requirement from the theoretical point of view. To prove this statement, Figure 2 shows the behavior of the linear and angular momentum error norm during several two feet balancing simulations and experiments. On the real iCub, the robot falls after few seconds when , as pointed out by the green line. When adding the regularization term as in Eq. (35), stability is retained and the momentum error norm does not diverge (purple line). On the other hand, the blue line is obtained in simulation with perfect estimation of the external forces, and we set . In this case, the momentum error norm does not diverge, thus proving that the regularization term is not needed when there is no noise on the FT sensor measurements. During simulation, we injected a constant offset of amplitude to the ”measured” component of one of the two contact wrenches. Results correspond to the orange line in Figure 2: as it is possible to see, stability is no more retained and the robot falls after balancing for few seconds. With the addition of the regularization term, the previous balancing performances are restored (red line).

(a) Linear Momentum error norm
(b) Angular Momentum error norm
Fig. 4: Linear and angular momentum error norm during contact switching from two feet to one foot and vice-versa (blue background) and performing dynamic movements on the left foot (white background). The momentum tracking performances of the two controllers are comparable.

Vi-C Comparison with a momentum-based QP controller

We designed an experiment to compare the performance of the momentum-based jerk controller with a classical momentum-based QP controller implementing the optimization problem Eq. (11) on the real iCub, during a contact switching scenario [18]. Initially, the robot starts balancing on two feet, then it switches to balance on the left foot via a finite state machine, and performs highly dynamic movements on the left foot. Finally, the robot returns back to two feet balancing.

The two controllers have both been fine tuned for the specific demo. The goal is to show that the momentum-based jerk control architecture guarantees performances that are comparable with a controller already available in literature. Furthermore, the momentum-based jerk control helps in providing smoother references to the torque controller, as the desired contact wrenches are always continuous.

In Figure 3

we show the norm of the left foot input contact forces and moments for both the momentum-based jerk control and the momentum-based QP control. Results have been achieved by running 10 experiments for each control strategy. The solid lines represents the average values, while the transparent regions are the variance over the 10 experiments. The blue background represents the instants in which the robot is balancing on two feet, while the white background is when the robot is balancing on the left foot. Looking at the transitions between different balancing states, corresponding to a change in the background color, it is clear that the momentum-based jerk controller helps in providing smoother references to the torque controller, thus reducing the risk of possible unstable behavior while switching from one foot to two feet balancing, and vice-versa.

Fig. 5: Joint position error norm during highly dynamics movements. The plot shows that the system’s zero dynamics does not diverge while achieving the primary task.

As for the previous experiment, also in this case we compare the norm of the linear and angular momentum error for the selected demo. Results are presented in Figure 4. During all the experiments, the momentum-based jerk controller (red line) guarantees the achievement of a performance which is comparable with the one of the momentum-based QP control (blue line).

(a) Momentum rate of change norm
(b) Momentum error norm
Fig. 6: Momentum rate of change and momentum error norm during the disturbance rejection experiment. The controller can retain stability despite the action of the external unmodeled forces.

In both controllers the input torques are calculated as in Eq. (34). Figure 5 verifies the stability of the system’s zero dynamics. In both cases, the zero dynamics does not diverge. Convergence to zero of the joints position error is not necessarily expected as the controllers implement strict task priorities, and the postural task is chosen as the lowest priority task. For further details, a video of the experiment is attached to this paper.

Vi-D Disturbance rejection

To evaluate the robustness of the momentum-based jerk controller against unmodeled external force perturbations, we performed the following experiment: the robot balances on two feet. Meanwhile, a person pushes and pulls continuously the robot’s upper body. The applied external force is unmodeled, and it is treated as a disturbance by the momentum-based jerk controller. Figure 6 shows the momentum rate of change error norm and the momentum error norm during interaction. It is possible to verify that despite the high peaks of errors while the external unmodeled force is applied, the controller is still able to retain stability and when the force is removed the momentum error and its rate of change still converge to a stable value. Exact convergence to zero of the momentum derivative error on the real iCub is difficult to obtain because of the low sensitivity of the FT sensors, therefore a compromise is achieved by properly tuning the corresponding feedback gains. A video describing the experiment is attached to the paper.

Vii Conclusions and Future Work

In this paper, we addressed some common limitations of force and torque controllers for floating base systems based on Quadratic Programming. More specifically, we removed inequality constraints from the optimization problem by designing an invertible, one-to-one mapping that parametrises the contact wrenches into a new set of unconstrained variables. This parametrization guarantees that the output wrenches always satisfy the contact stability constraints. Based on this mapping we designed a general jerk control optimization framework for floating base systems. We then analyzed a specific use case of the jerk controller, namely a momentum-based jerk control architecture for balancing a 23 DoFs humanoid robot. The controller has been validated both in simulation and on the real iCub, and compared with a classical momentum-based controller. Finally, theoretical and practical limitations of the proposed approach have been discussed. Poor FT sensors accuracy is identified as the main drawback of the proposed approach, as it may affect stability and convergence of the closed loop dynamics. A solution for increasing robustness of the controller w.r.t. low force sensors accuracy is presented. Future work will move towards improving FT sensors measurements both by acting at the hardware level and by involving post processing of the measured wrenches, e.g. by integrating a dedicated observer for removing the FT sensors offset. Concerning the control architecture, further development will be done in order to extend the jerk controller to the humanoid walking task.

Appendix A Proof of Lemma 1

Proof of 1): when , the constraint on the positivity of the vertical force Eq. (6a) is given by:

which is satisfied for all finite provided that . We substitute the remaining stability constraints Eq. (7a)–(10a) with the parametrization given by Eq. (14):


the vertical force is greater than zero and can be simplified from System (36a), leading to the following set of inequalities:


where also the coefficients and have been collected and simplified from Eq. (36a) and (39a), respectively. It is now straightforward to verify that constraint (43a) is verified for all finite . Direct calculations on Eq. (41a)-(42a) lead to the following expressions:

which is always satisfied for all finite . Concerning the remaining constraint Eq. (40a), the argument of the square root can be rearranged as follows:

which is always lower than for all finite .

Proof of 2): assume we are given with a feasible wrench . It is straightforward to compute the inverse mapping of the vertical force and moments parametrization:

Since the above equations are composed of one-to-one correspondences (hyperbolic tangent, logarithm), the solution is unique.

For what concerns the tangential forces and , let us recall the expressions for the parametrization of and :


An easy way to compute the inverse mapping is to raise to the square Eq. (44a), which gives:


In the resulting equations, the square hyperbolic tangents , appear linearly. Therefore they can be easily computed through matrix inversion:


Resolving Eq. (48) w.r.t. gives two possible solutions, namely . However, only one of the two solutions satisfies the parametrization Eq. (44a): in fact, the terms on the right-hand side of Eq. (44a) and the square root are always positive. Therefore the sign of () must correspond to the sign of (), leading to the unique solution:

Remark: it is possible to verify that if and belong to , then the matrix inversion in Eq. (48) can always be performed. In fact, singularities arise when