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

HAL
open science

# Maintaining Balance of Mobile Manipulators for Safe
Pick-Up Tasks

Francesco d'Orazio, Tommaso Belvedere, Spyridon G Tarantos, Giuseppe

Oriolo

► To cite this version:

Francesco d'Orazio, Tommaso Belvedere, Spyridon G Tarantos, Giuseppe Oriolo. Maintaining Balance of Mobile Manipulators for Safe Pick-Up Tasks. ICARCV 2024 - 18th International Conference on Control Automation, Robotics and Vision, Dec 2024, Dubai, United Arab Emirates. pp.1-6. hal-04809183

HAL Id: hal-04809183

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

Submitted on 28 Nov 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_2.png" /></figure>

Distributed under a Creative Commons Attribution 4.0 International License
# Maintaining Balance of Mobile Manipulators for Safe Pick-Up Tasks

Francesco D'Orazio<fnref n="1" />, Tommaso Belvedere<fnref n="2" />, Spyridon G. Tarantos<fnref n="3" />, Giuseppe Oriolo<fnref n="1" />

**Abstract**— This paper presents a novel method to maintain the dynamic balance of a Mobile Manipulator (MM) during the pick-up of heavy objects. The approach entails the generation of a preliminary reach-to-grasp trajectory, which is subsequently refined by an Optimization-Based Controller (OBC) formulated as a Quadratic Program (QP). The trajectory is modified in a minimal fashion to ensure that the robot maintains balance during the reaching phase and remains balanced when the payload is grasped. This is accomplished by incorporating a balance constraint into the OBC that predicts the Zero Moment Point (ZMP) position of the robot at the beginning of the pick-up phase. This accounts for the gravitational and inertial effects that the object has on the robot. The method is validated through simulations conducted with the TIAGo robot in Gazebo. The results demonstrate that the proposed approach effectively prevents the robot from tipping over when the payload is considered.

## I. INTRODUCTION

In recent decades, fixed-base manipulators have been widely used for pick-and-place operations. However, their limited workspace restricts their range of applications, leading to the development of Mobile Manipulators (MM), where a robotic arm is mounted on a mobile base. These robotic systems typically use kinematic commands to control their End-Effector (EE) movements [1]–[3]. When manipulating heavy objects or performing fast motions, especially with narrow bases designed for tight spaces, the robot may be at risk of tipping over. Moreover, during a pick-up motion the system dynamic parameters change instantaneously, risking a loss of balance if the state of the robot is not appropriate and the object is heavy. Therefore, to maintain safety the robot must adjust its configuration before grasping the object.

The Zero Moment Point (ZMP) method evaluates robot balance by identifying the point where the ground reaction forces act. Originally introduced by Vukobratovic for bipedal locomotion [4], and further developed by Takanishi [5], it has been applied to MMs by Sugano and Huang [6], [7]. Various researchers have addressed MM balance while performing specific tasks [8]–[12], either by generating corrective actions to modify the overall inertia tensor [8], [9] or by formulating balance constraints within optimization problems [10]–[13]. These constraints filter the planned trajectory to keep the ZMP within the balance region. A more robust approach uses the Control Barrier Function (CBF) framework [14],

<figure><img src="image_3.png" /><figcaption>Fig. 1: Snapshots of the motion of the TIAGo robot in Gazebo, obtained implementing the proposed method.</figcaption></figure>

[15], offering a tunable parameter to balance robustness and performance. CBFs define a Safe Set in the state space that can be rendered invariant under feedback where the robot remains balanced. This framework can be extended to discrete-time systems using Discrete-Time Control Barrier Functions (DT-CBFs) [16], [17], and is applicable to both autonomous control and teleoperation [18].

This work proposes a Real-time EE Trajectory Planner (REETP) that generates a reach-to-grasp trajectory, elaborated by an Optimization-Based Controller (OBC) that minimally modifies it to ensure (i) balance during the reaching motion and (ii) stability when lifting the object. The trajectory planner allows the EE to reach the target, while the OBC, formulated as a Quadratic Program (QP), enforces balance via CBF constraints.

The novel contribution is the inclusion of an additional balance constraint in the OBC to predict the ZMP position post-grasping, adjusting the configuration to maintain balance during dynamic parameter changes. As a second contribution, the DT-CBF condition for QP has been manipulated to avoid the explicit dynamic extension of the controller. In fact, these constraints typically require that the time derivative of the CBF is affine with respect to the decision variables. However, in the scenario presented in this paper, the CBF depends directly on the system input, forcing the QP to be defined at a higher differential level, increasing the number of the variables in the state vector.

Simulation tests using the TIAGo robot in the Gazebo environment demonstrate the effectiveness of the method which is, by design, applicable to any MM. Results show that, unlike cases without the additional balance constraint, this approach ensures stability when the robot lifts objects by adjusting its state.

The paper is structured as follows: Section II defines the problem and states the objectives of the proposed method;

