An SE(3)-based formulation of the shape servoing
problemLouis Dehaybe, Olivier BrülsMultibody and Mechatronic Systems Lab
University of LiègeLiège, Belgium{louis.dehaybe, o.bruls} @uliege.be

Abstract—The art of controlling the shape of deformableobjects autonomously, a.k.a. shape servoing, remains a challenging task for robots today. While some current works devoted
attention to improving controllers, we propose to reconsider theformulation of the problem itself, under the theory of Lie groups.
This results in a new representation of the state of the objectand leads to a new definition of the shape error. In return,results obtained from simulations demonstrate that the approach
can achieve better performance for large deformations tasks. Inaddition, mobile frame definitions reduce the non-linearity of theproblem. Importantly, this approach is not tied to the algorithm
developed here and can therefore be easily extended to othertypes of controllers.

linearities induced by rigid body motions are filtered out. Thus,
the shape Jacobian remains constant under finite rotations,which is not the case of classical approaches. The obtained
solution is also independent upon the choice of any global
coordinate system. In the next section, we describe a classicalapproach to the shape servoing problem, in one of its most
simple form (i.e. without all the specificities and improvements
made by previous research), and we show its adaptation insection III.

I. I NTRODUCTION

A common approach [10, 11, 12] is to approximate theconfiguration of the DO by the position of a subset of N”feature points” that compose it.
 The problem
 is then3NgenerallyT T
posed as follows: let y = y1T . . . yN
∈ R
be thevector gathering the current position of each of these points, yi
being the ith one. Let yd be their desired position. It is also
assumed that M independent grippers, each having 6 DOF,
grasp the deformable object rigidly. The pose of gripper j isnoted zj ∈ R6 and gathers the three position and the threerotation parameters. All the zj ’s are stacked in the vector
Tz = zT1 . . . zTM ∈ R6M
Then, if the grippers move slowly enough such that thesystem can be considered to be quasi-static at each instant t,
there should exist a nonlinear function F(t) : R6M → R3N
that maps a configuration of the grippers to the configurationof the feature points, i.e. y(t) = F(z(t)). Since F(z(t)) is apriori unknown, one solution is to linearize around the current∂F (z) ∂z
configuration: ∂y∂t = ∂z ∂t , which leads toThe robotic manipulation of deformable objects (DO) questions many assumptions that are taken for granted whenmanipulating rigid ones. In particular, DOs have in theory aninfinite number of degrees of freedom (DOF), while the robotic
manipulator controls a few ones only; the problem is therefore
highly underactuated [1, 2]. This makes the shape servoing
task, i.e. deforming the soft object as desired, considerably
hard. In order to solve these challenges, one can distinguish [3]between model-based techniques, whether precomputed [4, 5]or learned [6, 7], and model-free ones [8, 9].
Among model-free techniques, many works rely on 3D
points as feedback feature [10, 11, 12]. Instead, the approachpresented thereafter distinguishes itself by relying on Liegroups theory. It has found widespread applications in thestudy of rigid bodies in robotics [13, 14]. It has also beenextended to the context of flexible multibody dynamics [15],but to the best of our knowledge, has not yet been employed
for the robotic manipulation of DOs. In this line, we reformulate the core of the control algorithm, from which several
interesting properties are obtained.
In particular, a new way of representing the shape isprovided. Instead of using contours [16] or points, we use
mobile frames that are defined directly from the mesh of theDO. As a result, the shape error is also redefined such that it
couples position and rotation components. This representation
contains rich information, in the sense that for a given numberof dimensions, the range of achievable tasks is increased.
In addition, the movement of the feature frame(s) and thegripper(s) are defined in mobile frames. Consequently, non-II. BACKGROUNDδy = J(z) δz(1)where J = ∂F∂z(z) = ∂y∂z is the so-called shape Jacobian, thatrelates the motion of the grippers to that of the feature points,and δ indicates an infinitesimal variation. Estimating correctly
J is a key ingredient in this approach. One way, employed by[10, 17], is to observe the evolution of y and z and iteratively
update J using the Broyden update rule:
Jt = Jt−1 + α∆yt − Jt−1 ∆zt(∆zt )T(∆zt )T (∆zt )(2)

