This article has been accepted for publication in IEEE Robotics and Automation Letters.

This is the author's version which has not been fully edited and
content may change prior to final publication. Citation information: DOI 10.1109/LRA.2023.3239306

IEEE ROBOTICS AND AUTOMATION LETTERS. PREPRINT VERSION. ACCEPTED JANUARY, 2023 1

Imitation-Based Motion Planning and Control of A

Multi-Section Continuum Robot Interacting with the
Environment
Ibrahim A. Seleem1,3 , Haitham El-Hussieny2 , and Hiroyuki Ishii1

Abstract—Recently, flexible robots are growing in importance in complex and dynamic environments [1]. Due to their
owing to their merits over rigid robots in maneuverability and morphological structure, continuum robots can extend, shrink,
safety, which equips them to work in unstructured environments, and bend, which facilitates shaping their structure to suit
such as occur in medical applications. However, motion planning
and control of flexible manipulators is challenging due to their confined spaces in unstructured environments such as the
compliance behavior and system uncertainties. Thus, this paper biomedical field [2], Minimally Invasive Surgeries (MIS) [3],
presents an Imitation-based Motion Planning (IbMP) approach, and exploration missions [4].
along with dynamic impedance control for learning, planning, Despite the merits of soft robots, they still suffer from com-
and trajectory tracking of a two-section soft continuum robot in mon limitations in modelling, motion planning, and control
a dynamic environment. Point-to-point motion demonstrations,
including the robot’s tip position and orientation are intuitively due to their compliant structure and viscoelastic character-
provided by a motion capture system (OptiTrack V120-trio) and istics, leading to unpredictable motion behaviors. Recently,
a similar kinematic flexible interface. Additionally, a singularity- multiple motion planning techniques, which were purposed
free dynamic model based on Lagrangian formulation and Taylor originally for rigid robots, are used for continuum robots
expansion series of a two-section continuum robot is derived while [5], [6]. However, these methods still need programming
planning for robot motions. The simulation results show that
the IbMP approach, along with the dynamic impedance control, experts and significant computational time to precisely define a
generalizes the robot’s motion by varying the initial and goal of sequence of actions that the robot adheres to during movement
the robot’s tip pose while avoiding static and dynamic obstacles, to achieve the task and to keep up with environmental changes
and moving back to the desired track after disturbances. Finally, like changing goals or avoiding dynamic obstacles [7]. Thus,
the stability and performance of the IbMP algorithm against there has recently been an increasing interest in the Learning
input disturbances are assessed using a Monte Carlo approach
that can guide the selection of gain values. from Demonstration (LfD) framework due to its performance
that allows non-expert users with limited robotics knowledge
Index Terms—Soft robot, dynamic movement primitives, to quickly program and adapt the robot behaviors while con-
impedance control, Monte-Carlo
cerning the task constraints through kinesthetic learning or via
teleoperation [8]. In LfD, state-action pairs of the desired task
I. Introduction are recorded from the human demonstrations either kinestheti-
Recently, many researchers have been motivated by the cally or via teleoperation. Then, the robot learns to regenerate
incredible capabilities of natural organisms such as octopus and generalize the demonstrated motions. Impressive results
arms, elephant trunks, worms, and snakes to facilitate dexter- were acquired by LfD approaches in many applications [9]. In
ous manoeuvring in static and dynamic environments. Unlike [10], motion planning-based LfD of multiple-segment flexi-
classical manipulators with rigid and bulky elements, which ble manipulator actuated by ionic polymer–metal composite
face challenges, especially in crowded environments, contin- (IPMC) flexible actuators was developed using kinesthetic
uously bending continuum robots with compliance behavior teaching. Human demonstrations of the manipulator passing
and flexible backbones have shown promising performance through a narrow hole were extracted and smoothed based
on Gaussian Mixture Model (GMM) and Gaussian Mixture
Manuscript received: July, 31, 2022; Revised October, 3, 2022; Accepted Regression (GMR). The experimental results showed that the
January, 16, 2023.
This paper was recommended for publication by Editor Aniket Bera upon soft robot passed through a narrow hole successfully. However,
evaluation of the Associate Editor and Reviewers’ comments. This work was the human operator needed to adjust the voltage of each robot
supported by the Japan Society for the Promotion of Science (JSPS) under section manually during motion in static environments based
Grant Numbers JP22F21076, JP21H05055, and JP19H01130.
1 Ibrahim A. Seleem and Hiroyuki Ishii are with the Depart- on the traditional trial and error method, which represented a
ment of Modern Mechanical Engineering, Waseda University time-consuming task. In addition, the developed LfD approach
[email protected], [email protected] had no guarantee of generalizing the collected demonstrations
2 Haitham El-Hussieny is with School of Innovative Design and regenerating new movements.
Engineering, Department of Mechatronics and Robotics Engineering,
Egypt-Japan University of Science and Technology (EJUST) The development of pose planning-based Dynamic Move-
[email protected] ment Primitives (DMP) and control of a pneumatic actuated
3 Ibrahim A. Seleem is with Faculty of Electronic Engineering, Department
bionic handling assistant robot for apple picking tasks was
of Industrial Electronics and Control Engineering, Menoufia University (On
leave) [email protected] introduced in [11]. Kinesthetically, the robot’s tip poses were
Digital Object Identifier (DOI): see top of this page. recorded during teacher demonstrations. The experimental

This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/
 This article has been accepted for publication in IEEE Robotics and Automation Letters. This is the author's version which has not been fully edited and
content may change prior to final publication. Citation information: DOI 10.1109/LRA.2023.3239306

2 IEEE ROBOTICS AND AUTOMATION LETTERS. PREPRINT VERSION. ACCEPTED JANUARY, 2023

