CSshort=CS,long=Constant Strain CoMshort=CoM,long=Center of Mass
DOFshort=DOF,long=Degrees of Freedom EOMshort=EOM,long=Equations of Motion
GBNshort=GBN,long=Generalized Binary Noise HSAshort=HSA,long=Handed Shearing
Auxetic PCSshort=PCS,long=Piecewise Constant Strain RMSEshort=RMSE,long=Root
Mean-Squared Error 11institutetext: Delft University of Technology, 2628 CD
Delft, Netherlands,
11email<EMAIL_ADDRESS>
22institutetext: Massachusetts Institute of Technology, Cambridge, MA 02139
USA

# An Experimental Study of Model-based Control for Planar Handed Shearing
Auxetics Robots

Maximilian Stölzle 11 Daniela Rus 22 Cosimo Della Santina 11

###### Abstract

Parallel robots based on Handed Shearing Auxetics (HSAs) can implement complex
motions using standard electric motors while maintaining the complete softness
of the structure, thanks to specifically designed architected metamaterials.
However, their control is especially challenging due to varying and coupled
stiffness, shearing, non-affine terms in the actuation model, and
underactuation. In this paper, we present a model-based control strategy for
planar HSA robots enabling regulation in task space. We formulate equations of
motion, show that they admit a collocated form, and design a P-satI-D feedback
controller with compensation for elastic and gravitational forces. We
experimentally identify and verify the proposed control strategy in closed
loop.

###### Keywords:

Soft Robotics, Model-based Control, Underactuation

##  1 Motivation and related work

The deformability, adaptiveness, and compliance of invertebrates serve as an
inspiration for continuum soft robots. While serial continuum soft robots have
been intensively investigated in recent years [2], parallel soft robots [5]
are less studied despite exhibiting exciting properties such as an improved
stiffness-to-weight ratio. One recent development in this field is robots
based on HSA [12, 6] in which multiple HSA rods are connected together at
their distal end through a rigid platform. Twisting of the proximal end of an
HSA causes the rod to elongate and enables complex motion primitives in 3D
space. Recent work has investigated the mechanical characterization [4],
simulation [11], and kinematic modeling [3, 11] of HSA robots but control has
yet to be tackled. In this work, we make a first step towards achieving task-
space control by designing model-based regulators for planar motions. Our
approach takes into account essential characteristics of HSA robots, such as
underactuation, shear strains, and varying stiffness.

(a) Blockscheme

(b) Operational workspace

Figure 1: Panel (a): Block scheme of the closed-loop system: we plan the
steady-state behavior such that the end-effector matches the given desired
position $p_{\mathrm{ee}}^{\mathrm{d}}$. The outputs of this planning are the
steady-state actuation $\phi^{\mathrm{ss}}$ and a suitable end-effector
orientation $\theta_{\mathrm{ee}}^{\mathrm{d}}$. After leveraging inverse
kinematics to identify the desired and current configuration, $q$ is mapped
into a collocated form where the inputs are decoupled. Finally, we use a
P-satI-D feedback controller on the actuation coordinates $\varphi$. Panel
(b): Visualization of the operational workspace of a planar HSA robot
consisting of FPU rods. The colored area within the black dashed borders
represents the positions the end-effector (visualized as a dot) can reach. The
coloring denotes the mean magnitude of actuation (i.e., twisting of the rods).
Furthermore, we plot three sample configurations: the unactuated straight
configuration $q=[0,0,0]^{\mathrm{T}}$ (blue), maximum clockwise bending
$q=[$-11.2\text{\,}\mathrm{r}\mathrm{a}\mathrm{d}\mathrm{/}\mathrm{m}$,0.08,0.30]^{\mathrm{T}}$
(red), and maximum counter-clockwise bending
$q=[$11.2\text{\,}\mathrm{r}\mathrm{a}\mathrm{d}\mathrm{/}\mathrm{m}$,-0.08,0.30]^{\mathrm{T}}$
(green).

Kinematic models for parallel robots usually require separate configuration
variables for each limb and the enforcement of kinematic constraints [1]. We
propose to avoid this complexity by defining the CS of a virtual backbone in
the center of the robot to be our configuration variable. Subsequently, we
derive the system dynamics in Euler-Lagrangian form. We notice that the
resulting planar dynamics are underactuated and that the actuation forces are
non-affine with respect to the control inputs, which are the motor angles. The
latter is a peculiarity of these systems, rarely observed in other robots.
Based on the model knowledge, we devise a control strategy shown in Fig. 1(a)
that first maps end-effector positions to desired configurations and steady-
state (feedforward) control inputs and then also applies a P-satI-D [8]
feedback action on the collocated form [9] of the system dynamics.

In summary, we state our contributions as (i) a closed-form solution for the
inverse kinematics of a planar CS formulation, (ii) an Euler-Lagrangian
dynamical model for planar HSA robots and its expression in collocated form,
(iii) a provably stable model-based control strategy for guiding the end-
effector of the robot towards a desired position in Cartesian space, and (iv)
experimental verification of both the model and the controller. A video
accompanies this paper explaining the methodology and displaying video
recordings of the control experiments111https://youtu.be/5A5yhMibctQ.

##  2 Technical approach

In the following, we consider a parallel HSA robot moving in a plane. First,
we derive the kinematic and dynamic models. Subsequently, we devise a planning
and control strategy to move the end-effector (i.e., the platform) to a
desired position in Cartesian space.

###  2.1 Kinematic model