where 0 < α ≤ 1 is a parameter controlling the responsivenessof the estimation of J. The infinitesimal variations in (1) areapproximated by small but finite variations, i.e. ∆yt = yt −yt−1 , ∆zt = zt − zt−1 , the superscript t indicating the time
instant. Defining the error between the current and the desiredconfiguration of the feature points as e = yd − y, a simple
controller can be designed:∆z = K J† e(3)with K a small positive diagonal gain matrix and J† thepseudoinverse of J.
One can notice that in the preceding, the motion of thefeature points and the grippers are defined in an inertial frame.
As a consequence, under a rigid body rotation of the DO, both∆y and ∆z would vary in a nonlinear way, and following (2),J would too.
III. M ETHOD
In this section, we modify the control scheme presented
above by incorporating concepts of Lie groups theory.
A. Notation and definition of the concepts
The transformation between a frame {O} and a frame{A} can be represented by elements of the special euclideangroup SE(3), i.e. the group of homogeneous transformation
ROA xOA
matrices HOA =∈ SE(3) where xAB ∈ R301×31
is a position vector that represents the origin of frame {A}expressed in frame {O}, and ROA ∈ SO(3) (the specialorthogonoal group) is a 3 × 3 rotation matrix that represents
the orientation of {A} with respect to {O}, expressed in {O}.
An element of SE(3) can also be thought of as an operator
that applies a transformation to a frame:
HOB = HOA Hr ⇔ Hr = (HOA )−1 HOB(4)In this case, Hr is the relative transformation between{A} and {B}, and the fact that it right multiplies HOA
implies that it is expressed in {A}. An enjoyable property ofthis representation is that Hr is invariant under rigid body
motions. Indeed, if we apply an arbitrary transformation
Hc to both HOA and HOB , we obtain HOA′ = Hc HOA
and HOB ′ = Hc HOB . The relative transformation between′′{A } and {B } is thus (HOA′ )−1 HOB ′ , which is equal to(HOA )−1 HOB = Hr .
We build on this advantage to adapt (1) to (3) as follows.
It is assumed that the DO can be represented by a meshcomposed of triangles. Among these, a subset of size N istied to a desired configuration; they are called ”feature faces”.
Their selection depends on the task objective. In practice,each feature face is attributed a Darboux frame {P } definedfrom the position xi , xj , xk of its three vertices : t1 = xji ,n = t1 × xki , t2 = n × t1 , with xji = xj − xi /∥xj − xi ∥ andxki = xk −xi /∥xk −xi ∥. The origin of the frame, xP , is taken
as the center of the triangle. The transformation is therefore

Fig. 1. Definition of the framesRP xP, with RP = t1 t2 n .
01×3 1Likewise, we build the frame transformation HD from thesame three vertices on the deformed configuration of the DO.
Finally, HG is simply the pose of the gripper (known fromthe forward kinematics). The situation is illustrated in fig. 1.
Now, let us consider that the grippers and the feature faces’
frames vary with time. An infinitesimal variation of an element
Af , where δπ A ∈ R6 isof SE(3) is written δHOA = HOA δπAA
an infinitesimal motion of frame A (indicated by the subscript)
expressed in A (indicated by the superscript), i.e. the local
f belongs to the Lie algebra of SE(3),frame representation. δπnoted se(3), which is a linear space. It is therefore isomorphic
f From now
to R6 through the map R6 → se(3) : δπ → δπ.
on, only the local frame representation will be used, and thesuperscript is therefore dropped.
defined as HP =B. Adaptation of the method
With these definitions in mind, (1) is reformulated as:δπ p = J δπ g(5) TT
δπ P1 . . . δπ TPNwith δπ p =∈ R6N , δπ g = TTδπ G1 . . . δπ TGM ∈ R6M , and J ∈ R6N ×6M .
Accordingly, the method is reshaped as follows. First, aninitialisation of J is required. Several methods exist. In thiswork, each DOF of the gripper is perturbed by a small amount,
one at a time, and the column of J corresponding to theperturbed DOF is obtained using standard finite differences.
Next, we adapt the Broyden update rule to estimate J as:
Jt = Jt−1 + α∆up − Jt ∆ug(∆ug )T(∆ug )T (∆ug )(6)

where we replace infinitesimal motions
δ by smallT but finitevariations ∆, such that ∆up = ∆uTP1 . . . ∆uTPN∈ R6N ,−1gP = log(H t−1 HP t ). The subscript i indicates thewith ∆uiPii

feature frame considered, while t is the time instant. log refers
to the logarithmic map (see more details in [15]) that maps anelement of the Lie group to the Lie algebra. ∆ug ∈ R6M isdefined analogously, replacing feature frames {P } by gripper
frames {G}. Importantly, one can highlight that the argument
given to log takes a similar form as Hr in (4), and for the samereasons, it enjoys the invariance property with respect to any
rigid body motion. Due to (6), since both ∆π p and ∆π g areFig. 2. (a) Start state (b) End state using method 1 and (c) using method 2

