# Constrained Motion Planning of A Cable-Driven Soft Robot With Compressible
Curvature Modeling

Jiewen Lai, Bo Lu, Qingxiang Zhao, and Henry K. Chu This work was supported in
part by the Departmental Grant 4-RKCM of ME-HKPolyU. (Corresponding author:
Henry K. Chu.)J. Lai, Q. Zhao, and H. K. Chu are with the Department of
Mechanical Engineering, The Hong Kong Polytechnic University, Kowloon, Hong
Kong SAR, China. (email<EMAIL_ADDRESS><EMAIL_ADDRESS><EMAIL_ADDRESS>B. Lu is with the
School of Mechanical and Electrical Engineering, Robotics and Micro-systems
Center, Soochow University, China. (email: blu@suda.edu.cn)Video demo:
https://youtu.be/Zb3cA3hTvKg

###### Abstract

A cable-driven soft-bodied robot with redundancy can perform the tip
trajectory tracking task and in the meanwhile fulfill some extra constraints,
such as tracking with a designated tip orientation, or avoiding obstacles in
the environment. These constraints require proper motion planning of soft
material-based body that can be axially compressed. In this letter, we derived
the compressible curvature kinematics of a cable-driven soft robot which takes
the undesirable axial compression that caused by the cable-driven mechanism
into account. The motion planning of the soft robot for tip trajectory
tracking tasks in constrained conditions, including fixed orientation end-
effector and manipulator–obstacle collision avoidance, have been investigated.
The inverse solution of cable actuation was formulated as a damped least-
square optimization problem and iteratively computed off-line. The performance
of path and trajectory tracking and the obedience to constraints were
evaluated via the simulation we made open-source, as well as the prototype
experiments. The method can be generalized to the similar multisegment cable-
driven soft robotic systems by customizing the robot parameters for the prior
motion planning in tip trajectory following tasks.

###### Index Terms:

Modeling, Control, and Learning for Soft Robots, Whole-Body Motion Planning
and Control, Constrained Motion Planning

##  I Introduction

Soft robots are primarily composed of materials with low Young’s moduli that
are comparable with the biological materials like muscles [1, 2]. They can be
potentially used in many applications, such as robot-assisted minimally
invasive surgery [2, 3, 4, 5] and laser steering [6]. Using a robot tip to
track a designated trajectory poses an essential scene of automation. The
field of conducting trajectory tracking using rigid-bodied robots has been
well-explored, while challenges remain for the soft-bodied robots, especially
when they are redundant. The elasticity of the soft materials, as well as the
way of actuation, would bring complex variation to the robot modeling.

For a Cable-Driven Soft (Continuum) Robot (CDSR) which requires at least 3
actuation cables to achieve spatial manipulability, one can adopt the
geometrically-derived piecewise constant curvature (PCC) approach to obtain
the closed-form solutions for modeling [7, 8, 9, 10]. If assuming the arc
length (robot’s “backbone”) to be constant, the PCC would simplify a robot
segment to a 2-DOF module. It has been experimentally validated that the
neutral axis of a continuum segment would conditionally perform as a constant-
curvature arc [9].

However, the simplification or approximation of constant arc length in solving
the modeling equations may not precisely reveal the robot configuration,
especially when the axial compression of the soft body is significant enough
to vary the body stiffness along with different directions [11]. Some groups
provided a general approach to update the arc length with respect to the
bending [10]. Other model-based methods with better applicability have also
been proposed [8, 12, 13, 14, 15]. Particularly, the inevitable axial
compression of the cable-driven continuum robots has been investigated and
modeled in [8] and [12] using a mechanic-based method. Based on a series of
proof-of-concept verification in 2-D, the actuation-associated axial strain
has been studied with the cable decoupling being considered at the system
level. This could be further introduced to the 3-D scenarios with the
constrained motion being assigned.

The other thing is that many soft robots reported in the literature exclude
the control of tip orientation, i.e., only the tip position is considered [16,
17]. One of the major reasons is that the adoption of the PCC model simplifies
the controllable DOFs, requiring a soft manipulator to have at least three
segments to have full control in the task space [18]. But prototyping a CDSR
with three or even more segments could be challenging due to the densely
distributed cables, especially when it comes to the millimeter-grade, not to
mention the complicated coupling among the segments.

Moreover, a redundant robot is also capable of providing alternative motions
for the designated tip position, and these motions can lead to collision-free
control in the obstructed environment. Related work have been done in terms of
simulation [19, 20] and 2-D prototype experiments [21, 22, 23]. Collision-free
motion planning for a redundant CDSR under the constrained conditions remains
an important topic in the field. On one hand, the cable-driven mechanism
benefits the miniaturization of a spatially maneuverable soft robot. On the
other hand, proper motion planning would gift the redundant soft robots the
ability to work in constrained conditions, such as the task-based manipulation
and manipulator-obstacle collision avoidance, both of which might interest the
physicians.

In this letter, we explicitly derive the kinematic model for a multi-segment
cable-driven soft robot on a linear stage (i.e., $3n+1$ DOFs, where $n$
denotes the number of soft segments with three cables for actuation) with
improved accuracy by accounting for the passive axial compression which is
undesirably caused by the actuation and the coupling effect between segments.
The conventional PCC provides the framework for the model-based control of
continuum robots, but the applicability is not good enough for CDSRs where the
multiple cable actuation for desired bending would at the same time introduce
undesirable axial compression and coupling. Our improved model offers a higher
accuracy and will be more realistic in the task space with the above-mentioned
condition considered. In addition, a multi-segment CDSR simulator was
developed to illustrate the robot performance under different constrained
conditions, including (i) fixing the orientation of the tip, and (ii) avoiding
obstacles in tip path following. The improved model and motion planning method
were also examined through an experimental platform with 2 segments to
evaluate the actual performance and compare with the simulation results.

##  II Compressible Curvature Model

###  II-A Mechanics of A Single Soft Segment

A continuum manipulator is usually designed with a flexible backbone along the
neutral line to support the equidistantly distributed discs. The flexible
backbone has a high stiffness $K_{a}$ along its axial spine, and relatively
low stiffness $K_{b}$ in its lateral direction, so that it provide an ideal
“bending” deformation. However, for a soft material-based robot without a
backbone, the axial stiffness $K_{a}$ can be small, and neglecting its effect
could cause a perceptible error in the task space, given that most of the
maneuverable soft robots are extrinsically driven. Therefore, we take the
relationship between cable-driven mechanism and its undesirable axial
compression into the modeling consideration.

Figure 1: Sketch of a CDSR. The axial compression is colored in purple. The
sketch at the right is partially modified from [8]).

