Impact-Aware Task-Space Quadratic-Programming Control

Hardware resilience to impact shall inform on how much impact a robot can perform without breaking links, joint mechanisms and embedded electronics. For example, employing variable stiffness actuator (VSA) technologies lowers damage risks at impacts. However, it may need more than $10$ ms to generate the joint torque that can counterbalance the torque jump (Haddadin et al. 2009). Despite numerous VSA paradigms that have been proposed – refer to the excellent review by Vanderborght et al. 2013 – their force control capabilities suffer from limited bandwidth as pointed by Wensing et al. 2017.

Aiming to reduce the initial and post-impact forces, the proprioceptive actuator devised for the MIT Cheetah robot’s leg provides a mechanical approach to mitigate impacts without added compliance (Wensing et al. 2017). More recently, Singh and Featherstone 2020 proposed a novel quadruped robot leg design that removes the shock propagation from the floating-base. Another strategy absorbs the impacts at foot-strike through passive springs in the ankle (Reher et al. 2016). Designing robust hardware specifically for impact tasks is appealing, and notable progress has been made.

However, the question remains on how to deal with impacts with existing robots. An outer softcover approach was described in Battaglia et al. 2009, where the thickness of the cover is computed as a function of latency and impact velocity. Generally, shock-absorbing mechanisms or soft soles are added to the ankle/feet of humanoids. There is certainly more to do and investigate in order to increase the resilience of robots to impacts. Nevertheless, it should be noted that for almost all the existing robots, gathering from their manufacturer parameters concerning tolerable impact bounds is already informative of the current facts. To our requests, we always had the same pair of answers: Why do you need this? And, after our explanations… You are not supposed to do that with our robots! The exception is the Franka Emika Panda robot, for which such data are available upon request.

Impacts are sometimes rather seen as perturbations. For example, Pagilla and Yu 2001 consider impact as a transient behavior and uses it for stability analysis rather than for explicit control design. Yet, Stewart 2000 reviewed that locomotion tasks have treated the impulse effects for two decades. The complementarity dynamical system (CDS) proposed by Hurmuzlu et al. 2004 offers a general conceptual tool to describe the dynamics of a walking robot, e.g., a biped with one or two contacts. Interested readers find a summary of the CDS controllability and stabilizability for mechanical applications in Brogliato 2003. Impact-induced state jumps, which are embedded in a reset map, are applied to the robot states depending on the complementarity condition status. However, Grizzle et al. 2014 pointed out that there is no effective control design for CDS.

Although hybrid control is a powerful modeling and control tool for non-smooth mechanical systems in general, e.g., for robots undergoing intermittent impacts (Johnson et al. 2016), it scales poorly concerning the number of potential contacts, refer to improvements by Posa et al. 2015. For instance, in the case of biped robots, there are two single support modes, double supports mode, and the transition phases. If the model is not complete enough, Zeno phenomena might occur and lead to infinite accumulations of impacts in a finite time (Or and Ames 2010). Nevertheless, hybrid approaches achieved successful applications in practice, e.g., efficient bipedal walking by Sreenath et al. 2011, walking over uneven terrain Manchester et al. 2011, dynamic bipedal vertical climbing in simulation by Aghasadeghi et al. 2012, and dynamic bipedal walking by Reher et al. 2016.

Rather than dealing with periodic impacts or cyclic behaviors, in this paper, we are interested in the local feasibility properties before and right after a single impact event. Hence, modeling using the Poincaré map (Grizzle et al. 2014), and the controllers developed on top of it do not apply straightforwardly to our aim. Comparing our approach with the impact dynamics model applied for walking (Hurmuzlu et al. 2004; Grizzle et al. 2014) or hybrid control approaches by Rijnen et al. 2017 is pointless. This is because we target general-purpose tasks implying any limb of the robot. Contrarily to walking only, we do not assume that the holonomic constraints, e.g., the position and orientation of a contacting limb, remain constant before and after the impact. Furthermore, we estimate the end-effector velocity jump for free limbs and for established unilateral contacts (C.2).

