# Shape Servoing of a Soft Object Using Fourier Series and a Physics-based Model

Fouad Makiyeh, François Chaumette, Maud Marchal, Alexandre Krupa

► To cite this version:

Fouad Makiyeh, François Chaumette, Maud Marchal, Alexandre Krupa. Shape Servoing of a Soft Object Using Fourier Series and a Physics-based Model. IROS 2023 - IEEE/RSJ International Conference on Intelligent Robots and Systems, Oct 2023, Detroit (MI), United States. pp.6356-6363. hal-04215551

HAL Id: hal-04215551

https://hal.science/hal-04215551v1

Submitted on 22 Sep 2023

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>

Distributed under a Creative Commons CC BY 4.0 - Attribution - International License
# Shape Servoing of a Soft Object
Using Fourier Series and a Physics-based Model

Fouad Makiyeh<fnref n="1" />, François Chaumette<fnref n="1" />, Maud Marchal<fnref n="2" />, Alexandre Krupa<fnref n="1" />

**Abstract**— In this paper, we propose a physics-based robot controller to deform a soft object toward a desired 3D shape using a limited number of handling points. For this purpose, the shape of the deformable object is represented using Fourier descriptors. We derive the analytical relation that provides the variation of the Fourier coefficients as a function of the movements of the handling points by considering a mass-spring model (MSM). A control law is then designed from this relation. Since the MSM provides an approximation of the object behavior, which in practice can lead to a drift between the object and its model, an online realignment of the model with the real object is performed by tracking its surface from data provided by a remote RGB-D camera. Simulation results validate the approach for the case where many points interact on a 2D soft object while experimental results obtained with two robotic arms demonstrate the autonomous shaping of a 3D soft object.

## I. INTRODUCTION

While traditional robotic applications focus on manipulating rigid objects, interacting with soft objects remains a more challenging task due to their deformable behavior. Robotic manipulation of soft objects has already been considered in applications such as robotic surgical assistance, clothing, and food handling, as well as in the automotive industry, to name a few.

Recently, in [1], we proposed a vision-based and model-based control strategy to indirectly position one point belonging to a soft object, referred as the feature point, by acting on a distant point, referred as the manipulated point. This approach is useful when one point needs to be automatically positioned. However, when the entire shape of the object needs to be controlled, it is no longer appropriate as it would require a large number of feature points for representing the object shape. To address this issue, a global representation of the object shape can be used. For example, Collewet et al. [2] used Fourier coefficients as features in image-based visual servoing for describing the contour of a rigid object. Chaumette [3] described the object contours from image moments and Yazicioglu et al. [4] approximated the object boundaries with polynomials, to name a few. While these approaches have been used in visual servoing to control the displacement of a camera with respect to a rigid object,

Navarro et al. used truncated Fourier series to approximate the 2D contour of a soft object and to control its 2D shape in the image plane [5]. Recently, Qi et al. [6] used a compact feedback vector built from 2D contour moments and applied sliding mode control to servo the shape of a 3D deformable object. One limitation of these two previous approaches is that they only control the boundary contour observed from a particular incidence view of the camera. Indeed, for a 3D object, there exists an infinite number of possible outer boundary 2D contours since they depend on the relative orientation between the camera and the object. Our objective in this work is therefore to develop a new approach for controlling the shape of the complete visible 3D surface of the object and not only its outer 2D contour. Our goal is to align the 3D surface shape of the object to a desired 3D surface shape by controlling the motion of the robotic manipulators deforming it.

To perform this task, two key components have to be examined. The first one is the determination of the relation that links the variation of the extracted features to the displacement of the 3D points belonging to the soft object. This depends on the definition of the shape parametrization used. The second one involves determining the relation between the displacement of any 3D point belonging to the soft object and the motions applied to the available manipulated points. Some works have already focused on these topics and they can be classified in two categories. The first one concerns the model-free approaches, which do not require neither prior information on deformation behavior, nor physics-based models. For instance, Navarro-Alarcon et al. developed a series of works for controlling the configuration of deformable objects using visual servoing. In these works, the shape of the object is described by a set of visual features extracted from a 2D image corresponding, for example, to multiple 2D points, the relative angle of a line of interest constructed from two 2D points, the curvature of a contour of interest defined from three 2D points [7], [8], or the object 2D contour [9]. The shape servoing task is performed by numerically estimating a deformation Jacobian matrix, which relates the visual feature variations to the motions of the manipulated points, using data provided by a RGB camera [7]–[9]. These works present important advantages since they do not rely on any deformation model of the object. However, they are sensitive to measurement noise since the estimation of the deformation Jacobian is only based on these measurements. The second category of approaches concerns the physics-based methods, where a model approximating the physical behavior of the object

<footnote>*This work was supported by the GentleMAN (299757) and the BIFROST (313870) projects funded by The Research Council of Norway. Experiments presented in this paper were carried out thanks to a platform of the Robotex 2.0 French research infrastructure.</footnote>

<footnote n="1">¹F. Makiyeh, A. Krupa, F. Chaumette are with Inria, Univ. Rennes, CNRS, IRISA, Campus de Beaulieu, 35042 Rennes, France. Firstname.Name@inria.fr</footnote>

<footnote n="2">²M. Marchal is with Univ. Rennes, INSA, IRISA, Inria, CNRS, France and IUF. Maud.Marchal@irisa.fr</footnote>
is considered. Recently, Shetab-Bushehri et al. [10] used the As-Rigid-As-Possible deformation model to approximate the object shape and they considered 3D points belonging to the object as feedback for the shape servoing task. Other types of physics-based models such as the finite element model (FEM) [11] and the mass-spring model (MSM) [12], [13] were also used. Das and Sarkar [12] developed a method for handling a 2D deformable object relying on MSM with multiple robotic manipulators. The shape of the object is controlled by applying an optimization technique over the model boundary points, but only simulation results were presented. Das et al. [13] used three robotic fingers to deform an object represented by a MSM for driving a single internal point to a desired position. They used a proportional integrator (PI) controller for each finger and only simulation results were presented. Other approaches used FEM instead of MSM, such as Ficuciello et al. [11] who presented an approach for the dexterous manipulation of 3D deformable objects in an open-loop system.