Leveraging from the PCC model in [7], for a multisegment soft robot with $n$
segments, the $k$-th soft segment ($k\leq n$) can be geometrically
parameterized in the configuration space by
$\bm{\psi}_{k}=[\theta_{k},\phi_{k},\kappa_{k}]^{{\top}}$ where
$\theta_{k}\in\left[0,\theta_{max}\right]$ is the angle of bending,
$\phi_{k}\in\left[-\pi,\pi\right)$ is the angle of bending direction, and
$\kappa_{k}$ is the bending curvature related to the initial undeformed length
of the segment $L_{k}$ (therefore, time-invariant) and
$\kappa_{k}=\theta_{k}/L_{k}$. Thus, the position of the $k$-th segment with
respect to its base frame $\Sigma_{k-1}$ can be expressed by

$\normalsize\mathbf{x}_{k}^{k-1}=\begin{bmatrix}{x}_{k}\\\ {y}_{k}\\\
{z}_{k}\end{bmatrix}=\frac{1}{\kappa_{k}}\begin{bmatrix}\cos{\phi_{k}}\left(1-\cos{\theta_{k}}\right)\\\
\sin{\phi_{k}}\left(1-\cos{\theta_{k}}\right)\\\
\sin{\theta_{k}}\end{bmatrix}.$ (1)

As depicted in Fig. 1, the configuration of the $k$-th segment $\bm{\psi}_{k}$
can be derived from the cable actuation as [7]

$\normalsize\theta_{k}(\bm{q})\\!=\\!\frac{2\sqrt{q_{k,1}^{2}\\!+\\!q_{k,2}^{2}\\!+\\!q_{k,3}^{2}\\!-\\!q_{k,1}q_{k,2}\\!-\\!q_{k,1}q_{k,3}\\!-\\!q_{k,2}q_{k,3}}}{3r},$
(2)
$\normalsize\phi_{k}(\bm{q})\\!=\\!\mathrm{atan2}\left(3\left(q_{k,2}\\!-\\!q_{k,3}\right),\sqrt{3}\left(q_{k,2}\\!+\\!q_{k,3}\\!-\\!2q_{k,1}\right)\right),$
(3)
$\normalsize\kappa_{k}(\bm{q})\\!=\\!\frac{2\sqrt{q_{k,1}^{2}\\!+\\!q_{k,2}^{2}\\!+\\!q_{k,3}^{2}\\!-\\!q_{k,1}q_{k,2}\\!-\\!q_{k,1}q_{k,3}\\!-\\!q_{k,2}q_{k,3}}}{3r/{L_{k}}},$
(4)

where $r$ denotes the (constant) symmetric distance between the center of the
cross-section and the cable channel, and $q_{k,i}\subseteq\bm{q}_{k}$ denotes
the actuation-by-length of the $i$-th cable of $k$-th segment, given by the
subtraction of the in-body cable length $L_{k,i}$ and the length of the
neutral line $L_{k}$ as

$\normalsize
q_{k,i}=L_{k,i}-L_{k}={-\theta_{k}r\cos{\left(\phi_{k}+\left(i-1\right)\xi\right)}},$
(5)

where $i\in\left\\{1,2,3\right\\}$ and $\xi=\frac{2\pi}{3}$. Thus, the joint
variables of the $k$-th segment expressed in the linear displacements of the
actuation cables can be written in the form of
$\bm{q}_{k}=[q_{k,1},q_{k,2},q_{k,3}]^{\top}\in\mathcal{Q}^{3}$, with
$\mathcal{Q}^{3}$ being an admissible space on which the unconstrained joint
variables can evolve. Note that $\mathcal{Q}^{3}\not\subset\mathbb{R}^{3}$
since the parameter $\theta_{k}\in[0,\theta_{max}]$ is bounded. The above
equations (1)–(5) conclude the PCC model of a continuum segment, of which the
neutral line $L_{k}$ is assumed to be constant. However, for a compressible
soft segment, the segment length $s_{k}$ is a variable related to the tensile
force of cables—within the elastic limit, a shorter segment would be expected
under a higher tensile force. Note that $s_{k}\leq L_{k}$. It results in a
compressible curvature arc. The cable length variables can be mapped with the
tensile force variables under several assumptions, including (i) the neutral
line deforms as a circular arc, (ii) the soft segment demonstrates a linear
elasticity under tensile force, (iii) the friction between the cable and
channel is omitted, (iv) the gravitational potential energy of the elastomer
is neglected [9] (see supplementary).

For a multisegment soft robot with $n$ segments coupled together, as a result
of the accumulative tensile force, the axial strain of the ($k-1$)-th segment
would be larger than that of the $k$-th segment. In the local $k$-th segment,
we assume the tensile force of the cables as
$\bm{f}_{k}=[f_{k,1},f_{k,2},f_{k,3}]^{\top}$. When cables are actuated and at
the equilibrium, a suitable candidate function
$\bm{f}_{k}:\mathcal{Q}^{3}\mapsto\mathbb{R}^{3}$ that maps to the joint
variables to a force vector that satisfies the quasi-static equilibrium for
all $q_{k,i}\in\mathcal{Q}^{3}$ can be concluded, such that

$\normalsize\phi_{k}\left(\bm{f}\right)=\mathrm{atan2}\left(3\left(f_{k,2}-f_{k,3}\right),\sqrt{3}\left(f_{k,2}+f_{k,3}-2f_{k,1}\right)\right).$
(6)

In a quasi-static state, as shown in Fig. 1, we can write the static
equilibrium equation for a single cable as [8]

$\mathbf{F}_{k,i}=\bm{F}_{eq,i}+\bm{f}_{k,i}+\bm{f}^{\prime}_{k,i}=\mathbf{0},$
(7)

where $\bm{f}_{k,i}$ and $\bm{f}^{\prime}_{k,i}$ are the cable tensile and the
reaction force perpendicular to the tip plane, respectively. Note that,
different from the unbolded $f_{k,i}$, the bolded $\bm{f}_{k,i}$ represents
the vector of tensile. $\bm{F}_{eq,i}\in\mathbb{R}^{3}$ denotes the resultant
equilibrium force that bends the segment, which can be derived by taking the
integration over the time-variant arc length $s_{k}$ as

$\displaystyle\bm{F}_{eq,i}$
$\displaystyle=\int_{0}^{s_{k}}\bm{w}_{i}\left(\sigma_{k}\right)\mathrm{d}\sigma_{k}=\kappa_{k}^{-1}\int_{0}^{\theta_{k}}\bm{w}_{i}\left(\sigma_{k}\right)\mathrm{d}\sigma^{\left[\theta\right]}_{k}$
(8)
$\displaystyle=\kappa_{k}^{-1}\int_{0}^{\theta_{k}}\\!\mathbf{R}_{k}^{k-1}\cdot\begin{bmatrix}-f_{k,i}\kappa_{k}\cos\left(\phi_{k}\\!+\\!\left(i\\!-\\!1\right)\xi\right)\\\
-f_{k,i}\kappa_{k}\sin\left(\phi_{k}\\!+\\!\left(i\\!-\\!1\right)\xi\right)\\\
0\end{bmatrix}\\!{\mathrm{d}\sigma^{\left[\theta\right]}_{k}},$