Another difference comes from the number of impacts and contacts. Switching from a single support phase to a double support phase merely induce a single impact concerning the other support (established contact). Grizzle et al. 2014 pointed out that conservation of generalized momentum (in the joint space) for a single kinematic branch is used in one form or another for most of the walking-related works. Instead of solving joint velocity jumps on a limb-by-limb basis, we solve the state jumps that are compatible with each limb due to the whole body momentum conservation and balance (C.1 and C.2).

Alternatively, some control strategies simply avoid impacts when contacts are about to be created. Given the impact location, Pagilla and Yu 2001 modified the reference trajectories such that the reference velocity along the surface normal is zero. The control design based on Zero-tilting Moment Point (ZMP) ignores the impact dynamics and establishes contacts with close to zero velocity – checkout the example by Kajita et al. 2010. Grizzle et al. 2014 summarized that the impact-less reference trajectories are challenging to generate and inefficient to execute: we totally agree. Adding impact-awareness to the QP control framework (C.4), our proposed approach autonomously executes the reference motion with a possibly reduced, if it violates the hardware limits or other user-defined constraints (C.3), yet maximum safe contact velocity.

Commonly flexible models with regularization are used e.g. the mass-spring-damper (Hu et al. 2007; Stanisic and Fernández 2012; Heck et al. 2016) to enable well-established control tools for continuous-time dynamics. Recently, for dealing with general-purpose impact behaviors, Rijnen et al. 2017 proposed to switch once from pre- to post-impact reference based on the detection of the impact event, (referred to as reference spreading). Konno et al. 2011 solved a three-phase non-linear optimization problem to generate reference trajectories as well as the posture at the impact moment to maximize the force jump. These methods require off-line reference trajectory planning and switching, which compromises reactiveness. It further conflicts with the concept of a desirable general-purpose multi-objective controller that is based on real-time sensory feedback.

The reference switching is performed upon the impact detection. Note, that the observation is always delayed and may be inaccurate. Thus, in general, due to the unknown exact impact timing (Pagilla and Yu 2001), switching controllers would apply setpoints or references, that are defined for the pre-impact mode, during the post-impact mode for a specific time interval, no matter how small it is. The resulting undesired misbehavior imposes the risk of exceeding hardware limitations.

Our approach is rather conservative in that, nearby impact, the controller starts assuming the impact to happen in the next time step. Our proposed impact-aware QP controller (C.4) provides the mapping between the impact-induced state jumps and the pre-impact velocities in each iteration (C.2). The mapping enables the QP controller to regulate the robot motion in real-time such that the impact-aware constraints are respected (C.3). We can find a similar mapping provided by Partridge and Spong 2000 to control the trajectory of a three-link planar robot. The proposed QP controller is free of off-line trajectory computation and, therefore, robust to impact timing and location. The prediction of post-impact states that we propose (C.2) is based on the impact dynamics model along with the contact normal direction by Zheng and Hemami 1985 without considering tangential impulse and impact-induced friction.

A similar impact dynamics model is introduced by Hurmuzlu et al. 2004 to address the bipedal walking as summarized in Grizzle et al. 2014; Wieber et al. 2016. In the field of space robotics, Nenchev 2013 applied joint space momentum conservation to study the post-impact robot motion behavior. In this paper, we do not have the zero gravitational force assumption, and we utilize the centroidal momentum conservation (Orin et al. 2013).

Nava et al. 2016 pointed out that the state-of-the-art momentum controller may lead to unstable zero dynamics. Nenchev 2018 recently proposed a decoupled controller based on the relative momentum. Instead of applying the – not yet – fully exploited momentum-based control strategies, we prefer to limit the impact-induced centroidal momentum jumps (C.3).

Even though force jumps are typical for legged robots (Bergés and Bowling 2005), it is not straightforward to include an analytical solution of the tangential impulses. Recently, Jia et al. 2019 developed a flying object batting example, where a closed-form 2D impact dynamics model is applied. It generates the desired impulse by enumerating five possibilities. In the 3D cases, the closed-form solution is only available if we can control the initial sliding direction to an invariant subset, for more details, refer to (Jia and Wang 2017). Note that according to the energy conservation principle, it is not possible to treat impact dynamics along with the two tangential directions and normal direction separately. One caveat of this approach is the mathematical and computational complexity of the model.

