# A geometric approach towards inverse kinematics of soft extensible pneumatic
actuators intended for trajectory tracking

Mahboubeh Keyvanara1,∗, Arman Goshtasbi2, and Irene A. Kuling1 1 Reshape Lab,
Department of Mechanical Engineering, Dynamics and Control Group, Eindhoven
University of Technology, 5600 MB Eindhoven, The Netherlands.2Soft Robotics
Laboratory, Department of Biomechanical Engineering, Engineering Technology
Faculty, University of Twente, 7522 NB, The Netherlands ∗Corressponding
author<EMAIL_ADDRESS>
###### Abstract

Soft robots are interesting examples of hyper-redundancy in robotics, however,
the nonlinear continuous dynamics of these robots and the use of hyper-elastic
and visco-elastic materials makes modeling of these robots more complicated.
This study presents a geometric Inverse Kinematic (IK) model for trajectory
tracking of multi-segment extensible soft robots, where, each segment of the
soft actuator is geometrically approximated with multiple rigid links
connected with rotary and prismatic joints. Using optimization methods, the
desired configuration variables of the soft actuator for the desired end-
effector positions are obtained. Also, the redundancy of the robot is applied
for second task applications, such as tip angle control. The model’s
performance is investigated through simulations, numerical benchmarks, and
experimental validations and results show lower computational costs and higher
accuracy compared to most existing methods. The method is easy to apply to
multi segment soft robots, both in 2D and 3D. As a case study, a fully
3D-printed soft robot manipulator is tested using a control unit and the model
predictions show good agreement with the experimental results.

###### Index Terms:

Soft Robots, Inverse Kinematics, Hyper-redundancy, Trajectory Tracking,
Piecewise Constant Curvature Model, Secondary Tasks

##  I Introduction

Solutions to the IK of soft robots have been studied using different
approaches. In analytical solutions, using the geometric and kinematics of the
robot, an absolute solution for the required DoF is found. This solution is
not trivial due to these robots’ nonlinear equations and hyper-redundancy. To
overcome this, different models are proposed, the most famous one being the
constant curvature (CC) approximation [1, 2, 3]. Using this model, authors in
[4] suggested an absolute geometrical solution for a single-segment
inextensible continuum arm, showing it to be the most suitable method for a
single-segment robot. Yet, for multi-segment soft robots, the combination of
piecewise constant curvature (PCC) model and analytical solutions can lead to
complex mathematics, high computation costs, and so many simplifications are
required [5]. For this reason, a less intricate yet accurate model is needed
to overcome the complexity of the analytical solution.

Figure 1: 3D trajectory tracking of soft robots. A block diagram representing
closed loop control of the robot on the desired trajectory. Parameters defined
in this schematic are later defined in the paper.

Numerical solutions are a more promising alternative for IK solutions where
state variables are calculated approximately using iterative or optimization
techniques. In [6], deformations of a soft robot under different actuation
loads are simulated using FEM. The main benefit of the FEM model is its
capability to solve the inverse kinematics of nonuniform shape robots and
torsional robots ([7] [8]), yet, the results are dependent very much on the
robot itself. This method is also computationally intensive for multi-link
systems and real-time control. Only recently in [9], using visual servoing,
the authors have performed closed-loop trajectory tracking of a soft robot
using FEM. In other numerical methods, such as curvature discretization, the
nonlinear behavior of soft robots is approximated by multiple rigid links. In
[10], authors suggest 16 rigid link approximation to model single segment
polymer actuator. In [11], a more complicated shape of soft robots is
approximated by rigid-link modeling. Here, the strain of the robot during
actuation has not been taken into account and only bending is modeled and a
solution for extensible soft robots is still missing. Other numerical methods,
namely heuristics inverse kinematics algorithms, are famous for low
computation costs and the possibility of solving large DoFs systems [12]. [13]
proposed a new cyclic coordinate descent (CCD) algorithm for soft and
redundant robots by solving the shortcoming of the standard CCD algorithms.
Unfortunately, this method is limited to simulations only and has not been
tested with real-time controllers and experimental setups.

Learning methods are also employed to overcome the complexity of the IK
problem. Mostly, these techniques are combined with other well-known IK
solutions to propose faster solutions [14]. For example, in [15], authors
combined the Jacobian inverse kinematics method with feed-forward neural
networks to control not only the multi-link robot but also optimize energy
usage. In learning approaches, similar to FEM approaches, inverse kinematics
can be solved for nonuniform robots. However, a physical experimental setup or
a sophisticated dynamic model is required for the data acquisition.

Leveraging on this literature and with the aim of proposing a fast and
accurate IK solution for extensible multi-segment 3D soft robots that can be
used easily in experimental validations, we propose a geometric inverse
kinematic model for 2D and 3D single and multi-segment extensible soft robots.
In the proposed method, each robot segment is modeled with a CC approximation
and discretized with multiple rigid links connected together with prismatic
and rotary joints. The developed method is mathematically simple, can easily
be applied to various types of soft actuators, and has negligible error with
respect to the workspace. Increasing the DoF of the robot by adding segments
to the robot is straightforward and does not significantly affect the method’s
performance. Using this geometric model, an optimization method is employed to
find a configuration for the soft robot, which will result in the end-effector
reaching the required position. The optimization method allows for working
with the boundary conditions using redundancy of the robot for a second task,
such as tip angle control and pressure control. One advantage of the proposed
IK model is its application diversity, as it is possible to employ this method
with any desired dynamic model and real-time control systems.