where $\bm{w}_{i}\left(\sigma_{k}\right)$ denotes the distributed load in an
infinitesimal cable length of $\sigma_{k}$, with
$\mathrm{d}\sigma_{k}=\kappa_{k}^{-1}\mathrm{d}\sigma^{\left[\theta\right]}_{k}$,
and $\mathbf{R}_{k}^{k-1}$ is the rotation matrix that defines the $k$-th
frame orientation w.r.t. that of the $(k-1)$-th frame as

$\normalsize{\mathbf{R}}_{k}^{k-1}=\mathrm{Rot}\left(\mathbf{\bar{z}}_{k-1},{\phi}_{k}\right)\cdot\mathrm{Rot}\left(\mathbf{\bar{y}}_{k-1},{\theta}_{k}\right)\cdot\mathrm{Rot}\left(\mathbf{\bar{z}}_{k},-{\phi}_{k}\right),$
(9)

where the operator $\mathrm{Rot}(\mathbf{\bar{u}}_{k},\delta)\in
SO\left(3\right)$ is the rotation matrix of rotating angle $\delta$ along the
unit axis $\mathbf{\bar{u}}_{k}$ of the $k$-th segment.

As the cables are equally distributed, based on Eq. (8), we take the triple
product of them and find out that

$\bm{F}_{eq,1}^{\top}\cdot\left(\bm{F}_{eq,2}\times\bm{F}_{eq,3}\right)=\mathbf{0},$
(10)

meaning that the three resultant equilibrium forces are coplanar vectors
(abbreviation for $\mathrm{s}=\sin$ and $\mathrm{c}=\cos$). Given the fact
that $\bm{F}_{eq,i}$ is a function of $\bm{f}_{k,i}$, we can conveniently
decouple the tensiles. As the soft segment bends, the resultant of torque
acting on the tip of the $k$-th segment, ${\tau}_{k}$, can be derived from the
sum of cable tensile as

$\displaystyle{\tau}_{k}=\sum_{i=1}^{3}\left(\bm{r}_{i}\times\bm{f}_{k,i}\right)={r}\sum_{i=1}^{3}\left(f_{k,i}\cos\left(\phi_{k}+\left(i-1\right)\xi\right)\right),$
(11)

where $\times$ denotes the cross-product operation because there are $i$
different cables, and $\|\bm{r}_{i}\|=r$, with $\bm{r}_{i}$ being the
concerning vector from the neutral line to the $i$-th cable in a distance of
$r$. From (6), one can obtain $\cos\phi_{k}(\bm{f})$ and
$\sin\phi_{k}(\bm{f})$ (see supplementary), such that (11) can be rewritten as

$\normalsize\tau_{k}=-r\mathcal{F}_{k},$ (12)

where
$\mathcal{F}_{k}\\!=\\!\sqrt{f_{k,1}^{2}\\!+\\!f_{k,2}^{2}\\!+\\!f_{k,3}^{2}\\!-\\!f_{k,1}f_{k,2}\\!-\\!f_{k,1}f_{k,3}\\!-\\!f_{k,2}f_{k,3}}$.

In this model, we consider the cross-sectional area as
$\mathcal{A}_{k}=\pi\left(r_{o,k}^{2}-r_{i,k}^{2}\right)$ where both radii are
variables subjected to the compression of the soft body, and the segment
length can be derived as

$s_{k}\left(\bm{f}\right)=L_{k}\left(1+\frac{\sum_{j=k}^{n}\sum_{i=1}^{3}f_{j,i}}{E\mathcal{A}_{k}}\right)$
(13)

which in fact defines a variable axial stiffness. The variable radii are given
by

$\begin{cases}r_{o,k}\left(\bm{f}\right)=\displaystyle
r_{o}\left(1-\frac{\nu{\sum_{j=k}^{n}\sum_{i=1}^{3}f_{j,i}}}{E\pi\left(r_{o,k}^{2}-r_{i,k}^{2}\right)}\right)\\\
r_{i,k}\left(\bm{f}\right)=\displaystyle
r_{i}\left(1-\frac{\nu{\sum_{j=k}^{n}\sum_{i=1}^{3}f_{j,i}}}{E\pi\left(r_{o,k}^{2}-r_{i,k}^{2}\right)}\right),\end{cases}$
(14)

where $r_{o}$ and $r_{i}$ are the original radii of the soft segment, and
$\nu=0.45$ is the Poisson’s ratio of the soft body. The derivation of Eq. (14)
is available in the supplemented page and codes. When the robot is compact
with a relatively large aspect ratio ($\frac{r_{o}}{L_{k}}$), like the robot
adopted in our paper, the effect on the radial change may not be obvious.
Nevertheless, robots made from soft materials with a high Poisson’s ratio, low
elastic modulus, and a long segment length would see the difference.

The local bending angle can be derived as the free-end slope of a cantilever
beam under a couple moment of $M_{k}$ by
${\mathrm{d}\theta_{k}}/{\mathrm{d}s}=M_{k}/{EI}$, which yields

$\normalsize\theta_{k}\left(\bm{f}\right)=\int_{0}^{s_{k}}{\frac{M_{k}}{EI}}\mathrm{d}s=\frac{r\mathcal{F}_{k}s_{k}}{K_{b}}=\frac{\mathcal{M}_{k}}{K_{T,k}},$
(15)

where $K_{b}=EI$ is the flexural rigidity of the structure, $M_{k}=|\tau_{k}|$
is the local bending moment of the $k$-th segment, $\mathcal{M}_{k}$ is the
bending moment of the $k$-th segment subjected to the variable length, and
particularly,

$\normalsize
K_{T,k}\left(s_{k}\right)=\frac{K_{b}}{s_{k}}=\frac{EI}{s_{k}}=\frac{E\pi\left(r_{o,k}^{4}-r_{i,k}^{4}\right)}{4s_{k}}$
(16)

denotes the bending stiffness of the $k$-th segment, which can vary
dramatically for a soft-bodied robot. And thus, the curvature related to the
cable tensile can be computed as

$\normalsize\kappa_{k}\left(\bm{f}\right)=\frac{\theta_{k}\left(\bm{f}\right)}{s_{k}\left(\bm{f}\right)}.$
(17)

