O O O(q)A^#2_#1#3

# On the Collocated Form with Input Decoupling
of Lagrangian Systems

Pietro Pustina1, Cosimo Della Santina2,3, Frédéric Boyer4, Alessandro De
Luca1, Federico Renda5 The research of Pietro Pustina was supported by the US
Office of Naval Research Global under Grant N62909-21-1-2033, and in part by
the Khalifa University of Science and Technology under Grants CIRA-2020-074,
RC1-2018-KUCARS.The research of Cosimo Della Santina was supported by the
Horizon Europe Program from Project EMERGE under Grant 101070918.1Department
of Computer, Control and Management Engineering, Sapienza University of Rome,
00185 Rome, Italy<EMAIL_ADDRESS><EMAIL_ADDRESS>of Cognitive Robotics, Delft University of
Technology, Delft, The Netherlands<EMAIL_ADDRESS>of
Robotics and Mechatronics, German Aerospace Center (DLR), 82234
Oberpfaffenhofen, Germany.4 Institute Mines Telecom Atlantique, 44307 Nantes,
France<EMAIL_ADDRESS>University Center for
Autonomous Robotics System (KUCARS) and the Department of Mechanical
Engineering, Khalifa University of Science and Technology, 127788 Abu Dhabi,
UAE<EMAIL_ADDRESS>
###### Abstract

Suitable representations of dynamical systems can simplify their analysis and
control. On this line of thought, this paper considers the input decoupling
problem for input-affine Lagrangian dynamics, namely the problem of finding a
transformation of the generalized coordinates that decouples the input
channels. We identify a class of systems for which this problem is solvable.
Such systems are called collocated because the decoupling variables correspond
to the coordinates on which the actuators directly perform work. Under mild
conditions on the input matrix, a simple test is presented to verify whether a
system is collocated or not. By exploiting power invariance, it is proven that
a change of coordinates decouples the input channels if and only if the
dynamics is collocated. We illustrate the theoretical results by considering
several Lagrangian systems, focusing on underactuated mechanical systems, for
which novel controllers that exploit input decoupling are designed.

###### Index Terms:

Underactuated Robots, Dynamics, Motion Control, Modeling, Control, and
Learning for Soft Robots.

##  I Introduction

Electrical, hydraulic, and mechanical systems, or their combinations, are
Lagrangian systems that usually exhibit complex behavior. However, their
physical nature displays special properties, such as symmetry and passivity,
which have been exploited to solve many control problems [1, 2, 3, 4, 5],
otherwise difficult to address for generic nonlinear dynamics. To cope with
their high nonlinearity and large number of degrees of freedom (DOF),
representations with specific structures play a crucial role in simplifying
analysis, as well as control design and synthesis. For example, coordinate
transformations are often used to highlight some internal structure that
simplify derivation of feedback controllers for robotic systems and prove
their stability [6, 7, 8, 9].

Figure 1: Graphical representation of the proposed change of coordinates
addressing the input decoupling problem for a fully actuated Lagrangian
system. In the ${\boldsymbol{q}}$ coordinates, the input $\boldsymbol{u}$
undergoes a nonlinear transformation through the actuation matrix $\ActM$ when
performing work on ${\boldsymbol{q}}$, i.e.,
$\boldsymbol{\tau}_{{\boldsymbol{q}}}=\ActM\boldsymbol{u}$. The proposed
change of coordinates $\boldsymbol{\theta}=\boldsymbol{h}({\boldsymbol{q}})$
bends the configuration space so that each component of $\boldsymbol{u}$ acts
directly on one component of $\boldsymbol{\theta}$, namely
$\boldsymbol{\tau}_{\boldsymbol{\theta}}=\boldsymbol{u}$. The existence of
this transformation is possible because of the conservation of power
$\dot{\mathcal{H}}_{{\boldsymbol{q}}}$, represented by the yellow area, under
change of coordinates, i.e.,
$\dot{\mathcal{H}}_{{\boldsymbol{q}}}=\dot{\mathcal{H}}_{\boldsymbol{\theta}}$.

This paper considers the Input Decoupling (ID) problem for input-affine
Lagrangian systems. In particular, given a Lagrangian system whose inputs
enter the second-order equations of motion through a configuration-dependent
actuation matrix, we study under which conditions on the definition of
generalized coordinates each input affects one and only one equation of
motion. The coordinates solving the ID problem constitute a set of variables
that simplify control design and synthesis because the input directly affects
the equations of motion in a decentralized form. If the decoupling coordinates
are regarded as system outputs, any control law implemented in these
coordinates automatically decouples also the input-output channels. For fully
actuated or overactuated Lagrangian systems, i.e., when the number of
independent inputs is equal or larger than the number of generalized
coordinates, the ID problem can be solved by a configuration-dependent input
transformation that inverts (or pseudoinverts) the actuation matrix. On the
other hand, in the case of underactuated systems, a solution is available so
far if the actuation matrix is constant and requires only a linear change of
generalized coordinates [10].

For multi-input-multi-output (MIMO) plants described in state space form, the
input-output decoupling problem is solved by an inversion-based feedback
controller, both in the linear [11] and nonlinear case [12, 13, 14]. For this,
a feedback transformation is needed that requires a state and input
transformation together with a feedback action. In particular, input-output
decoupling is possible by a static state feedback if and only if the system
has well defined vector relative degree, namely a nonsingular decoupling
matrix [15, 16]. This result can be used to solve the ID problem for
Lagrangian systems by taking the configuration variables (or a subset of them)
as system outputs. In fact, in this way, the system has a vector relative
degree, with all outputs having uniform relative degree two. It can be shown
that in case of underactuation the necessary and sufficient condition is the
involutivity of the distribution spanned by the columns of the actuation
matrix [17]. Unfortunately, involutivity is not easy to check, especially for
high dimensional dynamics, and has to be evaluated case by case, see [18, 19,
20, 21]. Furthermore, finding the input decoupling coordinates in this case
requires solving a system of nonlinear partial differential equations [16,
Chap. 5], which could be impractical for control synthesis. Finally, in [22],
the authors have proposed input transformations for dynamic processes that
achieve exact linearization and input decoupling, under complete model
knowledge. However, the analysis is restricted to systems with equal number of
states, inputs, and outputs.

In this paper, we focus on Lagrangian systems and we show that the choice of
particular coordinates, called actuation coordinates, solve the ID problem
without requiring a configuration-dependent transformation of the input nor a
state-feedback. We derive necessary and sufficient conditions under which the
actuation coordinates exist and constructively show that these coordinates
solve the ID problem for fully actuated, overactuated, and underactuated
Lagrangian systems. In particular, when the system is underactuated, the
unactuated coordinates remain arbitrary.

These results stem from power invariance under change of coordinates, as
graphically illustrated in Fig. 1 for the fully actuated case. Remarkably,
similar considerations hold also when the dynamics is underactuated. We apply
the results to several mechanical examples as archetypal Lagrangian systems.
In addition, we prove that robotic systems driven by thread-like actuators,
such as inelastic tendons or thin fluidic chambers, always admit actuation
coordinates. Our results have relevant consequences on the control of
continuum soft robots and other underactuated mechanical systems. Indeed,
recent control laws for planar underactuated soft robots [23, 24, 25, 10, 26]
generalize to all collocated mechanical systems with damping, in particular to
soft robots moving in 3D. Interestingly, the energy-based regulator in [27]
uses in fact actuation coordinates to decouple the equations of motion of a
3-DOF underactuated soft robotic system. Similarly, in flexible link robots,
the actuation coordinates are the clamped angles at the base of each beam,
which have been extensively used in control design [28].

The paper is organized as follows. In Sec. II, we introduce the notation and
formalize the ID problem. Section III defines the class of Lagrangian systems,
called collocated, for which the ID problem is solvable. Necessary and
sufficient conditions for solving the ID problem are then derived for fully
actuated or overactuated dynamics (Sec. IV), and for underactuated systems
(Sec. V). In Sec. VI, we prove that thread-like actuators yield collocated
mechanical systems. Section VII extends two control strategies derived for
underactuated mechanical systems with constant actuation matrix to the
collocated case, validating one of these controllers on a 3D tendon-driven
underactuated soft robot. Finally, conclusions and future works are summarized
in Sec. VIII.

##  II Preliminaries

###  II-A Notation

We denote vectors and matrices with bold letters. Arguments of the functions
are omitted when clear from the context. Table I presents the notation adopted
in the paper.

Table I: Nomenclature Symbol |  Description
---|---
$\mathbb{R}^{n}$ |  Euclidean space of dimension $n$
$\mathbb{S}^{n}$ |  Unit sphere of dimension $n$
$\mathbb{R}^{n\times m}$ |  Space of $n\times m$ matrices over $\mathbb{R}$
$\mathcal{X}$ |  Manifold of interest with $\mathcal{X}=\\{\mathcal{M},\mathcal{N}\\}$
$\mathbb{R}^{>0}$ |  Positive real numbers $n$
$T_{\boldsymbol{v}}\mathcal{X}$ |  Tangent space of manifold $\mathcal{X}$ at $\boldsymbol{v}\in\mathcal{X}$
$\mathcal{B}(\boldsymbol{v})$ |  Neighbourhood of $\boldsymbol{v}\in\mathcal{X}$
$\mathfrak{se}(3)$ |  Special Euclidean algebra of dimension $3$
$\mathfrak{so}(3)$ |  Special orthogonal algebra of dimension $3$
$\boldsymbol{I}_{n}\in\mathbb{R}^{n\times n}$ |  Identity matrix of dimension $n$
$\boldsymbol{O}_{n\times m}$ |  Zero matrix of dimension $n\times m$
$\boldsymbol{P}>0$ |  Symmetric positive definite matrix
$\boldsymbol{S}_{i}\in\mathbb{R}^{n}$ |  Column $i$ of matrix $\boldsymbol{S}\in\mathbb{R}^{n\times m}$
$S_{ij}\in\mathbb{R}$ |  Element in row $i$ and column $j$ of $\boldsymbol{S}$
$[\boldsymbol{v}]_{i}\in\mathbb{R}^{i}$ |  Vector containing the first $i$ components of $\boldsymbol{v}\in\mathbb{R}^{n}$, with $i\leq n$
${\left\|{\boldsymbol{v}}\right\|}$ |  Euclidean norm of $\boldsymbol{v}$
$\tilde{{\boldsymbol{r}}}\in\mathfrak{so}(3)$ |  Skew symmetric matrix defined by ${\boldsymbol{r}}\in\mathbb{R}^{3}$
$\hat{\boldsymbol{\alpha}}=\small\left(\begin{array}[]{@{\hspace{-2pt}}cc@{\hspace{-2pt}}}\tilde{\boldsymbol{\beta}}&\boldsymbol{\gamma}\\\ \boldsymbol{0}&0\end{array}\right)\in\mathfrak{se}(3)$ |  Tensor representation of $\boldsymbol{\alpha}=\small\left(\boldsymbol{\beta}^{T}\ \boldsymbol{\gamma}^{T}\right)^{T}$ with $\boldsymbol{\beta},\boldsymbol{\gamma}\in\mathbb{R}^{3}$
$\boldsymbol{J}_{\boldsymbol{f}}(\boldsymbol{x})=\displaystyle\frac{\partial\boldsymbol{f}}{\partial\boldsymbol{x}}\in\mathbb{R}^{l\times h}$ |  Jacobian of the vector function $\boldsymbol{f}(\boldsymbol{x}):\mathbb{R}^{h}\rightarrow\mathbb{R}^{l}$
$\boldsymbol{\mathrm{tanh}}(\boldsymbol{v})$ |  Vector obtained by applying $\mathrm{tanh}(\cdot)$ componentwise to $\boldsymbol{v}$

###  II-B Dynamic model

Let ${\boldsymbol{q}}\in\mathcal{M}$ be the generalized coordinates of a
dynamical system evolving on a $n$-dimensional smooth manifold $\mathcal{M}$
with Lagrangian
$\mathcal{L}_{{\boldsymbol{q}}}({\boldsymbol{q}},\dot{\boldsymbol{q}})$. The
system trajectories satisfy the Euler-Lagrange equations of motions, i.e.,