Following the discrete Cosserat approach [10], we characterize the
configuration space of the virtual backbone by assuming a CS model
$\prescript{}{\mathcal{V}}{\xi}(t)=\begin{bmatrix}\prescript{}{\mathcal{V}}{\kappa}_{\mathrm{b}}&\prescript{}{\mathcal{V}}{\kappa}_{\mathrm{sh}}&\prescript{}{\mathcal{V}}{\sigma}_{\mathrm{ax}}\end{bmatrix}^{\mathrm{T}}=\mathbb{I}_{3}\,q(t)\in\mathbb{R}^{3}$,
where $\kappa_{\mathrm{be}}$, $\sigma_{\mathrm{sh}}$, and
$\sigma_{\mathrm{ax}}$ denote the bending, shear, and axial strain
respectively. Given $q$, the pose
$\chi=\begin{bmatrix}p_{x}&p_{y}&\theta\end{bmatrix}^{\mathrm{T}}\in SE(2)$,
and a point coordinate along the backbone $s\in[0,l^{0}]$, the forward and
inverse kinematics are provided in closed form as

$\small\chi=\pi(q,s)=\begin{bmatrix}\sigma_{\mathrm{sh}}\,\frac{\mathrm{s}_{\mathrm{b}}}{\kappa_{\mathrm{be}}}+\sigma_{\mathrm{ax}}\,\frac{\mathrm{c}_{\mathrm{be}}-1}{\kappa_{\mathrm{be}}}\\\
\sigma_{\mathrm{sh}}\,\frac{1-\mathrm{c}_{\mathrm{b}}}{\kappa_{\mathrm{be}}}+\sigma_{\mathrm{ax}}\,\frac{\mathrm{s}_{\mathrm{be}}}{\kappa_{\mathrm{be}}}\\\
\kappa_{\mathrm{be}}\,s\end{bmatrix},\qquad
q=\varrho(\chi,s)=\frac{\theta}{2s}\>\begin{bmatrix}2\\\
p_{y}-\frac{p_{x}\,\mathrm{s}_{\theta}}{\mathrm{c}_{\theta}-1}\\\
-p_{x}-\frac{p_{y}\,\mathrm{s}_{\theta}}{\mathrm{c}_{\theta}-1}\end{bmatrix},$
(1)

where we use the shorthand notations
$\mathrm{s}_{\mathrm{be}}=\sin(\kappa_{\mathrm{be}}s)$,
$\mathrm{c}_{\mathrm{be}}=\cos(\kappa_{\mathrm{be}}s)$,
$\mathrm{s}_{\theta}=\sin(\theta)$, and $\mathrm{c}_{\theta}=\cos(\theta)$.
Furthermore, the forward kinematics of the physical rods
$\mathcal{P}_{i},\,i\in\\{1,2\\}$ can be derived by first following the
transformations of the virtual backbone and then adding a local translation
$[\pm r_{\mathrm{off}},0]^{\mathrm{T}}$ with $r_{\mathrm{off}}$ being the
offset distance from the virtual backbone to the centerline of the HSA rod.
After closing the kinematic chain, we identify a mapping
$\beta_{i}:\prescript{}{\mathcal{V}}{\xi}\rightarrow\prescript{}{\mathcal{P}_{i}}{\xi}$
from the strains of the virtual backbone to the strains in the physical rods:
$\beta_{i}(\prescript{}{\mathcal{V}}{\xi})=\begin{bmatrix}\prescript{}{\mathcal{V}}{\kappa}_{\mathrm{b}},&\prescript{}{\mathcal{V}}{\sigma}_{\mathrm{sh}},&\prescript{}{\mathcal{V}}{\sigma}_{\mathrm{ax}}\pm
r_{\mathrm{off}}\prescript{}{\mathcal{V}}{\kappa}_{\mathrm{b}}\end{bmatrix}^{\mathrm{T}}$.
Prior work has shown that the auxetic trajectory of HSA can be modeled by
coupling the rest length $\tilde{l}_{i}$ to the twist strain
$\kappa_{\mathrm{tw},i}$ of the $i$th HSA rod [11, 4]:
$\tilde{l}_{i}=(1+\epsilon_{i})l^{0}=(1+h_{i}C_{\epsilon}\kappa_{\mathrm{tw},i})$
where $l^{0}$ is the printed length of the rod and $C_{\epsilon}$ a positive
constant. The handedness $h_{i}\in\\{-1,1\\}$ describes if positive or
negative twist angles are needed to elongate the closed HSA. For a given
vector of rod twist angles $\phi\in\mathbb{R}^{2}$ and after defining
$\phi_{i}^{+}=h_{i}\phi_{i}$, the elongation of the $i$th rod is then
$\epsilon_{i}=C_{\epsilon}\frac{\phi_{i}^{+}}{l^{0}}$. We provide examples in
Fig. 1(b) of the operational workspace that can be achieved with this
kinematic model.

Figure 2: Experimental setup: the parallel robot consists of four HSA rods
connected by a platform at their distal end. Four servo motors actuate the
HSAs. We track the pose of the end-effector with a motion capture system by
attaching reflective markers to the platform.

###  2.2 Dynamic model

We aim to devise a dynamic model in the Euler-Lagrange form
$M(q)\ddot{q}+C(q,\dot{q})\dot{q}+G(q)+K(q-q^{0})+D\dot{q}=\alpha(q,\phi),$
where $M(q),C(q,\dot{q}),K,D\in\mathbb{R}^{3\times 3}$ are the inertia,
Coriolis (derived with Christoffel symbols), elastic and damping matrices
respectively. $q^{0}\in\mathbb{R}^{3}$ captures the rest configuration. The
terms $G(q)$ and $\alpha(q,\phi)\in\mathbb{R}^{3}$ describe the gravitational
and actuation forces acting on the generalized coordinates. The state of the
robot at time $t$ can be therefore described by
$x(t)=\begin{bmatrix}q^{\mathrm{T}}(t)&\dot{q}^{\mathrm{T}}\end{bmatrix}^{\mathrm{T}}\in\mathbb{R}^{6}$.
The inertia matrix is found by following the standard procedure of integrating
mass and rotational inertia along the HSA rods [2]. Additionally, we consider
the inertial contribution of the platform mounted to the distal end of the
robot. Under the small strain assumption, the elastic forces of the $i$th HSA
rod can be modeled as

