selected to ensure elasticity, as discussed in Section 4.1.2, while also guaranteeing stability.

For the low resolution (sparse) model represented using 1542 nodes and depicted in Figure 3.6(a), the mass is considered to be the same for all points: $m_i^s = 0.35g$. Its corresponding stiffness matrix $K_{ij}^s$ depends on three constant values, $K_{ij}^s = \text{diag}(k_x^s, k_y^s, k_z^s)$ with $k_x^s = k_y^s = k_z^s = 13N/m$. Regarding the damping, we selected $D_v^s = 2\sqrt{m_i^s k_x^s} = 0.13Ns/m$.

For the dense model (7710 nodes) represented in Figure 3.6(b), the number of points is five times larger. The mass of the object is the same for both models, so the mass of each point of the dense one is $m_i^d = m_i^s/5$. For determining the stiffness $K_{ij}^d = \text{diag}(k_x^d, k_y^d, k_z^d)$ of the dense model, external forces are first applied on both models. Indeed, for points that have the same positions in these two models at their rest states, the same displacements of these points are expected when they reach their new equilibrium states. Therefore, the stiffness of the dense model is updated by minimizing the distance between these points using a gradient descent algorithm. The values obtained are $k_x^d = k_y^d = k_z^d = 8N/m$. Finally, for the damping, we selected $D_v^d = 2\sqrt{m_i^d k_x^d} = 0.05Ns/m$.

### 3.2.1.2 2D circular object

The 2D object is represented using 256 points, with the mass of each point being $m_i = 0.35g$. Moreover, the stiffness of the springs connecting these nodes is chosen to be the same as for the 3D object: $K_{ij} = \text{diag}(k_x, k_y)$ with $k_x = k_y = 13N/m$, leading to $D_v = 0.13Ns/m$.

The control law presented in Chapter 3.1.3 is designed for 3D meshes, but it can also be applied to 1D and 2D meshes with minor modifications to account for the different dimensions of the variables in (3.70). For a 3D mesh, $\mathbf{A} \in \mathbb{R}^{3Q \times 3M}$ and $\mathbf{b} \in \mathbb{R}^{3Q}$, while for a 2D mesh, $\mathbf{A} \in \mathbb{R}^{2Q \times 2M}$ and $\mathbf{b} \in \mathbb{R}^{2Q}$.

## 3.2.2 Robustness analysis – 3D Object

This paragraph discusses four different configurations of the simulated 3D object and how they affect the achievement of the deformation task using the PA.

### 3.2.2.1 Dense Model vs. Sparse Model

The first configuration consists in studying the influence of the mesh resolution on the deformation results. This is accomplished by examining both dense and sparse models.
<figure><img src="image_31.png" /><figcaption>Figure 3.6: Representation of the same 3D object with different mesh resolution: (a) and (b) represent respectively the sparse and the dense mesh.</figcaption></figure>

The system is fully actuated, and 3 feature points are indirectly positioned, i.e., $M = Q = 3$. Moreover, the position of the feature and manipulated points are chosen arbitrary, with the same positions being selected for both models. Two different views of the dense object in its initial rest state and at the end of the deformation task are shown in Figure 3.7. The evolution of the error norm $||\mathbf{x}_f - \mathbf{x}_f^*||$ when using the dense and sparse models is illustrated in Figure 3.8.

As it can be noticed from the orange and blue plots, the error converged in both cases, which demonstrates that the PA is robust to the model resolution. We can just note that the convergence is a bit faster using a higher resolution.

### 3.2.2.2 Robustness of the controller against model parameters

This paragraph studies the influence of model uncertainties, which occur when dealing with objects whose true physical properties are unknown. For that, the object is simulated using the dense resolution, while the parameters used in the controller differ from those used to simulate the object. More precisely, we consider the cases when the stiffness is half or one and a half times its real value (green and dark blue plots respectively in Figure 3.9), or the damping used in the controller is half or double of its true value (red and pink plots respectively in Figure 3.9), or even a combination of wrong stiffness and damping values (brown plot in Figure 3.9).

From Figure 3.9, the influence of parameter uncertainties can be outlined as follows: when the stiffness is less than its real value, the controller considers that the object is
<figure><img src="image_32.png" /><figcaption>Figure 3.7: Positioning 3 feature points with 3 manipulated points using a dense model: (a) and (c) show the initial configuration for two different views, while (b) and (d) show the final configuration for the same views. The red, green, purple and cyan points are model points, manipulated points, feature points, and their desired positions respectively. The dashed lines link the cyan points corresponding to the same desired points in the initial and final configurations.</figcaption></figure>
<figure><img src="image_33.png" /><figcaption>Figure 3.8: Evolution of the positioning error of 3 feature points using 3 manipulated points for two different mesh resolutions.</figcaption></figure>

more elastic and therefore larger control velocities are generated, which makes the error
norm converges faster to zero with a small perturbation. This conclusion can be noticed
by comparing the orange, green, and dark blue plots. When the damping is less than its
real value, the controller considers that less friction is exerted on the object and therefore
smaller control velocities are generated, which makes the error norm converges slower to
zero (see orange, red, and purple plots). Moreover, by comparing the orange plot with
the other plots, we can notice a perturbed exponential decrease in the error norm when
coarse parameters were used in the control law, as elaborated in Section 3.1.3.1.

To conclude, the PA is robust to model uncertainties, as evidenced
by the convergence of the system across all the aforementioned test
cases, as long as the coarse stiffness and damping values used in
the controller are not set extremely wrong. It is worth noting that
the chosen range of uncertainties is appropriate. For example, if
the estimated parameters deviate too significantly from their true
values, such as being less than half or more than double the actual
values, the simulated model ceases to accurately approximate the
real object physical behavior.
<figure><img src="image_34.png" /><figcaption>Figure 3.9: Evolution of the positioning error of 3 feature points using 3 manipulated points for testing the PA against wrong model parameters.</figcaption></figure>

### 3.2.3 Actuation study

This paragraph discusses the impact of the number of manipulated points in relation to the number of feature points.

#### 3.2.3.1 Under-actuated case

As already said in Section 3.1.3, when the system is under-actuated, i.e., $M < Q$, it may reach a local minimum. In fact, there exist two possibilities.

The first case is a challenging case in which the desired positions of the feature points are chosen arbitrarily. In this case, it may not be possible to reach the specified configuration using the available manipulated points, or due to the physical properties of the object, and a local minimum may be reached.

The second case consists in teaching by showing the desired position of the feature points during an offline teaching step. In this case, a succession of different motions are applied on the chosen manipulated points and the positions of the feature points are saved once the object reaches a new steady state. These saved positions are then considered as the desired positions. Then the model is reset to its initial state and the control scheme is applied to reach again the desired configuration. A similar strategy has already been employed in [Shetab-Bushehri et al., 2022], where a manual deformation was performed to select the desired positions of the feature points. The idea behind this teaching is to
ensure that the desired configuration of feature points is achievable with the employed
manipulated points.

**Under-actuated study - 2D object** In this experiment, the deformation of a 2D object is attempted using 5 manipulated points distributed around its periphery to indirectly position 9 feature points (see Figure 3.10(a)). The arbitrary selection of the desired 2D positions (cyan points) of the feature points (pink points) leads to the expected convergence to a local minimum, because there are fewer manipulated points than feature points ($M = 9$ while $Q = 5$). This can be shown in Figure 3.10(b) where the actual positions of the feature points do not match their desired positions at the end of the deformation. After updating the desired positions of the feature points to the final positions reached in Figure 3.10(b), the deformation process is repeated once the object is returned to its initial position (see Figure 3.11(a)). In that case, the control scheme is successful as shown in Figure 3.11(b) despite $rank(A) < 3Q$.

<figure><img src="image_35.png" /><figcaption>Figure 3.10: Positioning of 9 points of the planar object using 5 manipulated points: the desired positions of the feature points are chosen arbitrary. The red, green, purple and cyan points are model points, manipulated points, feature points, and their desired positions respectively. The dashed lines connect the same desired points.</figcaption></figure>

**Under-actuated study - 3D object** This experiment aims to deform the 3D object using $M = 2$ and $Q = 3$. The obtained results depicted in Figure 3.12 confirm the outcomes from the earlier 2D case study. The cyan plot indicates that selecting the desired positions of the feature points arbitrarily results in the error norm converging to
<figure><img src="image_36.png" /><figcaption>Figure 3.11: Positioning of 9 points of the planar object using 5 manipulated points when the desired positions of the feature points are reachable. (a)-(b) views, colored points and dashed lines defined as in caption of Figure 3.10.</figcaption></figure>

a nonzero value. However, for the teaching case (learned desired position) represented by
the black plot, the error converges to zero despite the under-actuated situation.

**3.2.3.2 Over-actuated case**

In this section, we discuss the cases when there are more manipulated points than feature
points, i.e $M > Q$.

**Over-actuated study – 2D object** In this part, we consider more manipulated points than feature points. It allows us to highlight the correct management of the coupling in the system, illustrated by $\gamma_{nm_l}$ in (3.65), since each manipulated point influences the position of several feature points.

For the first experiment (see Figure 3.13), 11 points manipulate the object simultane-
ously, and they are uniformly distributed along its periphery. Meanwhile, only 4 feature
points are selected on one side of the object. This selection not only shows that some
manipulated points act on the same feature point, but also highlights the influence of
the manipulated points that are close to the feature points versus those that are farther
away. The success of the deformation task is depicted in Figure 3.13(b), where the feature
points (pink points) have converged to their desired positions (cyan points).

For the second experiment (see Figure 3.14), the main objective is to test the amount
<figure><img src="image_37.png" /><figcaption>Figure 3.12: Evolution of the positioning error of 3 feature points and two sets of manipulated points, one with 2 and the other with 3 points.</figcaption></figure>

of deformation in function of the number of manipulated points. In this experiment, we either manipulate 5 or 9 points to position 5 feature points. As expected, the object experiences more stretching with a lower number of manipulated points, as can be seen by comparing Figures 3.14(b) and (c).

<figure><img src="image_38.png" /><figcaption>Figure 3.13: Positioning of 4 points located on one side of the planar object using 11 manipulated points. (a)-(b) views, colored points and dashed lines defined as in caption of Figure 3.10.</figcaption></figure>
<figure><img src="image_39.png" /><figcaption>Figure 3.14: Positioning of 5 feature points of the planar object : (a) shows the initial configuration, while (b) and (c) show the final configuration of the deformation task when using 5 and 9 manipulated points, respectively. The colored points and dashed lines are defined as in the caption of Figure 3.10.</figcaption></figure>

3.2.3.3 Over-actuated study - 3D object

In this experiment, we consider exactly the same configuration shown in Figure 3.7 apart
2 supplementary manipulated points have been added. For both cases, as shown on Fig-
ure 3.15, the error norm ||*x*<sub>*f*</sub> − *x*<sub>*f*</sub><sup>*</sup>|| converges to zero similarly with a nice exponential
decrease and with the same time-to-convergence. This result was expected since the same
proportional gain λ is used in the control law for both experiments. However, by com-
paring Figures 3.7 and 3.16, it is clear that when 5 manipulated points are employed, the
object exhibits less deformation, which corroborates the results presented in the previous
section.
<figure><img src="image_40.png" /><figcaption>Figure 3.15: Evolution of the positioning error of 3 feature points using PA and two sets of manipulated points, one with 3 and the other with 5 points.</figcaption></figure>

To conclude, the system falls into a local minimum when the num-
ber of manipulated points is less than the number of feature points,
and arbitrary desired positions are chosen. However, simulation
results showed that by teaching the positions of the desired feature
points, which respect the geometry and physical properties of the
object, the feature points can be driven to their desired positions
even when M < Q. Moreover, using more manipulated points to
position the same feature points results in less deformation and a
lower risk of tearing the object.

**3.2.4 Comparison with two state-of-the-art approaches**

To evaluate the proposed approach, we compare its performance with two model-free approaches [Navarro-Alarcon et al., 2013; J. Zhu et al., 2018] that are both based on an iterative numerical estimation of the deformation Jacobian matrix equivalent to **A** in (3.69) while neglecting the feed-forward term **b**.

In what follows, the system is considered fully actuated with *M* = *Q* = 3. Two
configurations are considered.
<figure><img src="image_41.png" /><figcaption>Figure 3.16: Positioning 3 feature points with 5 manipulated points: (a)-(d) views, colored points and dashed lines defined as in caption of Figure 3.7.</figcaption></figure>
### 3.2.4.1 First configuration

It consists in using manipulated points in the neighborhood of the feature points, as presented in Figure 3.17. Then, three cases are tested to compare the performance of the positioning task performed with the aforementioned model-free approaches and the PA.

**Case 1 – Noise free** The first case is an ideal case without measurements noise nor external disturbances. In other meanings, the exact positions of the feature points are fed to the controller. Figure 3.17 shows the configuration of the object in its rest state and after deformation using the PA. As seen in Figure 3.17(b) and (d), the feature points marked in pink are aligned with their desired positions marked in cyan at the end of the deformation. The evolution of the error norm for each approach is illustrated in Figure 3.18. The error norm converges to zero for all the three methods. However, we can note a perfect exponential decrease using the PA that converges faster without any perturbation (red plot).

**Case 2 – Noisy data** The second case differs from the first one by adding a white noise on the positions of the feature points with a maximum magnitude of 0.4 mm on each axis. It corresponds to the tenth of the distance between two neighboring points of the object mesh when it is in its initial rest state. The aim is to simulate measurement noises of the sensor in charge of providing the current positions of the feature points. Figure 3.19 presents the evolution of the error norm for each method. We can notice that the method proposed in [Navarro-Alarcon et al., 2013] (green plot) fails, while the error norm for our PA (red plot) and the method proposed in [J. Zhu et al., 2018] (blue plot) converge to a steady-state error of about 0.4 mm, which corresponds to the sensor noise norm. Nevertheless, the PA has the advantage of exhibiting an optimal exponential decrease of the error norm.

**Case 3 – External perturbations** The third case differs from the first by adding for some duration external perturbations to 3 points on the object. This emulates a potential situation in which the object comes into contact with its surroundings, limiting the motion of certain parts of the object that were not anticipated beforehand. The position of the chosen perturbation points is shown in Figure 3.20. Furthermore, these points are periodically displaced with an amplitude of displacement indicated by the cyan plot in Figure 3.21. We can notice in Figure 3.21 that the error norm converges to zero
<figure><img src="image_42.png" /><figcaption>Figure 3.17: First configuration for comparing the PA and the model-free approaches [Navarro-Alarcon et al., 2013; J. Zhu et al., 2018]: (a)-(d) views and colored points defined as in caption of Figure 3.7.</figcaption></figure>
<figure><img src="image_43.png" /><figcaption>Figure 3.18: Evolution of the error of the positioning task for the case shown in Fig-
ure 3.17(a, b) when using the PA and the model-free approaches [Navarro-Alarcon et al.,
2013; J. Zhu et al., 2018] in ideal conditions</figcaption></figure>

<figure><img src="image_44.png" /><figcaption>Figure 3.19: Evolution of the error of the positioning task for the case shown in Fig-
ure 3.17(a, b) when using the PA and the two model-free approaches [Navarro-Alarcon
et al., 2013; J. Zhu et al., 2018] and when noise is added to the measurements of the
feature point positions.</figcaption></figure>
at $t = 40$ s using the PA and the method proposed in [J. Zhu et al., 2018] (red and blue
plots), that is, soon after the perturbation is no more considered ($t > 30$ s). However,
it converges around $t = 80$ s for the approach proposed in [Navarro-Alarcon et al., 2013]
(green plot). Additionally, during the motion of the perturbation points, that is for
$t \le 30$ s, the error exhibits less oscillations when using the PA compared to the other
approaches. This shows that the PA is less vulnerable to external disturbances.

<figure><img src="image_45.png" /><figcaption>Figure 3.20: Same configuration as in Figure 3.17 showing in yellow the position of the perturbation points.</figcaption></figure>

3.2.4.2 Second configuration

It consists in using the same number of feature points as before, while their positions are
chosen a bit far from the manipulated points (see Figure 3.22). Figure 3.23 presents the
evolution of the error norm for each approach in the case where no measurement noise is
added. We can notice the convergence of the error norm using the PA and the method
proposed in [Navarro-Alarcon et al., 2013], while the method proposed in [J. Zhu et al.,
2018] fails.
<figure><img src="image_46.png" /><figcaption>Figure 3.21: Evolution of the error of the positioning task for the case shown in Figure 3.17(a, b) when using the PA and the model-free approaches [Navarro-Alarcon et al., 2013; J. Zhu et al., 2018] and when external perturbations are applied to the yellow points shown in Figure 3.20. The amount of perturbation is illustrated using a cyan plot.</figcaption></figure>

<figure><img src="image_47.png" /><figcaption>Figure 3.22: Second configuration for comparing the PA and the model-free approaches [Navarro-Alarcon et al., 2013; J. Zhu et al., 2018], which involves choosing manipulated points a bit far from the feature points: (a)-(b) show two distinct views of the object and colored points defined as in caption of Figure 3.7.</figcaption></figure>
<figure><img src="image_48.png" /><figcaption>Figure 3.23: Evolution of the error of the positioning task when using the PA and the model-free approaches [Navarro-Alarcon et al., 2013; J. Zhu et al., 2018] in the case where the feature points are far from the manipulated points.</figcaption></figure>

From this comparison study, we found that the PA improves drastically the robustness to measurement noise and external perturbations. Unlike the approach by [Navarro-Alarcon et al., 2013] which struggled in the presence of measurement noise, both our PA and the method proposed by [J. Zhu et al., 2018] demonstrated greater robustness to sensor noise. Furthermore, our PA exhibited reduced sensitivity to external disturbances on soft objects and quickly regained stability after the disturbance ended.

Moreover, both the PA and the method proposed by [Navarro-Alarcon et al., 2013] succeeded in performing the positioning task even when the manipulated points were far from the feature points.

However, the method proposed by [J. Zhu et al., 2018] required the manipulated points to be close to the feature points, limiting its effectiveness in such scenarios.

### 3.2.5 Effect of the feed-forward term

In this paragraph, we analyze the effect of the feed-forward term *b* involved in the control law (3.70) for the same example of the positioning task shown in Figure 3.17(a,c). We
perform the indirect positioning of the feature points both with and without including
this feed-forward term. The evolution of the task error norm is shown in Figure 3.24.

<figure><img src="image_49.png" /><figcaption>Figure 3.24: Evolution of the error norm of the positioning task shown in Figure 3.17(a,c) with and without the feed-forward term.</figcaption></figure>

As can be seen, without including **b**, the error initially converges but then starts oscillating. This is not the case when **b** is included, as the error converges to zero without any issues. In conclusion, the feed-forward term plays a crucial role in the system convergence, as its absence leads to system oscillation.

**3.3 Conclusion**

In this chapter, we presented a comprehensive pipeline detailing the proposed model-based control approach for driving object feature points to desired locations through the control of multiple manipulated points. This pipeline involves approximating spring forces using a first-order Taylor series within a closed-loop system.

We introduced in Section 3.1 a novel analytic formulation of the visual servoing problem utilizing a mass-spring-model. The aim was to establish a relationship that connects the velocities of feature points with the movements of the control points, which is one of the main contributions of this work. We presented intermediary equations to derive
this analytical formulation. We began by illustrating this process using a simplified 1D
mesh with a single control point influencing object deformation. This concept was then
expanded to encompass multiple control points. Furthermore, we extended the analytical
formulation to a general 3D mesh configuration, albeit with only a single control point.
Drawing upon insights from the 1D case, we generalized the formulation to encompass
multiple control points while adhering to a 3D mesh context.

Then, we provided in Section 3.1.3 simulation results aimed at validating the robustness of the proposed control law with respect to model uncertainties. Additionally, expected behaviors are obtained when the system is fully actuated, under-actuated, and over-actuated. For both the fully actuated and over-actuated cases, the positioning task was achieved without any problem in the tested cases. For the under-actuated case, the positioning error did not necessarily converge to zero when arbitrary desired positions were chosen for the feature points. However, it converged to zero when the desired position were taught during an offline procedure that consists in moving the robot to obtain desired reachable feature point positions.

Thereafter, we compared in Section 3.2.4 the performance of our controller with respect
to two model-free approaches and obtained faster convergence, better stability, and less
sensitivity to sensor noise and external perturbations. It is important to note that our
control law includes an interaction matrix and a feed-forward term, while model-free
control laws solely rely on the interaction matrix. This feed-forward term accounts for
the dynamics of the model and that is why we did not consider the hypothesis of a quasi-
static object, which is the hypothesis held by model-free approaches. To further illustrate
the significance of this feed-forward term for system stability, we presented in Section 3.2.5
a simulation result, in which we compared a positioning task with and without this term.
Notably, when the feed-forward term was not included, the system exhibited oscillations,
whereas its inclusion ensured stable error convergence to zero.
INDIRECT POSITIONING OF MULTIPLE
POINTS ON A SOFT OBJECT

In this chapter, we aim to demonstrate through real experiments the validity of the proposed control law described in the previous chapter. The robotic task is defined as indirectly positioning *Q* feature points on this object by manipulating *M* points on the same object.