tests showed that the soft robot successfully picked and placed dynamic environments. (2) Evaluating the robustness of IbMP
the apple. However, the proposed learning-based orientation based on a Monte Carlo simulation-based algorithm which
method combined unit quaternion and angle axis method, can guide the IbMP tuning process. (3) developing a dy-
which may generate angle error [12], and it was applied only namic impedance controller to compensate for the interaction
in a static environment. In addition, in many medical applica- forces with the environment while tracking the robot’s tip
tions, having a direct contact with the robot through kinesthetic position trajectory. The dynamic model is derived based on
teaching could be challenging. In our previous work [13], [14], a Lagrangian formulation with Taylor Expansion to overcome
a Demonstration Guided Pose Planning (DGPP) approach and singularities at specific configurations.
control of a two-section continuum robot were proposed. The The remainder of this article is organized as follows. Section
human demonstrations were recorded using a low accuracy II introduces kinematics and dynamic modeling of a two-
Inertial Measurement Unit (IMU). In [13], a rotation matrix section continuum robot based on Lagrangian formulation with
representing orientation suffered from singularity at specific Taylor expansion. Section III gives a detailed description of the
configurations. In addition, the computed torque with adaptive IbMP approach. Dynamic impedance control for tracking the
kinematic control was applied to track the robots’ tip position, tip position trajectory while interacting with the environment
which was not efficient while interacting with the environment. is presented in section IV. Simulation results of the IbMP
While in [14], a DGPP-based quaternion that was introduced approach and the developed impedance control are evaluated
to plan the tip position and orientation separately had the in section V. Finally, the conclusion and future works are
potential to cause unsynchronised movements. Moreover, the introduced in section VI.
developed adaptive kinematic control was not appropriate to
capture the dynamic behavior of the soft robot.
II. Kinematic and Dynamic Modelling
On the other hand, developing a control algorithm for
continuum manipulators that can successfully interact robustly Different actuation mechanisms are used in continuum
with dynamic environments is challenging [15]. Nonlinear robots to achieve the required tip bending, such as cable-
hybrid position/force control of a tendon-driven catheter was driven, pneumatic, hydraulic, and Shape Memory Alloy
presented in [16]. Cosserat rod theory was applied for static (SMA) actuation. Due to their simplicity, scalability, and sim-
modeling of the catheter. The simulation results showed the ple control, soft robots actuated by cable-driven mechanisms
effectiveness of the developed position/Force controller in are used in many fields such as inspection, exploration, and
regulating the constant applied force. In [17], a hybrid posi- medicine.
tion/force control algorithm was developed for a single-section The schematic diagram of a single-section continuum robot
cable-driven continuum robot. The control structure was built is shown in Fig. 1. Each section contains three cables spaced
depending on selection matrices to choose between position by 120◦ that can be varied to control the robot’s tip moment.
control application before or after interaction and force control Because of the precise representation and low computational
during interaction. The experimental results showed that the time derivation compared to other kinematic modeling tech-
controller was stable while interacting with rigid and soft niques, the constant curvature approach is chosen to describe
environments. However, it was not effective and safe in many the pose of section 𝑖 relative to its base 𝑖 − 1 [19], as follows:
applications, such as surgical operations, due to its high


velocities when applying position control directly before col- R𝑖 𝑞 𝑒 𝑝 𝑖 (𝑞𝑞 𝑒 ))
liding with the environment, which may lead to unpredictable T𝑖𝑖−1 = , for 𝑖 = 1, 2 (1)
0 1
interaction. Also, this control approach neglected to follow
the position trajectory during the interaction period leading to where 𝑞 𝑒 = [ℎ𝑖 , 𝜃 𝑖 , 𝛽𝑖 ] ∈ R6 , for 𝑖 = 1, 2 is the vector of con-
deviation from the reference motion trajectories [18]. figuration variables, in which ℎ𝑖 is the section length, 𝜃 𝑖
The contributions of this article are summarized in the represents the bending plane angle from +X axis, 𝛽𝑖 = 𝜅𝑖 ℎ𝑖 is
following aspects. (1) Proposing an Imitation-based Motion
Planning (IbMP) approach for simultaneously learning the
position and orientation of a two-section soft continuum robot
which is chosen to increase the flexibility and dexterity for
inspection tasks. Via teleoperation, the learning procedure
starts by collecting the human demonstrations using a motion
capture system (OptiTrack-V120) from a flexible interface,
which has similar kinematics to the simulated robot. The
proposed IbMP approach is not limited to reproducing the
demonstrated movements but extends to generalizing the tip
pose trajectories while changing the initial and goal poses,
and withstanding external disturbances. The collision of the
continuum robot with the environment represents an inevitable
problem, especially when the robot works in narrow spaces and
may lead to serious damage to the robot or the environment. Fig. 1: Construction of cable-driven single section continuum
Thus, the IbMP approach can avoid obstacles in static and robot.

This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/
 This article has been accepted for publication in IEEE Robotics and Automation Letters. This is the author's version which has not been fully edited and
content may change prior to final publication. Citation information: DOI 10.1109/LRA.2023.3239306

SELEEM et al.: IMITATION-BASED MOTION PLANNING AND CONTROL OF A MULTI-SECTION CONTINUUM ROBOT 3

the angle subtended
by the bending arc in which 𝜅 𝑖 is the cur- that monotonically changes from 1 to 0 during motion to
vature, and R 𝑞 𝑒 represents the  rotation that maps  the 𝑖 axes avoid explicit time dependencies of the forcing function 𝑓 ,
to the 𝑖 − 1 axes. While 𝐿 𝑖 = 𝐿 1𝑖 𝐿 2𝑖 𝐿 3𝑖 , for 𝑖 = 1, 2 𝑜 ∈ R3 is the obstacle position, 𝑥 is the robot section’s tip
are the cable lengths of each section which are related to the position trajectory, 𝑥 0 and 𝑥 𝑔 are the initial and goal positions
joint variables as follows: respectively. In addition, 𝑣 and 𝑣¤ represent the scaled velocity
(𝐿 1 + 𝐿 2 + 𝐿 3 )

1 (𝐿 3 + 𝐿 2 − 2𝐿 1 )
 and acceleration respectively, and 𝜏 > 0 is a temporal scaling