Eq. (16) indicates the fact that the segment compression would result in a
variable bending stiffness locally, and the proximity toward the base implies
an even stiffer case. Therefore, for a multisegment soft robot with coupled
cables, the local bending stiffness of each segment shall be different and
relevant if it is not being the distal-most. Instead of terming the local
bending stiffness independently [12, 13, 14], we formulate the variable
stiffnesses that are responsive to the axial compression, therefore, the cable
actuation. The formulation suggests that the undesirable axial compression
will not only affect the tip positioning, but also the necessary inverse
solution in reality. A numerical verification will be provided in Sec. VI-A.

###  II-B Mechanics of A Coupled Soft Segment

Since the cables of the distal segment would pass through the channels of the
proximal segments, the coupling effects shall be considered. In this
subsection, we use a hat sign to represent the coupled configurations of the
relative proximal segments, namely,
$\widehat{\bm{\psi}}_{k}=[\widehat{\theta}_{k},\widehat{\phi}_{k},\widehat{\kappa}_{k}]^{\top}$,
where $k\neq n$.

The bending angle of the far-most $n$-th segment, $\widehat{\theta}_{n}$, is
only related to the local tensile $\bm{f}_{n}$. Therefore, its configuration
can be obtained using (6), (15), and (17). However, the bending and twisting
of the ($n-1$)-th segment is subjected to the local actuation $\bm{q}_{n-1}$
as well as the successive actuation $\bm{q}_{n}$. The manipulation of the
successive segment would generate an additional moment that couples the
configuration of its previous segment backward [12], denoted by

$\normalsize\widehat{\mathcal{M}}_{n-1}=\mathcal{M}_{n-1}+\widehat{\mathcal{M}}_{n},$
(18)

where $\widehat{\mathcal{M}}_{n-1}$ denotes the incorporated moment acting on
the ($n-1$)-th segment, and
$\widehat{\mathcal{M}}_{n}\equiv{\mathcal{M}}_{n}$. Eq. (18) can be
trigonometrically solved by giving arbitrary local directions [14]. After
reformulation, the general coupled configuration can be solved as

$\displaystyle\widehat{\theta}_{n-1}=$
$\displaystyle\frac{1}{K_{T,n-1}}\left({\mathcal{M}_{n-1}^{2}+{K_{T,n}^{2}\theta_{n}^{2}}}\right.$
(19)
$\displaystyle\left.+2\mathcal{M}_{n-1}K_{T,n}\theta_{n}\cos\left(\phi_{n-1}-\phi_{n}+\zeta\right)\right)^{\frac{1}{2}},$
$\displaystyle\widehat{\phi}_{n-1}\\!=\pi-\mathrm{atan2}$
$\displaystyle\left\\{\mathcal{M}_{n-1}\cos\phi_{n-1}+K_{T,n}\theta_{n}\cos\left(\phi_{n}-\zeta\right)\right.\\!,$
(20)
$\displaystyle\left.\mathcal{M}_{n-1}\sin\phi_{n-1}+K_{T,n}\theta_{n}\sin\left(\phi_{n}-\zeta\right)\right\\}.$

Hence, the coupled curvature of the ($n-1$)-th segment can be obtained as

$\normalsize\widehat{\kappa}_{n-1}=\frac{\widehat{\theta}_{n-1}}{s_{n-1}}.$
(21)

Note that these coupled configurations are “compression-responsive”, implying
infinite solutions in the joint space unless with the bending stiffness
$K_{T,k}$ being specified.

##  III Robot Description

In this work, we evaluated the methodology using a two-segment CDSR prototype
in [5]. The dimension and physical parameters of the robot are given in Fig.
2.

Figure 2: Design and Setup of the prototype. ①-$q_{1,1}$ ②-$q_{1,2}$
③-$q_{1,3}$ ④-$q_{2,1}$ ⑤-$q_{2,2}$ ⑥-$q_{2,3}$. Young’s modulus $E$ = 0.8
MPa. Original length $L_{k}$ = 50 mm.

As shown in Fig. 2, for the second segment, three servo motors with actuation
cables are installed with an offset of 180$\degree$, viz., $\zeta=\pi$. The
manipulator is vertically mounted on a linear slide to form an insertion-
retraction motion which is noted as $q_{0}$ and $q_{0}\geq 0$. Therefore, the
actuator space can be expressed as

$\normalsize\bm{q}=\begin{bmatrix}{q_{0}}&q_{1,1}&q_{1,2}&q_{1,3}&q_{2,1}&q_{2,2}&q_{2,3}\end{bmatrix}^{\top},$
(22)

where
$\bm{q}_{c}:=\begin{bmatrix}q_{1,1}&q_{1,2}&q_{1,3}&q_{2,1}&q_{2,2}&q_{2,3}\end{bmatrix}^{{\top}}$
is defined as the cable actuation. The cables we used were braided fish wire,
which was relatively stiff in the axial direction (maximum 30 kg-force of
tension) and thus they are assumed to be inextensible under the actuation of
the selected servo motors (Dynamixel XM430-W350, ROBOTIS) with a capability of
maximum 10 Nm of torque. Therefore, the tensile force of cables can be
analogously directed as the motor actuation. The homogeneous transformation
matrix of the tip with respect to a spatially-fixed world frame $\Sigma_{0}$
can be expressed as

$\mathbf{T}_{tip}^{0}\left(\bm{q}\right)=\begin{bmatrix}\mathbf{I}_{3\times
3}&\mathbf{\mathcal{H}}\\\ \mathbf{0}_{1\times
3}&1\end{bmatrix}\begin{bmatrix}\widehat{\mathbf{R}}_{1}^{0}&\widehat{\mathbf{x}}_{1}^{0}\\\
\mathbf{0}_{1\times
3}&1\end{bmatrix}\begin{bmatrix}\widehat{\mathbf{R}}_{2}^{1}&\widehat{\mathbf{x}}_{2}^{1}\\\
\mathbf{0}_{1\times 3}&1\end{bmatrix},$ (23)

where $\mathcal{H}=[0,0,q_{0}]^{\top}$ denotes the slide motion.

##  IV Optimization-Based Constrained Motion Planning for Trajectory Tracking

Given a linear velocity for the robot tip $\dot{\mathbf{x}}\in\mathbb{R}^{3}$
where $\dot{\mathbf{x}}=\dot{\mathbf{x}}^{0}_{k}$, the corresponding actuation
velocity that drives that robot tip to reach the goal shall be inversely
computed by

$\normalsize\dot{\bm{q}}=\mathbf{J}^{\dagger}\dot{\mathbf{x}}+\left(\mathbf{I}-\mathbf{J}^{\dagger}\mathbf{J}\right)\dot{\bm{q}}_{\mathcal{N}},$
(24)

