<figure><img src="image_1.png" /></figure>

HAL
open science

# Supplementary Material: Multi-Contact Whole-Body Force Control for Position-Controlled Robots

Quentin Rouxel, Serena Ivaldi, Jean-Baptiste Mouret

<figure><img src="image_2.png" /></figure>

## To cite this version:

Quentin Rouxel, Serena Ivaldi, Jean-Baptiste Mouret. Supplementary Material: Multi-Contact Whole-Body Force Control for Position-Controlled Robots. 2024. hal-04557956

HAL Id: hal-04557956

https://inria.hal.science/hal-04557956

Submitted on 26 Apr 2024

HAL is a multi-disciplinary open access
archive for the deposit and dissemination of sci-
entific research documents, whether they are pub-
lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.

L'archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diffusion de documents
scientifiques de niveau recherche, publiés ou non,
émanant des établissements d'enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.

<figure><img src="image_3.png" /></figure>

Distributed under a Creative Commons Attribution 4.0 International License
<figure><img src="image_1.png" /></figure>

HAL
open science

# Supplementary Material: Multi-Contact Whole-Body Force Control for Position-Controlled Robots

Quentin Rouxel, Serena Ivaldi, Jean-Baptiste Mouret

<figure><img src="image_2.png" /></figure>

## To cite this version:

Quentin Rouxel, Serena Ivaldi, Jean-Baptiste Mouret. Supplementary Material: Multi-Contact Whole-Body Force Control for Position-Controlled Robots. 2024. hal-04557956

HAL Id: hal-04557956

https://inria.hal.science/hal-04557956v1

Submitted on 26 Apr 2024

HAL is a multi-disciplinary open access
archive for the deposit and dissemination of sci-
entific research documents, whether they are pub-
lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.

L'archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diffusion de documents
scientifiques de niveau recherche, publiés ou non,
émanant des établissements d'enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.

<figure><img src="image_3.png" /></figure>

Distributed under a Creative Commons Attribution 4.0 International License
## Supplementary Material:
Multi-Contact Whole-Body Force Control
for Position-Controlled Robots

Quentin Rouxel, Serena Ivaldi, and Jean-Baptiste Mouret

<figure><img src="image_4.png" /><figcaption>Figure S1. Comparison of our SEIKO Controller and effector admittance [17] for tracking inconsistent references on Talos humanoid robot simulated in double support using Gazebo simulator (left). SEIKO Retargeting is used to generate a configuration where most of the robot's weight is positioned above the right foot. At $t=1s$, the reference sent to the controller is overridden, requesting an equal weight distribution between the two feet, inconsistent with the desired posture and CoM position. Both controllers are initiated at $t=6s$ and foot normal force tracking is displayed for each controller (right). SEIKO Controller successfully tracks the overridden reference by shifting the CoM of the robot, while the effector admittance controller results in the robot falling.</figcaption></figure>

<figure><img src="image_5.png" /><figcaption>Figure S2. Comparison between our SEIKO Controller and effector admittance [17] for hand multi-contact and large model errors. Talos humanoid robot is simulated in Gazebo, with a posture featuring both feet and the right hand in contact (left). Tracking of the right hand force is compared across several initial contact forces (right). In the second row, a large external vertical force (200N) is applied on the robot's torso. The effector admittance scheme fails to track the reference when faced with large external disturbances.</figcaption></figure>

## SUPPLEMENTARY MATERIAL

### A. Comparison With Effector Admittance Control

As detailed in Section II, prior studies focusing on position-controlled robots [9]–[11] often use the method introduced in [8] for regulating contact forces. These approaches named “effector admittance”, “foot force difference control”, or “damping control for limb ends” all operate on a similar principle. They employ an admittance feedback law applied to each effector of the robot, which adjusts its Cartesian pose reference based on desired and measured contact forces before being realized via Inverse Kinematics (IK).