<footnote n="1">¹Department of Computer, Control and Management Engineering, Sapienza University of Rome, Rome, Italy. E-mail: {dorazio, oriolo}@diag.uniroma1.it.</footnote>

<footnote n="2">²CNRS, Université de Rennes, Inria, IRISA, Rennes, France. E-mail: tommaso.belvedere@irisa.fr.</footnote>

<footnote n="3">³Center of AI & Robotics (CAIR) and Engineering Division, New York University Abu Dhabi, Abu Dhabi, United Arab Emirates. E-mail: spyridon.tarantos@nyu.edu.</footnote>
Section III reports on the modeling and analytical computa-
tion of the ground reaction forces; Section IV presents the
proposed method; Section V shows the simulation settings
and results and Section VI reports some concluding remarks.

II. PROBLEM FORMULATION

Consider a MM operating in a 3-dimensional workspace W, moving on an horizontal surface. The configuration of the robot takes values in an *n*-dimensional space and is denoted by **q** ∈ ℝⁿ. The pose of the EE, namely position and orientation, is denoted by **ξ** ∈ SE(3) and is related to the robot configuration via the forward kinematic map **ξ** = f(**q**). The robot has to pick up a payload with its EE. Let us denote by **ξ̃** ∈ SE(3) a desired pose that allows the EE to rigidly grasp the payload.

The objective is to generate in real time a whole-body
motion that:

R1. starts from the robot initial configuration $q_s$ and brings
the robot EE in pose $\tilde{\xi}$;

R2. is kinodynamically feasible, in the sense that it is com-
patible with the robot dynamics and respects constraints
in joint, velocity and torque level;

R3. maintains the robot balance, in the sense that the robot
wheels remain in contact with the ground at all times;

R4. permits the robot to maintain its balance immediately
after the object is grasped.

Note that R2-R4 must be satisfied in order to ensure that the
robot maintains a safe behavior.

III. ROBOT MODEL

Consider a MM comprised of a mobile base with $n_w$ driving wheels and a manipulator arm with $n_m$ degrees of freedom. Following the same approach for modeling presented in [13], it is assumed that the wheels are in point contact with the ground and the friction is large enough to prevent the driving wheels from slipping. As for the caster wheels, they only account for robot balance, i.e. their point of contact with the ground is considered fixed with respect to the mobile base and their dynamics are neglected. To model the pose of the mobile base, $n_b = 6$ fictitious joints are considered to connect the base with the world frame. The first three are prismatic joints representing the displacement along the $x$, $y$ and $z$ axes and the last three represent the base rotation along roll, pitch and yaw axes ($\theta_x$, $\theta_y$, $\theta_z$). These variables are collected in a vector $\mathbf{q}_b = (x_b, y_b, z_b, \theta_x, \theta_y, \theta_z)$. The generalized variables of the manipulator are represented by $\mathbf{q}_m = (\theta_1, \dots, \theta_{n_m})$, whereas $\mathbf{q}_w$ is the vector associated with the driving wheels. The vector of generalized coordinates of the robot is $\mathbf{q} = (\mathbf{q}_b, \mathbf{q}_w, \mathbf{q}_m)$ of dimension $n = n_b + n_w + n_m$.

The equations of motion [19] are expressed as