where $\mathbf{J}\left(\bm{q}\right)\in\mathbb{R}^{3\times 7}$ is a Jacobian
matrix inferring a redundant robot, and ${\left(\cdot\right)}^{\dagger}$
denotes the right pseudo inverse operator as
$\mathbf{J}^{{\top}}\left(\mathbf{J}\mathbf{J}^{{\top}}\right)^{-1}$. The
second term projects the components of $\dot{\bm{q}}_{\mathcal{N}}$ to the
null space of $\mathbf{J}$, with $\mathbf{I}$ being a 7-by-7 identity matrix.
When the Jacobian matrix is updated in each time instance, the change in the
tip position in an infinitesimal period of time $\Delta\mathbf{x}$ can be
approximated based on the change of the actuation $\Delta\bm{q}$, such that
$\Delta\bm{q}=\mathbf{J}^{\dagger}\Delta\mathbf{x}$.

The above equation may provide infinite solutions due to the system
redundancy. However, it can be solved using some numerical approaches. One can
solve the inverse kinematics of using a Jacobian-based pseudo-inverse method
with a full rank $\mathbf{J}\left(\bm{q}\right)$. The other numerical approach
utilizes the damped least-squares (DLS) inverse [24], which can avoid
singularities with proper selection of the damping parameter $\lambda$, and
provides a numerically stable method of selecting $\Delta\bm{q}$. In order to
solve the needed actuation $\bm{q}$ for a desired path
$\mathcal{X}_{d}=[\mathbf{x}_{d}^{(1)},...,\mathbf{x}_{d}^{(N)}]$ expressed by
a finite set of discrete $N$ nodes, we compute the inverse kinematics (IK) by
solving a convex least-square optimization problem as

$\displaystyle\mathop{\arg\min}_{\Delta\bm{q}}\,{\|\mathbf{J}\Delta\bm{q}-\Delta\mathbf{x}\|_{2}^{2}+\lambda^{2}\|\Delta\bm{q}\|^{2}_{2}},$
(25)

where $\lambda\in\mathbb{R}_{>0}$ is a non-zero damping constant. This
quadratic optimization computes $\Delta\bm{q}$ that minimizes the error
between the generated tip position and the desired tip position (first term),
considering the feasible minimum motion in the actuation space (second term).
It can generate one of the solutions which fulfills the requirement of tip
path following, but the manipulator’s motion may not be optimal. By exploiting
the null space as introduced in (24), more sub-goals can be included to
satisfy different constraints from the environment and on the robot. In this
paper, we consider two important constraints for the motion planning.

###  IV-A Constraint 1: Fixed Orientation End-Effector

The control of the orientation of the soft robot’s tip is crucial in many
applications but has rarely been reported. However, the problem can be well-
solved by adding the incremental change of tip orientation as an optimization
term. Let the instantaneous change of tip orientation at the $j$-th point of
the desired path $\mathcal{X}_{d}$ be represented by the Euler angles as
$\mathbf{\Omega}\left(\Delta\bm{q}_{c}^{(j)}\right)=\Delta\begin{bmatrix}\alpha^{(j)}&\beta^{(j)}&\gamma^{(j)}\end{bmatrix}^{\top}$.
The Euler orientation of the tip is only related to the cable actuation
$\bm{q}_{c}\subseteq\mathcal{Q}^{3n}$. Hence, the optimization (25) can be
reformulated as

$\displaystyle\mathop{\arg\min}_{\Delta\bm{q}}$
$\displaystyle\|\mathbf{J}\Delta\bm{q}\\!-\\!\Delta\mathbf{x}\|_{2}^{2}\\!+\\!\sum_{i=1}^{3}\|\mathbf{\Omega}_{i}\left(\Delta\bm{q}_{c}\right)\\!-\\!\Delta\mathbf{\Omega}_{i}\|_{2}^{2}\\!+\\!\lambda^{2}\|\Delta\bm{q}_{c}\|_{2}^{2}$
(26) $\displaystyle\mathrm{s.t.}$
$\displaystyle\bm{A}\cdot\Delta\bm{q}\leq\bm{b}\quad\mathrm{and}\quad\bm{q}_{\min}\leq\bm{q}\leq\bm{q}_{\max},$

with $\bm{A}=\textrm{diag}(10^{-3},1,1,1,1,1,1)$ and
$\bm{b}=0.01\cdot\textrm{ones}(7,1)$, both of which define the linear
inequality constraints. The parameters are empirically selected with the rule
that the slide motion shall be minimal compared to the manipulator’s,
otherwise, the slide (joint) motion will be significant, compared to the
cables’, as they share the same unit of mm. $\mathbf{\Omega}_{i}$ means the
$i$-th row of the column vector, and $\Delta\mathbf{\Omega}_{i}$ is the
desired change in tip orientation. Based on the work region of the robot tip,
the bounded conditions are set as $\bm{q}_{\min}=-[0,2,2,2,2,2,2]^{{\top}}$,
$\bm{q}_{\max}=[60,0,0,0,0,0,0]^{{\top}}$. Compared to (25), the additional
term in (26) minimizes the error between the generated task-space orientation
change and the desired task-space orientation change under the linear
inequality and bounded constraints.

###  IV-B Constraint 2: Manipulator-Obstacle Collision Avoidance

As introduced in (24), the null space motion enables the CDSR to perform tip
trajectory tracking in an obstructed environment without any
manipulator–obstacle collision.

For an obstacle with an arbitrary shape, a sphere, with a radius of
$\mathcal{R}_{obs}$ and a spatial centroid of $\mathcal{O}$, can be defined to
enclose the entire obstacle. The (cylindrical) radius of the CDSR is
$\mathcal{R}_{sr}$, and therefore the critical distance between the virtual
backbone of the CDSR and the obstacle centroid for collision is
$\mathcal{R}_{obs}+\mathcal{R}_{sr}$. Thus, the critical distance at the
moment of collision shall be defined as the minimal Euclidean distance that
connects the obstacle centroid $\mathcal{O}$ to the critical point of the
robot virtual backbone at a particular instant of actuation
$\mathcal{S}\left(\Delta\bm{q}\right)$ as

$\textrm{if}\;\;\min\|\mathcal{S}\left(\Delta\bm{q}\right)-\mathcal{O}\|_{2}\begin{cases}<\mathcal{R}_{obs}\\!+\\!\mathcal{R}_{sr}&\,\textrm{collision}\\\
=\mathcal{R}_{obs}\\!+\\!\mathcal{R}_{sr}&\,\textrm{critical dist.}\\\
>\mathcal{R}_{obs}\\!+\\!\mathcal{R}_{sr}&\,\textrm{no collision},\\\
\end{cases}$ (27)