To compare our method against this baseline, we specifically implemented the “damping control” approach described in [17], Section IV.C. We substituted the SEIKO Controller block in Fig. 2 with an IK module. Prior to the IK calculation, we apply the following admittance law to each enabled and disabled effector:

$$X^{\text{IK}}(t) = X^{\text{d}}(t) \oplus \Delta r(t), \quad (15)$$

$$\Delta r(t+\Delta t) = \Delta r(t) \oplus \Delta t (K_f(\tilde{\lambda}^{\text{read}} - \lambda^d) \ominus K_s \Delta r(t)), \quad (16)$$

where $X^{\text{IK}}(t) \in SE(3)$ is the corrected effector pose input sent to the IK, $\Delta r(t) \in SE(3)$ is the computed admittance pose
## Supplementary Material:
Multi-Contact Whole-Body Force Control
for Position-Controlled Robots

Quentin Rouxel, Serena Ivaldi, and Jean-Baptiste Mouret

<figure><img src="image_4.png" /><figcaption>Figure S1. Comparison of our SEIKO Controller and effector admittance [17] for tracking inconsistent references on Talos humanoid robot simulated in double support using Gazebo simulator (left). SEIKO Retargeting is used to generate a configuration where most of the robot's weight is positioned above the right foot. At $t=1s$, the reference sent to the controller is overridden, requesting an equal weight distribution between the two feet, inconsistent with the desired posture and CoM position. Both controllers are initiated at $t=6s$ and foot normal force tracking is displayed for each controller (right). SEIKO Controller successfully tracks the overridden reference by shifting the CoM of the robot, while the effector admittance controller results in the robot falling.</figcaption></figure>

<figure><img src="image_5.png" /><figcaption>Figure S2. Comparison between our SEIKO Controller and effector admittance [17] for hand multi-contact and large model errors. Talos humanoid robot is simulated in Gazebo, with a posture featuring both feet and the right hand in contact (left). Tracking of the right hand force is compared across several initial contact forces (right). In the second row, a large external vertical force (200N) is applied on the robot's torso. The effector admittance scheme fails to track the reference when faced with large external disturbances.</figcaption></figure>

## SUPPLEMENTARY MATERIAL

### A. Comparison With Effector Admittance Control

As detailed in Section II, prior studies focusing on position-controlled robots [9]–[11] often use the method introduced in [8] for regulating contact forces. These approaches named “effector admittance”, “foot force difference control”, or “damping control for limb ends” all operate on a similar principle. They employ an admittance feedback law applied to each effector of the robot, which adjusts its Cartesian pose reference based on desired and measured contact forces before being realized via Inverse Kinematics (IK).

To compare our method against this baseline, we specifically implemented the “damping control” approach described in [17], Section IV.C. We substituted the SEIKO Controller block in Fig. 2 with an IK module. Prior to the IK calculation, we apply the following admittance law to each enabled and disabled effector:

$$X^{\text{IK}}(t) = X^{\text{d}}(t) \oplus \Delta r(t), \quad (15)$$

$$\Delta r(t+\Delta t) = \Delta r(t) \oplus \Delta t (K_f(\tilde{\lambda}^{\text{read}} - \lambda^d) \ominus K_s \Delta r(t)), \quad (16)$$

where $X^{\text{IK}}(t) \in SE(3)$ is the corrected effector pose input sent to the IK, $\Delta r(t) \in SE(3)$ is the computed admittance pose
offset (slightly abusing the SE(3) notation), and $K_f, K_s \in \mathbb{R}$ are the manually tuned wrench and spring gains, respectively. It is worth noting that we did not implement for our comparison the two additional feedback effects described in [17], as detailed in Sections IV.A and B. These two effects only use position measurements to regulate the centroidal pose. However, our Talos robot is controlled in position with stiff gains, and no significant position errors can be measured with respect to the desired posture optimized by our SEIKO Retargeting (posture flexibility is unobservable on joint sensors).

