# Whole-Body Motion Control in Orbodial Scenarios through Centroidal Momentum Conservation

Enrico Mingo Hoffman, Antonio Paolillo, Jean-Baptiste Mouret, Serena Ivaldi

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

To cite this version:

Enrico Mingo Hoffman, Antonio Paolillo, Jean-Baptiste Mouret, Serena Ivaldi. Whole-Body Motion Control in Orbodial Scenarios through Centroidal Momentum Conservation. 2024. hal-04588276v3

HAL Id: hal-04588276

https://hal.science/hal-04588276v3

Preprint submitted on 19 Jun 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_1.png" /></figure>

HAL
open science

# Whole-Body Motion Control in Orbital Scenarios through Centroidal Momentum Conservation

Enrico Mingo Hoffman, Antonio Paolillo, Jean-Baptiste Mouret, Serena Ivaldi

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

## To cite this version:

Enrico Mingo Hoffman, Antonio Paolillo, Jean-Baptiste Mouret, Serena Ivaldi. Whole-Body Motion Control in Orbital Scenarios through Centroidal Momentum Conservation. 2024. hal-04588276v3

HAL Id: hal-04588276

https://hal.science/hal-04588276v3

Preprint submitted on 19 Jun 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
# Whole-Body Motion Control in Orbital Scenarios through Centroidal Momentum Conservation

Enrico Mingo Hoffman†, Antonio Paolillo‡, Jean-Baptiste Mouret† and Serena Ivaldi†

*Abstract*—Manipulation in orbit represents a challenging task because the motion of the limbs can assist or even generate the angular motion of the floating base due to the conservation of centroidal momentum. Exploiting this aspect is fundamental for correctly accomplishing manipulation tasks in multi-limbed robotic systems employed in orbit. This short paper proposes a whole-body motion controller for teleoperation or visual servoing that can exploit collision-free null-space motions of the limbs to generate the desired angular motion of the base in the absence of contacts and gravity. The results are demonstrated through kinematic and dynamic simulations using the model of a humanoid robot with multiple Degrees of Freedom.

## I. INTRODUCTION

Multi-limbed robots, e.g. humanoids, have the great advantage of being general-purpose platforms that require minimal customization to operate in domestic or industrial scenarios and perform complex loco-manipulation tasks. The significant potential of these machines and their domain of applicability extends beyond the ground and can be successfully employed underwater, in the air, or in space. In particular, the space scenario opens new opportunities and poses challenges from various perspectives, including mechanical and sensory equipment design, actuation methods, and control paradigms. To facilitate their mobility in this domain, the articulations can be exploited to generate or assist the floating-base (FB) motion, especially in the absence of contacts. In fact, the nonholonomy of angular momentum conservation allows controlling the robot's orientation in the absence of contacts by moving its articulations, much like a cat that can land on its feet after falling from an upside-down posture. The same effect is also experienced in microgravity conditions by astronauts, who can change their body orientation with cyclic motions of arms and legs, as shown in Figure 1. In this work, we consider the scenario where a humanoid robot is performing Intra/Extravehicular Activities (IEVAs) and needs to observe its surroundings to find visual references for its motion. For example, the system may be tele-operated to align with a tool for further manipulation, whether for rescue or maintenance purposes. In practice, Visual Servoing (VS) or direct tele-operation of the base could be used to rotate the robot towards the tool. The Centroidal Momentum Conservation (CMC) of the robot can be exploited to generate motions of the limbs and

<figure><img src="image_2.png" /><figcaption>Fig. 1: In the absence of contacts, articulated systems can rotate by moving their articulations due to the nonholonomic nature of angular momentum conservation. This phenomenon is experienced by NASA astronauts in microgravity conditions and is replicated with humanoid robots in this work.</figcaption></figure>

achieve the desired rotation. Inspired by these considerations, we aim to extend the whole-body motion control of multi-limbed robots with the ability to change their body orientation using collision-free motion of the limbs in scenarios where contacts may not occur. Building on our previous work, where we integrated VS and CMC within the same whole-body control framework using Quadratic Programming (QP) optimization [1], we now further integrate collision avoidance as a constraint within the QP. This short paper recaps the theory from [1], extends it to consider collision avoidance, and reports preliminary results from kinematic simulations.