$\small\prescript{}{\mathcal{P}}{\tau}_{\mathrm{K},i}=\begin{bmatrix}S_{\mathrm{be},i}(\phi_{i})&S_{\mathrm{b},\mathrm{sh}}&0\\\
S_{\mathrm{b},\mathrm{sh}}&S_{\mathrm{sh},i}(\phi_{i})&0\\\
0&0&S_{\mathrm{ax},i}(\phi_{i})\end{bmatrix}\,\left(\begin{bmatrix}\prescript{}{\mathcal{P}_{i}}{\kappa}_{\mathrm{b}}\\\
\prescript{}{\mathcal{P}_{i}}{\sigma}_{\mathrm{sh}}\\\
\prescript{}{\mathcal{P}_{i}}{\sigma}_{\mathrm{ax}}\end{bmatrix}-\begin{bmatrix}\kappa_{\mathrm{be}}^{0}\\\
\sigma_{\mathrm{sh}}^{0}\\\
\sigma_{\mathrm{ax}}^{0}+\epsilon_{i}(\phi_{i})\end{bmatrix}\right),$ (2)

where
$\prescript{}{\mathcal{P}_{i}}{\xi}^{0}=\begin{bmatrix}\kappa_{\mathrm{be}}^{0}&\sigma_{\mathrm{sh}}^{0}&\sigma_{\mathrm{ax}}^{0}\end{bmatrix}^{\mathrm{T}}$
denotes the rest strain, $S_{\mathrm{be},i}(\phi_{i})$,
$S_{\mathrm{sh},i}(\phi_{i})$, $S_{\mathrm{ax},i}(\phi_{i})$ are the bending,
shear, and axial stiffnesses which are defined as linear functions with
respect to the twist angle of the rod $\phi_{i}$ [4, 11]:

$\small
S_{\mathrm{be},i}(\phi_{i})=\hat{S}_{\mathrm{b}}+C_{\mathrm{S}_{\mathrm{b}}}\,\phi_{i}^{+},\quad
S_{\mathrm{sh},i}(\phi_{i})=\hat{S}_{\mathrm{sh}}+C_{\mathrm{S}_{\mathrm{sh}}}\,\phi_{i}^{+},\quad
S_{\mathrm{ax},i}(\phi_{i})=\hat{S}_{\mathrm{ax}}+C_{\mathrm{S}_{\mathrm{ax}}}\,\phi_{i}^{+}.$
(3)

The coefficient $S_{\mathrm{b},\mathrm{sh}}$ accounts for the elastic coupling
between the bending and the shear strain. Subsequently, we project the forces
into the virtual backbone by premultiplying with
$J_{\beta}^{\mathrm{T}}=\frac{\partial\beta}{\partial q}^{\mathrm{T}}$ and
then sum the contribution of all rods. Finally, we group all terms depending
on the control input $\phi$ in $\alpha(q,\phi)$ and everything else in $K$.
After modeling the dissipative forces in each HSA as
$\mathrm{diag}(\zeta_{\mathrm{be}},\zeta_{\mathrm{sh}},\zeta_{\mathrm{ax}})\,\prescript{}{\mathcal{P}_{i}}{\dot{\xi}}$,
we derive the damping matrix in configuration space as
$D=\sum_{i=1}^{2}J_{\beta,i}^{\mathrm{T}}\,\mathrm{diag}(\zeta_{\mathrm{be}},\zeta_{\mathrm{sh}},\zeta_{\mathrm{ax}})\,J_{\beta,i}=2\,\mathrm{diag}\left((\zeta_{\mathrm{be}}+r_{\mathrm{off}}^{2}\,\zeta_{\mathrm{ax}}),\zeta_{\mathrm{sh}},\zeta_{\mathrm{ax}}\right)$.
We open-source the derivation of the Euler-Lagrangian dynamics and a JAX
implementation of a simulator based on them on
GitHub222https://github.com/tud-cor-sr/jax-soft-robot-modelling. We stress
that (a) the derived dynamical model is not affine in the control input and
(b) the system is underactuated.

###  2.3 Control

Our goal is to control the end-effector, which is defined as the distal
surface of the platform, to a desired position in Cartesian space
$p_{\mathrm{ee}}^{\mathrm{d}}\in\mathbb{R}^{2}$. However, the mapping into
configuration space is not trivial as we do not know which end-effector
orientation $\theta_{\mathrm{ee}}$ is feasible at steady-state. To tackle this
challenge, we perform steady-state planning identifying admittable
configurations $q^{\mathrm{d}}$ and matching steady-state actuations
$\phi^{\mathrm{ss}}$, which allow the robot’s end-effector to statically
remain at $p_{\mathrm{ee}}^{\mathrm{d}}$. More details on the used planning
procedure can be found in Section 3.4.

In principle, we can command $\phi=\phi^{\mathrm{ss}}$ to achieve regulation
towards the desired end-effector position. Nevertheless, we add a feedback
controller to compensate for any errors in $\phi^{\mathrm{ss}}$ caused by
unmodelled effects such as hysteresis. Unfortunately, the non-affine actuation
$\alpha(q,\phi)$ would complicate the design of such a feedback controller.
Therefore, we perform a first-order Taylor expansion of the actuation forces
with respect to $\phi$ resulting in a configuration-dependent actuation matrix
$A_{\phi^{\mathrm{ss}}}(q)=\frac{\partial\alpha}{\partial\phi}\big{|}_{\phi=\phi_{\mathrm{ss}}}\in\mathbb{R}^{3\times
2}$. This allows us to re-write the right side of the EOM as
$\tau_{q}=\alpha(q^{\mathrm{ss}},\phi^{\mathrm{ss}})+A_{\phi^{\mathrm{ss}}}(q)\,u$
where $u=\phi-\phi^{\mathrm{ss}}$ is the new control input. To improve the
robustness of the control loop, we compute $u$ with a P-satI-D control law
[8]. However, our system is underactuated and in a non-collocated form.
Therefore, we apply a coordinate transformation
$h:q\rightarrow\varphi\in\mathbb{R}^{3}$ recently introduced by Pustina et al.
[9] which maps the EOM into a form where $\phi$ applies direct forces on the
actuated configuration variables. The map is given by
$h(q)=\begin{bmatrix}\int_{0}^{t}\dot{q}^{\mathrm{T}}A_{\phi^{\mathrm{ss}}}(q)\mathrm{d}\tau,&\sigma_{\mathrm{sh}}\end{bmatrix}^{\mathrm{T}}=\begin{bmatrix}h_{1}(q),&h_{2}(q),&\sigma_{\mathrm{sh}}\end{bmatrix}^{\mathrm{T}}$
with