where $\min\left(\cdot\right)$ returns the minimum of $\left(\cdot\right)$.
Therefore, as shown in Fig. 3, the collision avoidance can be regulated as the
minimization of

$\normalsize
G\left(\Delta\bm{q}\right)=\frac{\mathcal{R}_{obs}+\mathcal{R}_{sr}}{\min\|\mathcal{S}\left(\Delta\bm{q}\right)-\mathcal{O}\|_{2}}.$
(28)

Noted that $G\left(\Delta\bm{q}\right)\rightarrow 0$ refers to the cases
without possible collision, i.e.,
$\min\|\mathcal{S}\left(\Delta\bm{q}\right)-\mathcal{O}\|_{2}\gg{\mathcal{R}_{obs}+\mathcal{R}_{sr}}$.
In this regard, equation (25) can be revised as

Figure 3: Interpretation of eq. (28) for collision avoidance. One can
iteratively compute the distance between the obstacle and each discrete point
along the cylindrical robot surface.

$\displaystyle\mathop{\arg\min}_{\Delta\bm{q}}$
$\displaystyle{\|\mathbf{J}\Delta\bm{q}-\Delta\mathbf{x}\|_{2}^{2}+\eta\|G\left(\Delta\bm{q}\right)\|_{2}^{2}+\lambda^{2}\|\Delta\bm{q}_{c}\|_{2}^{2}}$
(29) $\displaystyle\mathrm{s.t.}$
$\displaystyle\bm{A}\cdot\Delta\bm{q}\leq\bm{b}\quad\mathrm{and}\quad\bm{q}_{\min}\leq\bm{q}\leq\bm{q}_{\max},$

where $\eta$ is a positive constant to weight the sensitivity of collision
avoidance. The linear inequality and bounded values still follow the setting
in Constraint 1.

##  V Simulation

This section will demonstrate the simulated results of the constrained motion
of a two-segment CDSR for tracing the pre-defined paths without considering
the time prescription, with the additional requirement being fulfilled. The
simulations were written in Matlab using the parameters of our prototype. The
animation was built based on the plotting of a number of 3-D points along the
robot body by computing the compressible curvature forward kinematics using
the inverse solution that being solved from the optimization. Codes are
available online111URL: https://github.com/samlaipolyu/ncc_motion_planning.
Please refer to the README.md files for implementation. All simulations were
performed using a PC with an Intel Core i7-8750H CPU @2.20GHz 16GB RAM.

The basic bending motion of the CDSR using the improved compressible curvature
model is shown in Fig. 4. It can be seen that, due to the coupling effect,
actuating the distal segment would result in a passive bending for the
proximal segment.

Figure 4: Coupling effect of a two-segment CDSR in simulation. (a) Only the
proximal segment is bent; (b) only the distal segment is bent; (c) both
segments were bent, but the proximal bending compensated the distal bending;
(d) both segments were bent in the same direction.

Input: Desired path/traj.
$\mathcal{X}_{d}=[\mathbf{x}_{d}^{(1)},...,\mathbf{x}_{d}^{(N)}]\in\mathbb{R}^{3\times
N}$, optimization para. and constraints, desired orientation
$\mathbf{\Omega}_{d}\in\mathbb{R}^{3\times N}$ or obstacle para.
$\mathcal{R}_{obs}$, $\mathcal{O}$

Result: Actuation $\bm{q}\in\mathbb{R}^{7\times N}$ for the contrained motion

1 for  _$i=1:N$_ do

2 while  _$\|\Delta\mathbf{x}\|_{2} >{threshold}$_ do

3 Query $\bm{q}_{current}\in\mathbb{R}^{7\times 1}$;

4 Calculate $\mathbf{x}_{tip}^{0}=forwardKine\left(\bm{q}_{current}\right)$;

5 Calculate $\Delta\mathbf{x}=\mathbf{x}_{d}^{(i)}-\mathbf{x}_{tip}^{0}$;

6 Compute
$\Delta\bm{q}\leftarrow\arg\min_{\Delta\bm{q}}\\{objFun\left(\Delta\mathbf{x}\right)\\}$
from (26) or (29);

7 Set $\bm{q}_{current}\leftarrow\bm{q}_{previous}+\Delta\bm{q}$;

8 Set $\bm{q}_{previous}\leftarrow\bm{q}_{current}$;

9

10 Set $\bm{q}^{(i)}\leftarrow\bm{q}_{current}$;

Algorithm 1 Optimization of constrained motion of CDSR

###  V-A Simulation 1: Fixed Orientaiton End-Effector

The path tracking tasks using a fixed orientation tip were simulated based on
the compressible curvature model. The related demos are available in the
supplementary video. The simulation implemented equation (26) with the initial
value of $\Delta\bm{q}_{init}=-10^{-2}[-0.01,1,2,1,2,1,2]^{\top}$ and an
empirically-selected damping coefficient222Larger $\lambda$ will result in a
“stiffer” motion. Readers may try different values using our code according to
the practical need. of $\lambda^{2}=0.1$. The trial path were represented by a
finite number of nodes along its path using parabolic functions. The tracking
time can be controlled and predicted based on $N$ and the computational time
of each interval. The control algorithm is shown in Algorithm 1. The goal is
to use the optimized inverse solution to implement the CDSR such that it can
follow a pre-defined path with a constrained motion. The time required to
compute the solution using the PC with the given specification is about 0.4
second per for-loop using pure Matlab script. It can be further boosted to an
update rate of 60 Hz when being converted to C.

The simulation results of two examples are shown in Fig. 5. The left part of
the figure demonstrates the tracking performance of a planar square path (with
$N=91$ nodes) with its fixed orientation end-effector. The desired tip
orientation was set as $\mathbf{\Omega}_{d}=[0,0,0]^{\top}$, i.e.,
intuitively, pointing “upward” with respect to the base frame, forming a
vertical tip pose throughout the path tracking process. With the desired path
and orientation predefined, the proposed DLS IK solver computes the needed
cable actuation in the form of tensile force ($\bm{f}<0$) and the motion of
linear slide $q_{0}$ to compensate the change of $z_{tip}^{0}$ for the desired
working surfaces. Then, we input the actuation to the simulator to visualize
the move using forward kinematics. The results indicate that CDSR motion
generated from the computed actuation can satisfy the tip positioning within
an absolute error of 0.8 mm. The orientation can be well aligned as desired
within a 1-degree error.

Figure 5: Simulation 1: tracking paths with the orientation of the end-
effector fixed. Left: tracking a square with the tip pointing “upward” with
respect to the CDSR. Right: tracking an eight with its tip tilted by
45$\degree$. The corresponding cable actuation, tip position error, and tip
orientation error are given${}^{\textrm{a}}$.