## II. COLLISION-FREE CENTROIDAL MOMENTUM CONSTRAINED VISUAL SERVOING

Let us consider a humanoid robot with $n$ Degrees of Freedom (DoFs). Its generalized coordinates can be expressed by:

$$ q = \begin{bmatrix} q_b \\ q_a \end{bmatrix} \in \mathbb{R}^{d+n}, \quad v = \begin{bmatrix} \dot{p}_b \\ \omega_b \\ \dot{q}_a \end{bmatrix} \in \mathbb{R}^{6+n} \quad (1) $$

where $q_a \in \mathbb{R}^n$ contains the joints position of the articulations, while $q_b \in S\mathcal{O}(3)$ is a representation of the FB pose, e.g. a position and a quaternion vector ($d = 7$), $\dot{p}_b, \omega_b \in \mathbb{R}^3$ are the linear and angular velocity of the FB, respectively, and $\dot{q}_a \in \mathbb{R}^n$ are the joint velocities. It is possible to define the Visual Servoing task relative to a certain camera frame:

$$ \dot{s} = J_s(s, q) v \quad (2) $$

<footnote>† Université de Lorraine, CNRS, Inria, LORIA, France, 615 rue du Jardin Botanique, 54600 Villers les Nancy, France. E-mail: {jean-baptiste.mouret, enrico.mingo-hoffman, serena.ivaldi}@inria.fr</footnote>

<footnote>‡ Dalle Molle Institute for Artificial Intelligence (IDSIA), USI-SUPSI, Lugano, Switzerland antonio.paolillo@idsia.ch</footnote>
# Whole-Body Motion Control in Orbital Scenarios through Centroidal Momentum Conservation

Enrico Mingo Hoffman†, Antonio Paolillo‡, Jean-Baptiste Mouret† and Serena Ivaldi†

*Abstract*—Manipulation in orbit represents a challenging task because the motion of the limbs can assist or even generate the angular motion of the floating base due to the conservation of centroidal momentum. Exploiting this aspect is fundamental for correctly accomplishing manipulation tasks in multi-limbed robotic systems employed in orbit. This short paper proposes a whole-body motion controller for teleoperation or visual servoing that can exploit collision-free null-space motions of the limbs to generate the desired angular motion of the base in the absence of contacts and gravity. The results are demonstrated through kinematic and dynamic simulations using the model of a humanoid robot with multiple Degrees of Freedom.

## I. INTRODUCTION

Multi-limbed robots, e.g. humanoids, have the great advantage of being general-purpose platforms that require minimal customization to operate in domestic or industrial scenarios and perform complex loco-manipulation tasks. The significant potential of these machines and their domain of applicability extends beyond the ground and can be successfully employed underwater, in the air, or in space. In particular, the space scenario opens new opportunities and poses challenges from various perspectives, including mechanical and sensory equipment design, actuation methods, and control paradigms. To facilitate their mobility in this domain, the articulations can be exploited to generate or assist the floating-base (FB) motion, especially in the absence of contacts. In fact, the nonholonomy of angular momentum conservation allows controlling the robot's orientation in the absence of contacts by moving its articulations, much like a cat that can land on its feet after falling from an upside-down posture. The same effect is also experienced in microgravity conditions by astronauts, who can change their body orientation with cyclic motions of arms and legs, as shown in Figure 1. In this work, we consider the scenario where a humanoid robot is performing Intra/Extravehicular Activities (IEVAs) and needs to observe its surroundings to find visual references for its motion. For example, the system may be tele-operated to align with a tool for further manipulation, whether for rescue or maintenance purposes. In practice, Visual Servoing (VS) or direct tele-operation of the base could be used to rotate the robot towards the tool. The Centroidal Momentum Conservation (CMC) of the robot can be exploited to generate motions of the limbs and

