#  Model Predictive Control for Dynamic Cloth Manipulation: Parameter Learning
and Experimental Validation

Adrià Luque, David Parent, Adrià Colomé, Carlos Ocampo-Martinez
and Carme Torras This work was developed in the context of the project
CLOTHILDE (”CLOTH manIpulation Learning from DEmonstrations”), which has
received funding from the European Research Council (ERC) under the European
Union’s Horizon 2020 research and innovation programmee (Advanced Grant
agreement No 741930).All authors are with the Institut de Robòtica i
Informàtica Industrial, CSIC-UPC, Barcelona, Spain. {aluque, dparent, acolome,
cocampo<EMAIL_ADDRESS>Ocampo-Martinez is also with the Automatic
Control Department, Universitat Politècnica de Catalunya - BarcelonaTECH,
Barcelona, Spain<EMAIL_ADDRESS>
###### Abstract

Robotic cloth manipulation is a relevant challenging problem for autonomous
robotic systems. Highly deformable objects as textile items can adopt multiple
configurations and shapes during their manipulation. Hence, robots should not
only understand the current cloth configuration but also be able to predict
the future possible behaviors of the cloth.

This paper addresses the problem of indirectly controlling the configuration
of certain points of a textile object, by applying actions on other parts of
the object through the use of a Model Predictive Control (MPC) strategy, which
also allows to foresee the behavior of indirectly controlled points. The
designed controller finds the optimal control signals to attain the desired
future target configuration.

The explored scenario in this paper considers tracking a reference trajectory
with the lower corners of a square piece of cloth by grasping its upper
corners. To do so, we propose and validate a linear cloth model that allows
solving the MPC-related optimization problem in real time. Reinforcement
Learning (RL) techniques are used to learn the optimal parameters of the
proposed cloth model and also to tune the resulting MPC. After obtaining
accurate tracking results in simulation, the full control scheme was
implemented and executed in a real robot, obtaining accurate tracking even in
adverse conditions. While total observed errors reach the 5 cm mark, for a
30$\times$30 cm cloth, an analysis shows the MPC contributes less than 30% to
that value.

##  I INTRODUCTION

Robots have become a key component for increasing the productivity in industry
since the 20th century. Furthermore, nowadays robots are starting to be part
of domestic environments. In both situations, we can find deformable objects
like textiles. Until now, most robotics research has focused on rigid object
manipulation. However, textile industry is nowadays encountering major
technological difficulties in automating parts of their production processes.
A simple task such as taking a cloth garment from a rack and putting it in a
box for shipping is still an open problem which the industry is trying to find
a solution for. Highly deformable objects represent a major challenge due to
their physical properties: their shape and appearance can continuously vary
during manipulation. Because of this, it is not enough that the robot
understands the cloth current configuration, but it also needs to predict how
its action will change the cloth state.

Figure 1: Picture of the setup used in the real-world experiments

In a previous work [1], Dimensionality Reduction (DR) techniques applied to
motion characterization together with Reinforcement Learning (RL) were
successful at learning to fold a polo shirt. However, the authors realized how
sensitive the output of the action was to any perturbation on the initial
conditions. This results in large noise when mapping a robot motion
parametrization to the reward function to be optimized in the training phase.
Feedforward models have also been used in robotic cloth manipulation [2], in
which the control action results from the sum of the outcome of the Inverse
Dynamical Model (IDM) of the robot, i.e., the torque necessary to maintain a
tuple of position, velocity and acceleration, and a PID compensating term that
accounts for the model deviation from ground truth and external disturbances,
such as cloth dynamics. While this kind of controller allows for smaller gains
and thus more compliance, it loses precision with larger unmodelled external
disturbances. Ideally, one would include the IDM of the manipulated object so
as to compensate for its dynamics. While in [3], an IDM model for a rope is
learned through CNNs after 60k training interactions, learning such models for
a cloth garment would be more sample-expensive and, therefore, it is probably
not the best option for cloth manipulation. Nevertheless, dynamic knowledge of
the manipulated object needs to be included in the control loop as, otherwise,
the behavior of the cloth may drift away from the desired one, with the
controller being unable to compensate such error. This fact hinted the need to
include some kind of predictive behavior and prior knowledge in learning cloth
manipulation. Therefore, the natural next step is to use a control technique
that makes the robot proactively modify its motion according to the
predictions made based on a cloth model.

A suitable technique along this line is Model Predictive Control (MPC) [4].
The idea of MPC is to use a mathematical model of the plant to be controlled
together with an optimization algorithm. This algorithm looks for the best
possible control inputs according to a previously defined cost function in a
finite time horizon [5]. The model is used to predict the future states along
that time horizon. In addition, constraints in the related optimization
problem can be specified in a quite straightforward way. Bhardwaj et al. [6]
recently used MPC to control a robot under certain restrictions (e.g.: joint
limits, collision avoidance, etc.) given its dynamic model. In our case, the
plant to be modelled and controlled is a cloth garment. Cloth models are
represented as high dimensional systems [7]. These dimensions are required for
properly describing cloth behavior, but they make the control problem harder.
In literature, different cloth models that simulate the internal dynamics of
clothes are available [8], [9]. The models consist in solving large systems of
equations. However, the high cost of solving these systems limits their
utility for real-time applications [10]. Modeling the dynamics of woven
fabrics is a complex problem widely studied in computer graphics. Most models
available in literature need to be highly nonlinear to properly describe a
realistic behavior [11]. These nonlinearities will clearly affect the solving
speed of an optimization problem. In other works like [12], the cloth model is
included in the MPC design but the solution is obtained after hours of
computation.

Cloth manipulation has been a research challenge with successful cases such as
the PR2 robot folding towels [13], where a vision-based grasping point
detection algorithm is presented. The robot begins by picking up a dropped
towel on a table, goes through a sequence of vision-based re-grasps and
manipulations and finally stacks the folded towel in a target location.
Despite the impressive results, it takes almost 25 minutes to recognize the
different states and complete the task. More recently, Yan et al. [14] used
latent representations to learn the dynamic models of non-rigid objects and
used them for planning sequences of actions in order to obtain a desired state
of the manipulated object. The manipulation is, however, not real-time in the
sense that the models learned are used for predicting the outcomes of the
actions, rather than real-time control. In [12], the authors presented a
technique to synthesize dexterous manipulation of cloth for physics-based
computer animation. An optimization problem is formulated to find the
commanding forces of the hand so that the cloth follows the reference motion.
However, the computational cost is prohibitive for any real-world scenario.
Moreover, in 2018 Erickson et al. [15] presented an MPC approach that allows a
robot to reduce the force exerted during assisted dressing. Nonetheless, the
cloth mathematical model was not included in their problem. In the present
work, we are interested in incorporating the cloth model in the controller so
that the robot always considers the motion of the cloth caused by the movement
of its end-effector. The reference motion of the cloth will be represented by
the desired motion of certain interest points. Up to the knowledge of the
authors, MPC of cloth manipulation as we want to tackle it is not found in
literature.

Therefore, the main contribution of this paper is the design, simulation and
final implementation on a real setup of a closed-loop control strategy aimed
to improve robotic cloth manipulation, by using an MPC that includes a dynamic
cloth model satisfying both physical and operational constraints. This
controller finds the optimal control inputs (motions of the grasped points)
that yield minimum tracking error, predicting the behavior of the interest
points using a cloth Control-Oriented Model (COM). This model, built and
validated with captured evolutions of a real cloth piece, focuses on
describing the motion of the aforementioned interest points accurately when
given the control signals (motions of the grasped points), in contrast with
other cloth dynamic models that try to describe the behavior of the entire
piece accurately. Furthermore, RL techniques are used in order to validate the
model and find the optimal tuning to reduce tracking error. These
contributions are validated in experiments executed in a real robot in real
time.

The remainder of this paper is structured as follows: In Section II, we
present the cloth manipulation problem. In Section III, we explain our
proposed solution: the linear cloth model and the design of the MPC
controller. In Section IV, we show the case study, specifying differences
between simulations and real setup. In Section V, we present our results,
including model validation, controller tuning, simulations and trajectory
tracking in a real setup. Finally, in Section VI we extract conclusions of the
achieved results and we explain the forthcoming work directions.

##  II PROBLEM STATEMENT

The main application of this paper is to control the movement of a piece of
cloth so that certain parts of such cloth, the interest points, track a
reference trajectory, by controlling other grasped points. In this proof of
concept we do not include collision scenarios, leaving this aspect as further
work. For this tracking endeavor, we consider that the manipulation is done
with two robotic arms by holding the piece of cloth in the air with two pinch
grasps. Hence, in our case we manipulate the cloth by varying the position
($x,y,z$ coordinates) of the two upper corners, which are our grasped points
$\bm{u}\in\mathbb{U}\subset\mathbb{R}^{6}$ (see Fig. 2), where
$\mathbb{U}=\\{\bm{u}\in\mathbb{R}^{6}:\underline{\bm{u}}\leq\bm{u}\leq\overline{\bm{u}}\\}$,
with $\underline{\bm{u}}$ and $\overline{\bm{u}}$ being the minimum and
maximum control signals, respectively. This situation can also be adapted to a
single robot arm with a rigid link between the grasped points.

Figure 2: Representation of the trajectory tracking problem

In most manipulation applications, the positions of all the points of the
piece of cloth are not relevant for its control, and the efforts can be
focused in tracking interest points, which have a known relationship with the
grasped ones, for example a dynamical model. In our case, defining the
trajectory of both lower corners
$\bm{y}=\\{\bm{y}_{r},\bm{y}_{l}\\}^{T}\in\mathbb{Y}\subset\mathbb{R}^{6}$ is
enough to generate a reference trajectory to track, namely
$\bm{r}=\\{\bm{r}_{r},\bm{r}_{l}\\}^{T}\in\mathbb{Y}\subset\mathbb{R}^{6}$
(see Fig. 2), where
$\mathbb{Y}=\\{\bm{y}\in\mathbb{R}^{6}:\underline{\bm{y}}\leq\bm{y}\leq\overline{\bm{y}}\\}$,
with $\underline{\bm{y}}$ and $\overline{\bm{y}}$ being the minimum and
maximum position values for each lower corner, respectively. Controlling the
position of the lower corners by only moving the upper corners (the grasped
ones) is not a simple task, specially when they are far from each other.
Dynamic relationships among all the points of the piece of cloth are mostly
nonlinear and depend on both the position and velocity of the other points of
the cloth.

As stated before, the considered control strategy includes some kind of both
system and environment behavioral prediction and prior knowledge of the cloth
dynamics. Such a suitable strategy is Model Predictive Control (MPC), where
the model is used to predict the future behavior of the system states (and
then outputs) along a time-ahead horizon, while physical and operational
constraints are satisfied. In the context of this paper, the predictive
control strategy requires a mathematical model to predict the cloth behavior.
This model must resemble the real cloth statically and dynamically. In
literature, we can find multiple theoretical cloth models. Most of them are
highly nonlinear and not appropriate for real-time control but suitable for
simulation. Therefore, we propose to obtain a linear cloth model, faster but
accurate enough, such that the designed MPC strategy can be used in real time,
assuming a possible loss of realistic system evolutions in extreme
circumstances (e.g., fast dynamics). This reduced model is established and
further described in Section III. Thus, as previously explained, our idea is
to use a realistic Simulation-Oriented Model (SOM) to represent the real cloth
in simulation, while the simpler but accurate enough Control-Oriented Model
(COM) is to be used for control purposes.

Although the control strategy has certain degree of inherent robustness, and
the controller outputs an optimal solution to its optimization problem, its
formulation includes several parameters, weights and alternatives that might
help reaching minimum tracking errors. To find the best controller structure
and its optimal tuning, we propose the usage of RL techniques to learn them
automatically.

Summarizing, the problem consists in finding the optimal sequence of control
inputs ($\bm{u}$) via MPC to manipulate a cloth, making the interest points
($\bm{y}$) track a desired reference ($\bm{r}$) while satisfying the physical
and operational constraints. To this end, we will find a fast COM for real-
time implementation, able to reproduce the system dynamics to be controlled,
we will use RL to learn its parameters and we will validate the model against
the behavior of real cloth pieces. Additionally, the optimal tuning of the
controller will also be found with RL to minimize tracking error in closed-
loop.

##  III PROPOSED SOLUTION

In this section, we will firstly define the COM to use in our predictive
controller in Section III-A, followed by the definition of the control
optimization problem in Section III-B, with the specific real-time
requirements detailed in Section III-C.

###  III-A Control-Oriented Model Definition

We propose to use the simplest –and cheapest to evaluate– cloth model: a mass-
spring-damper system [16]. This model should be able to reproduce those system
dynamics to be controlled afterwards, while maintaining highest degree of
accuracy with respect to the real cloth. Here, the cloth is treated as a
system of particles (nodes) interconnected with spring-dampers. Usually, mass-
spring models use three types of connections between nodes to give them more
realism [16]: structural springs, shear springs and flexion springs. Our main
goal is not the most realistic model but a fast model that properly describes
the lower-corners behavior along a prediction horizon $H_{p}$. Note that,
whilst we want such a model to fit the behavior of the lower corners as
realistically as possible, the behavior of rest of the mesh is only enforced
indirectly. Hence, we are only going to use structural springs as shown in
Figure 3 with a mesh of $N$ nodes. In a general case, $N=n_{r}\times n_{c}$,
with $n_{r}$ and $n_{c}$ being the number of nodes per row and column,
respectively, but in a square mesh, we have $N=n^{2}$.

Figure 3: Mass-spring-damper system with structural springs ($N=n^{2}=16$)

The system in Figure 3 can be represented as a graph of $N$ nodes belonging to
a set $\mathcal{I}=\\{1,2,...,N\\}$ and a set of edges $\mathcal{E}$ that
represent the springs connecting those nodes. Moreover, a set of neighbouring
nodes related to the $i$-th node is defined as
$\mathcal{N}_{i}=\\{j:(i,j)\in\mathcal{E}\\}$, where its cardinality is
$|\mathcal{N}_{i}|=\rho$. In our case, we only use structural springs –which
yield a sufficiently proper results and save computational costs–, therefore,
$\rho=4$ for the interior points of the mesh. For each particle
$i\in\mathcal{I}$, we can compute the resulting force of the springs
$\bm{S}_{k}^{i}$ in time step $k\in\mathbb{Z}_{\geq 0}$ as

$\bm{S}_{k}^{i}=\sum_{\mathcal{N}_{i}}-K\frac{\|{\bm{d}_{k}^{j}}\|-l_{0}^{j}}{\|{\bm{d}_{k}^{j}}\|}{\bm{d}_{k}^{j}},$

where $K$ is the spring stiffness, ${\bm{d}_{k}^{j}}\in\mathbb{R}^{3}$ is the
vector pointing from the neighbor $j\in\mathcal{N}_{i}$ to the particle
$i\in\mathcal{I}$ and $l_{0}^{j}\in\mathbb{R}_{>0}$ is the initial length of
the spring connecting that $j$ neighbor with the $i$-th particle. The
resulting force of the dampers $\bm{D}_{k}^{i}$ is calculated as
$\bm{D}_{k}^{i}=\sum_{\mathcal{N}_{i}}-b(\bm{v}_{k}^{i}-\bm{v}_{k}^{j})$,
where $b$ is the damping constant and $\bm{v}_{k}\in\mathbb{R}^{3}$ is the
linear velocity of a particle. Moreover, $i\in\mathcal{I}$ and
$j\in\mathcal{N}_{i}$.

In addition, each particle is affected by the force of gravity
$\bm{G}_{k}^{i}=-m^{i}\bm{g}$, where $m^{i}$ is the mass of the particle
$i\in\mathcal{I}$ and $\bm{g}$ is the gravity constant. By adding these three
terms, we compute the total force $\bm{F}_{k}^{i}$ applied to the particle as
$\bm{F}_{k}^{i}~{}=~{}\bm{S}_{k}^{i}+\bm{D}_{k}^{i}+\bm{G}_{k}^{i}$. Once we
have computed all the forces, we can use Newton’s Second Law to compute the
acceleration as $\bm{a}_{k}^{i}={\bm{F}_{k}^{i}}/{m^{i}}$.

Having the accelerations of the nodes, we can integrate them over time to
obtain the positions and velocities. In the next subsections, we present the
proposed steps to obtain a reliable linear cloth model from the accelerations
previously calculated.

####  III-A1 Integration of the model

To integrate the acceleration and obtain the position and velocity of the
nodes, we can use either implicit or explicit integration methods. The former
are stable but require solving large systems of equations [10]. The high cost
of solving these systems limits their utility for real-time applications.
Instead, explicit integration methods are fast but could have stability
problems. To avoid these problems, we can put enough damping $\bm{D}^{i}_{k}$
in the system so that the energy decreases in a single time step [17]. We
propose to use explicit Euler’s integration method with a given time step
$\Delta t$ to update positions and velocities.

####  III-A2 Linearization of the model

Even if we have proposed a fast spring-damper model, the spring force vector
$\bm{S}^{i}_{k}$ is nonlinear since it depends on a quadratic norm of state
variables multiplied by other state variables. This norm computes the spring
length in three dimensions along $k\in\mathbb{Z}_{\geq 0}$ as
$\|{\bm{d}_{k}^{j}}\|=\sqrt{(p_{x,k}^{i}-p_{x,k}^{j})^{2}+(p_{y,k}^{i}-p_{y,k}^{j})^{2}+(p_{z,k}^{i}-p_{z,k}^{j})^{2}}$.
Instead of computing the $\mathcal{L}$2-norm, we propose to calculate it as an
$\mathcal{L}1$-norm to linearize it by creating three linear springs, one for
each direction of the space. Using this norm, we propose to associate
different stiffness constants for each direction $k_{x},k_{y},k_{z}$ so as to
preserve the different behavior in each direction, i.e.,

$\bm{S}_{k}^{i}\approx\sum_{\mathcal{N}_{i}}\begin{bmatrix}-k_{x}(p_{x,k}^{i}-p_{x,k}^{j}-l_{0x}^{j})\\\
-k_{y}(p_{y,k}^{i}-p_{y,k}^{j}-l_{0y}^{j})\\\
-k_{z}(p_{z,k}^{i}-p_{z,k}^{j}-l_{0z}^{j})\end{bmatrix}.$

With this transformation, we have a linear system that can be expressed
through a state-space realization as

$\displaystyle\begin{split}\bm{x}_{k+1}&=\bm{A}\bm{x}_{k}+\bm{B}\bm{u}_{k}+\bm{f}_{ct},\\\
\bm{y}_{k}&=\bm{C}\bm{x}_{k},\end{split}$ (1)

where $\bm{x}\in\mathbb{X}\subset\mathbb{R}^{6N}$ is the vector of the cloth
states (position $p$ and velocity $v$ in $x,y,z$ of nodes $i\in\mathcal{I}$),
$\bm{u}\in\mathbb{U}\subset\mathbb{R}^{6}$ is the control input vector,
$\bm{y}\in\mathbb{Y}\subset\mathbb{R}^{6}$ is the output vector (positions of
the lower corners) and $\bm{f}_{ct}\in\mathbb{F}\subset\mathbb{R}^{6N}$ is the
vector of constant forces applied to each node: gravity and natural spring
length force. Moreover, $\bm{A}\in\mathbb{R}^{6N\times 6N}$,
$\bm{B}\in~{}\mathbb{R}^{6N\times 6}$, $\bm{C}\in\mathbb{R}^{6\times 6N}$ are
the system state-space matrices and
$\mathbb{X}=\\{\bm{x}\in\mathbb{R}^{6N}:\underline{p}_{x}\leq
p_{x}\leq\overline{p}_{x},\ \underline{p}_{y}\leq p_{y}\leq\overline{p}_{y},\
\underline{p}_{z}\leq p_{z}\leq\overline{p}_{z},\ \underline{v}_{x}\leq
v_{x}\leq\overline{v}_{x},\ \underline{v}_{y}\leq v_{y}\leq\overline{v}_{y},\
\underline{v}_{z}\leq v_{z}\leq\overline{v}_{z}\\}$, where $\underline{p}_{i}$
and $\overline{p}_{i}$ represent the minimum and maximum position values for
each direction, respectively, whereas $\underline{v}_{i}$ and
$\overline{v}_{i}$ are the corresponding minimum and maximum velocities.

We previously discussed that the addition of the damping term guarantees the
stability of the system: If we have different spring constants for each
direction, we also need different damping constants to reduce the energy in
each direction. Similarly as before, we compute $\bm{D}^{i}_{k}$ as

$\bm{D}^{i}_{k}\approx\sum_{\mathcal{N}_{i}}\begin{bmatrix}b_{x}(v_{x,k}^{i}-v_{x,k}^{j})\\\
b_{y}(v_{y,k}^{i}-v_{y,k}^{j})\\\
b_{z}(v_{z,k}^{i}-v_{z,k}^{j})\end{bmatrix}.$

####  III-A3 The super-elastic problem

Elasticity is the major drawback of the mass-spring cloth model, as it might
stretch under its own weight [16]. Two solutions are found in the literature:
making the springs stiffer or applying a maximum deformation rate. The former
has its limits, as it can unstabilize the model. With the latter, the
elongation of the springs is corrected if it is over a 10% of the initial
length, making the model nonlinear, and thus the computations slower.

We propose a third option to solve the super-elastic problem: shortening the
initial length of the linear springs in the vertical direction in simulation
by $\Delta l_{0z}$. When gravity is then applied, it results in a force
equilibrium in the vertical direction, avoiding the mesh stretching under its
own weight.

Figure 4: Mesh positions showing the super-elastic problem (left) and the
result of applying $\Delta l_{0z}$, correcting it (right)

####  III-A4 Cloth model orientation

The defined parameters depend on the space axes, and must always be aligned
with the cloth model. Therefore, a local reference frame ($L$) must be defined
for the cloth model, such that if it rotates, the parameters are expressed in
this local base and rotate with it. With the available data during an
execution, the fastest approach is shown in Figure 5.

Figure 5: Process to obtain the local cloth base

The first step (Figure 5(a)) is to define the local $X$ vector (red) in the
direction from one corner to the other, $X_{L}~{}=~{}\bm{u}_{r}-\bm{u}_{l}$.
The other two axes must be contained in the plane perpendicular to this axis,
as shown in Figure 5(b). Without using any other data (full mesh or
reference), we can force the local $Y_{L}$ axis (green) to have null vertical
(absolute $Z$) component, as shown in Figure 5(c), which also defines the
local $Z_{L}$ axis (blue) as the cross product between the other two,
$Z_{L}=X_{L}\times Y_{L}$, as seen in Figure 5(d). This frame definition is
suitable for rotations along the $Z_{L}$ and $Y_{L}$ axis, but rotations along
$X_{L}$ without changing the positions of the upper corners are ignored,
always placing $Y_{L}$ horizontally.

###  III-B Control Strategy

The main objective of a tracking MPC strategy is to minimize the tracking
error while satisfying the system constraints. Closed-loop stability can be
guaranteed if the initial state is inside the feasibility region
$\bm{x}_{0}\in\mathbb{X}$ and the evolution of the state trajectories remain
inside the polytope defined by the system/operational constraints. The
controller has been obtained considering the following elements:

####  III-B1 Cost function

The goal is to find a sequence control inputs

$\bm{u}_{k}^{s}\triangleq\\{\bm{u}_{0},\dots,\bm{u}_{H_{p}-1}\\}$ (2)

along a prediction horizon $H_{p}$ such that the lower right and left nodes
$\bm{y}\in\mathbb{Y}$ follow the desired trajectories $\bm{r}\in\mathbb{Y}$ as
accurately as possible. Moreover, we also want to obtain smooth trajectories,
reducing noise and sudden changes in acceleration. To this end, reducing the
changes in consecutive control signals or slew rates (i.e., $\bm{\Delta
u}_{i}~{}=~{}\bm{u}_{i}-\bm{u}_{i-1}$) instead of the control inputs has been
proven to yield better results (Section V). The final multi-objective cost
function at used at each time step $k\in\mathbb{Z}_{\geq 0}$ is

$J_{k}=\sum_{i=0}^{H_{p}-1}\|\bm{y}_{k+i+1|k}-\bm{r}_{k+i+1}\|^{2}_{\bm{Q}}+\|\bm{\Delta
u}_{k+i|k}\|^{2}_{\bm{R}},$ (3)

where the notation $(k+i|k)$ refers to the prediction at time step $(k+i)$
based on measurements made at time step $k$. For $i=0$, $\bm{\Delta
u}_{k|k}=\bm{u}_{k|k}-\bm{u}_{k-1|k}$. This $\bm{u}_{k-1|k}=\bm{u}_{k-1}$ is
the control input applied in the previous step, and must be given as an
initial condition. Equation (3) will be optimized to find $\bm{\Delta
u}_{k|k}$, and $\bm{u}_{k|k}=\bm{\Delta u}_{k|k}-\bm{u}_{k-1|k}$ Note that
$(\bm{Q},\bm{R})\in\mathbb{R}^{6\times 6}$ are the weighting matrices of
suitable dimensions that prioritize each term of the multi-objective cost
function.

####  III-B2 System constraints

The robot arms that manipulate the cloth have restrictions of velocity
$\bm{v}_{max}$ and workspace $\mathbb{U}$, providing bounds for the search of
the optimal computed control input.

####  III-B3 Optimization problem formulation

Based on the receding horizon strategy [5] and considering a fixed prediction
horizon $H_{p}$, the goal is to obtain the input sequences that minimize the
tracking errors of the two lower corners while satisfying all constraints. The
controller is based on the solution of the following discrete-time open-loop
optimization problem (OOP):

$\displaystyle\displaystyle\min_{\bm{u}_{k}^{s}}\ J_{k},$ s.t. (5)
$\displaystyle\begin{array}[]{llc}\begin{aligned}
\bm{x}_{k+i+1|k}&=\bm{A}\bm{x}_{k+i|k}+\bm{B}\bm{u}_{k+i|k}+\bm{f}_{ct}\\\
\bm{y}_{k+i|k}&=\bm{C}\bm{x}_{k+i|k}\\\ \bm{x}_{k|k}&=\bm{x}_{k}\\\
\bm{u}_{k-1|k}&=\bm{u}_{k-1}\\\
\bm{x}_{k+i+1|k}&\in\mathbb{X}\subseteq\mathbb{R}^{n}\\\
\bm{u}_{k+i|k}&\in\mathbb{U}\subseteq\mathbb{R}^{m},\\\
\end{aligned}\end{array}$

$\forall i\in[0,H_{p}-1]$, where $\bm{u}_{k}^{s}$ is the sequence of control
signals in (2).

Assuming the OOP (5) is feasible, there exists an optimal sequence solution
$\bm{u}_{k}^{s,*}~{}\triangleq~{}\\{\bm{u}^{*}_{k|k},\bm{u}^{*}_{k+1|k},...,\bm{u}^{*}_{k+H_{p}-1|k}\\}$.
The applied control is $\bm{u}^{MPC}_{k}\triangleq\bm{u}^{*}_{k|k},\ (i=0)$,
ignoring the rest of the sequence. The entire process is repeated at the next
time instant $k$.

####  III-B4 Weighting matrix tuning

The $\bm{Q}$ matrix in (3) is the responsible of penalizing the tracking
errors for each coordinate of the lower corners, while $\bm{R}$ penalizes
sudden changes and quick fluctuations in the control signals.

An adaptive tuning of the $\bm{Q}$ matrix is proposed. In each time step, we
compute the distance vector between the current position and that of the
reference at the end of the prediction horizon, finding the main direction
$\bm{\beta}$ in which the lower corners move along $H_{p}$. By normalizing
this vector, the final adaptive weighting matrix $\bm{Q}_{a}$ is computed as

$\bm{\beta}_{k}=\frac{|\bm{r}_{k+H_{p}}-\bm{y}_{k|k}|}{\|\bm{r}_{k+H_{p}}-\bm{y}_{k|k}\|+\epsilon},\quad\bm{Q}_{a,k}=\mbox{diag}\begin{bmatrix}\bm{\beta}_{k}\end{bmatrix},$
(6)

where an $\epsilon$ term has been included to avoid numerical problems when
the corner is exactly at the reference. We must note that this adaptive term
changes in each time step, as is made explicit by the $k$ subscript.

This adaptive computation has been studied as a possible way to compute the
optimal tuning along with different constant matrix structures. The results of
this study are shown in Section V.

###  III-C Real-Time Control

To work in real time, the controller needs to have a constant output rate with
an updated control signal on every time step (sampling time $T_{s}$). It was
observed that sometimes, depending on initial conditions, optimizations could
take longer than the maximum allowed time (one step), thus slowing down the
executions. To avoid this, the solver was changed to run in parallel, and
return not only $\bm{u}^{*}_{k|k}$ but the entire sequence $\bm{u}_{k}^{s,*}$
of $H_{p}$ control signals. This way, the solver can be called with new
initial conditions on every time step, but if an optimization takes longer
than $T_{s}$ to finish, the controller can output a sub-optimal solution
coming from the most recently obtained control sequence. This can be done for
a maximum of $H_{p}$ steps, the length of each sequence, thus a hard time
limit was added to cancel optimizations running for too long. In the end, a
maximum time of $T_{s}H_{p}/4$ was set empirically, ensuring there are always
at least four optimizations with different initial conditions in one
prediction horizon.

##  IV CASE STUDY DESCRIPTION

This section starts with the specific changes that had to be incorporated to
the previously defined solution for the used setup with only one robotic arm,
described in Section IV-A. Then we divide the study in two, with simulations
in Section IV-B and the real setup in Section IV-C.

###  IV-A Application to Single-Arm Manipulation

The presented formulation in (5) assumes the evolutions of the two controlled
corners are independent, as they are when using two different robot arms to
pick them. In the studied case, only one robot is used, and thus the upper
corners are linked together with a rigid piece. This means they always keep a
constant distance, adding a new quadratic constraint to the problem. This
changes it from Linear to a Quadratically Constrained Quadratic Program
(QCQP), but even if it is more complex, convexity is kept [18], and
computations can be fast. The new OOP is:

$\displaystyle\displaystyle\min_{\bm{u}_{k}^{s}}\ J_{k},$ (7)
$\displaystyle\begin{array}[]{llc}\text{s.t.}&\\\ &\begin{aligned}
\bm{x}_{k+i+1|k}&=\bm{A}\bm{x}_{k+i|k}+\bm{B}\bm{u}_{k+i|k}+\bm{f}_{ct}\\\
\bm{y}_{k+i|k}&=\bm{C}\bm{x}_{k+i|k}\\\
\left\|d^{uc}_{k+i+1|k}\right\|^{2}&=L^{2}\\\ \bm{x}_{k|k}&=\bm{x}_{k}\\\
\bm{u}_{k-1|k}&=\bm{u}_{k-1}\\\
\bm{x}_{k+i+1|k}&\in\mathbb{X}\subseteq\mathbb{R}^{n}\\\
\bm{u}_{k+i|k}&\in\mathbb{U}\subseteq\mathbb{R}^{m},\\\
\end{aligned}\end{array}$ (9)

$\forall i\in[0,H_{p}-1]$, where $d^{uc}$ is the distance vector between the
upper corners.

The control signals obtained with the described MPC are the displacements of
the upper corners of the cloth in one time step. However, to move one robot,
we need a pose of its Tool Center Point (TCP), which must be computed with the
available data. The position of the TCP can be obtained by taking the absolute
positions of the upper corners of the cloth, $\bm{u}^{abs}$, computing the
midpoint and adding a constant offset ($\Delta h$) introduced by the rigid
piece that links both corners together. This offset is always in the $Z_{L}$
axis of the local cloth base, so it must be transformed to global coordinates
before being added as follows:

$\displaystyle\Delta\bm{p}^{L}$ $\displaystyle=\begin{bmatrix}0\\\ 0\\\ \Delta
h\end{bmatrix}\rightarrow\Delta\bm{p}=\bm{R}_{L}^{W}\cdot\Delta\bm{p}^{L},$
(10) $\displaystyle\bm{p}_{TCP}$
$\displaystyle=\frac{1}{2}\begin{bmatrix}u^{abs}_{1}+u^{abs}_{2}\\\
u^{abs}_{3}+u^{abs}_{4}\\\ u^{abs}_{5}+u^{abs}_{6}\end{bmatrix}+\Delta\bm{p}.$

The orientation of the TCP is obtained with the same local reference frame,
but inverting the $Z_{L}$ axis to point away from the robot and into the
cloth, as it is a convention in robotics. The other two axes are swapped for
practical convenience, resulting in $R_{TCP}^{W}=[Y_{L}\ X_{L}\
\mbox{-}Z_{L}]$.

###  IV-B Simulation

Figure 6: Closed-loop control scheme. The nonlinear SOM substitutes the real
cloth in simulation, and the linear B-SOM acts as a fast backup while the real
feedback is processed, which in the real setup can take several time steps

The proposed control scheme is shown in Figure 6. In simulation, instead of
using only two models, one inside the controller and one to simulate the real
cloth, we add a third model as an intermediate step. This is motivated by the
real setup, where the feedback signal from the real cloth is the output of a
Computer Vision algorithm which is the slowest part of the scheme, taking
multiple time steps to send an updated signal. This way, a linear “Backup”
Simulation-Oriented Model (B-SOM) can simulate the cloth with the obtained
control signal on each time step and update the initial conditions even if
there is no new real feedback on that step. Once the feedback signal is
processed, the state of this B-SOM is updated accordingly.

Both linear models (COM and B-SOM) follow the equations presented in Section
III, but they can have different sizes. The nonlinear model simulating the
real cloth is the one presented in [19], where a dynamic validation between
their model and real cloth is performed (errors lower than 6%).
Computationally speaking, this model employs finite elements to discretize the
Lagrangian of the mechanical system (kinetic energy minus potential energy)
but not the equations of motion. This way, Euler-Lagrange equations are
obtained as a system of ordinary differential equations (ODE) instead of a
partial differential equations (PDE), making integrations faster. Even then,
computations are slower than required for a real-time application, and this
nonlinear model cannot be used as COM, and is used only in simulation.

The nonlinear model can be discretized with a mesh of $10\times 10$ nodes to
achieve realistic behavior [19]. For the linear models, it was found that a
mesh of $4\times 4$ was enough for the considered trajectory tracking problem,
as shown in Section V. The proposed solution to be able to use different mesh
sizes together is to make the smaller ones be sub-meshes of the larger ones.
This can be done in square meshes when the side sizes $n$ follow
$(n_{L}-1)=p(n_{S}-1)$ for some proportion $p\in\mathbb{Z}^{+}$. Knowing
larger mesh sizes increase computational time, simulations were executed with
sizes $n=10$ and 13 for the nonlinear model, and $n=4$ and 7 for the linear
ones, testing cases where COM and B-SOM had different sizes or the same one.
It can be checked how a mesh of $n=4$ can be extracted from one of $n=7$ and,
in turn, this can be extracted from a larger mesh of $n=13$.

To evaluate the trajectory tracking performance, we define two Key Performance
Indicators (KPIs). The first one is related to the tracking error, and can be
obtained first computing the Root Mean Squared Error (RMSE) of the obtained
output over time, which results in a 6-dimensional vector $\bm{e}$. Then this
vector is split into the two corners, to compute the norm of each error and
the final average, i.e.,

$\displaystyle\bm{e}$ $\displaystyle=\text{RMSE}(\bm{y}-\bm{r}),$ (11a)
$\displaystyle\text{KPI}_{1}$
$\displaystyle=\frac{1}{2}\left(\|\bm{e}^{r}\|+\|\bm{e}^{l}\|\right),$ (11b)
$\displaystyle\text{KPI}_{2}$ $\displaystyle=\bar{\tau}.$ (11c)

The second KPI is the average computational time per step $\tau$, without
counting the time needed to simulate the nonlinear model. This is used to
check if optimizations are completed within a time step.

These two KPIs were evaluated executing the same 3D trajectory with a range of
different $H_{p}$ values to find a value that yielded low errors with times
under $T_{s}$. The results for the case $T_{s}=0.01$ s and $n=4$ for both
linear models are shown in Figure 7. The vertical blue line represents the
threshold where errors are 10% larger than the minimum value found at
$H_{p}=50$ steps. The red vertical line corresponds to the moment where times
go over $T_{s}$. The window between these lines, $H_{p}\in[11,16]$ represents
the range of possible values with optimal results. In the end, $H_{p}=15$ was
chosen for experiments in these conditions.

Figure 7: KPIs depending on $H_{p}$, for $T_{s}=10$ ms and $n=4$

It is worth noting $T_{s}=0.01$s is the most restrictive value among the
tested ones. Larger time steps allow for larger prediction horizons. On the
other hand, larger mesh sizes ($n=7$) are slower, meaning this analysis had to
be done for each specific case, leading to similar results.

###  IV-C Real Setup

The final closed-loop control scheme was implemented using ROS Kinetic [20].
Figure 8 shows the full diagram indicating the different nodes involved. The
messages that are published at a fixed rate (one per time step) are marked
with a solid line, and with dashed lines all which are not. The separation
between dashes is proportional to how slow these messages are published, with
the optimizer being the fastest among them (it can finish within one time step
or take longer, depending on initial conditions). The Vision node used needs
around $100$ms to process data and publish the mesh positions, thus its output
and the output of the processing node are the slowest ones. The contents of
each message are also specified, with $\bm{p}_{V}^{C}$ being the positions of
all nodes expressed relative to the reference frame of the camera,
$\bm{x}_{V}^{W}$ being the state vector obtained with Vision data in world
base, and $\mathcal{P}_{TCP}^{W}$ is the pose of the TCP (position and
quaternion) in global coordinates.

The RT Node processes the feedback data and updates the B-SOM state, the
additional backup model needed to have updated data between real feedback
samples, as discussed before. Every time step, the variable of initial
parameters, $\bm{P}_{0}$, is updated and published to the Optimizer Node. The
theoretical rate of the Opti Node is also set to $1/T_{s}$, but optimizations
can take longer than that to complete. Once done, the full sequence of control
inputs, $\bm{U}_{Hp}=\bm{u}^{s,*}$, is published and saved on the RT Node.

Figure 8: ROS Diagram of the implemented control scheme, with nodes and
connections between them

The Cartesian controller used to transform from a pose in Cartesian space to
torques in joint space is the one developed in [21], wrapped as a ROS node.
The hardware includes a 7-DoF WAM robot from Barrett [22], used in all
experiments with a custom gripper to hold the cloth by its upper corners (see
Figure 9), and a Kinect camera to obtain RGB-D images as real feedback data.

Figure 9: Picture of the WAM used in the real setup, with details of its end-
effector and piece that connects to the cloth

These images were processed with the Vision Node [23], obtaining all node
positions for a mesh of any given size. As mentioned, this process is the
slowest step in the scheme, at rates around $0.1$s, creating the need of the
B-SOM included in the RT Node.

The vector of positions needed some processing before closing the feedback
loop. First, positions are expressed relative to the camera, so a change of
base was performed. The camera reference frame can be obtained with available
data, as we can obtain the pose of the end-effector from both references:
using Forward Kinematics (FK) from the robot, and adding the known constant
offset from the upper central point of the cloth that the camera is seeing.
This can be written as

$T_{C}^{W}=T_{E}^{W}\cdot
T_{C}^{E}=T_{E}^{W}\cdot\left(T_{E}^{C}\right)^{-1},$ (12)

where $W$ indicates the world/robot reference frame, $C$ the camera and $E$
the end-effector one. Transform $T_{E}^{C}$ is computed in a calibration
process as an average inside a time window (set experimentally to 20 s), as
the camera has a noisy output, with mesh positions varying slightly from frame
to frame even with the cloth being completely still. After calibrating,
$T_{C}^{W}$ is saved for the experiments.

Besides this change of reference, feedback data also had to be filtered. With
the Vision Node being the slowest part of the system, waiting for the next
sample to filter the current one would add a delay between four and ten steps,
which was deemed too much and thus discarded. From the existing online
filtering options using only current and past data points, the most commonly
used ones are Moving Average (MA) filters [24], with several variants
depending on the weights of the points considered. The disadvantage of using
these filters is that their output corresponds to an average of the last
several points, which in an actual movement means the filtered output lags
behind the real position of the mesh, pulled back by the previous positions,
adding a form of delay.

The considered variants were Simple (SMA), Weighted (WMA) and Exponential
(EMA). The same set of experiments was executed for all three variants and
different weight values, as well as using no filter. The best results
(KPI${}_{1}<6$ cm, KPI${}_{2}<T_{s}$) were obtained using EMA, i.e.,

$\bm{y}_{f}(k)=\alpha\bm{y}(k)+(1-\alpha)\bm{y}_{f}(k-1),$ (13)

with $\alpha=0.66$, also obtained empirically. Therefore, this was the
selected filter for all experiments.

This filtered feedback data is sent back to the MPC node. To avoid spurious
and unreliable data affecting the system, a final filtering step discards data
points with delays or distances to the simulated B-SOM over certain
thresholds. In the real implementation, the B-SOM satisfies another important
function when real data is received. The incoming sample can be received on
the RT node with a significant delay with respect to when it was captured
($t_{c}$). Setting this data directly as the current initial state can lead to
unsuccessful tracking, hence the need to update it from $t_{c}$ to current
time. This is done with another instance of the B-SOM simulating the required
steps with a history of previously applied control signals until the current
step, yielding $\bm{x}_{V}$. Finally, the B-SOM state vector $\bm{x}_{SOM}$ is
updated with the new sample with

$\bm{x}_{SOM}\leftarrow W_{V}\bm{x}_{V}+(1-W_{V})\bm{x}_{SOM},$ (14)

where $W_{V}$ is the Vision weight, analogous to an observer gain, added to
reduce the effect of the remaining noise to the MPC and obtain smoother
evolutions.

As a summary of all the steps the captured Vision data must go through to
close the loop, we show Algorithm 1.

Algorithm 1 Closing the loop with Vision feedback data

1:Camera publishing RGB-D images

2:for each image captured at time $t_{c}$ do

3: Use Vision Node to get the mesh positions $\bm{p}(t_{c})$

4: Apply EMA to obtain $\bm{p}_{f}(t_{c})$

5: Order the node positions left to right, bottom to top

6: Apply a base change from camera to world reference

7: Add velocities to obtain full state vector

8: Publish mesh using acquisition time stamp, $\bm{x}_{V}(t_{c})$

9: Receive mesh on MPC Node: callback function

10: Compute delay $\Delta t=t-t_{c}$

11: if $\Delta t>\Delta t_{max}$ then

12: Discard feedback data

13: Exit callback function

14: else

15: Update data using B-SOM $\left\lceil\Delta t/T_{s}\right\rceil$ steps,
get $\bm{x}_{V}(t)$

16: if $\|\bm{x}_{V}(t)-\bm{x}_{SOM}(t)\|>\Delta d_{max}$ then

17: Discard feedback data

18: Exit callback function

19: else

20: $\bm{x}_{SOM}(t)\leftarrow W_{V}\bm{x}_{V}+(1-W_{V})\bm{x}_{SOM}$

21: end if

22: end if

23: Exit callback function

24:end for

##  V RESULTS

All simulations were performed using CPU power only on an i7-8550U @1.80GHz
with 8GB RAM using the optimization toolbox CasADi [25] in MATLAB [26]. All
real experiments were executed with the described setup, using ROS Kinetic
(written in C++), the same CasADi toolbox and the Eigen library [27]. All
executions were done with a square piece of cloth of $30\times 30$ cm.

###  V-A Model Validation

The linear model presented in (1) has seven parameters to tune in order for
the linear model to follow the same behavior as a real cloth: spring stiffness
$k_{x},k_{y},k_{z}$, damping $b_{x},b_{y},b_{z}$ and the $\Delta l_{0z}$
length previously introduced. It was found that these parameters depend on
mesh size and sampling time $T_{s}$. Therefore, a combination of parameters
had to be found for each tested situation.

First, we gathered data of real cloth evolutions (open-loop executions,
without MPC) following a set of trajectories, to ensure movement in all
directions of space. After being filtered and regularized in time, these
evolutions were used to find the parameters of the linear model via
Reinforcement Learning (RL). Concretely, the Relative Entropy Policy Search
(REPS) algorithm was used [28], penalizing the squared errors in node
positions for the entire mesh, but with higher weights for the lower corner
nodes, as they will be the output to be tracked.

Learning experiments were conducted to learn the model parameters for square
meshes of sizes $n=4$ and $7$, and for time steps of $T_{s}=10,\ 15,\ 20$ and
$25$ ms, obtaining results for all eight combinations. Figure 10 shows the
evolutions of the lower corner positions for the learnt linear model ($n=4,\
T_{s}=15$ ms) and the real cloth (dashed black line). We can see how both
evolutions have the same behavior, with a final RMSE of 1.5 cm.

Figure 10: Lower corner evolutions of the learnt linear model ($n=4,\
T_{s}=15$ ms) and the data gathered from the real cloth (dashed line)

Even though errors increase with $n=7$ and larger time steps due to small
oscillations that try to capture the nonlinear behavior of the real cloth, the
errors are always within the same order of magnitude, and the lower corner
evolutions of the linear model have the same behavior as the real ones.
Therefore, the model in (1) is validated with a real cloth, with parameter
combinations for all studied cases.

###  V-B Controller Structure and Tuning

Given the amount of parameters and weights present in the predictive
controller, REPS was also used in closed-loop simulations to find its optimal
structure and tuning. All experiments were made to learn the weight values
inside matrices $\bm{Q}$ and $\bm{R}$ depending on different conditions, with
the following options:

  * •

Using the adaptive weight in (6) or not to obtain $\bm{Q}$.

  * •

Minimizing the control signals $\bm{u}$ or the slew rates $\bm{\Delta u}$.

  * •

Three different structures of matrices $\bm{Q}$ and $\bm{R}$: scalars times
the identity ($q\bm{I},r\bm{I}$), with different weights per coordinate only
for $\bm{Q}$ ($\bm{Q}_{xyz},r\bm{I}$), or with both having different weights
per coordinate, but with the matrices being proportional to each other
($\bm{Q}_{xyz}=k\bm{R}_{xyz}$).

  * •

A reward function only penalizing tracking RMSE or also adding a cost for
computational times over the considered step time
($\mbox{TOV}=\max(\bar{\tau}/T_{s}-1,\ 0)$).

This results in a total of 24 learning experiments executed. We can organize
their results in groups with the same weight structure and reward function,
and make comparisons purely based on the use of $\bm{\Delta u}$ and
$\bm{Q}_{a}$, as shown in Figure 11.

Figure 11: RMSE obtained with all the considered structure options

The worst results are always obtained using $\bm{Q}_{a}$ and $\bm{u}$
regardless of weight structure and reward function, and the opposite
selection, with a constant $\bm{Q}$ and $\bm{\Delta u}$, yields the best
outcome. This is the reason behind using $\bm{\Delta u}$ in (9).

Differences in computational time are minimal, with only a slight time
increase when using $\bm{\Delta u}$. Knowing how time measurements are
sensitive to memory and CPU usage, and obtaining roughly the same results with
both reward functions, the second one, with TOV, was discarded for the
following experiments.

The final tracking error is approximately the same regardless of the chosen
weight structure (less than 0.5% difference between best and worst), and the
minimum errors are actually obtained with the first and simplest option. This
means that the more complex structures, with weights depending on direction,
do not adapt better to the trajectory leading to smaller errors. Experiments
on two other trajectories confirmed this tendency, which, together with
simplicity, pushed us to select the first structure, with just two weights,
leaving only one degree of freedom to learn as the final controller tuning:
the proportion between $\bm{Q}=q\bm{I}$ and $\bm{R}=r\bm{I}$.

Proceeding with more learning experiments, it was observed that the tracking
errors decreased with low $\bm{R}/\bm{Q}$ ratios, until a certain threshold
where $\bm{R}$ is too small and the system can unstabilize. This behavior was
found to be consistent across all tested trajectories and conditions, with the
threshold only varying slightly between them.

This led us to set a unique tuning to guarantee the best performance in any
given situation. The idea is getting as close as possible to the limit without
crossing it for any case. A possible combination that works safely for all
considered trajectories is $\bm{R}/\bm{Q}=0.2$ (e.g., $q=1,\ r=0.2$). It is
worth noting that, even for the case where the threshold had the lowest ratio,
using 0.2 increased the error less than 1 mm, which does not represent a great
sacrifice in performance.

###  V-C Trajectory Tracking Results

With the scheme successfully tested in simulation, real experiments were
conducted with the conditions explained in Section IV. To obtain the best
trajectory tracking, an experimental analysis was conducted with two parts.
The first one compared results with different linear model sizes in the same
set of situations, while the second analyzed the effects of the remaining
control parameters ($T_{s},\ H_{p},\ W_{V}$) on tracking performance.

####  V-C1 Linear model sizes

Given that we have two available sizes and two linear models, and that the COM
must be simpler or the same as the B-SOM, there are three possible
combinations. Executing them in five different situations, we obtained the
results shown in Figure 12. It is clear how larger mesh sizes yield worse
results, meaning the increase in computational time is more important than the
improvement in accuracy. Therefore, size $n=4$ was selected for both linear
models in the following experiments.

Figure 12: Obtained RMSE using different linear model sizes

####  V-C2 Control analysis

The range of $T_{s}$ values analyzed comes from the obtained parameters
through learning, being 10, 15, 20 and 25 ms. The prediction horizon $H_{p}$
can take any arbitrary ($\mathbb{Z}^{+}$) value, but thanks to the executed
simulations, we have an indicative range with relatively low errors without
increasing computational times over the limit. As the conditions in the real
setup are different with regards to timing and feedback data, horizons were
tested from 10 to 30 steps, in increases of 5 (10, 15, 20, 25 and 30).
Finally, the weight $W_{V}$ was tested in increases of 10% from 0 to 50%.
While higher values were also tested, the majority of combinations resulted in
unsuccessful executions. This results in a total of 120 finished real-robot
experiments under the same conditions except for these three parameters. After
they were completed, however, some additional changes were made to test
executions with higher $W_{V}$. They only worked after reducing the Cartesian
controller gains and using a filter with $\alpha=0.5$, but all cases with
$W_{V}=1$ (100%) finished correctly. This makes a total of 140 experiments,
with all their results shown in Figure 13. Given the conditions for $W_{V}=1$
had to be different from the rest, and that the obtained results were
consistently worse than their $W_{V}=0.5$ counterparts, no values of $W_{V}$
were tested in between to keep them separated and focus on the 120 executed
with the same conditions. It is worth noting that the worst result within
these 120 experiments had an RMSE of 12.1 cm, thus adding the additional
experiments does not change the color scale significantly. We have also marked
all results within the best 10% of errors, i.e., with an RMSE lower than 4 cm
with a magenta circle.

Figure 13: Results of all executed experiments depending on $T_{s},\ H_{p},\
W_{V}$

With these results, we can clearly see how a low $T_{s}$ combined with a high
$H_{p}$ does not produce correct tracking. During execution, these experiments
produced several timeouts on the optimizer, trying to keep up with a very fast
rate while predicting a lot of steps into the future. Other issues were
detected during the execution of all experiments with $T_{s}=10$ ms and
$W_{V}\geq 0.3$, where feedback data was consistently being discarded either
for it being too much into the past (could not be updated) or too distant to
the simulated state of the B-SOM. This means that these experiments actually
have an effect of the feedback data closer to $W_{V}=0$ than their actual
value, as most of the time the real data was unusable. Furthermore, a smaller
time step results in an increased difference between the rate at which the
control signals are computed and sent to the robot and the rate at which the
Vision node outputs feedback data, being around 10 times slower in these
conditions. This makes computed errors less reliable, as there are fewer
captured points to compare with the reference. With all this, even if we have
some results with really good tracking errors using $T_{s}=10$ ms, it is clear
that this sampling time is too fast for the majority of cases, and must be
avoided to obtain the best possible tracking results.

For both $T_{s}=15$ ms and $T_{s}=20$ ms, a horizon of 10 steps is too short
to track correctly, regardless of $W_{V}$. This effect disappears with
$T_{s}=25$ ms, having optimal results with the shortest $H_{p}$ too, which
means that it is a problem related to total prediction time and maximum
allowed time for the optimizer.

We can also see a tendency of errors increasing with higher $W_{V}$, with some
optimal results being obtained without considering the Vision feedback at all.
This is a product of noise, present even after filtering. In the executed
experiments, there were no strong rotations, sudden movements, offsets, nor
other disturbances (e.g., wind or human actions), and the B-SOM always started
in the exact same position as the real cloth, making its simulated evolution
an accurate one without any added noise. Of course, $W_{V}=0$ means the
control loop is not closed, and cannot be applied in a general scenario, where
external forces or initial deviations can make the B-SOM state have an
unreliable evolution, and the real feedback would have to correct its state.
Unfortunately, with the current camera and Computer Vision algorithms, this
comes at the cost of updating the initial state of the MPC with noisy data,
which can increase optimization times. A general application of this scheme
would need a camera with a faster refresh rate, more precision, not fixed in
place to enable more movements and orientations without losing the cloth, and
a fast and reliable algorithm to obtain an updated mesh.

All in all, even discarding cases with $T_{s}=10$ ms, $W_{V}=0$ and 1, and the
discussed cases with $H_{p}=10$, there is no singular delimited region with
optimal results, but a tendency towards longer prediction horizons as the time
step increases and the MPC module has more time to compute. Even then, all the
remaining cases, 65 different combinations, yielded errors lower than 5.5 cm,
which is not far from the previously considered threshold, and an acceptable
error considering the large range of options it includes, and the precision of
the camera and Vision algorithms used, also in the order of centimeters.

####  V-C3 Final tracking performance

Choosing $T_{s}=20$ ms, $H_{p}=25$ and $W_{V}=0.2$ as an example of a
combination that yielded optimal tracking, with an RMSE of only 3.8 cm, we can
plot the evolutions of all corners, as shown in Figure 14. We can see how some
noise persists, but the reference is tracked accurately.

Figure 14: Cloth corner evolutions for $T_{s}=20$ ms, $H_{p}=25$, $W_{V}=0.2$

###  V-D Experiments with Disturbances

Two different cases were studied: blocking the camera and applying forces to
the robot arm during execution.

####  V-D1 Blocking the camera

During execution, a human walked between the camera and the cloth, covering
its view for about two seconds on two occasions. The obtained results are
shown in Figure 15, where shaded areas correspond to moments where the camera
was blocked.

Sudden changes in captured mesh positions are caught before updating the state
of the linear models and discarded, so even if we see them in the Vision data,
they do not affect the controller. This is checked with the obtained TCP
evolutions, smooth even during these moments. This can be achieved thanks to
the B-SOM inside the controller, which simulates the real evolution of the
cloth during the moments where the vision feedback cannot provide updated
data.

Figure 15: Cloth corners and TCP evolutions with camera blocking

####  V-D2 Human interaction

The used Cartesian controller allows movements in the null space of the WAM
without offering much resistance [21]. If a human moves the elbow joint, while
theoretically it would not change the TCP pose, during a real experiment there
are slight displacements and forces that are transmitted from the arm into the
cloth, producing disturbances that are picked up by the camera.

In fact, the captured experiment started with a person walking in front of the
camera as before, and after moving the elbow joint, the rigid piece connecting
the upper corners of the cloth was pressed down on the right side, causing a
slight rotation and a vibration when released. This situation can be seen in
Figure 16.

Figure 16: Disturbance created by a person poking the cloth

The results of this experiment are shown in Figure 17. The shaded region in
magenta corresponds to the interval when the camera was blocked, as in the
previous experiment, while the region shaded in gray corresponds to the time
when a human agent was interacting with the robot arm.

Figure 17: Cloth corners and TCP evolutions with human interaction

Blocking the camera has the same effects as before. Moving the elbow of the
arm (from around 11 s to 15 s) produces slight movements in the TCP, which are
also visible in the upper corners, and are propagated to have a much greater
effect on the lower corners due to the non-rigid nature of the cloth. When the
rigid piece between the upper corners is pressed and released around the 15 s
mark, we can see how it creates a sudden disturbance, very clear in the right
upper corner, which adds to the oscillations already present in the lower
corners. Even in these situations, however, we can see how the reference
trajectory is tracked correctly.

With these results, we can say that the developed implementation has proven to
work in demanding conditions, and even under the effects of an external agent
creating disturbances while keeping a good trajectory tracking performance.
These last two experiments yielded RMSEs of 5.5 and 5.6 cm, respectively, once
the incorrect data obtained when the camera was partially obstructed (magenta
regions) had been removed.

###  V-E Error Analysis

A final set of experiments was conducted with a completely new and different
trajectory, which was also printed in real scale to record videos showing the
obtained tracking behavior. One of these videos can be found in the
supplementary material. These experiments were conducted to test the control
scheme in situations outside the ones used for testing and obtaining the
optimal parameters, and to do an analysis of all the possible error sources,
quantifying each one.

From the components of the used control scheme, shown in Figure 8, the ones
that can introduce significant errors are i) the MPC, ii) the Cartesian
Controller, iii) the physical robot, and iv) the Vision feedback, both from
the actual camera limitations and the processing algorithm.