ℎ= , 𝜃 = tan−1 √ factor to alter the duration of motion trajectory slower or faster.
3 3 (𝐿 2 − 𝐿 3 ) Multiple LfD approaches have been proposed for avoiding
√︃ (2)
𝐿 12 + 𝐿 22 + 𝐿 32 − 𝐿 1 𝐿 2 − 𝐿 2 𝐿 3 − 𝐿 1 𝐿 3 obstacles based on neural networks to model the coupling
𝛽=2 terms [22]. However, these methods require additional learning
3𝑑
of multiple human demonstrations to succeed in this task.
where 𝑑 is the distance from the robot center to each cable. Other approaches added a coupling term 𝑃 (𝑥𝑥 , 𝑣 ) (4) to avoid
The kinematic representation in (1) suffers from the singu- obstacles in R2 and R3 without additional demonstrations,
larities at specific configurations (𝜅𝑖 = 0) that can be tackled which is computed as follows:
by applying 7𝑡 ℎ order Taylor expansion to approximate the
𝑃 (𝑥𝑥 , 𝑣 ) = 𝜁r (𝑣𝑣 − 𝑜¤ ) 𝜙𝑒 − 𝜗 𝜙


Cartesian position 𝑝 𝑞 𝑒 . (6)
The Lagrangian formulation of the equation of motion is
computed as follows: where 𝜁 and 𝜗 are tuning gains which are selected based on



the size of the obstacle, 𝑜¤ is the obstacle velocity, r represents
M 𝑞 𝑒 𝑞¥ 𝑒 + N 𝑞 𝑒 , 𝑞¤ 𝑒 𝑞¤ 𝑒 + G 𝑞 𝑒 = 𝚪 + J 𝑇 𝐹 (3) a rotation matrix with rotation axis 𝑎 = (𝑜𝑜 − 𝑥 ) × 𝑣 and rotation
where 𝚪 ∈ R6 is the driving torques, 𝐹 ∈ R3 is the contact angle 𝜋/2 that is added to overcome the inconsistency that may
𝜕𝑝 happen between the acceleration generated by the coupling
𝑞 𝑒 ∈ R
represents the Jacobian
force vector, J = 𝜕𝑞 3×6 matrix,


term and the velocity of the robot [23]. The steering angle
M 𝑞𝑒 ∈ R 6×6 , N 𝑞 𝑒 , 𝑞¤ 𝑒 ∈ R6×6 and G 𝑞 𝑒 ∈ R are the
6
𝜙 is the angle between the robot tip position 𝑥 towards the
inertia matrix, the Coriolis and Centrifugal matrix and the
obstacle position 𝑜 and the robot tip velocity 𝑣 relative to the
gravity vector respectively.
obstacle velocity 𝑜¤ which is computed as follows:
 
III. Imitation based Motion Planning ⟨𝑜𝑜 − 𝑥 , 𝑣 − 𝑜¤ ⟩
𝜙 = arccos (7)
Dynamic Movement Primitives (DMP) is a motion learning ∥𝑜𝑜 − 𝑥 ∥ × ∥𝑣𝑣 − 𝑜¤ ∥
technique in which the soft robotic arm can execute many
The forcing function 𝑓 (𝑠) is parameterized by a nonlinear
tasks based on a collection of human demonstrations with-
radial basis functions Υ, which improves the robot’s capability
out relearning based on a second-order differential equation
to follow any trajectory from initial position 𝑥 0 to goal position
[20]. DMP is used to learn complex discrete and rhythmic
𝑥 𝑔 . The force equation is calculated as follows:
movements and can generalize motion trajectories to adapt to
variable conditions, including changing initial or goal poses ÍΥ
𝑗=1 𝑊 𝑗 𝜓 𝑗 (𝑠)𝑠
while avoiding static obstacles. Here, we are interested in 𝑓 (𝑠) = ÍΥ (8)
discrete point-to-point movements of the two section ends of 𝑗=1 𝜓 𝑗 (𝑠)
a continuum robot.
where 𝑊 𝑗 represents the learned weights that are computed
The learning steps of the proposed IbMP approach start
based on a local regression approach, and 𝜓 𝑗 (𝑠) are radial
by capturing demonstrated tip pose trajectories and their
basis functions that are approximately equidistantly spaced and
derivative, respectively. Then, learning the tip trajectory in
calculated as follows:
Cartesian space is introduced, with orientation represented in
quaternion form. After that, to ensure synchronized motion and (
−0.5 𝑠−𝑐 𝑗 )2
avoid external perturbations, a phase variable 𝑠 is introduced −𝑤 2
𝜓 𝑗 (𝑠) = 𝑒 𝑗 (9)
into the differential equation for the pose error.
[0:Υ−1]
where 𝑐 𝑗 = 𝑒 − 𝛼𝑠 Υ−1 and 𝑤 𝑗 = (𝑐 ( 𝑗+1) − 𝑐 𝑗 ) for 𝑗 = 1, 2...Υ
A. IbMP representation of robot tip position represents the centers and widths of the Gaussian functions
The transformation system for 3D discrete point-to-point distributed along the motion phase [24].
motion while avoiding obstacles is described by the differential
equation of a spring-damper system with forcing function 𝑓 (𝑠) B. IbMP representation of robot tip orientation
as follows [21]:
Recently, there has been an increased interest in developing
motion planning algorithms concerning the orientation of
𝜏 𝑣¤ = 𝐾 𝑎 (𝑥𝑥 𝑔 − 𝑥 ) − 𝐷 𝑎 𝑣 + (𝑥𝑥 𝑔 − 𝑥 0 ) 𝑓 (𝑠) + 𝑃 (𝑥𝑥 , 𝑣 ) (4) continuum robots [25]. The robot’s tip orientation can be
represented in different forms such as rotation matrix, Euler
angles, and Axis-angle. In this regard, equations (4)-(5) can
𝜏 𝑥¤ = 𝑣 (5)
be reformulated to describe the tip orientation in these forms.
where 𝐾 𝑎 and 𝐷 𝑎 are tuning gains, which act like the elastic However, representation in such a way may suffer from loss
and damping terms respectively, 𝑠 represents a phase variable of DOF, discontinuity, and high computational load [14].

This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/
 This article has been accepted for publication in IEEE Robotics and Automation Letters. This is the author's version which has not been fully edited and
content may change prior to final publication. Citation information: DOI 10.1109/LRA.2023.3239306