$\footnotesize
h_{i}(q)=C_{\mathrm{S},\mathrm{ax}}\,\frac{h_{i}}{l^{0}}\,\Big{[}2\,\varepsilon_{i}(\phi^{\mathrm{ss}}_{i})\left(\pm
r_{\mathrm{off}}\kappa_{\mathrm{be}}+\sigma_{\mathrm{ax}}\right)\mp
r_{\mathrm{off}}^{2}\frac{\kappa_{\mathrm{be}}^{2}}{2}\pm
r_{\mathrm{off}}\,\sigma_{\mathrm{ax}}^{0}\,\kappa_{\mathrm{be}}\mp
r_{\mathrm{off}}\,\kappa_{\mathrm{be}}\,\sigma_{\mathrm{ax}}+\sigma_{\mathrm{ax}}^{0}\,\sigma_{\mathrm{ax}}\\\
-\frac{\sigma_{\mathrm{ax}}^{2}}{2}\Big{]}+C_{\mathrm{S},\mathrm{b}}\,\frac{h_{i}}{l^{0}}\,\Big{[}\kappa_{\mathrm{be}}^{0}\,\kappa_{\mathrm{be}}-\frac{\kappa_{\mathrm{be}}^{2}}{2}\Big{]}+C_{\mathrm{S},\mathrm{sh}}\,\frac{h_{i}}{l^{0}}\,\Big{[}\sigma_{\mathrm{sh}}^{0}\,\sigma_{\mathrm{sh}}-\frac{\sigma_{\mathrm{sh}}^{2}}{2}\Big{]}+\hat{S}_{\mathrm{ax}}\,\frac{h_{i}}{l^{0}}\,C_{\varepsilon}\Big{[}\pm
r_{\mathrm{off}}\,\kappa_{\mathrm{be}}+\sigma_{\mathrm{ax}}\Big{]}.$ (4)

The Jacobian $\frac{\partial h}{\partial q}$ is used to formulate the dynamics
$M_{\varphi}\ddot{\varphi}+\eta(\varphi,\dot{\varphi})+G_{\varphi}+K_{\varphi}+D_{\varphi}\dot{\varphi}=A_{\varphi}\,\phi$
in the collocated variables [7], where
$A_{\varphi}^{\mathrm{T}}=\begin{bmatrix}\mathbb{I}^{2}&0^{\mathrm{2\times
1}}\end{bmatrix}^{\mathrm{T}}$. In the following, we will denote with the
subscript $a$ the first two actuated coordinates $\varphi_{\mathrm{a}}$.
Finally, the full control law of the _P-satI-D_ is given in collocated form as

$\small\phi=\phi^{\mathrm{ss}}+K_{\mathrm{p}}(\varphi_{\mathrm{a}}^{\mathrm{d}}-\varphi)-K_{\mathrm{d}}\dot{\varphi}_{\mathrm{a}}+K_{\mathrm{i}}\int_{0}^{t}\tanh(\gamma\,(\varphi_{\mathrm{a},t^{\prime}}^{\mathrm{d}}-\varphi_{\mathrm{a},t^{\prime}}))\>\mathrm{d}t^{\prime},$
(5)

where $K_{\mathrm{p}},K_{\mathrm{d}},K_{\mathrm{i}}\in\mathbb{R}^{2\times 2}$
are the proportional, derivative, and integral gains respectively, and
$\gamma\in\mathbb{R}^{2\times 2}$ horizontally compresses the hyperbolic
tangent. While the proposed P-satI-D control law compensates gravity through
$\phi^{\mathrm{ss}}$, we can extend the approach to include gravity
cancellation (_P-satI-D + GC_) by evaluating $G_{\varphi,\mathrm{a}}$ at the
current configuration:

$\small\phi=\phi^{\mathrm{ss}}-G_{\varphi,\mathrm{a}}(q^{\mathrm{d}})+G_{\varphi,\mathrm{a}}(q)+K_{\mathrm{p}}(\varphi_{\mathrm{a}}^{\mathrm{d}}-\varphi)-K_{\mathrm{d}}\dot{\varphi}_{\mathrm{a}}+K_{\mathrm{i}}\int_{0}^{t}\tanh(\gamma\,(\varphi_{\mathrm{a},t^{\prime}}^{\mathrm{d}}-\varphi_{\mathrm{a},t^{\prime}}))\>\mathrm{d}t^{\prime}.$
(6)

The implementation of all control laws is available on
GitHub333https://github.com/tud-cor-sr/hsa-planar-control.

(a) FPU: End-effector pose

(b) FPU: Configuration

(c) EPU: End-effector pose

(d) EPU: Configuration

Figure 3: Verification of the system model and the identified system
parameters on an unseen trajectory with the HSA being randomly actuated
through a GBN sequence: the solid line denotes the actual trajectory. In
contrast, the dashed line visualizes the trajectory simulated with the system
model. We report results for both FPU and EPU-based HSA.

##  3 Experimental validation

###  3.1 Experimental setup