Figure 18: Obtained trajectories of all relevant signals in the control
scheme, showing the different error sources

To evaluate the errors introduced by each of these components, we can compare
the following data: i) the input reference, which is a sequence of lower
corner positions, ii) the MPC output, a TCP pose from which we can obtain the
corresponding desired TCP positions, iii) the actual position of the TCP at
all times obtained via IK, and finally, iv) real Vision data, processed to
obtain positions of the entire mesh every instant, and the lower corners in
particular. To compare TCP (ii & iii) and lower corner positions (i & iv), we
can add a fixed offset of the initial distance between them. This is an
approximation, as the cloth is flexible and their relative position can
change, but these differences are reduced on slow trajectories, as the tested
one.

The new trajectory consists in a sinusoidal movement in the X-axis while
moving closer to the camera on a constant height, followed by a linear
movement to return to the initial position. In Figure 18, we show a comparison
of the four aforementioned signals for the lower right corner in one
execution.

The MPC output (plus a fixed offset) follows the reference trajectory closely
in situations of constant velocity, but it tends to smooth out changes in
direction. Part of this behavior can be explained due to its predictive
nature, seeing future points after the direction change, and also given the
flexible nature of the linear cloth model (COM), which will continue going in
the previous direction due to inertia for a short time after the change. Even
without taking these factors into account, the worst error is of 4.8 cm, and
happens in one of the sharp changes in X. The RMSE between these two
trajectories is of only 1.7 cm.