To express how this IK method can be integrated with dynamic models and can be
used for trajectory tracking in experimental setups, a model-based control
algorithm is required to assist with controlling the robot on a designed
trajectory. Different dynamic models have been presented for this reason [16,
17, 18, 19, 20, 21, 22]. In this research, as the IK method is presented for
the PCC model, a model-based control based on this modeling will be
chosen.Also, an experimental setup with a soft robot control unit has been
used to show the real-time applications of the models presented in this study.

To summarize, the contributions of this work include the following:

  1. 1.

A new fast and accurate IK model for trajectory tracking of extensible soft
robots that is easy to use and efficient for multi-segment soft robots.

  2. 2.

A new approach towards tip-angle control of soft robots on desired
trajectories.

  3. 3.

An overview of implementing the IK model for open-loop and real-time closed-
loop control via extensive simulations and experiments.

The organization of this work is as follow. After this Introduction, in
section (II) an overview of the IK method and also a description of the
dynamic model and the proposed controller are presented. Then, an overview of
the experimental setup which is used for validation of the models is
discussed. Conclusively, in section (III) different numerical simulation are
studied and results of the experiments are presented, followed by a brief
conclusion.

##  II Material and Methods

###  II-A Inverse Kinematics method

The complex dynamic behavior and geometries of soft robots make solving
inverse kinematics more challenging than traditional rigid robots. One
approach to reducing the complexity of soft robots is to discretize the robot
curvature with small rigid links. This model is known as the rigid-link model
and is mostly used in modeling human body motion and compliant mechanisms
([23][24]). The rigid links here are inextensible and connected via prismatic
and rotary joints to cover both extension and rotation in dynamic behavior
(Fig2). Applying this method to the soft robots makes it possible to implement
previously developed methods for rigid robotics into modeling soft robots.

Moreover, since in many soft robots, the shape is uniform without torsional
motion, the constant curvature approximation is valid. Therefore, to further
simplify the dynamic behavior, the robot can be divided into multiple segments
using PCC approximation and combined with the rigid-link model for a more
straightforward approximation. In this approach, each robot segment is
represented by multiple tiny rigid links connected to each other, forming part
of a regular polygon. For extensible robots, the joints between the links are
both rotary joints to emulate the bending of the actuator and prismatic joints
to demonstrate the change in length of the robot (Fig.2). One primary
assumption is that all rotary and prismatic joints move uniformly for each
segment. Hence, the overall change in the length of each segment would be the
deformation of each prismatic joint multiplied by the number of rigid links,
and the same applies to the curvature.

Figure 2: Combination of PCC and rigid-link model for the multi-segment soft
robot and the respective configuration variables. The configuration before
(left) and after actuation (right) is shown. Each PCC curvature is represented
with two rigid links presented in dark blue connected to each other with
rotary (circles), and prismatic (light blue) joints. Here, n is the number of
links in each segment, L is the initial length of the segment, $\Delta L$ is
the extension of each segment, $\theta$ is the bending angle, and $\phi$ is
the deflection angle from X-axis.

Although replacing the curvatures with multiple links reduces the complexity
of the kinematic equation, it is not possible for the multi-segment robots to
solve the problem analytically since the number of variables exceeds the
number of kinematic equations. Thus, a numerical solution can be employed.
From various numerical methods, we decided to use the optimization approach.
In this method, the forward kinematics of the 3D robot is derived from the PCC
and rigid-link model (Eq.(1)).

$\displaystyle\begin{bmatrix}X_{E_{i}}\\\ Y_{E_{i}}\\\ Z_{E_{i}}\\\
\end{bmatrix}=\begin{bmatrix}X_{E_{i-1}}\\\ Y_{E_{i-1}}\\\ Z_{E_{i-1}}\\\
\end{bmatrix}+\frac{(L_{i}+\Delta
L_{i})}{n}R_{z}(\phi_{i-1})R_{y}(\theta_{i-1})$ (1)
$\displaystyle\begin{bmatrix}cos(\phi_{i})\sum_{j=1}^{n}sin(\frac{2j-1}{2n}\theta_{i})\\\
sin(\phi_{i})\sum_{j=1}^{n}sin(\frac{2j-1}{2n}\theta_{i})\\\
\sum_{j=1}^{n}cos(\frac{2j-1}{2n}\theta_{i})\\\ \end{bmatrix}$

Where $X_{E_{i}}$, $Y_{E_{i}}$, and $Z_{E_{i}}$ are the end-effector positions
of the $i^{th}$ segment, $L_{i}$ is the length of the segment, $\Delta L_{i}$
is the change of length of the segment, $\theta_{i}$ is the bending angle of
the segment, $\phi_{i}$ is the deflection angle from the X-axis, $R_{y}$ and
$R_{z}$ are the rotation matrix around Y-axis and Z-axis respectively, and $n$
is the number of rigid links in each segment. It is worth mentioning that,
although the IK method is presented for a 3D case, Eq.(1) is also applicable
for 2D systems. The only difference is the deflection angle ($\phi$) being
zero in the 2D system. After obtaining the forward kinematics, an optimization
method is applied to find parameters that minimize the error between the
desired point,$[X_{d}\,\,Y_{d}\,\,Z_{d}]$, and the end-effector point derived
from the forward kinematics, [$X_{E_{f}}$ $Y_{E_{f}}$ $Z_{E_{f}}$], as in
Eq.(2). The optimization approach is not limited to just the PCC model and can
be applied to other soft robotics models.