The tip orientation with respect to the world frame can be assigned and
maintained as much as possible. The right part of Fig. 5 shows the simulation
result of tracking an “8”-shaped path ($N=91$ nodes), with a desired
orientation defined as $\mathbf{\Omega}_{d}=[0,0,45]^{\top}$ degrees. The
computed tip position error norm is within 0.6 mm. The error in the tip
pointing angle $\gamma$ is within 2$\degree$. The other two Euler angles,
$\alpha$ and $\beta$, are fluctuating but within minor angular errors. Another
two examples with more complex paths are given in the supplementary. The
accuracy of maintaining the orientation depends on the magnitude of the tilted
angle: higher accuracy could be expected when the tip is completely
“vertical”. The method is also applicable in a changing orientation
$\mathbf{\Omega}_{d}$.

###  V-B Simulation 2: Manipulator-Obstacle Collision Avoidance

The related demos are available in the supplementary video. The simulation
implemented equation (29) with the same $\Delta\bm{q}_{init}$ and
$\lambda^{2}$ in Sec. V-A. Regarding the obstacle setting, we consider a
sphere located at $\mathcal{O}=[-10,-30,90]^{\top}$ mm with respect to the
world frame $\Sigma_{0}$, with a radius of $\mathcal{R}_{obs}=7.5$ mm. The
collision avoidance sensitivity is $\eta=0.1$. The CDSR shall complete the
path tracking task accurately but avoid any manipulator-obstacle collisions.

Figure 6: Simulation 2: tracking paths in a constrained environment, where (a)
the CDSR would collide into the obstacle during the tracking process. (b) With
proper motion planning, the CDSR can avoid such collision while keeping a
satisfactory path tracking accuracy. All conditions are exactly the same
except the with or without collision-free motion planning. The collision
indicator is given by (28). More results are available in the supplementary.

To demonstrate the effectiveness of our method, as shown in Fig. 6-a, we
provide a circular path with prescribed time,
$\mathbf{x}_{d}^{(t)}=[41\cos\left(t\right),41\sin\left(t\right),110]^{\top}$
mm with $t=0:2\pi$ sec in an $\frac{\pi}{45}$ interval, which would have had a
collision with the robot body under normal IK solution from (25). By applying
(29) to algorithm 1, the solver can provide a null space solution such that
the CDSR can adjust its body motion and dodge the obstacle, while the tip
keeps tracing the planned path (Fig. 6-b).

##  VI Experiment

###  VI-A Model Verification

Figure 7: Comparison on compressible-associated $K_{a,k}$ and $K_{T,k}$ versus
the constant $K_{a}$ and $K_{T}$ based on simulation. The variation will be
more important on the use of softer materials and longer robots.

Based on the numerical simulation, we verified the differences on tip
positioning between using the compressible-associated $K_{a,k}$ and $K_{T,k}$
versus the constant $K_{a}$ and $K_{T}$. Here we provide two examples in Fig.
7. We first employed a random set of actuation to the simulator and obtained a
proposed robot pose (Green) using $K_{T,k}\left(s_{k}\right)$. Then, with the
same actuation, and setting the bending stiffness to be a constantly defined
$K_{a}$ and $K_{T}$, we could obtain a deviated robot pose (Red). Based on our
100 mm-long prototype, such tip positioning norm difference can be up to about
5 mm. A larger difference will be expected when using softer materials or
applying on longer robots. The results indicate the importance of considering
the axial compression when we were modeling a soft material-based actuator.

The compressible curvature model on a soft segment was experimentally
verified. As depicted in Fig. 8, a planar bending test was conducted on the
proximal segment of a CDSR. Due to the material softness, the segments would
undergo an axial compression when the cables were actuated. Here we use an
inextensible PCC model that excludes the arc length variation when the robot
is in motion, i.e., $s_{k}\left(\bm{f}\neq 0\right)=L_{k}$ as the baseline.
Note that the PCC model should be fitted to when $s_{k}(\bm{f}=0)=L_{k}$. In
Fig. 8, group 1 and group 2 denote the total arc length of 98 and 96 mm,
respectively. The result indicates that the inextensible PCC model for general
continuum robots may not be satisfactory enough for soft-bodied robots.
However, our compressible curvature model can better predict the tip position
with given actuation inputs.

Figure 8: Verification of the compressible curvature model on a soft robot.
Originated at the same point, pretension were added in group 1 and group 2 to
shorten the segment length.

###  VI-B Constrained Motion Planning

Experiments were conducted to evaluate the performance of the proposed motion
planning approach for a two-segment CDSR prototype based on the compressible
curvature model. Since the tensile force that applying on the inextensible
cables can be analogously viewed as the cable displacement, we experimentally
scaled the proportion between the applied tensile force and cable
displacement, i.e., $\bm{q}_{c}=\mu\bm{f}$, where $\mu=18$ mm/N based on the
tuned cable tension. To produce a smooth input for the motor, a moving median
filter was used to reduce the periodic trends and outliers in the solution. A
bench-top RGB-D camera (RealSense D435, Intel) was used to retrieve the
features of a labeled CDSR as shown in Fig. 2, and transform the data to the
world frame coordinate.

The performance of trajectory tracking under the constraint 1 (fixed
orientation) have been tested. We used parametric functions of time step to
represent the trajectories. We verified that the computed input can produce
constrained manipulator motions as desired with satisfactory accuracy. Fig. 9
shows the results in the case where the end-effector was required to trace an
oval with its tip orientation being vertical to the ground. The visual
measurement indicates an axial error within 2 mm and an orientation error
within 10$\degree$. Fig. 10 shows the situation when a 20$\degree$ tilt angle
was assigned to the CDSR under the same path. Even though the positioning
accuracy at the long end of the elliptical path was not as good as in the
vertical case as the manipulator attempted to flex itself to maintain the
certain tip orientation along with the tracking, the result was satisfactory
in fulfilling the fixed orientation requirement.

Figure 9: Experiment of tracking an oval with the tip being vertical to the
ground (tilt angle = 0$\degree$). Snapshot by time sequence; actuation input
$\bm{q}_{c}$; tip positioning error on 3 axes and orientation error; and
overall tracking performance. Figure 10: Experiment of tracking an oval with
the tip being tilted to the ground (tilt angle = 20$\degree$).

The performance of trajectory tracking under constraint 2 (collision
avoidance) has also been tested on the prototype. A $\diameter$15 mm spherical
obstacle was located at $[-10,-15,95]$ mm in $\Sigma_{0}$, which would have
had blocked the robot motion in tracking the square trajectory. By deploying
the proposed motion planning method, the computed inverse solution was able to
drive the CDSR to avoid collision with the desired tip path being tracked
(Fig. 11). The experiment shows that the tracking error was within 5 mm, which
was overall satisfactory. However, it should be noted that, aligning with the
simulation result, the error of tracking would be enlarged near the critical
point of collision due to the deliberate motion for the avoidance. This can be
treated as a trade-off for the collision-free motion depending on the
selection of $\eta$.