In this work, we present a new approach that controls multiple manipulator points to drive the entire shape of a soft object parametrized by a set of Fourier coefficients to a desired configuration. We previously proposed a physics-based control law to move a single 3D feature point to a desired location by acting on a different manipulated point [1]. In this study, we expand our method to simultaneously control the position of several 3D feature points by acting on multiple manipulation points. Additionally, we employ Fourier series to describe the surface of a 3D object using spherical coordinates. In case of planar objects, Fourier series can also be used from the polar coordinates of the 2D contour shape. We use the MSM to simulate the object physical behavior and utilize a RGB-D camera to online track its current shape and correct any possible drift between the simulated model and the actual object. The main contribution of this work is the analytic derivation of the relation that links the variation of the Fourier coefficients, which represent the object shape parameters, to the movements of multiple manipulated points. Based on this relation, a closed-loop control law is then developed to automatically deform the soft object toward a desired shape.

An overview of our approach is presented in Fig. 1. The main steps can be outlined as:

1) Compute the velocities to be applied to the manipulated points (see Section II-E).

2) Update the physics-based model using the internal and external forces acting on it (see Section II-A).

3) Reduce the drift between the model and the real object by imposing external constraints on the model surface from data provided by the RGB-D camera (see Section IV-A).

4) Compute the error between the feature vector representing the object surface and the one corresponding to the desired shape (see Sections II-C and II-E). If this error is not low enough, go to step 1).

The paper is organized as follows: Section II presents

<figure><img src="image_2.png" /><figcaption>Fig. 1. Block diagram of the approach: visual tracking part in blue, physics-based simulator in green, and closed-loop control scheme in red.</figcaption></figure>

briefly the MSM physics-based model, details the modelling of the features representing the object shape, and develops the control law. Section III and Section IV show simulation and experimental results respectively. Finally, Section V ends the paper with a conclusion on the proposed approach and introduces future works.

## II. MANIPULATION CONTROLLER DESIGN

### A. MSM model

This part presents a brief description of the mass-spring model used to approximate the soft object behavior. It has been chosen since it is simple to implement and it has already been applied to simulate and deform soft objects as in [1]. The position of any point $P_i$, where $i \in \mathcal{N} = [1, ..., N]$ and $N$ is the number of model points, with corresponding mass $m_i$, coordinate $\mathbf{x}_i$ and velocity $\dot{\mathbf{x}}_i$, is given at time $t = t+dt$ by:

$$ \mathbf{x}_i(t + dt) = \mathbf{x}_i(t) + \frac{dt^2}{m_i} \mathbf{f}_{si}(t) + (dt - \frac{dt^2}{m_i} D_v) \dot{\mathbf{x}}_i(t) + \frac{dt^2}{m_i} \mathbf{f}_{ei}(t) \quad (1) $$

with $dt$ the simulation step time and $\mathbf{f}_{ei} = \mathbf{f}_{Gi} + \mathbf{f}_{Gi} + \mathbf{f}_{ei}$. $\mathbf{f}_{Gi}$ and $\mathbf{f}_{Gi}$ are respectively the 3D gravitational force and the normal force exerted by the support on which the object lies, while $\mathbf{f}_{ei}$ is a 3D force vector representing additional external constraints. This force is applied each time a drift between the model and the real object is detected (see Section IV-A). Finally, $\mathbf{f}_{si}$ is the 3D force vector acting on $P_i$ due to springs with stiffness $K_{ij}$ connecting $P_i$ to its neighbors, $P_j, \forall j \in \nu_i \subset \mathcal{N}$, given by:

$$ \mathbf{f}_{si} = \sum_{j \in \nu_i} \mathbf{f}_{sij} = \sum_{j \in \nu_i} \Gamma_{ij} K_{ij} (\mathbf{x}_j - \mathbf{x}_i) \quad (2) $$

where $\Gamma_{ij} = \frac{\|\mathbf{x}_i - \mathbf{x}_j\| - l_{ij}^0}{\|\mathbf{x}_i - \mathbf{x}_j\|}$ with $l_{ij}^0$ the rest length of the spring connecting $P_i$ and $P_j$, and $\nu_i$ denotes the indices in $\mathcal{N}$ of the points in the neighborhood of $P_i$.
## B. Problem formulation

The main objective of this work is to deform a 3D soft object with w surface points, represented by $\mathbf{x}_o = (\mathbf{x}_{o1}, ..., \mathbf{x}_{oW})$; $\forall 1 \le i \le w$; $o_i \in \mathcal{N}$, where $o_i$ is the index of each surface point in $\mathcal{N}$. Let $\mathbf{s}$ be a feature vector representing the 3D surface $\mathbf{x}_o$ of the object. The goal is to drive $\mathbf{s}$ to a desired target shape $\mathbf{s}^*$ by simultaneously manipulating M points on the object, represented by $[P_{m1}, ..., P_{mM}]$, with coordinates $(\mathbf{x}_{m1}, ..., \mathbf{x}_{mM})$. By determining how $\mathbf{s}$ changes due to the motion of these manipulated points, the required motions to be applied on them for achieving the desired shape can be determined as a classical visual servoing problem [14].

## C. Surface shape parameters