4 IEEE ROBOTICS AND AUTOMATION LETTERS. PREPRINT VERSION. ACCEPTED JANUARY, 2023

The IbMP based quaternion can be represented similar to Learning weights Demonstrations
(4) and (5) as follows [26]: , , ,

𝜏 𝜂¤ = 2𝐾 𝑧 log(𝑞𝑞 𝑔 𝑞¯ ) − 𝐷 𝑧 𝜂 + K𝑑 𝑓 𝑞 (𝑠) (10) Transformation

System

1 Fig. 2: Schematic diagram Imitation based Motion Planning

𝜏 𝑞¤ = (𝜂 + 2𝐾 𝑑 𝑙𝑜𝑔(𝑞𝑞 𝑔 𝑞¯ ) + 𝑙𝑜𝑔(𝑞𝑞 𝑑 𝑞¯ ))𝑞𝑞 (11)
2 algorithm.
where 𝐾 𝑧 and 𝐷 𝑧 are tuning parameters, 𝑞 = 𝑞 0 + 𝑞 1𝑖 + 𝑞 2 𝑗 +
𝑞 3 𝑘 ∈ R4 represents the orientation in quaternion form, 𝑞 𝑔
is the goal orientation and 𝜂 = 𝜏𝜔 𝜔 , and 𝜂¤ = 𝜏 𝜔¤ are the After that, these learned weights are the core of computing the
quaternion derivatives which are related to the angular velocity actual forcing terms (8) and (12). Finally, by integrating (4)-(5)
𝜔 = 2 log(𝑞𝑞 𝑔 𝑞¯ ). Kd = log(R𝑔 R𝑇0 ) ∈ R3×3 is a diagonal scaling and (10)-(11), the actual position and orientation trajectories
matrix in which R0 and R𝑔 are the reference and goal ori- are easily obtained.
entation matrices. The 𝑓 𝑞 (𝑠) is a nonlinear forcing term that
allows the robot tip to track the orientation trajectory from the IV. Controller Design
initial orientation 𝑞 0 to the goal orientation 𝑞 𝑔 . Impedance control is characterized by regulating the dy-
ÍΥ namic behavior between the robot’s tip position and the exerted
𝑗=1 𝜇 𝑗 𝜓 𝑗 (𝑠)𝑠
𝑓 𝑞 (𝑠) = ÍΥ (12) force on the environment. Meanwhile, achieving accurate force
𝑗=1 𝜓 𝑗 (𝑠) tracking is not mandatory, unlike traditional hybrid posi-
tion/force control algorithms, which focus on position tracking
where 𝜇 𝑗 are the learned weights required to follow the
and force tracking separately. Therefore, impedance control
orientation trajectory.
represents a reasonable trade-off between position tracking
To achieve motion synchronization while learning both the
and force control, making it a suitable controller in many
position and orientation trajectories of the robot’s end-effector,
applications such as surgical operations.
the phase variable 𝑠 should combine the tracking error of
position and orientation to overcome the time dependency Interaction between the robot tip and the environment causes
problem. 𝑠 is computed as follows: shape deformation, leading to non-zero contact forces. Assum-
ing the deformed part of the environment could be represented
𝛼𝑠 𝑠
𝜏 𝑠¤ = (13) by a mass-spring-damper system as follows:
1 + 𝛼𝑟 𝑒 𝑥𝑞
where 𝛼𝑠 and 𝛼𝑟
are tuning gains, 𝑒 𝑥𝑞 = H 𝑒¥ + D𝑒¤ + K𝑒𝑒 = 𝐹 (16)
𝑒 𝑥0 + ∥𝑒𝑒 𝑥 ∥ + 𝑒 𝑞0 + 𝑒 𝑞 is the tracking error, in which where H ∈ R3×3 , D ∈ R3×3 , K ∈ R3×3 are inertia, damping,
𝑒 𝑥0 = 𝑥 𝑔 − 𝑥 and 𝑒 𝑥 = 𝑥 𝑑 − 𝑥 are the tip position errors in and stiffness matrices of the environment that are selected as
which 𝑥 𝑑 is the reference position trajectory, 𝑒 𝑞0 = 𝑞 𝑔 − 𝑞 symmetric positive definite to ensure that the system will be
and 𝑒 𝑞 = 𝑞 𝑑 − 𝑞 represents the orientation errors in which 𝑞 𝑑 asymptotically stable. 𝑒 = 𝑥 𝑑 − 𝑥 ∈ R3 is the error between the
represents the desired orientation. reference and actual position trajectories, 𝑒¤ = 𝑥¤ 𝑑 − 𝑥¤ in which
On the other hand, to overcome discontinuity and improve 𝑥¤ 𝑑 is the desired velocity and 𝑥¤ = J 𝑞¤ 𝑒 , and 𝑒¥ = 𝑥¥ 𝑑 − 𝑥¥ in which
motion smoothness, the goal position 𝑥 𝑔 in (4) is modified 𝑥¥ 𝑑 is the desired acceleration and 𝑥¥ = J¤ 𝑞¤ 𝑒 + J 𝑞¥ 𝑒 . It is worth
to vary continuously with time by integrating the following noting that the reference tip position 𝑥 𝑑 trajectory is changed
equation: during interaction which is represented by step disturbance for

simplicity.
𝜏 𝑥¤ 𝑔 = 𝛼𝑔 𝑥 𝑔0 − 𝑥 𝑔 (14) The impedance control equation in joint coordinates is
where 𝛼𝑔 is constant gain, 𝑥 𝑔0 is the new goal position. derived as follows [27]:

 
Similarly, the goal orientation 𝑞 𝑔 in (10) varies with time 𝚪 = M 𝑞 e J † 𝑎 + H −1 (D𝑒¤ + K𝑒𝑒 ) − J¤ 𝑞¤ 𝑒 + N 𝑞 𝑒 , 𝑞¤ 𝑒 𝑞¤ 𝑒 +


to keep up with orientation discontinuity that occurs due to

 
G 𝑞 𝑒 + M 𝑞 𝑒 J † H −1 − J 𝑇 𝐹