$\displaystyle\min_{\Delta L,\theta,\phi}$
$\displaystyle\lVert\begin{pmatrix}X_{d}-X_{E_{f}},Y_{d}-Y_{E_{f}},Z_{d}-Z_{E_{f}}\end{pmatrix}\rVert_{2}$
(2) s.t. $\displaystyle\Delta L_{i_{min}}<\Delta L_{i}<\Delta L_{i_{max}}$
$\displaystyle\theta_{i_{min}}<\theta_{i}<\theta_{i_{max}}$
$\displaystyle-\pi<\phi_{i}<\pi$

The optimization method applied here is MATLAB fmincon function which uses the
Sequential Quadratic Programming algorithm(SQP). Since the SQP algorithm can
not find the global optimized solution, the initial conditions and constraints
are essential factors in finding the most optimal solution for the inverse
kinematics. We set the initial conditions for optimization as the
configuration variables of the robots at the previous posture to have a more
smooth and energy-efficient solution over the trajectory and avoid abrupt
changes in robot posture. In addition, since robots do not have unlimited
changes in length or bend, physical constraints are added to the optimization
equation to have a more realistic solution according to the robots’
capability. Eq.(2) includes these physical constraints, which are the
extension and bending limitations of the robot. Since the method is designed,
so the configuration variables have minimum change between different points of
the trajectory, the change of energy throughout the trajectory and hence, the
overall energy consumption is minimum.

In the case of a multi-segment robot, since the configuration variables are
more than the kinematic equations, there are multiple possible solutions for
each desired position. These multiple solutions give the robot redundancy,
which is beneficial for a secondary task such as tip angle control. The
secondary task can be included in the inverse kinematics solution by adding
additional constraints. This additional constraint for the tip angle control
of the 2D robot is presented in Eq.(3).

$\theta_{d}-\sum_{i}^{n}\theta_{i}=0$ (3)

Where $\theta_{d}$ is the desired angle and the $\theta_{i}$ is the bending
angle of each soft segment. This additional constraint, the IK model benefits
from the redundancy to not only find parameters that follow the trajectory but
also calculate the bending angle of each segment in a way that keeps the tip
angle at a certain angle. However, the priority of the IK model is to follow
the trajectory and, if possible, achieve the second task. Using the boundary
condition of the method enables us to add the secondary task. However, this
method limits the IK model to satisfy the secondary task. Therefore, we
manipulate the program in a way that the inverse kinematics solution is the
priority, and if possible, it follows the second task, which is tip angle
control. Furthermore, for other secondary tasks such as pressure optimization,
or obstacle avoidance, using the boundary conditions is still applicable since
we need both inverse kinematics and obstacle avoidance and pressure
optimization to happen simultaneously. To the best of the authors’ knowledge,
adding second-priority tasks to the IK solution of soft robots has yet to be
studied in previous studies.

####  II-A1 Error in Inverse kinematics method

Despite the method being straightforward to implement, one of its main
limitations is the calculated length of the robot. As each curvature is
modeled with multiple straight lines, the calculated length is smaller than
the actual value. The first solution to overcome the drift is increasing the
number of rigid links so that the error between the actual and the calculated
length limits toward zero. However, the computation would be more expensive
with this method, and calculation time would increase significantly. In
addition, in very high DoFs robots, this method may even affect the
optimization solution and make the robot’s motion unstable over a trajectory.
The other solution to overcome this drift is a geometrical approach. The drift
can be compensated by using the cosine law and geometrical equations to find
the ratio between the actual length and the calculated length (Eq4). This
equation is implemented at the end of each iteration of the inverse kinematics
solution to compensate for the actual and calculated value difference.

$\frac{L}{\sum_{j=1}^{n}L^{{}^{\prime}}}=\frac{\theta}{n\sqrt{2-2cos{\frac{\theta}{n}}}}$
(4)

Where $L^{{}^{\prime}}$ is the length of each segment, $L$ is the length of
the curvature, $n$ is the number of segments, and $\theta$ is the bending
angle. For example, using Eq4, for a $90^{\circ}$ of bending angle, at least
ten links are required to have an error length of $0.1\%$. However, having
this many links in the IK model, especially for multi-segment robots,
significantly increases the computational costs. Therefore, as shown in III-A,
using Eq4 helps to have an accurate solution even with just a few links
representing the curvature.

###  II-B Dynamics and Control

For the IK model to be studied, different trajectories will be applied to a
three-bellowed soft robot, this will show the integration of the IK model with
dynamic models. A model-based controller is employed to control the robot on
the desired trajectory. In this section, the dynamics and also the controller
applied to the robot are presented.

For the dynamics of the PCC model, a bishop frame is attached to every point
on the backbone curve (in SE(3)), which is parameterized by a state vector
$q=[\epsilon\,\,k_{x}\,\,k_{y}]^{T}\in R^{3}$. Here, $\epsilon$ is the strain
of the robot , $k_{x}$ and $k_{y}$ are the curvatures of the robot in the x-z
and y-z plane, respectively. The relation between the newly defined state
variables and the configuration variables defined in the IK model (Fig.(2)) is
as follows:

$\displaystyle\epsilon=(l-l_{0})/l_{0}$ (5a) $\displaystyle
k_{x}=cos(\phi)\times\theta/l$ (5b) $\displaystyle
k_{y}=sin(\phi)\times\theta/l$ (5c)

The position of each point on the backbone curve is defined as:

$\displaystyle p(q,\sigma)=\int_{0}^{\sigma}\Phi(q,\eta)U(q)d\eta$ (6)

Where $\Phi(q,\eta)$ and $U$ describe the differential geometry of the
backbone curve. Thus, the equation of motion of the soft robot can be
expressed as:

$\displaystyle M(q)\ddot{q}+C(q,\dot{q})\dot{q}+G(q)+K(q)+D(\dot{q})=\tau$ (7)

Where, $M(q)\in{\rm I\\!R}^{n\times n}$ is the inertia matrix,
$C(q,\dot{q})\dot{q}\in{\rm I\\!R}^{n}$ contains the Coriolis and centrifugal
forces, and $G(q)\in{\rm I\\!R}^{n}$ is the gravitational forces acting on the
robot. As mentioned previously, there exists a rich literature on dynamic
modeling of PCC models [25, 26, 27]. In this study, a dynamic model is needed
that can be used for real-time control of multi-link soft robots and can
perform efficiently and accurately. So, the model presented in [28] is
employed in this study. In Eq.(7), two matrices of $K(q)$ and $D(\dot{q})$
define the hyper-elastic and visco-elastic properties of the robot. The hyper-
elastic potential energy is defined as:

$\displaystyle U_{e}(q)=\int k_{e}(\eta)\eta d\eta+\int k_{b}(\eta)\eta
d\eta,$ (8)

where $k_{e}(\eta)$ and $k_{b}(\eta)$ are elongation and bending stiffness.
The elongation stiffness is defined as
$k_{e}(1)=\alpha_{1}+\alpha_{2}(tanh[\alpha_{3}\epsilon]^{2}-1)$. Also, due to
the layout of the pneumatic bellows of the soft actuator, the bending
stiffness is asymmetric and defined as
$k_{b}(1)=\alpha_{\phi}(q).[\alpha_{4}+\alpha_{5}(tanh[\alpha_{6}\beta]^{2}-1)]$,
where, $\alpha_{\phi}(q)=1/2\beta[sin(m.\phi)+1]+1$. The overall stiffness
matrix is presented as:

$\displaystyle K(q)=\frac{\text{d}U_{e}}{\text{d}q}$ (9)

Also, the Rayleigh damping matrix is defined as $R$, and:

$\displaystyle D(\dot{q})=R\dot{q}$ (10)

In this modeling, the generalized input vector is chosen to be $\tau(u)=Hu$
with $H$ being a mapping from input space to joint actuation space. Using
[28], $H$ is chosen as in Eq.(11), where $\gamma_{i}=(i-1).2\pi/m$ and $m$ is
the number of pneumatic bellows. Here $\alpha_{1}>0$ and $\alpha_{2}>0$ have
to be identified using experimental data.

$\displaystyle H=\begin{bmatrix}\alpha_{1}&\alpha_{1}&\alpha_{1}\\\
-\alpha_{2}cos(\gamma_{1})&-\alpha_{2}cos(\gamma_{2})&-\alpha_{2}cos(\gamma_{3})\\\
\alpha_{2}cos(\gamma_{1})&\alpha_{2}cos(\gamma_{2})&\alpha_{2}cos(\gamma_{3})\\\
\end{bmatrix}$ (11)

The aim of this research is to control the soft manipulator to follow a
desired trajectory in the Cartesian space. Using the inverse kinematics, the
equivalent goal would be for the robot to reach a desired posture in the state
space, which theoretically means $\lim_{t\to\infty}q=q_{d}$. It is worth
emphasizing that since the IK method is independent of the controller, any
controller that works in the state space can be used with this IK method,
which gives this method a priority over learning methods. As mentioned
previously in Eq.(7), the two matrices of $K(q)$ and $D(\dot{q})$ exhibit the
hyper-elastic and visco-elastic properties of the system. Due to this property
of soft robots, as discussed in [29], soft robots have a self-stabilizing
feature that proves to be advantageous for controlling purposes. With this
analysis, the controller used for trajectory tracking is:

$\displaystyle\tau$ $\displaystyle=\ddot{q}+K(q_{d})+G(q_{d})$ (12)
$\displaystyle where,\ddot{q}$
$\displaystyle=\ddot{q_{d}}-k_{v}(\dot{q_{d}}-\dot{q})-k_{p}(q_{d}-q)$

Where two terms of $K(q_{d})$, the stiffness matrix, and $G(q_{d})$, the
gravity matrix, are the feedforward terms and $\ddot{q}$ is chosen, so the
feedback controller is a PD controller. Here, $q_{d}$ is the desired
trajectory in the state space which has been designed using the inverse
kinematics model. Also, $\ddot{q_{d}}=0$, and as the trajectory tracking is
assumed to be slow, $\dot{q_{d}}=0$. The graphical interpretation of this
controller is pictured in Fig.(1).

Figure 3: The inverse kinematics model results for a single-segment robot over
a given trajectory divided into 1000 points. (A) the robot configuration and
calculated end-effector position using IK model (B), The calculated parameters
at different points of the trajectory ($\theta$, $\phi$, $L$) using the IK
model and analytical solution, and the error between the desired position and
the IK model over the trajectory. (C) The difference between the length before
and after compensate for the drift using Eq4. (D) The computation cost of
solving the inverse kinematics versus the number of links in each segment. By
decreasing the number of links, the computation cost decreases, but the length
error increases significantly.

##  III Results and Discussions

Using the proposed methods in sec.(II), a set of simulations and experiments
that can demonstrate how a soft manipulator can use the IK model to track a
trajectory is presented.

###  III-A Single-Segment Soft Robot