Control formulations based on a quadratic program (QP) with linear constraints have become the predominant scheme for operating legged robots, (Kuindersma et al. 2016; Liu et al. 2016; Bouyarmane and Kheddar 2018) to name just a few. The approach allows pursuing multiple prioritized objectives simultaneously while satisfying strict bounds. It has been applied both to position- and torque-controlled robots, usually employing $1\!-\!5$ ms update loops. We adopt and further build on our initial concept proposed in simulation for fixed-base robots in Wang and Kheddar 2019 and the preliminary extension to the floating-base robot in Wang et al. 2019. Concerning these and previously cited contributions, this article provides (i) a detailed discussion on multi-impact cases and different least-squares solutions that were not covered before; (ii) additional constraints (C.2), e.g., angular momentum, framed as a generic impact-aware template (C.3); (iii) improving the impact effect estimation (C.1) by adding the centroidal momentum conservation constraint and minimizing a weighted minimum norm solution.

The Zero-tilting Moment Point (ZMP) is widely used as a balance criterion for biped walking on the planar ground, for example, refer to (Hildebrandt et al. 2019). Recently, the concept has been extended to the multi-contact case through excellent analysis in Caron et al. 2017. For tasks that require large impulsive forces, e.g., a nailing task discussed in Tsujita et al. 2008 and a wooden piece breaking task performed in Konno et al. 2011, ZMP is used to analyze the dynamic equilibrium of each robot configuration instance. Introducing impact-aware constraints (C.3) to bound the impact-induced state jumps (C.2) including ZMP, Center-of-Mass (COM) velocity, Divergent Component of Motion (abbreviated as DCM by Koolen et al. 2012) and the centroidal momentum, our proposed method allows more reliable and robust motion generation.

To our best knowledge, intentionally generating high impacts with humanoids is studied only in very few works and for specific scenarios, e.g., Konno et al. 2011. Despite impressive results achieved with non-linear optimization for planning, their controller neither accounts for uncertainties in most impact parameters nor explicitly accounts for constraints in the closed-loop motion. We aim to extend state-of-the-art task-space multi-objective and multi-sensory whole-body control framework formulated as a quadratic program (QP) to encompass impact tasks (C.4).

Model-based whole-body control requires to express multiple prioritized task objectives and constraints in the operational or the joint space (Dehio 2018; Lober et al. 2019; Dietrich and Ott 2020). The approach uses robot kinematic and dynamic models, see examples in Kuindersma et al. 2016; Nenchev 2013; Liu et al. 2016.

Consider an arborescent kinematic structure with $m={m_{1}}+{m_{2}}+{m_{3}}$ independent end-effectors that we separate into three categories:

1. 1.

   ${m_{1}}$ end-effectors with established contacts;

2. 2.

   ${m_{2}}$ end-effectors for which we expect an upcoming impact (or dynamic contact transition), and

3. 3.

   ${m_{3}}$ free limbs whose momentum jump can mitigate an external impulse propagating from other end-effectors.

We denote these three sets of end-effectors with $\sigma_{{m_{1}}}$, $\sigma_{{m_{2}}}$, and $\sigma_{{m_{3}}}$ respectively. Fig. 1 illustrates an exemplary impact task with varying ${m_{1}}$, ${m_{2}}$, ${m_{3}}$ over time.

For legged robots, we can represent the floating-base, which holds a configuration in $SE{(3)}$, by six virtual joints. Thus the generalized joint position writes $\boldsymbol{q}\in\mathbb{R}^{(n+6)}$. We choose the base link velocity expressed in the inertial frame $\boldsymbol{V}_{b}\in\mathbb{R}^{6}$ to construct the generalized velocities as:

A floating-base robot with ${m_{1}}$ established contacts is governed with the following equation of motion:

where $B\in\mathbb{R}^{(n+6)\times n}$ selects actuated joints, $\mathbf{M}(\boldsymbol{q})\in\mathbb{R}^{(n+6)\times(n+6)}$ denotes the inertia matrix, the vector $\mathbf{N}(\boldsymbol{q},\dot{\boldsymbol{q}})\in\mathbb{R}^{(n+6)}$ gathers both the Coriolis $\mathbf{C}(\boldsymbol{q},\dot{\boldsymbol{q}})\in\mathbb{R}^{(n+6)\times(n+6)}$ and the gravitational forces, vector $\boldsymbol{\tau}\in\mathbb{R}^{n}$ corresponds to the actuated joint torques. As can be noted, the ${m_{2}}$ end-efforts are not accounted at this stage.