coordinate-independent and insensitive to superimposed frametransformations, so will be J.
Then, this new framework also leads to a redefinition of
the shape error. For the ith feature, it is defined as Hei =HPi Di = H−1
Pi HDi and its Lie algebra counterpart isge = log(He )∆uii(7)

ge is thus expressed in the mobile frame {P }. It is∆uiimportant to notice the introduction of the rotation in the error(since it is contained in the transformation matrices), and its
coupling to position variables via SE(3). This modification isprecisely at the origin of the difference between the trajectory
followed by the grippers in this method, compared to theformer one that uses positions only to define the error.
Consequently, the control law (3) needs to be adapted asfollows:∆ug = K J† ∆ue(8) TTwhere ∆ue = ∆ue1 . . . ∆uTeN . The output is therefore
directly obtained in the coordinate system of the gripper.
Lastly, for each gripper i, we can use either position control:gG ) where exp is the exponential map
HGt+1 = HGti exp(∆uii(see [15]), or velocity control by simply dividing the desired
motion increment by the time step size h: vg = h1 (∆ug )IV. E XPERIMENTS AND RESULTS
In what follows, position-based servoing (PBS) refers to themethod presented in section II, while SE(3)-based servoing(SE3BS) refers to the adapted one of section III. To demonstrate the difference between both, several experiments havebeen performed in simulation (only one result is shown forconciseness). The simulator, Pybullet [18], has been chosen
for its ability to simulate soft bodies. In particular, a beamlike object is modelled as an hyperelastic material that follows
a Neo-Hookean law. We emphasize that the controller has noaccess to the DO model. Without hurting the generality of themethod, the gripper is considered as ”floating”, i.e. it is not
attached to a robot. It is represented by the green cube (see fig.
2). The white cubes are the grasped nodes. The other side of
the beam has been clamped, and the only external force is dueto gravity. To obtain the target shape, in practice, the grippers
are manually moved and the position of all the vertices of thedeformed mesh are recorded. This ensures that the target isreachable from the starting configuration. The gain matrix is
K = 0.02 I6 and α = 0.6. The beam starts fully extended(fig. 2a), and the recorded shape is as represented by the reddots. Note that they are shown only for visualization purposes.
Indeed, in SE3BS, the algorithm only knows the state of one
feature frame (fig. 2c) that it uses to calculate the shape error(hence, P = 1). In PBS, only the position of the 3 white dots
(P = 3) is used (fig. 2b). To ensure a fair comparison between
methods, these three points are the same as those used to build
the Darboux frame in method 2.
The results of such a task show the added value of working
with frame transformations to represent the shape, and theadvantage of adding rotation components in the error. Indeed,
during the whole deformation, twists of the beam are avoided
as much as possible by SE3BS because it degrades the rotational part of the error. The trajectory is consequently smootherand more straightforward. Starting from 2.03, ∥∆ue ∥ < 0.03
is reached (the current and target frame coincide in fig. 2c)after a simulation time of 20.7s. In contrast, after the sametime, PBS does not manage to bring the points to the desired
locations, lowering ∥e∥ from 1.31 to 1.2 only. At the beginningof the task, twisting the beam improves the reduction inposition error, but it leads the controller in a deadlock near theend (i.e. the final state shown), from which it can not recover.
Notice also that the second method reduces a little bit thedimension of the problem (∆ue ∈ R6 against y ∈ R9 ).
The methods have been tested in other setups. In general,
for simple tasks where the deformation involves translations
mostly, both achieve similar performance. However, in manycases, it allows to succeed in high deformation tasks that PBS
can not execute properly because of deadlocks such as theaforementioned one.
Finally, we tested the case were the beam was subjected toa rigid body rotation while held by a gripper at both ends. Itwas observed, as expected, that the components of J varied
non linearly while in the case of SE3BS, it remained constant.
Therefore, it confirms that (6) only captures changes in theshape of the object, as it is supposed to.
V. C ONCLUSION AND OUTLOOKIn this work, a Lie group approach is used to tackle theshape servoing problem. It involves a new representation ofthe DO which couples translation and rotation components.
Starting from a typical controller used in this field, we
adapt the method according to these considerations. Someexperiments have been carried out in simulation and showedthat this alternative is more appropriate to execute certain
high deformation tasks. We emphasize that the framework
developed here is not specific to the controller used in thiswork, nor to the Broyden update rule. Hence, future works will
be devoted to the integration of this theory into more efficientshape Jacobian estimators and controllers. Finally, experiments
on a real setup will be proposed to validate the method.
ACKNOWLEDGMENT
This work was supported by the Interreg VA programme
Euregio Meuse-Rhine (European Fund for Regional Development), within the AACOMA project.

R EFERENCES[1] J. Zhu, A. Cherubini, C. Dune, D. Navarro-Alarcon,
F. Alambeigi, D. Berenson, F. Ficuciello, K. Harada,
J. Kober, X. Li et al., “Challenges and outlook in robotic
manipulation of deformable objects,” IEEE Robotics andAutomation Magazine, 2021.
[2] J. Das, Robotic control of deformable continua andobjects therein. Vanderbilt University, 2010.
[3] R. Herguedas, G. López-Nicolás, R. Aragüés, andC. Sagüés, “Survey on multi-robot manipulation of deformable objects,” in 2019 24th IEEE International Conference on Emerging Technologies and Factory Automation (ETFA). IEEE, 2019, pp. 977–984.
[4] F. Ficuciello, A. Migliozzi, E. Coevoet, A. Petit, andC. Duriez, “Fem-based deformation control for dexterous
manipulation of 3d soft objects,” in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS). IEEE, 2018, pp. 4007–4013.
[5] J. Das and N. Sarkar, “Robust shape control of deformable objects using model-based techniques,” Advanced Robotics, vol. 25, no. 16, pp. 2099–2123, 2011.
[6] C. Shin, P. W. Ferguson, S. A. Pedram, J. Ma, E. P.
Dutson, and J. Rosen, “Autonomous tissue manipulation
via surgical robot using learning based model predictivecontrol,” in 2019 International Conference on Robotics
and Automation (ICRA). IEEE, 2019, pp. 3875–3881.
[7] Z. Hu, P. Sun, and J. Pan, “Three-dimensional deformable object manipulation using fast online gaussian
process regression,” IEEE Robotics and Automation Letters, vol. 3, no. 2, pp. 979–986, 2018.
[8] D. Berenson, “Manipulation of deformable objects without modeling and simulating deformation,” in 2013
IEEE/RSJ International Conference on Intelligent Robotsand Systems. IEEE, 2013, pp. 4525–4532.
[9] M. Aranda, J. A. C. Ramon, Y. Mezouar, A. Bartoli,and E. Özgür, “Monocular visual shape tracking andservoing for isometrically deforming objects,” in 2020
IEEE/RSJ International Conference on Intelligent Robotsand Systems (IROS). IEEE, 2020, pp. 7542–7549.
[10] F. Alambeigi, Z. Wang, R. Hegeman, Y.-H. Liu, andM. Armand, “A robust data-driven approach for online
learning and manipulation of unmodeled 3-d heterogeneous compliant objects,” IEEE Robotics and Automation
Letters, vol. 3, no. 4, pp. 4140–4147, 2018.
[11] M. Shetab-Bushehri, M. Aranda, Y. Mezouar, andE. Ozgur, “As-rigid-as-possible shape servoing,” IEEE
Robotics and Automation Letters, 2022.
[12] D. McConachie and D. Berenson, “Estimating modelutility for deformable object manipulation using multiarmed bandit methods,” IEEE Transactions on Automation Science and Engineering, vol. 15, no. 3, pp. 967–979, 2018.
[13] J. M. Selig, Geometric fundamentals of robotics.
Springer, 2005, vol. 128.
[14] K. M. Lynch and F. C. Park, Modern robotics. Cam-bridge University Press, 2017.
[15] V. Sonneville, “A geometric local frame approach for
flexible multibody systems,” Ph.D. dissertation, Université de Liège, Liège, Belgique, 2015.
[16] D. Navarro-Alarcon and Y.-H. Liu, “Fourier-based shapeservoing: A new feedback method to actively deform
soft objects into desired 2-d image contours,” IEEE
Transactions on Robotics, vol. 34, no. 1, pp. 272–279,
2017.
[17] J. Huang, Y. Cai, X. Chu, R. H. Taylor, and K. S. Au,“Non-fixed contact manipulation control framework for
deformable objects with active contact adjustment,” IEEE
Robotics and Automation Letters, vol. 6, no. 2, pp. 2878–2885, 2021.
[18] E. Coumans and Y. Bai, “Pybullet, a python module
for physics simulation for games, robotics and machinelearning,” http://pybullet.org, 2016–2021.