In the first step, the proposed inverse kinematics solution is applied to a
single-segment 3D soft robot that follows a four-sided flower trajectory.
Since an analytical solution is possible in this case, the obtained results
can be compared with analytical parameters for validation. Here, the
analytical solution is from [30]. As presented in Fig.(3-A), the IK model
follows the desired trajectory with an average error of $8.41\times 10^{-6}mm$
which is less than 0.001$\%$ of the workspace. It can also be detected that
the calculated bending and deflection angles are identical for both analytical
and presented IK models. The only difference between these two solutions is
the length of the robot. This difference was expected as the actual curvature
is longer than the rigid-link approximation. As explained in sectionII-A1,
there are two solutions to overcome this error, either increasing the number
of links in each segment or applying geometry compensation. However, as
presented in Fig.(3-D), increasing the number of links in each segment
increases the computation cost significantly. Therefore, in order to
compensate for this drift, a coefficient dependent on the angle and the number
of segments is multiplied by the calculated length. The computation cost for
six links and 1150 points over the given trajectory is 3.07 seconds. In
addition, the average computation cost for one point on the trajectory is just
0.28 ms which is less than reported for kinematics in similar studies such as
[31]. As shown in Fig.(3-C), the coefficient removes the drift between the
actual and calculated length. With this drift compensation, the parameters
obtained from the IK model are identical to those from the analytical
solution, showing how accurately the model tracks a trajectory.

###  III-B Multi-Segment Soft Robot

Unlike for the single-segment robot, the analytical solution is not possible
for multi-segment soft robots since the number of DoFs exceeds the number of
kinematic equations; thus, the calculated parameters can not be compared with
a particular result. Here, we study a two-segment soft robot that is going to
track a 3D trajectory as shown in Fig.(4). As can be seen, the average error
between the end-effector and the desired position over the given trajectory is
$7.78\times 10^{-5}mm$, and this is negligible and lower than the reported
error in similar prior work, [15],[5]. In addition, as shown in the calculated
parameters, Fig.(4-B), there are no abrupt changes in the change of length,
the bending angles, or the deflection angle, which leads to the smooth motion
of the robot over the given trajectory.

Using the additional constraint defined in Eq.(3), it is also possible to
design trajectories for a robot with a constant tip angle. To further study
this, a 4-segment 2D soft robot is chosen to follow the desired trajectory of
$X_{d}=175+50cos(t)$ and $Y_{d}=100+50sin(t)$. This trajectory is given as
input to the IK code. The IK method is employed without the additional tip
angle constraint in Fig.(5-A). It can be seen that the state variables are
calculated at every step so that the change of these variables with respect to
the previous step on the trajectory is minimum, and hence, energy consumption
is minimum. This is a key factor in generating smooth trajectories without
unwanted jumps in the robot’s state variables. Since, in this example, the
robot has redundancy with respect to the task space, except for following the
desired trajectory, it is possible to add tip angle constraints on the robot
with lower priority. As explained in section(II-A), an extra constraint is
added to the robot’s trajectory with the aim of having a constant tip angle
throughout the trajectory tracking. Fig.(5-B) depicts how different the robot
moves as it is constrained to keep the tip angle constant.

It has been shown that the IK model is able to generate smooth 2D and 3D
results even on complex trajectories. The next step is to demonstrate how it
can be used as an input to dynamic models. To be able to control the robot on
desired trajectories, the physical and material parameters of the robot need
to be identified. The soft manipulator used in this research has the following
physical parameters: the mass $m_{0}=17.3g$, and the relaxation length of
$l_{0}=64.4mm$. Considering preliminary uni-axial tension tests, the
3D-printed elastomer material is estimated to be linear isotropic with Young’s
modulus of $E=80$ MPa and a Poisson ration $\nu=0.49$. Considering the
geometry and material of the soft robot, the hyper-elastic and visco-elastic
material parameters and the Rayleigh damping matrix are chosen similarly to
[28] (their Table 1), and no further identification is required.

Figure 4: The inverse kinematics model results for a two-segmented robot, the
first segment is shown in blue and second segment in black, (A) the robot
configuration and calculated end-effector position using IK model, (B) The
calculated parameters ($\theta$, $\phi$, $L$) of both segments using IK model,
and the error between the desired position and the IK model solution. The
average error in this case is $1.95\times 10^{-5}\%$ of the workspace. (C) The
computation time of the inverse kinematics versus the number of links in each
segment.

For the first simulation, the circular trajectory defined as the desired
trajectory for the 4-segment 2D soft robot is chosen (Fig.(5)). Here, using
the IK model, the desired state variables for each robot segment are
calculated, and using the model-based controller, the robot is controlled on
the desired trajectory. To control the robot, a combination of feedforward and
feedback control, as explained in section(II-B) has been employed. For the
sake of brevity, only the results of the controller applied to only the
trajectory of the Fig.(5-B) have been reported in Fig.(5-C and D). The PD
controller used in this case is formulated as
$\tau=B^{-1}[M(\ddot{q}-k_{v}(\dot{q_{d}}-\dot{q})-k_{p}(q_{d}-q))+K(q_{d})+G(q_{d})]$
with $k_{p}=1000\times I_{8}$ with $k_{v}=5\times I_{8}$. The robot starts
tracking the trajectory from the initial state of zero, and as in Fig.(5-D),
the norm of error of following this trajectory is less than $2.5\%$ which is
acceptable. The results presented in Fig.(5) can verify that the results of IK
are very smooth and even controlling a multi-segment soft robot on a desired
trajectory with a second desired task does not have any abrupt changes. This
is an advantage of the presented method compared to some existing ones, such
as [15]. Also, the controller used in this example is independent of the IK
model, which is an advantage to the proposed IK model.