Many constraints, such as joint limits, velocity limits, field-of-view, collision avoidance… to name just a few, do not express directly in the robot state acceleration. Such constraints need to be derived numerically prior to being added as a QP task objective or constraints. By approximating the derivative using Euler backward method, we predict (to some precision) the joint positions $\mathbf{q}\in\mathbb{R}^{n}$ at time step $k$ using the decision variable ${\ddot{\mathbf{q}}}_{k}$ and the measured joint positions and velocities at time step $k-1$:

where $\Delta t$ denotes the sampling period. Similarly, we obtain the joint velocities as:

Given the joint limits $\underaccent{\bar}{\jangles}\leq{\mathbf{q}}_{k}\leq\bar{\mathbf{q}}$ and the kinematic relationship (2) we obtain

For velocity bounds $\mathbf{\dot{\underaccent{\bar}{\jangle}}}\leq{\dot{\mathbf{q}}}_{k}\leq\mathbf{\dot{\bar{q}}}$ and (3) yields

A closed-loop stable implementation of such a class of constraints is solved in Djeha et al. 2020.

Given the torque limits $\underaccent{\bar}{\jtorques}\leq{\boldsymbol{\tau}}_{k}\leq\bar{\boldsymbol{\tau}}$ and the joint space dynamics (1) results in

Assuming rigid contacts, we constrain zero contact acceleration $J_{i}{\ddot{\mathbf{q}}}_{k}+\dot{J}_{i}{\dot{\mathbf{q}}}_{k}=\mathbf{0}$. Another possible writing is by substituting (3)

which is rearranged as

We require the $i$-th contact wrench ${\boldsymbol{W}_{{}_{i}}}=[\boldsymbol{f}^{\top},\boldsymbol{\tau}^{\top}]^{\top}$ at the origin of its local contact frame to fulfill non-sliding constraints:

and, if needed, the center of pressure (CoP) within each contact area

where $\mu$ denotes the friction coefficient, $\text{C}_{\text{area}}$ the planar contact area. The half-plane representation of the above inequalities write:

As long as (7) and (8) hold, the contact is sustained without slipping or taking off. The details of $\mathcal{C}_{i}$ are given in Caron et al. 2015 or similarly in (Grizzle et al. 2014, Sec. 3.3).

In the multi-contact setting, we define the Zero-tilting Moment Point (ZMP) as $\boldsymbol{z}\in\mathbb{R}^{2}$:

The wrench $\boldsymbol{W}_{O}=[\boldsymbol{f}_{O}^{\top},\boldsymbol{\tau}_{O}^{\top}]^{\top}$ denotes the net contact wrench measured in the inertial frame $\mathcal{F}_{O}$ whose origin is typically the center between the feet contacts (Caron et al. 2017). The ZMP is restricted by its feasible area $\boldsymbol{z}\in\mathcal{S}_{s}$ or $\boldsymbol{z}\in\mathcal{S}_{\boldsymbol{z}}$. In the simple case with coplanar contacts (for example double support stance on flat ground) the bounding polygon is defined by the support polygon $\mathcal{S}_{s}$, i.e., the convex hull of the ground contact points. Caron et al. 2017 derived a feasible multi-contact ZMP area $\mathcal{S}_{\boldsymbol{z}}$ for the more general case. Introducing $A_{\boldsymbol{z}}$ and $\mathbf{a}_{\boldsymbol{z}}$, the convex half-plane representation of $\mathcal{S}_{\boldsymbol{z}}$ becomes:

As pointed by Sugihara 2009, the ZMP is a measure of the force. Thus given the ZMP definition (9), we rearrange the above constraint with respect to the external wrench:

Using the unit vector $[0,0,1]^{\top}$ (the reverse direction of the gravity) as the surface normal $\boldsymbol{n}\in\mathbb{R}^{3}$, $\mathcal{G}_{\boldsymbol{z}}$ writes as: $\mathcal{G}_{\boldsymbol{z}}=\begin{bmatrix}-\mathbf{a}_{\boldsymbol{z}}\boldsymbol{n}^{\top}&A_{\boldsymbol{z}}\widehat{\boldsymbol{n}}\end{bmatrix}$.