Additionally, we can compare the errors due to orientation, given that the
reference trajectory has the TCP pointing straight down. Of course, we cannot
say a change in rotation produces an error of the angle times the entire
length of the cloth, as it is a non-rigid object, but we have a rigid link
between the TCP and the cloth of 9 cm that can produce some position error due
to changes in orientation. In this case, the MPC output follows the reference
almost perfectly, with maximum errors of 0.1 mm in position due to orientation
errors.

Next, we can obtain the error between the desired TCP pose and the real one,
also saved throughout the experiment. In Figure 18, we can see how these two
trajectories (blue and magenta) are close, with some discrepancies in the
Z-axis and especially on the change of direction in the Y-axis. The maximum
position error between the two is of 2.6 cm, with an RMSE of 1.7 cm. In the
plots, we can see how in the most critical points (turns), the two mentioned
errors are additive, meaning the actual TCP position is even further to the
input reference. With regards to changes in position due to orientation
errors, here they are more notable, with a maximum of 1 cm and an RMSE of 5
mm.

Finally, the last signal corresponds to the processed data from the Vision
algorithm. While we could expect this signal to deviate from the TCP
evolutions due to the flexible nature of the cloth piece, ideally being
between the last signals and the input reference, here we are also adding the
errors produced by the camera itself, with its maximum resolution and frame
rate, and by the subsequent algorithm to detect and extract the positions of
all the mesh nodes, and as we can see in Figure 18 (red line), as in all
previous experiments, the obtained data is very noisy. Besides that, it is
clear how it is also unreliable in the Z-axis, as the TCP does not change in
this direction, the cloth is always extended vertically, but we get an
evolution with constant changes. This difference being the most severe in this
axis can be due to the table placed directly under the cloth with the
reference trajectory, to serve as a visual indicator for the videos, which
might affect the detection process. In the X and Y axes, we can see how the
reference is tracked correctly, with the largest deviations being in the
direction changes. While in X the trajectory seems to be between the TCP and
reference, in Y we always have it further from the reference, adding onto the
previous errors.