By expressing the points $\mathbf{x}_{o1}, l \in [1, ..., w]$ on the object surface with spherical coordinates, each component can be written as:

$$ \left\{ \begin{aligned} x_{o1} &= x_o^g + \rho(\theta_{o1}, \phi_{o1}) \cos(\phi_{o1}) \sin(\theta_{o1}) \\ y_{o1} &= y_o^g + \rho(\theta_{o1}, \phi_{o1}) \sin(\phi_{o1}) \sin(\theta_{o1}) \\ z_{o1} &= z_o^g + \rho(\theta_{o1}, \phi_{o1}) \cos(\theta_{o1}) \end{aligned} \right. \quad (3) $$

with $\mathbf{x}_o^g = (x_o^g, y_o^g, z_o^g)$ the 3D coordinates of the centroid of $\mathbf{x}_o$, $\rho(\theta_{o1}, \phi_{o1})$ the radial distance $\|\mathbf{x}_{o1} - \mathbf{x}_o^g\|$ of the point $P_{o1}$ from $\mathbf{x}_o^g$, $\theta_{o1} = \arccos(z_{o1}/\rho(\theta_{o1}, \phi_{o1}))$ the polar angle and $\phi_{o1} = \text{atan2}(y_{o1}, x_{o1})$ the azimuthal angle. $\rho(\theta, \phi)$ is a closed continuous periodic function, which can be represented as a sum of cosine and sine functions with increasing frequencies, known as Fourier descriptors. It can be approximated using Fourier series as follows:

$$ \rho(\theta_{o1}, \phi_{o1}) = \sum_{i=0}^{p} \sum_{j=0}^{q} \left[ a_{ij}c(i\theta_{o1})c(j\phi_{o1}) + b_{ij}c(i\theta_{o1})s(j\phi_{o1}) + c_{ij}s(i\theta_{o1})c(j\phi_{o1}) + d_{ij}s(i\theta_{o1})s(j\phi_{o1}) \right] \quad (4) $$

where c stands for cos, s for sin, $a_{ij}$, $b_{ij}$, $c_{ij}$ and $d_{ij}$ are the Fourier coefficients, and p and q are the number of harmonics corresponding to $\theta$ and $\phi$ respectively. Hence, $\mathbf{s}$ representing the shape parameters is selected as:

$$ \mathbf{s} = (x_o^g, y_o^g, z_o^g, a_{00}, ..., a_{pq}, b_{01}, ..., b_{pq}, c_{pq}, d_{pq}) \quad (5) $$

As noticed from (5), the dimension k of $\mathbf{s}$, ($k = 4(p+1)(q+1) - 2(p+q)$) is significantly less than the number of object surface points for low values of p and q.

Regarding the components of $\mathbf{s}$, they are obtained as follows:

1) The centroid of the model surface points is first computed: $\mathbf{x}_o^g = \frac{1}{w} \sum_{i=1}^{w} \mathbf{x}_{o1}$

2) The model surface points are centered with respect to the centroid: $\hat{\mathbf{x}}_o = \mathbf{x}_o - \mathbf{x}_o^g$

3) The Fourier coefficients are then computed by a least squares method. In theory, only $(\text{dim}(\mathbf{s}) - 3)$ surface points are needed, but to increase robustness to measurement noise, all w points are considered in the computation.

Once $\mathbf{s}$ has been calculated, an approximation of the object surface can be reconstructed by setting the range of $\theta$ to $[0, \pi]$ and $\phi$ to $[0, 2\pi]$.

## D. Modeling

By approximating the physical behavior of the object using the MSM, we demonstrated in [1] that the velocity $\dot{\mathbf{x}}_f$ of a feature point $P_f$ due to the velocity $\dot{\mathbf{x}}_m$ applied on a manipulated point $P_m$ is given by:

$$ \dot{\mathbf{x}}_f = \gamma_{fm} \dot{\mathbf{x}}_m + \delta_f \quad (6) $$

where the vector $\delta_f = \mathbf{r}_f/\Delta t$ represents the transient dynamics of the system and $\Delta t$ denotes the step time of the control law. The $3 \times 3$ matrix $\gamma_{fm}$ is the velocity ratio between $\dot{\mathbf{x}}_f$ and $\dot{\mathbf{x}}_m$ and is given at each time t by [1]:

$$ \gamma_{fm}(t + dt) = \gamma_{fm}(t) + \frac{dt^2}{m_f}\gamma_{sf}(t) + \left(1 - \frac{dt}{m_f}D_v\right)(\gamma_{fm}(t) - \gamma_{fm}(t - dt)) \quad (7) $$

with

$$ \gamma_{sf}(t) = \sum_{F \in \nu_f} \frac{\partial f_{sf}}{\partial \mathbf{x}_F} (\gamma_F(t) - \gamma_f(t)) $$

where $\nu_f$ is the set of points in the neighborhood of $P_f$ and $f_{sfF}$ is the spring forces between $P_f$ and its neighbors. Finally, the form of $\mathbf{r}_f$ is quite complex but fully given in [1]. Let us note that $\mathbf{r}_f = \mathbf{0}$ at the beginning and at the end of the servo.

Eq. (6) is valid when a single manipulated point is considered. When several manipulated points deform the object simultaneously, the velocity of a feature point $P_f$ is obtained from the combination of the motions of these manipulated points. More precisely, when M manipulators ($P_{m1}, P_{m2}, ..., P_{mM}$) are deforming the object simultaneously with corresponding velocities $\dot{\mathbf{x}}_m = (\dot{\mathbf{x}}_{m1}, ..., \dot{\mathbf{x}}_{mM})$, we can show by similarity with (6) that the velocity of $P_f$ is given by:

$$ \dot{\mathbf{x}}_f = \sum_{l=1}^{M} \gamma_{fm_l} \dot{\mathbf{x}}_{m_l} + \delta_f \quad (8) $$