Since the proposed approach is model-based, we first need to create a model that approximates the physical behavior of the real object. This procedure is detailed in Section 4.1. We begin in Section 4.1.1 by providing a solution to create the object 3D mesh that describes the geometrical part of the object. Then, the object physical behavior is approximated using a simple MSM, and its parameters are estimated as described in Section 4.1.2. However, the MSM provides an approximation of the real object behavior, which in practice may lead to a drift between the real object and its model. To address this issue, an online realignment of the model is performed by tracking the object surface during the deformation using data provided by an RGB-D camera. The object tracking and model correction will be discussed in Section 4.1.3.

Next, in Section 4.2, we present experimental results to demonstrate the effectiveness of the proposed approach (PA). The approach combines vision-based and model-based control. In the previous chapter, we presented different simulation results showing the effectiveness of the PA whether the system is fully actuated or not. In this chapter, we present experimental results and consider both cases, whether the system is fully actuated or over-actuated.

Furthermore, the displacement of object feature points as a function of the applied mo-
tions on the manipulated points, as given in (3.69), is divided into two elements: **A** and **b**.
As already explained in the previous chapter, the former term is similar to the interaction
matrix in the classical visual servoing framework [Chaumette & Hutchinson, 2008] or the
deformation Jacobian in model-free approaches [Lagneau et al., 2020a; Navarro-Alarcon
et al., 2013; J. Zhu et al., 2018]. The latter term is the feed-forward term, which does not exist in the classical visual servoing problems. Therefore, to study its effect in the developed control law (3.70), we will conduct multiple tests both with and without the feed-forward term to further evaluate the effectiveness of this term.

# 4.1 Deformable object modeling

## 4.1.1 Model 3D mesh generation

In this paragraph, we present the step-by-step process for constructing the 3D mesh of an unknown object placed on a planar support (typically a table). To achieve this, we utilize one RGB-D camera and the open-source Point Cloud Library (PCL) [Rusu & Cousins, 2011]. If multiple cameras are used, the same processing can be applied, but extrinsic calibration between the cameras is required. The PCL is an open-source library that allows us to work with point clouds and perceive the world in a 3D manner. The RGB-D camera is a 3D sensor that captures color and depth images simultaneously at a high frame rate, typically around 30 Hz, resulting in 3D point clouds. In our work, we use the Realsense D435 camera, which acquires high-resolution large-scale scans consisting of several million 3D points. However, these cameras may introduce various measurement noises, necessitating initial filtering to obtain accurate 3D models. We use several filtering methods proposed by the Realsense library core to improve the data, specifically the decimation filter, fast bilateral filter, temporal filter, and holes filling filter. Once the point cloud is filtered, the data collected by the camera includes not only the deformable object of interest but also its surrounding environment. To focus solely on the deformable object, we need to segment the filtered point cloud.

Since the object is positioned on a flat surface, a hole is created in the plane corresponding to the shadow cast by the object on this surface. We first define the planar surface in the scene using PCL. Then, we compute the convex hull of the plane hole thanks to the method described in [Barber et al., 1996]. In our context, the convex hull represents the smallest convex shape that envelops the boundary of the area where the shadow is cast on the flat surface. Next, we project the points of the point cloud onto the plane, and finally, we determine which projected points lie inside the area enclosed by the convex hull. These points belong to the object sitting on the flat surface.

Given now the set of point cloud corresponding to the segmented object, our objective
is to estimate the object surface by connecting points with each other, ending up with
a continuous polygon mesh, such as triangles in our case. The surface triangulation is
performed locally by projecting the local neighborhood of a point along the point normal
and connecting unconnected points. To compute normals directly from the point cloud,
we employ an approximation algorithm called Normalestimation from [Rusu & Cousins,
2011]. This algorithm estimates a point normal from its surrounding points, and we can
adjust the level of detail by setting the value of k in the k-neighborhood concept introduced
by PCL. However, selecting the appropriate k value is crucial; a too small value might fail
to represent a local normal, while a too large value could lead to inefficient computations.
In practice, we have used k = 75 for all objects considered.

Once the normal estimation is completed, we combine the estimated normals with the object surface coordinates for each point. With these estimated features, we can proceed with the surface reconstruction using the Poisson surface reconstruction method from [Kazhdan et al., 2006]. It is essential to note that our work does not pursue highly accurate reconstruction but rather, to guide a servoing task effectively. The program is written in C++ Builder and utilizes the OpenGL library for 3D display. An example of the constructed mesh is illustrated in Figure 4.1. As can be seen from the reconstructed mesh, the method used is robust against illumination variations.

4.1.2 Model physics parameters estimation

As discussed in Section 2.4, a common approach to estimate the rheological parameters
of soft objects involves the following methodology: external forces are applied to the soft
object using a robotic manipulator equipped with a force sensor. Then, the same forces
are replicated on the object model. Consequently, the model parameters are updated until
the deformation observed on the surface of the modeled object aligns with the surface of
the real object captured using the camera. We will use the same methodology while
employing the MSM.

We recall that the main objective of this work is not to create a perfect model but
a simple model sufficient for achieving the shape servoing task. For that purpose, we
consider a simple homogeneous MSM.

Regarding the parameters of the MSM used, some of them are imposed by the recon-
structed geometrical object mesh: the initial point coordinates, **x**<sub>i(t=0)</sub>, and the length of
springs connecting any two neighboring points, l<sub>ij</sub><sup>0</sup>, ∀i ∈ N, j ∈ **v**<sub>i</sub>, at rest state. Since
the object is supposed to be homogeneous, as stated before, the mass of the object is
<figure><img src="image_50.png" /><figcaption>Figure 4.1: An example of mesh reconstruction: (a) corresponds to an ellipsoid ball image obtained from the Realsense D435 camera. (b) represents two views of the constructed mesh of the segmented object.</figcaption></figure>

equally distributed among all points, i.e., $m_i = m_1, \forall i \in \mathcal{N}$.

### 4.1.2.1 Stiffness matrix estimation

Concerning the spring stiffness $K_{ij}$, it depends on three constant values $K_{ij} = \text{diag}(k_x, k_y, k_z)$, while $k_{max}$ represents the maximum value in $K_{ij}$. This is evident from the work proposed in [Lloyd et al., 2007], which derived an analytical expression for the stiffness of every tetrahedron constituting a mesh and representing it using a MSM. The stiffness is proportional to the volume of the tetrahedron and the Young's modulus of the material properties, which is considered constant according to our hypothesis. Furthermore, during the object deformation, the volume of the tetrahedra constituting the mesh changes, resulting in changes in the stiffnesses of the springs. Based on [Duan et al., 2014], the biggest stiffness corresponding to the largest volume that could appear is chosen to be the same for all springs to prevent collisions between mesh points. At the end, the stiffness matrix is considered constant for the homogeneous object.

We estimate the stiffness matrix by updating it through a gradient descent-based error minimization approach. Our proposed method allows for estimating the stiffness
matrix using either a robotic manipulator equipped with a force sensor or relying solely on vision. In the force-based method, an arbitrary sequence of forces is applied to the real object, and the force sensed by the force sensor is then applied to the object model at the same points where these forces were exerted. Note that these points are not included in the participation of the stiffness matrix update. In the vision-based method, we apply an arbitrary deformation on the real object. Then, some points belonging to the model surface are utilized to induce deformations in the object model. The proportion of these points, in relation to the complete set of surface points, will be analyzed later. Subsequently, these points are moved to be aligned with their corresponding points from the deformation observed on the real object. As for the force-based method, these model points, once aligned, do not participate in updating the stiffness matrix, leaving the remaining points responsible for this update.

Let us denote the set of model surface points as follows: $\mathbf{x}_o = [\mathbf{x}_{o_1}^T, ..., \mathbf{x}_{o_w}^T]^T$, $\forall 1 \le i \le w$, $o_i \in \mathcal{N}$ where $w$ is the number of the mesh surface points and $o_i$ is the indices of their corresponding points in $\mathcal{N}$. The segmented surface object using the RGB-D camera is denoted by $\mathbf{p}^o$. For every point $\mathbf{x}_{o_i} \in \mathbf{x}_o$, $\forall 1 \le i \le w$, $o_i \in \mathcal{N}$, we denote by $\hat{\mathbf{p}}_{o_i} \in \mathbf{p}^o$ its correspondent point on the point cloud of the real object. In order to get this correspondence, we apply the same matching technique as in [Petit, Lippiello, Fontanelli, et al., 2017]. Two sets of nearest neighbor correspondences from $\mathbf{x}_o$ to $\mathbf{p}^o$ and from $\mathbf{p}^o$ to $\mathbf{x}_o$ are determined. Finally, the correspondence $\hat{\mathbf{p}}_{o_i}$ is obtained as a trade-off between these last two correspondences. The discrepancy between the positions of the model points and their actual deformation from the segmented object is employed to update the stiffness matrix. We start by calculating $\mathcal{E}$, the average distance between the two point clouds, $\mathbf{x}_o$ and $\mathbf{p}^o$:

$$ \mathcal{E} = \frac{1}{2w} \sum_{i=1}^{w} \|\mathbf{x}_{o_i} - \hat{\mathbf{p}}^{o_i}\|^2, \qquad (4.1) $$

In practice, the stiffness matrix is well estimated when the error $\mathcal{E}$ is minimized to zero. As a result, the stiffness matrix is adjusted to minimize this error by computing the gradient of the error. We analytically determine this gradient $\nabla \mathcal{E} = \frac{\partial \mathcal{E}}{\partial \mathbf{K}_{ij}}$ of the error function (4.1):

$$ \nabla \mathcal{E} = \frac{1}{w} \sum_{i=1}^{w} \left[ \frac{dt^2}{m_i} \sum_{j \in \nu_{o_i}} (\alpha_{o_{ij}}(\mathbf{x}_j - \mathbf{x}_{o_i})) (\hat{\mathbf{p}}^{o_i} - \bar{\mathbf{x}}_{o_i}) \right] \qquad (4.2) $$
with

$$ \bar{\mathbf{x}}_{o_i} = \mathbf{x}_{o_i} + \frac{dt^2}{m_{o_i}} \sum_{j \in \nu_{o_i}} \alpha_{o_{ij}} \mathbf{K}_{ij} (\mathbf{x}_j - \mathbf{x}_{o_i}) + (dt - \frac{dt^2}{m_{o_i}} D_v) \dot{\mathbf{x}}_{o_i} $$

Finally, the iterative update of the stiffness matrix is obtained as follows:

$$ \mathbf{K}_{ij} = \mathbf{K}_{ij} - 0.5 \mathbf{I}_{3 \times 3} \nabla \mathcal{E} \qquad (4.3) $$

with 0.5 in (4.3) denotes the learning rate, and $I_{3 \times 3}$ the 3 × 3 identity matrix. We can notice that this calculation does not require a high computation cost, and when $\mathbf{x}_o$ and $\mathbf{p}^o$ are aligned, the last term in (4.2) will be zero, resulting in a gradient of zero, which ends the update of the stiffness matrix.

Let us note that the simulation operates at a significantly higher frequency than the camera frame rate. As a result, $\nabla \mathcal{E}$ in (4.2) can be calculated multiple times before a new deformation is applied to the object. Consequently, $\mathbf{K}_{ij}$ is updated several times for each applied deformation with (4.3). For instance, when employing a typical mesh with around 5k points, $\mathbf{K}_{ij}$ undergoes updates between 5 to 10 times for each applied deformation.

**Vision-based stiffness estimation** In this paragraph, we want to validate the stiffness estimation of a real soft object subjected to deformations from a robotic manipulator, using the approach proposed above. Moreover, the employed robot is not equipped with force sensor, therefore two distinct categories of points emerge in the stiffness matrix estimation. The first category involves some model surface points that are constrained to be aligned with their corresponding positions on the segmented object surface. The second category involves model points that are free to move according to the MSM simulation, which are used to update the stiffness matrix. This involves comparing their positions obtained using the MSM kinematic model (3.3) with its actual stiffness matrix, and their corresponding points on the segmented object surface.

The selection of points within the first category is not arbitrary; it has to be carefully chosen to accurately reflect the deformation characteristics of the object. For example, if deformation primarily occurs on one side of the object, exclusively selecting points belonging to the first category from that side could impede the stiffness matrix update. This is due to the other side representing the static portion of the object, resulting in $\mathcal{E} = 0$ in (4.1). Therefore, it is necessary to strategically select points that encompass a balanced representation of both deforming and non-deforming regions. This meticulous selection process ensures that model refinement aligns faithfully with the real physical be-
behavior of the object, consequently fostering precise deformations and accurate mechanical responses.

In our case, the points in both categories are uniformly spread across the object surface. Furthermore, we explore four distinct distributions of these points denoted as $y_2, y_3, y_5$, and $y_{10}$. In the context of the first distribution, $y_2$, for any two points used in the first category, there exists one point that belongs to the second category. Employing a similar approach, we investigate the remaining distributions. The primary objective of these diverse distributions is to assess their impact on the stiffness update process.

In the subsequent results, we present only $K_x$, the first component of the stiffness matrix, since an identical process is replicated for the remaining elements. We initiate the procedure by using two arbitrary stiffness values: $K_x = 30 \ N.m^{-1}$ (see Figure 4.2) and $K_x = 1 \ N.m^{-1}$ (see Figure 4.3). The purpose of testing these values is to assess whether the system might become trapped in a local minimum when the initial value is significantly far from the optimal value. Additionally, we aim to determine the number of steps the system takes to converge to a range within the vicinity of the optimal values. It is important to acknowledge that there is not a singular optimal value because the object undergoes a series of deformations. With each deformation, a distinct stiffness value is obtained. For a small deformation, the stiffness required to replicate the physical behavior eventually differs from the stiffness needed to simulate large deformations. Consequently, our objective is to identify a value that proves effective for both minor and substantial deformations.

Subsequently, all deformations are applied to the object, triggering the launch of the stiffness estimation process. The progression of $K_x$ values at each step is illustrated in Figure 4.2 and Figure 4.3. Each step corresponds to a deformation, gradually incrementing until the final step (320), which corresponds to the most significant deformation. Upon completion, indicated by the vertical dashed line in the mentioned figures, the object is returned to its initial state, and the same deformations are applied once more. However, this time, the last estimated stiffness value is adopted as the initial stiffness for the model. Ultimately, the selected stiffness value is determined based on the results obtained in the second configuration (steps > 320).

We notice from Figure 4.2 that in just a few iterations, the stiffness value decreases rapidly, covering a range of values between 1.5 and 4.0 across the different tested distributions. This result is crucial, particularly when starting with an arbitrary initial value, the process quickly reaches a point of agreement. This is important when dealing with
unknown objects because setting an initial guess of the stiffness value would be difficult.

In addition, both Figure 4.2 and Figure 4.3 highlight that the range of estimated values for $K_x$ remains relatively narrow across different deformations (steps) for $y_2$ and steps > 320. This is because the object behavior is heavily influenced by visual constraints, rather than faithfully representing its physical response. On the other hand, with other distributions, fluctuations in stiffness are observed depending on the applied deformations. Notably, the choice of distributions does not exert a significant influence on stiffness estimation, given that a sufficient number of freely movable points are updated through the MSM. However, it is important to note that this was not the case for the distribution $y_2$. Leveraging these results, the highest stiffness value attained is selected, yielding $K_x = 3.0 \ N.m^{-1}$.

<figure><img src="image_51.png" /><figcaption>Figure 4.2: Evolution of stiffness updates as a function of applied deformations at each step for each distribution ($y_2$, $y_3$, $y_5$, and $y_{10}$), starting from an arbitrary initial value of $K_x = 30 \ N.m^{-1}$.</figcaption></figure>

### 4.1.2.2 Damping estimation

Regarding the damping value $D_v$, it is calculated as presented in [Bhasin & Liu, 2006] in order to guarantee the numerical stability of the system: For that, we have to ensure
<figure><img src="image_52.png" /><figcaption>Figure 4.3: Evolution of stiffness updates as a function of applied deformations at each step for each distribution ($y_2, y_3, y_5$, and $y_{10}$), starting from an arbitrary initial value of $K_x = 1 \text{ N.m}^{-1}$.</figcaption></figure>
$2\sqrt{m_1k_{max}} \le D_v \le \frac{\|\dot{\mathbf{x}}_i^{m_1} + \mathbf{f}_{s_i} + \mathbf{f}_{D_i}\|}{\|\dot{\mathbf{x}}_i\|}$ for $\|\dot{\mathbf{x}}_i\| \ne 0$. When $\|\dot{\mathbf{x}}_i\| = 0$, $D_v = 0$ since the damping has no effect on a static point. In this work, we choose:

$$D_v = 2\sqrt{m_1k_{max}} \qquad (4.4)$$

### 4.1.3 Model correction by tracking

The object model is updated using (3.3), taking into account the spring forces $\mathbf{f}_s$ given in (3.2), which are influenced by the model parameters. However, the MSM is an approximation of the real behavior of the object. Consequently, the gap between the object model deformation and its real one will be aggregated during the object manipulation, which could lead to an unrealistic object representation. To address this issue and prevent the model from drifting from reality, a continuous tracking of the object during deformation is employed using the RGB-D camera. Whenever a deviation between the real object and its model is detected, the model is adjusted each time the camera captures a new image. To make this adjustment, we propose a modified versions of (3.1) and (3.3) already presented in 2.11:

$$m_i \ddot{\mathbf{x}}_i = \mathbf{f}_{s_i} + \mathbf{f}_{D_i} + \mathbf{f}_{c_i} \qquad (4.5)$$

$$\mathbf{x}_{i(t+dt)} = \mathbf{x}_{i(t)} + \left( dt - \frac{dt^2}{m_i} D_v \right) \dot{\mathbf{x}}_{i(t)} + \frac{dt^2}{m_i} \mathbf{f}_{s_i(t)} + \frac{dt^2}{m_i} \mathbf{f}_{c_i(t)} \qquad (4.6)$$

where $\mathbf{f}_{c_i}$ is a 3D force vector representing additional external constraints, which is applied on point $P_i$ of the mesh each time a drift between the model and the real object is detected at that point.

To determine the constraint forces $\mathbf{f}_c$, the object is first segmented in each camera frame. Hence, by tracking $\mathbf{x}_o$ and $\mathbf{p}^o$, the drift can be detected. It appears when these two point clouds are no more superimposed. To compensate the drift, the model is adjusted by rigidly aligning $\mathbf{x}_o$ on $\mathbf{p}^o$, using an iterative closest point (ICP) procedure. As a result, $\mathbf{p}^o$ correspond to some points of $\mathbf{x}_o$ and they serve as the input displacements to $\mathbf{x}_o$. In [Petit, Lippiello, Fontanelli, et al., 2017], Petit et al. considered these displacements as external forces exerted by $\mathbf{p}^o$ on $\mathbf{x}_o$ and then integrated them to the mechanical model using co-rotational FEM. Similarly, using the same procedure but employing the MSM, we apply the external constraint forces $\mathbf{f}_c$ as follows:

$$\mathbf{f}_{c_o_i} = K_{ij}(\hat{\mathbf{p}}^{o_i} - \mathbf{x}_{o_i}) \qquad (4.7)$$
For the remaining mesh points not belonging to the surface, no external constraint is applied to them, i.e., $f_e = 0$. However, the effect of the force applied on the outer points of the model is propagated to the inner points with (3.3). As a result, the correction phase leads the volumetric model to approach the real object deformation behavior.

Since this correction is performed at a frequency of 30 Hz (camera frame rate), while the model is updated with a higher frequency (typically around 600 Hz), the external constraints are considered constant between two successive images.

### 4.1.3.1 Soft object tracking

In this paragraph, we present the tracking results achieved through the use of 4 RGB-D cameras, a soft object, and a robotic manipulator deforming the object (see Figure 4.4). Initially, the RGB-D cameras detect the object and construct its geometrical mesh, using the method elaborated in Section 4.1.1. Subsequently, the model parameters are estimated using the methodology outlined in Section 4.1.2. Once the object model is approximated using the MSM, the tracking procedure is initiated.

The objective of this experiment is to track large deformations using exclusively RGB-D data and the MSM. To facilitate deformation, a rigid ball acting as a contact tool is attached to the manipulator end-effector. However, the presence of this ball creates significant occlusion, making it impossible to track a significant part of the object surface. To address this occlusion issue, a model of the rigid ball is created, and its points are incorporated into the segmented object surface points for each camera frame. This step is straightforward, as the ball points follow the rigid movements of the robot end-effector. It is important to clarify that the displacements applied to the end-effectors do not directly impact the tracking process. Instead, these displacements are just employed to update the positions of the ball points. It is worth noting that if minor occlusion arises from the end-effector, this step is no more necessary. This holds true for the experimental results that will be presented in Section 4.2, where small sticks are attached to the robot end-effectors.