goal variation and is calculated as follows:
(17)
𝜏 𝑞¤ 𝑔 = 𝛼𝑔0 log(𝑞𝑞 𝒈0 𝑞¯ 𝑔 )𝑞𝑞 𝑔 (15)
where 𝑎 = 𝑥¥ 𝑑 is the desired acceleration. The block diagram
where 𝛼𝑔0 ∈ R4×4 is positive diagonal gain matrix and 𝑞 𝒈0 is of the dynamic impedance control is shown in Fig. 3.
the new goal orientation.
As depicted in Fig. 2, the robot’s tip pose variables 𝑥 , 𝑣 , 𝑣¤ ,
𝑞 , 𝜂 and 𝜂¤ are recorded at each time step 𝑡 = 0, 1, ....𝑇. Then, A. Stability analysis
the phase variable 𝑠 is computed by integrating the differential Substituting the driving torques 𝚪 (17) into (3), yields a
equation (13), then the target forces can be computed easily closed-loop dynamics in (16). Thus, the user-defined matrices
from (4) and (10) respectively. The problem is now addressed H, D and K determine the robot’s behaviors when subjected to
as a local weighted regression problem that could be solved external forces. In this work, we are concerned environment
based on linear least squares to get the weights 𝑊𝑖 and 𝜇𝑖 . with fixed stiffness for simplicity. To discuss the stability

This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/
 This article has been accepted for publication in IEEE Robotics and Automation Letters. This is the author's version which has not been fully edited and
content may change prior to final publication. Citation information: DOI 10.1109/LRA.2023.3239306

SELEEM et al.: IMITATION-BASED MOTION PLANNING AND CONTROL OF A MULTI-SECTION CONTINUUM ROBOT 5

Fig. 4: Capturing human demonstrations while moving the

flexible interface with Motion Capture System
Fig. 3: Block diagram of the dynamic impedance control.

analysis of (16), the Lyapunov candidate function is assumed

as follows:
1 1
¤ = 𝑒¤ 𝑇 H 𝑒¤ + 𝑒 𝑇 K𝑒𝑒
𝑉 (𝑒, 𝑒) (18)
2 2
Differentiating 𝑉 (𝑒, 𝑒)
¤ along the trajectories of (16) with H
constant yields:
𝑉¤ (𝑒, 𝑒)
¤ = 𝑒¤ 𝑇 H 𝑒¥ + 𝑒 𝑇 K 𝑒¤ (19) Fig. 5: The captured and filtered tip position trajectory.
By Substituting the value of 𝑒¥ from (16) in (19) with 𝐹 = 0
and symmetric stiffness matrix K, results: A. Data capture
𝑉¤ (𝑒, 𝑒) 𝑇 𝑇
¤ = 𝑒¤ (−D𝑒¤ − K𝑒𝑒 ) + 𝑒 K 𝑒¤ = −𝑒¤ D𝑒¤ 𝑇
(20) The two-section continuum robot is controlled via tele-
operation based on the motion of a flexible interface with
Equation (20) shows that 𝑉¤ is negative definite for positive similar kinematics for precise mapping, as shown in Fig.
diagonal matrix D. Hence, we can conclude stability if all 4. The interface consists of two sections attached in series;
diagonal values of the damping matrix are constant and each section is printed from the flexible filament (Ninja-flex
positive [28]. (1.75mm)) by using a 3D printer (MagiX MF-2500EP). As
depicted in Fig. 4, a polygon shape with six markers represents
V. Results and Discussion the pose at the end of each section of the robot. Due to its high
accuracy, simplicity, and auto-calibration feature, the motion
This section presents the simulation results of IbMP and
capture system (OptiTrack V120-trio) is used to capture each
dynamic impedance control as follows. First, capturing the
marker’s position and orientation relative to the base frame.
human demonstrations experimentally is introduced. Then,
Meanwhile, a cubic interpolation algorithm is implemented
multiple simulation scenarios are conducted to evaluate the
in the vision system (Motive software) to to compensate
performance of IbMP, including trajectory tracking, avoiding
automatically for missing data. At the same time, a Moving
obstacles and rejecting external disturbances. Additionally, the
Average Filter (MAF) is used to remove the noise in pose
robustness of IbMP is analyzed based on the Monte-Carlo
trajectories, as shown in Fig. 5.
approach. Finally, the performance of dynamic impedance
As depicted in Fig. 4, the learning process is started by
control of a two-section continuum robot while interacting
asking the user to move the flexible interface, each section of
with the environment is evaluated in terms of trajectory
which 16.18𝑐𝑚 long, for randomly inspection tasks from one
tracking. The simulated parameters of the IbMP approach and
pose to another while capturing the pose of the end of each
dynamic impedance control are listed in Table. I.
section of the robot by using the motion capture system.

Parameter Description Value

B. Learning of the tip position trajectories while avoiding
𝑑 robot center to the cable (2) 2cm
𝑥 𝑜𝑏 Obstacle position (6) [5.52, 25.42, 31.10]cm obstacles.
𝛾 No. of basis functions (8) 100 Based on equations (4), (5), and (13), the developed IbMP
𝜏 Scaling factor (4) 9.36 s
[𝐾𝑎 , 𝐷𝑎 ] Constant gains (4) [200, 50]
can regenerate the tip position trajectory from initial posi-
[𝛾, 𝜗 ] Tuning gains (6) [500, 1] tion 𝑥 0 = [7.54, 19.82, 31.58] cm to the goal position 𝑥 𝑔 =
[𝐾 𝑧 , 𝐷 𝑧 , 𝐾𝑑 ] Tuning gains (10)-(11) [500, 450, 0.01] [9.69, 15.07, 30.95] cm successfully, as shown in Fig. 6a.
[ 𝛼𝑠 , 𝛼𝑟 ] Tuning gains (13) [1, 1] While the error between the reference and learned trajectories
𝛼𝑔 Tuning gain (14) 10
[H,D, K ] Environment matrices(16) diag([0.01, 0.01, 0.01])
is shown in Fig. 6b. Figure 6a illustrates the ability of IbMP