where $\dot{\mathbf{x}}_{m_l}$ is the applied velocity on $P_{m_l}$, $l \in [1, ..., M]$. $\gamma_{fm_l}$ is calculated in the same way as presented in (7). Additionally, $\delta_f$ can also be computed similarly as in [1].

From (8), we directly obtain the velocity of all points belonging to the model surface:

$$ \dot{\mathbf{x}}_o = \mathbf{L}_o \dot{\mathbf{x}}_m + \delta_o \quad (9) $$

with

$$ \mathbf{L}_o = \begin{bmatrix} \gamma_{o1m1} & \cdots & \gamma_{o1m_M} \\ \gamma_{o2m1} & \cdots & \gamma_{o2m_M} \\ \vdots & \ddots & \vdots \\ \gamma_{owm1} & \cdots & \gamma_{owm_M} \end{bmatrix}, \delta_o = \begin{bmatrix} \delta_{o1} \\ \vdots \\ \delta_{ow} \end{bmatrix}, \dot{\mathbf{x}}_o = \begin{bmatrix} \dot{\mathbf{x}}_{o1} \\ \vdots \\ \dot{\mathbf{x}}_{ow} \end{bmatrix} $$

The ultimate goal is to find the variation $\dot{\mathbf{s}}$ of the features with respect to the motions $\dot{\mathbf{x}}_m$ of the manipulated points, through the following form:

$$ \dot{\mathbf{s}} = \mathbf{L}_{\mathbf{s}} \dot{\mathbf{x}}_m + \delta_{\mathbf{s}} \quad (10) $$
By deriving (3) and (4), we obtain the motion $\dot{x}_{ol} = (\dot{x}_{ol}, \dot{y}_{ol}, \dot{z}_{ol})$ of each point belonging to the object surface, as a function of $\dot{s}, \dot{\theta}_{ol}$ and $\dot{\phi}_{ol}$. However, we have to eliminate $\dot{\theta}$ and $\dot{\phi}$ in order to express the motions of these points solely in terms of $\dot{s}$. For that, by summing the three equations in (3) after introducing terms $\alpha_{ol}, \beta_{ol}$ and $\sigma_{ol}$, we obtain:

$$ \alpha_{ol} \dot{x}_{ol} + \beta_{ol} \dot{y}_{ol} + \sigma_{ol} \dot{z}_{ol} = [\alpha_{ol} \ \beta_{ol} \ \sigma_{ol} \ \nu_{ol} \frac{\partial \rho(\theta_{ol}, \phi_{ol})}{\partial \mathbf{q}}] \dot{\mathbf{s}} \quad (11) $$

with $\forall l \in [1, ..., w]$

$$ \alpha_{ol} = b_1 c_2 - b_2 c_1 $$

$$ \beta_{ol} = a_2 c_1 - a_1 c_2 $$

$$ \sigma_{ol} = a_1 b_2 - a_2 b_1 $$

$$ \nu_{ol} = \alpha_{ol} [(\cos(\phi_{ol}) + \beta_{ol} \sin(\phi_{ol})) \sin(\theta_{ol}) + \sigma_{ol} \cos(\theta_{ol})] $$

$$ \mathbf{q} = (a_{00}, ..., a_{pq}, b_{01}, ..., b_{pq}, c_{pq}, d_{pq}) $$

and

$$ a_1 = \left( \frac{\partial \rho(\theta_{ol}, \phi_{ol})}{\partial \theta_{ol}} \sin(\theta_{ol}) + \rho(\theta_{ol}, \phi_{ol}) \cos(\theta_{ol}) \right) \cos(\phi_{ol}), $$

$$ b_1 = \left( \frac{\partial \rho(\theta_{ol}, \phi_{ol})}{\partial \theta_{ol}} \sin(\theta_{ol}) + \rho(\theta_{ol}, \phi_{ol}) \cos(\theta_{ol}) \right) \sin(\phi_{ol}), $$

$$ c_1 = \frac{\partial \rho(\theta_{ol}, \phi_{ol})}{\partial \theta_{ol}} \cos(\theta_{ol}) - \rho(\theta_{ol}, \phi_{ol}) \sin(\theta_{ol}), $$

$$ a_2 = \left( \frac{\partial \rho(\theta_{ol}, \phi_{ol})}{\partial \phi_{ol}} \cos(\phi_{ol}) - \rho(\theta_{ol}, \phi_{ol}) \sin(\phi_{ol}) \right) \sin(\theta_{ol}), $$

$$ b_2 = \left( \frac{\partial \rho(\theta_{ol}, \phi_{ol})}{\partial \phi_{ol}} \sin(\phi_{ol}) + \rho(\theta_{ol}, \phi_{ol}) \cos(\phi_{ol}) \right) \sin(\theta_{ol}), $$

$$ c_2 = \frac{\partial \rho(\theta_{ol}, \phi_{ol})}{\partial \phi_{ol}} \cos(\theta_{ol}). $$

By processing similarly with (9) and identifying the equation obtained with (11), we get:

$$ \begin{bmatrix} \alpha_{o1} \dot{x}_{o1} + \beta_{o1} \dot{y}_{o1} + \sigma_{o1} \dot{z}_{o1} \\ \vdots \\ \alpha_{oW} \dot{x}_{oW} + \beta_{oW} \dot{y}_{oW} + \sigma_{oW} \dot{z}_{oW} \end{bmatrix} = \mathbf{A}_o \dot{\mathbf{x}}_m + \mathbf{b}_o \\ = \mathbf{C}_o \dot{\mathbf{s}} \quad (12) $$

with