The final RMSE of the experiment (from reference to Vision data) is of 4.1 cm,
but the error is not constant or distributed evenly, as it reaches maximums of
4.4, 5.7 and 5.8 cm in X, Y and Z, respectively. While these errors can seem
large at first, we must point out that they are the result of adding several
sources together, and we do not have the exact position of the lower corners
without camera noise and error, making difficult the isolation of the error
due to the predictive controller. For example, for the worst case in the
Y-axis, the difference between reference and MPC output is 1.4 cm, as shown in
Figure 19 (in blue), while the remaining 4.3 cm come from other sources
(Cartesian controller and physical actuators in red, and not predicted cloth
behaviors and Vision error together in yellow). Additionally, this difference
is a conservative measure, as we are adding a constant offset to the TCP
position and not accounting for the inertia and non-rigid behavior of the
cloth, which, in theory, would help make the real evolution be closer to the
input reference. We can see how this is not the case here, but as another
example, in the X-axis, the captured evolution is closer to the reference than
the MPC output. This is what we would expect in good tracking, as the MPC must
consider these behaviors and correct them, and the TCP evolution must not be a
perfect copy of the reference trajectory with a constant offset, as it would
be with a rigid piece. In this case then, we can only remove the error between
the desired and actual TCP pose (around 1 cm) and say that the remaining 3.4
cm of error are shared between MPC and Vision processing.