Subsequently, using correspondences between the adjusted segmented point cloud derived from the camera and the model surface points, constraint forces are computed using (4.7), and the model is iteratively refined using (4.6). To demonstrate the efficiency of the tracking procedure, captured images of the deformed object from the viewpoint of one camera are showcased in the first and third rows of Figure 4.4. Beneath each image, the corresponding result of the deformed mesh is depicted. The evolution of the mesh dur-
ing the deformation is shown in the second and fourth rows of Figure 4.4. Furthermore,
for enhanced visualization of the alignment between the deformed mesh and the actual
object, the mesh is also projected into the previously mentioned images. The black con-
nected points on the soft object delineate the projected mesh. As evident from Figure 4.4,
the deformed mesh accurately overlays the actual deformed object throughout the entire
deformation sequence. This success underlies the effectiveness of the tracking procedure,
which enables the simple MSM to faithfully reproduce the real-world deformations of the
object.

Despite the simplicity of the selected model parameters, which al-
lows high-speed real-time capacities, we will see that it is possible to
consider large deformations on real objects thanks to the proposed
closed-loop strategy, both in terms of control and object-model reg-
istration.

4.2 Experiments

In this section, we demonstrate the validity of the PA in real experiments involving two robotic manipulators and a RGB-D camera. We present the results of various tasks involving the indirect positioning of points on different soft objects, as well as a comparison between the PA and a model-free approach [Wada et al., 2001a].

4.2.1 Experimental setup

Our experimental setup is shown in Figure 4.5. It consists of two 6-dof robotic arms from ADEPT, a Viper 850 and a Viper 650. Each robot is equipped with a rigid stick that serves as a tool. The stick attached to the Viper 850 is dedicated solely to the deformation process throughout the experiments. On the other hand, the stick attached to the Viper 650 serves a dual purpose depending on the specified task. It prevents the object from rigidly sliding on the table when the Viper 850 is the sole robot deforming the object, and contributes in the deformation process when both robots are involved. These sticks are in contact with the object at t = 0 and their contact points are used as the manipulated points.

The visual perception of the object is performed with a remote RGB-D Intel Realsense-D435 camera. It acquires around 3.10⁵ 3D points at 30 frames per second. Prior to starting
<figure><img src="image_53.png" /><figcaption>Figure 4.4: An example of the tracking results: The first and third rows show the RGB image with the projection of the object mesh (black lines) onto the object. The second and fourth rows show the deformed mesh.</figcaption></figure>
<figure><img src="image_54.png" /><figcaption>Figure 4.5: 1: Viper 650. 2: Viper 850. 3: RGB-D camera. 4: Soft objects. 5: Rigid sticks attached to Viper 850 and Viper 650.</figcaption></figure>

the deformation task, the camera is utilized to create a model for each soft object used,
as described in Section 4.1.1. Two types of soft objects have been considered: volumetric
and planar objects, and their model parameters are determined following the procedure
described in Section 4.1.2. Then, an eye-to-hand calibration is performed as the velocities
generated to accomplish the deformation task are calculated in the camera frame. Since
the base frame of each robot and the camera position are fixed, this calibration process
only needs to be executed once, regardless of the specific task requirements. After ob-
taining the transformations between the camera and the robots, the velocities computed
by the control scheme in the camera frame are converted and applied to the end-effectors
of the robots to apply the deformations.

Moreover, the camera is also utilized to measure the position of the features points that
are used as input of the control scheme, and to correct any drift between the simulated
deformation using the MSM and the actual deformation of the soft object. The drift occurs
when the model surface points and the real object surface points are not superimposed.
In this case, external constraints are applied to the model surface points to rectify it,
following the methodology explained in Section 4.1.3.
4.2.2 Experimental results

For the experimental tests, we consider either one feature point or two feature points to be positioned at some desired locations. When one feature point is considered, either one or both robots deform the soft objects simultaneously. However, when two feature points are considered, both robots are engaged. The selection of the desired position for each feature point must be carefully chosen. In other words, the desired position for each feature point should be attainable from the initial configuration of the robot(s) without damaging the object. Therefore, for each following experiment, once one or both robots come into contact with the object, we apply arbitrary movements to the robot(s), and the resulting position of each feature point is considered to be the desired position. We then repeat the experiment when the object is not deformed, aiming to achieve the same attained position. By doing so, we can ensure that the desired task is achievable without damaging the object.

As for the simulation study, the performance of the positioning task is evaluated from
the evolution of the error between the current and desired positions of the feature points.
In practice, this error is measured directly using the RGB-D camera by rigidly attaching
one marker per feature point. In several figures of this section, colored dots will represent
the projection of the desired 3D positions of the feature points in the RGB image.

The effectiveness of the servoing task for various experiments manipulating soft objects is outlined in the subsequent sections. The positioning task is considered successful when the markers and the colored dots are aligned, indicating that the feature points have reached their targeted positions.

To be noted that the depth measurements obtained from the RGB-D camera are
subject to sensor noise in a range of 1 mm. Therefore, in what follows, the conver-
gence is considered to be reached when the error norm for each feature point is less than
1 mm. Some experimental results are available on these online videos [ICARCV-video]
and [Indirect-positioning-video].

4.2.2.1 One feature point

An example of the indirect positioning of a 3D feature point on a 3D object is presented in Figure 4.6. The distance between the manipulated point and the feature point is chosen arbitrary. The object is restricted on one of its sides, using the Viper 650, to prevent its free 3D translational movement. A visual marker is rigidly attached to the object at
the location of the feature point so that it is easily tracked. This marker is also used to facilitate the validation of the transportation of the feature point to the desired position. The blue dot indicates the projection of the desired 3D position of the feature point. Figure 4.6.(a) and Figure 4.6.(b) depict respectively the initial and final states for the positioning task using our proposed approach (PA). The red, green, blue and black lines in Figure 4.7 represent respectively the evolution of the positioning error along (x, y, z) axes and the error norm when using the PA. The proposed controller drives the error norm within 1 mm, between the dashed magenta lines, with an initial error around 45 mm. We can notice the nice exponential decrease of this error, which indicates the success of the positioning task and the correct modeling of the system.

<figure><img src="image_55.png" /><figcaption>Figure 4.6: Indirect positioning of one feature point on a 3D object: (a) 3D soft object at its rest state. (b) 3D soft object after the deformation process. The blue dot represents the projection in the image of the 3D desired position of the feature point represented by a white marker.</figcaption></figure>

Another example of the indirect positioning of a 2D feature point on a planar object is represented in Figure 4.8. The object is modeled using a 2D representation, neglecting its thickness. The same methodology as the previous experiment is applied, where the Viper 650 robot restricts free 3D translational movement. The blue dot indicates the projection in the camera frame of the desired 2D position of the feature point. The object before and after deformation is depicted in Figure 4.8(a) and Figure 4.8(b), respectively. Moreover, the evolution of the positioning error during the deformation is illustrated in Figure 4.9, showing an exponential decrease. Furthermore, the error at the end of the deformation is within the range of 1 mm, which validates the success of the positioning task.

**Comparison to a simple PID**
One commonly employed approach for indirectly positioning a feature point was proposed in [Shibata & Hirai, 2006; Wada et al., 2001a],
<figure><img src="image_56.png" /><figcaption>Figure 4.7: Evolution of the error for the feature point along the three axes (x, y, z) and the error norm of the positioning task represented in Figure 4.6.</figcaption></figure>

<figure><img src="image_57.png" /><figcaption>Figure 4.8: Indirect positioning of one feature point on a planar object: (a) 2D soft object at its rest state. (b) 2D soft object after the deformation process. The blue dot represents the projection in the image of the 2D desired position of the feature point represented by a white marker.</figcaption></figure>
<figure><img src="image_58.png" /><figcaption>Figure 4.9: Evolution of the error for the feature point along the three axes (x, y, z) and the error norm of the positioning task represented in Figure 4.8.</figcaption></figure>

where the authors suggested the use of a simple PID controller to accomplish the task. They define the control law as follows:

$$ \dot{\mathbf{x}}_m = -G_p \mathbf{e} + G_d \dot{\mathbf{e}} + G_i \int \mathbf{e} \qquad (4.8) $$

with $G_p$, $G_d$ and $G_i$ being proportional, derivative and integral gains respectively. However, while their approach is deemed valid for small deformations, they did not explicitly define what constitutes a small deformation. In our case, we call small deformation (S) when the desired position of the feature point is far from its initial position by a distance less than 10% of the largest dimension of the object, i.e., height, width and thickness, and large deformation (L) when this distance is between 12% to 30% of the latter. In addition to comparing our PA with the PID controller for both small and large deformations, we perform different tests where the positions of the manipulated point and the feature one are close (N) and far apart (F). In what follows, we use some notations, such as S-N to represent small deformations and the case where the manipulated point is near to the feature point. Finally, both the proposed controller and the PID controller are tested and compared for these different types of deformations.
The main problem when using a simple PID controller is the fine-tuning of the gains of the controller for each task to be accomplished. For a small proportional gain and depending on the soft object, we noticed that the error either converges towards zero, or diverges, or converges towards a local minimum. For a large proportional gain, the error either diverges, or oscillates around a local minimum, and sometimes the object is destroyed. Therefore, we have adjusted the gains of the PID by a trial and error method. Unlike this controller, our PA does not need any parameter setting except the approximate physical model parameters and the control proportional gain, which has been fixed to $\lambda = 0.7$.

Additionally, since the robot used is equipped with a force sensor, the applied forces are measured during the deformation. The comparison results are summarized in Table 4.1. The table shows that when the manipulated point is near the feature point, the proposed method and the PID converge similarly without any problem. For the other cases, our method guarantees that the error norm converges to less than 1 mm, while it is not the case for the PID controller. The table summarizes the statistics of the different tests carried out, since many experiments have been done for each case. For example, for S-N case, many manipulated positions are chosen around the feature point within the same distance. An example of the initial state for the near case is presented in Figure 4.10. In addition, the measured forces clearly show that the robot applies less force to the object using the PA, which could be sometimes half and sometimes quarter of the applied force using the PID controller. The importance of this characteristic is that when working with some elastic objects, a high force can cause the object to lose its elasticity.

<figure><img src="image_59.png" /><figcaption>Figure 4.10: (a), (b) represent respectively the initial position of the feature point and its desired position for the S-N and L-N cases. The figures also present the initial absolute error in terms of pixels.</figcaption></figure>

Moreover, an example of L-F case is represented in Figure 4.11. The distance between the manipulated point and the feature point is around 75 mm. The yellow rectangle
<table><caption>Table 4.1: PA versus a simple PID for different experiments with different initial errors, in terms of final average error $||\bar{e}||$ and the maximal norm of the measured force $Max ||F||$ (N).</caption><thead><tr><th rowspan="2">Initial error (mm)</th><th colspan="2">Proposed</th><th colspan="2">Simple PID</th></tr><tr><th>||<span style="text-decoration: overline;">e</span>||</th><th>Max ||<span style="text-decoration: overline;">F</span>||<sub>2</sub></th><th>||<span style="text-decoration: overline;">e</span>||</th><th>Max ||<span style="text-decoration: overline;">F</span>||<sub>2</sub></th></tr></thead><tbody><tr><td>S-N<br>(15,10,-5)</td><td>0.7</td><td>2</td><td>0.7</td><td>4</td></tr><tr><td>S-F<br>(10,10,0)</td><td>0.9</td><td>5</td><td>Diverged</td><td>&gt; 33</td></tr><tr><td>L-N<br>(35,-30,10)</td><td>0.9</td><td>10</td><td>0.9</td><td>12.25</td></tr><tr><td>L-F<br>(35,-30,10)</td><td>1.1</td><td>13</td><td>Diverged</td><td>&gt; 30</td></tr></tbody></table>

presents the restricted side of the object, in order to avoid the object free 3D translation. Figure 4.11.(a) and Figure 4.11.(b) depict respectively the initial and final state for the positioning task example using our proposed approach (PA). The red, green, blue and black solid lines in Figure 4.12 represent respectively the evolution of the positioning error along (x, y, z) axes and the error norm when using the PA. The proposed controller drives the error norm within 1 mm, with an initial error around 55 mm. On the other side, the positioning error when using the simple PID controller, illustrated by dashed lines in Figure 4.12, initially decreased and then diverged.

**Over-actuated case with external perturbations** In this experiment, two robots are utilized to deform one feature point while external disturbances are applied to the soft object. The aim is to evaluate both the robustness of the PA against external perturbations and the positioning of the feature point when two manipulated points deform the object, rather than just one, leading to an over-actuated case (Q=1 and M=2).

The case where the feature point is in the vicinity of its desired position is depicted in Figure 4.13. The object in its rest state is represented in Figure 4.13(a). Subsequently, the deformation process is initiated without external perturbations, except for the presence of sensor noise, and the state of the object at the end of the deformation is shown in Figure 4.13(b). Following this, a third stick is employed to manually apply a small deformation to the object in order to add a perturbation, resulting in the feature point moving from its desired position achieved previously (see Figure 4.13(c)). The system
<figure><img src="image_60.png" /><figcaption>Figure 4.11: An example of L-F indirect positioning task: (a) Soft object at its rest state. (b) Soft object after the deformation process.</figcaption></figure>

<figure><img src="image_61.png" /><figcaption>Figure 4.12: Evolution of the error for the feature point along the three axes ($x, y, z$) and the error norm of the positioning task represented in Figure 4.8. Solid lines represent the positioning error when using the PA, while dashed lines represent the positioning error when employing a PID controller.</figcaption></figure>
is then allowed to re-achieve the positioning task, which is successfully accomplished as evidenced in Figure 4.13(d). In a second attempt, the same stick is used to apply a larger perturbation, as shown in Figure 4.13(e), and the same feature point is again driven to its desired position, as demonstrated in Figure 4.13(f).

Finally, the evolution of the positioning error along the three axes (x, y, z) and the overall error norm of the positioning task are represented in Figure 4.14. It illustrates how the positioning error decreases exponentially to within 2 mm despite the perturbations occurring at t = 1.0 s and t = 1.5 s.

**Robustness of the controller against model parameters**

Additional experiments are illustrated in Figure 4.15 to test the robustness of the PA against model parameter errors. The variation in error norms is presented for a L-F case, using five different controller configurations as depicted in Figure 4.15.

The error converges nicely with an exponential decrease when our PA and the identified model parameters are used (see blue graph). In contrast, the red graph shows the evolution of the error norm when the PID controller is used. We can see that the error oscillates around a local-minimum then a divergence occurs, as expected from Table 4.1.

In the third configuration, the parameters are modified: stiffness is doubled, and damping is halved. This configuration is represented by the green plot in Figure 4.15. We can observe that the error norm converges to zero but with some perturbations during the exponential decrease. This is explained by the perturbations introduced in the control scheme through $\hat{A}$. However, when the real stiffness is multiplied by 6, the system fails in a local minimum, as indicated by the black graph. Once again, this can be explained from our discussion in Section 3.1.3.1. Since the model converges, it ensures that the condition (3.72) is met. However, due to the use of very coarse parameters, the non-negligible term $(b - A\hat{A}^+\hat{b})$ causes the error to converge to $\bar{e} \neq 0$, and its norm converges to $|\bar{e}| \neq 0$. In conclusion, our controller demonstrates robustness to a considerable range of coarse model parameter variations.

Lastly, the cyan graph in Figure 4.15 presents the error norm when our PA is used with a reduced mesh resolution for the object model. In this configuration, the object mesh contains 2059 nodes, as opposed to the high mesh resolution of 7103 nodes used in previous experiments. We can notice that such a reduction induces perturbations in the system behavior, but no divergence occurs, and the error still converges to a value less than 2 mm.
<figure><img src="image_62.png" /><figcaption>Figure 4.13: Indirect positioning of one feature point of a semi-spherical ball using two robots: (a) and (b) depict the initial and final configurations of the positioning task, respectively. (c) and (e) illustrate intermediate states of the object when it is subjected to small and large external perturbations. (d) and (f) show respectively the final states of the object once the external disturbances have been removed.</figcaption></figure>
<figure><img src="image_63.png" /><figcaption>Figure 4.14: Evolution of the error for the feature point along the three axes (x, y, z) and the error norm of the positioning task represented in Figure 4.13 when external perturbations are applied to the object.</figcaption></figure>

<figure><img src="image_64.png" /><figcaption>Figure 4.15: Evolution of the positioning task error norm for a L-F case (illustrated on the right of the figure) and for five different controller configurations.</figcaption></figure>
4.2.2.2 Two feature points

In this section, our aim is to demonstrate the validation of the proposed control law when two feature points fixed on a soft object need to be driven to some desired positions while manipulating two robotic manipulators simultaneously. Furthermore, as shown in the previous section, the feed-forward term **b** distinguishes our proposed control law from the relevant model-free approaches. Hence, we also assess the significance of this term in our proposed approach. Subsequently, two different configurations are discussed in the following experiments.

In the first configuration, two robots are employed to indirectly position two feature points on distinct soft objects in a fully actuated scenario. The purpose of this configuration is to validate the stability and efficiency of the PA for various objects with different geometries and physical properties.

In the second configuration, we focus on the examination of the impact of the feed-forward term **b** in (3.70).

**First configuration - Fully actuated Case** In this experiment, four objects with different geometries and physical parameters are considered with a fully actuated system ($M = Q = 2$).

The first object (semi-spherical ball) with two different initial placements of the manipulated points and feature point positions are presented in Figure 4.16(a) and (c). The final state of the semi-spherical ball are displayed in Figure 4.16(b) and (d) respectively. Two thumbtacks are attached to the object at the selected feature points, enabling their easy tracking through the RGB-D camera. Furthermore, in Figure 4.16, the cyan and blue dots represent the projection of the desired 3D positions of these feature points. As can be seen in Figure 4.16(b) and (d), the thumbtacks are aligned with the cyan and blue dots, indicating the successful completion of the positioning task. Furthermore, Figures 4.17 and 4.18 show the evolution of the positioning error for the tasks illustrated in Figure 4.16(a-b) and Figure 4.16(c-d) respectively. Both cases demonstrate an exponential decrease of the error norm to within 2 mm in less than 4 seconds, validating the successful positioning task.

To test the efficiency of the model, three other objects are considered: a semi-ellipsoid ball that differs from the first object in terms of geometry (see Figure 4.19), another semi-spherical ball much stiffer than the one already shown (see Figure 4.20), and finally another object with a completely different shape (see Figure 4.21). The initial and final
<figure><img src="image_65.png" /><figcaption>Figure 4.16: First experience for a fully actuated case using a semi-spherical ball: two configurations of positioning 2 feature points using 2 robots. (a) and (c) represent the initial state of the object while (b) and (d) depict its state at the end of the deformation, respectively. The cyan and blue dots represent the projection in the image of the 3D desired positions of the two feature points represented by white thumbtacks.</figcaption></figure>
<figure><img src="image_66.png" /><figcaption>Figure 4.17: Evolution of the error for each feature point along the three axes (x, y, z), and the overall error norm of the positioning task represented in Figure 4.16(a, b).</figcaption></figure>

<figure><img src="image_67.png" /><figcaption>Figure 4.18: Evolution of the error for each feature point along the three axes (x, y, z), and the overall error norm of the positioning task represented in Figure 4.16(c, d).</figcaption></figure>
configurations of the deformation process using each of these objects are shown in views (a) and (b) respectively. We can see on all (b) views that the markers and the blue and cyan dots are superimposed, demonstrating the success of the deformation task. Additionally, Figure 4.22 illustrates the evolution of the error norm for each of these tasks. As shown, the error norm converges to within 2 mm in less than 5 seconds, indicating the success of the positioning of the three objects.

<figure><img src="image_68.png" /><figcaption>Figure 4.19: Second experience for a fully actuated case using a semi-ellipsoid ball: Initial (a) and last state (b) of the object undergoing the indirect positioning of two feature points (white markers) using two robots. Colored points defined as in caption of Figure 4.16.</figcaption></figure>

<figure><img src="image_69.png" /><figcaption>Figure 4.20: Third experience for a fully actuated case using a semi-spherical ball: Initial (a) and last state (b) of the semi-spherical ball undergoing the indirect positioning of two feature points (white thumbtacks) using two robots. Colored points defined as in caption of Figure 4.16.</figcaption></figure>
<figure><img src="image_70.png" /><figcaption>Figure 4.21: Fourth experience for a fully actuated case using a planar object: (a) and (b) views and colored points defined as in caption of Figure 4.20.</figcaption></figure>

<figure><img src="image_71.png" /><figcaption>Figure 4.22: Evolution of the error norm measured at each time step for the positioning tasks, represented in Figures 4.19, 4.20 and 4.21.</figcaption></figure>
**Effect of the feed-forward term**
This paragraph presents two experiments that focus on the positioning of two feature points employing two robots. These experiments differ in whether the feed-forward term **b** employed in the control law (3.70) is included or not. The goal of these experiments is therefore to analyze the impact of **b** on the stability and successful completion of the positioning task.

The state of the object at the end of the deformation process when $\mathbf{b} \neq \mathbf{0}$ is shown in Figure 4.23(b). However, by setting $\mathbf{b} = \mathbf{0}$, the deformation task fails. The convergence and divergence for each case are illustrated in Figure 4.24.

This experiment illustrates the importance of including the feed-forward term in the control law, as it leads to an error norm of less than 2mm (red plot), while not including it leads to an oscillating and non-converging behavior (green plot), where a loss of contact with the ball occurs.