$\left\\{\begin{array}[]{l}\displaystyle\frac{\mathrm{d}}{\mathrm{d}t}\left(\displaystyle\frac{\partial\mathcal{L}_{{\boldsymbol{q}}}({\boldsymbol{q}},\dot{\boldsymbol{q}})}{\partial\dot{\boldsymbol{q}}}\right)^{T}-\left(\displaystyle\frac{\partial\mathcal{L}_{{\boldsymbol{q}}}({\boldsymbol{q}},\dot{\boldsymbol{q}})}{\partial{\boldsymbol{q}}}\right)^{T}=\boldsymbol{\tau}_{{\boldsymbol{q}}}({\boldsymbol{q}},\boldsymbol{u}),\\\
\boldsymbol{\tau}_{{\boldsymbol{q}}}({\boldsymbol{q}},\boldsymbol{u})=\ActM\boldsymbol{u},\end{array}\right.$
(1)

where $\boldsymbol{u}\in\mathbb{R}^{m}$ are the available actuation inputs,
$\ActM\in\mathbb{R}^{n\times m}$ is the actuation matrix, and
$\boldsymbol{\tau}_{{\boldsymbol{q}}}({\boldsymbol{q}},\boldsymbol{u})\in\mathrm{Im}(\ActM)$
collects the generalized forces performing work on ${\boldsymbol{q}}$. For all
${\boldsymbol{q}}\in\mathcal{M}$, we assume that $\ActM$ is a full-rank
matrix, i.e., $r=\mathrm{rank}(\ActM)=\min(m,n)$. When the dynamics is fully
actuated ($m=n$) or underactuated ($m<n$), this is equivalent to asking that
the actuation channels are all independent. On the other hand, if (1) is
overactuated ($m>n$), we assume that there are exactly $n$ independent inputs.
Note that, for the following derivations, when $r=m^{*}<\min(m,n)$ one can
consistently discard $m-m^{*}$ the linearly dependent columns of $\ActM$ and
consider the dynamics as underactuated.

In Appendix -A, we recall two basic properties of Lagrangian dynamics used in
the following results.

###  II-C Problem statement

We look for a change of coordinates
$\boldsymbol{\theta}=\boldsymbol{h}({\boldsymbol{q}})$ from
$\mathcal{B}({\boldsymbol{q}})\subset\mathcal{M}$ to $\mathcal{N}$ where each
of the first $r$ equations of motion in (1) is affected by one, and only one,
independent actuator input, i.e., the right-hand side of the transformed
equations of motion (see Property 2 in Appendix -A) takes the form

$\boldsymbol{\tau}_{\boldsymbol{\theta}}(\boldsymbol{\theta},\boldsymbol{u})=\ActM[\boldsymbol{\theta}][][(\boldsymbol{\theta})]\boldsymbol{u}=\left(\begin{array}[]{c}\boldsymbol{I}_{r}\\\
\boldsymbol{O}_{n-r\times r}\end{array}\right)\boldsymbol{u}.$ (2)

We refer to such problem as the Input Decoupling (ID) problem for the
Lagrangian dynamics (1). If a solution exists, then we say that (1) admits a
collocated form. Note that (2) covers only the fully- and overactuated cases.
When the dynamics is overactuated, i.e., $m>n$, it is impossible to obtain (2)
because only $r=n$ input channels can be decoupled, and the remaining $m-n$
inputs will affect the dynamics through a configuration-dependent actuation
matrix.

##  III Collocated Lagrangian Systems

In this section, we characterize a new class of Lagrangian systems, which we
call collocated because only such Lagrangian dynamics admit a collocated form
under a change of generalized coordinates. In addition, a set of coordinates
that solve the ID problem come for free without further system analysis.

To this end, we will exploit a concept that is known as the _passive output_
in the context of passivity-based control [29]. Consider the following vector
function linear in the velocity

$\dot{\boldsymbol{y}}=\boldsymbol{A}^{T}({\boldsymbol{q}})\dot{\boldsymbol{q}},$

which is called the passive output because (1) is passive with respect to the
pair $(\boldsymbol{u},\dot{\boldsymbol{y}})$, with the storage function being
the system Hamiltonian (see Appendix -A). We will assume that (1) has
$\dot{\boldsymbol{y}}$ integrable, i.e.,

###### Integrability assumption.

For all ${\boldsymbol{q}}\in\mathcal{M}$, there exists a function
$\boldsymbol{g}({\boldsymbol{q}}):\mathcal{M}\rightarrow\mathbb{R}^{m}$ such
that

$\boldsymbol{J}_{\boldsymbol{g}}({\boldsymbol{q}})=\displaystyle\frac{\partial\boldsymbol{g}}{\partial{\boldsymbol{q}}}=\boldsymbol{A}^{T}({\boldsymbol{q}}).$
(3)

We define $\boldsymbol{y}=\boldsymbol{g}({\boldsymbol{q}})$ as _actuation
coordinates_ because, in such coordinates, $\boldsymbol{u}$ acts directly on
the equations of motion according to (2).

######  Remark.

Each component of $\boldsymbol{g}({\boldsymbol{q}})$ is defined up to a
constant since any function
$\bar{\boldsymbol{g}}({\boldsymbol{q}})=\boldsymbol{g}({\boldsymbol{q}})+\boldsymbol{k}$,
with $\boldsymbol{k}\in\mathbb{R}^{m}$, satisfies the condition
$\boldsymbol{J}_{\bar{\boldsymbol{g}}}=\boldsymbol{J}_{{\boldsymbol{q}}}$.

The integrability assumption requires each column of $\ActM$ to be the
gradient of a scalar function of the configuration variables. If $\ActM$ is
constant, i.e., $\ActM=\ActM[][][]$, then
$\boldsymbol{g}({\boldsymbol{q}})=\boldsymbol{A}^{T}{\boldsymbol{q}}$. More in
general, when the column $\boldsymbol{A}_{i}({\boldsymbol{q}})$ has continuous
partial derivatives, $\dot{y}_{i}=\ActM[i][T]\dot{\boldsymbol{q}}$ is
integrable [30, Chap. 2] if and only if

$\displaystyle\frac{\partial A_{ji}}{\partial
q_{k}}=\displaystyle\frac{\partial A_{ki}}{\partial q_{j}};\quad\forall
j,k\in\\{1,\cdots,n\\}.$ (4)

Note that this condition is equivalent to asking that, when $u_{i}$ is
constant, the generalized work done by $u_{i}$ on ${\boldsymbol{q}}$ does not
depend on the system trajectories but only on the initial and final
configurations ${\boldsymbol{q}}_{a}$ and ${\boldsymbol{q}}_{b}$,
respectively, i.e.,

$W_{u_{i}}({\boldsymbol{q}}):=\int^{{\boldsymbol{q}}_{b}}_{{\boldsymbol{q}}_{a}}u_{i}\ActM[i][T]d{\boldsymbol{q}}=u_{i}\left[g_{i}({\boldsymbol{q}}_{b})-g_{i}({\boldsymbol{q}}_{a})\right],$

where the last equality follows from the Gradient Theorem [30, Prop. 1, Chap.
2]. In other words,
$P_{i}(u_{i},{\boldsymbol{q}}):=u_{i}g_{i}({\boldsymbol{q}})$ plays the role
of a potential energy for the dynamics.

Even when (4) holds, it could be challenging to integrate the passive output
$\dot{\boldsymbol{y}}$ in closed form. Nonetheless, it is always possible to
perform the numerical integration online based on the measure of
${\boldsymbol{q}}$ and $\dot{\boldsymbol{q}}$.

In some cases, the integrability may come directly as a consequence of the
physical nature of the system, as illustrated in the following.

Example 1 (Cartesian forces on a robot). Consider a manipulator with $n$-DOF
subject to $m$ external forces applied on its structure. Assume that each
force $\boldsymbol{f}_{i}$ changes its magnitude over time but keeps the same
direction in the global Cartesian frame so that

$\boldsymbol{f}_{i}=\boldsymbol{d}_{\boldsymbol{f},i}u_{i};\quad
i\in\\{1,\cdots,m\\},$

with ${\left\|{\boldsymbol{d}_{\boldsymbol{f},i}}\right\|}=1$, and where
$\boldsymbol{d}_{\boldsymbol{f},i}\in\mathbb{R}^{3}$ represents the direction
of $\boldsymbol{f}_{i}$ and $u_{i}\in\mathbb{R}^{>0}$ its magnitude. If
$\boldsymbol{p}_{i}({\boldsymbol{q}})\in\mathbb{R}^{3}$ denotes the point of
application of $\boldsymbol{f}_{i}$ in the global frame, then the effect of
the force in the dynamics is

$\boldsymbol{\tau}_{{\boldsymbol{q}},i}=\boldsymbol{J}^{T}_{\boldsymbol{p}_{i}}({\boldsymbol{q}})\boldsymbol{f}_{i}=\boldsymbol{J}^{T}_{\boldsymbol{p}_{i}}({\boldsymbol{q}})\boldsymbol{d}_{\boldsymbol{f},i}u_{i}=\boldsymbol{A}_{i}({\boldsymbol{q}})u_{i}.$

Thus, one can integrate $\boldsymbol{A}_{i}({\boldsymbol{q}})$ as
$g_{i}({\boldsymbol{q}})=\boldsymbol{d}_{\boldsymbol{f},i}^{T}\boldsymbol{p}_{i}({\boldsymbol{q}})$.
$\vartriangleleft$

On the other hand, the integrability conditions may not hold even for
elementary dynamics.

Example 2 (Geostationary satellite).

Figure 2: A geostationary satellite actuated by a normal force $u_{1}$ and
tangential force $u_{2}$. The body configuration is described by the distance
$q_{1}$ from the Earth center and the angle $q_{2}$ with respect to the
horizontal axis. Only the normal force is collocated because it performs work
directly and only on $q_{1}$.

Consider a geostationary satellite orbiting around the Earth in a plane. The
configuration ${\boldsymbol{q}}=(q_{1}\,\,q_{2})^{T}$ describe its motion,
being $q_{1}$ the distance from the Earth center and $q_{2}$ the angle with
respect to a reference axis, as shown in Fig. 2. The satellite is controlled
by a normal and tangential force, denoted as $u_{1}$ and $u_{2}$,
respectively. The application of the Lagrangian formalism yields

$\boldsymbol{\tau}_{{\boldsymbol{q}}}=\ActM\boldsymbol{u}=\left(\begin{array}[]{cc}1&0\\\
0&q_{1}\end{array}\right)\boldsymbol{u}.$

The first column of $\ActM$ is already in the collocated form because $u_{1}$
performs work only on $q_{1}$. However, the second column of $\ActM$ is non-
integrable because

$\displaystyle\frac{\partial A_{12}}{\partial q_{2}}=0\neq
1=\displaystyle\frac{\partial A_{22}}{\partial q_{1}}.$

Indeed, the generalized work done on ${\boldsymbol{q}}$ by the tangential
force $u_{2}$ depends on the time evolution (history) of $q_{1}$.
$\vartriangleleft$

In the following, we consider the three different actuation scenarios, i.e.,
fully actuated, overactuated and underactuated dynamics.

##  IV Change of Coordinates
for Fully Actuated Systems

In this section, we show that when the system is fully actuated, the existence
of $n$ actuator coordinates is necessary and sufficient to bring the dynamics
to the collocated form. This case allows illustrating the results in the most
simple scenario, where a coordinate transformation is sufficient to solve the
ID problem. However, the same arguments will be used also for underactuated
systems.

######  Theorem 1.

Suppose the system is fully actuated, i.e., $m=n$. There exists a change of
coordinates
$\boldsymbol{\theta}=\boldsymbol{h}({\boldsymbol{q}}):\mathcal{B}({\boldsymbol{q}})\rightarrow\mathcal{N}$
such that (1) takes the form

$\frac{\mathrm{d}}{\mathrm{d}t}\left(\displaystyle\frac{\partial\mathcal{L}_{\boldsymbol{\theta}}(\boldsymbol{\theta},\dot{\boldsymbol{\theta}})}{\partial\dot{\boldsymbol{\theta}}}\right)^{T}-\left(\displaystyle\frac{\partial\mathcal{L}_{\boldsymbol{\theta}}(\boldsymbol{\theta},\dot{\boldsymbol{\theta}})}{\partial\boldsymbol{\theta}}\right)^{T}=\boldsymbol{u},$
(5)

i.e.,
$\ActM[\boldsymbol{\theta}][][(\boldsymbol{\theta})]=\boldsymbol{I}_{n}$, if
and only if the integrability assumption (3) holds. Let
$\boldsymbol{g}({\boldsymbol{q}})$ be the integral of
$\ActM[][T]\dot{\boldsymbol{q}}$. Then, a possible choice for
$\boldsymbol{\theta}$ is
$\boldsymbol{\theta}=\boldsymbol{g}({\boldsymbol{q}})$.

###### Proof.

Under the integrability assumption,
$\boldsymbol{\theta}=\boldsymbol{g}({\boldsymbol{q}})$ defines a change of
coordinates since its Jacobian
$\boldsymbol{J}_{\boldsymbol{g}}({\boldsymbol{q}})=\boldsymbol{A}^{T\\!}({\boldsymbol{q}})$
has rank $n$ at ${\boldsymbol{q}}$.

From Property 2 in Appendix -A, it follows

$\dot{\boldsymbol{\theta}}^{T}\boldsymbol{\tau}_{\boldsymbol{\theta}}=\dot{\boldsymbol{q}}^{T}\boldsymbol{\tau}_{{\boldsymbol{q}}}.$

Noting that
$\dot{\boldsymbol{\theta}}=\boldsymbol{J}_{\boldsymbol{g}}({\boldsymbol{q}})\dot{\boldsymbol{q}}$
and using $\boldsymbol{\tau}_{{\boldsymbol{q}}}=\ActM\boldsymbol{u}$, the
above equation rewrites as

$\dot{\boldsymbol{q}}^{T}\boldsymbol{J}^{T}_{\boldsymbol{g}}({\boldsymbol{q}})\boldsymbol{\tau}_{\boldsymbol{\theta}}=\dot{\boldsymbol{q}}^{T}\ActM\boldsymbol{u},$

or, equivalently,

$\dot{\boldsymbol{q}}^{T}\left(\boldsymbol{J}^{T}_{\boldsymbol{g}}({\boldsymbol{q}})\boldsymbol{\tau}_{\boldsymbol{\theta}}-\ActM\boldsymbol{u}\right)=0.$
(6)

Since (6) holds for all $\dot{\boldsymbol{q}}\in
T_{{\boldsymbol{q}}}\mathcal{M}$, it follows that

$\boldsymbol{J}^{T}_{\boldsymbol{g}}({\boldsymbol{q}})\boldsymbol{\tau}_{\boldsymbol{\theta}}-\ActM\boldsymbol{u}=\boldsymbol{0}.$

Furthermore,
$\boldsymbol{J}_{\boldsymbol{g}}({\boldsymbol{q}})=\boldsymbol{A}^{T}({\boldsymbol{q}})$
leads to

$\ActM\left(\boldsymbol{\tau}_{\boldsymbol{\theta}}-\boldsymbol{u}\right)=\boldsymbol{0}.$

The above equation defines a homogeneous linear system in the unknown
$\boldsymbol{\tau}_{\boldsymbol{\theta}}-\boldsymbol{u}$, which admits the
unique solution $\boldsymbol{\tau}_{\boldsymbol{\theta}}=\boldsymbol{u}$ since
$\ActM$ is nonsingular, thus yielding the sufficiency of (5).

As for the necessity, suppose that a change of coordinates
$\boldsymbol{\theta}=\boldsymbol{h}({\boldsymbol{q}})$ exists such that (5)
holds. Property 2 implies that, for all $\dot{\boldsymbol{q}}\in
T_{{\boldsymbol{q}}}\mathcal{M}$,

$\dot{\boldsymbol{q}}^{T}\left(\boldsymbol{J}^{T}_{\boldsymbol{h}}({\boldsymbol{q}})-\ActM\right)\boldsymbol{u}=0,$

leading to

$\left(\boldsymbol{J}^{T}_{\boldsymbol{h}}({\boldsymbol{q}})-\ActM\right)\boldsymbol{u}=\boldsymbol{0},\quad\forall\boldsymbol{u}\in\mathbb{R}^{n}.$

If one chooses $\boldsymbol{u}=(\boldsymbol{I}_{n})_{i};\
i\in\\{1,\cdots,n\\}$, then

$\left(\boldsymbol{J}^{T}_{\boldsymbol{h}}({\boldsymbol{q}})-\ActM\right)\boldsymbol{u}=(\boldsymbol{J}^{T}_{\boldsymbol{h}}({\boldsymbol{q}})-\ActM)_{i}=\boldsymbol{0}.$

Thus, it holds $\boldsymbol{J}^{T}_{\boldsymbol{h}}({\boldsymbol{q}})=\ActM$
and $\boldsymbol{g}({\boldsymbol{q}})=\boldsymbol{h}({\boldsymbol{q}})$. ∎

The following example illustrates the above result.

Example 3 (Spring actuated mechanism). Consider a planar mechanism with two
passive revolute joints, having angles $q_{1}$ and $q_{2}$ so that
${\boldsymbol{q}}=(q_{1}\,\,q_{2})^{T}$. A spring with stiffness $k_{i}$ is
attached to the distal end of each link, whose length is $l_{i},\ i=1,2$. The
springs are also connected to two carts moving on linear rails under the
forces $u_{1}$ and $u_{2}$, with reference to Fig. 3.

Figure 3: A planar mechanism with two rotational joints having configuration
${\boldsymbol{q}}\in\mathbb{S}^{1}\times\mathbb{S}^{1}$. For $i=1,2$, the
force $u_{i}$ actuates a cart coupled to the robot by a linear spring of
stiffness $k_{i}$. The dynamics of the carts is negligible, and thus the
forces $\boldsymbol{u}$ act instantaneously on the mechanism.

Assuming that the dynamics of the carts is negligible, it can be shown that
the actuator inputs $\boldsymbol{u}$ directly affect the joint motion through
the actuation matrix

$\ActM=\left(\begin{array}[]{cc}-l_{1}\mathrm{s}_{1}&l_{1}\mathrm{c}_{1}+l_{2}\mathrm{c}_{12}\\\
0&l_{2}\mathrm{c}_{12}\end{array}\right),$

where $\mathrm{s}_{i}(\mathrm{c}_{i})=\sin(q_{i})(\cos(q_{i}))$ and
$\mathrm{s}_{ij}(\mathrm{c}_{ij})=\sin(q_{i}+q_{j})(\cos(q_{i}+q_{j}))$.
Outside of singularities $q_{1}\in\\{0,\pi\\}$ and
$q_{1}+q_{2}\in\\{\pm\pi/2\\}$, $\mathrm{rank}(\ActM)=2$ and the integrability
test (4) is successful since

$\displaystyle\small\displaystyle\frac{\partial A_{11}}{\partial q_{2}}$
$\displaystyle=\displaystyle\frac{\partial\left(-l_{1}\mathrm{s}_{1}\right)}{\partial
q_{2}}=0=\displaystyle\frac{\partial A_{21}}{\partial q_{1}},$
$\displaystyle\displaystyle\frac{\partial A_{21}}{\partial q_{2}}$
$\displaystyle=\displaystyle\frac{\partial\left(l_{1}\mathrm{c}_{1}+l_{2}\mathrm{c}_{12}\right)}{\partial
q_{2}}=-l_{2}\mathrm{s}_{12}$
$\displaystyle=\displaystyle\frac{\partial\left(l_{2}\mathrm{c}_{12}\right)}{\partial
q_{1}}=\displaystyle\frac{\partial A_{22}}{\partial q_{1}}.$

The passive output is integrable as

$\boldsymbol{y}=\left(\begin{array}[]{c}l_{1}\mathrm{c}_{1}\\\
l_{1}\mathrm{s}_{1}+l_{2}\mathrm{s}_{12}\end{array}\right).$

Note that $y_{1}$ is the $x$-coordinate of the position of the spring end
attached to link $1$. Similarly, $y_{2}$ is the $y$-coordinate of the spring
attached to link $2$. Indeed, the forces $u_{1}$ and $u_{2}$ perform work on
the distal ends of the spring attached to the mechanism along these
directions. $\vartriangleleft$

###  IV-A Overactuated case

The previous result extends to overactuated systems, i.e., dynamics with more
inputs than generalized coordinates. Thus, we have $r=n<m$. We partition
$\ActM$, which is a wide matrix, as

$\ActM=\left(\begin{array}[]{cc}\ActM[a]&\ActM[o]\end{array}\right),$ (7)

where $\ActM[a]\in\mathbb{R}^{n\times n}$ and
$\ActM[o]\in\mathbb{R}^{n\times(m-n)}$. Without loss of generality, we can
have that $\mathrm{rank}\left(\ActM[a]\right)=n$ and the integrability
condition holds for $\ActM[a]$.

######  Corollary 1.

If the system is overactuated, i.e., $r=n<m$ and the same hypotheses of
Theorem 1 hold for $\ActM[a]$, then there exists a change of coordinates
$\boldsymbol{\theta}=\boldsymbol{h}({\boldsymbol{q}}):\mathcal{B}({\boldsymbol{q}})\rightarrow\mathcal{N}$
such that (1) takes the form

$\frac{\mathrm{d}}{\mathrm{d}t}\left(\displaystyle\frac{\partial\mathcal{L}_{\boldsymbol{\theta}}(\boldsymbol{\theta},\dot{\boldsymbol{\theta}})}{\partial\dot{\boldsymbol{\theta}}}\right)^{T}-\left(\displaystyle\frac{\partial\mathcal{L}_{\boldsymbol{\theta}}(\boldsymbol{\theta},\dot{\boldsymbol{\theta}})}{\partial\boldsymbol{\theta}}\right)^{T}=\left(\boldsymbol{I}_{n}\quad\ActM[o,\boldsymbol{\theta}][][(\boldsymbol{\theta})]\right)\boldsymbol{u},$
(8)

where
$\ActM[o,\boldsymbol{\theta}][][(\boldsymbol{\theta})]\in\mathbb{R}^{n\times(m-n)}$.
Let $\boldsymbol{g}({\boldsymbol{q}})$ be the integral of
$\ActM[a][T]\dot{\boldsymbol{q}}$. Then, $\boldsymbol{\theta}$ can be chosen
as $\boldsymbol{\theta}=\boldsymbol{g}({\boldsymbol{q}})$.

###### Proof.

Choosing again $\boldsymbol{\theta}=\boldsymbol{g}({\boldsymbol{q}})$ and
following steps similar to those of the proof of Theorem 1, one obtains

$\ActM[a]\boldsymbol{\tau}_{\boldsymbol{\theta}}=\ActM\boldsymbol{u}.$

Expanding $\ActM$ into (7) and left-multiplying the above equation by
$\ActM[a][-1]$ gives

$\begin{split}\boldsymbol{\tau}_{\boldsymbol{\theta}}&=\ActM[a][-1]\left(\begin{array}[]{cc}\ActM[a]&\ActM[o]\end{array}\right)\boldsymbol{u}\\\
&=(\,\,\boldsymbol{I}_{n}\quad\underbrace{\ActM[a][-1]\ActM[o]}_{\ActM[o,\boldsymbol{\theta}][][(\boldsymbol{\theta})]}\,\,)\boldsymbol{u},\end{split}$

with ${\boldsymbol{q}}=\boldsymbol{g}^{-1}(\boldsymbol{\theta})$.

Suppose a change of coordinates
$\boldsymbol{\theta}=\boldsymbol{h}({\boldsymbol{q}})$ exists such that (8)
holds. After some computations, power invariance leads to the algebraic system

$\left(\begin{array}[]{cc}\boldsymbol{J}^{T}_{\boldsymbol{h}}({\boldsymbol{q}})({\boldsymbol{q}})-\ActM[a]&\boldsymbol{J}^{T}_{\boldsymbol{h}}({\boldsymbol{q}})\ActM[o,\boldsymbol{\theta}]-\ActM[o]\end{array}\right)\boldsymbol{u}=\boldsymbol{0},$

which must hold for all $\boldsymbol{u}\in\mathbb{R}^{m}$. By taking
$\boldsymbol{u}_{i}=(\boldsymbol{I}_{m})_{i};\ i\in\\{1,\cdots,m\\}$, it
follows $\boldsymbol{J}^{T}_{\boldsymbol{h}}({\boldsymbol{q}})=\ActM[a]$ and
$\ActM[o]=\boldsymbol{J}^{T}_{\boldsymbol{h}}({\boldsymbol{q}})\ActM[o,\boldsymbol{\theta}]$.
Hence, at least $n$ passive outputs are integrable as
$\boldsymbol{y}=\boldsymbol{h}({\boldsymbol{q}})$. ∎

Note that it is not possible, in general, to simplify the expression of both
terms in the actuation matrix because there are too many input variables to be
decoupled.

Example 4 (Tendon driven joint). Consider the tendon driven finger of [31,
Chap. 6.4] with one DOF $q$ and two actuator inputs
$\boldsymbol{u}=(u_{1}\,\,u_{2})^{T}$, as sketched in Fig. 4.

Figure 4: A tendon-driven rotational joint having a single configuration
variable $q\in\mathbb{S}^{1}$. The cables tension $u_{1}$ and $u_{2}$ generate
a torque at the joint.

Assume for simplicity that the angle $q>0$ (similar results hold for $q<0$).
The system input is $\boldsymbol{\tau}_{q}=\ActM[][][(q)]\boldsymbol{u}$,
where

$\ActM[][][(q)]=\left(\begin{array}[]{@{\hspace{-1pt}}cc@{\hspace{-1pt}}}\displaystyle\sqrt{a^{2}+b^{2}}\sin\left(\tan^{-1}\left(\frac{a}{b}\right)+\frac{q}{2}\right)&-R\end{array}\right),$

and $\boldsymbol{u}$ collects the cable tensions. Thus, the passive output is

$\dot{\boldsymbol{y}}=\ActM[][T][(q)]\dot{q}=\left(\begin{array}[]{c}\displaystyle\sqrt{a^{2}+b^{2}}\sin\left(\tan^{-1}\left(\frac{a}{b}\right)+\frac{q}{2}\right)\dot{q}\\\
-R\dot{q}\end{array}\right).$

Since the system is overactuated, there are two possible choices of the
actuation coordinates out of singularities of $\ActM[][][(q)]$. When
$q=\displaystyle-2\tan^{-1}\left(\frac{a}{b}\right)$, the first column of
$\ActM[][][(q)]$ becomes zero, and the first actuator does not have any effect
on the motion. In this case, one can easily integrate $\dot{\boldsymbol{y}}$
as

$\boldsymbol{y}=\boldsymbol{g}(q)=\left(\begin{array}[]{c}\displaystyle-2\sqrt{a^{2}+b^{2}}\cos\left(\tan^{-1}\left(\frac{a}{b}\right)+\frac{q}{2}\right)\\\
-Rq\end{array}\right).$

As expected, the choice of either the first or second component of
$\boldsymbol{y}$ yields dynamics in the form of (8). Note that
$\boldsymbol{y}$ is the tendon displacement with respect to the
$\vartriangleleft$

##  V Change of Coordinates
for Underactuated Systems

We now focus on underactuated systems, i.e., dynamics for which the number of
actuator inputs is less than that of degrees of freedom. Also in this case the
actuation coordinates solve the input decoupling problem. First, it is
convenient to expand the actuation matrix as

$\ActM=\left(\begin{array}[]{c}\ActM[a]\\\ \ActM[u]\end{array}\right),$ (9)

where $\ActM[a]\in\mathbb{R}^{m\times m}$ is non-singular and
$\ActM[u]\in\mathbb{R}^{(n-m)\times m}$. Note that such partition is always
possible after reordering the linearly independent rows of $\ActM$.
Furthermore, since $n>r=m$, $\dot{\boldsymbol{y}}$ is an $m$-dimensional
vector.

######  Theorem 2.

Assume that (1) is underactuated, i.e., $m<n$. The integrability assumption is
a necessary and sufficient condition for a change of coordinates
$\boldsymbol{\theta}=\boldsymbol{h}({\boldsymbol{q}}):\mathcal{B}({\boldsymbol{q}})\rightarrow\mathcal{N}$
to exist such that (1) becomes

$\frac{\mathrm{d}}{\mathrm{d}t}\left(\displaystyle\frac{\partial\mathcal{L}_{\boldsymbol{\theta}}(\boldsymbol{\theta},\dot{\boldsymbol{\theta}})}{\partial\dot{\boldsymbol{\theta}}}\right)^{T}-\left(\displaystyle\frac{\partial\mathcal{L}_{\boldsymbol{\theta}}(\boldsymbol{\theta},\dot{\boldsymbol{\theta}})}{\partial\boldsymbol{\theta}}\right)^{T}=\left(\begin{array}[]{c}\boldsymbol{u}\\\
\boldsymbol{0}_{n-m}\end{array}\right),$ (10)

i.e., $\ActM[\boldsymbol{\theta}][][](\boldsymbol{\theta})$ has the form

$\ActM[\boldsymbol{\theta}][][(\boldsymbol{\theta})]=\left(\begin{array}[]{c}\boldsymbol{I}_{n}\\\
\boldsymbol{O}_{(n-m)\times m}\end{array}\right).$

Let $\boldsymbol{g}({\boldsymbol{q}})$ be the integral of
$\ActM[][T]\dot{\boldsymbol{q}}$. Then, a possible choice of
$\boldsymbol{\theta}$ is

$\begin{split}\boldsymbol{\theta}&=\left(\begin{array}[]{c}\boldsymbol{g}({\boldsymbol{q}})\\\
\boldsymbol{0}_{n-m}\end{array}\right)+\left(\begin{array}[]{cc}\boldsymbol{O}_{m\times
m}&\boldsymbol{O}_{m\times(n-m)}\\\ \boldsymbol{O}_{(n-m)\times
m}&\boldsymbol{I}_{n-m}\end{array}\right){\boldsymbol{q}}.\end{split}$ (11)

###### Proof.

The choice of $\boldsymbol{\theta}$ as given in (11) qualifies as a change of
coordinates because its Jacobian

$\boldsymbol{J}_{\boldsymbol{h}}({\boldsymbol{q}})=\left(\begin{array}[]{cc}\ActM[a][T]&\ActM[u][T]\\\
\boldsymbol{O}_{(n-m)\times m}&\boldsymbol{I}_{n-m}\end{array}\right),$ (12)

is non singular at ${\boldsymbol{q}}$. Power invariance and (12) imply

$\begin{split}\dot{\boldsymbol{\theta}}^{T}\boldsymbol{\tau}_{\boldsymbol{\theta}}=\dot{\boldsymbol{q}}^{T}\boldsymbol{J}^{T}_{\boldsymbol{h}}({\boldsymbol{q}})\boldsymbol{\tau}_{\boldsymbol{\theta}}=\dot{\boldsymbol{q}}^{T}\ActM\boldsymbol{u}=\dot{\boldsymbol{q}}^{T}\boldsymbol{\tau}_{{\boldsymbol{q}}}.\end{split}$

Furthermore, being $\dot{\boldsymbol{q}}$ arbitrary, it follows
$\boldsymbol{J}^{T}_{\boldsymbol{h}}({\boldsymbol{q}})\boldsymbol{\tau}_{\boldsymbol{\theta}}=\ActM\boldsymbol{u},$
or, equivalently,

$\underbrace{\left(\begin{array}[]{cc}\ActM[a]&\boldsymbol{O}_{m\times(n-m)}\\\
\ActM[u]&\boldsymbol{I}_{n-m}\end{array}\right)}_{\boldsymbol{J}^{T}_{\boldsymbol{h}}({\boldsymbol{q}})}\left(\begin{array}[]{c}\boldsymbol{\tau}_{\boldsymbol{\theta}_{a}}-\boldsymbol{u}\\\
\boldsymbol{\tau}_{\boldsymbol{\theta}_{u}}\end{array}\right)=\boldsymbol{0},$
(13)

where we expanded $\boldsymbol{\tau}_{\boldsymbol{\theta}}$ into the two
vectors $\boldsymbol{\tau}_{\boldsymbol{\theta}_{a}}\in\mathbb{R}^{m}$ and
$\boldsymbol{\tau}_{\boldsymbol{\theta}_{u}}\in\mathbb{R}^{n-m}$ performing
work on $\boldsymbol{\theta}_{a}$ and $\boldsymbol{\theta}_{u}$, respectively.
Equation (13) describes a homogeneous linear system, which admits the unique
solution

$\left(\begin{array}[]{c}\boldsymbol{\tau}_{\boldsymbol{\theta}_{a}}-\boldsymbol{u}\\\
\boldsymbol{\tau}_{\boldsymbol{\theta}_{u}}\end{array}\right)=\boldsymbol{0},$
(14)

being $\boldsymbol{J}^{T}_{\boldsymbol{h}}({\boldsymbol{q}})$ non-singular.

Suppose there exists $\boldsymbol{\theta}=\boldsymbol{h}({\boldsymbol{q}})$
such that (10) holds and partition

$\boldsymbol{J}_{\boldsymbol{h}}({\boldsymbol{q}})=\left(\begin{array}[]{c}\boldsymbol{J}_{\boldsymbol{h}_{a}}({\boldsymbol{q}})\\\
\boldsymbol{J}_{\boldsymbol{h}_{u}}({\boldsymbol{q}})\end{array}\right),$

with
$\boldsymbol{J}_{\boldsymbol{h}_{a}}({\boldsymbol{q}})\in\mathbb{R}^{m\times
n}$ and
$\boldsymbol{J}_{\boldsymbol{h}_{u}}({\boldsymbol{q}})\in\mathbb{R}^{(n-m)\times
n}$. Exploiting once again power invariance, we obtain, after some
computations,

$\dot{\boldsymbol{q}}^{T}\left(\boldsymbol{J}^{T}_{\boldsymbol{h}_{a}}({\boldsymbol{q}})-\ActM\right)\boldsymbol{u}=0,$

or, equivalently,

$\boldsymbol{J}_{\boldsymbol{h}_{a}}({\boldsymbol{q}})=\ActM[][T].$

Thus, the first $m$ components of $\boldsymbol{h}({\boldsymbol{q}})$ satisfy
the integrability assumption. ∎

In Appendix -B, we report an alternative proof of the sufficient part of
Theorem 2, which uses algebraic arguments instead of power invariance.

######  Remark.

There is no constraint on choosing the unactuated variables, except that the
corresponding Jacobian is non singular. Indeed, the factorization given in
(13) holds independently of $\boldsymbol{\theta}_{u}$. In other words, Theorem
2 does not rely on a specific choice of $\boldsymbol{\theta}_{u}$, which could
be used to further simplify the structure of the equations of motion.

Note that the previous results can also be derived in a Hamiltonian
formulation by considering the type 2 generating function [32]
$G_{2}({\boldsymbol{q}},\tilde{\boldsymbol{p}})=\boldsymbol{h}^{T}({\boldsymbol{q}})\tilde{\boldsymbol{p}}$,
where $\tilde{\boldsymbol{p}}$ is the momentum in the actuation coordinates.
We illustrate the application of Theorem 2 on soft robotic arm.

Example 5 (Continuum soft robot). Consider a continuum soft robot discretized
into two bodies, modeled under the piecewise constant curvature (PCC)
hypothesis. Then, each body has three DOF, corresponding to its curvature
$\kappa_{i}$, bending direction $\phi_{i}$ and elongation $\delta L_{i}$,
$i=1,2$, so that

${\boldsymbol{q}}=\left(\begin{array}[]{cccccc}\kappa_{1}&\phi_{1}&\delta
L_{1}&\kappa_{2}&\phi_{2}&\delta L_{2}\end{array}\right)^{T}.$

Three tendons that run from the base to the tip actuate the robot. Each
actuator is located at a distance $d\in\mathbb{R}^{+}$ from the backbone and
rotated from the previous by $120\text{\,}\mathrm{\SIUnitSymbolDegree}$, as
illustrated in Fig. 5.

Figure 5: A continuum soft robot discretized into two bodies segments. Under
the piecewise constant curvature assumption, the configuration of each body is
described by its curvature $\kappa_{i}$, bending angle $\phi_{i}$ and
elongation $\delta L_{i}$ for $i=1,2$. Three tendons run from the base to the
tip, and spatial motion is obtained by applying a suitable tension
$\boldsymbol{u}\in\mathbb{R}^{3}$.

By exploiting the principle of virtual works, one can show that
$\boldsymbol{\tau}_{{\boldsymbol{q}}}=\ActM\boldsymbol{u}$, where

$\ActM=\small\left(\begin{array}[]{ccc}-d\mathrm{c}_{2}&d\left(\frac{1}{2}\mathrm{c}_{{2}}-\frac{\sqrt{3}}{2}\mathrm{s}_{2}\right)&d\left(\frac{1}{2}\mathrm{c}_{2}+\frac{\sqrt{3}}{2}\mathrm{s}_{2}\right)\\\
dq_{1}\mathrm{s}_{{2}}&-dq_{1}\left(\frac{1}{2}\mathrm{s}_{2}+\frac{\sqrt{3}}{2}\mathrm{c}_{2}\right)&-dq_{1}\left(\frac{1}{2}\mathrm{s}_{2}-\frac{\sqrt{3}}{2}\mathrm{c}_{2}\right)\\\
1&1&1\\\
-d\mathrm{c}_{{5}}&d\left(\frac{1}{2}\mathrm{c}_{{5}}-\frac{\sqrt{3}}{2}\mathrm{s}_{5}\right)&d\left(\frac{1}{2}\mathrm{c}_{5}+\frac{\sqrt{3}}{2}\mathrm{s}_{5}\right)\\\
dq_{4}\mathrm{s}_{{5}}&-dq_{4}\left(\frac{1}{2}\mathrm{s}_{5}+\frac{\sqrt{3}}{2}\mathrm{c}_{5}\right)&-dq_{4}\left(\frac{1}{2}\mathrm{s}_{5}-\frac{\sqrt{3}}{2}\mathrm{c}_{5}\right)\\\
1&1&1\end{array}\right),$ (15)

and $\boldsymbol{u}\in\mathbb{R}^{3}$ collects the cables tension. It is easy
to verify that $\mathrm{rank}(\ActM)=3$ except when
$q_{1}=q_{4}=\leavevmode\nobreak\ 0$ and $q_{2}=q_{5}+k\pi$ with
$k\in\mathbb{Z}$. In fact, control authority is lost when the arm is in the
stretched configuration. However, this is an artifact due to the choice of the
bending direction as generalized coordinate [33]. Thus, the system is
underactuated with $m=3$ and $n=6$.

It can be shown that (derivations are omitted for the sake of space)
$\dot{\boldsymbol{y}}=\ActM[][T]\dot{\boldsymbol{q}}\in\mathbb{R}^{3}$ can be
integrated as

$\small\boldsymbol{y}=\left(\begin{array}[]{c}q_{3}+q_{6}+d(q_{1}\mathrm{c}_{{2}}+q_{4}\mathrm{c}_{{5}})\\\
q_{3}+q_{6}-\frac{d}{2}(q_{1}\mathrm{c}_{{2}}+q_{4}\mathrm{c}_{{5}})+\frac{\sqrt{3}}{2}d(q_{1}\mathrm{s}_{{2}}+q_{4}\mathrm{s}_{{5}})\\\
q_{3}+q_{6}-\frac{d}{2}(q_{1}\mathrm{c}_{{2}}+q_{4}\mathrm{c}_{{5}})-\frac{\sqrt{3}}{2}d(q_{1}\mathrm{s}_{{2}}+q_{4}\mathrm{s}_{{5}})\end{array}\right).$

Thus, according to Theorem 2, the ID problem is solvable through a coordinate
change having the form

$\boldsymbol{\theta}=\left(\begin{array}[]{c}\boldsymbol{\theta}_{a}\\\
\boldsymbol{\theta}_{u}\end{array}\right)=\left(\begin{array}[]{c}\boldsymbol{y}\\\
\boldsymbol{\theta}_{u}\end{array}\right),$ (16)

where $\boldsymbol{\theta}_{u}\in\mathbb{R}^{3}$ is any complement to
$\boldsymbol{y}$. For example, a possible choice is

$\boldsymbol{\theta}_{u}=\left(\begin{array}[]{c}q_{3}+dq_{1}\mathrm{c}_{{2}}\\\
q_{3}-\frac{d}{2}q_{1}\mathrm{c}_{{2}}+\frac{\sqrt{3}}{2}dq_{1}\mathrm{s}_{{2}}\\\
q_{3}-\frac{d}{2}q_{1}\mathrm{c}_{{2}}-\frac{\sqrt{3}}{2}dq_{1}\mathrm{s}_{{2}}\end{array}\right).$

Remarkably, the components of $\boldsymbol{\theta}_{a}$ correspond to the
change of tendons length inside the arm. Similarly, $\boldsymbol{\theta}_{u}$
collects the tendons elongation in the first body only.

The reader can verify that $\boldsymbol{J}^{-T}_{\boldsymbol{h}}$ takes the
expression given by (17), which yields

$\boldsymbol{\tau}_{\boldsymbol{\theta}}(\boldsymbol{\theta},\boldsymbol{u})=\boldsymbol{J}^{-T}_{\boldsymbol{h}}({\boldsymbol{q}})\ActM\boldsymbol{u}=\left(\begin{array}[]{c}\boldsymbol{I}_{3}\\\
\boldsymbol{O}_{3\times 3}\end{array}\right)\boldsymbol{u}.$

$\vartriangleleft$

$\boldsymbol{J}^{-T}_{\boldsymbol{h}}=\left(\begin{array}[]{cccccc}0&0&0&-\frac{2}{3d}\mathrm{c}_{5}&\frac{2}{3dq_{4}}\mathrm{s}_{5}&\frac{1}{3}\\\
0&0&0&\frac{2}{3d}\left(\frac{1}{2}\mathrm{c}_{5}-\frac{\sqrt{3}}{2}\mathrm{s}_{5}\right)&-\frac{2}{3dq_{4}}\left(\frac{1}{2}\mathrm{s}_{5}+\frac{\sqrt{3}}{2}\mathrm{c}_{5}\right)&\frac{1}{3}\\\
0&0&0&\frac{2}{3d}\left(\frac{1}{2}\mathrm{c}_{5}+\frac{\sqrt{3}}{2}\mathrm{s}_{5}\right)&-\frac{2}{3dq_{4}}\left(\frac{1}{2}\mathrm{s}_{5}-\frac{\sqrt{3}}{2}\mathrm{c}_{5}\right)&\frac{1}{3}\\\
-\frac{2}{3d}\mathrm{c}_{{2}}&\frac{2}{3dq_{1}}\mathrm{s}_{{2}}&\frac{1}{3}&\frac{2}{3d}\mathrm{c}_{{5}}&-\frac{2}{3dq_{4}}\mathrm{s}_{{5}}&-\frac{1}{3}\\\
\frac{2}{3d}\left(\frac{1}{2}\mathrm{c}_{2}-\frac{\sqrt{3}}{2}\mathrm{s}_{2}\right)&-\frac{2}{3d}\left(\frac{1}{2}\mathrm{c}_{2}+\frac{\sqrt{3}}{2}\mathrm{s}_{2}\right)&\frac{1}{3}&-\frac{2}{3d}\left(\frac{1}{2}\mathrm{c}_{5}-\frac{\sqrt{3}}{2}\mathrm{s}_{5}\right)&\frac{2}{3dq_{4}}\left(\frac{1}{2}\mathrm{s}_{5}+\frac{\sqrt{3}}{2}\mathrm{c}_{5}\right)&-\frac{1}{3}\\\
\frac{2}{3d}\left(\frac{1}{2}\mathrm{c}_{2}+\frac{\sqrt{3}}{2}\mathrm{s}_{2}\right)&-\frac{2}{3d}\left(\frac{1}{2}\mathrm{c}_{2}-\frac{\sqrt{3}}{2}\mathrm{s}_{2}\right)&\frac{1}{3}&-\frac{2}{3d}\left(\frac{1}{2}\mathrm{c}_{5}+\frac{\sqrt{3}}{2}\mathrm{s}_{5}\right)&\frac{2}{3dq_{4}}\left(\frac{1}{2}\mathrm{s}_{5}-\frac{\sqrt{3}}{2}\mathrm{c}_{5}\right)&-\frac{1}{3}\\\
\end{array}\right).$ (17)

##  VI Integrability of Thread-Like Actuators

This section shows that the conclusions drawn in Examples 1 and Remark hold
for any mechanical system driven by thread-like actuators. This type of
actuation is growing in popularity because it allows creating lightweight
structures with high power density, and precise and distributed actuation
[34]. We first prove the existence of the actuation coordinates for chains of
rigid bodies. We then extend such result to continuum bodies described by
reduced-order models.

Consider a mechanical system of rigid bodies with $n$ DOF actuated through $m$
inelastic tendons. As described in [31, Chap. 6], one can always define $m$
extension functions $g_{i}({\boldsymbol{q}});\ i\in\\{1,\cdots,m\\}$ that
measure the tendons displacement as a function of ${\boldsymbol{q}}$. The
application of the principle of virtual works yields

$\boldsymbol{\tau}_{{\boldsymbol{q}}}=\boldsymbol{J}_{\boldsymbol{g}}^{T\\!}({\boldsymbol{q}})\boldsymbol{u}=\ActM\boldsymbol{u},$

where $\boldsymbol{u}\in\mathbb{R}^{m}$ collects the tendons tension. It
immediately follows that the passive output $\dot{\boldsymbol{y}}$ is
integrable as $\boldsymbol{y}=\boldsymbol{g}({\boldsymbol{q}})$.

This result extends to mechanical systems with continuum bodies modeled under
the Geometric Variable Strain (GVS) technique [35, 36]. In the following, we
denote the strain as $\boldsymbol{\xi}(X)\in\mathbb{R}^{6}$, where $X\in[0,L]$
is the curvilinear abscissa with $L$ the body rest length. The GVS approach
reduces the infinite-dimensional state of the system by assuming that
$\boldsymbol{\xi}$ admits a representation of the form

$\boldsymbol{\xi}=\boldsymbol{\phi}(X,{\boldsymbol{q}}),$ (18)

where ${\boldsymbol{q}}\in\mathbb{R}^{n}$ is the configuration vector,
parameterizing the strain. Under (18), the dynamic model of a continuum takes
the form of (1) [37]. For thread-like actuators, the generalized actuation
force is $\boldsymbol{\tau}_{{\boldsymbol{q}}}=\ActM\boldsymbol{u}$ where

$\ActM=\int_{0}^{L}\boldsymbol{J}^{T}_{\boldsymbol{\phi}}\boldsymbol{\Phi}_{a}(X,{\boldsymbol{q}})\mathrm{d}X,$

and $\boldsymbol{\Phi}_{a}(X,{\boldsymbol{q}})\in\mathbb{R}^{6\times m}$ is
the spatial actuation matrix, whose $i$-th column

$(\boldsymbol{\Phi}_{a}(X,{\boldsymbol{q}}))_{i}=\left(\begin{array}[]{c}\tilde{\boldsymbol{d}}_{i}(X)\boldsymbol{t}_{i}(X,{\boldsymbol{q}})\\\
\boldsymbol{t}_{i}(X,{\boldsymbol{q}})\end{array}\right)\in\mathbb{R}^{6},$

represents the distributed force of the $i$-th actuator. In the above
expression, $\boldsymbol{d}_{i}(X)\in\mathbb{R}^{3}$ and
$\boldsymbol{t}_{i}(X,{\boldsymbol{q}})\in\mathbb{R}^{3}$ are the actuator
distance to the body backbone and its unit tangent vector [38], respectively.
The latter can be computed as

$\boldsymbol{t}_{i}(X,{\boldsymbol{q}})=\frac{[\hat{\boldsymbol{\xi}}\boldsymbol{d}_{i}+\boldsymbol{d}^{{}^{\prime}}_{i}]_{3}}{{\left\|{\hat{\boldsymbol{\xi}}\boldsymbol{d}_{i}+\boldsymbol{d}^{{}^{\prime}}_{i}}\right\|}},$

where $\boldsymbol{d}_{i}$ is expressed in homogeneous coordinates and
$(\cdot)^{{}^{\prime}}:=\displaystyle\frac{\partial(\cdot)}{\partial X}$.
Given $\boldsymbol{t}_{i}(X,{\boldsymbol{q}})$ it is also possible to compute
the length $L_{c_{i}}$ of the actuator as

$L_{c_{i}}({\boldsymbol{q}})=\int_{0}^{L}\boldsymbol{t}^{T}_{i}(X,{\boldsymbol{q}})[\hat{\boldsymbol{\xi}}\boldsymbol{d}_{i}(X)+\boldsymbol{d}^{{}^{\prime}}_{i}(X)]_{3}\mathrm{d}X,$
(19)

or, after some manipulations,

$L_{c_{i}}({\boldsymbol{q}})=\int_{0}^{L}(\boldsymbol{\Phi}_{a}(X,{\boldsymbol{q}}))_{i}^{T}\left(\boldsymbol{\xi}+\left(\begin{array}[]{c}\boldsymbol{0}_{3}\\\
\boldsymbol{d}^{{}^{\prime}}_{i}\end{array}\right)\right)\mathrm{d}X.$

The time derivative of (19) is

$\begin{split}\dot{L}_{c_{i}}({\boldsymbol{q}})&=\int_{0}^{L}\boldsymbol{t}^{T}_{i}[\dot{\hat{\boldsymbol{\xi}}}\boldsymbol{d}_{i}]_{3}dX=\int_{0}^{L}(\boldsymbol{\Phi}_{a}(X,{\boldsymbol{q}}))^{T}_{i}\boldsymbol{J}_{\boldsymbol{\phi}}\mathrm{d}X\dot{\boldsymbol{q}}\\\
&=(\ActM)^{T}_{i}\dot{\boldsymbol{q}},\end{split}$

which implies that the passive output
$\dot{\boldsymbol{y}}=\ActM[][T]\dot{\boldsymbol{q}}$ is integrable as

$\boldsymbol{y}=\boldsymbol{g}({\boldsymbol{q}})=\left(\begin{array}[]{c}L_{c_{1}}({\boldsymbol{q}})\\\
\vdots\\\ L_{c_{m}}({\boldsymbol{q}})\end{array}\right)\in\mathbb{R}^{m}.$

Thus, the actuation coordinates correspond to the length of the actuators, as
for rigid systems. The above results are independent of the number of DOF and
actuators. In other words, finite-dimensional models of mechanical systems
actuated via tendons always admit a collocated form, independently of being
fully-, over-, or underactuated. Recalling that the actuator coordinates are
defined up to a constant, it is also possible to consider the actuator
elongation $\delta L_{c_{i}}:=L_{c_{i}}-L^{*}_{c_{i}};\ i\in\\{1,\cdots,m\\}$
with respect to a reference length $L^{*}_{c_{i}}\in\mathbb{R}$, such as that
in the stress-free configuration. This way, proprioceptive sensors like
encoders can easily measure the actuation coordinates. Thus, the proposed
change of coordinates is also helpful for control synthesis.

##  VII Control of Collocated Underactuated
Mechanical Systems

The above results prove a fact empirically observed in soft and continuum
robot control. Several works [39, 40, 41, 42] have shown that it is possible
to obtain excellent closed-loop performance in shape and position tasks by
controlling the actuator length. This is the case for both model-based and
model-free approaches. However, to the best of Authors knowledge, it has never
been clarified why these coordinates represent a better choice than others,
such as the curvature and bending coordinates. In the actuation coordinates,
the dynamics is collocated, which is expected to simplify and robustify the
closed loop, especially when the control law does not require significant
system knowledge. Remarkably, this remains true also when the underactuation
of these systems is explicitly modeled. There results allow extending the
controllers of [36, 25, 10, 23, 24, 26] for planar underactuated mechanical
systems with damping to those moving in 3D. The following corollary formalizes
such statement for the regulators of [10] and [26].

######  Corollary 2.

Consider an underactuated mechanical system satisfying the same hypotheses of
Theorem 2. Suppose that there exists a dissipation function
$\cal{F}({\boldsymbol{q}},\dot{\boldsymbol{q}})$ such that, for all
$\dot{\boldsymbol{q}}\in T_{{\boldsymbol{q}}}\mathcal{M}$,

$\displaystyle\frac{\partial\cal{F}({\boldsymbol{q}},\dot{\boldsymbol{q}})}{\partial\dot{\boldsymbol{q}}}\dot{\boldsymbol{q}}>0,$
(20)

and, in the actuation coordinates,

$\displaystyle\frac{\partial^{2}\mathcal{L}_{\boldsymbol{\theta}}(\boldsymbol{\theta},\boldsymbol{0})}{\partial\boldsymbol{\theta}^{2}_{u}}>0.$
(21)

Let
$\boldsymbol{K}_{P},\boldsymbol{K}_{D},\boldsymbol{K}_{I}\in\mathbb{R}^{m\times
m}>0$ and $\gamma>0$. There exist constants $\alpha>0$ and $\bar{\gamma}>0$
such that, if $\boldsymbol{K}_{P}>\alpha\boldsymbol{I}_{m}$ and
$\gamma>\bar{\gamma}$, any of the following regulators

  1. 1.

PD with feedforward [10]:

$\begin{split}\boldsymbol{u}&=-\left(\displaystyle\frac{\partial\mathcal{L}_{\boldsymbol{\theta}}(\boldsymbol{\theta}_{ad},\boldsymbol{\theta}_{ud},\boldsymbol{0},\boldsymbol{0})}{\partial\boldsymbol{\theta}_{a}}\right)^{T}\\\
&\quad+\boldsymbol{K}_{P}(\boldsymbol{\theta}_{ad}-\boldsymbol{\theta}_{a})-\boldsymbol{K}_{D}\dot{\boldsymbol{\theta}}_{a},\end{split}$
(22)

  2. 2.

P-satI-D [26]:

$\begin{split}\boldsymbol{u}&=\boldsymbol{K}_{P}\left(\boldsymbol{\theta}_{ad}-\boldsymbol{\theta}_{a}\right)-\boldsymbol{K}_{D}\dot{\boldsymbol{\theta}}_{a}\\\
&\quad+\frac{\boldsymbol{K}_{I}}{\gamma}\int_{0}^{t}\boldsymbol{\mathrm{tanh}}\left(\boldsymbol{\theta}_{ad}-\boldsymbol{\theta}_{a}\right)(z)\mathrm{d}z,\end{split}$
(23)

globally asymptotically stabilizes the closed-loop system at
$(\boldsymbol{\theta}_{a}\,\,\boldsymbol{\theta}_{u}\,\,\dot{\boldsymbol{\theta}})=(\boldsymbol{\theta}_{ad}\,\,\boldsymbol{\theta}_{ud}\,\,\boldsymbol{0})$,
where $\boldsymbol{\theta}_{ad}\in\mathbb{R}^{m}$ and
$\boldsymbol{\theta}_{ud}\in\mathbb{R}^{n-m}$ is the unique solution to

$\left(\displaystyle\frac{\partial\mathcal{L}_{\boldsymbol{\theta}}(\boldsymbol{\theta}_{ad},\boldsymbol{\theta}_{u},\boldsymbol{0},\boldsymbol{0})}{\partial\boldsymbol{\theta}_{u}}\right)^{T}=\boldsymbol{0}.$

Under (21), for any value of the actuated coordinates
$\boldsymbol{\theta}_{a}$, there is a unique equilibrium of those unactuated,
i.e., the system equilibria are uniquely determined by
$\boldsymbol{\theta}_{a}$. Instead, (20) guarantees internal stability of the
closed-loop system. Note that the above controllers admit more general
structures, see [10] and [26].

We exploit the previous corollary to perform a shape regulation task for a
continuum soft robot moving in 3D. The robot has rest length
$L=$0.4\text{\,}\mathrm{[}\mathrm{m}\mathrm{]}$$ and cross section radius
$R\in[0.02;\ 0.008]\leavevmode\nobreak\ $\mathrm{[}\mathrm{m}\mathrm{]}$$,
which varies linearly from the base to the tip. The mass density is
$\rho=$680\text{\,}\mathrm{[}\mathrm{k}\mathrm{g}\mathrm{/}\mathrm{{}^{3}}\mathrm{m}\mathrm{]}$$.
Furthermore, we consider Young modulus
$E=$8.88\text{\times}{10}^{5}$$\mathrm{[}\mathrm{N}\mathrm{/}\mathrm{{}^{2}}\mathrm{m}\mathrm{]}$$,
Poisson ratio $P=0.5$ and material damping
$D=$1\text{\times}{10}^{4}$$\mathrm{[}\mathrm{N}\mathrm{/}\mathrm{{}^{2}}\mathrm{m}\mathrm{s}\mathrm{]}$$.
Eight tendons actuate the robot. The first six have an oblique routing and are
displaced $60^{\circ}$ each. Their initial distance from the center line is
$0.0016\text{\,}\mathrm{[}\mathrm{m}\mathrm{]}$. Three of these run from the
base to half of the robot, while the remaining ones up to the tip. The last
two tendons have an helical routing with pitch
$\frac{0.4}{2\pi}$\mathrm{[}\mathrm{m}\mathrm{]}$$ and are displaced
$180^{\circ}$, with a distance from the backbone of
$0.006\text{\,}\mathrm{[}\mathrm{m}\mathrm{]}$. We model the strain as

$\boldsymbol{\xi}(X,{\boldsymbol{q}})=\underbrace{\left(\begin{array}[]{cc}\boldsymbol{\Sigma}^{-1}\boldsymbol{\Phi}_{a}(X,{\boldsymbol{q}}^{*})&\boldsymbol{\Phi}_{u}(X)\end{array}\right)}_{\boldsymbol{\Phi}_{\boldsymbol{\xi}}(X)}{\boldsymbol{q}}+\boldsymbol{\xi}^{*},$

where ${\boldsymbol{q}}\in\mathbb{R}^{11}$,
${\boldsymbol{q}}^{*}=\boldsymbol{0}_{11}$ and
$\boldsymbol{\xi}^{*}=\left(0\,\,0\,\,1\,\,0\,\,0\,\,0\right)^{T}$ denote the
stress-free configuration and strain, respectively, and
$\boldsymbol{\Sigma}(X)>0\in\mathbb{R}^{6\times 6}$ is the body stiffness
matrix. The strain basis
$\boldsymbol{\Sigma}^{-1}\boldsymbol{\Phi}_{a}(X,{\boldsymbol{q}}^{*})$ has
proven to accurately describe the deformations due to the actuation forces
[38]. Instead, $\boldsymbol{\Phi}_{u}(X)\in\mathbb{R}^{6\times 3}$, which
collects the coefficients of three Legendre polynomials on the angular
components of the strain, models the deformations due to the gravitational
field, not captured by $\boldsymbol{\Sigma}^{-1}\boldsymbol{\Phi}_{a}$. Since
$n=11$ and $r=m=8$, the system is underactuated. Note that only shape
regulation tasks can be achieved in general. We compare (23) with the PD+
regulator in ${\boldsymbol{q}}$ space of [43]

$\begin{split}\boldsymbol{u}&=-\ActM[][\dagger][({\boldsymbol{q}}_{d})]\left(\displaystyle\frac{\partial\mathcal{L}_{{\boldsymbol{q}}}({\boldsymbol{q}}_{d},\boldsymbol{0})}{\partial{\boldsymbol{q}}}\right)^{T}\\\
&\quad+\ActM[][T]\left[k_{\mathrm{P}}\left({\boldsymbol{q}}_{d}-{\boldsymbol{q}}\right)-k_{\mathrm{D}}\dot{\boldsymbol{q}}\right],\end{split}$
(24)

Due to $\ActM$, the above control law guarantees only local asymptotic
stability [43, 2], and it requires information of the entire state of the
robot to be implemented. The control gains of (23) and (24) are
$\boldsymbol{K}_{P}=k_{P}\boldsymbol{I}_{8}$,
$\boldsymbol{K}_{D}=k_{D}\boldsymbol{I}_{8}$,
$\boldsymbol{K}_{I}=k_{I}\boldsymbol{I}_{8}$ and $\gamma=1$, with
$k_{P}=2.1\times 10^{3}\leavevmode\nobreak\
$\mathrm{[}\mathrm{N}\mathrm{/}\mathrm{m}\mathrm{]}$$,
$k_{D}=5\leavevmode\nobreak\
$\mathrm{[}\mathrm{N}\mathrm{s}\mathrm{/}\mathrm{m}\mathrm{]}$$ and
$k_{I}=2\times 10^{3}\leavevmode\nobreak\
$\mathrm{[}\mathrm{N}\mathrm{/}\mathrm{m}\mathrm{s}\mathrm{]}$$. Because of
the underactuation, only the configurations satisfying the equilibrium
equation

$\left(\displaystyle\frac{\partial\mathcal{L}_{{\boldsymbol{q}}}({\boldsymbol{q}}_{eq},\boldsymbol{0})}{\partial{\boldsymbol{q}}}\right)^{T}=\ActM[][][({\boldsymbol{q}}_{eq})]\boldsymbol{u},$

with $\boldsymbol{u}\in\mathbb{R}^{8}$, can be controlled. We command the
three desired shapes given in (25) as step references spaced in time by
$5\text{\,}\mathrm{[}\mathrm{s}\mathrm{]}$.

$\begin{array}[]{l}{\boldsymbol{q}}_{d,1}=\left(\begin{array}[]{ccccccccccc}-8.41&\hskip
2.0pt-2.44&-4.09&-26.21&-23.38&-26.43&-1.73&-1.79&\hphantom{+}0.21&\hphantom{+}0.68&-1.50\end{array}\right),\\\
{\boldsymbol{q}}_{d,2}=\left(\begin{array}[]{ccccccccccc}-12.85&-6.65&-15.37&-15.03&-14.18&-14.64&-0.35&-0.62&0.21&\hphantom{+}0.44&-1.71\end{array}\right),\\\
{\boldsymbol{q}}_{d,3}=\left(\begin{array}[]{ccccccccccc}-13.12&-5.50&-26.39&-22.79&-19.77&-20.56&-0.77&-0.97&-0.38&-2.19&1.50\end{array}\right).\end{array}$
(25)

Furthermore, ${\boldsymbol{q}}_{d,i}$ is converted into a desired tendon
displacement $\boldsymbol{\theta}_{ud,i}=\boldsymbol{y}_{d,i}(t)$, $i=1,2,3$,
for (23). The robot starts from the straight (stress-free) configuration at
rest, and the simulation runs for $15\text{\,}\mathrm{[}\mathrm{s}\mathrm{]}$.

Figs. 6(b)–6(e) and 6(a)–6(d) show the evolution of the actuation coordinates
and the configuration variables under (23) and (24), respectively. As
expected, the P-satI-D regulates the actuation coordinates to the desired set
point. On the other hand, the PD+ in ${\boldsymbol{q}}$ space fails this task,
always showing a steady state error. However, the closed-loop system remains
stable. Compared to the PD+ regulator, the P-satI-D has slower transients, as
evident also from the control action in Figs. 6(c)–6(f). This is expected
because the P-satI-D learns, through the integral action, the feedforward
command zeroing the tracking error. Finally, Fig. 7 presents a photo sequence
of the two closed-loop systems. Note that the end-effector reaches the correct
position only under the P-satI-D.

(a) P-satI-D in $\boldsymbol{y}$ space

(b) P-satI-D in $\boldsymbol{y}$ space

(c) P-satI-D in $\boldsymbol{y}$ space

(d) PD+ in ${\boldsymbol{q}}$ space

(e) PD+ in ${\boldsymbol{q}}$ space

(f) PD+ in ${\boldsymbol{q}}$ space

Figure 6: Time evolutions of (a)–(d) actuator elongations, (b)–(e)
configuration variables, and (c)–(f) control actions under the P-satI-D (23)
in $\boldsymbol{y}$ space and the PD+ (24) in ${\boldsymbol{q}}$ space,
respectively. The P-satI-D regulates the actuation coordinates to the desired
target. On the other hand, the closed-loop system under the PD+ regulator is
stable but has a steady-state error.

(a) P-satI-D in $\boldsymbol{y}$ space; $t\in[0;\
5]$\mathrm{[}\mathrm{s}\mathrm{]}$$

(b) P-satI-D in $\boldsymbol{y}$ space; $t\in[5;\
10]$\mathrm{[}\mathrm{s}\mathrm{]}$$

(c) P-satI-D in $\boldsymbol{y}$ space; $t\in[10;\
15]$\mathrm{[}\mathrm{s}\mathrm{]}$$

(d) PD+ in ${\boldsymbol{q}}$ space; $t\in[0;\
5]$\mathrm{[}\mathrm{s}\mathrm{]}$$

(e) PD+ in ${\boldsymbol{q}}$ space; $t\in[5;\
10]$\mathrm{[}\mathrm{s}\mathrm{]}$$

(f) PD+ in ${\boldsymbol{q}}$ space; $t\in[10;\
15]$\mathrm{[}\mathrm{s}\mathrm{]}$$

Figure 7: Frame sequences of robot motion, divided in three time windows.
Figs. (a)–(c) and (d)–(f) show the robot when controlled in the
$\boldsymbol{y}$ or ${\boldsymbol{q}}$ coordinates using the laws (23) and
(24), respectively. The initial and final configurations of each interval are
shown in blue, while light gray shapes represent intermediate configurations.
A red star indicates the constant target position of the end-effector.

##  VIII Conclusions

This article has considered the input decoupling problem for Lagrangian
systems. We have shown that there exists a class of Lagrangian dynamics,
called collocated, for which a coordinate transformation decouples actuator
inputs entering the equations of motion through a configuration-dependent
actuation matrix. These coordinates have a physical interpretation and can be
easily computed. Under mild conditions on the differentiability of the
actuation matrix, a simple test allows verifying if the dynamics is collocated
or not. As a consequence of power invariance, the results equally apply to
fully actuated, overactuated and underactuated systems. In case of
underactuated dynamics, inputs are collocated with the actuation coordinates,
while some freedom in left in the definition of the unactuated coordinates.
Since we consider only coordinate transformations, the proposed method differs
from the standard differential geometric approach used for controlling input-
affine nonlinear systems, which typically makes use of complete feedback
transformations. As a byproduct of our approach, we have shown that all
mechanical systems driven by thread-like actuators are collocated. Moreover,
we were able to extend control laws recently developed for underactuated
mechanical systems with constant actuation matrix to collocated Lagrangian
systems with damping.

Future works will be devoted to the experimental validation of the proposed
method and to special choices of the unactuated variables that further
simplify the equations of motion.

###  -A Properties of Lagrangian systems

We recall two important properties of Lagrangian systems that play a key role
in deriving the results in this paper.

######  Property 1.

Let
$\mathcal{H}_{{\boldsymbol{q}}}({\boldsymbol{q}},\dot{\boldsymbol{q}}):=\displaystyle\frac{\partial\mathcal{L}_{{\boldsymbol{q}}}({\boldsymbol{q}},\dot{\boldsymbol{q}})}{\partial\dot{\boldsymbol{q}}}\dot{\boldsymbol{q}}-\mathcal{L}_{{\boldsymbol{q}}}({\boldsymbol{q}},\dot{\boldsymbol{q}})$
be the system Hamiltonian. For all
${\boldsymbol{q}}\in\mathcal{M},\dot{\boldsymbol{q}}\in
T_{{\boldsymbol{q}}}\mathcal{M}$ and $\boldsymbol{u}\in\mathbb{R}^{m}$, it
holds

$\dot{\mathcal{H}}_{{\boldsymbol{q}}}({\boldsymbol{q}},\dot{\boldsymbol{q}})=\dot{\boldsymbol{q}}^{T}\boldsymbol{\tau}_{{\boldsymbol{q}}}({\boldsymbol{q}},\boldsymbol{u}).$
(26)

Equation (26) states that the time rate of change of the Hamiltonian, i.e.,
the system total energy, equals the input power. It also follows from (1) and
(26) that the dynamics is passive with respect to the pair
$(\boldsymbol{u},\dot{\boldsymbol{y}})=\left(\boldsymbol{u},\ActM[][T]\dot{\boldsymbol{q}}\right)$
with storage function
$\mathcal{H}_{{\boldsymbol{q}}}({\boldsymbol{q}},\dot{\boldsymbol{q}})$.

######  Property 2.

If
$\boldsymbol{\theta}:\mathcal{B}({\boldsymbol{q}})\rightarrow\mathcal{N}=\boldsymbol{h}({\boldsymbol{q}})$
is a (local) diffeomorphism, with
$\boldsymbol{J}_{\boldsymbol{h}}({\boldsymbol{q}})=\displaystyle\displaystyle\frac{\partial\boldsymbol{h}}{\partial{\boldsymbol{q}}}$,
then

$\displaystyle\frac{\mathrm{d}}{\mathrm{d}t}\left(\displaystyle\frac{\partial\mathcal{L}_{\boldsymbol{\theta}}(\boldsymbol{\theta},\dot{\boldsymbol{\theta}})}{\partial\dot{\boldsymbol{\theta}}}\right)^{T}-\left(\displaystyle\frac{\partial\mathcal{L}_{\boldsymbol{\theta}}(\boldsymbol{\theta},\dot{\boldsymbol{\theta}})}{\partial\boldsymbol{\theta}}\right)^{T}=\boldsymbol{\tau}_{\boldsymbol{\theta}}(\boldsymbol{\theta},\boldsymbol{u}),$
(27)

where

$\mathcal{L}_{\boldsymbol{\theta}}(\boldsymbol{\theta},\dot{\boldsymbol{\theta}})=\mathcal{L}_{{\boldsymbol{q}}}({\boldsymbol{q}}=\boldsymbol{h}^{-1}(\boldsymbol{\theta}),\dot{\boldsymbol{q}}=\boldsymbol{J}^{-1}_{\boldsymbol{h}}\dot{\boldsymbol{\theta}}),$

and

$\boldsymbol{\tau}_{\boldsymbol{\theta}}(\boldsymbol{\theta},\boldsymbol{u})=\boldsymbol{J}^{-T}_{\boldsymbol{h}}\boldsymbol{\tau}_{{\boldsymbol{q}}}({\boldsymbol{q}}=\boldsymbol{h}^{-1}(\boldsymbol{\theta}),\boldsymbol{u}).$

This also implies that, for all
$\boldsymbol{\theta}\in\mathcal{N},\dot{\boldsymbol{\theta}}\in
T_{\boldsymbol{\theta}}\mathcal{N},{\boldsymbol{q}}\in\mathcal{M},\dot{\boldsymbol{q}}\in
T_{{\boldsymbol{q}}}\mathcal{M}$ and $\boldsymbol{u}\in\mathbb{R}^{m}$, it
holds

$\dot{\mathcal{H}}_{\boldsymbol{\theta}}(\boldsymbol{\theta},\dot{\boldsymbol{\theta}})=\dot{\boldsymbol{\theta}}^{T}\boldsymbol{\tau}_{\boldsymbol{\theta}}(\boldsymbol{\theta},\boldsymbol{u})=\dot{\boldsymbol{q}}^{T}\boldsymbol{\tau}_{{\boldsymbol{q}}}({\boldsymbol{q}},\boldsymbol{u})=\dot{\mathcal{H}}_{{\boldsymbol{q}}}({\boldsymbol{q}},\dot{\boldsymbol{q}}),$
(28)

being
$\mathcal{H}_{\boldsymbol{\theta}}(\boldsymbol{\theta},\dot{\boldsymbol{\theta}})=\mathcal{H}_{{\boldsymbol{q}}}({\boldsymbol{q}}=\boldsymbol{h}^{-1}(\boldsymbol{\theta}),\dot{\boldsymbol{q}}=\boldsymbol{J}^{-1}_{\boldsymbol{h}}\dot{\boldsymbol{\theta}})$.

According to the above property, the Euler-Lagrange equations and the power
are invariant, i.e., they do not depend on the choice of coordinates
representing the dynamics.

###  -B Alternative proof of Theorem 2

We provide an alternative proof of the if part of Theorem 2. Similar
considerations also hold for Theorem 1 and Corollary 1.

###### Proof.

From Property 2, we have

$\boldsymbol{\tau}_{\boldsymbol{\theta}}=\boldsymbol{J}^{-T}_{\boldsymbol{h}}\ActM[][][({\boldsymbol{q}}=\boldsymbol{h}^{-1}(\boldsymbol{\theta}))]\boldsymbol{u}.$
(29)

By exploiting the block triangular structure of
$\boldsymbol{J}_{\boldsymbol{h}}({\boldsymbol{q}})$ it follows

$\boldsymbol{J}^{-T}_{\boldsymbol{h}}({\boldsymbol{q}})=\left(\begin{array}[]{cc}\ActM[a][-1]&\boldsymbol{O}_{m\times(n-m)}\\\
-\ActM[u][][]\ActM[a][-1]&\boldsymbol{I}_{n-m}\end{array}\right),$

which yields

$\begin{split}\boldsymbol{J}^{-T}_{\boldsymbol{h}}\ActM[][][]&=\left(\begin{array}[]{cc}\ActM[a][-1]&\boldsymbol{O}_{m\times(n-m)}\\\
-\ActM[u][][]\ActM[a][-1]&\boldsymbol{I}_{n-m}\end{array}\right)\left(\begin{array}[]{c}\ActM[a][]\\\
\ActM[u][]\end{array}\right)\\\
&=\left(\begin{array}[]{c}\boldsymbol{I}_{m}\\\ \boldsymbol{O}_{(n-m)\times
m}\end{array}\right).\end{split}$

∎

## References

  * [1] G. Chen and F. L. Lewis, “Distributed adaptive tracking control for synchronization of unknown networked Lagrangian systems,” _IEEE Trans. on Systems, Man, and Cybernetics_ , vol. 41, no. 3, pp. 805–816, 2010.
  * [2] R. Ortega, J. A. L. Perez, P. J. Nicklasson, and H. J. Sira-Ramirez, _Passivity-based Control of Euler-Lagrange Systems: Mechanical, Electrical and Electromechanical Applications_. Springer, 2013.
  * [3] A. Loría, “Observers are unnecessary for output-feedback control of Lagrangian systems,” _IEEE Trans. on Automatic Control_ , vol. 61, no. 4, pp. 905–920, 2015.
  * [4] M.-F. Ge, Z.-W. Liu, G. Wen, X. Yu, and T. Huang, “Hierarchical controller-estimator for coordination of networked Euler–Lagrange systems,” _IEEE Trans. on Cybernetics_ , vol. 50, no. 6, pp. 2450–2461, 2019\.
  * [5] Y. Sun, D. Dong, H. Qin, and W. Wang, “Distributed tracking control for multiple Euler–Lagrange systems with communication delays and input saturation,” _ISA Trans._ , vol. 96, pp. 245–254, 2020.
  * [6] A. M. Giordano, C. Ott, and A. Albu-Schäffer, “Coordinated control of spacecraft’s attitude and end-effector for space robots,” _IEEE Robotics and Automation Lett._ , vol. 4, no. 2, pp. 2108–2115, 2019.
  * [7] B. Yi, R. Ortega, I. R. Manchester, and H. Siguerdidjane, “Path following of a class of underactuated mechanical systems via immersion and invariance-based orbital stabilization,” _Int. J. of Robust and Nonlinear Control_ , vol. 30, no. 18, pp. 8521–8544, 2020.
  * [8] R. Mengacci, F. Angelini, M. G. Catalano, G. Grioli, A. Bicchi, and M. Garabini, “On the motion/stiffness decoupling property of articulated soft robots with application to model-free torque iterative learning control,” _Int. J. of Robotics Research_ , vol. 40, no. 1, pp. 348–374, 2021\.
  * [9] M. Keppler, C. Ott, and A. Albu-Schäffer, “From underactuation to quasi-full actuation: Aiming at a unifying control framework for articulated soft robots,” _Int. J. of Robust and Nonlinear Control_ , vol. 32, no. 9, pp. 5453–5484, 2022.
  * [10] P. Borja, A. Dabiri, and C. Della Santina, “Energy-based shape regulation of soft robots with unactuated dynamics dominated by elasticity,” in _Proc. 5th IEEE Int. Conf. on Soft Robotics_ , 2022, pp. 396–402.
  * [11] W. Zhang, Y. Wang, Y. Liu, and W. Zhang, “Multivariable disturbance observer-based ${H}_{2}$ analytical decoupling control design for multivariable systems,” _Int. J. of Systems Science_ , vol. 47, no. 1, pp. 179–193, 2016\.
  * [12] A. J. Krener, “On the equivalence of control systems and the linearization of nonlinear systems,” _SIAM J. on Control_ , vol. 11, no. 4, pp. 670–676, 1973.
  * [13] R. W. Brockett, “Feedback invariants for nonlinear systems,” _IFAC Proceedings_ , vol. 11, no. 1, pp. 1115–1120, 1978.
  * [14] A. Isidori, A. Krener, C. Gori-Giorgi, and S. Monaco, “Nonlinear decoupling via feedback: A differential geometric approach,” _IEEE Trans. on Automatic Control_ , vol. 26, no. 2, pp. 331–345, 1981.
  * [15] H. Nijmeijer and A. Van der Schaft, _Nonlinear Dynamical Control Systems_. Springer, 1990.
  * [16] A. Isidori, _Nonlinear control systems: an introduction_. Springer, 1992.
  * [17] R. Ortega, J. G. Romero, P. Borja, and A. Donaire, _PID Passivity-Based Control of Nonlinear Systems with Applications_. John Wiley & Sons, 2021.
  * [18] J. Wu and Y. Lu, “Adaptive backstepping sliding mode control for boost converter with constant power load,” _IEEE Access_ , vol. 7, pp. 50 797–50 807, 2019.
  * [19] A. Lazrak and A. Abbou, “An improved control strategy for DFIG wind turbine to ride-through voltage dips,” in _Proc. 6th Int. Renewable and Sustainable Energy Conf._ , 2018, pp. 1–6.
  * [20] R. N. Mishra and K. B. Mohanty, “Design and implementation of a feedback linearization controlled IM drive via simplified neuro-fuzzy approach,” _IETE J. of Research_ , vol. 64, no. 2, pp. 209–230, 2018.
  * [21] X. Ding, J. Liang, S. Lu, F. Kong, and Y. Chen, “Robust back-stepping sliding mode control for LCL-type grid-connected inverters in weak grids,” _J. of Power Electronics_ , vol. 23, pp. 758–768, 2022.
  * [22] S. Skogestad, C. Zotică, and N. Alsop, “Transformed inputs for linearization, decoupling and feedforward control,” _J. of Process Control_ , vol. 122, pp. 113–133, 2023.
  * [23] E. Franco and A. Garriga-Casanovas, “Energy-shaping control of soft continuum manipulators with in-plane disturbances,” _Int. J. of Robotics Research_ , vol. 40, no. 1, pp. 236–255, 2021.
  * [24] B. Caasenbrood, A. Pogromsky, and H. Nijmeijer, “Energy-based control for soft manipulators using Cosserat-beam models,” in _Proc. 18th Int. Conf. on Informatics in Control, Automation and Robotics_ , 2021, pp. 311–319.
  * [25] P. Pustina, C. Della Santina, and A. De Luca, “Feedback regulation of elastically decoupled underactuated soft robots,” _IEEE Robotics and Automation Lett._ , vol. 7, no. 2, pp. 4512–4519, 2022.
  * [26] P. Pustina, P. Borja, C. Della Santina, and A. De Luca, “P-satI-D shape regulation of soft robots,” _IEEE Robotics and Automation Lett._ , vol. 8, no. 1, pp. 1–8, 2023.
  * [27] G. Soleti, J. Prechtl, P. R. Massenio, M. Baltes, and G. Rizzello, “Energy based control of a bi-stable and underactuated soft robotic system based on dielectric elastomer actuators,” in _Proc. 22th IFAC World Congr._ , 2023\.
  * [28] A. De Luca, “Flexible robots,” in _Encyclopedia of Systems and Control_ , 2nd ed., J. Baillieul and T. Samad, Eds. Springer, 2021, pp. 814–822.
  * [29] A. Tsolakis and T. Keviczky, “Distributed IDA-PBC for a class of nonholonomic mechanical systems,” _IFAC-PapersOnLine_ , vol. 54, no. 14, pp. 275–280, 2021.
  * [30] M. P. do Carmo, _Differential Forms and Applications_. Springer, 1998.
  * [31] R. M. Murray, Z. Li, and S. S. Sastry, _A Mathematical Introduction to Robotic Manipulation_. CRC Press, 1994.
  * [32] H. Goldstein, _Classical mechanics_. Addison Wesley, 1980.
  * [33] C. Della Santina, A. Bicchi, and D. Rus, “On an improved state parametrization for soft robots with piecewise constant curvature and its use in model based control,” _IEEE Robotics and Automation Lett._ , vol. 5, no. 2, pp. 1001–1008, 2020.
  * [34] G. Palli, G. Borghesan, and C. Melchiorri, “Modeling, identification, and control of tendon-based actuation systems,” _IEEE Trans. on Robotics_ , vol. 28, no. 2, pp. 277–290, 2011.
  * [35] F. Renda, C. Armanini, V. Lebastard, F. Candelier, and F. Boyer, “A geometric variable-strain approach for static modeling of soft manipulators with tendon and fluidic actuation,” _IEEE Robotics and Automation Lett._ , vol. 5, no. 3, pp. 4006–4013, 2020.
  * [36] F. Boyer, V. Lebastard, F. Candelier, and F. Renda, “Dynamics of continuum and soft robots: A strain parameterization based approach,” _IEEE Trans. on Robotics_ , vol. 37, no. 3, pp. 847–863, 2020.
  * [37] C. Armanini, F. Boyer, A. T. Mathew, C. Duriez, and F. Renda, “Soft robots modeling: A structured overview,” _IEEE Trans. on Robotics_ , vol. 39, no. 3, pp. 1728–1748, 2023.
  * [38] F. Renda, C. Armanini, A. Mathew, and F. Boyer, “Geometrically-exact inverse kinematic control of soft manipulators with general threadlike actuators’ routing,” _IEEE Robotics and Automation Lett._ , vol. 7, no. 3, pp. 7311–7318, 2022.
  * [39] B. A. Jones and I. D. Walker, “Kinematics for multisection continuum robots,” _IEEE Trans. on Robotics_ , vol. 22, no. 1, pp. 43–55, 2006.
  * [40] D. Braganza, D. M. Dawson, I. D. Walker, and N. Nath, “A neural network controller for continuum robots,” _IEEE Trans. on Robotics_ , vol. 23, no. 6, pp. 1270–1277, 2007.
  * [41] V. Falkenhahn, A. Hildebrandt, R. Neumann, and O. Sawodny, “Dynamic control of the bionic handling assistant,” _IEEE/ASME Trans. on Mechatronics_ , vol. 22, no. 1, pp. 6–17, 2016.
  * [42] M. Li, R. Kang, S. Geng, and E. Guglielmino, “Design and control of a tendon-driven continuum robot,” _Trans. of the Institute of Measurement and Control_ , vol. 40, no. 11, pp. 3263–3272, 2018.
  * [43] C. Della Santina, C. Duriez, and D. Rus, “Model based control of soft robots: A survey of the state of the art and open challenges,” _IEEE Control Systems Mag._ , vol. 43, no. 3, pp. 30–65, 2023.