<figure><img src="image_4.png" /><figcaption>Fig. 1: In the absence of contacts, articulated systems can rotate by moving their articulations due to the nonholonomic nature of angular momentum conservation. This phenomenon is experienced by NASA astronauts in microgravity conditions and is replicated with humanoid robots in this work.</figcaption></figure>

achieve the desired rotation. Inspired by these considerations, we aim to extend the whole-body motion control of multi-limbed robots with the ability to change their body orientation using collision-free motion of the limbs in scenarios where contacts may not occur. Building on our previous work, where we integrated VS and CMC within the same whole-body control framework using Quadratic Programming (QP) optimization [1], we now further integrate collision avoidance as a constraint within the QP. This short paper recaps the theory from [1], extends it to consider collision avoidance, and reports preliminary results from kinematic simulations.

## II. COLLISION-FREE CENTROIDAL MOMENTUM CONSTRAINED VISUAL SERVOING

Let us consider a humanoid robot with $n$ Degrees of Freedom (DoFs). Its generalized coordinates can be expressed by:

$$ q = \begin{bmatrix} q_b \\ q_a \end{bmatrix} \in \mathbb{R}^{d+n}, \quad v = \begin{bmatrix} \dot{p}_b \\ \omega_b \\ \dot{q}_a \end{bmatrix} \in \mathbb{R}^{6+n} \quad (1) $$

where $q_a \in \mathbb{R}^n$ contains the joints position of the articulations, while $q_b \in \mathbb{S}^O(3)$ is a representation of the FB pose, e.g. a position and a quaternion vector ($d = 7$), $\dot{p}_b, \omega_b \in \mathbb{R}^3$ are the linear and angular velocity of the FB, respectively, and $\dot{q}_a \in \mathbb{R}^n$ are the joint velocities. It is possible to define the Visual Servoing task relative to a certain camera frame:

$$ \dot{s} = J_s(s, q) v \quad (2) $$

<footnote>† Université de Lorraine, CNRS, Inria, LORIA, France, 615 rue du Jardin Botanique, 54600 Villers les Nancy, France. E-mail: {jean-baptiste.mouret, enrico.mingo-hoffman, serena.ivaldi}@inria.fr</footnote>

<footnote>‡ Dalle Molle Institute for Artificial Intelligence (IDSIA), USI-SUPSI, Lugano, Switzerland antonio.paolillo@idsia.ch</footnote>
as the joint velocities required to track the visual features velocities $\dot{s} = \dot{s}_d + \lambda(s_d - s) \in \mathbb{R}^f$, with $s$ the measured visual features, $s_d$ the desired visual features, and $\lambda \ge 0$. In particular, $J_s(s, q) = L(s)^c J_{w,c}(q) \in \mathbb{R}^{f \times (n+6)}$ is the feature Jacobian, being: $L(s) \in \mathbb{R}^{f \times 6}$ the interaction matrix, relating the time derivative of the features to the camera velocity in the camera frame and $J_{w,c}(q) \in \mathbb{R}^{6 \times (n+6)}$ the Jacobian of the camera frame w.r.t. world, expressed in camera frame. Similarly, we can define the *centroidal momentum constraint* computed from the centroidal momentum $h \in \mathbb{R}^6$ of the robot as the resultant momentum acting on its Center of Mass (CoM), related to $\nu$ through the following equation:

$$h = A(q)\nu, \quad (3)$$

where $A(q) \in \mathbb{R}^{6 \times (6+n)}$ is the so-called centroidal momentum matrix. For the Newton's equations of motion, in the absence of contacts and gravity, $\dot{h} = 0$, implying that $h$ is constant. Furthermore, if the system starts from rest, it is $h = 0$. From (2) and (3) it is possible to cast a hierarchical QP problem and consider a further *postural task* in joint space acting in the null-space of the VS task:

$$\begin{array}{ll}
\nu^* = \underset{\nu}{\operatorname{argmin}} & \|\nu - k(q_d - q)\|^2 + \epsilon\|\nu\|^2 \\
\text{s.t.} & J_s(s, q)\nu = \dot{s}^*, \\
& A(q)\nu = 0,
\end{array}
\quad (4)$$