Figure 19: Mean and maximum errors in the Y-axis of the right lower corner,
and their distribution in different sources

All in all, without separating error sources, we obtain values of KPI1 similar
to the ones obtained before, and this new analysis proves that the errors come
from different parts of the control scheme and are added together, and how the
Predictive Controller itself is not the main source of error.

###  V-F Comparison

The obtained control strategy is suitable for dynamic cloth manipulation, but
it should be compared with other existing techniques. Firstly, the simplest
strategy one could think of is to use a classical controller such as a PID.
However, it is not possible to use a PID because it cannot incorporate the
dynamical model and the motion predictions.

Secondly, we can compare our proposal with existing MPC techniques applied to
cloth manipulation. In [12], the goal of the authors is to have position
precision on the entire cloth. Hence, solving the optimization problem takes
several minutes while we solve it in milliseconds, as we are only focused on
the lower corners positions. Another existing technique for cloth manipulation
is presented in [13], where a PR2 robot folding towels is presented. However,
the cloth manipulation is static or quasi-static, as in the majority of the
existing literature. By contrast, our approach is to dynamically manipulate
cloth by incorporating a dynamic cloth model in the control system. A
different dynamic manipulation approach is presented in [29], complementary to
the one presented here. It tries to learn cloth movement without a previous
model using RL. However, the learning requires thousands of simulated samples,
and the policies generated for specific tasks would require further real-robot
samples to transfer from simulation to reality.