<figure><img src="image_72.png" /><figcaption>Figure 4.23: Indirect positioning of 2 feature points using 2 robots for testing the impact of the feed-forward term in (3.70). Initial (a) and last state (b) of the semi-spherical ball obtained using the feed-forward term.</figcaption></figure>

**4.3 Conclusion**

In this chapter, we validated our proposed real-time model-based and vision-based control law based on a simple MSM. We achieved this by driving various feature points on multiple soft objects with diverse geometric and physical properties to their desired positions. This manipulation was carried out by controlling one or two robotic manipulators deforming the objects simultaneously.
<figure><img src="image_73.png" /><figcaption>Figure 4.24: Evolution of the error norm of the positioning task, shown in Figure 4.23 with and without the feed-forward term.</figcaption></figure>

In the first section, we outlined the essential steps for constructing the object model. The initial step involved the generation of a mesh that describes the object geometrical structure. Subsequently, we conducted parameter estimation for this model to establish a physical representation, enabling it to accurately replicate the real-world deformations of the actual object. These parameters are the stiffness matrix and damping coefficient of the MSM. It is important to note that these estimated parameters are approximate, potentially leading to discrepancies between the model and the real object. To address this, we introduced object tracking techniques to rectify any deviations between the model and the actual object.

In the second section, we presented experimental results obtained from interactions between one and two robot manipulators and various soft objects. These results further confirm the efficiency and robustness of the proposed control approach in dealing with external perturbations and model uncertainties. It allows to efficiently, accurately and rapidly bring multiple points of a deformable object to desired 3D positions using a simple MSM and without requiring the exact knowledge of the model parameters. To ensure that the object can be deformed so that the feature points reach their desired positions, it is however important to carefully select the positions of the manipulated points. Moreover, the attainability of the desired configuration depends on the object being deformed. For example, if the object has a limited range of motion, it may not be possible to deform it
as much as desired.
# SHAPE SERVOING OF A SOFT OBJECT

In this chapter, we propose a physics-based robot controller for shape servoing of soft objects. The object shape is represented by the object 2D contour when deforming 2D objects, while it is represented by the object complete surface when dealing with 3D objects. The objective is to manipulate the soft object shape toward a desired configuration using a limited number of manipulated points. However, due to the high number of points used to represent the object shape and the limited number of manipulated points, this results in a highly under-actuated system. As a result, the targeted deformation of the system may not be ensured. For instance, achieving the deformation task would require at least one manipulated point for each point belonging to the object shape as proposed in [Wada et al., 2001a] and as discussed in Chapter 3, which is not feasible. To alleviate this problem, the shape of the deformable object is represented using a low-dimensional feature vector.

We derive two types of dimension reduction in this chapter: the first using Fourier descriptors (see Section 5.2), and the second employing 3D moments (see Section 5.3). For each of these reduction techniques, we obtain a low-dimensional vector and establish an analytical relationship describing the variation of the resulting vector in function of the manipulated points motions. As previously, these relationships are derived by approximating the object with a mass-spring model (MSM). Subsequently, we develop a control law for each shape representation using the aforementioned relations. Ultimately, we apply these developed control laws to achieve shape servoing tasks.

Both simulation and experimental results are presented, which showcase the effectiveness of our approach in soft object shape servoing.
## 5.1 Problem formulation

The main objective of this section is to deform a 3D soft object with w surface points, represented by

$$ \mathbf{x}_o = (\mathbf{x}_{o1}, \dots, \mathbf{x}_{ow}), \qquad (5.1) $$

where $o_i$, $\forall 1 \le i \le w$; $o_i \in \mathcal{N}$, 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_{m_1}, \dots, P_{m_M}]$, with coordinates $(\mathbf{x}_{m_1}, \dots, \mathbf{x}_{m_M})$.

As previously, 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 [Chaumette & Hutchinson, 2008].

## 5.2 Shape servoing of a soft object using Fourier Descriptors

In this section, 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 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 we develop the analytic relation that links the variation of the Fourier coefficients 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.

### 5.2.1 3D shape servoing

By expressing the object surface points $\mathbf{x}_{o_i}$, $i \in [1, \dots, w]$, with spherical coordinates, the 3D cartesian position of each point can be written as:

$$ \left\{ \begin{aligned} x_{o_i} &= x_o^g + \rho(\theta_{o_i}, \phi_{o_i}) \cos(\phi_{o_i}) \sin(\theta_{o_i}) \\ y_{o_i} &= y_o^g + \rho(\theta_{o_i}, \phi_{o_i}) \sin(\phi_{o_i}) \sin(\theta_{o_i}) \\ z_{o_i} &= z_o^g + \rho(\theta_{o_i}, \phi_{o_i}) \cos(\theta_{o_i}) \end{aligned} \right. \qquad (5.2) $$
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_{o_i}, \phi_{o_i})$ the radial distance $\|\mathbf{x}_{o_i} - \mathbf{x}_o^g\|$ of the point $P_{o_i}$ from $\mathbf{x}_o^g$, $\theta_{o_i} = \arccos(z_{o_i}/\rho(\theta_{o_i}, \phi_{o_i}))$ the polar angle and $\phi_{o_i} = \operatorname{atan2}(y_{o_i}, x_{o_i})$ the azimuthal angle.

$\rho(\theta, \phi)$ is a closed 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 following:

$$ \rho(\theta_{o_i}, \phi_{o_i}) = \sum_{l=0}^{p} \sum_{j=0}^{q} \left[ \begin{aligned} & a_{lj} \cos(l\theta_{o_i}) \cos(j\phi_{o_i}) + b_{lj} \cos(l\theta_{o_i}) \sin(j\phi_{o_i}) \\ & + c_{lj} \sin(l\theta_{o_i}) \cos(j\phi_{o_i}) + d_{lj} \sin(l\theta_{o_i}) \sin(j\phi_{o_i}) \end{aligned} \right] \quad (5.3) $$

where $a_{lj}$, $b_{lj}$, $c_{lj}$ and $d_{lj}$ 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}, \dots, a_{pq}, b_{01}, \dots, b_{pq}, c_{pq}, d_{pq}) \quad (5.4) $$

As noticed from (5.4), 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}_{o_i}$

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 $(\mathrm{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]$.

### 5.2.1.1 Modeling

By approximating the physical behavior of the object using the MSM, and using (3.69), we demonstrated in Chapter 3 the velocity $\dot{\mathbf{x}}_{o_i}$, $1 \le i \le w$, of any surface point due to the velocity $\dot{\mathbf{x}}_m = (\dot{\mathbf{x}}_{m_1}, \dots, \dot{\mathbf{x}}_{m_M})$ applied on manipulated points ($P_{m_1}, P_{m_2}, \dots, P_{m_M}$).
Therefore, we directly obtain the velocity of all points belonging to the model surface:

$$ \dot{\mathbf{x}}_o = \mathbf{A}_o \dot{\mathbf{x}}_m + \mathbf{b}_o \qquad (5.5) $$

with

$$ \mathbf{A}_o = \begin{bmatrix} \mathbf{A}_{o1m_1} & \dots & \mathbf{A}_{o1m_M} \\ \mathbf{A}_{o2m_1} & \dots & \mathbf{A}_{o2m_M} \\ \vdots & \ddots & \vdots \\ \mathbf{A}_{owm_1} & \dots & \mathbf{A}_{owm_M} \end{bmatrix}, \mathbf{b}_o = \begin{bmatrix} \mathbf{b}_{o1} = \sum_{l=1}^{M} \mathbf{b}_{o1m_l} \\ \vdots \\ \mathbf{b}_{ow} = \sum_{l=1}^{M} \mathbf{b}_{owm_l} \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{A}_s \dot{\mathbf{x}}_m + \mathbf{b}_s \qquad (5.6) $$

By deriving (5.2) and (5.3), we obtain the motion $\dot{\mathbf{x}}_{o_i} = (\dot{x}_{o_i}, \dot{y}_{o_i}, \dot{z}_{o_i})$ of each point belonging to the object surface, as a function of $\dot{\mathbf{s}}$, $\dot{\theta}_{o_i}$ and $\dot{\phi}_{o_i}$. However, we have to eliminate $\dot{\theta}$ and $\dot{\phi}$ in order to express the motions of these points solely in terms of $\dot{\mathbf{s}}$. For that, we obtain, by linearly combining the three equations in (5.2), $\forall i \in [1, ..., w]$:

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

with

$$ \alpha_{o_i} = b_1 c_2 - b_2 c_1, \quad \beta_{o_i} = a_2 c_1 - a_1 c_2, \quad \sigma_{o_i} = a_1 b_2 - a_2 b_1 $$

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

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

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

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

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

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

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

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

By using the same linear combination on (5.5) and identifying the equation obtained with (5.7), 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} = L_o \dot{x}_m + \delta_o = C_o \dot{s} \quad (5.8)$$

with

$$L_o = \begin{bmatrix} \begin{bmatrix} \alpha_{o1} & \beta_{o1} & \sigma_{o1} \end{bmatrix} & \begin{bmatrix} A_{o1m1} & \dots & A_{o1m_M} \end{bmatrix} \\ \vdots & \vdots \\ \begin{bmatrix} \alpha_{ow} & \beta_{ow} & \sigma_{ow} \end{bmatrix} & \begin{bmatrix} A_{owm1} & \dots & A_{owm_M} \end{bmatrix} \end{bmatrix}$$

$$\delta_o = \begin{bmatrix} \begin{bmatrix} \alpha_{o1} & \beta_{o1} & \sigma_{o1} \end{bmatrix} & \mathbf{b}_{o1} \\ \vdots & \\ \begin{bmatrix} \alpha_{ow} & \beta_{ow} & \sigma_{ow} \end{bmatrix} & \mathbf{b}_{ow} \end{bmatrix}$$

$$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 (5.8) with (5.6), we finally obtain:

$$A_s = C_o^+ L_o \quad \text{and} \quad b_s = C_o^+ \delta_o \qquad (5.9)$$

with $C_o^+$ the Moore-Penrose pseudo-inverse of $C_o$. Matrix $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. $A_s$ can be considered as the interaction matrix by similarity with the classical visual servoing framework [Chaumette & Hutchinson, 2008], while $b_s$ is an inertia term.

In this paragraph, we developed the variation of the low-dimensional feature vector $s$ as a function of the manipulated point velocities, as shown in (5.6). This expression is similar to the one presented in (3.69), which was proposed for contexts involving discrete object feature point displacements in response to manipulated point motions. These equations incorporate both an interaction matrix and an inertia term, and their forms have been derived analytically.

### 5.2.1.2 Control scheme

This section presents the control law to be applied to the manipulated points in order to drive the object to its desired 3D shape. Similar to what was previously exploited in Section 3.1.3, which allowed us to obtain (3.70) from (3.69), the control velocities to be applied to the manipulated points to drive the visual features $s$ to their desired values $s^*$ are obtained from (5.6) as:

$$\dot{x}_m = -\lambda \hat{A}_s^+ (s - s^*) - \hat{A}_s^+ \hat{b}_s \qquad (5.10)$$

The stability analysis of the closed-loop control law proposed in (5.10) is similar to what was presented in Section 3.1.3.1. Regarding the achievement of the 3D surface deformation task, it depends on the number of harmonics $p$ and $q$. When $k > 3M$, the controller (5.10) 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 $s$ towards $s^*$ is theoretically guaranteed as long as $\hat{A}_s$ is not a significantly coarse approximation of $A_s$.

Note, however, that this does not necessarily mean that the entire object will reach its desired shape, but only its representation by the selected Fourier coefficients. Thus, the choice of the number of harmonics to be injected in $s$ depends on the complexity of the shape to be obtained, i.e. complex deformations require more harmonics and
consequently more manipulated points.

### 5.2.2 2D shape servoing

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

$$ \boldsymbol{x}_{c_i} = (x_c^g + \rho(\theta_{c_i}) \cos(\theta_{c_i}), y_c^g + \rho(\theta_{c_i}) \sin(\theta_{c_i})). \quad (5.11) $$

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

$$ \rho(\theta_{c_i}) = a_0^c + \sum_{l=1}^{p_c} [a_l^c \cos(l\theta_{c_i}) + b_l^c \sin(l\theta_{c_i})] \quad (5.12) $$

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