From a theoretical standpoint, contact forces in rigid multi-contact scenarios are influenced by two key factors. Firstly, the distribution of forces among different contacts can be selected within the redundant contact nullspace without inducing motion. Secondly, this nullspace is defined by the posture, specifically the position of the CoM. As its formulation is not whole-body, the effector admittance approach only utilizes the first component of force redistribution, while our SEIKO Controller offers a unified formulation capable of leveraging both force redistribution and postural adjustments.

For instance, the effector admittance approach relies on the assumption that retracting the effector away from the surface will decrease the contact force. While this assumption holds true in many scenarios, it can lead to failure when confronted with significant model errors, flexibility, or challenging postures that bring the robot's configuration close to its feasibility limits. The following simulation experiments highlight two cases where the effector admittance method fails to accurately track the force reference due to its inability to account for whole-body postural adjustments.

1) **Inconsistent Reference Input:** Fig. S1 presents a clear example of a simulated double support scenario where retracting the foot position away from the surface fails to decrease the contact force. This situation arises when the planner module (in this case, SEIKO Retargeting) outputs an erroneous desired reference, subsequently sent to the underlying controller. Due to the inconsistency between the desired contact force and the desired posture, the controller is unable to track both simultaneously. The effector admittance control attempts to lift the right foot, which bears most of the robot's weight, leading to the robot's fall. In contrast, our SEIKO Controller prioritizes balance by focusing on tracking the contact forces and adjusting the posture through the whole-body formulation.

2) **Large Model Errors:** In numerous instances where the robot's configuration remained away from the feasibility limits, we observed that the effector admittance method effectively regulated the contact forces. However, the robustness of the controller is challenged in scenarios with large model errors. Fig. S2 illustrates a failure case where a substantial external force applied to the torso necessitates the adaptation of the robot's posture to accurately track the desired contact forces, a capability only exhibited by the SEIKO Controller.

## B. Derivative of Equilibrium Equations

In the quasi-static case, the whole-body equilibrium equation in joint space is expressed as:

$$g(q) = S\tau + J(q)^{\mathrm{T}}\lambda. \quad (17)$$

Since this equation is nonlinear with respect to $q$, we linearize it by approximating its derivative through consideration of small variations in the configuration $(q+\Delta q, \lambda+\Delta \lambda, \tau+\Delta \tau)$:

$$g(q+\Delta q) = S(\tau+\Delta\tau)+J(q+\Delta q)^{\mathrm{T}}(\lambda+\Delta\lambda), \quad (18)$$

which is linearized and approximated as following considering partial derivatives of the gravity vector and the contact Jacobian matrix:

$$g(q) + \frac{\partial g}{\partial q}(q) \Delta q = S\tau + S\Delta\tau + J(q)^{\mathrm{T}}\lambda + J(q)^{\mathrm{T}}\Delta\lambda + \left(\frac{\partial J^{\mathrm{T}}}{\partial q}(q)\lambda\right) \Delta q + \left(\frac{\partial J^{\mathrm{T}}}{\partial q}(q)\Delta\lambda\right) \Delta q. \quad (19)$$

We further neglect the following term, considering it to be of second order:

$$\left(\frac{\partial J^{\mathrm{T}}}{\partial q} \Delta \lambda\right) \Delta q \approx 0, \quad (20)$$

which leads to the derivative-based linear approximation of the equilibrium equation:

$$g(q) + \frac{\partial g}{\partial q}(q) \Delta q = S\tau + S\Delta\tau + J(q)^{\mathrm{T}}\lambda + J(q)^{\mathrm{T}}\Delta\lambda + \left(\frac{\partial J^{\mathrm{T}}}{\partial q}(q)\lambda\right) \Delta q. \quad (21)$$

Note that if we choose instead to linearize the equilibrium equation by only considering small variations in the posture $(q+\Delta q, \lambda, \tau)$, then the following term appears:

$$\left(\frac{\partial J^{\mathrm{T}}}{\partial q}(q)\lambda\right) \Delta q, \quad (22)$$

which is of first order but also bilinear with respect to the decision variables $(\Delta q, \lambda)$. However, bilinear terms cannot be expressed in a QP formulation. Therefore, instead of neglecting the term (22), which is of first order, the former expression (21) provides a better approximation of the equilibrium equation by considering $(\Delta q, \Delta \lambda, \Delta \tau)$ as the decision variables.

In the formulation of SEIKO Retargeting, the equality constraint $(9g)$ is obtained by setting $q=q^d$ and $\lambda=\lambda^d$ in (21).

In the formulation of SEIKO Controller, the derivative of the joint flexibility model (4) is combined with (21) by setting $q=q^{\mathrm{flex}}$, $\lambda=\lambda^{\mathrm{flex}}$, $\tau=\tau^{\mathrm{flex}}$. This results in the derivative-based linear approximation of the equilibrium under flexibility:

$$g(q^{\mathrm{flex}}) + \frac{\partial g}{\partial q}(q^{\mathrm{flex}}) \Delta q^{\mathrm{flex}} = S\tau^{\mathrm{flex}} + SK\Delta\theta^{\mathrm{cmd}} - SKS'\Delta q^{\mathrm{flex}} + J(q^{\mathrm{flex}})^{\mathrm{T}}\lambda^{\mathrm{flex}} + J(q^{\mathrm{flex}})^{\mathrm{T}}\Delta\lambda^{\mathrm{flex}} + \left(\frac{\partial J^{\mathrm{T}}}{\partial q}(q^{\mathrm{flex}})\lambda^{\mathrm{flex}}\right) \Delta q^{\mathrm{flex}}, \quad (23)$$

yielding equation (5).
offset (slightly abusing the SE(3) notation), and $K_f, K_s \in \mathbb{R}$ are the manually tuned wrench and spring gains, respectively. It is worth noting that we did not implement for our comparison the two additional feedback effects described in [17], as detailed in Sections IV.A and B. These two effects only use position measurements to regulate the centroidal pose. However, our Talos robot is controlled in position with stiff gains, and no significant position errors can be measured with respect to the desired posture optimized by our SEIKO Retargeting (posture flexibility is unobservable on joint sensors).

From a theoretical standpoint, contact forces in rigid multi-contact scenarios are influenced by two key factors. Firstly, the distribution of forces among different contacts can be selected within the redundant contact nullspace without inducing motion. Secondly, this nullspace is defined by the posture, specifically the position of the CoM. As its formulation is not whole-body, the effector admittance approach only utilizes the first component of force redistribution, while our SEIKO Controller offers a unified formulation capable of leveraging both force redistribution and postural adjustments.

For instance, the effector admittance approach relies on the assumption that retracting the effector away from the surface will decrease the contact force. While this assumption holds true in many scenarios, it can lead to failure when confronted with significant model errors, flexibility, or challenging postures that bring the robot's configuration close to its feasibility limits. The following simulation experiments highlight two cases where the effector admittance method fails to accurately track the force reference due to its inability to account for whole-body postural adjustments.

1) **Inconsistent Reference Input:** Fig. S1 presents a clear example of a simulated double support scenario where retracting the foot position away from the surface fails to decrease the contact force. This situation arises when the planner module (in this case, SEIKO Retargeting) outputs an erroneous desired reference, subsequently sent to the underlying controller. Due to the inconsistency between the desired contact force and the desired posture, the controller is unable to track both simultaneously. The effector admittance control attempts to lift the right foot, which bears most of the robot's weight, leading to the robot's fall. In contrast, our SEIKO Controller prioritizes balance by focusing on tracking the contact forces and adjusting the posture through the whole-body formulation.