K 𝑝 , K𝑣 Gain matrices (17) diag([500, 500, 500]) to reproduce complex movements accurately, which can be
observed at beginning and end of the demonstrated trajectory.
TABLE I: Simulation parameters of IbMP algorithm and Additionally, the generalization capability of the proposed
Impedance controller IbMP has been evaluated to reproduce novel trajectories by

This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/
 This article has been accepted for publication in IEEE Robotics and Automation Letters. This is the author's version which has not been fully edited and
content may change prior to final publication. Citation information: DOI 10.1109/LRA.2023.3239306

6 IEEE ROBOTICS AND AUTOMATION LETTERS. PREPRINT VERSION. ACCEPTED JANUARY, 2023

0.6
X
Y
0.4 Z

Error [cm]
0.2

-0.2 (a) (b) (c) (d)

-0.4 Fig. 8: Snapshots of the robot motion while following one of
0 2 4 6 8 10
Time [s]
the learned demonstrations.
(a) (b)
Fig. 6: In the diagrams, the position of the middle of (a) The
demonstrated and learned tip position trajectories and (b) The
corresponding tracking error.

(a) (b)
Fig. 9: Adaptation of IbMP to avoid: (a) static obstacle with
diameter 1 cm and 2 cm respectively., and (b) dynamic obstacle
with diameter 1 cm.
(a) (b)
Fig. 7: The adaptation of IbMP to learn novel movements by -0.96
0.2
-0.97
changing: (a) initial positions and (b) goal positions. 0.1
q0

q1
-0.98 0
-0.99 -0.1
-1 -0.2 0.04
0 5 10 0 5 10
Reference trajectory

changing the robot’s tip initial positions 𝑥 01 = [5, 19.82, 31.58], 0.1
Learn trajectory

Error [cm]
𝑥 02 = [3, 17.82, 31.58] and 𝑥 03 = [10, 17.82, 31.58], and new 0.04
0 0
0.02
q2

q3

goal positions 𝑥 𝑔1 = [9.69, 12, 30.95], 𝑥 𝑔2 = [9.69, 8, 30.95] 0

-0.1

and 𝑥 𝑔3 = [9.69, 4, 30.95] as indicated in Fig. 7a and Fig. -0.02 -0.2

0 5 10 0 5 10 -0.04
0 2 4 6 8 10
7b respectively. The generated movement has successfully Time [s] Time [s] Time [s]
adapted to the position trajectories without an additional
(a) (b)
learning phase. Figure 8 illustrates a motion simulation of the
robot’s tip while tracking one of the learned demonstrations Fig. 10: (a) The demonstrated and learned robot’s tip orienta-
using the Unity platform. In Unity, a kinematics model is built tion trajectories. and (b) The corresponding error difference.
for the two-sections continuum robot with the assumption that
the velocities of the robot are low and no need to incorporate
the robot’s dynamics. The code is made publicly available for C. Learning of the tip orientation trajectories while rejecting
visualization https://github.com/elhussieny/continuum_robot. external perturbations.
Furthermore, IbMP has avoided collision successfully with Similarly, IbMP has succeeded in reproducing
a static and a dynamic sphere-shaped obstacles with different the tip orientation trajectories based on equations
diameters that may exist within the pathway of the robot’s tip (10), (11), and (13) from initial orientation
before returning to the reference trajectory as shown in Fig. 𝑞 0 = [−0.002993, −0.01466, 0.01983, −0.99969] to the goal
9a and Fig. 9b respectively. It is worth mentioning that the orientation 𝑞 𝑔 = [0.09164, −0.001396, 0.063942, −0.99373]
position and velocity of obstacles are assumed here. However, is shown in Fig. 10a. The error between the
in real dynamic environments, the motion capture system of reference and actual orientation trajectories is shown
LiDar scanner could be used to estimate these variables. How- in Fig. 10b. Additionally, IbMP generates novel
ever, there is no guarantee that the proposed IbMP approach orientation trajectories by changing the goal orientation
can allow the whole robot to avoid obstacles. Due to the 𝑞 𝑔0 = [0.30, 0.649, 0.0779, 0.694], with optimized gain
compliance behavior of continuum robots, it is acceptable variable 𝛼𝑔0 = 𝑑𝑖𝑎𝑔 [767.67, 993.01, 999.95, 999.93]), as
to hit obstacles within reasonable ranges compared to rigid shown in Fig. 11. Figure 12a illustrates the capability of
robots in many applications. In this regard, future plans will IbMP in avoiding collisions and returning to the target
be proposed to apply the IbMP in actuation space to reliably pose after external perturbations. The error between the
generate similar shapes while considering the constraints of demonstrated and learned orientations after applying a step
cable length. disturbance is shown in Fig. 12b.

This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/
 This article has been accepted for publication in IEEE Robotics and Automation Letters. This is the author's version which has not been fully edited and
content may change prior to final publication. Citation information: DOI 10.1109/LRA.2023.3239306

SELEEM et al.: IMITATION-BASED MOTION PLANNING AND CONTROL OF A MULTI-SECTION CONTINUUM ROBOT 7

0.6 0.4

RMSE [cm]
0
0.4 0.3
q0

q1
-0.5 0.2 0.2
-1 0 0.1
0 5 10 0 5 10
0
0.2 0 4 8 12
0.1 0.5
q2

q3
50
0
40

x g0 [cm]
0 30
0 5 10 0 5 10
Time [s] Time [s] 20
10
Fig. 11: The adaptation of IbMP to learn new orientation goals.
0
0 4 8 12
Reference trajectory
Learn trajectory
Iteration number
0.4
-0.94
0.2
Fig. 13: (Top) The RMSE results of Monte-Carlo simulation
-0.96
q0

q1

0
scenarios at various gain values (𝐾 𝑎 , 𝐷 𝑎 ) with the correspond-
-0.98
ing goal position in Cartesian space (bottom).
-1 -0.2
0 5 10 0 5 10 0.04

0.02
0.1 0.2
35
Error [cm]

0
X-coordinate
0.05 -0.02 Y-coordinate
q2

q3

0 25 Z-coordinate
-0.04