We evaluate the system model and our proposed control approach on a robot
consisting of four HSA rods. The material choice of the HSA is crucial and has
a significant influence on the resulting mechanical characteristics of the
robot (e.g., blocked force, holding torque, bending stiffness, etc.) [12].
Furthermore, specific material requirements are dictated by the nature of the
design of the HSA rod. The structure of the metamaterial is made of struts
connected by living hinges. These living hinges need to be thin, flexible, and
accommodate high strains [12]. Therefore, we decided to 3D-print the HSA via
digital projection lithography either from the photopolymer resin Carbon FPU
50 (stiffer) or the elastomeric polyurethane EPU 40 resin (softer).

Each HSA rod is actuated by a Dynamixel MX-28 servo motor. The Dynamixel
motors are set to use position control mode. The robot is mounted platform-
down on a cage with an Optitrack motion capture system, which measures the
SE(3) pose of the platform at $200\text{\,}\mathrm{H}\mathrm{z}$. Our
algorithms run within a ROS2 framework444https://github.com/tud-cor-
sr/ros2-hsa. The pose measurements are first projected into the plane of
actuation and serve as an input to the closed-form inverse kinematics
introduced in (1). We use a Savitzky-Golay filter with a window duration of
$0.1\text{\,}\mathrm{s}$ to numerically differentiate $\chi_{\mathrm{ee}}(t)$,
$q(t)$ and gather with that $\dot{\chi}_{\mathrm{ee}}(t)$ and $\dot{q}(t)$.

###  3.2 System identification

Next, we strive to identify the parameters used in our dynamic model. We
assume the robot’s geometric and mass density properties to be known or easily
measurable. As knowledge about the damping coefficients is not required by the
control law, only the experimental identification of elongation and stiffness
characteristics remains. For this, we measure the response of the system to
step and staircase actuation sequences. Afterward, the parameters are
regressed using least squares. For the FPU-based robot, we identify
$C_{\varepsilon}^{\mathrm{FPU}}=$0.0079\text{\,}\mathrm{m}\mathrm{/}\mathrm{r}\mathrm{a}\mathrm{d}$$,
$S_{\mathrm{be}}^{\mathrm{FPU}}=-2.5\cdot 10^{-5}+3.9\cdot
10^{-7}\,\frac{\phi_{i}^{+}}{l^{0}}$\mathrm{N}\mathrm{m}^{2}$$,
$S_{\mathrm{sh}}^{\mathrm{FPU}}=0.043+0.0029\,\frac{\phi_{i}^{+}}{l^{0}}$\mathrm{N}$$,
$S_{\mathrm{ax}}^{\mathrm{FPU}}=0.74+0.0098\,\frac{\phi_{i}^{+}}{l^{0}}$\mathrm{N}$$,
and $S_{\mathrm{b,sh}}^{\mathrm{FPU}}=-5.0\cdot
10^{-4}$\mathrm{N}\mathrm{m}\mathrm{/}\mathrm{r}\mathrm{a}\mathrm{d}$$ where
$l^{0}=$0.059\text{\,}\mathrm{m}$$. Furthermore, we regress
$C_{\varepsilon}^{\mathrm{EPU}}=$0.0098\text{\,}\mathrm{m}\mathrm{/}\mathrm{r}\mathrm{a}\mathrm{d}$$,
$S_{\mathrm{be}}^{\mathrm{EPU}}=5.7\cdot 10^{-4}-9.7\cdot
10^{-6}\,\frac{\phi_{i}^{+}}{l^{0}}$\mathrm{N}\mathrm{m}^{2}$$,
$S_{\mathrm{sh}}^{\mathrm{EPU}}=0.59-0.00047\,\frac{\phi_{i}^{+}}{l^{0}}$\mathrm{N}$$,
$S_{\mathrm{ax}}^{\mathrm{EPU}}=5.7+0.015\,\frac{\phi_{i}^{+}}{l^{0}}$\mathrm{N}$$,
and
$S_{\mathrm{b,sh}}^{\mathrm{EPU}}=-$0.000\,48\text{\,}\mathrm{N}\mathrm{m}\mathrm{/}\mathrm{r}\mathrm{a}\mathrm{d}$$
for the EPU HSA which have the same length as the FPU HSA. Finally, we
identify the axial rest strain $\sigma_{\mathrm{ax}}^{0}$ before the start of
each experiment. We notice that the EPU-based HSA robot is approximately one
order of magnitude more flexible compared to the FPU-based robot.

(a) End-effector position

(b) Configuration

Figure 4: Step response of the _baseline PID_ , _P-satI-D_ (with gravity
compensation), and _P-satI-D + GC_ (with gravity cancellation) controllers on
an FPU-based HSA robot.

###  3.3 Model verification

We verify the accuracy of the proposed system model and the identified
parameters on trajectories unseen during system identification. We generate
the trajectories by actuating the robot with a GBN [13] sequence with a
settling time of $0.5\text{\,}\mathrm{s}$ and at each time step $k$ randomly
sample $\phi(k)\sim\mathcal{U}(0,\phi_{\mathrm{max}})$. We simulate the model
evolution with a Dormand-Prince 5(4) integrator and a time step of
$0.1\text{\,}\mathrm{m}\mathrm{s}$. Fig. 3(a) shows the model exhibiting
excellent accuracy for representing the behavior of FPU-based HSA robots. For
EPU-based HSA robots, we observe in Fig.3(d) more significant errors in the
shear estimate. Specifically, the CS model does not seem sufficient anymore
for capturing the shape of the robot, particularly for larger bending angles.
Therefore, we suggest for future work to employ kinematic models with more DOF
such as PCS as proposed for example in [11].

(a) End-effector position

(b) Configuration

(c) Control input

Figure 5: Experimental results for tracking a reference trajectory of eleven
step functions with the baseline PID controller on an FPU-based HSA robot.
Panel (a): End-effector position with the dotted and solid lines denoting the
task-space reference and actual position, respectively. Panel (b): The planned
(dotted) and the actual (solid) configuration. Panel (c): The planned (dotted)
and the actual (solid) actuation coordinates of the collocated system.
Panel(d): The saturated planar control inputs are visualized with solid lines,
and the computed steady-state actuation with dotted lines.