$$ \mathbf{A}_o = \begin{bmatrix} \begin{bmatrix} \alpha_{o1} & \beta_{o1} & \sigma_{o1} \\ \vdots & \vdots & \vdots \\ \alpha_{oW} & \beta_{oW} & \sigma_{oW} \end{bmatrix} \begin{bmatrix} \gamma_{o1} m_1 & \cdots & \gamma_{o1} m_M \end{bmatrix} \\ \underbrace{\begin{bmatrix} \alpha_{oW} & \beta_{oW} & \sigma_{oW} \end{bmatrix}}_{1 \times 3} \begin{bmatrix} \gamma_{oW} m_1 & \cdots & \gamma_{oW} m_M \end{bmatrix} \end{bmatrix}_{3 \times 3M} $$

$$ \mathbf{b}_o = \begin{bmatrix} \begin{bmatrix} \alpha_{o1} & \beta_{o1} & \sigma_{o1} & \delta_{o1} \\ \vdots & \vdots & \vdots & \vdots \\ \alpha_{oW} & \beta_{oW} & \sigma_{oW} & \delta_{oW} \end{bmatrix} \end{bmatrix} $$

$$ \mathbf{C}_o = \begin{bmatrix} \alpha_{o1} & \beta_{o1} & \sigma_{o1} & \nu_{o1} \frac{\partial \rho(\theta_{o1}, \phi_{o1})}{\partial \mathbf{q}} \\ \vdots & \vdots & \vdots & \vdots \\ \alpha_{oW} & \beta_{oW} & \sigma_{oW} & \nu_{oW} \frac{\partial \rho(\theta_{oW}, \phi_{oW})}{\partial \mathbf{q}} \end{bmatrix} $$

By identifying (12) with (10), we finally obtain:

$$ \mathbf{L}_s = \mathbf{C}_o^+ \mathbf{A}_o \quad \text{and} \quad \mathbf{\delta}_s = \mathbf{C}_o^+ \mathbf{b}_o \quad (13) $$

with $\mathbf{C}_o^+$ the Moore-Penrose pseudo-inverse of $\mathbf{C}_o$. Matrix $\mathbf{C}_o$ is of dimension $w \times k$ and is full rank since the number $w$ of surface points is highly larger than the number of harmonics. $\mathbf{L}_s$ can be considered as the interaction matrix by similarity with the classical visual servoing framework [14], while $\mathbf{\delta}_s$ is a feed-forward term that does not appear in classical shape servoing works.

### E. Control scheme

This section presents the control law to be applied to the manipulated points in order to drive the object to its desired shape. Similarly to what is classically done in visual servoing, by specifying a decoupled exponential decay for the visual features $\mathbf{s}$ to their desired values $\mathbf{s}^*$, that is, $\dot{\mathbf{s}} = -\lambda (\mathbf{s} - \mathbf{s}^*)$ where $\lambda$ is a positive gain, we obtain from (10):

$$ \dot{\mathbf{x}}_m = -\lambda \mathbf{L}_s^+ (\mathbf{s} - \mathbf{s}^*) - \mathbf{L}_s^+ \mathbf{\delta}_s \quad (14) $$

Regarding the achievement of the deformation task, it depends on the number of harmonics $p$ and $q$. When $k > 3M$, the controller (14) is under-actuated and cannot guarantee the asymptotic convergence of the system. Conversely, when $k \le 3M$, the controller is either fully-actuated or over-actuated and therefore stability and convergence of $\mathbf{s}$ towards $\mathbf{s}^*$ is theoretically guaranteed. Let us note however that this does not necessarily mean that the full object will reach its desired shape, but only its representation by the selected Fourier coefficients. So, the choice of number of harmonics to be injected in $\mathbf{s}$ depends on the complexity of the shape to be obtained, while complex deformations require more harmonics and consequently more manipulated points.

### F. Specific application to planar objects

The method described previously can easily be applied to planar objects. The shape of a 2D object is represented by its contour $\mathbf{x}_c$ containing $w_c$ contour points $\mathbf{x}_{c1}$. It can be parameterized by its centroid $(\mathbf{x}_c^g, \mathbf{y}_c^g)$ and radial distance $\rho(\theta)$ that now only depends on a single angle $\theta$. Doing so, any point $\mathbf{x}_{c1}$ in $\mathbf{x}_c$ can be expressed using polar coordinates as follows:

$$ \mathbf{x}_{c1} = (\mathbf{x}_c^g + \rho(\theta_{c1}) \cos(\theta_{c1}), \mathbf{y}_c^g + \rho(\theta_{c1}) \sin(\theta_{c1})). \quad (15) $$

As $\rho(\theta)$ is periodic, it can be approximated using Fourier series as following:

$$ \rho(\theta_{c1}) = a_0^c + \sum_{i=1}^{p_c} [a_i^c \cos(i\theta_{c1}) + b_i^c \sin(i\theta_{c1})] \quad (16) $$

with $p_c$ the number of harmonics corresponding to $\theta$. The features $\mathbf{s}_c$ are thus naturally selected as:

$$ \mathbf{s}_c = (x_c^g, y_c^g, a_0^c, a_1^c, b_1^c, \dots, a_{p_c}^c, b_{p_c}^c) \quad (17) $$
Following the same reasoning as in Section II-D, the variation of $\mathbf{s}_c$ with respect to the motions of the manipulated points $\dot{\mathbf{x}}_m$ can be expressed as in (10):

$$ \dot{\mathbf{s}}_c = \mathbf{L}_{s_c} \dot{\mathbf{x}}_m + \mathbf{\delta}_{s_c} \quad (18) $$

with $\mathbf{L}_{s_c} = \mathbf{C}_e^+ \mathbf{A}_e$ and $\mathbf{\delta}_{s_c} = \mathbf{C}_e^+ \mathbf{b}_e$ where