where $\dot{s}^*$ is computed solving another QP similar to (4) where the VS task is minimized subject to the CMC constraint only. The joint space task acts as an *actor* around a nominal configuration that, if carefully chosen, permits to achieve cyclic motions, avoiding stack in local minima, even considering the instantaneous nature of the controller. With the controller in (4) we have demonstrated the possibility of correctly tracking visual features in the absence of gravity and contacts, as shown in Figure 2, and reported in [1]$^1$.

In this short paper, we considered a further linear inequality constraint added in the QPs to avoid self-collisions, which can be formulated as:

$$\left[ \frac{cp_2 - cp_1}{\|cp_2 - cp_1\|} \right]^T [J_{cp_1}(q) - J_{cp_2}(q)] \nu \le \varepsilon \frac{d - d_s}{\Delta t}, \quad (5)$$

with $cp_1, cp_2 \in \mathbb{R}^3$ the closest point computed on two collision shapes, e.g. capsules or meshes, and $J_{cp_1}(q) \in \mathbb{R}^{3 \times (6+n)}$ the closest point Jacobian, $d$ the distance between the closest point and $d_s$ a safety margin, more details can be found in [2]. While the presence of this constraint does not allow for the generation of periodic cyclic motions, it permits to achieve FB orientation control through CAM avoiding self-collisions for a decent angular range, as shown in Figure 3$^2$.

### III. FUTURE WORKS

In this short paper, we presented preliminary results of an instantaneous collision-free whole-body controller that permits to control FB orientation and VS tasks in absence of gravity

<figure><img src="image_3.png" /><figcaption>(a) Configuration of the robot</figcaption></figure>

<figure><img src="image_4.png" /><figcaption>(b) Norm of the visual error</figcaption></figure>

<caption>Fig. 2: Tracking performances of controller in (4).</caption>

and contacts through the CAM constraint. Future works will consider further evaluation in simulation, real experiments in recreated scenarios on Earth with simulated reduced gravity along one of the principal FB axes, and a novel formulation based on model predictive control to overcome the limitations due to the absence of a preview horizon.

<figure><img src="image_5.png" /><figcaption>Fig. 3: The humanoid robot performing collision-free limb motions to rotate its base. On the right, the collision model used.</figcaption></figure>

### REFERENCES

[1] E. M. Hoffman and A. Paolillo, "Exploiting visual servoing and centroidal momentum for whole-body motion control of humanoid robots in absence of contacts and gravity," in *IEEE International Conference on Robotics and Automation (ICRA)*, 2021, pp. 2979-2985.

[2] C. Fang, A. Rocchi, E. M. Hoffman, N. G. Tsagarakis, and D. G. Caldwell, "Efficient self-collision avoidance based on focus of interest for humanoid robots," in *IEEE-RAS International Conference on Humanoid Robots (Humanoids)*, 2015, pp. 1060-1066.

<footnote>$^1$A video of the simulations can be found at https://youtu.be/-iN3zNbHQFo</footnote>

<footnote>$^2$Kinematic simulations can be found at https://youtu.be/r2ybwP5TyY8</footnote>
as the joint velocities required to track the visual features velocities $\dot{s} = \dot{s}_d + \lambda(s_d - s) \in \mathbb{R}^f$, with $s$ the measured visual features, $s_d$ the desired visual features, and $\lambda \ge 0$. In particular, $J_s(s, q) = L(s)^c J_{w,c}(q) \in \mathbb{R}^{f \times (n+6)}$ is the feature Jacobian, being: $L(s) \in \mathbb{R}^{f \times 6}$ the interaction matrix, relating the time derivative of the features to the camera velocity in the camera frame and $J_{w,c}(q) \in \mathbb{R}^{6 \times (n+6)}$ the Jacobian of the camera frame w.r.t. world, expressed in camera frame. Similarly, we can define the *centroidal momentum constraint* computed from the centroidal momentum $h \in \mathbb{R}^6$ of the robot as the resultant momentum acting on its Center of Mass (CoM), related to $\nu$ through the following equation:

$$h = A(q)\nu, \quad (3)$$

where $A(q) \in \mathbb{R}^{6 \times (6+n)}$ is the so-called centroidal momentum matrix. For the Newton's equations of motion, in the absence of contacts and gravity, $\dot{h} = 0$, implying that $h$ is constant. Furthermore, if the system starts from rest, it is $h = 0$. From (2) and (3) it is possible to cast a hierarchical QP problem and consider a further *postural task* in joint space acting in the null-space of the VS task:

$$ \begin{array}{ll} \nu^* = \underset{\nu}{\operatorname{argmin}} & \|\nu - k(q_d - q)\|^2 + \epsilon\|\nu\|^2 \\ \text{s.t.} & J_s(s, q)\nu = \dot{s}^*, \\ & A(q)\nu = 0, \end{array} \quad (4) $$

where $\dot{s}^*$ is computed solving another QP similar to (4) where the VS task is minimized subject to the CMC constraint only. The joint space task acts as an *actor* around a nominal configuration that, if carefully chosen, permits to achieve cyclic motions, avoiding stack in local minima, even considering the instantaneous nature of the controller. With the controller in (4) we have demonstrated the possibility of correctly tracking visual features in the absence of gravity and contacts, as shown in Figure 2, and reported in [1]$^1$.

In this short paper, we considered a further linear inequality constraint added in the QPs to avoid self-collisions, which can be formulated as:

$$ \left[ \frac{cp_2 - cp_1}{\|cp_2 - cp_1\|} \right]^T [J_{cp_1}(q) - J_{cp_2}(q)] \nu \le \varepsilon \frac{d - d_s}{\Delta t}, \quad (5) $$

with $cp_1, cp_2 \in \mathbb{R}^3$ the closest point computed on two collision shapes, e.g. capsules or meshes, and $J_{cp_1}(q) \in \mathbb{R}^{3 \times (6+n)}$ the closest point Jacobian, $d$ the distance between the closest point and $d_s$ a safety margin, more details can be found in [2]. While the presence of this constraint does not allow for the generation of periodic cyclic motions, it permits to achieve FB orientation control through CAM avoiding self-collisions for a decent angular range, as shown in Figure 3$^2$.

### III. FUTURE WORKS

In this short paper, we presented preliminary results of an instantaneous collision-free whole-body controller that permits to control FB orientation and VS tasks in absence of gravity

<figure><img src="image_5.png" /><figcaption>(a) Configuration of the robot</figcaption></figure>

<figure><img src="image_6.png" /><figcaption>(b) Norm of the visual error</figcaption></figure>

<caption>Fig. 2: Tracking performances of controller in (4).</caption>

and contacts through the CAM constraint. Future works will consider further evaluation in simulation, real experiments in recreated scenarios on Earth with simulated reduced gravity along one of the principal FB axes, and a novel formulation based on model predictive control to overcome the limitations due to the absence of a preview horizon.

<figure><img src="image_7.png" /><figcaption>Fig. 3: The humanoid robot performing collision-free limb motions to rotate its base. On the right, the collision model used.</figcaption></figure>

### REFERENCES

[1] E. M. Hoffman and A. Paolillo, "Exploiting visual servoing and centroidal momentum for whole-body motion control of humanoid robots in absence of contacts and gravity," in *IEEE International Conference on Robotics and Automation (ICRA)*, 2021, pp. 2979-2985.

[2] C. Fang, A. Rocchi, E. M. Hoffman, N. G. Tsagarakis, and D. G. Caldwell, "Efficient self-collision avoidance based on focus of interest for humanoid robots," in *IEEE-RAS International Conference on Humanoid Robots (Humanoids)*, 2015, pp. 1060-1066.

<footnote>$^1$A video of the simulations can be found at https://youtu.be/-iN3zNbHQFo</footnote>

<footnote>$^2$Kinematic simulations can be found at https://youtu.be/r2ybwP5TyY8</footnote>