2) **Large Model Errors:** In numerous instances where the robot's configuration remained away from the feasibility limits, we observed that the effector admittance method effectively regulated the contact forces. However, the robustness of the controller is challenged in scenarios with large model errors. Fig. S2 illustrates a failure case where a substantial external force applied to the torso necessitates the adaptation of the robot's posture to accurately track the desired contact forces, a capability only exhibited by the SEIKO Controller.

## B. Derivative of Equilibrium Equations

In the quasi-static case, the whole-body equilibrium equation in joint space is expressed as:

$$g(q) = S\tau + J(q)^{\mathrm{T}}\lambda. \qquad (17)$$

Since this equation is nonlinear with respect to $q$, we linearize it by approximating its derivative through consideration of small variations in the configuration $(q+\Delta q, \lambda+\Delta \lambda, \tau+\Delta \tau)$:

$$g(q+\Delta q) = S(\tau+\Delta\tau)+J(q+\Delta q)^{\mathrm{T}}(\lambda+\Delta\lambda), \qquad (18)$$

which is linearized and approximated as following considering partial derivatives of the gravity vector and the contact Jacobian matrix:

$$g(q) + \frac{\partial g}{\partial q}(q) \Delta q = S\tau + S\Delta\tau + J(q)^{\mathrm{T}}\lambda + J(q)^{\mathrm{T}}\Delta\lambda + \left(\frac{\partial J^{\mathrm{T}}}{\partial q}(q)\lambda\right) \Delta q + \left(\frac{\partial J^{\mathrm{T}}}{\partial q}(q)\Delta\lambda\right) \Delta q. \qquad (19)$$

We further neglect the following term, considering it to be of second order:

$$\left(\frac{\partial J^{\mathrm{T}}}{\partial q} \Delta \lambda\right) \Delta q \approx 0, \qquad (20)$$

which leads to the derivative-based linear approximation of the equilibrium equation:

$$g(q) + \frac{\partial g}{\partial q}(q) \Delta q = S\tau + S\Delta\tau + J(q)^{\mathrm{T}}\lambda + J(q)^{\mathrm{T}}\Delta\lambda + \left(\frac{\partial J^{\mathrm{T}}}{\partial q}(q)\lambda\right) \Delta q. \qquad (21)$$

Note that if we choose instead to linearize the equilibrium equation by only considering small variations in the posture $(q+\Delta q, \lambda, \tau)$, then the following term appears:

$$\left(\frac{\partial J^{\mathrm{T}}}{\partial q}(q)\lambda\right) \Delta q, \qquad (22)$$

which is of first order but also bilinear with respect to the decision variables $(\Delta q, \lambda)$. However, bilinear terms cannot be expressed in a QP formulation. Therefore, instead of neglecting the term (22), which is of first order, the former expression (21) provides a better approximation of the equilibrium equation by considering $(\Delta q, \Delta \lambda, \Delta \tau)$ as the decision variables.

In the formulation of SEIKO Retargeting, the equality constraint $(9g)$ is obtained by setting $q=q^d$ and $\lambda=\lambda^d$ in (21).

In the formulation of SEIKO Controller, the derivative of the joint flexibility model (4) is combined with (21) by setting $q=q^{\mathrm{flex}}$, $\lambda=\lambda^{\mathrm{flex}}$, $\tau=\tau^{\mathrm{flex}}$. This results in the derivative-based linear approximation of the equilibrium under flexibility:

$$g(q^{\mathrm{flex}}) + \frac{\partial g}{\partial q}(q^{\mathrm{flex}}) \Delta q^{\mathrm{flex}} = S\tau^{\mathrm{flex}} + SK\Delta\theta^{\mathrm{cmd}} - SKS'\Delta q^{\mathrm{flex}} + J(q^{\mathrm{flex}})^{\mathrm{T}}\lambda^{\mathrm{flex}} + J(q^{\mathrm{flex}})^{\mathrm{T}}\Delta\lambda^{\mathrm{flex}} + \left(\frac{\partial J^{\mathrm{T}}}{\partial q}(q^{\mathrm{flex}})\lambda^{\mathrm{flex}}\right) \Delta q^{\mathrm{flex}}, \qquad (23)$$