##  VI CONCLUSIONS

In this paper, we have presented an MPC strategy to track the trajectories of
the lower corners of a piece of cloth by only grasping the upper corners with
one robot arm. To the best of the authors’ knowledge, this is the first time
that an MPC using a cloth model is used in real time for this kind of
manipulation. Moreover, we have developed a linear cloth model that is shown
to be an accurate approximation of real cloth and, therefore, is suitable to
be used as a Control-Oriented Model in the MPC strategy.

Both the linear cloth model and the predictive controller have been improved
via Reinforcement Learning, finding the parameters of the model in several
scenarios to validate it against the behavior of a real cloth and enable its
use inside the MPC, as well as learning the optimal structure and tuning of
the controller.

Simulations showed accurate 3D trajectory tracking results, yielding mean
errors in the order of millimeters for a cloth of 30$\times$30 cm, and solving
the optimization problem in near real time.

After simulating, a full closed-loop control scheme was implemented in a real
setup, including MPC, a Cartesian controller, and Vision feedback and
processing. Working with a real environment raised additional issues, like
noise and timing, which had to be addressed. The final implementation runs at
a constant rate fixed by the chosen $T_{s}$, filters the output noise and has
fail-saves against optimizations taking too long and vision data being too
unreliable (with a B-SOM serving as a reference). Tracking errors increase
with respect to the simulation values, to the order of centimeters, but they
include new sources of error only present in the real setup, like sensor
noise, actuator tolerances, and Cartesian controller inaccuracies. Even with
disturbances, the errors (RMSE) are around the 5 cm mark, and an error
analysis shows its principal source is not the Predictive Controller, even
when considering a conservative approach.