$$ \begin{aligned} \mathbf{A}_e &= \begin{bmatrix} [\alpha_{c_1} & \beta_{c_1}] [\gamma_{c_1 m_1} & \cdots & \gamma_{c_1 m_M}] \\ [\alpha_{c_{w_c}} & \beta_{c_{w_c}}] [\gamma_{c_{w_c} m_1} & \cdots & \gamma_{c_{w_c} m_M}] \end{bmatrix}, \\ \mathbf{b}_e &= \begin{bmatrix} [\alpha_{c_1} & \beta_{c_1}] \mathbf{\delta}_{c_1} \\ [\alpha_{c_{w_c}} & \beta_{c_{w_c}}] \mathbf{\delta}_{c_{w_c}} \end{bmatrix}, \\ \mathbf{C}_e &= \begin{bmatrix} \alpha_{c_1} & \beta_{c_1} & \nu_{c_1} \frac{\partial \rho(\theta_{c_1})}{\partial \mathbf{q}_c} \\ \vdots & \vdots & \vdots \\ \alpha_{c_{w_c}} & \beta_{c_{w_c}} & \nu_{c_{w_c}} \frac{\partial \rho(\theta_{c_{w_c}})}{\partial \mathbf{q}_c} \end{bmatrix}, \end{aligned} $$

and

$$ \alpha_{c_l} = \rho(\theta_{c_l}) \cos(\theta_{c_l}) + \frac{\partial \rho(\theta_{c_l})}{\partial \theta_{c_l}} \sin(\theta_{c_l}), $$

$$ \beta_{c_l} = \rho(\theta_{c_l}) \sin(\theta_{c_l}) - \frac{\partial \rho(\theta_{c_l})}{\partial \theta_{c_l}} \cos(\theta_{c_l}), $$

$$ \nu_{c_l} = \alpha_{c_l}(\cos(\theta_{c_l}) + \beta_{c_l}\sin(\theta_{c_l})), $$

$$ \mathbf{q}_c = (a_0^c, a_1^c, b_1^c, \dots, a_{p_c}^c, b_{p_c}^c). $$

The closed-loop control law to be applied by the robotic manipulators is directly deduced from (18):

$$ \dot{\mathbf{x}}_m = -\lambda \mathbf{L}_{s_c}^{+} (\mathbf{s}_c - \mathbf{s}_c^*) - \mathbf{L}_{s_c}^{+} \mathbf{\delta}_{s_c}, \quad (19) $$

### III. SIMULATION RESULTS

In this section, the proposed controller is tested in simulation for deforming a 2D object to a desired shape using Fourier descriptors.

As already mentioned, the number of selected harmonics influences the accuracy of the deformation task. In other terms, it is crucial to choose an appropriate number of harmonics for obtaining a correct approximation of an object contour. For complex shapes, selecting a higher number of harmonics will improve the accuracy of the approximation. However, when the contour can be approximated using a low number of harmonics all along its deformation, there is no need to select a large number, since it will make the system more and more under-actuated without providing any significant information. Additionally, the desired shape must be feasible. This can be obtained through simulation by applying arbitrary motions on the chosen manipulated points in order to deform the object. The resulting contour can then be selected as the desired one since we are sure that it is feasible.

The chosen 2D object is depicted in Fig. 2. It is represented using a MSM with 256 points, where the small red points are the model points while the 6 larger red points denote the manipulated points that have been uniformly distributed around the object. Green points denote contour

points ($w_c = 32$) and the white lines correspond to the springs between the model points. The white points represent the target points, which correspond to the green points acquired after manually deforming the object to obtain a feasible desired shape. The blue ones depict the desired reconstructed contour using $p_c = 8$ harmonics. The yellow points on Fig. 2.b correspond to the reconstructed contour at the end of the servo using the same number of 8 harmonics. We can notice that the yellow points fit with the green and blue ones and the green points align with the white points, showing the success and the accuracy of the system using 8 harmonics.

The physical parameters of the object have been chosen as follows. The mass of each point is $m_i = 0.35g$. The stiffness matrix $\mathbf{K}_{ij}$ depends on two constant values: $\mathbf{K}_{ij} = \text{diag}(k_x, k_y)$ with $k_x = k_y = 13N/m$. The damping has been chosen according to [1] as $D_v = 2\sqrt{m_i k_x} = 0.13Ns/m$. Finally, we have selected $\lambda = 0.9$ as gain for the control scheme.

<figure><img src="image_3.png" /><figcaption>Fig. 2. Deformation of a planar object: (a) initial and desired configurations, (b) final configuration (see text for explanations on color code).</figcaption></figure>

Different simulations have been performed by selecting a different number of harmonics ($p_c = 6, 8$ and $10$), while the number of manipulated points is always the same ($M = 6$). The purpose of testing these different numbers of harmonics is to study their effects on the deformation performance. The greater the number of harmonics, the better the reconstruction of the contour. However, from a certain amount of $p_c$, the reconstruction is perfect and we no longer need to increase it. To correctly reconstruct the contour of the desired shape shown in Fig. 2, at least 6 harmonics are needed. For that, we test $p_c = 6$. Additionally, to achieve a more accurate approximation, we also test two larger numbers of harmonics, $p_c = 8$ and $10$. Moreover, we also aim to evaluate the achievement of the task when the system is under-actuated (here, we have 12 DOF while $k_c = \text{dim}(\mathbf{s}_c) = 2p_c + 3$).

The time evolution of the error norm $||\mathbf{s}_c - \mathbf{s}_c^*||$ for the different number of harmonics is presented in Fig. 3. The error norm converges exponentially to nearly zero when $p_c = 6$, despite the system being under-actuated (green plot). For the other cases (blue and black plots), a very small residual
remains at the convergence of the system.