Figure 11: Experiment of tracking a square in an obstructed environment with
collision-free motion planning of the soft manipulator.

##  VII Conclusions

In this work, we demonstrated a constrained motion planning approach
incorporated with the compressible curvature modeling for a multisegment
cable-driven soft robot under the segment coupling effect. The robot modeling
includes the undesirable segment shortening due to the material elasticity and
cable-driven mechanism. Based on the mechanics of the cable and soft body, the
decoupled mapping among spaces of the actuator, configuration, and task were
derived. On top of that, we developed an optimization-based motion planning
algorithm to extend the controllability of a redundant soft robot for tip
trajectory tracking in constrained conditions, including with a fixed
orientation tip, and with the avoidable manipulator–obstacle collision. Based
on a two-segment robot prototype, numerical simulation and experiments were
carried out to confirm and evaluate the explicitly-formulated model and the
proposed algorithm for tip trajectory tracking tasks. Our method may be
generalized for similar multisegment cable-driven soft robots in constrained
motion planning.

However, force-related interaction has been excluded in the current method.
Future effort may be made in studying the variable-stiffness motion planning
by exploiting the soft body compression which varies the soft body’s
stiffness.

## References

  * [1] D. Rus and M. T. Tolley, “Design, fabrication and control of soft robots,” _Nature_ , vol. 521, no. 7553, pp. 467–475, 2015.
  * [2] M. Cianchetti _et al._ , “Soft robotics technologies to address shortcomings in today’s minimally invasive surgery: the stiff-flop approach,” _Soft Robot._ , vol. 1, no. 2, pp. 122–131, 2014.
  * [3] M. Runciman _et al._ , “Soft robotics in minimally invasive surgery,” _Soft Robot._ , vol. 6, no. 4, pp. 423–443, 2019.
  * [4] J. Burgner-Kahrs _et al._ , “Continuum robots for medical applications: A survey,” _IEEE Trans. Robot._ , vol. 31, no. 6, pp. 1261–1280, 2015.
  * [5] J. Lai _et al._ , “Verticalized-tip trajectory tracking of a 3d-printable soft continuum soft robot: Enabling surgical blood suction automation,” _IEEE/ASME Trans. Machatron._ , 2021, DOI: 10.1109/TMECH.2021.3090838.
  * [6] H. Mo _et al._ , “Control of a flexible continuum manipulator for laser beam steering,” _IEEE Robot. Autom. Lett._ , vol. 6, no. 2, pp. 1074–1081, 2021.
  * [7] R. J. Webster III and B. A. Jones, “Design and kinematic modeling of constant curvature continuum robots: A review,” _Int. J. Robot. Res._ , vol. 29, no. 13, pp. 1661–1683, 2010.
  * [8] D. B. Camarillo _et al._ , “Mechanics modeling of tendon-driven continuum manipulators,” _IEEE Trans. Robot._ , vol. 24, no. 6, pp. 1262–1273, 2008\.
  * [9] K. Xu and N. Simaan, “An investigation of the intrinsic force sensing capabilities of continuum robots,” _IEEE Trans. Robot._ , vol. 24, no. 3, pp. 576–587, 2008.
  * [10] C. Della Santina _et al._ , “On an improved state parametrization for soft robots with piecewise constant curvature and its use in model based control,” _IEEE Robot. Autom. Lett._ , vol. 5, no. 2, pp. 1001–1008, 2020\.
  * [11] J. Lai _et al._ , “Variable-stiffness control of a dual-segment soft robot using depth vision,” _IEEE/ASME Trans. Mechatron._ , 2021, DOI: 10.1109/TMECH.2021.3078466.
  * [12] D. B. Camarillo _et al._ , “Configuration tracking for continuum manipulators with coupled tendon drive,” _IEEE Trans. Robot._ , vol. 25, no. 4, pp. 798–808, 2009.
  * [13] W. S. Rone and P. Ben-Tzvi, “Mechanics modeling of multisegment rod-driven continuum robots,” _J. Mech. Robot._ , vol. 6, no. 4, 2014.
  * [14] P. S. Gonthina _et al._ , “Mechanics for tendon actuated multisection continuum arms,” in _Proc. IEEE Int. Conf. Robot. Autom. (ICRA)_ , 2020, pp. 3896–3902.
  * [15] F. Boyer _et al._ , “Dynamics of continuum and soft robots: A strain parameterization based approach,” _IEEE Trans. Robot._ , vol. 37, no. 3, pp. 847–863, 2020.
  * [16] P. Qi _et al._ , “Kinematic control of continuum manipulators using a fuzzy-model-based approach,” _IEEE Trans. Ind. Electron._ , vol. 63, no. 8, pp. 5022–5035, 2016.
  * [17] F. Xu _et al._ , “Adaptive visual servoing for an underwater soft robot considering refraction effects,” _IEEE Trans. Ind. Electron._ , vol. 67, no. 12, pp. 10 575–10 586, 2020.
  * [18] I. S. Godage _et al._ , “Dynamics for variable length multisection continuum arms,” _Int. J. Robot. Res._ , vol. 35, no. 6, pp. 695–722, 2016\.
  * [19] J. Xiao and R. Vatcha, “Real-time adaptive motion planning for a continuum manipulator,” in _IEEE/RSJ Int. Conf. Intell. Robot. Syst. (IROS)_ , 2010, pp. 5919–5926.
  * [20] A. Ataka _et al._ , “Real-time pose estimation and obstacle avoidance for multi-segment continuum manipulator in dynamic environments,” in _Proc. IEEE/RSJ Int. Conf. Intell. Robot. Syst. (IROS)_ , 2016, pp. 2827–2832.
  * [21] R. J. Roesthuis and S. Misra, “Steering of multisegment continuum manipulators using rigid-link modeling and fbg-based shape sensing,” _IEEE Trans. Robot._ , vol. 32, no. 2, pp. 372–382, 2016.
  * [22] A. D. Marchese _et al._ , “Whole arm planning for a soft and highly compliant 2d robotic manipulator,” in _IEEE/RSJ Int. Conf. Intell. Robot. Syst. (IROS)_ , 2014, pp. 554–560.
  * [23] A. D. Marchese _et al._ , “Dynamics and trajectory optimization for a soft spatial fluidic elastomer manipulator,” _Int. J. Robot. Res._ , vol. 35, no. 8, pp. 1000–1019, 2016.
  * [24] B. Siciliano _et al._ , _Robotics: modelling, planning and control_. Springer Science & Business Media, 2010.