yielding equation (5).
### C. Whole-Body Balance Conditions

Ensuring the robustness and stability of the overall system requires sending feasible desired configurations to the controller, taking into account the robot's balance to prevent potential falls. Balance sufficient conditions such as the projection of the CoM onto the support polygon (for flat ground) or the Gravito-Inertial Wrench Cone (GIWC) [33] are advantageous for reduced models like the centroidal model, as they enable faster computations.

But these conditions do not consider the whole-body state and thus overlook joint kinematic and actuator torque limits. Instead, our formulation considers the complete whole-body configuration in quasi-static equilibrium. In this case, considering the CoM is not necessary, and both our SEIKO Retargeting and Controller formulation deliberately never explicitly include CoM consideration.

A sufficient condition for the robot to be balanced is that all contacts remain asymptotically stable, i.e., in contact with the environment without any relative motion between the effector in contact and the surface. For these conditions to be satisfied, both in static and dynamic cases, it is sufficient that for each contact, the contact wrench remains within a 6d or 3d convex polytope (see Section D). These conditions are explicitly enforced in SEIKO Retargeting QP by the inequality constraints in equation (9k).

Given our quasi-static assumption and instantaneous formulation (without considering the future), enforcing conditions at the next time step is sufficient to meet the asymptotic requirement. If these conditions hold true at the next time step, they should theoretically extend into the future. Furthermore, the quasi-static equilibrium assumption is enforced by ensuring that the computed posture and contact forces satisfy the equilibrium equation (1) through its linear approximation in equations (2) and (5).

### D. Contact Stability Conditions

A plane contact, which constrains 6 DoFs, between the robot's effector and the environment is considered stable when the relative linear and angular velocities between the effector and the contact surface are zero. Similarly, for point contact (3 DoFs), only the linear velocity is taken into account. For stability to be ensured, a sufficient condition is that the forces and torques, denoted by $\lambda = [f_x \ f_y \ f_z \ \tau_x \ \tau_y \ \tau_z]^T$, exerted on the environment by this contact, must satisfy the following convex inequalities [34] (the unilaterally, friction pyramid and Center of Pressure (CoP) constraints):

$$f_z \ge 0, \qquad (24)$$

$$\left| \frac{f_x}{f_z} \right| \le \mu, \quad \left| \frac{f_y}{f_z} \right| \le \mu, \qquad (25)$$

$$\left| \frac{\tau_y}{f_z} \right| \le CoP_x^{\max}, \quad \left| \frac{\tau_x}{f_z} \right| \le CoP_y^{\max}, \qquad (26)$$

$$\tau_z^{\min} \le \tau_z \le \tau_z^{\max}, \qquad (27)$$

where $\mu, CoP_x^{\max}, CoP_y^{\max} \in R$ are respectively the friction coefficient, and the maximum half lengths in the X and Y directions of the rectangular contact surface. The local Z axis

is aligned with the contact normal. Please refer to [34] for the expression of $\tau_z^{\min}$ and $\tau_z^{\max}$.

Additionally, a bound on the maximum normal force $f^{\max} \in R$ can be included to prevent excessive force from being applied, particularly on a fragile effector:

$$f_z \le f^{\max}. \qquad (28)$$

These inequality equations can be linearly rewritten as (refer to [35] for details):

$$C_{\text{contact}}\lambda + c_{\text{contact}} \ge 0, \qquad (29)$$

where $C_{\text{contact}} \in R^{18 \times 6}$ and $c_{\text{contact}} \in R^{18}$ for a plane contact, and $C_{\text{contact}} \in R^{6 \times 3}$ and $c_{\text{contact}} \in R^6$ for a point contact.