Furthermore, the difference between the actual and desired contour is shown in Fig. 4 for the different values of $p_c$. This difference has been computed using the Hausdorff distance [15] by considering all contour points. We can notice a nice exponential decrease of this error in all cases. After convergence, the remaining error is less than 3% of the initial error using 6 harmonics (green plot) while it is reduced to less than 2% for $p_c = 8$ and 10 (blue and black plots). Since the residual of the Hausdorff distance is almost the same for these two harmonics, using 8 harmonics is here sufficient to achieve the task with high accuracy.

<figure><img src="image_4.png" /><figcaption>Fig. 3. Evolution of the error norm for different numbers of harmonics and using 6 manipulated points.</figcaption></figure>

<figure><img src="image_5.png" /><figcaption>Fig. 4. Evolution of the Hausdorff distance between the actual and desired contours for different numbers of harmonics and using 6 manipulated points.</figcaption></figure>

# IV. REAL EXPERIMENTS

## A. Setup & Implementation

Our experimental setup consists of two 6-DOF robotic arms from ADEPT ($M = 2$), a Viper 850 and a Viper 650. Two rigid sticks are attached to the robots, used as tools for the deformation process. These sticks are in contact with the deformable object and the contact points are used as the manipulated points of the soft object. The visual perception of the object is performed with a remote RGB-D Intel Realsense-D435 camera. It acquires around $3.10^5$ 3D points at 30 frames per second (fps).

A simulator has been developed in C++ to simulate the deformable object model and update its point positions at a frequency of 600 Hz, i.e., $dt = 1.66$ ms. The velocities computed by the control scheme (14) are initially expressed in the camera frame since the data are measured in this

frame. Thanks to a classical hand-eye calibration (the camera is fixed in the workspace), these velocities are then expressed in their respective end-effector frame and applied to the manipulated points. The control law is executed at the same frequency as the camera, which is 30 Hz, i.e., $\Delta t = 33.33$ ms. The overall setup is shown in Fig. 5.

<figure><img src="image_6.png" /><figcaption>Fig. 5. 1: Viper 650. 2: Viper 850. 3: RGB-D camera. 4: Soft object. 5: Rigid sticks attached to Viper850 and Viper650.</figcaption></figure>

The chosen object is an ellipsoid ball shown in Fig. 6. Its MSM parameters are approximated as presented in [1]. More precisely, we set $K_{ij} = \text{diag}(30, 10, 30) \text{ N.m}^{-1}, \forall i \in \mathcal{N}$, $j \in \nu_i$ and $D_v = 0.35 \text{ N.s.m}^{-1}$. Since both the model and its parameters are coarse, a drift may appear between the real dynamical behavior of the object and its model. To get rid of this drift, the object is segmented and tracked each time a new image is acquired using the BGSLibrary [16]. The object surface points $\mathbf{x}_o$ consist of two components: one that is in contact with the table, and the other $\mathbf{x}_s \subset \mathbf{x}_o$ that is observable from the camera. For every surface point of the model with coordinates $\mathbf{x}_{s_i}$, its corresponding point $\hat{\mathbf{p}}_{s_i}$ on the real object surface is obtained thanks to an iterative closest point (ICP) procedure [1], [17]. A drift is detected when these two point clouds are no longer overlapping. Finally, for compensating this drift, external forces $\mathbf{f}_{es_i} = K_{ij}(\hat{\mathbf{p}}^{s_i} - \mathbf{x}_{s_i})$ are set in (1) for all the observable surface points (see Section II-A) so that all points in the model are deformed to comply with these constraints, as proposed in [1]. As a result, this correction phase brings the model very close to the real object and we use the former for the calculation of the features $\mathbf{s}$ given in (5). We selected $p = 1$ and $q = 3$ harmonics in $\mathbf{s}$ since this choice enables to correctly represent the object without being excessively under-actuated ($k = 24$ while 6 DOF are available with $M = 2$ manipulated points).

## B. Experimental results

Two experiments are described in this paper to validate our approach, while additional results are presented in the accompanying video. The convergence is considered achieved as soon as the error $||\mathbf{s} - \mathbf{s}^*||$ reaches 5% of its initial value.
For the first experiment, Fig. 6(a) shows the object at its initial configuration while Fig. 6(b) displays the object after convergence to its desired configuration. Fig. 7 displays a

<figure><img src="image_7.png" /><figcaption>Fig. 6. First experiment: initial (a) and final (b) images acquired by the RGB-D camera.</figcaption></figure>

3D representation of the object surface points observed from two different viewpoints (initial configuration in red, final and desired configurations respectively in green and blue).

<figure><img src="image_8.png" /><figcaption>Fig. 7. First experiment: 3D visualization from 2 different viewpoints of the object surface points (initial positions in red, final positions in green, and desired positions in blue).</figcaption></figure>

The second experiment involves a more complex deformation, as can be seen on Figs. 8 and 9.

<figure><img src="image_9.png" /><figcaption>Fig. 8. Second experiment: initial and final images acquired by the RGB-D camera.</figcaption></figure>

<figure><img src="image_10.png" /><figcaption>Fig. 9. 3D visualization from 2 different viewpoints for the second experiment.</figcaption></figure>

For both experiments, the time evolution of the error norm $||s - s^*||$ is shown in Fig. 10, while Fig. 11 presents the evolution of the Hausdorff distance between the current and desired surface points.

The correct achievement of the deformation task is directly visible from Figs. 7 and 9 where we can note that the final shapes (in green) are well superposed on the desired ones (in blue) despite the displacements and deformations to be achieved. For both medium and large deformations, we can
<figure><img src="image_11.png" /><figcaption>Fig. 10. Evolution of $||s - s^*||$ (first experiment in red, second one in green).</figcaption></figure>