###  3.4 Steady-state planning

Our approach, as detailed in Section 2.3, requires us for a given desired end-
effector position $p_{\mathrm{ee}}^{\mathrm{d}}$ to identify a statically-
feasible configuration $q^{\mathrm{d}}$ with the matching steady-state
actuation $\phi^{\mathrm{ss}}$.

We perform online static inversion to identify admittable desired
configurations $q^{\mathrm{d}}$ and matching steady-state control inputs
$\phi^{\mathrm{ss}}$ during our experiments involving the FPU HSA robots.
First, we substitute the inverse kinematics
$\varrho_{\mathrm{ee}}(\chi_{\mathrm{ee}})$ into the static EOM. Then, we find
the roots of the equation
$G\circ\varrho_{\mathrm{ee}}(\chi_{\mathrm{ee}}^{\mathrm{d}})+K\circ\varrho_{\mathrm{ee}}(\chi_{\mathrm{ee}}^{\mathrm{d}})-\alpha(\varrho_{\mathrm{ee}}(\chi_{\mathrm{ee}}^{\mathrm{d}}),\phi_{\mathrm{ss}})$
with respect to $(\theta_{\mathrm{ee}},\phi_{1},\phi_{2})$ using nonlinear
least-squares while enforcing constraints on the sign of $\phi$. We solve this
optimization problem with projected gradient descent.

In contrast, the static inversion optimization problem is not well-behaved for
the identified EPU system parameters. Instead, we rely on rolling out the
dynamics over a duration $t_{\mathrm{ss}}$ to steady-state and then optimize
the steady-state input $\phi^{\mathrm{ss}}$ such that the final end-effector
error $\lVert
p_{\mathrm{ee}}^{\mathrm{d}}-p_{\mathrm{ee}}^{\mathrm{ss}}\rVert$ is as small
as possible. We formalize this optimization problem in a least-squares fashion

$\displaystyle\phi^{\mathrm{ss}}=\operatorname*{arg\\!\min}_{\mathrm{\phi}}$
$\displaystyle\frac{1}{2}\,\lVert
p_{\mathrm{ee}}^{\mathrm{d}}-p_{\mathrm{ee}}^{\mathrm{ss}}(\phi)\rVert_{2}^{2},$
(7) s.t. $\displaystyle
x^{\mathrm{ss}}=x(t_{0})+\int_{t_{0}}^{t_{\mathrm{ss}}}f(x(t),\phi)\,\mathrm{d}t,\quad\chi_{\mathrm{ee}}^{\mathrm{ss}}=\begin{bmatrix}p_{\mathrm{ee}}^{\mathrm{ss}}\\\
\theta_{\mathrm{ee}}^{\mathrm{ss}}\end{bmatrix}=\pi_{\mathrm{ee}}(q^{\mathrm{ss}}),$

where $\dot{x}(t)=f(x(t),\phi)$ are the nonlinear state-space dynamics based
on the EOM derived in Section 2.2 and $\phi\in\mathbb{R}^{2}$ is constant in
time. We solve (7) online using the Levenberg-Marquardt algorithm. Finally, we
choose $q^{d}=q^{\mathrm{ss}}$ and
$\chi_{\mathrm{ee}}^{\mathrm{d}}=\pi_{\mathrm{ee}}(q^{d})$.

###  3.5 Closed-loop control

Next, we implement the closed-loop control strategy laid out in Section 2.3.
After evaluating the control law at a rate of
$40\text{\,}\mathrm{H}\mathrm{z}$ and saturating the control inputs to the
ranges $[0,3.40]\,$\mathrm{r}\mathrm{a}\mathrm{d}$$ for FPU and
$[0,4.71]\,$\mathrm{r}\mathrm{a}\mathrm{d}$$ for EPU, respectively, we map
$\phi\in\mathbb{R}^{2}$ to desired positions of the four motors. For this, we
take into account the handedness of the HSA and apply the same actuation
magnitude to both rods on the same side of the virtual backbone. After tuning
the gains for the feedback part of the model-based control laws in (5) and
(6), we select $K_{\mathrm{p}}=\mathrm{diag}(0.3,0.3)$,
$K_{\mathrm{i}}=\mathrm{diag}(0.05,0.05)\,$\mathrm{1}\mathrm{/}\mathrm{s}$$,
$K_{\mathrm{d}}=\mathrm{diag}(0.01,0.01)\,$\mathrm{s}$$, and
$\gamma=\mathrm{diag}(100,100)$. Furthermore, we report the performance of a
model-free PID controller as a baseline. Here, the control input in task-space
is given by
$u_{\mathrm{ts}}=\begin{bmatrix}u_{\mathrm{ts,x}}&u_{\mathrm{ts,y}}\end{bmatrix}^{\mathrm{T}}=K_{\mathrm{p}}^{\mathrm{PID}}\,(p_{\mathrm{ee}}^{\mathrm{d}}-p_{\mathrm{ee}})-K_{\mathrm{d}}^{\mathrm{PID}}\,\dot{p}_{\mathrm{ee}}+K_{\mathrm{i}}^{\mathrm{PID}}\int_{0}^{t}p_{\mathrm{ee},t^{\prime}}^{\mathrm{d}}-p_{\mathrm{ee},t^{\prime}}\>\mathrm{d}t^{\prime}$,
which is then mapped to the actuation via
$\phi=\begin{bmatrix}u_{\mathrm{ts,x}}+u_{\mathrm{ts,y}},&-u_{\mathrm{ts,x}}+u_{\mathrm{ts,y}}\end{bmatrix}^{\mathrm{T}}$.
Here, we select
$K_{\mathrm{p}}^{\mathrm{PID}}=\mathrm{diag}(10,10)\,$\mathrm{r}\mathrm{a}\mathrm{d}\mathrm{/}\mathrm{m}$$,
$K_{\mathrm{i}}^{\mathrm{PID}}=\mathrm{diag}(110,110)\,$\mathrm{r}\mathrm{a}\mathrm{d}\mathrm{/}\mathrm{m}\mathrm{/}\mathrm{s}$$,
and
$K_{\mathrm{d}}^{\mathrm{PID}}=\mathrm{diag}(0.25,0.25)\,$\mathrm{r}\mathrm{a}\mathrm{d}\,\mathrm{s}\mathrm{/}\mathrm{m}$$.