In (9k), all the contacts are stacked and $\lambda$ is replaced with $\lambda^d + \Delta\lambda^d$.

### E. Improved Contact Switching Procedure

We enhanced the contact switching procedure compared to our prior SEIKO Retargeting work [22]. To remove a contact, the desired contact force must smoothly be reduced to zero, often necessitating a smooth adjustment of the whole-body posture. To address this, SEIKO Retargeting employs a strategy of increasing the weight associated with the regularization task (9e): by heavily penalizing the contact force, the whole-body optimization naturally diminishes the force and adjusts the posture accordingly.

In [22], we empirically observed that smoothly increasing the weight of task (9e) using exponential growth yielded satisfactory switching motions. The contact is removed from the formulation when the desired force falls below a small threshold. However, the exponential choice and tuning of this procedure were arbitrary and not based on the robot's physical limits.

In this letter, we propose a refinement of this procedure. Instead of gradually increasing the weight of task (9e), we set it instantaneously to a high value (typically $1e4$). The smoothness of the transition is ensured by introducing two inequality constraints on the maximum rates of change for joint positions $\dot{\theta}^{\max}$ (9l) and contact wrenches $\dot{\lambda}^{\max}$ (9m).

While the overall generated motions between the two procedures are very similar, this updated approach requires fewer arbitrary choices and tuning. Additionally, it offers the added benefit of explicit control over transition duration by setting the rate of change limits, which has clear physical semantics.

## SUPPLEMENTARY REFERENCES

[33] S. Caron, Q.-C. Pham, and Y. Nakamura, "Leveraging cone double description for multi-contact stability of humanoids with applications to statics and dynamics." in *Robotics: science and systems*, vol. 11, 2015.

[34] —, "Stability of surface contacts for humanoid robots: Closed-form formulae of the contact wrench cone for rectangular support areas," in *IEEE ICRA*, 2015.

[35] S. Feng, E. Whitman, X. Xinjilefu, and C. G. Atkeson, "Optimization-based full body control for the darpa robotics challenge," *Journal of field robotics*, vol. 32, no. 2, 2015.
### C. Whole-Body Balance Conditions

Ensuring the robustness and stability of the overall system requires sending feasible desired configurations to the controller, taking into account the robot's balance to prevent potential falls. Balance sufficient conditions such as the projection of the CoM onto the support polygon (for flat ground) or the Gravito-Inertial Wrench Cone (GIWC) [33] are advantageous for reduced models like the centroidal model, as they enable faster computations.

But these conditions do not consider the whole-body state and thus overlook joint kinematic and actuator torque limits. Instead, our formulation considers the complete whole-body configuration in quasi-static equilibrium. In this case, considering the CoM is not necessary, and both our SEIKO Retargeting and Controller formulation deliberately never explicitly include CoM consideration.

A sufficient condition for the robot to be balanced is that all contacts remain asymptotically stable, i.e., in contact with the environment without any relative motion between the effector in contact and the surface. For these conditions to be satisfied, both in static and dynamic cases, it is sufficient that for each contact, the contact wrench remains within a 6d or 3d convex polytope (see Section D). These conditions are explicitly enforced in SEIKO Retargeting QP by the inequality constraints in equation (9k).

Given our quasi-static assumption and instantaneous formulation (without considering the future), enforcing conditions at the next time step is sufficient to meet the asymptotic requirement. If these conditions hold true at the next time step, they should theoretically extend into the future. Furthermore, the quasi-static equilibrium assumption is enforced by ensuring that the computed posture and contact forces satisfy the equilibrium equation (1) through its linear approximation in equations (2) and (5).

### D. Contact Stability Conditions