When the robot is subject to external forces, Sugihara 2009 showed that the horizontal COM velocity $\dot{\boldsymbol{c}}_{x,y}$ has to remain inside a convex 2D polygon $\mathcal{S}_{\dot{\boldsymbol{c}}}$ in order to ensure the dynamic equilibrium. This area $\mathcal{S}_{\dot{\boldsymbol{c}}}$ relates to both the feasible ZMP area $\mathcal{S}_{\boldsymbol{z}}$ and the COM position $\boldsymbol{c}_{x,y}$. The constraint $\dot{\boldsymbol{c}}_{x,y}\in\mathcal{S}_{\dot{\boldsymbol{c}}}$ is transformed into the half-plane representation, introducing $\mathcal{G}_{\dot{\boldsymbol{c}}_{x,y}}$ and $\boldsymbol{h}_{\dot{\boldsymbol{c}}_{x,y}}$:

Due to the kinematic and actuation limits, the robot controller should minimize the angular momentum $\mathcal{L}_{\boldsymbol{c}}\in\mathbb{R}^{3}$ (Lee and Goswami 2012; Wiedebach et al. 2016). Suppose the angular momentum is bounded by $\mathcal{L}_{\boldsymbol{c}}\leq\underaccent{\bar}{\bar{\cmmAngularMomentum}}$, we take the angular part $A_{\omega\boldsymbol{c}}({\mathbf{q}}_{k})$ from the centroidal momentum matrix $A_{\boldsymbol{c}}({\mathbf{q}}_{k})$ proposed by Orin et al. 2013 to formulate:

The Divergent Component of Motion (DCM) is widely used for gait generation, e.g. refer to the example by Englsberger et al. 2018. DCM is defined as a two-dimensional point $\boldsymbol{\xi}\in\mathbb{R}^{2}$ by

where the scalar $\omega$ is defined by gravity and the COM position along the gravity direction: $\omega=\frac{g}{\boldsymbol{c}_{z}}$. The feasible DCM support area $\mathcal{S}_{\boldsymbol{\xi}}=\mathcal{S}_{\boldsymbol{z}}\bigcap\mathcal{S}_{\boldsymbol{c}}$ equals the intersection of the static-equilibrium COM area ${\mathcal{S}_{\boldsymbol{c}}}$, and the multi-contact ZMP support area ${\mathcal{S}_{\boldsymbol{z}}}$. We calculate $\mathcal{S}_{\boldsymbol{\xi}}$, ${\mathcal{S}_{\boldsymbol{c}}}$ and ${\mathcal{S}_{\boldsymbol{z}}}$ following the derivation given by Caron et al. 2017. Fig. 11 shows an example captured from a real-time computation. Again, we represent the constraint $\boldsymbol{\xi}\in\mathcal{S}_{\boldsymbol{\xi}}$ using half-planes where $\mathcal{G}_{\boldsymbol{\xi}}$ and $\boldsymbol{h}_{\boldsymbol{\xi}}$ are constructed based on $\mathcal{S}_{\boldsymbol{\xi}}$:

To our best knowledge, a real-time capable implementation of (11-14) has not yet been provided for QP controllers.

The whole-body controller is formulated as a quadratic program (QP) with desired task objectives and strict constraints (Bouyarmane et al. 2019). Prioritized tasks shall be met at best according to the associated cost function. Constraints shall be met strictly.

where $\sum_{i\in\mathcal{I}_{o}}w_{i}\|\boldsymbol{e}_{i}(\boldsymbol{\mu})\|^{2}$ scalarizes multiple task objectives included in a set $\mathcal{I}_{o}$, $w_{i}$ weights the $i$-th task and $\boldsymbol{e}_{i}(\boldsymbol{\mu})$ is the task error, which is linear in terms of the QP decision variables $\boldsymbol{\mu}$, see the details in Bouyarmane et al. 2019. Both the generalized joint accelerations ${\ddot{\boldsymbol{q}}}_{k}$ and the generating vectors of the contact wrench cone $\boldsymbol{f}_{\lambda}(k)$ are optimized, hence ${\boldsymbol{\mu}}_{k}\vcentcolon=\{{\ddot{\mathbf{q}}}_{k},\boldsymbol{f}_{\lambda}(k)\}.$