Figure 5: Comparison of two different IK solutions on the same trajectory for
a four segment planar soft robot, (A) the robot only has the task of following
the desired trajectory (B) the robot has a second task of keeping the tip
angle a constant value of $30^{o}$ with respect to the horizon. (C) simulation
results of the robot being controlled on the desired trajectory pictured in
(B), grey line is the desired trajectory and the pink dashed line is the
trajectory followed by the robot. Initial state of the robot is
$q_{0}=[0,0,0,0,0,0,0,0]$, (D) the norm of error between the desired
trajectory and the position of the end-effector

###  III-C Experiments

An experimental setup, including a soft robot and a hardware system to control
the robot, is demanded to show how the IK model and designed controllers can
be implemented on a soft robot. To this end, a 3D printed soft manipulator
consisting of 3 parallel embedded pneumatic bellows is used; this robot is
pictured in Fig(1). By inflating or deflating each bellow, the manipulator can
change its posture. If all the bellows are actuated simultaneously, the robot
changes length, and with unequal pressurization of the bellows, the robot can
create a pose that forms a curve with constant curvature. Hence, this robotic
system is suitable for experiments in this research case, as it can represent
a PCC model. Furthermore, since this type of robot has already been used in
[28], the dynamic model presented in section(II-B) is also compatible with
this experimental setup.

A Soft Robotic Control unit (SRC), presented in [32], has been used to create
fast, accurate model-based control. In this control unit, the control and data
acquisition is done using a Raspberry Pi 4 (2GB RAM). A proportional piezo-
actuated pressure regulator (Festo, VEAB-L-26-D7-Q4-V1-1R1) with a custom
Raspberry Pi Hat simultaneously allows pressure measurements. The main
software of this setup is designed to read multiple sensors, simultaneously
control several VEAB regulators, and communicate with other devices via
TCP/IP. It is important to briefly emphasize that due to large continuous
deformations and potential changes in dynamics, sensing and adding sensors is
known to be quite challenging in soft robotics. For this reason, to record the
angular deflections of the soft robot, a 9-DOF inertial measurement unit
(Bosch, BNO055) is attached to the robot’s end-effector. The information from
the IMU sensors is read directly from the SRC software and can be used
throughout the real-time control of the soft robot. In this control setup, the
sensor is connected via $I^{2}C$ to the SRC. In order to read the Cartesian
movements of different points on the backbone curve of the soft robot, an
OpenMV H7 R2 Camera is employed. Using colored markers and color detection
methods, the position of the markers on the backbone is recorded, and with
that, the length of the robot is determined. Hence, all state variables of the
robot can be known throughout the experiments. In this setup, the regulators
have a 100-150 ms delay, and the control rate can reach a max of 300-400 Hz.

To start the experiments, first, the parameters for the mapping matrix defined
in Eq.(11) are identified. For this reason, different experiments are run, and
in each experiment, a set of quasi-static pressure is added to the robot. With
each set of pressure, the robot reaches a final point in the cartesian space.
This point can be identified using the data from the OpenMv camera
($[X,Y,Z]_{markers}$) and IMU sensors ($[roll,pitch,yaw]$). So one can
identify the desired state variable of the dynamic model. These state
variables are fed into the model, and the required input pressure for the
robot is calculated via simulations, which are then compared with the pressure
input of the experiments. With this comparison, the parameters are identified
for the mapping matrix of Eq.(11), as $\alpha_{1}=8\times 10^{-4}$ and
$\alpha_{2}=7.91\times 10^{-7}$. It is worth mentioning that since in these
experiments we are using the IMU sensors, only one position data from the
cameras is sufficient to find the state variable, here we use the
$Z_{markers}$.

Having a bridge between the model and the experimental setup, different
trajectories in experiments are achievable, and this can be used for further
validation of the model. First, an open loop controller is employed to control
the robot on the desired trajectory presented in Fig.(3). Here, the trajectory
is added to the dynamic model, and the desired pressures calculated from the
simulations of the closed-loop controllers are added to the robot to perform
an open-loop control. The top view of the final trajectory tracking of the
robot is presented in Fig.(6). The error between the desired trajectory and
the following one is less than 3 mm, which is acceptable considering the
trajectory’s complexity for a three-bellowed robot and the fact that we are
using an open-loop controller. In this experiment, as shown in Fig.(3), the
robot experiences a change of length, and this shows that the IK model is also
valid for trajectories with the change of length.

Figure 6: Experiment 1 - (A) Top view of the control of the robot on the
designed trajectory of Fig.(4). Dashed pink plot is the desired trajectory,
the grey plots are different experiments, and the continuous blue plot is the
average of the followed trajectory by the robot. (B) The norm of the error
between the followed trajectory by the tip point and the desired one. Video of
the experiment available at: https://youtu.be/1kQ8FbXpwf0

In the second experiment, a closed-loop controller is designed to keep the
robot’s end-effector on the trajectory of $X_{d}=20cos(6t)$,
$Y_{d}=20sin(6t)$, $Z_{d}=60$. For both experiments, the initial condition is
zero, $q_{0}=[0,0,0]$, and the control parameters are chosen as
$k_{p}=0.16\times I_{3}$ and $k_{v}\approx 0\times I_{3}$.