A plane contact, which constrains 6 DoFs, between the robot's effector and the environment is considered stable when the relative linear and angular velocities between the effector and the contact surface are zero. Similarly, for point contact (3 DoFs), only the linear velocity is taken into account. For stability to be ensured, a sufficient condition is that the forces and torques, denoted by $\lambda = [f_x \ f_y \ f_z \ \tau_x \ \tau_y \ \tau_z]^T$, exerted on the environment by this contact, must satisfy the following convex inequalities [34] (the unilaterally, friction pyramid and Center of Pressure (CoP) constraints):

$$f_z \ge 0, \qquad (24)$$

$$\left|\frac{f_x}{f_z}\right| \le \mu, \quad \left|\frac{f_y}{f_z}\right| \le \mu, \qquad (25)$$

$$\left|\frac{\tau_y}{f_z}\right| \le CoP_x^{\max}, \quad \left|\frac{\tau_x}{f_z}\right| \le CoP_y^{\max}, \qquad (26)$$

$$\tau_z^{\min} \le \tau_z \le \tau_z^{\max}, \qquad (27)$$

where $\mu, CoP_x^{\max}, CoP_y^{\max} \in R$ are respectively the friction coefficient, and the maximum half lengths in the X and Y directions of the rectangular contact surface. The local Z axis

is aligned with the contact normal. Please refer to [34] for the expression of $\tau_z^{\min}$ and $\tau_z^{\max}$.

Additionally, a bound on the maximum normal force $f^{\max} \in R$ can be included to prevent excessive force from being applied, particularly on a fragile effector:

$$f_z \le f^{\max}. \qquad (28)$$

These inequality equations can be linearly rewritten as (refer to [35] for details):

$$C_{\text{contact}}\lambda + c_{\text{contact}} \ge 0, \qquad (29)$$

where $C_{\text{contact}} \in R^{18 \times 6}$ and $c_{\text{contact}} \in R^{18}$ for a plane contact, and $C_{\text{contact}} \in R^{6 \times 3}$ and $c_{\text{contact}} \in R^6$ for a point contact.

In (9k), all the contacts are stacked and $\lambda$ is replaced with $\lambda^d + \Delta\lambda^d$.

### E. Improved Contact Switching Procedure

We enhanced the contact switching procedure compared to our prior SEIKO Retargeting work [22]. To remove a contact, the desired contact force must smoothly be reduced to zero, often necessitating a smooth adjustment of the whole-body posture. To address this, SEIKO Retargeting employs a strategy of increasing the weight associated with the regularization task (9e): by heavily penalizing the contact force, the whole-body optimization naturally diminishes the force and adjusts the posture accordingly.

In [22], we empirically observed that smoothly increasing the weight of task (9e) using exponential growth yielded satisfactory switching motions. The contact is removed from the formulation when the desired force falls below a small threshold. However, the exponential choice and tuning of this procedure were arbitrary and not based on the robot's physical limits.

In this letter, we propose a refinement of this procedure. Instead of gradually increasing the weight of task (9e), we set it instantaneously to a high value (typically $1e4$). The smoothness of the transition is ensured by introducing two inequality constraints on the maximum rates of change for joint positions $\dot{\theta}^{\max}$ (9l) and contact wrenches $\dot{\lambda}^{\max}$ (9m).

While the overall generated motions between the two procedures are very similar, this updated approach requires fewer arbitrary choices and tuning. Additionally, it offers the added benefit of explicit control over transition duration by setting the rate of change limits, which has clear physical semantics.

## SUPPLEMENTARY REFERENCES

[33] S. Caron, Q.-C. Pham, and Y. Nakamura, “Leveraging cone double description for multi-contact stability of humanoids with applications to statics and dynamics,” in *Robotics: science and systems*, vol. 11, 2015.

[34] ——, “Stability of surface contacts for humanoid robots: Closed-form formulae of the contact wrench cone for rectangular support areas,” in *IEEE ICRA*, 2015.

[35] S. Feng, E. Whitman, X. Xinjilefu, and C. G. Atkeson, “Optimization-based full body control for the darpa robotics challenge,” *Journal of field robotics*, vol. 32, no. 2, 2015.