(a) End-effector position

(b) Configuration

(c) Actuation coordinates

(d) Control input

Figure 6: Experimental results for tracking a reference trajectory of eleven
step functions with the P-satI-D controller on an FPU-based HSA robot. Panel
(a): End-effector position with the dotted and solid lines denoting the task-
space reference and actual position, respectively. Panel (b): The planned
(dotted) and the actual (solid) configuration. Panel (c): The planned (dotted)
and the actual (solid) actuation coordinates of the collocated system.
Panel(d): The saturated planar control inputs are visualized with solid lines,
and the computed steady-state actuation with dotted lines.

Evaluation: We define a reference trajectory
$p_{\mathrm{ee}}^{\mathrm{d}}(k),k\in\\{1,\dots,n_{k}\\}$ with a duration of
$110\text{\,}\mathrm{s}$ and consisting of eleven step functions as the
reference trajectory. We report the RMSE metric
$\sqrt{\sum_{k=1}^{n_{k}}\frac{\lVert
p_{\mathrm{ee}}^{\mathrm{d}}(k)-p_{\mathrm{ee}}(k)\rVert_{2}^{2}}{n_{k}}}$ for
assessing the control performance, where $p_{\mathrm{ee}}(k)$ is the actual
trajectory of the end-effector.

Control of an FPU-based HSA robot: The _baseline PID_ achieves an RMSE of
$5.86\text{\,}\mathrm{m}\mathrm{m}$ with respect to the reference trajectory.
The _P-satI-D_ based on (5) (with gravity compensation) exhibits an RMSE of
$4.17\text{\,}\mathrm{m}\mathrm{m}$. Similarly, the _P-satI-D + GC_ based on
(6) (with gravity cancellation) displays an RMSE of
$4.13\text{\,}\mathrm{m}\mathrm{m}$. We present a comparison of the three
different controllers for a step response in Fig. 4 and plot the entire
trajectories of the _baseline PID_ and the _P-satI-D_ in Figures 5 and 6,
respectively. Additionally, we discretize various continuous reference
trajectories into setpoints: star trajectory ($873$ setpoints and duration of
$109\text{\,}\mathrm{s}$), the flame of the TU Delft logo ($680$ setpoints and
duration of $85\text{\,}\mathrm{s}$), the contour of the MIT-CSAIL logo
($1046$ setpoints and duration of $131\text{\,}\mathrm{s}$), and the outline
of a bat at three different sizes ($1510$ setpoints and
$189\text{\,}\mathrm{s}$ duration). The resulting Cartesian evolutions of the
_P-satI-D_ controller tracking these continuous references are displayed in
Fig. 7.

(a) Star

(b) TUD flame

(c) MIT-CSAIL

(d) Bat trajectories

Figure 7: Cartesian evolution of the proposed P-sat-D controller (solid lines)
tracking various continuous reference trajectories (dotted lines) on the FPU
robot.

(a) t=$0\text{\,}\mathrm{s}$

(b) t=$47\text{\,}\mathrm{s}$

(c) t=$94\text{\,}\mathrm{s}$

(d) t=$141\text{\,}\mathrm{s}$

(e) t=$188\text{\,}\mathrm{s}$

Figure 8: Sequence of stills for the large bat trajectory performed with the
P-satD controller on the FPU robot. The red and black dots visualize the
desired and current end-effector positions, respectively. The past trajectory
is plotted in red (reference) and black (actual). The blue line renders the
shape of the virtual backbone.

The step response in Fig. 4 shows how the two model-based controllers _P-satI-
D_ and _P-satI-D + GC_ are able to leverage the planned $\phi^{\mathrm{ss}}$
and $q^{\mathrm{d}}$ to achieve a fast response time of roughly
$1.2\text{\,}\mathrm{s}$. In contrast, the baseline PID needs to wait for the
integral error to build up and thus has a much slower response time of
approximately $4.2\text{\,}\mathrm{s}$. Furthermore, overshooting caused by
the baseline PID is usually more extensive than that caused by the model-based
controllers. We conclude that _P-satI-D_ (gravity compensation) and _P-satI-D
+ GC_ (gravity cancellation) exhibit quite similar behavior. Sometimes, _P-
satI-D_ exhibits undershooting at the beginning of the transient and _P-satI-D
+ GC_ overshooting towards the end of the transient (see Fig. 4(a)).

(a) End-effector position

(b) Configuration

Figure 9: Step responses of the _baseline PID_ , _P-satI-D_ (with gravity
compensation), and _P-satI-D + GC_ (with gravity cancellation) controllers on
an EPU-based HSA robot.

(a) End-effector position

(b) Configuration

(c) Actuation coordinates

(d) Control input

Figure 10: Experimental results for tracking a reference trajectory of eleven
step functions with the P-satI-D controller on an EPU-based HSA robot. Panel
(a): End-effector position with the dotted and solid lines denoting the task-
space reference and actual position, respectively. Panel (b): The planned
(dotted) and the actual (solid) configuration. Panel (c): The planned (dotted)
and the actual (solid) actuation coordinates of the collocated system.
Panel(d): The saturated planar control inputs are visualized with solid lines,
and the computed steady-state actuation with dotted lines.