Impacts result in instantaneous jumps in the contacting end-effector’s velocity, which affects many constraints expressed in terms of joint velocity such as robot state or kinematic bounds, ZMP, angular momentum, COM velocity, DCM, etc. As a result, the QP feasible search space, defined by the constraints, may suddenly shrink to an empty set. In this case, the QP is infeasible for the next control iteration. An illustrative toy example is given later in Sec. 5.6. Moreover, the controller must also be extended by constraints related to hardware-acceptable impacts obtained from mechanical stress limitations.

In order to deal with impact-induced state jumps, the main idea is to adjust the pre-impact contact velocity. Here, adjusting means that we shall find a compromise between task dictated impact-induced velocities (ideally, the maximum possible impact velocity if requiring the largest possible force jump achievable by the robot) and the QP controller feasibility set. In other words, we propose additional constraints that allow the controller to drive the robot to generate impacts that ensure a feasible closed-loop QP (i.e., non-empty search space including hardware impact limits) right after the impact (post-impact).

In the following, we explicitly show how to predict impact-induced state jumps (based on the assumption of rigid contact surfaces and the coefficient of restitution) in Sec. 4 and then explain how to formulate impact-aware constraints in Sec. 5.

Let us consider a robot end-effector undergoing the desired impact task. We choose the following impact model: The scalar coefficient of restitution $c_{\text{r}}>0$ represents the relationship between the pre-impact end-effector velocity $\boldsymbol{\dot{x}}^{-}\in\mathbb{R}^{3}$ and the post-impact velocity ${\boldsymbol{\dot{x}}}^{+}\in\mathbb{R}^{3}$ assuming no sliding

where $P_{\boldsymbol{n}}=\boldsymbol{n}\boldsymbol{n}^{\top}\in\mathbb{R}^{3\times 3}$ is the projection operator onto the contact normal $\boldsymbol{n}\in\mathbb{R}^{3}$. It is important to note that angular velocity terms are neglected. In this model, the pre-impact velocity is not necessarily aligned with the contact normal. The resulting three-dimensional end-effector velocity jump becomes¹¹ 1 The chosen impact model (16) can be replaced by experimental models. Replacing the resulting end-effector velocity jump in (17), our approach applies without further adaptation.

We approximate the pre-impact velocity (in the next control cycle) as:

Neglecting all terms in $\Delta t^{n}$ where $n>1$ since $\Delta t\leq 5$ ms, we expand (17) to express the jump ${\Delta\boldsymbol{\dot{x}}}_{k+1}$ with respect to the QP decision variable ${\ddot{\mathbf{q}}}_{k}$:

where ${\dot{\mathbf{q}}}_{k}$ is obtained from the current robot state, and $\Delta t$ denotes the sampling period.

Impacts result in impulses $\boldsymbol{\iota}$ characterized by contact force jumps $\Delta\boldsymbol{f}$ in end-effectors with established contacts, and velocity jumps $\Delta\boldsymbol{\dot{x}}$ in contact-free end-effectors.

Many complexities of rigid body contact dynamics can be avoided by the integral of contact forces $\boldsymbol{f}$ over the impact duration $\delta t$. Thus we define the impulse:

whose unit is (N$\cdot$s). Equivalently, on a discretized form, we can define the associated force jump

There is no distinction between impulses $\boldsymbol{\iota}$ and finite force jump $\Delta\boldsymbol{f}$ over $\delta t$, see Chapter 48 by Wieber et al. 2016.

The stacked Jacobian $\boldsymbol{J}=[J^{\top}_{1},\ldots,J^{\top}_{m}]^{\top}\in\mathbb{R}^{3m\times(n+6)}$ collects all the end-effector Jacobians that possibly experience an impulse. Considering the robot’s joint space equations of motion (1) with the generalized force $B\boldsymbol{\tau}=\boldsymbol{J}^{\top}\boldsymbol{f}$, we obtain

where $\boldsymbol{\alpha}$ denotes the bias force (i.e., free acceleration) that we do not consider for impact dynamics. When integrating (20) over the impact duration $\delta t$ the vector $\boldsymbol{\alpha}$ vanishes:

Lets us now map this relationship onto the space of all end-effectors by left-multiplying (20) by $\boldsymbol{J}$:

where $\Upsilon=\boldsymbol{J}\mathbf{M}^{-1}\boldsymbol{J}^{\top}$. Note that (22) characterizes the impulse propagation among the $m$ end-effectors. Let us expand (22), into three blocks:

where a block $\Upsilon_{{\sigma_{i}}{\sigma_{j}}}\in\mathbb{R}^{3m_{i}\times 3m_{j}}$ gathers matrices $\boldsymbol{J}_{k}\mathbf{M}^{-1}\boldsymbol{J}_{l}$ of the articulated bodies inertia matrices (when $k=l$) and the cross-coupling inertia when $k\neq l$; $\Delta\boldsymbol{\dot{x}}_{\sigma_{{m_{1}}}}\in\mathbb{R}^{3{m_{1}}}$ gathers the provisional velocity jumps of the $m_{1}$ already existing contacts which will have reaction impulse forces $\boldsymbol{\iota}_{\sigma_{{m_{1}}}}\in\mathbb{R}^{3{m_{1}}}$; $\Delta\boldsymbol{\dot{x}}_{\sigma_{{m_{2}}}}\in\mathbb{R}^{3{m_{2}}}$ gathers the velocity jumps of the $m_{2}$ end-effectors that are supposed to contact with impulse force vector $\boldsymbol{\iota}_{\sigma_{{m_{2}}}}\in\mathbb{R}^{3{m_{2}}}$ at the next iteration; $\Delta\boldsymbol{\dot{x}}_{\sigma_{{m_{3}}}}\in\mathbb{R}^{3{m_{3}}}$ denotes the free end-effectors (i.e., no external force or impulse is acting on them), for which we aim to monitor the velocity jumps. Recall that $m=m_{1}+m_{2}+m_{3}$.

It is important to note that with redundant robots there exist multiple candidates of joint velocity jumps $\Delta\dot{\mathbf{q}}$ that can fulfill $\Delta\boldsymbol{\dot{x}}=\boldsymbol{J}\Delta\dot{\mathbf{q}}$ in (22). However, each $\dot{\mathbf{q}}$ determines a unique centroidal momenta $\boldsymbol{h}_{\boldsymbol{c}}\in\mathbb{R}^{6}$. Thus we introduce the centroidal space impact dynamics separately to specify the centroidal momenta jump $\Delta\boldsymbol{h}_{\boldsymbol{c}}$ uniquely.

The aggregation of external forces determines the derivative of the centroidal momentum $\boldsymbol{h}_{\boldsymbol{c}}$:

where the scalar $M>0$ denotes the total mass of the robot, $\boldsymbol{c}\in\mathbb{R}^{3}$ denotes the center of mass (CoM) and $\boldsymbol{p}_{i}\in\mathbb{R}^{3}$ denotes the $i$-th contact location (the CoP position).

The centroidal frame external force $\boldsymbol{f}_{i}=R_{\boldsymbol{c}\boldsymbol{p}_{i}}\boldsymbol{f}^{m}_{i}\in\mathbb{R}^{3}$ is calculated from the measured force $\boldsymbol{f}^{m}_{i}$. The Jacobian $J_{\boldsymbol{c}\boldsymbol{p}_{i}}^{\top}\in\mathbb{R}^{6\times 3}$ that calculates the induced wrench in the centroidal frame $\mathcal{F}_{\boldsymbol{c}}$ is given by: $J_{\boldsymbol{c}\boldsymbol{p}_{i}}^{\top}\vcentcolon=[I^{\top},\overrightarrow{\boldsymbol{c}\boldsymbol{p}_{i}}\times^{\top}]^{\top}.$

Integrating (24) over the impact duration $\delta t$ and assuming ${m_{1}}$ established contacts and ${m_{2}}$ impacts, we obtain the centroidal momentum jump as the sum of the external impulses:

where $[\Delta P_{\boldsymbol{c}}^{\top},\Delta\mathcal{L}_{\boldsymbol{c}}^{\top}]^{\top}=A_{\boldsymbol{c}}\Delta\dot{\mathbf{q}}\in\mathbb{R}^{6}$ employing the centroidal momentum matrix (CMM) $A_{\boldsymbol{c}}\in\mathbb{R}^{6\times(n+6)}$. The constant term $M\boldsymbol{g}\delta t$ being negligible w.r.t other impulses. The compact form yields