The outcome of this paper can serve as a proof of concept of a new methodology
for robotic cloth manipulation: using MPC with cloth dynamics prediction in
real-time. A key aspect that allows for such real-time capability is the fact
that, unlike other approaches [12], we do not need to predict an accurate
evolution for the entire cloth mesh, but rather few relevant points (e.g.,
lower corners plus the grasped points) can provide most of the information
needed to achieve success in a task. There are, however, many aspects that can
be further developed in the future:

  * •

While the cloth models proposed can be changed according to the user/task
demands and situations, finding a general expression for the parameters of the
linear model could improve the proposed method in this paper. We have seen how
these parameters depend on both $T_{s}$ and mesh size $n$, and found the
values for some combinations through learning, but an interesting step could
be to try to find relations between these two conditions and the resulting
parameters, and with enough time and data, even a model that can be used for
any combination within a given range of values.

  * •

Applying Online Learning. The found tuning and controller structure have
proven to be the optimal ones for the tested cases, but for a more general
application, a learning algorithm could use recent data during executions to
adapt the parameters of the controller to their optimal values specifically
for the task at hand.

  * •

Change the Vision setup. This is probably the most important step to improve
the setup, which includes using a faster and more precise camera, and a faster
identification algorithm to obtain feedback data, but more importantly,
changing from a situation where a single camera is fixed in place to another
setup with either multiple cameras or mobile ones. This would allow for a much
more varied range of trajectories and tasks that could be executed, and the
control scheme could be tested to its maximum potential, without the Vision
part being a bottleneck that limits manipulation speed.