Error [cm]
0
-0.06
-0.05 -0.2 15
0 5 10 0 5 10 -0.08
0 1 2 3 4 5
Time [s] Time [s] Time [s]
5

(a) (b)
-5
0 2 4 6 8 10
Fig. 12: (a) Adaptation of IbMP to react to an external Time [s]
disturbance., and (b) The corresponding errors during the (a) (b)
trajectory.
Fig. 14: (a) The reference and actual position trajectories while
interaction with environment and (b) The corresponding error.
D. Robustness analysis
Multiple simulation scenarios based on the Monte Carlo that the continuum robot’s tip interacted with the environment
approach [29] were carried out to analyze the stability and during the period 𝑡 = 5 : 6 s. For simplicity, this interaction is
performance of the IbMP approach in terms of a wide vari- represented as a step disturbance of amplitude 2 cm, which
ation of the goal position. In each scenario, 200 MATLAB means that the reference trajectory is changed to be 𝑥 𝑑 = 𝑥 𝑑 +
simulations were performed while changing the goal position 2 . The error between the reference and actual trajectories is
randomly within acceptable uniform distributions [4, 7, 20] 𝑇 ≤ shown in Fig. 14b.
[𝑥, 𝑦, 𝑧 cm] 𝑇 ≤ [11, 24, 32] 𝑇 . Due to limited space, only 10
simulations are evaluated and sorted based on Root Mean VI. Conclusion
Square Error (RMSE) calculations. Based on the suggested
gains 𝐾 𝑎1 = 200, 𝐷 𝑎1 = 50, figure 13 shows an insignificant In this research article, the Imitation-based Motion Planning
change in the RMSE values during simulations while reaching (IbMP) approach is developed for planning discrete complex
the goal position, which implies the performance of the pro- movements of a two-section continuum robot. The human
posed IbMP. Subsequently, Monte Carlo simulation scenarios demonstrations are captured while moving the flexible inter-
have been conducted to show the effect of changing the face, with similar kinematics to the main robot, in front of a
gain values (𝐾 𝑎 , 𝐷 𝑎 ) in (4) on the robustness of the motion capture system. The performance capability of IbMP

IbMP is not limited to regenerating the captured demonstrations but
algorithm. As shown in Fig. 13, the gains 𝐾 𝑎1 , 𝐷 𝑎1 give the
lowest RMSE values which implies the best
performance to
extends to producing novel movements by changing the initial
reach to the goal position. While 𝐾 𝑎2 , 𝐷 𝑎2 and 𝐾 𝑎6 , 𝐷 𝑎6 and goal positions, avoiding static and dynamic obstacles,
result in worst performance. As noted, the Monte Carlo method and rejecting external disturbances with no need to relearn
could guide the direction of choosing the optimal gains that the weights. The robustness of the proposed IbMP has been
enhance the system performance. successfully evaluated based on the Monte-Carlo algorithm
that could suggest the direction of selecting the optimal gains
for best performance. On the other hand, dynamic impedance
E. The performance of the dynamic impedance control control is developed to regulate the interaction forces between
The dynamic impedance control has successfully tracked the robot and environment while smoothly tracking the ref-
the tip position trajectory as shown in Fig. 14a. It is suggested erence position trajectory. The dynamic model of two section

This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/
 This article has been accepted for publication in IEEE Robotics and Automation Letters. This is the author's version which has not been fully edited and
content may change prior to final publication. Citation information: DOI 10.1109/LRA.2023.3239306

8 IEEE ROBOTICS AND AUTOMATION LETTERS. PREPRINT VERSION. ACCEPTED JANUARY, 2023