In every step of the real-time control, first, the control of the robot is
simulated using a feedback controller presented in Eq.(12). At this step, we
have a set of three pressures that indicate how much each bellow needs to be
pressurized to follow the desired trajectory. These values are fed directly
into the robot using pressure regulators. As the robot is pressurized, the IMU
sensors and camera data send feedback to the model to help calculate the error
with respect to the desired state variables. From there, a new set of the
pressure signal is calculated. The designed controller in this study is
programmed in MATLAB/Simulink and communicates via TCP/IP with the SCR unit at
250 Hz. As the experiments run, it can be verified that the robot is able to
follow the desired trajectory. Fig.(7-A) pictures the designed values for the
pressure of each bellow and compares it with the pressure produced by the
regulators in each bellow. It depicts how these two values overlap after the
initial transient state is finished, meaning the experimental setup can
generate the pressure inputs very close to the designed pressure values, which
helps the trajectory tracking for the soft robot. Also, using the data from
the camera and the IMU sensors, it is possible to study the state variables of
the robot as it is tracking the trajectory. Fig(7-B) compares these state
variables between the designed values from the IK solution, the model
simulations, and the controlled values in the experiments. With this figure,
it is also verified that trajectory tracking can be achieved with the
combination of the IK model, the dynamic model, and the experimental setup.

Figure 7: Experiment 2 - (A) Pressure values, dashed lines are the designed
values and blue lines are the regulator measurements (B) State variables of
the robot in two different planes indicating state variables of the dynamic
model. The grey plots are the results of IK model which are the desired
trajectory, the dashed lines show the simulations results of the controller,
and the blue lines show the movements of the robot in experiments.

##  IV Conclusions

This research aims to propose an inverse kinematic model for soft extensible
actuators that can be modeled with a piece-wise constant curvature approach,
whether 2D or 3D, single-segment or multi-segment. In this method, each
segment of the soft actuator was modeled with multi-rigid links that are
connected through rotatory and prismatic joints. To have lower computational
costs, we proposed a minimum amount for the number of rigid links without
changing the accuracy of the results. The approach uses a simple procedure to
minimize the error between the desired position of the end-effector and the
forward kinematics equations. In this energy-efficient method, adding a second
desired task, such as tip angle control, is possible using additional
constraints. The presented method is verified through simulations and
different experimental results, which have validated the method’s
applicability to different trajectories. For future work, we aim to focus on
achieving a more comprehensive range of secondary tasks within the algorithm,
including tasks such as obstacle avoidance. Also, we aim to use other
optimization methods, such as a generic algorithm, to find the globally
optimized points and have much higher precision. One main advantage of the
presented method is the independence of the controller applied to the robot.
Regardless of the type of controller and the dynamic model, the IK model has a
low computational cost and high accuracy for trajectory tracking.