<figure><img src="image_12.png" /><figcaption>Fig. 11. Evolution of the Hausdorff distance between the current and desired object surface points (first experiment in red, second one in green).</figcaption></figure>

see that the features error and Hausdorff distance converge to less than 5% of their initial value in few seconds while we recall that the system is under-actuated. The decreasing is more erratic in the case of the large deformation, which is not surprising for this difficult case. Notice that the initial errors are smaller in this case just because the rigid displacement is smaller. Finally, these results confirm that the selected harmonics were adequate for approximating the object shape and performing the required deformations.

## V. CONCLUSION

In this paper, a new physics-based control scheme was introduced for shaping soft objects into desired configurations using a RGB-D camera and multiple manipulated points. The shape of the object, whether it is planar or volumetric, is represented by a selected number of Fourier coefficients approximating respectively its surface or contour. The relationship between the variation of these parameters and the motion of the robotic manipulators was established using a coarse mass-spring model. This relation allowed us to design the controller that contains a feedback and a feed-forward term. To get rid of the model approximation, a tracking step was performed to cancel the difference between this model and the observed shapes. The proposed approach was evaluated through simulations and real experiments, yielding promising results without the need for special markers or textures. In contrast to existing methods that were limited to servo only the 2D contour of planar or volumetric objects, the proposed approach allows deforming the complete shape of the object by considering its entire surface. As future work,

we aim to explore alternative methods of parameterization to represent soft object shapes.

## REFERENCES

[1] F. Makiyeh, M. Marchal, F. Chaumette, and A. Krupa, "Indirect positioning of a 3d point on a soft object using rgb-d visual servoing and a mass-spring model," in *Int. Conf. on Control, Automation, Robotics and Vision (ICARCV'22)*, 2022, pp. 235-242.

[2] C. Collewet and F. Chaumette, "A contour approach for image-based control on objects with complex shape," in *IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS'2000)*, vol. 1, 2000, pp. 751-756.

[3] F. Chaumette, "Image moments: a general and useful set of features for visual servoing," *IEEE Trans. on Robotics*, vol. 20, no. 4, pp. 713-723, 2004.

[4] A. Y. Yazicioglu, B. Calli, and M. Unel, "Image based visual servoing using algebraic curves applied to shape alignment," in *IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS'09)*, 2009, pp. 5444-5449.

[5] D. Navarro-Alarcon and Y.-H. Liu, "Fourier-based shape servoing: A new feedback method to actively deform soft objects into desired 2-d image contours," *IEEE Trans. on Robotics*, vol. 34, no. 1, pp. 272-279, 2018.

[6] J. Qi, G. Ma, J. Zhu, P. Zhou, Y. Lyu, H. Zhang, and D. Navarro-Alarcon, "Contour moments based manipulation of composite rigid-deformable objects with finite time model estimation and shape/position control," *IEEE/ASME Trans. on Mechatronics*, vol. 27, no. 5, pp. 2985-2996, 2022.

[7] D. Navarro-Alarcon, Y.-H. Liu, J. G. Romero, and P. Li, "Model-free visually servoed deformation control of elastic objects by robot manipulators," *IEEE Trans. on Robotics*, vol. 29, no. 6, pp. 1457-1468, 2013.

[8] D. Navarro-Alarcon, Y.-h. Liu, J. G. Romero, and P. Li, "On the visual deformation servoing of compliant objects: Uncalibrated control methods and experiments," *The Int. Journal of Robotics Research*, vol. 33, no. 11, pp. 1462-1480, 2014.

[9] J. Zhu, D. Navarro-Alarcon, R. Passama, and A. Cherubini, "Vision-based manipulation of deformable and rigid objects using subspace projections of 2d contours," *Robotics and Autonomous Systems*, vol. 142, p. 103798, 2021.

[10] M. Shetab-Bushehri, M. Aranda, Y. Mezouar, and E. Özgür, "As-rigid-as-possible shape servoing," *IEEE Robotics and Automation Letters*, vol. 7, no. 2, pp. 3898-3905, 2022.

[11] F. Ficuciello, A. Migliozzi, E. Coevoet, A. Petit, and C. Duriez, "Fem-based deformation control for dexterous manipulation of 3d soft objects," in *IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS'18)*, 2018, pp. 4007-4013.

[12] J. Das and N. Sarkar, "Autonomous shape control of a deformable object by multiple manipulators," *Journal of Intelligent & Robotic Systems*, vol. 62, no. 1, pp. 3-27, 2011.

[13] —, "Passivity-based target manipulation inside a deformable object by a robotic system with noncollocated feedback," *Advanced Robotics*, vol. 27, no. 11, pp. 861-875, 2013.

[14] F. Chaumette and S. Hutchinson, "Visual servo control. i. basic approaches," *IEEE Robotics & Automation Magazine*, vol. 13, no. 4, pp. 82-90, 2006.

[15] A. A. Taha and A. Hanbury, "An efficient algorithm for calculating the exact hausdorff distance," *IEEE Trans. on Pattern Analysis and Machine Intelligence*, vol. 37, no. 11, pp. 2153-2163, 2015.

[16] A. Sobral and T. Bouwmans, "Bgs library: A library framework for algorithm's evaluation in foreground/background segmentation," in *Handbook on "Background Modeling and Foreground Detection for Video Surveillance"*, Chapter 23. Taylor & Francis, 2014.

[17] A. Petit, V. Lippiello, G. A. Fontanelli, and B. Siciliano, "Tracking elastic deformable objects with an rgb-d sensor for a pizza chef robot," *Robotics and Autonomous Systems*, vol. 88, pp. 187-201, 2017.