continuum robot is derived based on a Lagrangian formulation [12] B. Kenwright, “A beginners guide to dual-quaternions: what they are,
with Taylor expansion to avoid singularities due to specific how they work, and how to use them for 3d character hierarchies,” in
20th International Conference on Computer Graphics, Visualization and
configurations. The compliance behavior of soft continuum Computer Vision. Citeseer, 2012.
robots, IbMP approach, and dynamic impedance control en- [13] I. A. Seleem, S. F. Assal, H. Ishii, and H. El-Hussieny, “Guided pose
hance the performance of robot in many applications, such as planning and tracking for multi-section continuum robots considering
robot dynamics,” IEEE Access, vol. 7, pp. 166 690–166 703, 2019.
in the biomedical field that require a repeatable sequence of [14] I. A. Seleem, H. El-Hussieny, S. F. Assal, and H. Ishii, “Development
movements, high precision, and safety characteristics. and stability analysis of an imitation learning-based pose planning
The IbMP may suffer from an oscillatory behavior, espe- approach for multi-section continuum robot,” IEEE Access, vol. 8, pp.
99 366–99 379, 2020.
cially with volumetric obstacles. Additionally, a significant [15] H. El-Hussieny, I. A. Hameed, and J.-H. Ryu, “Nonlinear model pre-
computational time is required to adjust all of the tuning gains. dictive growth control of a class of plant-inspired soft growing robots,”
In future work, an obstacle avoidance approach based on a IEEE Access, vol. 8, pp. 214 495–214 503, 2020.
[16] M. K. Soltani, S. Khanmohammadi, F. Ghalichi, and F. Janabi-Sharifi,
dynamic potential field with super-quadratic function will be “A soft robotics nonlinear hybrid position/force control for tendon driven
developed to overcome the limitations of the current approach catheters,” International Journal of Control, Automation and Systems,
while allowing the whole robot to avoid volumetric obstacles vol. 15, no. 1, pp. 54–63, 2017.
[17] S. Xu, B. He, Y. Zhou, Z. Wang, and C. Zhang, “A hybrid position/force
not only in ambient space, but also in joint space. On the other control method for a continuum robot with robotic and environmental
hand, the Nonlinear Model Predictive Control (NMPC) will compliance,” IEEE Access, vol. 7, pp. 100 467–100 479, 2019.
also be developed to improve the controller performance while [18] C. Della Santina, R. K. Katzschmann, A. Biechi, and D. Rus, “Dynamic
control of soft robots interacting with the environment,” in 2018 IEEE
applying constraints on the cable lengths and the application International Conference on Soft Robotics (RoboSoft). IEEE, 2018, pp.
workspace volume. Furthermore, a practical validation of the 46–53.
IbMP approach with dynamic impedance control will be [19] A. Chawla, C. Frazelle, and I. Walker, “A comparison of constant
curvature forward kinematics for multisection continuum manipulators,”
conducted in a dynamic environment. in 2018 Second IEEE International Conference on Robotic Computing
(IRC). IEEE, 2018, pp. 217–223.
Acknowledgment [20] M. Ginesi, N. Sansonetto, and P. Fiorini, “Overcoming some drawbacks
of dynamic movement primitives,” Robotics and Autonomous Systems,
Ibrahim A. Seleem would like to acknowledge the Japan vol. 144, p. 103844, 2021.
Society for the Promotion of Science (JSPS) for granting him [21] A. Ude, B. Nemec, T. Petrić, and J. Morimoto, “Orientation in cartesian
space dynamic movement primitives,” in 2014 IEEE International
a scholarship to carry out his postgraduate studies at Waseda Conference on Robotics and Automation (ICRA). IEEE, 2014, pp.
University and fully support this research work. 2997–3004.
[22] M. Ginesi, D. Meli, A. Calanca, D. Dall’Alba, N. Sansonetto, and P. Fior-
References ini, “Dynamic movement primitives: Volumetric obstacle avoidance,” in
2019 19th international conference on advanced robotics (ICAR). IEEE,
[1] J. Kim, M. de Mathelin, K. Ikuta, and D.-S. Kwon, “Advancement of 2019, pp. 234–239.
flexible robot technologies for endoluminal surgeries,” Proceedings of [23] M. Chi, Y. Yao, Y. Liu, and M. Zhong, “Learning, generalization, and
the IEEE, 2022. obstacle avoidance with dynamic movement primitives and dynamic
[2] S. Jadhav, M. R. A. Majit, B. Shih, J. P. Schulze, and M. T. Tolley, potential fields,” Applied Sciences, vol. 9, no. 8, p. 1535, 2019.
“Variable stiffness devices using fiber jamming for application in soft [24] F. Stulp and G. Raiola, “Dmpbbo: A versatile python/c++ library for
robotics and wearable haptics,” Soft Robotics, vol. 9, no. 1, pp. 173–186, function approximation, dynamical movement primitives, and black-box
2022. optimization,” Journal of Open Source Software, vol. 4, no. 37, p. 1225,
[3] D. F. Gruber and R. J. Wood, “Advances and future outlooks in soft 2019.
robotics for minimally invasive marine biology,” Science Robotics, vol. 7, [25] T. da Veiga, J. H. Chandler, P. Lloyd, G. Pittiglio, N. J. Wilkinson, A. K.
no. 66, p. eabm6807, 2022. Hoshiar, R. A. Harris, and P. Valdastri, “Challenges of continuum robots
[4] S. M. Youssef, M. Soliman, M. A. Saleh, M. A. Mousa, M. Elsamanty, in clinical context: a review,” Progress in Biomedical Engineering, vol. 2,
and A. G. Radwan, “Underwater soft robotics: A review of bioinspiration no. 3, p. 032003, 2020.
in design, actuation, modeling, and control,” Micromachines, vol. 13, [26] T. Kastritsi, F. Dimeas, and Z. Doulgeri, “Progressive automation with
no. 1, p. 110, 2022. dmp synchronization and variable stiffness control,” IEEE Robotics and
[5] T. Greigarn, N. L. Poirot, X. Xu, and M. C. Çavuşoğlu, “Jacobian-based Automation Letters, vol. 3, no. 4, pp. 3789–3796, 2018.
task-space motion planning for mri-actuated continuum robots,” IEEE [27] T. Sun, L. Peng, L. Cheng, Z.-G. Hou, and Y. Pan, “Stability-guaranteed
robotics and automation letters, vol. 4, no. 1, pp. 145–152, 2018. variable impedance control of robots based on approximate dynamic in-
[6] J. Lai, B. Lu, Q. Zhao, and H. K. Chu, “Constrained motion planning of version,” IEEE Transactions on Systems, Man, and Cybernetics: Systems,
a cable-driven soft robot with compressible curvature modeling,” IEEE vol. 51, no. 7, pp. 4193–4200, 2019.
Robotics and Automation Letters, vol. 7, no. 2, pp. 4813–4820, 2022. [28] K. Kronander and A. Billard, “Stability considerations for variable
[7] H. Ravichandar, A. Polydoros, S. Chernova, and A. Billard, “Recent impedance control,” IEEE Transactions on Robotics, vol. 32, no. 5, pp.
advances in robot learning from demonstration,” Annual Review of 1298–1305, 2016.
Control, Robotics, and Autonomous Systems, vol. 3, pp. 297–330, 2020. [29] S. Bembli, N. K. Haddad, and S. Belghith, “Robustness analysis of an
[8] X. Li and O. Brock, “Learning from demonstration based on environ- upper-limb exoskeleton using monte carlo simulation,” in 2018 Inter-
mental constraints,” IEEE Robotics and Automation Letters, vol. 7, no. 4, national Conference on Advanced Systems and Electric Technologies
pp. 10 938–10 945, 2022. (IC_ASET). IEEE, 2018, pp. 78–84.
[9] D. Mukherjee, K. Gupta, L. H. Chang, and H. Najjaran, “A survey
of robot learning strategies for human-robot collaboration in industrial
settings,” Robotics and Computer-Integrated Manufacturing, vol. 73, p.
102231, 2022.
[10] H. Wang, J. Chen, H. Y. Lau, and H. Ren, “Motion planning based on
learning from demonstration for multiple-segment flexible soft robots
actuated by electroactive polymers,” IEEE Robotics and Automation
Letters, vol. 1, no. 1, pp. 391–398, 2016.
[11] M. S. Malekzadeh, J. F. Queißer, and J. J. Steil, “Multi-level control
architecture for bionic handling assistant robot augmented by learning
from demonstration for apple-picking,” Advanced Robotics, vol. 33,
no. 9, pp. 469–485, 2019.

This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/