## References

  * [1] M. W. Hannan and I. D. Walker, “Kinematics and the implementation of an elephant’s trunk manipulator and other continuum style robots,” _Journal of Robotic Systems_ , vol. 20, no. 2, pp. 45–63, 2003. [Online]. Available: https://onlinelibrary-wiley-com.ezproxy2.utwente.nl/doi/abs/10.1002/rob.10070
  * [2] B. Jones and I. Walker, “Kinematics for multisection continuum robots,” _IEEE Transactions on Robotics_ , vol. 22, no. 1, pp. 43–55, 2006.
  * [3] I. Robert J. Webster and B. A. Jones, “Design and kinematic modeling of constant curvature continuum robots: A review,” _The International Journal of Robotics Research_ , vol. 29, no. 13, pp. 1661–1683, 2010. [Online]. Available: https://doi.org/10.1177/0278364910368147
  * [4] A. A. Nazari, D. Castro, and I. S. Godage, “Forward and inverse kinematics of a single section inextensible continuum arm,” _CoRR_ , vol. abs/1907.06518, 2019. [Online]. Available: http://arxiv.org/abs/1907.06518
  * [5] Z. Gong, J. Cheng, K. Hu, T. Wang, and L. Wen, “An inverse kinematics method of a soft robotic arm with three-dimensional locomotion for underwater manipulation,” in _2018 IEEE International Conference on Soft Robotics (RoboSoft)_ , 2018, pp. 516–521.
  * [6] K.-H. Lee, M. C. W. Leong, M. C. K. Chow, H.-C. Fu, W. Luk, K.-Y. Sze, C.-K. Yeung, and K.-W. Kwok, “Fem-based soft robotic control framework for intracavitary navigation,” in _2017 IEEE International Conference on Real-time Computing and Robotics (RCAR)_ , 2017, pp. 11–16.
  * [7] G. Fang, C.-D. Matte, R. B. N. Scharff, T.-H. Kwok, and C. C. L. Wang, “Kinematics of soft robots by geometric computing,” _IEEE Transactions on Robotics_ , vol. 36, no. 4, pp. 1272–1286, 2020.
  * [8] J. M. Bern and D. Rus, “Soft ik with stiffness control,” in _2021 IEEE 4th International Conference on Soft Robotics (RoboSoft)_ , 2021, pp. 465–471.
  * [9] Z. Zhang, T. M. Bieze, J. Dequidt, A. Kruszewski, and C. Duriez, “Visual servoing control of soft robots based on finite element model,” in _2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ , 2017, pp. 2895–2901.
  * [10] R. Mutlu, G. Alici, and W. Li, “An effective methodology to solve inverse kinematics of electroactive polymer actuators modelled as active and soft robotic structures,” _Mechanism and Machine Theory_ , vol. 67, pp. 94–110, 2013.
  * [11] R. J. Roesthuis and S. Misra, “Steering of multisegment continuum manipulators using rigid-link modeling and fbg-based shape sensing,” _IEEE Transactions on Robotics_ , vol. 32, no. 2, pp. 372–382, 2016.
  * [12] R. Mukundan, “A fast inverse kinematics solution for an n-link joint chain,” 2008\.
  * [13] A. Martín, A. Barrientos, and J. del Cerro, “The natural-ccd algorithm, a novel method to solve the inverse kinematics of hyper-redundant and soft robots,” _Soft Robotics_ , vol. 5, no. 3, pp. 242–257, 2018, pMID: 29565775\. [Online]. Available: https://doi.org/10.1089/soro.2017.0009
  * [14] G. Fang, X. Wang, K. Wang, K.-H. Lee, J. D. L. Ho, H.-C. Fu, D. K. C. Fu, and K.-W. Kwok, “Vision-based online learning kinematic control for soft robots using local gaussian process regression,” _IEEE Robotics and Automation Letters_ , vol. 4, no. 2, pp. 1194–1201, 2019.
  * [15] G. Fang, Y. Tian, Z.-X. Yang, J. M. P. Geraedts, and C. C. L. Wang, “Efficient jacobian-based inverse kinematics of soft robots by learning,” 2021.
  * [16] S. Kriegman, D. Blackiston, M. Levin, and J. Bongard, “A scalable pipeline for designing reconfigurable organisms,” _Proceedings of the National Academy of Sciences_ , vol. 117, no. 4, pp. 1853–1859, 2020.
  * [17] Z. Li, L. Wu, H. Ren, and H. Yu, “Kinematic comparison of surgical tendon-driven manipulators and concentric tube manipulators,” _Mechanism and machine theory_ , vol. 107, pp. 148–165, 2017.
  * [18] R. Katzschmann, C. Della Santina, Y. Toshimitsu, A. Bicchi, and D. Rus, “Dynamic motion control of multi-segment soft robots using piecewise constant curvature matched with an augmented rigid body model,” 02 2019, pp. 454–461.
  * [19] G. S. Chirikjian and J. W. Burdick, “A modal approach to hyper-redundant manipulator kinematics,” _IEEE Transactions on Robotics and Automation_ , vol. 10, no. 3, pp. 343–354, 1994.
  * [20] D. Bruder, B. Gillespie, C. D. Remy, and R. Vasudevan, “Modeling and control of soft robots using the koopman operator and model predictive control,” _arXiv preprint arXiv:1902.02827_ , 2019.
  * [21] I. S. Godage, G. A. Medrano-Cerda, D. T. Branson, E. Guglielmino, and D. G. Caldwell, “Modal kinematics for multisection continuum arms,” _Bioinspiration & biomimetics_, vol. 10, no. 3, p. 035002, 2015.
  * [22] F. Boyer, V. Lebastard, F. Candelier, and F. Renda, “Dynamics of continuum and soft robots: A strain parameterization based approach,” _IEEE Transactions on Robotics_ , vol. 37, no. 3, pp. 847–863, 2020.
  * [23] R. Pàmies-Vilà, J. M. Font-Llagunes, J. Cuadrado, and F. J. Alonso, “Analysis of different uncertainties in the inverse dynamic analysis of human gait,” _Mechanism and Machine Theory_ , vol. 58, pp. 153–164, 2012\. [Online]. Available: https://www.sciencedirect.com/science/article/pii/S0094114X12001528
  * [24] P. Liu and P. Yan, “a modified pseudo-rigid-body modeling approach for compliant mechanisms with fixed-guided beam flexures,” _Mechanical Sciences_ , vol. 8, no. 2, pp. 359–368, 2017. [Online]. Available: https://ms.copernicus.org/articles/8/359/2017/
  * [25] R. K. Katzschmann, C. D. Santina, Y. Toshimitsu, A. Bicchi, and D. Rus, “Dynamic motion control of multi-segment soft robots using piecewise constant curvature matched with an augmented rigid body model,” in _2019 2nd IEEE International Conference on Soft Robotics (RoboSoft)_ , 2019, pp. 454–461.
  * [26] C. D. Santina, R. K. Katzschmann, A. Bicchi, and D. Rus, “Model-based dynamic feedback control of a planar soft robot: trajectory tracking and interaction with the environment,” _The International Journal of Robotics Research_ , vol. 39, no. 4, pp. 490–513, 2020. [Online]. Available: https://doi.org/10.1177/0278364919897292
  * [27] A. D. Marchese and D. Rus, “Design, kinematics, and control of a soft spatial fluidic elastomer manipulator,” _The International Journal of Robotics Research_ , vol. 35, no. 7, pp. 840–869, 2016. [Online]. Available: https://doi.org/10.1177/0278364915587925
  * [28] B. Caasenbrood, A. Pogromsky, and H. Nijmeijer, “Control-oriented models for hyperelastic soft robots through differential geometry of curves,” _Soft Robotics_ , vol. 0, no. 0, p. null, 0, pMID: 35748646. [Online]. Available: https://doi.org/10.1089/soro.2021.0035
  * [29] 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,” _arXiv preprint arXiv:2110.01358_ , 2021.
  * [30] A. Garriga-Casanovas and F. Rodriguez y Baena, “Kinematics of Continuum Robots With Constant Curvature Bending and Extension Capabilities,” _Journal of Mechanisms and Robotics_ , vol. 11, no. 1, 11 2018, 011010. [Online]. Available: https://doi.org/10.1115/1.4041739
  * [31] X. Huang, J. Zou, and G. Gu, “Kinematic modeling and control of variable curvature soft continuum robots,” _IEEE/ASME Transactions on Mechatronics_ , vol. 26, no. 6, pp. 3175–3185, 2021.
  * [32] B. J. Caasenbrood, F. E. van Beek, H. K. Chu, and I. A. Kuling, “A desktop-sized platform for real-time control applications of pneumatic soft robots,” in _2022 IEEE 5th International Conference on Soft Robotics (RoboSoft)_. IEEE, 2022, pp. 217–223.