Control of an EPU-based HSA robot: Tracking the reference trajectory of eleven
step functions with an EPU-based robot, the _baseline PID_ controller has an
RMSE of $4.40\text{\,}\mathrm{m}\mathrm{m}$. The _P-satI-D_ (with gravity
compensation) is able to able to achieve an RMSE of
$3.63\text{\,}\mathrm{m}\mathrm{m}$. The _P-satI-D + GC_ controller exhibits
similar performance(RMSE of $3.71\text{\,}\mathrm{m}\mathrm{m}$). We visualize
the step response of all three controllers in Fig. 9 and the entire trajectory
of the _P-satI-D_ controller in Fig. 10.

Again, we notice that the response time of the model-based controllers
($0.54\text{\,}\mathrm{s}$) is much shorter than the response time of the
baseline PID ($3.84\text{\,}\mathrm{s}$). Furthermore, the importance of a
model-based control law is motivated by the oscillations in the transient of
the baseline PID (see x-coordinate in Fig. 9(a)). The steady-state error for
the model-based controllers on the EPU material is slightly higher compared to
the FPU material, as seen in Figures 9(a) & 10(a). In Section 3.3, we noticed
that the shear model doesn’t fully capture the actual system behavior. This
then results in an error in the planned desired configuration
$q^{\mathrm{d}}$, which the controller is not able to resolve because of the
underactuation of the robot (see Fig. 10(b)).

##  4 Experimental insights

In this work, we have shown effective, model-based regulation with planar HSA
robots. The conducted experiments gave us deep insights into the special
characteristics of HSA and how well our model is able to capture them. We see
excellent agreement for predicting the dynamical behavior of HSA robots made
of FPU material. For EPU-based HSA rvits, we observe that the model does not
fully capture the shear dynamics.

The good agreement of the model with the actual system behavior enables our
model-based controllers to perform very well at the task of setpoint
regulation. For the model-based controllers, any mismatch between the dynamic
model and the actual system (as analyzed in Section 3.3) has two impacts: (i)
the steady-state planning provides us with a desired configuration
$q^{\mathrm{d}}$ which the underactuated robot cannot achieve. This then, in
turn, causes a small steady-state error in the end-effector position as seen
for the manual setpoints in Fig. 6(a) and for the continuous references in
Fig.7. This steady-state error is not present in the baseline PID as its
integral term is acting directly in task space. We suggest for future work to
include an integral term directly on the end-effector position to remove the
remaining steady-state error of the model-based controller. Secondly, as (ii),
model errors will lead to an offset in the planned steady-state actuation
$\phi^{\mathrm{ss}}$. Therefore, applying a constant $\phi^{\mathrm{ss}}$ will
not move the robot exactly to $p_{\mathrm{ee}}^{\mathrm{d}}$. As shown in Fig.
6(d), the P-satI-D feedback term can make up for this effect through its
proportional and integral terms applied in the collocated variables.

##  5 Acknowledgements

The work by Maximilian Stölzle was supported under the European Union’s
Horizon Europe Program from Project EMERGE - Grant Agreement No. 101070918.
The authors thank Pietro Pustina for the valuable insights into a coordinate
transformation into collocated variables, control of underactuated soft
robots, and for his help in revising the manuscript.

## References

  * [1] Armanini, C., Hussain, I., Iqbal, M.Z., Gan, D., Prattichizzo, D., Renda, F.: Discrete cosserat approach for closed-chain soft robots: Application to the fin-ray finger. IEEE Transactions on Robotics 37(6), 2083–2098 (2021)
  * [2] Della Santina, C., Duriez, C., Rus, D.: Model-based control of soft robots: A survey of the state of the art and open challenges. IEEE Control Systems Magazine 43(3), 30–65 (2023)
  * [3] Garg, A., Good, I., Revier, D., Airis, K., Lipton, J.: Kinematic modeling of handed shearing auxetics via piecewise constant curvature. In: 2022 IEEE 5th International Conference on Soft Robotics (RoboSoft). pp. 423–430. IEEE (2022)
  * [4] Good, I., Brown-Moore, T., Patil, A., Revier, D., Lipton, J.I.: Expanding the design space for electrically-driven soft robots through handed shearing auxetics. In: 2022 International Conference on Robotics and Automation (ICRA). pp. 10951–10957. IEEE (2022)
  * [5] Hughes, J., Della Santina, C., Rus, D.: Extensible high force manipulator for complex exploration. In: 2020 3rd IEEE International Conference on Soft Robotics (RoboSoft). pp. 733–739. IEEE (2020)
  * [6] Kaarthik, P., Sanchez, F.L., Avtges, J., Truby, R.L.: Motorized, untethered soft robots via 3d printed auxetics. Soft Matter 18(43), 8229–8237 (2022)
  * [7] Khatib, O.: A unified approach for motion and force control of robot manipulators: The operational space formulation. IEEE Journal on Robotics and Automation 3(1), 43–53 (1987)
  * [8] Pustina, P., Borja, P., Della Santina, C., De Luca, A.: P-sati-d shape regulation of soft robots. IEEE Robotics and Automation Letters 8(1), 1–8 (2022)
  * [9] Pustina, P., Della Santina, C., Boyer, F., De Luca, A., Renda, F.: On the collocated form with input decoupling of lagrangian systems. arXiv preprint arXiv:2306.07258 (2023)
  * [10] Renda, F., Boyer, F., Dias, J., Seneviratne, L.: Discrete cosserat approach for multisection soft manipulator dynamics. IEEE Transactions on Robotics 34(6), 1518–1533 (2018)
  * [11] Stölzle, M., Chin, L., Truby, R.L., Rus, D., Della Santina, C.: Modelling handed shearing auxetics: Selective piecewise constant strain kinematics and dynamic simulation. In: 2023 IEEE International Conference on Soft Robotics (RoboSoft). pp. 1–8. IEEE (2023)
  * [12] Truby, R.L., Chin, L., Rus, D.: A recipe for electrically-driven soft robots via 3d printed handed shearing auxetics. IEEE Robotics and Automation Letters 6(2), 795–802 (2021)
  * [13] Tulleken, H.J.: Generalized binary noise test-signal concept for improved identification-experiment design. Automatica 26(1), 37–49 (1990)