$$
\begin{align}
M_q(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{n}_q(\mathbf{q}, \dot{\mathbf{q}}) &= E_q(\mathbf{q})\boldsymbol{\tau} + \mathbf{A}(\mathbf{q})\boldsymbol{\lambda} \tag{1} \\
\mathbf{A}(\mathbf{q})\dot{\mathbf{q}} &= \mathbf{0}, \nonumber
\end{align}
$$

where $M_q(\mathbf{q}) \in \mathbb{R}^{n \times n}$ is the mass matrix, $\mathbf{n}_q(\mathbf{q}, \dot{\mathbf{q}}) \in \mathbb{R}^n$
contains the centrifugal, Coriolis and gravitational terms,
$E_q(\mathbf{q}) \in \mathbb{R}^{n \times n_\tau}$ maps the inputs $\boldsymbol{\tau} \in \mathbb{R}^{n_\tau}$ to the generalized

forces that produce work on the generalized coordinates
with $n_{\tau} = n_{w} + n_{m}$. The model is subject to $\kappa$ kinematic
constraints collected in a matrix $\mathbf{A}(\mathbf{q}) \in \mathbb{R}^{n \times \kappa}$, whereas
$\boldsymbol{\lambda} \in \mathbb{R}^{\kappa}$ is the vector of the contact forces. A matrix $\mathbf{S}(\mathbf{q}) \in$
$\mathcal{N}(\mathbf{A}^{\mathrm{T}}(\mathbf{q}))$ can be defined that relates the generalized ve-
locities and accelerations $\dot{\mathbf{q}}, \ddot{\mathbf{q}} \in \mathbb{R}^n$ to the pseudo velocities
$\boldsymbol{\nu} = (\dot{\mathbf{q}}_w, \dot{\mathbf{q}}_m) \in \mathbb{R}^{(n-\kappa)}$ and their time derivative $\dot{\boldsymbol{\nu}}$,

$$
\begin{align}
\dot{\mathbf{q}} &= \mathbf{S}(\mathbf{q})\boldsymbol{\nu} \tag{2} \\
\ddot{\mathbf{q}} &= \mathbf{S}(\mathbf{q})\dot{\boldsymbol{\nu}} + \dot{\mathbf{S}}(\mathbf{q}, \dot{\mathbf{q}})\boldsymbol{\nu}. \nonumber
\end{align}
$$

Premultiplying (1) by $\mathbf{S}^T(q)$ leads to

$$
\mathbf{S}^T(\mathbf{q}) \mathbf{M}_q(\mathbf{q}) \ddot{\mathbf{q}} + \mathbf{S}^T(\mathbf{q}) \mathbf{n}_q(\mathbf{q}, \dot{\mathbf{q}}) = \mathbf{S}^T(\mathbf{q}) \mathbf{E}_q(\mathbf{q}) \boldsymbol{\tau}. \quad (3)
$$

The reduced equation of motion [19] is obtained substituting
(2) into (3)

$$
M(\mathbf{q})\dot{\mathbf{n}} + \mathbf{n}(\mathbf{q}, \mathbf{v}) = \mathbf{S}^T(\mathbf{q})\mathbf{E}_q(\mathbf{q})\mathbf{\tau}, \quad (4)
$$

where

$$
\begin{align*}
M(\mathbf{q}) &= \mathbf{S}(\mathbf{q})^T \mathbf{M}_q(\mathbf{q}) \mathbf{S}(\mathbf{q}) \\
\mathbf{n}(\mathbf{q}, \dot{\mathbf{q}}) &= \mathbf{S}^T(\mathbf{q})(\mathbf{M}(\mathbf{q})\dot{\mathbf{S}}(\mathbf{q}, \dot{\mathbf{q}})\boldsymbol{\nu} + \mathbf{n}_q(\mathbf{q}, \dot{\mathbf{q}})).
\end{align*}
$$

Since the MM moves of a flat surface, $\mathbf{q}_f = (z_b, \theta_x, \theta_y) \in$
$\mathbf{q}_b$ are constant. The balance of the system is determined
by the reaction forces, which can be analytically computed
through the dynamic model due to the presence of $\mathbf{q}_f$.

As in [13], two constant selection matrices $Q_a \in \mathbb{R}^{(n-3) \times n}$ and $Q_f \in \mathbb{R}^{3 \times n}$ can be defined, dividing the dynamics variables into $\mathbf{q}_f = Q_f \mathbf{q}$ and $\mathbf{q}_a = Q_a \mathbf{q}$, representing the fixed and active parts of the dynamics respectively. Since $\ddot{z}_b$, $\ddot{\theta}_x$ and $\ddot{\theta}_y$ are identically zero, the relation $\ddot{q} = Q_a^T \ddot{q}_a$ holds. For simplicity, the dependencies are omitted from the following equations. Premultiplying (1) by $Q_a$ and $Q_f$ yields to

$$
Q_a M_q Q_a^T \ddot{q}_a + Q_a n_q = Q_a E_q \tau + Q_a A \lambda \quad (5)
$$

$$
Q_f M_q Q_a^T \ddot{q}_a + Q_f n_q = Q_f A \lambda. \quad (6)
$$

Consider a matrix $G \in \mathcal{N}(A^T Q_a^T)$ such that

$$
\begin{align}
\dot{q}_a &= G\nu \tag{7} \\
\ddot{q}_a &= G\dot{\nu} + \dot{G}\nu. \nonumber
\end{align}
$$

Premultiplying (5) by $G^T$, together with (7) yields to the
reduced equation of motion of the active dynamics

$$
M_a \dot{\boldsymbol{\nu}} + \boldsymbol{n}_a = E_a \boldsymbol{\tau}, \quad (8)
$$

where $M_a = G^T Q_a M_q Q_a^T G$, $n_a = G^T Q_a M_q Q_a^T \dot{G} \nu + G^T Q_a n_a$ and $E_a = G^T Q_a E_q$.

The reaction forces at the generalized coordinate level can
be computed analytically from (6) using (7) and (8). Since
$(f_z \mu_x \mu_y)^T = Q_f A \boldsymbol{\lambda}$

$$
\begin{pmatrix} f_z \\ \mu_x \\ \mu_y \end{pmatrix} = \Sigma \tau + \alpha, \qquad (9)
$$

with $\Sigma = Q_f M_q Q_a^T G M_a^{-1} E_a$ and $\alpha = Q_f M_q Q_a^T \dot{G} \nu + Q_f n_q - Q_f M_q Q_a^T G M_a^{-1} n$.

Remark 1: The relation (9) shows that the ground reaction forces are linear with respect to the torques and can be computed from the system dynamics knowing the robot state.
<figure><img src="image_4.png" /><figcaption>Fig. 2: The block scheme of the proposed method. The planner takes the desired pose $\xi$ as input and generates acceleration commands $\ddot{\xi}_r$, which are then used by the OBC to obtain the torque command $\tau$.</figcaption></figure>

## IV. PROPOSED APPROACH

The proposed motion generation approach consists of two modules, namely the Real-time End-Effector Trajectory Planner (REETP) and the Optimization-Based Controller (OBC) as illustrated in Fig. 2. The first module considers the desired pose $\xi$ that the EE must assume together with the current EE pose, which is related to the state of the robot $(q, v)$ by the forward kinematics map. As an output, it provides a reference end-effector acceleration $\ddot{\xi}_r$. The second module takes as an input the current state of the robot and the reference $\ddot{\xi}_r$. Through the solution of a QP generates appropriate control commands for the robot $\tau$ that track $\ddot{\xi}_r$ while satisfying requirements R2-R4.

The task can be divided into two phases to highlight the different parameters of the dynamic model of the robot: reach, in which the nominal parameter of the robot are considered, and pick up, where the model also accounts for the payload, rigidly grasped in the EE.

### A. Real-Time End-Effector Trajectory Planner

The trajectory planner implemented in this work is based on a PD control law, which imposes exponential convergence of the EE to the desired pose

$$ \xi_r = K_P e - K_D \dot{\xi}, \quad (10) $$

where $K_P$ and $K_D$ are diagonal positive definite matrices of appropriate dimensions. The error $e$ is composed of a linear and an angular component $e = (e_p^T, e_\phi^T)^T$. The position error $e_p$ is simply defined as the difference between the desired and actual position of the EE, $e_p = \xi_p - \xi_p$, while $e_\phi$ depends on the representation used to describe its orientation. In this paper, the error is defined using rotation matrices [20]. Given the desired EE orientation $\bar{R}(\xi)$ and the current one $R(\xi)$, defined with respect to the same reference frame, $e_\phi$ can be obtained by

$$ e_{\phi} = \frac{1}{2} \left( [\bar{r}_1]_{\times}^T \quad [\bar{r}_2]_{\times}^T \quad [\bar{r}_3]_{\times}^T \begin{pmatrix} r_1 \\ r_2 \\ r_3 \end{pmatrix} \right) \in \mathbb{R}^3, $$

where $r_i$ and $\bar{r}_i$ are the i-th column of matrices $R$ and $\bar{R}$ respectively, and $[\bar{r}_i]_\times$ represents the skew-symmetric matrix constructed from $\bar{r}_i$.

### B. Optimization-Based Controller

The OBC is formulated as a QP problem that elaborates the planned acceleration $\dot{\xi}_r$ to enforce kinodynamic feasibility and two sets of constraints: the Balance Constraint (BC),

designed to enforce that the robot ZMP is within its Support Polygon (SP), and the Preemptive Balance Constraint (PBC), aimed at maintaining balance after pick up. The decision variables are the joint torques of the robot. For the sake of clarity, the cost function and the QP constraints are expressed in terms of joint accelerations which can be mapped to generalized forces via (4).

The cost function to be minimized takes into account the squared error norm between the desired and actual task acceleration plus a regularization term

$$ \mathcal{I}_{\text{cost}} = \|\mathbf{J}\dot{\mathbf{v}} + \dot{\mathbf{J}}\mathbf{v} - \ddot{\xi}_r\|^2 + \|\boldsymbol{\tau}\|_{\Gamma}^2. \quad (11) $$

To ensure the existence of an optimal solution, it is essential to confirm that the Hessian of the optimization problem is positive definite [21]. The Hessian, defined as the second derivative of the Lagrangian function with respect to the decision variables, is expressed as $\mathbf{H} = \mathbf{J}^T \mathbf{J} + \mathbf{\Gamma}$. For a redundant robot, such as those considered in this paper, the matrix $\mathbf{J}^T \mathbf{J}$ is inherently rank deficient. The regularization term $\|\boldsymbol{\tau}\|_{\Gamma}^2$ is added to guarantee the convexity of $\mathbf{H}$ utilizing a diagonal positive definite weighting matrix $\mathbf{\Gamma}$, which penalizes the norm of the vector of decision variables.

The commanded torque $\boldsymbol{\tau}$ has to be compatible with the joint limits on $q_m$ and with the velocity limits on the entire vector $v$. Since the state $(q, v)$ is already determined at the current time $t_k$, these constraints are imposed on the predicted state at time instant $t_{k+1} = t_k + \delta$, where $\delta \in \mathbb{R} > 0$ is the sampling time of the discrete-time controller:

$$ \nu_{k+1} = v + \delta \dot{v} \quad (12) $$

$$ q_{m,k+1} = q_m + \delta \dot{q}_m + \frac{\delta^2}{2} \ddot{q}_m. \quad (13) $$

After simple manipulation, (12) becomes

$$ -\delta \dot{v} \le v - v_{\min} $$

$$ \delta \dot{v} \le v_{\max} - v, $$

whereas (13) reads as

$$ -\frac{\delta^2}{2} \ddot{q}_m \le q_m - q_{\min} + \delta \dot{q}_m $$

$$ \frac{\delta^2}{2} \ddot{q}_m \le q_{\max} - q_m - \delta \dot{q}_m. $$

The QP to be solved is:

$$ \begin{array}{ll} \min_{\tau \in [\tau_{\min}, \tau_{\max}]} & \|\mathbf{J}\dot{\mathbf{v}} + \dot{\mathbf{J}}\mathbf{v} - \ddot{\xi}_r\|^2 + \|\boldsymbol{\tau}\|_{\Gamma}^2 \\ \text{subject to} & \text{joint bounds} \\ & \text{balance constraints} \\ & \text{preemptive balance constraints} \end{array} \quad (14) $$

### C. Balance Constraint

The goal of the BC is to enforce balance throughout the motion of the robot. A mechanical system is in balance if its ZMP lies within the SP of the structure.

The ZMP position $p_{zmp}$ can be easily derived using the Newton-Euler equations. From the principle of action and reaction between forces and torques exchanged by consecutive joints, it is possible to compute the wrench $\mathcal{F} = (\mu, f)$ that the base exchanges with the ground [19]. The wrench $\mathcal{F}$ can
<figure><img src="image_5.png" /><figcaption>Fig. 3: The i-th DT-CBF represents the signed distance between the ZMP position and the edge connecting $v_i$ and $v_{i+1}$. It is computed as the x-component of the $p_{zmp}$ expressed in the reference frame $RF_{v_i}$.</figcaption></figure>

be computed either using (9) by including the base variables $q_b$ in the model (Section III), or by extending the algorithm in SE(3) to compute the generalized forces exchanged in all directions [8], [9], [22], [23]. Having computed the ground reaction wrench, the ZMP position can be expressed as

$$p_{zmp} = \begin{pmatrix} x_{zmp} \\ y_{zmp} \\ z_{zmp} \end{pmatrix} = \begin{pmatrix} -\mu_y/f_z \\ \mu_x/f_z \\ 0 \end{pmatrix}. \quad (15)$$

The SP of a MM can be modeled with linear inequalities that connect the vertices of the polyhedron obtained from the wheel positions $v_i \in \mathcal{V}$ with $i = 1, \dots, \rho$ (see Fig. 3). To evaluate the balance condition, the signed distance between the ZMP position and the i-th edge connecting two vertices $v_i$ and $v_{i+1}$ is considered. Referring to Fig. 3, the ZMP position in the world frame $p_{zmp}$, is expressed in a reference frame $RF_{v_i}$ that has the y-axis aligned with the edge, and the x-axis directed towards the center of the SP. The origin of $RF_{v_i}$ is placed at the i-th vertex, so $v_i p_{zmp}$ is obtained as

$$v_i p_{zmp} = R^T(\theta_{\text{ort}})(p_{zmp} - v_i), \quad (16)$$

where $\theta_{\text{ort}}$ represents the angle between $RF_{v_i}$ and the world frame, and $R(\theta_{\text{ort}})$ is the associated rotation matrix.

The x-component of (16) can be written as

$$h_i(v_i, p_{zmp}) = \cos \theta_{\text{ort}}(p_{zmp,x} - v_{i,x}) + \sin \theta_{\text{ort}}(p_{zmp,y} - v_{i,y}). \quad (17)$$

By construction, $h_i > 0$ whenever the ZMP is inside the SP of the robot, $h_i = 0$ whenever it is on the i-th edge, while $h_i < 0$ whenever it is outside. Therefore, enforcing $h_i \ge 0$ is a necessary and sufficient condition to maintain balance.

However, a more robust approach consists in using DT-CBFs, which enforces safety for a generic nonlinear system, and that is briefly introduced before being applied to the problem. Safety is defined through a function $h(x) : \mathcal{X} \to \mathbb{R}$, used to generate a Safe Set $C$ that can be rendered invariant under feedback [16], [17]:

$$C = \{x \in \mathcal{X} : h(x) \ge 0\}$$

$$\partial C = \{x \in \mathcal{X} : h(x) = 0\}.$$

If the relation

$$\Delta h(x_k, u_k) \ge -\gamma h(x_k), \quad \gamma \in (0, 1] \quad (18)$$

where $\Delta h(x_k, u_k) = h(x_{k+1}) - h(x_k)$ and $u$ represents the input of the system. If (18) holds for all time instants $t_k \in \mathbb{Z}^+$, the set $C$ is forward invariant, i.e. the evolution of the state remains confined in the set starting from $x(0) \in C$, and $h(x)$ is a DT-CBF. Condition (18) can be equivalently written as

$$h(x_{k+1}) \ge (1 - \gamma)h(x_k). \quad (19)$$

**Remark 2:** Condition (19) fixes the exponential rate of the function, in fact at $t_k \in \mathbb{Z}^+$, $h(x_k) \ge (1 - \gamma)^k h(x_0)$.

**Remark 3:** If $x_k \in C$, condition (19) is more restrictive than the simple constraint $h(x_{k+1}) \ge 0$; whereas if $x_k \notin C$, it imposes exponential convergence toward the safe set.

The DT-CBF in (19) is a function of the current state only, whereas (17) is a function of both state and input since (9) shows that the ZMP depends directly on the torques. Therefore, to apply DT-CBFs for constraining the ZMP position, the controller would have to be defined at a higher differential level including $u$ as a part of the state. Considering a dynamically extended system of the form

$$x_{k+1} = f(x_k, u_k)$$

$$u_{k+1} = v_k,$$

condition (19) becomes

$$h(x_{k+1}, u_{k+1}) \ge (1 - \gamma)h(x_k, u_k),$$

which is nonlinear and therefore cannot be used inside a QP. However, at time $t_{k-1}$, the same condition must have been satisfied for some $u_k = v_{k-1}$ for the system to be safe:

$$h(x_k, u_k) \ge (1 - \gamma)h(x_{k-1}, u_{k-1}). \quad (20)$$

Then, one can enforce (20) using the system without dynamic extension to compute the feasible $u_k^*$, solution of the QP, for which the equivalent condition on $v$ is satisfied at time $t_{k-1}$, implying safety.

This can be applied to impose a safety condition directly on (17), as the resulting constraint is linear. Substituting (17) into (20), together with (15), yields

$$-\cos \theta_{\text{ort}}(\frac{\mu_y}{f_z} + v_{i,x}) + \sin \theta_{\text{ort}}(\frac{\mu_x}{f_z} - v_{i,y}) \ge (1 - \gamma)h_i^{\text{prev}},$$

where $h_i^{\text{prev}} = h_i(x_{k-1}, u_{k-1}^*)$. Multiplying both sides by $f_z$, allows to collect the ground reaction forces in a vector

$$\begin{pmatrix} \cos \theta_{\text{ort}} v_{i,x} + \sin \theta_{\text{ort}} v_{i,y} + (1 - \gamma)h_i^{\text{prev}} \\ -\sin \theta_{\text{ort}} \\ \cos \theta_{\text{ort}} \end{pmatrix}^T \begin{pmatrix} f_z \\ \mu_x \\ \mu_y \end{pmatrix} \le 0.$$

Denoting as $\beta$ the vector multiplying the ground reaction forces, the BC can be expressed linearly in terms of the generalized forces using (9):

$$\beta^T \Sigma \tau \le -\beta^T \alpha. \quad (21)$$

## D. Preemptive Balance Constraint

The PBC represents the main contribution of this work and its purpose is to force the Virtual ZMP (VZMP) to remain within the SP. The VZMP represents the ZMP position computed with the augmented dynamic model of the robot, namely the model obtained when the payload is rigidly attached to the gripper. After the pick-up phase, the VZMP
becomes the ZMP due to the change in the model parameters, meaning that the PBC affects the optimization problem only during the reach phase. At that moment, balance is affected by the configuration of the robot and by the mass of the payload. However, the BC does not consider these influences, lacking critical information for the optimization problem, which can lead to a potential tip-over of the robot.

The BC and the PBC are both designed to maintain balance, however, the PBC cannot replace the BC in general due to the mismatch between the models before grasping the object. Consequently, relying solely on the PBC in the OBC may result in the generation of commands that destabilize the system when the robot is in certain configurations. For this reason, the OBC incorporates both the BC and the PBC constraints during the reach phase.

The procedure for constructing the PBC is the same as presented in Section IV-C, but it accounts for the augmented dynamic model. However, considering the full mass of the object from the start of the motion would result in a more conservative condition than needed. To avoid this, we define a transition region, i.e., a cylindrical area of radius $r_{safe}$ around the payload, and let the mass increase linearly in this region:

$$m_{\text{PBC}} = m \left( 1 - \min \left( 1, \frac{\|e_p\|}{r_{\text{safe}}} \right) \right), \quad (22)$$

where $m$ is the actual mass of the object. Outside the region, $m_{PBC} = 0$ and the PBC coincides with the BC.

## V. SIMULATIONS

To highlight the effectiveness of the proposed method, two simulations were performed using TIAGo robot. All simulations were run in Gazebo, on an Intel Core i9-9900K CPU at 3.60 GHz. The QP is solved using the Eigen-QuadProg library. The control loop takes 5 ms on average, allowing for real time implementation.

The TIAGo robot is equipped with a differential drive base, a prismatic joint in the torso, and an arm with $n_m = 7$ degrees of freedom. The mobile base is fitted with four passive caster wheels and $n_w = 2$ driving wheels. Throughout this work, the position of the torso is held constant.

The base of TIAGo is velocity-controlled and its torque commands are obtained through the relation

$$\tau_i = k_{\omega_i}(\omega_i - \dot{q}_i), \quad i = \{L, R\}, \quad (23)$$

where $k_{\omega_i}$ is a fixed gain chosen by the manufacturer, $\omega_i$ is the velocity command to each wheel and $(\dot{q}_L, \dot{q}_R)$ are the current velocities of the left and right wheels respectively. Since the relation is affine with respect to $\omega_L$ and $\omega_R$, they can be included in the QP presented in Section IV-B as decision variables, including (23) as additional equality constraints.

TIAGo has a heavy and large base, and even in simulation it is difficult to get the ZMP of the robot at the edge of the SP with realistic movements. To highlight the effects of the balance constraints, it is then treated as having a smaller SP.

Animations of the simulations can be found in the accompanying video (https://youtu.be/7601Cj4qSuU).

<figure><img src="image_6.png" /><figcaption>Fig. 4: Simulation 1: ZMP profiles obtained with BC only vs. BC plus PBC (in $m$). With BC only, the ZMP jumps outside the SP during the pick-up phase.</figcaption></figure>

<figure><img src="image_7.png" /><figcaption>Fig. 5: Simulation 1: final robot configuration.</figcaption></figure>

In the first simulation, the proposed method (BC+PBC) is compared with a version of it that does not consider the PBC in the OBC (BC only). Figure 4 shows the ZMP trajectories of the robot in the two cases. At the outset of the motion, the profiles are identical until the EE enters the transition region. Subsequently, the parameters of the dynamic model change due to the consideration of the external payload according to (22). At this point, the PBC starts affecting the solution of the QP, modifying the decision variables. After the pick up, the ZMP becomes the predicted VZMP, jumping instantaneously to its new position. The plots clearly demonstrate that in the simulation run without the PBC, the ZMP jumps outside the SP, whereas in the simulation with the PBC, balance is preserved since the new ZMP position is at the edge of the SP and the horizontal moments are compensated by the structure. A careful observation of the video reveals the dynamic effects of the PBC. When the PBC is taken into account, the optimization procedure finds a local minimum, affecting the velocity rather than altering the configuration of the robot. This results in a smoother deceleration with a slight overshoot instead of an abrupt stop. Fig. 5 shows the final configurations assumed by the robot at the end of the reach phase in both cases.

In the second simulation, both the BC and the PBC are considered. Figure 6 illustrates the evolution of the ZMP and the VZMP, along with the profiles of their respective CBFs, while Fig. 7 provides snapshots of the motion. The plots clearly shows that both the ZMP and VZMP remain within the SP throughout the entire motion. In this scenario, the ZMP remains within the SP by robot reconfiguration rather than by break modulation therefore, no overshoot can be observed in the video at the end of the motion.
<figure><img src="image_8.png" /><figcaption>Fig. 6: Simulation 2. Left: ZMP profile obtained with the proposed method (in *m*); both the ZMP and the VZMP remain within the SP. Right: the CBF profiles show the distance from the closest edge for both ZMP and VZMP. The vertical red line in the CBF plot marks the instant when the EE enters the transition region.</figcaption></figure>

<figure><img src="image_9.png" /><figcaption>Fig. 7: Simulation 2: snapshots of the robot motion.</figcaption></figure>

## VI. CONCLUSIONS

This paper presents a novel approach to maintain balance using MM that performs pick-up tasks, where dynamic changes in the system parameters pose significant stability challenges. By leveraging the ZMP and CBF frameworks, an OBC is deployed to adjust the trajectory of the robot to ensure balance both during the reach-to-grasp motion and at the moment when the robot starts lifting the object.

The method incorporates the PBC into the OBC, which involves the prediction of the ZMP position after grasping the object. This allows to anticipate and counteract the destabilizing effects of the inertial and gravitational forces that the payload exerts on the system, guiding the robot with a smooth transition of the considered payload mass. The OBC can be formulated as a QP providing a method suitable for real-time control. The simulation results using the TIAGo robot demonstrate the effectiveness of the approach in maintaining stability. However, the dynamic effects caused by incorporating of the PBC can be observed at the velocity level, resulting in smoother wheel deceleration. This corresponds to a local solution of the optimization problem, whereas a global solution would involve changing the configuration of the robot.

The findings underscore the critical role of dynamic balance control in enhancing the operational safety and flexibility of MMs, particularly in environments requiring precise and stable object handling. Future work could explore extending this approach to more complex tasks and real-world scenarios, further improving the robustness and autonomy of MMs.

## REFERENCES

[1] E. Brzozowska, O. Lima, and R. Ventura, "A generic optimization based cartesian controller for robotic mobile manipulation," in *2019 Int. Conf. on Robotics and Automation*, 2019, pp. 2054–2060.

[2] S. Stavridis, P. Falco, and Z. Doulgeri, "Pick-and-place in dynamic environments with a mobile dual-arm robot equipped with distributed distance sensors," in *2020 IEEE-RAS 20th Int. Conf. on Humanoid Robots*, 2021, pp. 76–82.

[3] M. Sorour, A. Cherubini, and P. Fraisse, "Motion control for steerable wheeled mobile manipulation," in *2019 European Conf. on Mobile Robots*, 2019, pp. 1–7.

[4] M. Vukobratovic and B. Borovac, "Zero-moment point—thirty five years of its life," *Int. J. of Humanoid Robotics*, vol. 1, no. 01, pp. 157–173, 2004.

[5] A. Takanishi, M. Tochizawa, H. Karaki, and I. Kato, "Dynamic biped walking stabilized with optimal trunk and waist motion," in *IEEE Int. Conf. on Intelligent Robots and Systems*, vol. 1989, 1989, pp. 187–192.

[6] S. Sugano, Q. Huang, and I. Kato, "Stability criteria in controlling mobile robotic systems," in *Proceedings of 1993 IEEE/RSJ Int. Conf. on Intelligent Robots and Systems*, vol. 2, 1993, pp. 832–838.

[7] Q. Huang, K. Tanie, and S. Sugano, "Coordinated motion planning for a mobile manipulator considering stability and manipulation," *The Int. J. of Robotics Research*, vol. 19, no. 8, pp. 732–742, 2000.

[8] S.-H. Lee, J. Kim, F. C. Park, M. Kim, and J. E. Bobrow, "Newton-type algorithms for dynamics-based robot movement optimization," *IEEE Trans. on Robotics*, vol. 21, no. 4, pp. 657–667, 2005.

[9] S. Lee, M. Leibold, M. Buss, and F. C. Park, "Rollover prevention of mobile manipulators using invariance control and recursive analytic zmp gradients," *Advanced Robotics*, vol. 26, no. 11-12, pp. 1317–1341, 2012.

[10] X. Li, Y. Gu, L. Wu, Q. Sun, and T. Song, "Time and energy optimal trajectory planning of wheeled mobile dual-arm robot based on tip-over stability constraint," *Applied Sciences*, vol. 13, no. 6, p. 3780, 2023.

[11] S. Furuno, M. Yamamoto, and A. Mohri, "Trajectory planning of mobile manipulator with stability considerations," in *2003 IEEE Int. Conf. on Robotics and Automation*, vol. 3, 2003, pp. 3403–3408.

[12] K. Alipour, P. Daemi, A. Hassanpour, and B. Tarvirdizadeh, "On the capability of wheeled mobile robots for heavy object manipulation considering dynamic stability constraints," *Multibody System Dynamics*, vol. 41, pp. 101–123, 2017.

[13] S. G. Tarantos and G. Oriolo, "Real-time motion generation for mobile manipulators via NMPC with balance constraints," in *2022 30th Mediterranean Conf. on Control and Automation*, 2022, pp. 853–860.

[14] A. D. Ames, X. Xu, J. W. Grizzle, and P. Tabuada, "Control barrier function based quadratic programs for safety critical systems," *IEEE Trans. on Automatic Control*, vol. 62, no. 8, pp. 3861–3876, 2017.

[15] A. D. Ames, S. Coogan, M. Egerstedt, G. Notomista, K. Sreenath, and P. Tabuada, "Control barrier functions: Theory and applications," in *2019 18th European Control Conf.*, 2019, pp. 3420–3431.

[16] A. Agrawal and K. Sreenath, "Discrete control barrier functions for safety-critical control of discrete systems with application to bipedal robot navigation," in *Robotics: Science and Systems*, vol. 13, 2017, pp. 1–10.

[17] J. Zeng, B. Zhang, and K. Sreenath, "Safety-critical model predictive control with discrete-time control barrier function," in *2021 American Control Conf.*, 2021, pp. 3882–3889.

[18] B. Xu and K. Sreenath, "Safe teleoperation of dynamic uavs through control barrier functions," in *2018 IEEE International Conference on Robotics and Automation*, 2018, pp. 7848–7855.

[19] B. Siciliano, L. Sciavicco, L. Villani, and G. Oriolo, *Robotics: Modelling, Planning and Control*. Springer, 2008.

[20] R. Campa and H. De La Torre, "Pose control of robot manipulators using different orientation representations: A comparative review," in *2009 American Control Conf.*, 2009, pp. 2855–2860.

[21] S. J. Wright, *Numerical optimization*. Springer, 2006.

[22] F. C. Park, J. E. Bobrow, and S. R. Ploen, "A lie group formulation of robot dynamics," *The Int. J. of Robotics Research*, vol. 14, no. 6, pp. 609–618, 1995.

[23] S. R. Ploen and F. C. Park, "Coordinate-invariant algorithms for robot dynamics," *IEEE Trans. on Robotics and Automation*, vol. 15, no. 6, pp. 1130–1135, 1999.