where $S\in\mathbb{R}^{6\times({m_{1}}+{m_{2}})}$ gathers the associated Jacobian matrices.

We define the optimization variable as:

where $\sigma_{e}=\sigma_{{m_{1}}}\cup\sigma_{{m_{2}}}$, thus $\boldsymbol{\iota}_{\sigma_{e}}\in\mathbb{R}^{({m_{1}}+{m_{2}})}$, and all the impulses are defined in the inertial frame²² 2 The centroidal frame keeps the same orientation of the inertial frame. Thus we can use $\boldsymbol{\iota}$ for both (22) and (26).. Assuming that the robot is applying one or multiple impacts along with given directions in the next control iteration, we formulate the Impulse Distribution Quadratic Program (IDQP) that returns the optimal $\boldsymbol{u}^{*}$:

where scalar weights $\gamma_{a},\gamma_{b}>0$ impose the relative importance between the two quadratic costs $\left\lVert\mathbf{M}\Delta\dot{\mathbf{q}}\right\rVert^{2}$ and $\left\lVert\boldsymbol{\iota}_{\sigma_{e}}\right\rVert^{2}$. In general, these two costs cannot be both wholly minimized, i.e., they conflict with each other. We chose the inertia matrix in the cost function, to prioritize among the task-space impulse $\boldsymbol{\iota}$ and the joint space impulse equivalent $\mathbf{M}\Delta\dot{\mathbf{q}}$. We reformulate the IDQP in matrix form

with block matrices

where we collect the Jacobians of the ${m_{2}}$ end-effectors that are applying impacts as:

Given the impact dynamics model (18), the end-effector velocity jumps $\Delta\boldsymbol{\dot{x}}_{\sigma_{{m_{2}}}}\in\mathbb{R}^{3{m_{2}}}$ are represented as the decision variable of (15), i.e., $\ddot{\mathbf{q}}$. The weighting matrix $H\in\mathbb{R}^{(n+3({m_{1}}+{m_{2}}))\times(n+3({m_{1}}+{m_{2}}))}$ is

The solution to the IDQP (28), i.e., the joint velocity jump ${\Delta\dot{\mathbf{q}}}^{*}$ and the impulse ${\boldsymbol{\iota}}^{*}_{\sigma_{e}}$ at the time step $t_{k+1}$, can be obtained analytically:

where $B^{H,\dagger}$ is the $H$-weighted pseudo-inverse of $B$:

Only the right $3{m_{2}}$ columns (related to the $3{m_{2}}$ lower, nonzero elements of $\mathbf{b}$) of $B^{H,\dagger}$ are of interest. Note that $K_{\Delta\dot{\mathbf{q}}}$, $K_{\boldsymbol{\iota}}$ are decomposed into individual end-effector contributions:

where $K_{\boldsymbol{\iota}}\in\mathbb{R}^{3({m_{1}}+{m_{2}})\times 3{m_{2}}}.$

Here, we summarize all the essential state jumps. In a generic form, the quantity jump ${\Delta\boldsymbol{\lambda}}_{k+1}$ relates to joint accelerations ${\ddot{\mathbf{q}}}_{k}$ and is linearly decomposed as

where the matrix $\mathcal{J}_{\Delta\boldsymbol{\lambda}}$ and $\mathcal{C}_{\Delta\boldsymbol{\lambda}}$ incorporates the impact model and the impulse propagation through the kinematic tree. The coefficient of restitution contributes to $\mathcal{J}_{\Delta\boldsymbol{\lambda}}$ and $\mathcal{C}_{\Delta\boldsymbol{\lambda}}$ as a scalar multiplier by $(1+c_{\text{r}})$. Hence, higher coefficients lead to higher jumps. We specify the matrix $\mathcal{J}_{\Delta\boldsymbol{\lambda}}$ and $\mathcal{C}_{\Delta\boldsymbol{\lambda}}$ on a case-by-case basis in the rest of this section. These expressions are used in Sec. 5 to formulate impact-aware constraints.