## References

  * [1] A. Colomé and C. Torras, “Dimensionality reduction for dynamic movement primitives and application to bimanual manipulation of clothes,” IEEE Transactions on Robotics, pp. 602–615, 2018.
  * [2] A. Colomé, A. Planells, and C. Torras, “A friction-model-based framework for reinforcement learning of robotic tasks in non-rigid environments,” in 2015 IEEE International Conference on Robotics and Automation (ICRA), pp. 5649–5654, 2015.
  * [3] A. Nair, D. Chen, P. Agrawal, P. Isola, P. Abbeel, J. Malik, and S. Levine, “Combining self-supervised learning and imitation for vision-based rope manipulation,” in 2015 IEEE International Conference on Robotics and Automation (ICRA), pp. 2146–2153, 05 2017.
  * [4] J. B. Rawlings, D. Q. Mayne, and M. Diehl, Model predictive control: theory, computation, and design.  Nob Hill Publishing Madison, 2017.
  * [5] J. M. Maciejowski, Predictive control: with constraints.  Pearson education, 2002.
  * [6] M. Bhardwaj, B. Sundaralingam, N. R. A. Mousavian, F. R. D. Fox, and B. Boots, “Storm: An integrated framework for fast joint-space model-predictive control for reactive manipulation,” in Conference on Robot Learning (CoRL), p. to appear, 2021.
  * [7] X. Man and C. C. Swan, “A mathematical modeling framework for analysis of functional clothing,” Journal of Engineered Fibers and Fabrics, vol. 2, no. 3, p. 155892500700200302, 2007.
  * [8] D. Terzopoulos, J. Platt, A. Barr, and K. Fleischer, “Elastically deformable models,” in Proceedings of the 14th Annual Conference on Computer Graphics and Interactive Techniques, pp. 205–214, 1987.
  * [9] A. Nealen, M. Müller, R. Keiser, E. Boxerman, and M. Carlson, “Physically based deformable models in computer graphics,” in Computer Graphics Forum, vol. 25, pp. 809–836, Wiley Online Library, 2006.
  * [10] D. Baraff and A. Witkin, “Large steps in cloth simulation,” in Proceedings of the 25th Annual Conference on Computer Graphics and Interactive Techniques, pp. 43–54, 1998.
  * [11] X. Hu, Y. Bai, S. Cui, X. Du, and Z. Deng, “Review of cloth modeling,” in ISECS International Colloquium on Computing, Communication, Control, and Management, vol. 4, pp. 338–341, 2009.
  * [12] Y. Bai, W. Yu, and C. K. Liu, “Dexterous manipulation of cloth,” in Computer Graphics Forum, vol. 35, pp. 523–532, Wiley Online Library, 2016.
  * [13] J. Maitin-Shepard, M. Cusumano-Towner, J. Lei, and P. Abbeel, “Cloth grasp point detection based on multiple-view geometric cues with application to robotic towel folding,” in 2010 IEEE International Conference on Robotics and Automation, pp. 2308–2315, IEEE, 2010.
  * [14] W. Yan, A. Vangipuram, P. Abbeel, and L. Pinto, “Learning predictive representations for deformable objects using contrastive estimation,” in Proceedings of the 2020 Conference on Robot Learning (J. Kober, F. Ramos, and C. Tomlin, eds.), vol. 155 of Proceedings of Machine Learning Research, pp. 564–574, PMLR, 16–18 Nov 2021.
  * [15] Z. Erickson, H. M. Clever, G. Turk, C. K. Liu, and C. C. Kemp, “Deep haptic model predictive control for robot-assisted dressing,” in 2018 IEEE International Conference on Robotics and Automation (ICRA), pp. 4437–4444, IEEE, 2018.
  * [16] X. Provot et al., “Deformation constraints in a mass-spring model to describe rigid cloth behaviour,” in Graphics Interface, pp. 147–147, Canadian Information Processing Society, 1995.
  * [17] C. Zhou, X. Jin, and C. C. Wang, “Efficient and stable simulation of cloth undergoing large rotations,” Computing in Science & Engineering, vol. 10, no. 4, pp. 30–40, 2008.
  * [18] S. Boyd, S. P. Boyd, and L. Vandenberghe, Convex optimization.  Cambridge university press, 2004.
  * [19] F. Coltraro, J. Amorós, M. Alberich-Carramiñana, and C. Torras, “An inextensible model for the robotic manipulation of textiles,” Applied Mathematical Modelling, vol. 101, pp. 832–858, 2022.
  * [20] Stanford Artificial Intelligence Laboratory, “Robot Operating System - Kinetic Kame.” https://wiki.ros.org/kinetic.  [Online; accessed July 2021].
  * [21] D. Parent, A. Colomé, and C. Torras, “Variable impedance control in cartesian latent space while avoiding obstacles in null space,” in 2020 IEEE International Conference on Robotics and Automation (ICRA), pp. 9888–9894, IEEE, 2020.
  * [22] Barrett Technology, Inc., “Barrett WAM 7-DOF Joints & Frames.” https://web.barrett.com/files/B2576_RevAC-00.pdf, 2005.  [Online; accessed July 2021].
  * [23] M. Arduengo, C. X. Zheng, A. Colomé, and C. Torras, “Cloth Point Cloud Segmentation.” https://github.com/MiguelARD/cloth_point_cloud_segmentation, 2021.
  * [24] S. W. Smith, “Chapter 15 - moving average filters,” in Digital Signal Processing (S. W. Smith, ed.), pp. 277–284, Boston: Newnes, 2003.
  * [25] J. A. E. Andersson, J. Gillis, G. Horn, J. B. Rawlings, and M. Diehl, “CasADi – A software framework for nonlinear optimization and optimal control,” Mathematical Programming Computation, 2018.
  * [26] The MathWorks Inc., “Matlab version 9.7.0 (r2019b),” 2019.
  * [27] G. Guennebaud, B. Jacob, et al., “Eigen v3.” http://eigen.tuxfamily.org, 2010.  [Online; accessed July 2021].
  * [28] J. Peters, K. Mulling, and Y. Altun, “Relative entropy policy search,” in Twenty-Fourth AAAI Conference on Artificial Intelligence, 2010.
  * [29] R. Jangir, G. Alenyà, and C. Torras, “Dynamic cloth manipulation with deep reinforcement learning,” in 2020 IEEE International Conference on Robotics and Automation (ICRA), pp. 4630–4636, IEEE, 2020.