$$ \boldsymbol{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 (5.13) $$

and $\dim(\boldsymbol{s}_c) = (2p_c + 3)$. Following the same reasoning as in Section 5.2.1.1, the variation of $\boldsymbol{s}_c$ with respect to the motions of the manipulated points $\dot{\boldsymbol{x}}_m$ can be expressed as in (5.6):

$$ \dot{\boldsymbol{s}}_c = \boldsymbol{A}_{\boldsymbol{s}_c} \dot{\boldsymbol{x}}_m + \boldsymbol{b}_{\boldsymbol{s}_c} \quad (5.14) $$

with $\boldsymbol{A}_{\boldsymbol{s}_c} = \boldsymbol{C}_c^+ \boldsymbol{L}_c$ and $\boldsymbol{b}_{\boldsymbol{s}_c} = \boldsymbol{C}_c^+ \boldsymbol{\delta}_c$ where

$$ \boldsymbol{L}_c = \begin{bmatrix} [\alpha_{c_1} & \beta_{c_1}] [\gamma_{c_1 m_1} & \dots & \gamma_{c_1 m_M}] \\ \vdots \\ [\alpha_{c_{w_c}} & \beta_{c_{w_c}}] [\gamma_{c_{w_c} m_1} & \dots & \gamma_{c_{w_c} m_M}] \end{bmatrix}, $$

$$ \boldsymbol{\delta}_c = \begin{bmatrix} [\alpha_{c_1} & \beta_{c_1}] \boldsymbol{\delta}_{c_1} \\ \vdots \\ [\alpha_{c_{w_c}} & \beta_{c_{w_c}}] \boldsymbol{\delta}_{c_{w_c}} \end{bmatrix}, $$

$$ \boldsymbol{C}_c = \begin{bmatrix} \alpha_{c_1} & \beta_{c_1} & \nu_{c_1} \frac{\partial \rho(\theta_{c_1})}{\partial \boldsymbol{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 \boldsymbol{q}_c} \end{bmatrix}, $$
and

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

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

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

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

### 5.2.2.1 Control scheme

The closed-loop control law to be applied by the robotic manipulators to drive $\mathbf{s}_c$ to a desired contour $\mathbf{s}_c^*$ is directly deduced from (5.14):

$$ \dot{\mathbf{x}}_m = -\lambda \widehat{\mathbf{A}}_{\mathbf{s}_c}^{+} (\mathbf{s}_c - \mathbf{s}_c^*) - \widehat{\mathbf{A}}_{\mathbf{s}_c}^{+} \widehat{\mathbf{b}}_{\mathbf{s}_c}, \quad (5.15) $$

with $\lambda > 0$ always denoting a positive gain and $\widehat{\mathbf{A}}_{\mathbf{s}_c}^{+}$ representing the Moore-Penrose pseudo-inverse of the approximation of $\mathbf{A}_{\mathbf{s}_c}$.

Concerning the achievement of the contour deformation task, it depends on the number of harmonics $p_c$. When $[\mathrm{dim}(\mathbf{s}_c) = (2p_c + 3)] > 2M$, the controller in (5.15) is underactuated and cannot guarantee the asymptotic convergence of $\mathbf{s}_c$ to $\mathbf{s}_c^*$. Conversely, when $\mathrm{dim}(\mathbf{s}^c) < 2M$, the stability and convergence of the system is theoretically guaranteed as long as $\widehat{\mathbf{A}}_{\mathbf{s}_c}$ is not a significantly coarse approximation of $\mathbf{A}_{\mathbf{s}_c}$. As before, the choice of number of harmonics to be used depends on the complexity of the contour to be obtained, with complex deformations requiring more harmonics and hence more manipulated points.

### 5.2.3 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 verified through 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 Figure 5.1. 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 Figure 5.1.b correspond to the reconstructed contour at the end of the servo using the same number of 8 harmonics. We notice that the yellow points align with the green points on one side and with the blue points on the other side. The alignment with the green points indicates the successful reconstruction of the contour using 8 harmonics, while the alignment with the blue points demonstrates the accuracy and success of the shape servoing task. Furthermore, the alignment of the green points with the white points indicates the success of the contour representation using the proposed contour parameters (5.13).

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

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 higher 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 Figure 5.1, at least 6 harmonics are needed. For that, we test $p_c = 6$. Additionally, to achieve a more accurate reconstruction, we also test two
<figure><img src="image_74.png" /><figcaption>Figure 5.1: Deformation of a planar object: (a) initial and desired configurations, (b) final configuration (see text for explanations on color code).</figcaption></figure>

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 $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 Figure 5.2. 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. Recall that the same set of manipulated points is used in all these cases, and since the system is under-actuated, the rank of $A_{\mathbf{s}_c} \hat{\mathbf{A}}_{\mathbf{s}_c}^+$ is at most rank $12 < dim(\mathbf{s}_c)$. Consequently, following the discussion provided in Section 3.1.3.1, $A_{\mathbf{s}_c} \hat{\mathbf{A}}_{\mathbf{s}_c}^+$ has a non-zero null space, leading to a local minimum. As a result, the error norm converges to a non-zero value. Moreover, an increase in the number of harmonics results in an increase in the feature vector dimension, and as a result, the higher residual shape servoing error is observed with a greater number of harmonics.

Furthermore, the difference between the actual and desired contour is shown in Figure 5.3 for the different values of $p_c$. This difference has been computed using the Hausdorff distance [Taha & Hanbury, 2015] 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 two tests performed with different number of harmonics,,
using 8 harmonics is here sufficient to achieve the task with high accuracy.

<figure><img src="image_75.png" /><figcaption>Figure 5.2: Evolution of the error norm for different numbers of harmonics and using 6 manipulated points.</figcaption></figure>

**5.2.4 Experimental results**

Two experiments are described in this section to validate our approach. Additional ex-
perimental results can be found here: [IROS-video].

In the following experiments, we use (5.4) for the calculation of the features **s** and we
select *p* = 1 and *q* = 3 harmonics in **s** since this choice enables to correctly represent the
object without being excessively under-actuated. The convergence is considered achieved
as soon as the error ||**s** - **s***|| reaches 5% of its initial value. For the first experiment,
Figure 5.4(a) shows the object at its initial configuration while Figure 5.4(b) displays
the object after convergence to its desired configuration. Figure 5.5 exhibits a 3D rep-
resentation of the object surface points observed from two different viewpoints (initial
configuration in red, final and desired configurations respectively in green and blue).

The second experiment involves a more complex deformation, as can be seen in Fig-
ures 5.6 and 5.7. The deformation process begins with the object in its rest state, as
represented in Figure 5.6(a). Additionally, the object surface points and their desired
positions are represented by green points and blue points in a 3D visualization provided
<figure><img src="image_76.png" /><figcaption>Figure 5.3: Evolution of the Hausdorff distance between the actual and desired contours for different numbers of harmonics and using 6 manipulated points.</figcaption></figure>

<figure><img src="image_77.png" /><figcaption>Figure 5.4: First experiment: initial (a) and final (b) images acquired by the RGB-D camera.</figcaption></figure>
<figure><img src="image_78.png" /><figcaption>Figure 5.5: 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>

in Figure 5.7(a). Then, the shape servoing task is launched and the deformed object is depicted in Figure 5.6(b). As can be seen from Figure 5.7(b), the green points are superimposed with the blue points, indicating the success of the deformation task.

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

For both experiments, the time evolution of the error norm $||s - s*||$ is shown in Figure 5.8, while Figure 5.9 presents the evolution of the Hausdorff distance between the current and desired surface points. The convergence of the error norm to a local minimum in Figure 5.8 is evident following the same methodology provided previously. The rank of
<figure><img src="image_80.png" /><figcaption>Figure 5.7: 3D visualization for the second experiment.</figcaption></figure>

$\mathbf{A}_s \hat{\mathbf{A}}_s^+$ is $6 < dim(\mathbf{s}) = 24$. Consequently, $\mathbf{A}_s \hat{\mathbf{A}}_s^+$ has a non-zero null space, leading the convergence to a local minimum.

<figure><img src="image_81.png" /><figcaption>Figure 5.8: Evolution of $\|\mathbf{s} - \mathbf{s}^*\|$ (first experiment in red, second one in green).</figcaption></figure>

The correct achievement of the deformation task is directly visible from Figures 5.5 and 5.7 where we can note that the final shapes (in green) are well superposed on the desired ones (in blue) for the various deformations to be achieved, even in the presence of external perturbations. For both medium and large deformations, we can 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. Note that the initial errors are smaller in this case since the rigid displacement is smaller.
<figure><img src="image_82.png" /><figcaption>Figure 5.9: Evolution of the Hausdorff distance between the current and desired object surface points (first experiment in red, second one in green).</figcaption></figure>

## 5.3 Shape servoing of a soft object using 3D moments

In this section, we introduce a novel method for manipulating various points on the object surface to change its shape, parameterized by a set of 3D moments, ultimately bringing it to a desired configuration.

### 5.3.1 3D shape servoing

This work is motivated by the work presented in [Galvez & Canton, 1993], which focused on 3D shape recognition. The 3D coordinates of the object surface points are expressed in the origin frame (center of mass) of the object. The resulted centered surface is then described using moments up to order 2, resulting in an ellipsoid representation of the object. This ellipsoid can be described by 6 parameters. Among these parameters, three correspond to the principal axes of the ellipsoid, which are associated with the eigenvectors of the inertia matrix $M$. Additionally, the other three parameters represent the lengths of the principal axes of the ellipsoid and are related to the eigenvalues of $M$.

The inertia matrix $M$ is given below:

$$ M = \begin{bmatrix} \mu_{020} + \mu_{002} & -\mu_{110} & -\mu_{101} \\ -\mu_{110} & \mu_{200} + \mu_{002} & -\mu_{011} \\ -\mu_{101} & -\mu_{011} & \mu_{200} + \mu_{020} \end{bmatrix} \quad (5.16) $$

with $\mu_{ijk}$ being the central moments of order $i+j+k$ corresponding to the surface points,
which are defined as follows:

$$ \mu_{ijk} = \sum_{l=1}^{w} (x_{o_l} - x_g)^i (y_{o_l} - y_g)^j (z_{o_l} - z_g)^k \quad (5.17) $$

where $x_g = (x_g, y_g, z_g)$ is the centroid of the surface points. Let $\lambda_1, \lambda_2$, and $\lambda_3$ be the eigenvalues of $M$. From these eigenvalues, the lengths of the principal axes can be calculated as follows:

$$ l_1 = \sqrt{\lambda_1/w}, \quad l_2 = \sqrt{\lambda_2/w}, \quad l_3 = \sqrt{\lambda_3/w} $$

Let $v_1, v_2$, and $v_3$ represent the corresponding eigenvectors of $M$. The rotation matrix that represents the orientation of the principles axes is then given by:

$$ R = \begin{pmatrix} v_1(1)/\|v_1\| & v_2(1)/\|v_2\| & v_3(1)/\|v_3\| \\ v_1(2)/\|v_1\| & v_2(2)/\|v_2\| & v_3(2)/\|v_3\| \\ v_1(3)/\|v_1\| & v_2(3)/\|v_2\| & v_3(3)/\|v_3\| \end{pmatrix} \quad (5.18) $$

with $v_j(i)$ being the $i^{th}$ element in the vector $v_j$, and $\|v_j\| = \sqrt{v_j^T v_j}$.

Following a similar approach, we first propose nine parameters to represent the object surface by an ellipsoid. The first three parameters pertain to the translational movement of the object and are represented by the coordinates of the centroid of the surface points, $x_g$, given by:

$$ x_g = \sum_{l=1}^{w} x_{o_l}/w = \mu_{100}/w, \quad y_g = \sum_{l=1}^{w} y_{o_l}/w = \mu_{010}/w, \quad z_g = \sum_{l=1}^{w} z_{o_l}/w = \mu_{001}/w \quad (5.19) $$

The remaining six parameters include the three lengths of the principal axes computed from the eigenvalues of the inertia matrix $M$, which are obtained via second-order central moments, and the three angles that define the ellipsoid orientation. In this work, we are interested in calculating this orientation using a local rotation matrix, denoted as $\tilde{R}$, which can be obtained by computing the difference between the actual rotation matrix, denoted as $R$, and the desired one, denoted as $R^*$, as follows: $\tilde{R} = RR^{*T}$. Furthermore, we represent this local orientation matrix using $[\theta U]$ vector representation, which can be
obtained as follows:

$$[\theta U] = ([\theta U]_x, [\theta U]_y, [\theta U]_z) \qquad (5.20)$$

$$\cos(\theta) = (\tilde{R}(1, 1) + \tilde{R}(2, 2) + \tilde{R}(3, 3) - 1.0) / 2 \qquad (5.21)$$

$$[\theta U]_x = (\tilde{R}(3, 2) - \tilde{R}(2, 3)) / 2\sin(\theta) \qquad (5.22)$$

$$[\theta U]_y = (\tilde{R}(1, 3) - \tilde{R}(3, 1)) / 2\sin(\theta)$$

$$[\theta U]_z = (\tilde{R}(2, 1) - \tilde{R}(1, 2)) / 2\sin(\theta)$$

with $\tilde{R}(i, j)$ representing the element at the i-th row and j-th column of the matrix $\tilde{R}$.

These nine parameters are utilized to represent the ellipsoid that best fits at each time the object surface points, starting from its initial configuration and moving towards its desired configuration. To efficiently represent the complete surface, additional moments with an order greater than two are then added. For this purpose, we use the following centered moments, which possess translational invariance:

$$r_1 = (\mu_{300} + \mu_{030} + \mu_{003})/w^2 \qquad (5.23)$$

$$r_2 = (\mu_{140} + \mu_{014} + \mu_{401})/w^2 \qquad (5.24)$$

Finally, the visual feature vector $\mathbf{s}$ that we propose to regulate by visual servoing is composed as follows:

$$\mathbf{s} = [x_g, y_g, z_g, r_1, r_2, l_1, l_2, l_3, [\theta U]_x, [\theta U]_y, [\theta U]_z] \qquad (5.25)$$

### 5.3.1.1 Modeling

In the previous paragraph, we defined a low-dimensional feature vector $\mathbf{s}$ to represent the object surface points. In this paragraph, our objective is to analytically calculate the variation of $\mathbf{s}$ with respect to the manipulated point motions, i.e. $\dot{\mathbf{s}}$ as a function of $\dot{\mathbf{x}}_m$.

Since each element of $\mathbf{s}$ is a combination of various centered 3D moments, $\mu_{ijk}$, we begin by providing the derivation of each element of $\mathbf{s}$ in terms of the centered 3D moments. This derivation allows us to express $\mathbf{s}$ as a function of a vector of centered 3D moments, denoted as $\boldsymbol{\mu}$. Subsequently, we derive $\boldsymbol{\mu}$ as a function of $\dot{\mathbf{x}}_m$. Afterward, we proceed to derive $\dot{\mathbf{s}}$ as a function of $\dot{\mathbf{x}}_m$.

The first five elements of $\mathbf{s}$ are defined as combinations of $\mu_{ijk}$, as shown in (5.19), (5.23), and (5.24). Benefiting from $\mathbf{M}$ being a real and symmetric matrix, the analytical
formulations of eigenvalues and eigenvectors of **M** can be derived [Deledalle et al., 2017].
More precisely, we have:

$$
\lambda_1 = [a + b + c - 2\sqrt{x_1} \cos(\phi / 3)] / 3 \qquad (5.26)
$$

$$
\lambda_2 = [a + b + c + 2\sqrt{x_1} \cos((\phi - \pi)/3)] / 3 \quad (5.27)
$$

$$
\lambda_3 = [a + b + c - 2\sqrt{x_1} \cos((\phi + \pi)/3)] / 3 \quad (5.28)
$$

with

$$
\begin{align*}
& a = \mu_{020} + \mu_{002}, \quad b = \mu_{200} + \mu_{002}, \quad c = \mu_{200} + \mu_{020}, \quad d = -\mu_{110}, \quad e = -\mu_{011}, \quad f = -\mu_{101}, \\
& x_1 = a^2 + b^2 + c^2 - ab - ac - bc + 3(d^2 + f^2 + e^2), \quad \phi = \operatorname{atan2}(\sqrt{4x_1^3 - x_2^2}, x_2) \\
& x_2 = -(2a - b - c)(2b - a - c)(2c - a - b) - 54def \\
& \qquad + 9[(2c - a - b)d^2 + (2b - a - c)f^2 + (2a - b - c)e^2]
\end{align*}
$$

Regarding the eigenvectors, they are calculated as follows:

$$
\mathbf{v}_1 = \begin{pmatrix} (\lambda_1 - c - em_1)/f \\ m_1 \\ 1 \end{pmatrix}, \mathbf{v}_2 = \begin{pmatrix} (\lambda_2 - c - em_2)/f \\ m_2 \\ 1 \end{pmatrix}, \mathbf{v}_3 = \begin{pmatrix} (\lambda_3 - c - em_3)/f \\ m_3 \\ 1 \end{pmatrix}, \qquad (5.29)
$$

with

$$
m_1 = \frac{d(c - \lambda_1) - ef}{f(b - \lambda_1) - de}, \quad m_2 = \frac{d(c - \lambda_2) - ef}{f(b - \lambda_2) - de}, \quad m_3 = \frac{d(c - \lambda_3) - ef}{f(b - \lambda_3) - de}
$$

The [θU] vector in **s** is obtained from [$v_1$ $v_2$ $v_3$] using (5.20). These vectors depend on
the eigenvalues and other elements, all of which solely rely on centered moments. As a
result of this dependency, all the elements of the low-dimensional vector **s** are analytically
calculated as functions of the centered moments. Finally, **s** can be expressed as follows:

$$
\mathbf{s} = \mathbf{g}(\boldsymbol{\mu}) \tag{5.30}
$$

$$
\text{with } \boldsymbol{\mu} = [\mu_{100}, \mu_{010}, \mu_{001}, \mu_{110}, \mu_{101}, \mu_{011}, \mu_{200}, \mu_{020}, \mu_{002}, \mu_{300}, \mu_{030}, \mu_{003}, \mu_{140}, \mu_{014}, \mu_{401}]^T
$$

By using the chain rule algorithm, $\dot{\mathbf{s}}$ can be calculated as $\dot{\mathbf{s}} = \frac{\partial \mathbf{g}}{\partial \mathbf{\mu}} \dot{\mathbf{\mu}}$. Therefore, we get:

$$
\dot{\mathbf{s}} = \frac{\partial \mathbf{g}}{\partial \boldsymbol{\mu}} L_{\boldsymbol{\mu}} \dot{\mathbf{x}}_m + \frac{\partial \mathbf{g}}{\partial \boldsymbol{\mu}} \mathbf{b}_{\boldsymbol{\mu}} = L_{\mathbf{s}} \dot{\mathbf{x}}_m + \mathbf{b}_{\mathbf{s}} \quad (5.31)
$$
with

$$ \dot{\mu} = L_{\mu} \dot{x}_{m} + b_{\mu} \qquad (5.32) $$

The complete derivation of $L_s$ and $b_s$ is given in Appendix (Section 7.1).

### 5.3.1.2 Control scheme

From the relation (5.31), we use the same strategy as in Section 5.2.1.2 to design the control scheme and we obtain:

$$ \dot{x}_{m} = -\lambda \widehat{L}_{s}^{+}(s - s^{*}) - \widehat{L}_{s}^{+} \widehat{b}_{s} \qquad (5.33) $$

The stability analysis of the closed-loop control law introduced in (5.33) follows a similar discussion to that presented in Section 3.1.3.1. The achievement of the 3D surface deformation task depends on two primary factors. The first factor pertains to the rank of $L_s$ and to the actuation state of the system, whether it is over-actuated or under-actuated. The dimension of $s$, as defined in (5.25), is 11, which means that at least 4 manipulated points must be engaged to ensure the transportation of $s$ to $s^*$. Consequently, if fewer than 4 manipulated points are used, the reduction of $s$ to $s^*$ is not guaranteed, thereby impeding the achievement of the shape servoing task. On the other hand, if $L_s$ and $\widehat{L}_s^+$ are of full rank 11, and $\widehat{L}_s$ is not too coarse, then error convergence to zero is theoretically ensured. The second factor is related to the representation of the object surface using the defined low-dimensional feature vector $s$ (5.25), which is based on 3D moments. In essence, if the object surface can be adequately represented using these features, then driving $s$ to $s^*$ implies that the object surface will be transformed into its desired shape.

### 5.3.2 Simulation results

In this section, we test the ability of the proposed controller (5.33) to deform a semi-spherical 3D object, as illustrated in Figure 5.10(a). The objective is to drive its actual surface points, represented by green points, to their desired positions, indicated by blue points. This deformation is achieved through the manipulation of a limited number of three manipulated points, represented by big red points.

To validate the deformation process, we ensure the feasibility of the desired shape. Initially, the object is at rest state, and then it is deformed by arbitrarily moving the positions of the three manipulated points. The resulting deformed surface is selected
as the desired shape since it is attainable using the chosen manipulated points. Two different views of the deformation task are presented in Figure 5.10. In the first row (Perspective 1), green points represent actual object surface points, blue points denote their desired positions, and large red points correspond to manipulated points. The second row (Perspective 1) in Figure 5.10 displays green ellipsoids, which correspond to the ellipsoids parameterized by the 9 components of $\mathbf{s}$ that best fit the object surface points shown in the first row, while blue ellipsoids represent the ellipsoids that best fit the desired shape points, also shown in the first row. These ellipsoids correspond to points with the same colour represented in the images directly above them. For instance, the green ellipsoid in Figure 5.10(c) corresponds to the ellipsoid that fits the green points in Figure 5.10(a), while the blue ellipsoid in Figure 5.10(c) corresponds to the desired ellipsoid that fits the blue points in Figure 5.10(a). Rows 3 and 4 provide a second perspective view on the deformation process, analogous to rows 1 and 2, respectively.

Once the desired object shape is defined, we establish the initial and desired surface parameters as $\mathbf{s}$ and $\mathbf{s}^*$. The deformation task is then started with the goal of driving $\mathbf{s}$ towards $\mathbf{s}^*$. The resulting deformed object, obtained when the system converges, is illustrated in Figure 5.10(b) and Figure 5.10(f). These two images depict the same object state from different perspective views. In Figure 5.10(b), we can see the deformed state of the object from the same perspective as in Figure 5.10(a), where the object was in its initial state before stating the visual servoing. Similarly, we provide a second perspective in Figure 5.10(e) and Figure 5.10(f) showing respectively the object in its initial and final shape.

We can observe that the green points approach the blue points in Figure 5.10(b) and Figure 5.10(f), signifying the success of the deformation task. Furthermore, in Figure 5.10(d) and Figure 5.10(h), the actual ellipsoid closely matches the desired ellipsoid. We further present in Figure 5.11 the evolution of $\|\mathbf{s} - \mathbf{s}^*\|$, which shows an exponential decrease of this error from an initial value of 17.5 to 0.142. By analogy with the discussion provided in Section 3.1.3.1, we can notice that $\text{rank}(\mathbf{L}_{\mathbf{s}}\mathbf{L}_{\mathbf{s}}^{+}) = 9 < \text{dim}(\mathbf{s}) = 11$. Consequently, $\mathbf{L}_{\mathbf{s}}\mathbf{L}_{\mathbf{s}}^{+}$ has a non-zero null space, which explains why we obtained a non-zero value rather than a value close to zero.

Furthermore, we analyze in Figure 5.12 the variation in the Hausdorff distance throughout the deformation process. Initially, this distance increases slightly, mainly due to the initial error on the orientation parameters in $\mathbf{s}$. Despite this effect, the distance error subsequently converges to a value around 3 mm, starting from an initial value of 9 cm.
<figure><img src="image_83.png" /><figcaption>Figure 5.10: 3D Deformation of a semi-spherical ball using 3D moments: The first column depicts the initial state, while the second column illustrates the final state obtained at the end of the shape servoing task.</figcaption></figure>
This low value at the convergence of the system validates the success of the deformation
task, as it indicates that the actual object surface points closely approach their desired
positions.

<figure><img src="image_84.png" /><figcaption>Figure 5.11: Evolution of the error norm for the shape servoing task using 3D moments and represented in Figure 5.10.</figcaption></figure>

**5.3.3 Experimental results**

In this section, our objective is to validate the proposed approach based on 3D moments for shape servoing in an experimental scenario that includes a real soft object, two robotic manipulators, and a RGB-D camera. Since we use two robots, we can only manipulate two points, resulting in an under-actuated system ($6 < 11$). Therefore, the convergence of the error ($s - s^*$) to a local minimum is expected. In such cases, we consider to achieve convergence when $||s - s^*||$ reaches 5% of its initial value.

We start by displaying the object states before deformation (left image) and after
deformation (right image) in Figure 5.13. As for our previous experiments, the sticks
appearing in the images are rigid sticks attached to the robotic manipulators, with their
contact points on the object serving as manipulated points. The contact between the
sticks and the object is established at $t = 0$, when no deformation is applied to the object.
<figure><img src="image_85.png" /><figcaption>Figure 5.12: Evolution of the Hausdorff distance between the actual and desired surface points for the task represented in Figure 5.10.</figcaption></figure>

To present the deformation process more effectively, we show in Figure 5.14 the evolution of the error norm $||s - s^*||$ during the deformation task. As expected, since rank($L_s \hat{L}_s^+$) = 6 < dim($s$) = 11, the shape servoing error norm exponentially converges to a local minimum, which is within 5% of the initial error norm.

Furthermore, to provide a clearer view of the deformation process, we visualize in Figure 5.15 the 3D surface of the object before the deformation process (red points), its final surface (green points), and its desired surface (blue points). As shown in Figure 5.15(a) and (b), the green points closely align with the blue points, indicating the successful completion of the deformation task.

## 5.4 Conclusion

In this chapter, we proposed two real-time model-based and vision-based control laws for driving the object shape toward a desired configuration. In other words, we validated two approaches for indirectly positioning the object surface points at desired positions by manipulating a limited number of points.

In the first approach, we approximated the object shape using Fourier descriptors,
<figure><img src="image_86.png" /><figcaption>Figure 5.13: Experiment on shape servoing based on 3D moments: initial (a) and final (b) images acquired by the RGB-D camera.</figcaption></figure>

<figure><img src="image_87.png" /><figcaption>Figure 5.14: Evolution of the error norm for the experimental shape servoing task using 3D moments and represented in Figure 5.13.</figcaption></figure>
<figure><img src="image_88.png" /><figcaption>Figure 5.15: 3D visualization of the surface points of the object shown in Figure 5.13 from two different viewpoints (initial positions in red, final positions in green, and desired positions in blue).</figcaption></figure>
which construct a low-dimensional feature vector **s**. For a 3D object, its shape was
defined by its 3D surface points, while for a 2D object, its shape was defined by its 2D
contours. We then analytically derived the variation of each element in **s** in function of the
motions to be applied to the manipulated points by approximating the soft object with
a MSM. This relation constitutes the basis for our first control law for achieving shape
servoing. We first validated this approach by driving the 2D contour of soft planar objects
in a simulation environment. Subsequently, we validated the positioning of object surface
points in real experiments, involving a real soft object and two robotic manipulators.

In the second approach, we approximated the surface of a 3D object using a low-
dimensional feature vector **s**, where its elements are obtained through a combination of 3D
moments. We then analytically formulated the variation of **s** in response to the motions
to be applied to the manipulated points, once again approximating the object using a
MSM. From the equations we have developed to find this relationship, we established our
second control law for shaping the object to a desired configuration. We validated this
approach through simulations using synthetic soft objects, and experiments involving real
soft objects with two robotic manipulators.
# CONCLUSION

## 6.1 Conclusion

This PhD thesis delved into the field of robotic manipulation of soft objects, addressing various critical aspects and challenges associated with this field. The contributions and key findings from each chapter have collectively advanced our understanding and capabilities in controlling and manipulating deformable objects. Furthermore, there are numerous noteworthy applications to delve into in future work for a deeper exploration of the methodologies introduced in this thesis. These will be thoroughly examined in Section 6.2 and Section 6.3.

### 6.1.1 Indirect positioning of multiple points on a soft object using a simple mass-spring-model

In Chapter 3, we introduced our first contribution, which involves the analytical formulation of the displacement of feature points belonging to a deformable object in function of the motions of manipulated points and the object model parameters. In our approach, we approximated the object using a mass-spring model, with the model stiffness matrix and damping value being its physical parameters. The success and robustness of indirectly positioning multiple object points to their desired locations were demonstrated through simulation results. In the robustness study, we addressed several key aspects, including model density (whether a sparse or dense mesh was used) and testing the manipulation task using our approach with coarse model parameters instead of exact parameters. Additionally, we demonstrated the effectiveness of the deformation task under different conditions: the positioning task error converged to zero when the system is either fully-actuated or over-actuated, and it converged to a local minimum when the system is under-actuated. Furthermore, a comparative analysis with two state-of-the-art model-free methods underscored the effectiveness of our approach in accomplishing this task,
regardless of whether the manipulated points are near or far from the feature points and
whether external measurement noise is present or not.

In Chapter 4, we validated this first contribution through practical experimentations, involving real soft objects with varying geometries and material properties, two robotic manipulators, and a RGB-D camera. As an initial step, we used a methodology for reconstructing the geometric model of soft objects, enabling us to handle objects with diverse geometries. Subsequently, we presented a parameter estimation process that allows for the creation of a relative mass-spring model to represent the physical behavior of each object. Since the mass-spring model reproduces a coarse approximation of the real object behavior, there may be a drift between the model and the actual object deformations. To address this issue, we presented a correction method for this drift by tracking the real object using the RGB-D camera. This process realigns the simulation with the object actual state each time a new RGB-D image is acquired. These steps serve as a bridge from simulation results to real-world applications. Then, we validated the efficiency of our approach in indirectly positioning one or two feature points using robot arms. Furthermore, we tested the positioning task in both fully-actuated and over-actuated cases, considering the presence of external perturbations. These diverse experiments validate the applicability of our approach in real-world situations, thereby confirming the success of our first methodological contribution from both simulation and experimental results.

### 6.1.2 Shape servoing of a soft object using a simple mass-spring-model

In Chapter 5, we expanded upon our first contribution by introducing complete shape servoing of the soft object, which comprises the second and third contributions. The object shape is represented by its 2D contour when dealing with 2D objects and its complete 3D surface when dealing with 3D objects.

The second main contribution of this thesis involved representing the object shape using Fourier descriptors, which yields a low-dimensional feature vector. We then derived the analytical relation that links the variation of each vector element to the movements of the manipulated points. Based on this developed relations, we formulated a physics-based control law for deforming the shape of a soft object using Fourier descriptors. We successfully validated the accuracy of driving the 2D object contour to its desired contour
through simulation results. Furthermore, when dealing with 3D objects, the validation
of the approach for driving the object 3D surface to its desired 3D surface was achieved
through real experiments.

In the third contribution, the object shape was approximated using 3D moments. We derived analytically their variations in function of the displacements of the malipulated points and used this relationship in the design of the control law. In this approach, we considered only 3D objects and ensured the validity of this contribution by presenting both simulation and experimental results. In both cases, the desired shape of the object was successfully obtained.

## 6.2 Short-term perspectives

### 6.2.1 Advancing precision in deformable object manipulation -
Exploring advanced control strategies and model refine-
ment

Accurate modeling of deformable objects is crucial for achieving precision in deformation tasks. Soft objects exhibit complex behaviors that challenge traditional control approaches. Here are possible strategies for improving the models used in deformable object manipulation.

#### 6.2.1.1 Parameter tuning

To enhance our approach, we could begin by adjusting the parameters within the mass-spring model to better match the exact properties of the deformable object in question. These parameter adjustments may encompass variables such as spring constants, mass distribution, and damping coefficients, resulting in a more precise representation of the object behavior.

While the proposed control laws were designed for a general model that allows for varying stiffness between neighboring points and different point masses, our parameter estimation process assumed a uniform stiffness matrix between adjacent points and uniform mass distribution. Therefore, a promising direction for future research would be to refine our approach to model parameter estimation to accommodate model heterogeneity.

Additionally, addressing damping within the model is of interest. Selecting appropriate damping values for the springs between adjacent points could provide a more comprehen-
sive representation of real-world scenarios. However, it is important to note that modi-
fying the proposed control laws will be necessary to incorporate this choice of damping
adjustment.

6.2.1.2 Adaptive parameterization

Another interesting future research would involve the development of algorithms capable of adaptively parameterizing the mass-spring model. These algorithms could continuously adjust the model parameters based on the object response during manipulation, thereby accommodating variations in object properties. In this scenario, object tracking during deformation would be necessary, and model parameter estimation would be employed to update the model parameters, whether choosing a simple model, as used in this thesis, or a more complex model, as proposed in the preceding paragraph.

6.2.1.3 Enhancing deformable object manipulation: Beyond PID control

In this thesis, we used a Proportional controller, but it may not always be the most
suitable choice for deformable object manipulation. An alternative controller that could
be suited for this task is Model Predictive Control (MPC). MPC is a control strategy that
leverages a predictive model of the system to optimize control inputs over a finite time
horizon.

For shape servoing of soft objects, MPC formulates the control problem as an opti-
mization task and could rely on a mass-spring model to represent the dynamic of the
object. The objective function would typically include terms like the distance between
feature point positions and their desired locations or the norm of the difference between
shape parameters and their desired values, depending on the desired task. The goal is
to determine a sequence of control inputs, manipulating point motions, over a finite time
horizon to optimize the defined objective function.

At each time step, MPC could predict the future behavior of the system by simu-
lating the dynamic model using the current state and control inputs over the prediction
horizon. This prediction would account for how the deformable object will respond to
applied forces. Subsequently, MPC could optimize the control inputs over the prediction
horizon to minimize the objective function. Only the first control input from the opti-
mized sequence will be applied to the system, and this process will repeat at the next time
step. This iterative approach will allow the controller to adapt to changing conditions
and evolving deformable object behavior over time.
### 6.2.2 Complex scene - multiple objects

Another interesting future work is to implement multiple object modeling, rather than focusing on a single object. This would enable the deformation of objects within complex scenes, where the entities surrounding the object to be deformed are modeled and integrated into the deformation process.

We demonstrated the robustness of our approaches against possible external perturbations. When these perturbations originate from objects surrounding the target object, and by incorporating modeling of these objects, we could consider these perturbations in the control law. Ultimately, we would enable more precise deformations within complex scenes. The modeling procedure will not only focus on developing algorithms to simultaneously track the motion of different objects but also on modeling their interactions in a dynamic environment, including the detection and resolution of potential collisions between them.

## 6.3 Long-term perspectives

### 6.3.1 Multi-Modal sensing to enhance perception and precision in soft object manipulation

An interesting future work is to fuse tactile and vision sensors since many advantages should arise.

#### 6.3.1.1 Enhanced perception and sensing

Dealing with vision sensors poses various challenges, particularly in positioning the sensor relative to the deformable object. For successful object parameter estimation and tracking, the deformable object has to remain within the camera field of view. However, during object manipulation, the object may become occluded by the robot grippers. To mitigate this issue, we used thin sticks (see Figure 4.5) to prevent occlusion, but this may not always be feasible. In cases like the one shown in the tracking process (see Figure 4.4), the robot end-effector can cause significant occlusion, resulting in the loss of visual information. Consequently, interesting future work would involve integrating tactile sensors to provide information on contact forces. These tactile sensors would enable the recovery of visual information lost due to occlusion by the robot grippers. For example, the estimated
applied forces could be directly integrated into the object model as constraint forces
in (4.6), enabling the reproduction of the real deformation of the soft object.

6.3.1.2 Contact loss and slippage detection

During manipulation, another challenge such as contact loss between the soft object and the robotic end-effector may arise. While this contact loss cannot be easily detected using the vision sensor, tactile sensors would prove their utility in such cases. In case the tactile sensor does not register any applied force during the deformation, it could serve as an alert for contact loss. In such case, the deformation task may need to be repeated. Another solution would be to add constraints on the applied forces to prevent any contact loss.

Moreover, during manipulation, brief instances of slippage between the end-effector and the object can occur, often being unnoticed by the vision sensor. This poses a challenge for model-based approaches. Indeed, when the stick attached to the robotic manipulator makes initial contact with the object, the contact point is detected. We then establish a correspondence between the object model points and this contact point, determining the model node number, and consequently, defining the manipulated point. This correspondence remains constant, assuming that the contact point corresponds to the same model manipulated point throughout the deformation process. In cases of significant slippage, this initial correspondence must be re-evaluated, otherwise, accurate deformation may not be achieved.

In conclusion, tactile sensors provide information about contact forces and object
deformation, while vision sensors offer visual information about the object shape. In
future works, by integrating these two modalities, we could attain a more comprehensive
understanding of the objects being manipulated while surpassing the limitations of the
RGB-D sensor used.

6.3.2 Sensitive organ manipulation - Human-Machine collaboration

An interesting future surgical application would involve the delicate manipulation of sen-
sitive organs, marking a significant advancement in medical technology and techniques.
Given the sensitivity of these organs, a possible approach is to break down the deforma-
tion task into incremental steps, each requiring collaboration between a skilled medical
professional, typically a surgeon, and a machine that integrates the object model and
generates a precise control law. In this collaborative process, the surgeon assumes the responsibility of both monitoring the manipulation task and intervening when necessary. Simultaneously, a machine integrates the object model and generates the control law to meet the deformation task requirements, as previously discussed in this thesis.

This paragraph delves into the intricacies of this collaborative process, shedding light on how expertise and technology could merge to achieve desired outcomes in scenarios requiring sensitive organ manipulation. Moreover, to achieve controlled and incremental deformations, especially in scenarios involving sensitive organ manipulation, this method could comprise a multi-step process. Here is a proposition of what this multi-step process might look like:

1. Optimal trajectory planning: Before initiating each deformation step, an optimal trajectory planning algorithm calculates the ideal path and motion trajectory for the robotic system or surgical tool. This planning considers the organ characteristics, constraints, and desired deformation.

2. Integration with coarse model-based control law: The results of the optimal trajectory planning are integrated into a simple model-based control law. The trajectory planning provides a set of controlled points (feature points) and intermediate desired positions. This information is then integrated into the control law, which, in turn, determines the velocities to be applied by the surgical tool to ensure that the feature points follow the planned trajectory. This step can be accomplished using the proposed approach for an indirect positioning task.

3. Real-Time monitoring: The medical professional continually monitors the deformation process in real-time, closely observing how the actual deformation aligns with the planned trajectory. If the medical professional detects significant deviations from the desired outcome or perceives potential risks to the organ integrity, he can intervene.

4. Expert corrections: Drawing on the expertise of the medical professional and sensory feedback, the physicians makes necessary adjustments to the deformation strategy to rectify any discrepancies or issues within the specific procedural goals. For example, he can halt the deformation process, correct any deviations between the output of the controlled points and their desired positions generated by the path planning, and then resume it. In the latter case, we ensure that errors do not accumulate during the process for better precision.
5. Gradual deformation progression: With each iteration, the organ undergoes a con-
trolled deformation increment. This incremental approach continues until the overall
deformation goal is achieved. The aim of these incremental iterations is to ensure
that, at each step, the medical professional can detect any potential harm to the
organ, allowing for timely intervention based on their deep knowledge of organ be-
havior and anatomical considerations.

6. Ensuring safety and precision: Throughout the entire process, the combined efforts of trajectory planning, automated control inputs, and expert intervention maintain a delicate balance between safety and precision, ultimately achieving the desired organ deformation while prioritizing patient safety and optimal medical outcomes.

This multi-step approach reflects a cautious methodology for achieving controlled de-
formations in sensitive organ manipulation scenarios. It leverages both computational
models and human expertise to optimize the process while ensuring the highest standards
of patient safety and desired medical results.
## 7.1 Shape servoing based on 3D moments - Derivation of the interaction matrix and the feed-forward term

In this appendix, we derive analytically the required equations to obtain $L_s$ and $b_s$ as defined in (5.31). We can notice that these elements depend on $\frac{\partial g}{\partial \mu}$, $L_\mu$ and $b_\mu$.

We begin by calculating $L_\mu$ and $b_\mu$. By approximating the physical behavior of the object using the MSM, we obtain similarly to (5.5) the following equation for any surface point $P_{o_i}$, with $1 \le i \le w$:

$$ \dot{x}_{o_i} = \sum_{l=1}^{M} \mathbf{A}_{o_i m_l} (1, :) \dot{\mathbf{x}}_{m_l} + \mathbf{b}_{o_i} (1) \quad (7.1) $$

$$ \dot{y}_{o_i} = \sum_{l=1}^{M} \mathbf{A}_{o_i m_l} (2, :) \dot{\mathbf{x}}_{m_l} + \mathbf{b}_{o_i} (2) \quad (7.2) $$

$$ \dot{z}_{o_i} = \sum_{l=1}^{M} \mathbf{A}_{o_i m_l} (3, :) \dot{\mathbf{x}}_{m_l} + \mathbf{b}_{o_i} (3) \quad (7.3) $$

with $\mathbf{A}_{o_i m_l} (i, :)$ representing the $i^{th}$ row of the 3x3 matrix $\mathbf{A}_{o_i m_l}$. From the definition of $\mu_{ijk}$ given in (5.17), we obtain:

$$ \dot{\mu}_{ijk} = \sum_{l=1}^{w} i(x_{o_l} - x_g)^{(i-1)} (\dot{x}_{o_l} - \dot{x}_g) + j(y_{o_l} - y_g)^{(j-1)} (\dot{y}_{o_l} - \dot{y}_g) + k(z_{o_l} - z_g)^{(k-1)} (\dot{z}_{o_l} - \dot{z}_g) $$

which can be written using (7.1), (7.2) and (7.3) as follows:

$$ \dot{\mu}_{ijk} = L_{\mu_{ijk}} \dot{\mathbf{x}}_m + \mathbf{b}_{\mu_{ijk}} \quad (7.4) $$
with

$$L_{\mu_{ijk}} = \left[ \underbrace{L_{\mu_{ijk}}(1, 1:4)}_{1 \times 3} \dots \underbrace{L_{\mu_{ijk}}(1, 3p+1:3p+4)}_{1 \times 3} \dots \underbrace{L_{\mu_{ijk}}(1, 3M-2:3M+1)}_{1 \times 3} \right] \quad (7.5)$$

$$\mathbf{b}_{\mu_{ijk}} = \sum_{l=1}^{w} \left[ i(x_{o_l} - x_g)^{(i-1)} \left( \mathbf{b}_{o_l}(1) - \frac{1}{w} \sum_{q=1}^{w} \mathbf{b}_{o_q}(1) \right) \right. \\ \left. + j(y_{o_l} - y_g)^{(j-1)} \left( \mathbf{b}_{o_l}(2) - \frac{1}{w} \sum_{q=1}^{w} \mathbf{b}_{o_q}(2) \right) + k(z_{o_l} - z_g)^{(k-1)} \left( \mathbf{b}_{o_l}(3) - \frac{1}{w} \sum_{q=1}^{w} \mathbf{b}_{o_q}(3) \right) \right] \quad (7.6)$$

and

$$L_{\mu_{ijk}}(1, 3p+1:3p+4) = \sum_{l=1}^{w} \left[ \begin{aligned} & i(x_{o_l} - x_g)^{(i-1)} \left( A_{o_l m_{p+1}}(1, :) - \frac{1}{w} \sum_{q=1}^{w} A_{o_q m_{p+1}}(1, :) \right) \\ & + j(y_{o_l} - y_g)^{(j-1)} \left( A_{o_l m_{p+1}}(2, :) - \frac{1}{w} \sum_{q=1}^{w} A_{o_q m_{p+1}}(2, :) \right) \\ & + k(z_{o_l} - z_g)^{(k-1)} \left( A_{o_l m_{p+1}}(3, :) - \frac{1}{w} \sum_{q=1}^{w} A_{o_q m_{p+1}}(3, :) \right) \end{aligned} \right]$$

Then, $L_μ$ and $b_μ$ are obtained as follows:

$$L_{\mu} = [L_{\mu_{100}} \ L_{\mu_{010}} \ L_{\mu_{001}} \ L_{\mu_{110}} \ L_{\mu_{101}} \ L_{\mu_{011}} \ L_{\mu_{200}} \ L_{\mu_{020}} \ L_{\mu_{002}} \ L_{\mu_{300}} \ L_{\mu_{030}} \ L_{\mu_{003}} \ L_{\mu_{140}} \ L_{\mu_{014}} \ L_{\mu_{401}}]^T,$$

$$\mathbf{b}_{\mu} = [\mathbf{b}_{\mu_{100}} \ \mathbf{b}_{\mu_{010}} \ \mathbf{b}_{\mu_{001}} \ \mathbf{b}_{\mu_{110}} \ \mathbf{b}_{\mu_{101}} \ \mathbf{b}_{\mu_{011}} \ \mathbf{b}_{\mu_{200}} \ \mathbf{b}_{\mu_{020}} \ \mathbf{b}_{\mu_{002}} \ \mathbf{b}_{\mu_{300}} \ \mathbf{b}_{\mu_{030}} \ \mathbf{b}_{\mu_{003}} \ \mathbf{b}_{\mu_{140}} \ \mathbf{b}_{\mu_{014}} \ \mathbf{b}_{\mu_{401}}]^T,$$

with each element of $L_μ$ and $b_μ$ obtained directly from (7.5) and (7.6).

Returning back to $\frac{\partial g}{\partial \mu}$, its first eight rows that correspond to the first eight elements of $s$ (5.25) can be obtained as follows:

$$\frac{\partial \mathbf{g}}{\partial \boldsymbol{\mu}}(1, :) = [1/w, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],$$

$$\frac{\partial \mathbf{g}}{\partial \boldsymbol{\mu}}(2, :) = [0, 1/w, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],$$

$$\frac{\partial \mathbf{g}}{\partial \boldsymbol{\mu}}(3, :) = [0, 0, 1/w, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],$$

$$\frac{\partial \mathbf{g}}{\partial \boldsymbol{\mu}}(4, :) = [0, 0, 0, 0, 0, 0, 1/w^2, 1/w^2, 1/w^2, 0, 0, 0],$$
$$ \frac{\partial \mathbf{g}}{\partial \boldsymbol{\mu}}(5, :) = [0, 0, 0, 0, 0, 0, 0, 0, 1/w^2, 1/w^2, 1/w^2], $$

$$ \frac{\partial \mathbf{g}}{\partial \boldsymbol{\mu}}(6, :) = \left[ g_6 \quad g_6 \quad g_6 \quad 0 \quad 0 \quad 0 \quad -g_6 \frac{\cos\left(\frac{\phi}{3}\right)}{\sqrt{x_1}} \quad 0 \quad g_6 \frac{2\sqrt{x_1} \sin\left(\frac{\phi}{3}\right)}{3} \right] \mathbf{Q}, $$

$$ \frac{\partial \mathbf{g}}{\partial \boldsymbol{\mu}}(7, :) = \left[ g_7 \quad g_7 \quad g_7 \quad 0 \quad 0 \quad 0 \quad g_7 \frac{\cos\left(\frac{\phi-\pi}{3}\right)}{\sqrt{x_1}} \quad 0 \quad -g_7 \frac{2\sqrt{x_1} \sin\left(\frac{\phi-\pi}{3}\right)}{3} \right] \mathbf{Q}, $$

$$ \frac{\partial \mathbf{g}}{\partial \boldsymbol{\mu}}(8, :) = \left[ g_8 \quad g_8 \quad g_8 \quad 0 \quad 0 \quad 0 \quad -g_8 \frac{\cos\left(\frac{\phi+\pi}{3}\right)}{\sqrt{x_1}} \quad 0 \quad g_8 \frac{2\sqrt{x_1} \sin\left(\frac{\phi+\pi}{3}\right)}{3} \right] \mathbf{Q} $$

with

$$ g_6 = \frac{\sqrt{a + b + c - 2\sqrt{x_1} \cos\left(\frac{\phi}{3}\right)}}{2\sqrt{3w}(a + b + c - 2\sqrt{x_1} \cos\left(\frac{\phi}{3}\right))} $$

$$ g_7 = \frac{\sqrt{a + b + c + 2\sqrt{x_1} \cos\left(\frac{\phi-\pi}{3}\right)}}{2\sqrt{3w}(a + b + c + 2\sqrt{x_1} \cos\left(\frac{\phi-\pi}{3}\right))} $$

$$ g_8 = \frac{\sqrt{a + b + c - 2\sqrt{x_1} \cos\left(\frac{\phi+\pi}{3}\right)}}{2\sqrt{3w}(a + b + c - 2\sqrt{x_1} \cos\left(\frac{\phi+\pi}{3}\right))} $$

and $\mathbf{Q}$ is a 9x15 matrix, whose non-zero elements are given below:

$$ \mathbf{Q}(1, 8) = 1, \mathbf{Q}(1, 9) = 1, \mathbf{Q}(2, 7) = 1, \mathbf{Q}(2, 9) = 1, \mathbf{Q}(3, 7) = 1, \mathbf{Q}(3, 8) = 1, $$

$$ \mathbf{Q}(4, 4) = 1, \mathbf{Q}(5, 6) = -1, \mathbf{Q}(6, 5) = -1, \mathbf{Q}(7, 4) = 6\mu_{110}, \mathbf{Q}(7, 5) = 6\mu_{101}, $$

$$ \mathbf{Q}(7, 6) = 6\mu_{011}, \mathbf{Q}(7, 7) = -\mu_{002} - \mu_{020} + 2\mu_{200}, \mathbf{Q}(7, 8) = -\mu_{002} + 2\mu_{020} - \mu_{200} $$

$$ \mathbf{Q}(7, 9) = 2\mu_{002} - \mu_{020} - \mu_{200}, \mathbf{Q}(8, 4) = -54\mu_{011}\mu_{101} + 18\mu_{110}(-2\mu_{002} + \mu_{020} + \mu_{200}), $$

$$ \mathbf{Q}(8, 5) = -54\mu_{011}\mu_{110} + 18\mu_{101}(\mu_{002} - 2\mu_{020} + \mu_{200}), $$

$$ \mathbf{Q}(8, 6) = 18\mu_{011}(\mu_{002} + \mu_{020} - 2\mu_{200}) - 54\mu_{101}\mu_{110}, $$

$$ \begin{aligned} \mathbf{Q}(8, 7) = & -18\mu_{011}^2 + 9\mu_{101}^2 + 9\mu_{110}^2 + (-2\mu_{002} + \mu_{020} + \mu_{200})(-\mu_{002} - \mu_{020} + 2\mu_{200}) \\ & + 2(-2\mu_{002} + \mu_{020} + \mu_{200})(\mu_{002} - 2\mu_{020} + \mu_{200}) \\ & + (\mu_{002} - \mu_{020} + 2\mu_{200})(\mu_{002} - 2\mu_{020} + \mu_{200}), \end{aligned} $$

$$ \begin{aligned} \mathbf{Q}(8, 8) = & 9\mu_{011}^2 - 18\mu_{101}^2 + 9\mu_{110}^2 - 2(-2\mu_{002} + \mu_{020} + \mu_{200})(-\mu_{002} - \mu_{020} + 2\mu_{200}) \\ & - (-2\mu_{002} + \mu_{020} + \mu_{200})(\mu_{002} - 2\mu_{020} + \mu_{200}) \\ & + (-\mu_{002} - \mu_{020} + 2\mu_{200})(\mu_{002} - 2\mu_{020} + \mu_{200}), \end{aligned} $$

$$ \begin{aligned} \mathbf{Q}(8, 9) = & 9\mu_{011}^2 + 9\mu_{101}^2 - 18\mu_{110}^2 + (-2\mu_{002} + \mu_{020} + \mu_{200})(-\mu_{002} - \mu_{020} + 2\mu_{200}) \\ & - (-2\mu_{002} + \mu_{020} + \mu_{200})(\mu_{002} - 2\mu_{020} + \mu_{200}) \end{aligned} $$
$$-2(-\mu_{002} - \mu_{020} + 2\mu_{200})(\mu_{002} - 2\mu_{020} + \mu_{200}),$$

$$Q(9, :) = \frac{3x_2}{2x_1\sqrt{4x_1^3 - x_2^2}} Q(7, :) + \left( \frac{x_2^2}{4x_1^3\sqrt{4x_1^3 - x_2^2}} - \frac{\sqrt{4x_1^3 - x_2^2}}{4x_1^3} \right) Q(8, :)$$

Concerning the last three terms of $\frac{\partial g}{\partial \mu}$, a more complex calculation is required. Since they depend on the rotation matrix (5.18) that corresponds to the ellipsoid orientation, we begin by presenting the derivatives of the inertia matrix eigenvectors. Subsequently, we will obtain the derivative of the rotation matrix, and finally, we will derive the derivative of its $[\theta U]$ representation (5.20), denoted as $[\theta \dot{U}]$, which corresponds to the last three terms of $\frac{\partial g}{\partial \mu}$. The derivative of the inertia matrix eigenvectors is obtained from (5.29) as follows:

$$\dot{\mathbf{v}}_1(1) = \begin{bmatrix} \frac{1}{f}(v_{11} - v_{12} + \frac{1}{3}) \\ \frac{1}{f}(v_{11} + 2v_{12} + \frac{1}{3}) \\ \frac{1}{f}(-2v_{11} - v_{12} - \frac{2}{3}) \\ \frac{1}{f}(-3v_{12}\frac{e}{f} - v_{14}) \\ \frac{1}{f}(3v_{11}\frac{f}{d} - 3v_{12}\frac{d}{f} - \frac{v_{12}}{v_{11}}\frac{d}{f}) \\ \frac{1}{f^2}[3v_{11}\frac{ef}{d} - v_{12}v_{13} - \frac{1}{3}v_{15}] \\ \frac{\cos(\frac{\phi}{3})}{f\sqrt{x_1}}(-v_{11} + v_{12} - \frac{1}{3}) \\ \frac{2}{3f}\sin(\frac{\phi}{3})\sqrt{x_1}(v_{11} - v_{12} + \frac{1}{3}) \end{bmatrix}, \quad \mathbf{Q}, \quad \dot{\mathbf{v}}_1(2) = \begin{bmatrix} \frac{1}{e}(-v_{11} + v_{12}) \\ \frac{1}{e}(-v_{11} - 2v_{12}) \\ \frac{1}{e}(2v_{11} + v_{12}) \\ \frac{3}{f}v_{12} + \frac{1}{e}v_{14} \\ -\frac{3f}{de}v_{11} + \frac{3d}{ef}v_{12} \\ -\frac{3}{d}v_{11} + \frac{v_{12}v_{13}}{ef} \\ -\frac{v_{12}\cos(\frac{\phi}{3})}{e\sqrt{x_1}} + \frac{d\cos(\frac{\phi}{3})}{3\sqrt{x_1}v_{16}} \\ 0 \\ 2\sqrt{x_1}\sin(\frac{\phi}{3})\left(-\frac{d}{9v_{16}} + \frac{fv_{17}}{9v_{16}^2}\right) \end{bmatrix}, \quad \mathbf{Q}$$

$$\dot{\mathbf{v}}_2(1) = \begin{bmatrix} \frac{1}{f}(v_{21} - v_{22} + \frac{1}{3}) \\ \frac{1}{f}(v_{21} + 2v_{22} + \frac{1}{3}) \\ \frac{1}{f}(-2v_{21} - v_{22} - \frac{2}{3}) \\ \frac{1}{f}(-3v_{22}\frac{e}{f} - v_{24}) \\ \frac{1}{f}(3v_{21}\frac{f}{d} - 3v_{22}\frac{d}{f} - \frac{v_{22}}{v_{21}}\frac{d}{f}) \\ \frac{1}{f^2}[3v_{21}\frac{ef}{d} - v_{22}v_{23} - \frac{1}{3}v_{25}] \\ \frac{\cos(\frac{\pi-\phi}{3})}{f\sqrt{x_1}}(v_{21} - v_{22} + \frac{1}{3}) \\ \frac{2}{3f}\sin(\frac{\pi-\phi}{3})\sqrt{x_1}(v_{21} - v_{22} + \frac{1}{3}) \end{bmatrix}, \quad \mathbf{Q}, \quad \dot{\mathbf{v}}_2(2) = \begin{bmatrix} \frac{1}{e}(-v_{21} + v_{22}) \\ \frac{1}{e}(-v_{21} - 2v_{22}) \\ \frac{1}{e}(2v_{21} + v_{22}) \\ \frac{3}{f}v_{22} + \frac{1}{e}v_{24} \\ -\frac{3f}{de}v_{21} + \frac{3d}{ef}v_{22} \\ -\frac{3}{d}v_{21} + \frac{v_{22}v_{23}}{ef} \\ \frac{v_{22}\cos(\frac{\pi-\phi}{3})}{e\sqrt{x_1}} - \frac{d\cos(\frac{\pi-\phi}{3})}{3\sqrt{x_1}v_{26}} \\ 0 \\ 2\sqrt{x_1}\sin(\frac{\pi-\phi}{3})\left(-\frac{d}{9v_{26}} + \frac{fv_{27}}{9v_{26}^2}\right) \end{bmatrix}, \quad \mathbf{Q}$$
$$
\begin{aligned}
\dot{\mathbf{v}}_3(1) &=
\begin{bmatrix}
\frac{1}{f}(v_{31} - v_{32} + \frac{1}{3}) \\
\frac{1}{f}(v_{31} + 2v_{32} + \frac{1}{3}) \\
\frac{1}{f}(-2v_{31} - v_{32} - \frac{2}{3}) \\
\frac{1}{f}(-3v_{32} \frac{e}{f} - v_{34}) \\
\frac{1}{f}(3v_{31} \frac{f}{d} - 3v_{32} \frac{d}{f} - \frac{v_{32} \frac{d}{f}}{v_{31} f}) \\
\frac{1}{f^2}[3v_{31} \frac{ef}{d} - v_{32}v_{33} - \frac{1}{3}v_{35}] \\
\frac{\cos(\frac{\pi+\phi}{3})}{f\sqrt{x_1}}(-v_{31} + v_{32} - \frac{1}{3}) \\
0 \\
\frac{2}{3f}\sin(\frac{\pi+\phi}{3})\sqrt{x_1}(v_{31} - v_{32} + \frac{1}{3})
\end{bmatrix}
,
&
\mathbf{Q},
&
\dot{\mathbf{v}}_3(2) &=
\begin{bmatrix}
\frac{1}{e}(-v_{31} + v_{32}) \\
\frac{1}{e}(-v_{31} - 2v_{32}) \\
\frac{1}{e}(2v_{31} + v_{32}) \\
\frac{3}{f}v_{32} + \frac{1}{e}v_{34} \\
-\frac{3f}{de}v_{31} + \frac{3d}{ef}v_{32} \\
-\frac{3}{d}v_{31} + \frac{v_{32}v_{33}}{ef} \\
-\frac{v_{32}\cos(\frac{\pi+\phi}{3})}{e\sqrt{x_1}} + \frac{d\cos(\frac{\pi+\phi}{3})}{3\sqrt{x_1}v_{36}} \\
0 \\
2\sqrt{x_1}\sin(\frac{\pi+\phi}{3})\left(-\frac{d}{9v_{36}} + \frac{fv_{37}}{9v_{36}^2}\right)
\end{bmatrix}
,
&
\mathbf{Q}
&
\dot{\mathbf{v}}_1(3) = \dot{\mathbf{v}}_2(3) = \dot{\mathbf{v}}_3(3) = \mathbf{0}
\end{aligned}
$$

with

$$
\begin{aligned}
v_{11} &= \frac{de}{-3de + f \left(-a + 2b - c + 2\sqrt{x_1} \cos\left(\frac{\phi}{3}\right)\right)} \\
v_{12} &= \frac{ef \left(d \left(-a - b + 2c + 2\sqrt{x_1} \cos\left(\frac{\phi}{3}\right)\right) - 3ef\right)}{\left(-3de + f \left(-a + 2b - c + 2\sqrt{x_1} \cos\left(\frac{\phi}{3}\right)\right)\right)^2}
\end{aligned}
$$

$$
v_{13} = a - 2b + c - 2\sqrt{x_1} \cos\left(\frac{\phi}{3}\right), \quad v_{16} = \frac{de}{3v_{11}}
$$

$$
v_{14} = \frac{e \left(-a - b + 2c + 2\sqrt{x_1} \cos\left(\frac{\phi}{3}\right)\right)}{-3de + f \left(-a + 2b - c + 2\sqrt{x_1} \cos\left(\frac{\phi}{3}\right)\right)}
$$

$$
v_{15} = \frac{a}{3} + \frac{b}{3} - 2\frac{c}{3} - \frac{v_{12}ed}{v_{11}f} - 2\sqrt{x_1}\cos\left(\frac{\phi}{3}\right)
$$

$$
v_{17} = -ef + \frac{d}{3} \left( -a - b + 2c + 2\sqrt{x_1} \cos\left(\frac{\phi}{3}\right) \right)
$$

$$
\begin{aligned}
v_{21} &= \frac{de}{-3de + f \left(-a + 2b - c - 2\sqrt{x_1} \cos\left(\frac{\pi-\phi}{3}\right)\right)} \\
v_{22} &= \frac{ef \left(d \left(-a - b + 2c - 2\sqrt{x_1} \cos\left(\frac{\pi-\phi}{3}\right)\right) - 3ef\right)}{\left(-3de + f \left(-a + 2b - c - 2\sqrt{x_1} \cos\left(\frac{\pi-\phi}{3}\right)\right)\right)^2}
\end{aligned}
$$

$$
v_{23} = a - 2b + c + 2\sqrt{x_1} \cos\left(\frac{\pi - \phi}{3}\right), \quad v_{26} = \frac{de}{3v_{21}}
$$

$$
v_{24} = \frac{e \left(-a - b + 2c - 2\sqrt{x_1} \cos\left(\frac{\pi-\phi}{3}\right)\right)}{-3de + f \left(-a + 2b - c - 2\sqrt{x_1} \cos\left(\frac{\pi-\phi}{3}\right)\right)}
$$
$$v_{25} = \frac{a}{3} + \frac{b}{3} - 2\frac{c}{3} - \frac{v_{22}ed}{v_{21}f} + 2\sqrt{x_1}\cos\left(\frac{\pi-\phi}{3}\right)$$

$$v_{27} = -ef + \frac{d}{3} \left( -a - b + 2c - 2\sqrt{x_1} \cos\left(\frac{\phi}{3}\right) \right)$$

$$v_{31} = \frac{de}{-3de + f \left(-a + 2b - c + 2\sqrt{x_1} \cos\left(\frac{\pi+\phi}{3}\right)\right)}$$

$$v_{32} = \frac{ef \left(d \left(-a - b + 2c + 2\sqrt{x_1} \cos\left(\frac{\pi+\phi}{3}\right)\right) - 3ef \right)}{\left(-3de + f \left(-a + 2b - c + 2\sqrt{x_1} \cos\left(\frac{\pi+\phi}{3}\right)\right)\right)^2}$$

$$v_{33} = a - 2b + c - 2\sqrt{x_1} \cos\left(\frac{\pi+\phi}{3}\right), \quad v_{36} = \frac{de}{3v_{31}}$$

$$v_{14} = \frac{e \left(-a - b + 2c + 2\sqrt{x_1} \cos\left(\frac{\phi}{3}\right)\right)}{-3de + f \left(-a + 2b - c + 2\sqrt{x_1} \cos\left(\frac{\phi}{3}\right)\right)}$$

$$v_{35} = \frac{a}{3} + \frac{b}{3} - 2\frac{c}{3} - \frac{v_{32}ed}{v_{31}f} - 2\sqrt{x_1} \cos\left(\frac{\pi+\phi}{3}\right)$$

$$v_{17} = -ef + \frac{d}{3} \left( -a - b + 2c + 2\sqrt{x_1} \cos\left(\frac{\pi+\phi}{3}\right) \right)$$

Next, from (5.18) we obtain the derivative of the rotation matrix as:

$$\dot{\mathbf{R}} = \begin{pmatrix} \frac{\dot{v}_1(1)}{\|v_1\|} - \frac{v_1(1)\|\dot{v}_1\|}{\|v_1\|^2} & \frac{\dot{v}_2(1)}{\|v_2\|} - \frac{v_2(1)\|\dot{v}_2\|}{\|v_2\|^2} & \frac{\dot{v}_3(1)}{\|v_3\|} - \frac{v_3(1)\|\dot{v}_3\|}{\|v_3\|^2} \\ \frac{\dot{v}_1(2)}{\|v_1\|} - \frac{v_1(2)\|\dot{v}_1\|}{\|v_1\|^2} & \frac{\dot{v}_2(2)}{\|v_2\|} - \frac{v_2(2)\|\dot{v}_2\|}{\|v_2\|^2} & \frac{\dot{v}_3(2)}{\|v_3\|} - \frac{v_3(2)\|\dot{v}_3\|}{\|v_3\|^2} \\ -\frac{\|v_1\|}{\|v_1\|^2} & -\frac{\|v_2\|}{\|v_2\|^2} & -\frac{\|v_3\|}{\|v_3\|^2} \end{pmatrix} \mathbf{R}^{*T} \quad (7.7)$$

with

$$\|\dot{v}_1\| = \frac{v_1(1)\dot{v}_1(1) + v_1(2)\dot{v}_1(2)}{\|v_1\|}, \quad \|\dot{v}_2\| = \frac{v_2(1)\dot{v}_2(1) + v_2(2)\dot{v}_2(2)}{\|v_2\|}$$

$$\|\dot{v}_3\| = \frac{v_3(1)\dot{v}_3(1) + v_3(2)\dot{v}_3(2)}{\|v_3\|}$$

We can simplify this expression as follows:

$$\dot{\mathbf{R}} = \begin{pmatrix} g_{\tilde{R}}(1, 1)Q & g_{\tilde{R}}(1, 2)Q & g_{\tilde{R}}(1, 3)Q \\ g_{\tilde{R}}(2, 1)Q & g_{\tilde{R}}(2, 2)Q & g_{\tilde{R}}(2, 3)Q \\ g_{\tilde{R}}(3, 1)Q & g_{\tilde{R}}(3, 2)Q & g_{\tilde{R}}(3, 3)Q \end{pmatrix} \quad (7.8)$$

with $g_{\tilde{R}}$ obtained by identifying (7.8) with (7.7). Furthermore, calculating the derivative
of $[\dot{\theta}\dot{U}]$ requires $\dot{\theta}$, which is obtained from (5.21) as:

$$ \dot{\theta} = - \left[ \frac{(g_{\tilde{R}}(1,1) + g_{\tilde{R}}(2,2) + g_{\tilde{R}}(3,3))}{\sin(\theta)} \right] Q $$

Finally, we get from (5.22):

$$ [\dot{\theta}\dot{U}]_x = \frac{\left(\dot{\tilde{R}}(3,2) - \dot{\tilde{R}}(2,3)\right) 2\sin(\theta) - 2\dot{\theta}\cos(\theta) \left(\tilde{R}(3,2) - \tilde{R}(2,3)\right)}{4\sin^2(\theta)} $$

from which we obtain:

$$ \frac{\partial g}{\partial \mu}(9, :) = \frac{g_{\tilde{R}}(3,2) - g_{\tilde{R}}(2,3)}{2\sin(\theta)} Q + \frac{\cos(\theta) (g_{\tilde{R}}(1,1) + g_{\tilde{R}}(2,2) + g_{\tilde{R}}(3,3)) (\tilde{R}(3,2) - \tilde{R}(2,3))}{2 \sin^3(\theta)} Q $$

Similarly, by calculating $[\dot{\theta}\dot{U}]_y$ and $[\dot{\theta}\dot{U}]_z$, we obtain:

$$ \frac{\partial g}{\partial \mu}(10, :) = \frac{g_{\tilde{R}}(1,3) - g_{\tilde{R}}(3,1)}{2\sin(\theta)} Q + \frac{\cos(\theta) (g_{\tilde{R}}(1,1) + g_{\tilde{R}}(2,2) + g_{\tilde{R}}(3,3)) (\tilde{R}(1,3) - \tilde{R}(3,1))}{2 \sin^3(\theta)} Q $$

$$ \frac{\partial g}{\partial \mu}(11, :) = \frac{g_{\tilde{R}}(2,1) - g_{\tilde{R}}(1,2)}{2\sin(\theta)} Q + \frac{\cos(\theta) (g_{\tilde{R}}(1,1) + g_{\tilde{R}}(2,2) + g_{\tilde{R}}(3,3)) (\tilde{R}(2,1) - \tilde{R}(1,2))}{2 \sin^3(\theta)} Q $$

In conclusion, in this appendix we developed analytically all the rows of the matrix $\frac{\partial g}{\partial \mu}$, which can be concatenated to form the entire matrix $\frac{\partial g}{\partial \mu}$. Furthermore, earlier in this appendix, we analytically derived $L_\mu$ (7.5) and $b_\mu$ (7.6). Therefore, the interaction matrix $L_s$ that relates the variation of the low-dimensional feature vector $s$, given in (5.25), as a function of the manipulated points $\dot{x}_m$, can be analytically calculated using: $L_s = \frac{\partial g}{\partial \mu} L_\mu$. Moreover, the term representing the model inertia, $b_s$, is also analytically obtained as: $b_s = \frac{\partial g}{\partial \mu} b_\mu$, which concludes our development.
BIBLIOGRAPHY

Aranda, M., Ramon, J. A. C., Mezouar, Y., Bartoli, A., & Özgür, E., (2020), Monocular visual shape tracking and servoing for isometrically deforming objects, in *2020 ieee/rsj international conference on intelligent robots and systems (iros)*, IEEE.

Arriola-Rios, V. E., & Wyatt, J. L., (2017), A multimodal model of object deformation under robotic pushing, *IEEE Transactions on Cognitive and Developmental Systems*, 9(2), 153–169.

Barber, C. B., Dobkin, D. P., & Huhdanpaa, H., (1996), The quickhull algorithm for convex hulls, *ACM Transactions on Mathematical Software*, 22(4), 469–483.

Basdogan, C., (2001), Real-time simulation of dynamically deformable finite element models using modal analysis and spectral lanczos decomposition methods. in *Medicine meets virtual reality 2001* (pp. 46–52), IOS Press.

Bender, J., Müller, M., & Macklin, M., (2017), A survey on position based dynamics, 2017, *Proceedings of the European Association for Computer Graphics: Tutorials*, 1–31.

Bhasin, Y., & Liu, A., (2006), Bounds for damping that guarantee stability in mass-spring systems, *Studies in health technology and informatics*, 119, 55–60.

Bianchi, G., Solenthaler, B., Székely, G., & Harders, M., (2004), Simultaneous topology and stiffness identification for mass-spring models based on fem reference deformations, in *Medical image computing and computer-assisted intervention-miccai 2004: 7th international conference, saint-malo, france, september 26-29, 2004. proceedings, part ii 7*, Springer.

Bickel, B., Bächer, M., Otaduy, M. A., Matusik, W., Pfister, H., & Gross, M., (2009), Capture and modeling of non-linear heterogeneous soft tissue, *ACM transactions on graphics (TOG)*, 28(3), 1–9.

Bourguignon, D., & Cani, M.-P., (2000), Controlling anisotropy in mass-spring systems, in *Computer animation and simulation 2000: proceedings of the eurographics workshop in interlaken, switzerland, august 21-22, 2000*, Springer.

Broyden, C., (2000), On the discovery of the “good broyden” method, *Mathematical programming*, 87, 209–213.
Caccamo, S., Bekiroglu, Y., Ek, C. H., & Kragic, D., (2016), Active exploration using gaussian random fields and gaussian process implicit surfaces, in *2016 ieee/rsj international conference on intelligent robots and systems (iros)*, IEEE.

Chaumette, F., & Hutchinson, S. A., (2008), Visual servoing and visual tracking, in *Springer handbook of robotics*.

Chi, C., & Berenson, D., (2019), Occlusion-robust deformable object tracking without physics simulation, in *2019 ieee/rsj international conference on intelligent robots and systems (iros)*, IEEE.

Clegg, A., Yu, W., Erickson, Z., Tan, J., Liu, C. K., & Turk, G., (2017), Learning to navigate cloth using haptics, in *Ieee/rsj int. conf. on intelligent robots and systems (iros'17)*.

Coevoet, E., Reynaert, N., Lartigau, E., Schiappacasse, L., Dequidt, J., & Duriez, C., (2015), Registration by interactive inverse simulation: application for adaptive radiotherapy, *International journal of computer assisted radiology and surgery, 10*, 1193-1200.

Das, J., & Sarkar, N., (2011), Autonomous shape control of a deformable object by multiple manipulators, *Journal of Intelligent & Robotic Systems, 62*(1), 3-27.

Deledalle, C.-A., Denis, L., Tabti, S., & Tupin, F., (2017), *Closed-form expressions of the eigen decomposition of 2 x 2 and 3 x 3 hermitian matrices* (Doctoral dissertation), Université de Lyon.

de Mathelin, M., & Lozano, R., (1999), Robust adaptive identification of slowly time-varying parameters with bounded disturbances, *Automatica*, **35**(7), 1291-1305.

Dou, M., Davidson, P., Fanello, S. R., Khamis, S., Kowdle, A., Rhemann, C., Tankovich, V., & Izadi, S., (2017), Motion2fusion: real-time volumetric performance capture, *ACM Transactions on Graphics (ToG)*, **36**(6), 1-16.

Duan, Y., Huang, W., Chang, H., Chen, W., Zhou, J., Teo, S. K., Su, Y., Chui, C. K., & Chang, S., (2014), Volume preserved mass-spring model with novel constraints for soft tissue deformation, *IEEE journal of biomedical and health informatics*, **20**(1), 268-280.

Einarsdóttir, H., Guðmundsson, B., & Ómarsson, V., (2022), Automation in the fish industry, *Animal Frontiers*, **12**(2), 32-39.

Etzmuss, O., Gross, J., & Strasser, W., (2003), Deriving a particle system from continuum mechanics for the animation of deformable objects, *IEEE Transactions on Visualization and Computer Graphics*, **9**(4), 538-550.
Ficuciello, F., Migliozzi, A., Coevoet, E., Petit, A., & Duriez, C., (2018), Fem-based deformation control for dexterous manipulation of 3d soft objects, in *2018 ieee/rsj international conference on intelligent robots and systems (iros)*, IEEE.

Frank, B., Schmedding, R., Stachniss, C., Teschner, M., & Burgard, W., (2010), Learning the elasticity parameters of deformable objects with a manipulation robot, in *2010 ieee/rsj international conference on intelligent robots and systems*, IEEE.

Frank, B., Stachniss, C., Schmedding, R., Teschner, M., & Burgard, W., (2014), Learning object deformation models for robot motion planning, *Robotics and Autonomous Systems*, 62(8), 1153–1174.

Galvez, J. M., & Canton, M., (1993), Normalization and shape recognition of three-dimensional objects by 3d moments, *Pattern Recognition*, 26(5), 667–681.

Greminger, M. A., & Nelson, B. J., (2008), A deformable object tracking algorithm based on the boundary element method that is robust to occlusions and spurious edges, *International Journal of Computer Vision*, 78, 29–45.

Güler, P., Pieropan, A., Ishikawa, M., & Kragic, D., (2017), Estimating deformability of objects using meshless shape matching, in *2017 ieee/rsj international conference on intelligent robots and systems (iros)*, IEEE.

Hadfield-Menell, D., Lee, A. X., Finn, C., Tzeng, E., Huang, S., & Abbeel, P., (2015), Beyond lowest-warping cost action selection in trajectory transfer, in *Ieee int. conf. on robotics and automation (icra'15)*.

Han, T., Zhao, X., Sun, P., & Pan, J., (2018), Robust shape estimation for 3d deformable object manipulation, *arXiv preprint arXiv:1809.09802*.

Hauser, K. K., Shen, C., & O'Brien, J. F., (2003), Interactive deformation using modal analysis with constraints.

Hauth, M., Etzmuß, O., & Straßer, W., (2003), Analysis of numerical methods for the simulation of deformable models, *The Visual Computer*, 19, 581–600.

Higashimori, M., Yoshimoto, K., & Kaneko, M., (2010), Active shaping of an unknown rheological object based on deformation decomposition into elasticity and plasticity, in *2010 ieee international conference on robotics and automation*, IEEE.

Hosoda, K., & Asada, M., (1994), Versatile visual servoing without knowledge of true jacobian, in *Proceedings of ieee/rsj international conference on intelligent robots and systems (iros'94)*, https://doi.org/10.1109/IROS.1994.407392

Hu, M.-K., (1962), Visual pattern recognition by moment invariants, *IRE transactions on information theory*, 8(2), 179–187.
Hu, Z., Han, T., Sun, P., Pan, J., & Manocha, D., (2019), 3-d deformable object manipulation using deep neural networks, *IEEE Robotics and Automation Letters*, **4**(4), 4255-4261.

Jägersand, M., & Nelson, R., (1996), On-line estimation of visual-motor models using active vision, *image*, **11**, 1, https://apisemanticscholar.org/CorpusID:6886542

James, D. L., & Pai, D. K., (1999), Artdefo: accurate real time deformable objects, in *Proceedings of the 26th annual conference on computer graphics and interactive techniques*.

Kazhdan, M., Bolitho, M., & Hoppe, H., (2006), Poisson surface reconstruction, in *4th eurographics symp. on geometry processing*.

Kinio, S., & Patriciu, A., (2012), A comparative study of $h\infty$ and pid control for indirect deformable object manipulation, in *Ieee int. conf. on robotics and biomimetics (robio)*, IEEE.

Lagneau, R., Krupa, A., & Marchal, M., (2020a), Active deformation through visual servoing of soft objects, in *2020 iee international conference on robotics and automation (icra)*, IEEE.

Lagneau, R., Krupa, A., & Marchal, M., (2020b), Automatic shape control of deformable wires based on model-free visual servoing, *IEEE Robotics and Automation Letters*, **5**(4), 5252-5259.

Lehnert, C., English, A., McCool, C., Tow, A. W., & Perez, T., (2017), Autonomous sweet pepper harvesting for protected cropping systems, *IEEE Robotics and Automation Letters*, **2**(2), 872-879, https://doi.org/10.1109/LRA.2017.2655622

Levi, Z., & Gotsman, C., (2014), Smooth rotation enhanced as-rigid-as-possible mesh animation, *IEEE transactions on visualization and computer graphics*, **21**(2), 264-277.

Lloyd, B., Székely, G., & Harders, M., (2007), Identification of spring parameters for deformable object simulation, *IEEE Trans. on Visualization and Computer Graphics*, **13**, 1081-94.

Macklin, M., Müller, M., Chentanez, N., & Kim, T.-Y., (2014), Unified particle physics for real-time applications, *ACM Transactions on Graphics (TOG)*, **33**(4), 1-12.

Makiyeh, F., Chaumette, F., Marchal, M., & Krupa, A., (2023), Shape Servoing of a Soft Object Using Fourier Series and a Physics-based Model, in *IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, IROS'23*, Detroit, USA.
Makiyeh, F., Marchal, M., Chaumette, F., & Krupa, A., (2022), Indirect positioning of a 3d point on a soft object using rgb-d visual servoing and a mass-spring model, in *2022 17th international conference on control, automation, robotics and vision (icarcv)*, https://doi.org/10.1109/ICARCV57592.2022.10004285

Massoud Farahmand, A., Shademan, A., & Jagersand, M., (2007), Global visual-motor estimation for uncalibrated visual servoing, in *2007 ieee/rsj international conference on intelligent robots and systems*, IEEE.

Moré, J. J., (2006), The levenberg-marquardt algorithm: implementation and theory, in *Numerical analysis: proceedings of the biennial conference held at dundee, june 28-july 1, 1977*, Springer.

Morris, D., & Salisbury, K., (2008), Automatic preparation, calibration, and simulation of deformable objects, *Computer methods in biomechanics and biomedical engineering*, 11(3), 263-279.

Müller, M., & Gross, M. H., (2004), Interactive virtual materials., in *Graphics interface*.

Myronenko, A., & Song, X., (2010), Point set registration: coherent point drift, *IEEE transactions on pattern analysis and machine intelligence*, 32(12), 2262-2275.

Natsupakpong, S., & Çavuşoğlu, M. C., (2010), Determination of elasticity parameters in lumped element (mass-spring) models of deformable objects, *Graphical Models*, 72(6), 61-73.

Navarro-Alarcon, D., & Liu, Y.-H., (2017), Fourier-based shape servoing: a new feedback method to actively deform soft objects into desired 2-d image contours, *IEEE Transactions on Robotics*, 34(1), 272-279.

Navarro-Alarcon, D., Liu, Y.-H., Romero, J. G., & Li, P., (2014), On the visual deformation servoing of compliant objects: uncalibrated control methods and experiments, *The International Journal of Robotics Research*, 33(11), 1462-1480.

Navarro-Alarcon, D., Liu, Y.-H., Romero, J. G., & Li, P., (2013), Model-free visually servoed deformation control of elastic objects by robot manipulators, *IEEE Transactions on Robotics*, 29(6), 1457-1468.

Navarro-Alarcon, D., Yip, H. M., Wang, Z., Liu, Y.-H., Zhong, F., Zhang, T., & Li, P., (2016), Automatic 3-d manipulation of soft objects by robotic arms with an adaptive deformation model, *IEEE Transactions on Robotics*, 32(2), 429-441.

Nelder, J. A., & Mead, R., (1965), A simplex method for function minimization, *The computer journal*, 7(4), 308-313.
Nurnberger, A., Radetzky, A., & Kruse, R., (1998), A problem specific recurrent neural network for the description and simulation of dynamic spring models, in *1998 international joint conference on neural networks proceedings. ieee world congress on computational intelligence* (cat. no. 98ch36227), IEEE.

Papanikolopoulos, N. P., Khosla, P. K., & Kanade, T., (1993), Visual tracking of a moving target by a camera mounted on a robot: a combination of control and vision, *IEEE transactions on robotics and automation*, 9(1), 14–35.

Pentland, A., & Williams, J., (1989), Good vibrations: modal dynamics for graphics and animation, in *Proceedings of the 16th annual conference on computer graphics and interactive techniques*.

Petit, A., Cotin, S., Lippiello, V., & Siciliano, B., (2018), Capturing deformations of interacting non-rigid objects using rgb-d data, in *2018 ieee/rsj international conference on intelligent robots and systems (iros)*, IEEE.

Petit, A., Ficuciello, F., Fontanelli, G. A., Villani, L., & Siciliano, B., (2017), Using physical modeling and rgb-d registration for contact force sensing on deformable objects, in *Icinco 2017-14th international conference on informatics in control, automation and robotics*, ScitePress; Springer.

Petit, A., Lippiello, V., Fontanelli, G. A., & Siciliano, B., (2017), Tracking elastic deformable objects with an rgb-d sensor for a pizza chef robot, *Robotics and Autonomous Systems*, 88, 187–201.

Petit, A., Lippiello, V., & Siciliano, B., (2015), Real-time tracking of 3d elastic objects with an rgb-d sensor, in *2015 ieee/rsj international conference on intelligent robots and systems (iros)*, IEEE.

Qi, J., Ma, G., Zhu, J., Zhou, P., Lyu, Y., Zhang, H., & Navarro-Alarcon, D., (2021), Contour moments based manipulation of composite rigid-deformable objects with finite time model estimation and shape/position control, *IEEE/ASME Transactions on Mechatronics*, 27(5), 2985–2996.

Romeo, M., Monteagudo, C., & Sánchez-Quirós, D., (2020), Muscle and fascia simulation with extended position based dynamics, in *Computer graphics forum*, Wiley Online Library.

Royer, L., Krupa, A., Dardenne, G., Le Bras, A., Marchand, E., & Marchal, M., (2017), Real-time target tracking of soft tissues in 3d ultrasound images based on robust visual information and mechanical simulation, *Medical image analysis*, 35, 582–598.
Rusu, R. B., & Cousins, S., (2011), 3d is here: point cloud library (pcl), in *Ieee international conference on robotics and automation (icra)*.

Schulman, J., Gupta, A., Venkatesan, S., Tayson-Frederick, M., & Abbeel, P., (2013), A case study of trajectory transfer through non-rigid registration for a simplified suturing scenario, in *Ieee/rsj int. conf. on intelligent robots and systems (iros'13)*.

Schulman, J., Lee, A., Ho, J., & Abbeel, P., (2013), Tracking deformable objects with point clouds, in *2013 ieee international conference on robotics and automation*, IEEE.

Sengupta, A., Krupa, A., & Marchand, E., (2019), Tracking of non-rigid objects using rgb-d camera, in *2019 ieee international conference on systems, man and cybernetics (smc)*, IEEE.

Sengupta, A., Lagneau, R., Krupa, A., Marchand, E., & Marchal, M., (2020), Simultaneous tracking and elasticity parameter estimation of deformable objects, in *2020 ieee international conference on robotics and automation (icra)*, IEEE.

Shademan, A., Farahmand, A.-M., & Jägersand, M., (2010), Robust jacobian estimation for uncalibrated visual servoing, in *2010 ieee international conference on robotics and automation*, IEEE.

Shah, A. J., & Shah, J. A., (2016), Towards manipulation planning for multiple interlinked deformable linear objects, in *Ieee int. conf. on robotics and automation (icra'16)*, https://doi.org/10.1109/ICRA.2016.7487580

Shahamiri, M., & Jagersand, M., (2005), Uncalibrated visual servoing using a biased newton method for on-line singularity detection and avoidance, in *2005 ieee/rsj international conference on intelligent robots and systems*, IEEE.

Shetab-Bushehri, M., Aranda, M., Mezouar, Y., & Özgür, E., (2022), As-rigid-as-possible shape servoing, *IEEE Robotics and Automation Letters*, 7(2), 3898-3905.

Shibata, M., & Hirai, S., (2006), Soft object manipulation by simultaneous control of motion and deformation, in *Ieee int. conf. on robotics and automation (icra)*.

Shin, C., Ferguson, P. W., Pedram, S. A., Ma, J., Dutson, E. P., & Rosen, J., (2019), Autonomous tissue manipulation via surgical robot using learning based model predictive control, in *Ieee int. conf. on robotics and automation (icra'19)*.

Sorkine, O., & Alexa, M., (2007), As-rigid-as-possible surface modeling, in *Symposium on geometry processing*, Citeseer.
Taha, A. A., & Hanbury, A., (2015), An efficient algorithm for calculating the exact hausdorff distance, *IEEE Trans. on Pattern Analysis and Machine Intelligence*, 37(11), 2153–2163, https://doi.org/10.1109/TPAMI.2015.2408351

Tang, T., Liu, C., Chen, W., & Tomizuka, M., (2016), Robotic manipulation of deformable objects by tangent space mapping and non-rigid registration, in *Ieee/rsj int. conf. on intelligent robots and systems (iros'16)*.

Tang, T., Wang, C., & Tomizuka, M., (2018), A framework for manipulating deformable linear objects by coherent point drift, *IEEE Robotics and Automation Letters*, 3(4), 3426–3433.

Tang, Y.-M., Zhou, A., & Hui, K., (2006), Comparison of fem and bem for interactive object simulation, *Computer-Aided Design*, 38(8), 874–886.

Teschner, M., Heidelberger, B., Muller, M., & Gross, M., (2004), A versatile and robust model for geometrically complex deformable solids, in *Proceedings computer graphics international, 2004*. IEEE.

Tian, Y., Yang, Y., Guo, X., & Prabhakaran, B., (2013), Haptic-enabled interactive rendering of deformable objects based on shape matching, in *2013 iee international symposium on haptic audio visual environments and games (have)*, IEEE.

Tonnesen, D. L., (1998), *Dynamically coupled particle systems for geometric modeling, reconstruction, and animation*, University of Toronto.

Trinh, S., Spindler, F., Marchand, E., & Chaumette, F., (2018), A modular framework for model-based visual tracking using edge, texture and depth features, in *2018 iee/rsj international conference on intelligent robots and systems (iros)*, IEEE.

Wada, T., Hirai, S., Kawamura, S., & Kamiji, N., (2001a), Robust manipulation of deformable objects by a simple pid feedback, in *Ieee int. conf. on robotics and automation (icra)*.

Wada, T., Hirai, S., Kawamura, S., & Kamiji, N., (2001b), Robust manipulation of deformable objects by a simple PID feedback, in *Ieee int. conf. on robotics and automation (icra'01)*, https://doi.org/10.1109/ROBOT.2001.932534

Wada, T., Hirai, S., & Kawamura, S., (1998), Indirect simultaneous positioning operations of extensionally deformable objects, in *Proceedings. 1998 iee/rsj international conference on intelligent robots and systems. innovations in theory, practice and applications (cat. no. 98ch36190)*, IEEE.

Wang, B., Wu, L., Yin, K., Ascher, U. M., Liu, L., & Huang, H., (2015), Deformation capture and modeling of soft objects., *ACM Trans. Graph.*, 34(4), 94–1.
Xu, L., Lu, Y., & Liu, Q., (2018), Integrating viscoelastic mass spring dampers into position-based dynamics to simulate soft tissue deformation in real time, *Royal Society open science*, 5(2), 171587.

Yang, P.-C., Sasaki, K., Suzuki, K., Kase, K., Sugano, S., & Ogata, T., (2016), Repeatable folding task by humanoid robot worker using deep learning, *IEEE Robotics and Automation Letters*, 2(2), 397-403.

Yu, W., Kapusta, A., Tan, J., Kemp, C. C., Turk, G., & Liu, C. K., (2017), Haptic simulation for robot-assisted dressing, in *Ieee int. conf. on robotics and automation (icra'17)*, https://doi.org/10.1109/ICRA.2017.7989716

Zhang, J., Zhong, Y., Smith, J., & Gu, C., (2019), Neural dynamics-based poisson propagation for deformable modelling, *Neural Computing and Applications*, 31, 1091-1101.

Zhang, Z., Bieze, T. M., Dequidt, J., Kruszewski, A., & Duriez, C., (2017), Visual servoing control of soft robots based on finite element model, in *2017 ieee/rsj international conference on intelligent robots and systems (iros)*, IEEE.

Zheng, D., Wang, H., Wang, J., Zhang, X., & Chen, W., (2019), Toward visibility guaranteed visual servoing control of quadrotor uavs, *IEEE/ASME Transactions on Mechatronics*, 24(3), 1087-1095.

Zhu, B., Gu, L., Zhang, J., Yan, Z., Pan, L., & Zhao, Q., (2008), Simulation of organ deformation using boundary element method and meshless shape matching, in *2008 30th annual international conference of the iee engineering in medicine and biology society*, IEEE.

Zhu, J., Navarro, B., Fraisse, P., Crosnier, A., & Cherubini, A., (2018), Dual-arm robotic manipulation of flexible cables, in *2018 ieee/rsj international conference on intelligent robots and systems (iros)*, IEEE.
<figure><img src="image_89.png" /></figure>
**Titre :** Asservissement visuel d'objets déformables à partir du modèle masse-ressort

**Mot clés :** Asservissement visuel, manipulation d'objets déformables, modèle physique, suivi d'objets déformables.

**Résumé :** La manipulation d'objets rigides a depuis longtemps alimenté l'automatisation, tandis que l'adaptation aux matériaux souples reste un problème ouvert. Cette étude se concentre sur la commande de la forme d'objets déformables en utilisant le modèle masse-ressort (MSM) pour développer des relations analytiques entre les déplacements des points de l'objet et le mouvement des points manipulés. Nous dérivons une loi de commande basée sur cette relation pour positionner avec précision en temps réel plusieurs points de l'objet dans différentes expériences mettant en œuvre divers objets souples, deux mani-

pulateurs robotiques et une caméra RGB-D. Nous proposons en outre deux méthodes de déformation de la forme des objets souples, en utilisant des vecteurs de caractéristiques de faible dimension, en exploitant les descripteurs de Fourier ou les moments 3D, pour paramétrer la surface de l'objet. Diverses expériences confirment l'efficacité de ces paramétrisations dans la commande de la forme. Nous détaillons également la création du modèle, l'estimation de ses paramètres et le suivi des objets déformables, comblant ainsi l'écart entre les déformations du modèle et celles du monde réel.

**Title:** Vision-based shape servoing of soft objects using mass-spring model

**Keywords:** Visual servoing, real-time shape control, compliant manipulation, physics-based model, deformable object tracking.

**Abstract:** Rigid object manipulation has long driven automation, while adapting to soft materials remains an open problem. This study focuses on deformable object shape control, utilizing the mass-spring model for developing analytical relations between object points displacements and manipulator motions. We derive a control law based on this relation for accurate real-time positioning of multiple object points in different experiments employing various soft objects, two robotic manipulators,

and a RGB-D camera. We further provide two methods for soft object shape deformation, using low-dimensional feature vectors, leveraging Fourier descriptors or 3D moments, to parametrize the object surface. Various experiments affirm the efficacy of these parameterizations in shape control. We also elaborate on model creation, its parameter estimation, and deformable object tracking, bridging the gap between the model and real-world deformations.