# Learning Contact-aware CPG-based Locomotion in a Soft Snake Robot

Xuan Liu1, Cagdas Onal1, Jie Fu1 1Xuan Liu is a PhD student under the
supervision of Jie Fu and Cagdas Onal in the Robotics Engineering program at
Worcester Polytechnic Institute.xliu9, cdonal<EMAIL_ADDRESS>work was
supported in part by the National Science Foundation under grant #1728412.

###### Abstract

In this paper, we present a model-free learning-based control scheme for the
soft snake robot to improve its contact-aware locomotion performance in a
cluttered environment. The control scheme includes two cooperative
controllers: A bio-inspired controller (C1) that controls both the steering
and velocity of the soft snake robot, and an event-triggered regulator (R2)
that controls the steering of the snake in anticipation of obstacle contacts
and during contact. The inputs from the two controllers are composed as the
input to a Matsuoka CPG network to generate smooth and rhythmic actuation
inputs to the soft snake. To enable stable and efficient learning with two
controllers, we develop a game-theoretic process, fictitious play, to train C1
and R2 with a shared potential-field-based reward function for goal tracking
tasks. The proposed approach is tested and evaluated in the simulator and
shows significant improvement of locomotion performance in the obstacle-based
environment comparing to two baseline controllers.

##  I Introduction

Soft continuum robots have unique advantages in traversing within cluttered
and confined environments, due to their flexible body and deformable
materials. Applications of soft continuum robots can include search-and-rescue
[1], pipe inspection[2] and medical surgery [3]. In particular, comparing to
the mobility of the legged animals who can only use their feet as push points,
biological snakes have the unique advantage that any part of their body, if
properly controlled, could benefit from the propulsion force generated by the
contact with obstacles. In this paper, we investigate the following question:
How to design locomotion control for soft snakes that can intelligently adapt
to the contact with the obstacles and even benefit from propulsion forces in
an obstacle-based environment?

In literature, several research groups have studied this unique feature in
snake robot locomotion. Transeth et al.[4] first defined this feature as the
obstacle-aided locomotion, which means that the snake robot actively utilizes
external objects other than flat ground to generate propulsion during the
locomotion. In their pioneering work, they proposed a two-module task
framework of the obstacle-aided locomotion with: (a) a path planner that looks
for a trajectory with more active contact chance for the rigid snake robot,
and (b) a motion controller that controls the snake robot’s real-time body
movements to optimally utilize the contacts between the robot and the
environment and generate desired propulsion force for the locomotion. In [5],
a hybrid controller is developed, where a contact event is detected and
calculated to trigger a reactive control that maximizes the total propulsion
force at the moment. This controller has been applied to a rigid snake robot
and showed its reliability in maintaining beneficial propulsion force. When
using this controller, a big challenge is that rigid snake robots can at times
get jammed by obstacles. To handle this issue, Liljeback et al.[6] proposed to
detect a predefined jam event and resolve it by rotating the contacted links
to increase the total propulsive contact force. Kano et al.[7] proposed local
reflexive mechanisms that interrogate the contact status between a soft-bodied
scaffold snake robot and the obstacles to determine whether the contact is
beneficial to the locomotion. However, the soft body in [7] is not real but an
approximation by increasing the degrees of freedom of each joint by using
linear actuators.

So far, the theoretical base of obstacle-aided locomotion[8] is mainly studied
and implemented with rigid snake robots. It is appealing to enable such a
capability for soft snakes due to the important tasks that are not feasible
for rigid robots. However, few studies have investigated this control problem
for soft snake robots. Moving from rigid snake robots to soft snake robots has
a number of challenges: Firstly, the pneumatic actuators in soft snake robots
have nonlinear, delayed, and stochastic dynamical response given inputs,
making it difficult to achieve fast responses comparing to rigid snake robot.
Secondly, it is challenging to construct accurate kinematic/dynamic models for
a soft snake robot, due to the continuum of the pneumatic actuators. Thus, the
aforementioned model-based design will not be feasible for soft robots.

To this end, we develop a learning-based control method for obstacle-aided
locomotion in soft snake robots. Using the limited perception of contact, this
controlled robot will be aware of the existence of obstacles in the vicinity
and react correctly by either taking obstacle-avoidance movement or obstacle-
aided locomotion to achieve optimal goal tracking performance. This work is
built on our previous work on soft snake robot [9], in which we established a
model-free closed-loop goal tracking controller using central pattern
generator (CPG) and reinforcement learning (RL). This controller is featured
by a hierarchical framework where the RL controller learns to control the
input to the CPG network, and the output of the CPG network will generate
actuator inputs to the soft snake robot to achieve smooth and rhythmic
locomotion in an obstacle-free environment. To enable reactive motions in a
cluttered environment, we introduce a new control module–an event-triggered
neural-network regulator, and design a game-theoretic approach that trains
jointly the original RL controller and the new event-triggered regulator to
achieve the optimal obstacle-aided locomotion. Intuitively, we view the two
controllers as two players in a cooperative game, with a shared goal to
achieve successful and fast goal-tracking. Figure 1 provides a schematic
overview of the control design.

Learning-based control for force interaction has recently been studied. The
authors [10] have shown that a fully proprioceptive RL agent can learn four-
legged locomotion on cluttered terrain without extra sensors by including
additional memory of history actions. Another work shows that a simulated
ribbon eel could traverse the cluttered environment with RL controller learned
in the free space[11], under the assumption of perfect actuation without any
noise or delay. However, both controllers do not incorporate the contact
information into the feedback. Our learning-based control design has the
unique features of: 1) an event-triggered controller that reacts to contact
force and the prediction of contact; 2) a combination of reinforcement
learning and CPG to enable flexible and diverse locomotion patterns with
smooth trajectories like biological systems; 3) a game-theoretic training
process that achieves fast adaptation from controllers trained in an obstacle-
free environment to two “cooperative” controllers in a cluttered environment.

Figure 1: Schematic view of learning based CPG controller.

The paper is organized as follows. First, we provide an overview of our
schematic design of a soft snake robot equipped with contact sensors, as well
as the state space representation in this work in Section II. In Section III,
we propose a learning-based control framework and game-theoretic training
process. In Section IV, we design an experiment to showcase the improvement of
the snake robot’s locomotion performance in an obstacle-based environment.
Section V concludes and discuss future work.

##  II System Overview

A full snake robot consists $n$ pneumatically actuated soft links. Each soft
link of the robot is made of Ecoflex™ 00-30 silicone rubber. The links are
connected through rigid bodies enclosing the electronic components that are
necessary to control the snake robot. In addition, the rigid body components
have a pair of one-direction wheels to model the anisotropic friction of real
snakes. Only one chamber on each link is active (pressurized) at a time.

In [12], we developed a physics-based simulator that models the inflation and
deflation of the air chamber and the resulting deformation of the soft bodies
with tetrahedral finite elements. The simulator runs in real-time using GPU.
Between the soft bodies, we have rigid bodies for mounting the electronic
circuits, valves, and sensors. We use the simulator for generating data and
learning the locomotion controller for the soft snake robot. The learned
controller is then applied to the real snake robot. We have shown that with
the high-fidelity simulator, the controller has a similar performance in both
simulations and the real robot [12, 9].

Figure 2: The 3D model of rigid head (left) and rigid body (right). The
regions marked red are places installed with contact sensors. Figure 3: Free
body contact force composition of $i$-th rigid body.

To enable force sensing and obstacle-aided locomotion, we add four point
contact force sensors on two sides of each rigid body. The placement of these
contact sensors is illustrated in Fig. 2. Inspired by a previous study on
obstacle-aided locomotion of rigid snake robots [6], we approximate the total
contact force acted on each rigid body based on the inputs sampled from the
force sensors. As Fig. 3 shows, each pair of sensors $A,D$ and $B,C$ are
designed to have their diagonal line perpendicular to their sensing surfaces.
This allows us to simplify the force representation by subtracting the two
sensory inputs from a pair of diagonal sensors. Therefore, for the $i$-th
rigid body (counted from the head as $1$st rigid body), we have

$\displaystyle f_{2i-1}=f_{iB}-f_{iC},$ (1) $\displaystyle
f_{2i}=f_{iD}-f_{iA}.$

For simplicity, we use a vector

$\mathbf{f}=[f_{1},f_{2},f_{3},f_{4},f_{5},f_{6},f_{7},f_{8},f_{9},f_{10}]^{T},$

to represent the vector of contact forces in a four-link soft snake robot with
five rigid parts (including the head). This force representation generalizes
to $n$-link soft snakes, with $n\geq 4$.

Figure 4: Notation of the state space configuration of the robot.

The positional state space configuration (Fig. 4) of the robot is the same as
our previous setting in [9].

##  III Main Results

In this section, we describe our design of learning-based controller for
obstacle-aided navigation with the soft snake robot. In section III-A, we
first review the Central Pattern Generator (CPG) system used for a learning-
based control in obstacle-free environments. In section III-B, we present a
reinforcement learning (RL) architecture to the locomotion control in a
cluttered environment with multiple obstacles. We introduce two cooperative
controllers, one is the original RL controller used in an obstacle-free
environment, and another is an event-triggered RL regulator. Based on the
theory and solution concepts of potential games, the proposed RL algorithm
learns the Nash equilibrium using fictitious play.

###  III-A Learning-Based Controller with Matsuoka CPG Network

This section reviews the model-free RL method for soft-snake locomotion in an
obstacle-free environment, presented in our recent work [9]. Our control
design employs Matsuoka oscillators as primitive nodes to construct a CPG
network whose outputs translate to actuation inputs to the soft snake robot.

Primitive Matsuoka CPG: A primitive Matsuoka CPG consists a pair of mutually
inhibited neuron models. The dynamical model of a primitive Matsuoka CPG is
given as follows,

$\displaystyle\begin{split}&K_{f}\tau_{r}\dot{x}_{i}^{e}=-x_{i}^{e}-az_{i}^{f}-by_{i}^{e}-\sum_{j=1}^{N}w_{ji}y_{j}^{e}+u_{i}^{e},\\\
&K_{f}\tau_{a}\dot{y}_{i}^{e}=z_{i}^{e}-y_{i}^{e},\\\
&K_{f}\tau_{r}\dot{x}_{i}^{f}=-x_{i}^{f}-az_{i}^{e}-by_{i}^{f}-\sum_{j=1}^{N}w_{ji}y_{j}^{f}+u_{i}^{f},\\\
&K_{f}\tau_{a}\dot{y}_{i}^{f}=z_{i}^{f}-y_{i}^{f},\end{split}$ (2)

where the subscripts $e$ and $f$ represent variables related to extensor
neuron and flexor neuron, respectively. The tuple $(x_{i}^{q},y_{i}^{q})$,
$q\in\\{e,f\\}$ represents the activation state and self-inhibitory state of
$i$-th neuron respectively, $z_{i}^{q}=g(x_{i}^{q})=\max(0,x_{i}^{q})$ is the
output of $i$-th neuron, $b\in\mathbf{R}$ is a weight parameter,
$u_{i}^{e},u_{i}^{f}$ are the tonic inputs to the oscillator, and
$K_{f}\in\mathbf{R}$ is the frequency ratio. The set of parameters in the
system includes: the discharge rate $\tau_{r}\in\mathbf{R}$, the adaptation
rate $\tau_{a}\in\mathbf{R}$, the mutual inhibition weights between flexor and
extensor $a\in\mathbf{R}$ and the inhibition weight $w_{ji}\in\mathbf{R}$
representing the coupling strength with the neighboring primitive oscillator.
In our system, all coupled signals including $x_{i}^{q},y_{i}^{q}$ and
$z_{i}^{q}$ ($q\in\\{e,f\\}$) are inhibiting signals (negatively weighted),
and only the tonic inputs are activating signals (positively weighted).

In [9], we show that steering control can be achieved by imbalanced tonic
inputs. The steering bias of a primitive Matsuoka oscillator is proportional
to the amplitude of $u^{e}$ when $u^{e}$ and $u^{f}$ are exclusive within the
range $[0,1]$, that is $u^{f}=1-u^{e}$. As the tonic inputs have to be
positive in Matsuoka oscillators, we define a four dimensional action vector
$\mathbf{a}=[a_{1},a_{2},a_{3},a_{4}]^{T}\in\mathbf{R}^{4}$ and map
$\mathbf{a}$ to tonic input vector $\mathbf{u}$ as follows,

$u_{i}^{e}=\frac{1}{1+e^{-a_{i}}},\text{ and }u_{i}^{f}=1-u_{i}^{e},\text{ for
}i=1,\ldots,4.$ (3)

This mapping bounds the tonic input within $[0,1]$. The input vector
$\mathbf{u}$ for the 4-link snake robot is an eight-dimension vector,

$\mathbf{u}=[u_{1}^{e},u_{1}^{f},u_{2}^{e},u_{2}^{f},u_{3}^{e},u_{3}^{f},u_{4}^{e},u_{4}^{f}]^{T}.$

We use the notation $\mathbf{u^{e}}$ or $\mathbf{u^{f}}$ to represent the
vector of all extensor or flexor elements of $\mathbf{u}$ respectively, i.e.,

$\mathbf{u^{e}}=[u_{1}^{e},u_{2}^{e},u_{3}^{e},u_{4}^{e}]^{T},\quad\mathbf{u^{f}}=[u_{1}^{f},u_{2}^{f},u_{3}^{f},u_{4}^{f}]^{T}.$

From our previous work [9], we have discovered that the velocity control of
the snake robot can be achieved by the frequency ratio $K-f$-a unique feature
of Matsuoka CPG networks. Thus, we determine the encoded input vector of the
CPG net to be tonic input vector $\mathbf{u}$ for steering control and
frequency ratio $K_{f}$ for velocity control.

This input vector of the CPG is the output vector of the Neural Network (NN)
controller. The input to the NN controller is the state feedback of the robot,
given by
$\mathbf{s}=[\rho_{g},\dot{\rho_{g}},\theta_{g},\dot{\theta_{g}},\kappa_{1},\kappa_{2},\kappa_{3},\kappa_{4}]^{T}\in\mathbf{R}^{8}$.

We name the NN controller as the RL controller 1 (C1) and the proposed scheme
that connects R1 and CPG net as a PPOC-CPG [9]. The RL controller is trained
with the option-critic method [13] to control both tonic inputs $\mathbf{u}$
and frequency ratio $K_{f}$ in the CPG system.

###  III-B Event-triggered RL regulator

The PPOC-CPG controller learned in obstacle-free environments does not take
contact sensory information as its feedback to modulate the control of the
soft snake robot. To enable contact-awareness and force feedback in closed-
loop control, we introduce an event-triggered controller, called R2. An event-
triggered controller only outputs actuator signals when an event-triggering
condition is satisfied.

First, we define the _event-triggering condition_ as follows: At each time
step, given the contact force vector $\mathbf{f}$ and the distance
$d_{o}\in\mathbf{R}$ between the head of the snake robot and the nearest
obstacle. Define $D\in\mathbf{R}$ as a detection threshold that limits the
spatial sensing range of the robot. The event-triggering condition for the
contact-aware scenario is $\lVert f\rVert\neq 0$ or $d_{o}<D$. When the event-
triggering condition is satisfied, the contact-aware regulator is triggered to
join the manipulation of the CPG system.

In the obstacle-based locomotion scenario, there are in total $26$ observation
states $\mathbf{\zeta}=\\{\zeta_{1},\zeta_{2},...,\zeta_{26}\\}$, where
$\zeta_{1:4}$ represents the dynamic state of the robot referenced on the goal
position, $\zeta_{5:8}$ represents the real-time body curvature of the $4$
soft links, $\zeta_{9:14}$ contains actions in the last time step including
the previous option and the terminating probability, $\zeta_{15:24}$ contains
the pre-processed contact forces, and $\zeta_{25:26}$ are the distance and
relative angle to the closest obstacle in the environment.

The regulator is also a NN controller that takes the total observation states
and is trained using the actor-critic method. Specifically, the RL regulator
and the RL controller (C1) share the control of the tonic inputs to the CPG
system, as described next. However, only the RL controller can control the
frequency ratio of the CPG.

Linear tonic input composition between C1 and R2: According to the bias
property of the Matsuoka oscillator[9], let $\Delta
u_{i}=u_{i}^{e}-u_{i}^{f}$, it can be shown that

$bias(\Delta u_{i})\propto bias(\psi_{i}),$

where $\psi_{i}$ is the output wave of the Matsuoka oscillator. That is, the
bias between the extensor and flexor tonic inputs is linearly proportional to
the oscillation bias of the output wave of the Matsuoka oscillator. This
property implies that a linear composition of tonic input signals generated
from different sources yields a linear composition of oscillation bias,
regardless of the oscillation feature of each source signal. Inspired by this
result, the event-triggered RL regulator can manipulate the bias of tonic
input through direct linear superposition to the control signal generated by
the RL controller.

Let $\mathbf{a}_{1}$ represent the action vector generated by the controller
(C1) and $\mathbf{a}_{2}$ represent the action vector generated by the
regulator respectively. According to (3), the raw actions needs to be further
decoded into tonic input vectors. Thus we use $\mathbf{u_{k}}$ to represent
the tonic inputs generated from $\mathbf{a_{k}}$, $k=1,2$. We have the final
tonic input for the CPG network as a linear composition of the tonic inputs by
C1 and R1,

$\mathbf{u}=w_{1}\mathbf{u_{1}}+w_{2}\mathbf{u_{2}},$ (4)

where $w_{1}$ and $w_{2}$ are positive constant weights satisfying
$w_{1}+w_{2}=1$, and $\mathbf{u}$ is the vector representing total tonic
inputs to the CPG network[9].

###  III-C Fictitious cooperative game for learning-based control

The learning-based control design in an obstacle-free environment [9] is
formulated as a reinforcement learning in a Markov decision process, which is
a single-agent decision-making problem. In obstacle-aided locomotion, by
introducing the event-triggered RL regulator, we propose to model the
interaction between the robot and its environment/obstacles as a _two-player
cooperative_ game $G=(S,A_{1}\cup A_{2},P,R,\gamma)$ where $S$ is the set of
states, $A_{1}$ is the action set of player 1(RL controller, C1) and $A_{2}$
is the action set of player 2 (RL regulator, R2). The transition function
$P:S\times A_{1}\times A_{2}\rightarrow\Delta(S)$ is unknown. The two players
share the same reward function $R:S\times A_{1}\times
A_{2}\rightarrow\mathbf{R}$ and $\gamma\in(0,1)$ is the discounting factor.
Specifically, the two inputs from C1 and R2 are combined through a weighted
sum (4).

In this cooperative game, players share the common reward function and aim to
maximize the discounted sum of rewards. Let $\pi_{i}$ be the strategy of
player $i$, for $i=1,2$. The payoff of player $i$ given a strategy profile
$(\pi_{1},\pi_{2})$ and the initial state $s\in S$ is

$V_{i}(s,\pi_{1},\pi_{2})=\sum_{t=0}^{\infty}\gamma^{t}\mathop{\bf
E{}}\nolimits(R_{t}\mid\pi_{1},\pi_{2},s_{0}=s).$

where $R_{t}$ is the reward obtained at the $t$-th step in the stochastic
process induced by the strategy profile.

Because the reward function is common to both players, we have
$V_{1}(s,\pi_{1},\pi_{2})=V_{2}(s,\pi_{1},\pi_{2})$ for all $s\in S$ and all
possible strategy profiles. The cooperative game is a special case of
potential game where the payoff function $V_{i}$ equals the potential function
$V$.

The Nash equilibrium is a tuple $(\pi_{1}^{\ast},\pi_{2}^{\ast})$ such that

$V_{i}(s,\pi_{i}^{\ast},\pi_{j}^{\ast})\geq
V_{i}(s,\pi_{i},\pi_{j}^{\ast}),\text{ for }(i,j)\in\\{(1,2),(2,1)\\}.$

To enable learning in the cooperative game, we employ fictitious play. First,
we introduce the notion of perturbed best-response [14] strategy for player
$i$ given player $j$’s strategy $\pi_{j}$, which is

$\displaystyle\pi_{i}^{BR}(s,a)\propto\exp(\lambda R(s,a,\pi_{j}(s))+$
$\displaystyle\mathop{\bf
E{}}\nolimits_{s^{\prime}}\left[V_{i}(s^{\prime},\pi^{BR}_{i},\pi_{j})\right]),$
$\displaystyle\forall s\in S,\forall a\in A_{i},$

where $\lambda>0$ is a pre-defined constant and $\propto$ is the symbol for
“proportional to”.

The perturbed best-response strategy for player $i$ is the (near-) optimal
policy in the Markov decision process obtained from the game $G$ by
marginalizing out the other player’s strategy.

Next, we describe our design of smooth fictitious play to learn the Nash
equilibrium in the Markov game: At the round $0$, we initialize the RL
controller to be the trained goal-tracking controller $\pi_{1}^{0}$ in an
obstacle-free environment and fix the RL controller. We then train the RL
regulator until it converges to the perturbed best-response strategy. Let $n$
denote the inner episode of the RL algorithm and $V_{i}^{n}(s)$ is the reward
acquired at episode $n$ in the RL learning process. Upon the convergence of
episode reward ($||V_{i}^{n}(s)-V_{i}^{n-1}(s)||\leq\epsilon$) measured by the
RL algorithm (PPO Learning), a new round starts and the RL controller learns
to best-respond to the current strategy of RL regulator. This process iterates
for a number of rounds until both controllers converge.

It is noted that we employ the softmax instead of hardmax in the computation
of the perturbed best response. This is needed to avoid the sensitivity issues
in fictitious play. Besides, the response of one player is calculated given
the knowledge of the opponent’s strategy instead of the empirical estimate of
the opponent’s strategy. Our approach is a class of multi-agent RL, in which
each agent employs RL algorithms treating other agents as part of the
probabilistic environment. Due to the potential game formulation, the
convergence to the Nash equilibrium is ensured using the fictitious play [14].

Finally, the learning algorithm is concluded in Algorithm 1 below.
$\pi_{1}^{0}$ is the policy of C1 which has been initially trained in an
obstacle-free environment, while $\pi_{2}^{0}$ is a random policy for R2 at
the beginning. The two policies are first evaluated in the obstacle-based
training environment in order to initialize the value function $V^{0}$. We use
a flag to determine which players should be updated at the current iteration.
In the algorithm, the updating flag switches when the actor-critic learning
process exceeds a manually set terminating time. For each macro iteration step
$i$, only one of the players is updated, while the other one has all its
parameters frozen. The algorithm terminates until the improvement of the value
function in the new iteration is below a certain threshold $\epsilon$ or an
upper bound on the total number of iterations is reached.

Result: Return a (near-)optimal policy pair: $\pi_{1}^{N}$ for the controller
(C1) and $\pi_{2}^{N}$ for the regulator (R2).

Input: An initial policy $\pi_{1}^{0}$ for C1 and $\pi_{2}^{0}$ for R2, two
threshold constant $\epsilon_{i},i=1,2$, an upper bound $N$ on the number of
iterations

1 $V^{0}\leftarrow\textnormal{{PolicyEval}}(\pi_{1}^{0},\pi_{2}^{0})$;
$\triangleright$ Simulation-based policy evaluation.

2 $\triangleright$ The superscript is the number of the macro iteration.
$\textsc{flag}\leftarrow$ R2;

3 for  _$i=1,\ldots,N$_ do

4 if  _$\textsc{flag}=$ C1_ then

5 $(V^{i},\pi_{1}^{i})\leftarrow\textnormal{{PPO Learning}}(G,\pi_{2}^{i-1})$;
$\triangleright$ Fix the policy for R2, and employ RL to learn the controller
C1

6 $\textsc{flag}\leftarrow$ R2;

7 else

8 $(V^{i},\pi_{2}^{i})\leftarrow\textnormal{{PPO Learning}}(G,\pi_{1}^{i-1})$;
$\triangleright$ Fix the policy for C1, and employ RL to learn the regulator
R2

9 $\textsc{flag}\leftarrow$ C1;

10 end if

11 if  _$\lVert V^{i}(s_{0})-V^{i-1}(s_{0})\rVert\leq\epsilon$_ then

12 return $(\pi_{1}^{i},\pi_{2}^{i})$;

13

14 end if

15

16 end for

Algorithm 1 Fictitious cooperative game to train the controller and the
regulator.

###  III-D Design of the shared reward function

Figure 5: Illustration of obstacle-aware locomotion scenario with goal
reaching tasks.

We now present our design for the reward function shared by C1 and R2. Our
design will ensure that by maximizing the discounted sum of reward, the
learned controller can achieve efficient locomotion and accurate set-point
tracking.

To improve learning efficiency, we employ a potential field-based reward
function. Artificial potential field (APF) is widely applied in planning
problems and potential game theory [15, 16, 17] to accelerate the process of
searching for the optimal strategy.

In this paper, we use a simple APF to formulate the shared reward function for
both the controller agent and the regulator agent. The potential field can be
classified into two categories – the attracting field for target reaching and
the repulsive field for obstacle avoidance. The attracting field function is
defined as follows

$U_{att}(\mathbf{p})=\frac{1}{2}k_{att}||\mathbf{p}-\mathbf{p}_{g}||^{2},$

where $\mathbf{p}$ is the coordinate of the agent and $\mathbf{p}_{g}$ is the
coordinate of the goal. Coefficient $k_{att}$ is a positive constant
indicating the strength of the potential field. Since the attracting gravity
is always pointing toward the goal coordinate from any position of the map,
the value of gravity force should be negative. By taking the negative gradient
of $U_{att}$, we have the attracting force function

$\mathbf{F}_{att}(\mathbf{p})=-\nabla
U_{att}=-k_{att}(\mathbf{p}-\mathbf{p}_{g}).$

For obstacle avoidance purposes, we adapt the repulsive potential function
named FIRAS defined in[15]. The repulsive potential function of the $i$-th
obstacle in the environment is represented as

$U_{rep}(\mathbf{p};i)=\begin{cases}\frac{1}{2}k_{rep}(\frac{1}{\rho_{i}}-\frac{1}{\rho_{0}})^{2},\quad\textit{if
}\rho_{i}\leq\rho_{0}\\\ 0\quad\textit{if }\rho_{i}>\rho_{0}.\end{cases}$

where $\rho_{i}=||\mathbf{p}-\mathbf{p}_{o_{i}}||$ indicates the shortest
distance from the robot to the $i$-th obstacle, and $\rho_{0}$ is a threshold
constant indicating that the repulsive field is only effective in the vicinity
of an obstacle.

When there are multiple obstacles, the total potential force is supposed to be
the summation of all potential forces generated by all obstacles involved in
the task,

$\displaystyle\mathbf{F}_{rep}(\mathbf{p})$
$\displaystyle=\sum_{i=0}^{n}F_{rep}(\mathbf{p};i)=\sum_{i=0}^{n}\nabla
U_{rep}(\mathbf{p};i)$
$\displaystyle=\sum_{i=0}^{n}k_{rep}(\mathbf{p}-\mathbf{p}_{o_{i}})\frac{1/\rho_{i}-1/\rho_{0}}{\rho_{i}^{3}}.$

The reward is designed to encourage the goal-reaching and following the guide
by the artificial potential field. We design the reward to be composed of
three rewards:

$R=\omega_{1}R_{goal}+\omega_{2}R_{att}+\omega_{2}R_{rep},$ (5)

where $\omega_{i},i=1,2,3$ are constant weights. $R_{goal}$ is the termination
reward for reaching a circular accepting area centered at the goal.

$R_{goal}=\cos\theta_{g}\sum_{k=0}^{i}{\frac{1}{l_{k}}\mathbf{1}(\rho_{g}<l_{k})}.$

where $\theta_{g}$ is the deviation angle between the locomotion direction of
the snake robot and the direction of the goal, $l_{k}$ defines the radius of
the accepting area in task-level $k$, for $k=0,\ldots,i$.
$\rho_{g}=\lVert\mathbf{p}-\mathbf{p}_{g}\rVert$ is the linear distance
between the head of the robot and the goal, and $\mathbf{1}(\rho_{g}<l_{k})$
is an indicator function to determine whether the robot’s head is within the
accepting area of the goal. $R_{att}$ (resp. $R_{rep}$) is the reward function
of the attracting potential field (resp. repulsive potential field):

$R_{att}=\mathbf{v}\cdot\mathbf{F}_{att}(\mathbf{p}),\quad
R_{rep}=\mathbf{v}\cdot\mathbf{F}_{rep}(\mathbf{p}),$

where $\mathbf{v}$ is the velocity vector. The dot product between
$\mathbf{v}$ and the potential field vector represents the extent of the
agent’s movement on following the potential-flow in the task space. In this
reward design, though the repulsive potential function generates a cost for
the contact, its combination with the other two reward terms may encourage the
contact especially when the contact force can aid the locomotion. We discuss
this aspect in the experimental validation.

##  IV Experiments

###  IV-A Basic Settings

We use a 4-layer neural network with $128\times 128$ hidden layer neurons for
both controller (C1) and regulator (R2). For the total $26$ observation states
$\mathbf{\zeta}=\\{\zeta_{1},\zeta_{2},...,\zeta_{26}\\}$, C1 only takes
$\zeta_{1:14}$ as the inputs to the policy network, while R2 takes all
observation states $\zeta_{1:26}$ as its inputs. The outputs for C1 includes
an action vector $\mathbf{a}_{1}$ to control the tonic inputs of the CPG
network, and an option vector $\mathbf{o}=[o,\beta]^{T}$ to control the change
of frequency parameter of the neural oscillator. The outputs for R2 contain
only the action vector $\mathbf{a}_{2}$ adding to the tonic inputs. The hyper-
parameters for the policy network and value network in both C1 and R2 are the
same as our previous work[9].

#### Task specification

As Fig. 5 shows, the robot is required to traverse a group of obstacles and
reach the goal sampled in a certain range of distances and deviation angles
from the initial position and heading of the robot. The dotted circle around
the goal indicates the accepting area, which means that the robot needs to be
close enough to the goal in order to succeed and receive a terminal reward. At
each time step, the robot also receives a reward from the potential field
defined in Section III-D. If the agent reaches the accepting region of the
current goal, a new goal is randomly sampled. There are two failing
situations, where the desired goal will be re-sampled and updated. The first
situation is starving, which happens when the robot is jammed for a certain
amount of time. The starvation time for failing condition is $900\text{ ms}$.
The second case is missing the goal, which happens when the robot keeps
heading a wrong direction as oppose to moving towards the goal for a certain
amount of time. Specifically, in our experiment, if the linear velocity of the
snake robot stays negative on the goal direction for over $60$ time steps,
then we consider that the robot misses the goal.

#### Training Environment

Figure 5 shows a snapshot of the training environment with a $3\times 3$
obstacle maze for each goal reaching task. The distance between the robot and
the goal is fixed to $1.5$ meters. The deviation angle between the snake robot
and the goal is initially sampled from $0$ degree to $60$ degrees with a
uniform distribution. The base distance between every two obstacles is $0.08$
meters. The coordinate of each obstacle is added by an additional clipped
standard Gaussian noise ($\omega\sim\mathcal{N}(0,1)$, clipped by
$-0.01<\omega<0.01$).

#### Fictitious play results

Figure 6: Learning process of fictitious cooperative game with PPOC-CPG
compared with vanilla PPOC-CPG, and vanilla PPO, all three methods are
recorded in an obstacle-based training environment.

Figure 6 illustrates the learning process of Alg. 1 applied to the obstacle-
aware locomotion learning of the soft snake robot. First, the initial policies
$\pi_{1}^{0},\pi_{2}^{0}$ for C1 and R2 are evaluated in the obstacle-based
environment, during which the parameters of both $\pi_{1}^{0}$ and
$\pi_{2}^{0}$ are frozen. Particularly, $\pi_{1}^{0}$ has its policy network
pre-trained in an obstacle-free environment[9]. This step is to obtain a
proper estimation of the approximated value function of the initial method in
the new environment. After that, the actor-critic algorithm (PPO) only updates
R2’s parameters in macro iteration $1$, which is the first ”R-phase” in Fig.
6. Then the algorithm iterates until the difference of value function between
two consecutive iterations drop below $\epsilon=10$ in our experiment setting.
It is noticed that every time after the switch, the expected episode reward
decreases slightly. This is because the learning player’s initial strategy has
not converged to the best response given the new environment (due to the
change in the policy of the other player).

Besides, we compare our proposed controller with a single PPOC-CPG that trains
$\pi_{1}$ of the controller in the obstacle-based environment. It is observed
that the expected episode reward keeps dropping after the evaluation phase due
to the exploration in the new environment. Then the reward recovers to the
value that is close to the end of the evaluation phase, but never has a chance
to keep improving and surpass the initial performance. This shows the
significance of the regulator on improving the overall performance in the
environment with multiple obstacles.

TABLE I: Performance Comparison

Method | Jam ratio |  Avg. linear velocity  ($m/s$) | Success rate |  Avg. time per goal ($s$)
---|---|---|---|---
obstacle-aware PPOC-CPG | 0.165 | 0.139 | 0.91 | 36.1
obstacle-free PPOC-CPG | 0.235 | 0.124 | 0.82 | 40.8
Vanilla PPO | 0.279 | 0.093 | 0.74 | 61.7

###  IV-B Performance evaluation and comparison analysis

Figure 7: Obstacle configuration of training and testing environments.

(a) $0$ episode

(b) $100$ episode

(c) $500$ episode

(d) $1000$ episode

Figure 8: Sample navigation paths controlled by the policy of the obstacle-
aware PPOC-CPG tested at different learning stage. The color bar on the right
indicates the value of potential reward gained at each time step.

(a)

(b)

(c)

Figure 9: Sample action trajectories of obstacle-aware PPOC-CPG controller in
the situations with (a) negative contact force (b) positive contact force and
(c) zero contact force value of the first contact sensor $f_{1}$. Note
$\kappa_{1}$ is the curvature of the first link.

To demonstrate the effectiveness of the proposed scheme, we compare the
performance in obstacle-aware locomotion for three different learning-based
controllers: (1) the PPOC-CPG equipped with contact-aware regulator (obstacle-
aware PPOC-CPG, proposed); (2) the PPOC-CPG trained in free space; and (3) the
vanilla PPO (a neural network without CPG) trained in the environment without
obstacles. All controllers are trained in the training environment in Fig. 7
until convergence.

The data for comparison is collected from a testing scenario which is more
complicated than the training environment. In the testing environment, there
are more number of obstacles in each task, and the coordinate of each obstacle
is also added by a standard Gaussian noise ($\omega\sim\mathcal{N}(0,1)$,
clipped by $-0.01<\omega<0.01$). In the testing case, the distance between the
robot and the goal is fixed to $2$ meters, and the initial deviation angle
between the snake robot and the goal is uniformly sampled from $0$ degree to
$90$ degrees. As Fig.7 shows, each task in the testing scenario includes
$5\times 6$ identical circular obstacles (radius=$0.02$ meters) between the
initial position of the robot and the goal. In addition to total accumulated
reward, we also evaluate each controller’s capability of dealing with the
contact situation. Since we do not assume that the head of the robot could
always avoid obstacles perfectly, we add a jam-detection module such that the
entire snake robot is also determined to be jammed if its head joint is jammed
and its linear velocity stays lower than a threshold over a certain amount of
time. In all experiments of this paper we set this low velocity threshold to
be $v_{0}=0.02$ m/s and the time threshold to be $300$ ms. By calculating the
total time of the snake robot being jammed throughout the testing process, we
further adapt this statement to a measurement named ”jam ratio”, such that

$jam\text{ }ratio=\frac{total\text{ }jam\text{ }time}{total\text{ }task\text{
}time}.$

We run the soft snake robot with three different controllers in the testing
scenario for $100$ tests each and compare the performance with several
measurement criteria listed in Table I. For the calculation of the success
rate in the testing scenario, the starvation criteria also applies. The
results in Table I show that the obstacle-aware PPOC-CPG controller
outperforms the other two significantly, with the lowest jam ratio, highest
average linear velocity and highest success rate. This result provides an
important evidence on the capability of the obstacle-aware PPOC-CPG on
improving the efficiency of obstacle-aware locomotion. In addition, it is
worth noticing that the PPOC-CPG trained in free space also shows good
adaptability to the obstacle-based environment with memoryless and fully
proprioceptive observations. To better illustrate the improvement of obstacle-
aware locomotion strategy progressively, we retrain the model using Alg. 1 for
$1000$ actor-critic episodes and test the control policy of obstacle-aware
PPOC-CPG at different learning stage in a goal-reaching task. The resulting
locomotion traces are presented in Fig. 8. The colors on the paths represent
the potential reward at each segment of a path. The lighter the color, the
higher the reward as indicated by the color bar on the right of the figure.
Besides, the dark-colored segments in the path usually indicate a notable
contact when they are close to the obstacles. Such contact could slow down the
robot or even cause a jam. From the figure, we can observe a significant
improvement of the potential reward collected by the snake robot along with
the learning process. Particularly, the policy switches the preferred path
between $100$ episodes and $500$ episodes, and significantly reduces the
portion of low potential reward by either avoiding the contact or taking the
contact more efficiently.

Next we plot three typical trajectories of the first link actions when the
$f_{1}$ sensor on the head link detects negative contact force ($f_{1}<0$),
positive contact force ($f_{1}>0$) and no contact force based on Eq. (1). The
contact force is defined in Fig. 3. In Fig. 9(a), there are three consecutive
force signals and one minimal force detected by the sensors. From $t=0$ s to
$t=3$ s, the tonic input signal as a linear composition of transformed actions
by C1 and R2 shows the same trend of amplifying the extensor tonic input,
which increases the oscillating bias to a positive level. In the meantime, the
inverse of frequency parameter $K_{f}^{-0.5}$ also increases, which indicates
an increase of oscillation frequency given that
$\hat{\omega}\propto\frac{1}{\sqrt{K_{f}}}$ [9, Eq.(11)]. These actions tend
to cause more contacts to create enough lateral propulsion force that pushes
the snake robot away from the obstacle. Right after the contact vanishes at
around $t=3.1$ s, the regulator R2 reacts quickly and reduces the extensor
tonic input of the head joint to zero to recover normal oscillation, with the
oscillation frequency recovered at the same time. Figure 9(b) roughly shows an
anti-symmetric behavior to the trajectories in Fig. 9(a) when the contacts
occur on the other side of the head link. A more significant difference can be
observed by further comparing the contact-aware trajectories with a plot of
contact-free trajectories in Fig. 9(c), where the oscillation signal shows a
relatively more steady oscillation with zero bias and stabilized frequency.

##  V Conclusion

This paper develops a learning-based controller for obstacle-aware locomotion
with a soft snake robot. The control scheme employs two cooperative
controllers: one RL controller is trained with free space locomotion and
adapted to interact with obstacles, while the other RL regulator is event-
triggered and cooperates with the RL controller to improving the locomotion
efficiency while the robot interacts with and navigates through obstacles to
achieve the desired goal position. Both controllers manipulates the inputs to
a Matsuoka CPG system, whose outputs translate to the actuation inputs for the
soft snake robot. As a dynamical system, the special property of the Matsuoka
CPG system enables us to design a linear composition of the outputs of the two
modules for steering control, and also enables the separation of steering and
velocity control. This allows the soft snake robot to adapt to the obstacle-
based environment rapidly given its previous knowledge for locomotion in an
obstacle-free environment. The simulation result shows a significant
improvement in locomotion performance in a cluttered environment. In the
future, we plan to carry out extensive study in the physical system and
investigate the relation between the actions of C1 and R2 and the placement of
the contact sensors. Moreover, it is hypothesized that the link number of the
snake robot is too few to realize more efficient obstacle-aided locomotion. We
plan to model and manufacture a longer soft snake robot ($\geq 10$ links). We
expect a longer snake could create more contacts between its body and the
environment and leverage some of these contact points on its body as footholds
to propel it towards the desired direction for locomotion efficiency.

## References

  * [1] E. W. Hawkes, L. H. Blumenschein, J. D. Greer, and A. M. Okamura, “A soft robot that navigates its environment through growth,” _Science Robotics_ , vol. 2, no. 8, 2017.
  * [2] C. Majidi, “Soft robotics: A perspective–Current trends and prospects for the future,” _Soft Robotics_ , vol. 1, no. 1, pp. 5–11, 2014.
  * [3] H. Wang, R. Zhang, W. Chen, X. Wang, and R. Pfeifer, “A cable-driven soft robot surgical system for cardiothoracic endoscopic surgery: preclinical tests in animals,” _Surgical endoscopy_ , vol. 31, no. 8, pp. 3152–3158, 2017.
  * [4] A. A. Transeth, R. I. Leine, C. Glocker, K. Y. Pettersen, and P. Liljebäck, “Snake robot obstacle-aided locomotion: Modeling, simulations, and experiments,” _IEEE Transactions on Robotics_ , vol. 24, no. 1, pp. 88–104, 2008.
  * [5] P. Liljeback, K. Y. Pettersen, Ø. Stavdahl, and J. T. Gravdahl, “Hybrid modelling and control of obstacle-aided snake robot locomotion,” _IEEE Transactions on Robotics_ , vol. 26, no. 5, pp. 781–799, 2010.
  * [6] ——, “Experimental investigation of obstacle-aided locomotion with a snake robot,” _IEEE Transactions on Robotics_ , vol. 27, no. 4, pp. 792–800, 2011\.
  * [7] T. Kano, T. Sato, R. Kobayashi, and A. Ishiguro, “Local reflexive mechanisms essential for snakes’ scaffold-based locomotion,” _Bioinspiration & biomimetics_, vol. 7, no. 4, p. 046008, 2012.
  * [8] F. Sanfilippo, J. Azpiazu, G. Marafioti, A. A. Transeth, Ø. Stavdahl, and P. Liljebäck, “A review on perception-driven obstacle-aided locomotion for snake robots,” in _2016 14th International Conference on Control, Automation, Robotics and Vision (ICARCV)_ , 2016, pp. 1–7.
  * [9] X. Liu, R. Gasoto, Z. Jiang, C. Onal, and J. Fu, “Learning to locomote with artificial neural-network and cpg-based control in a soft snake robot,” in _International Conference on Intelligent Robots and Systems_ , 2020.
  * [10] J. Lee, J. Hwangbo, L. Wellhausen, V. Koltun, and M. Hutter, “Learning quadrupedal locomotion over challenging terrain,” _Science robotics_ , vol. 5, no. 47, 2020.
  * [11] S. Min, J. Won, S. Lee, J. Park, and J. Lee, “Softcon: simulation and control of soft-bodied animals with biomimetic actuators,” _ACM Transactions on Graphics (TOG)_ , vol. 38, no. 6, pp. 1–12, 2019.
  * [12] R. Gasoto, M. Macklin, X. Liu, Y. Sun, K. Erleben, C. Onal, and J. Fu, “A validated physical model for real-time simulation of soft robotic snakes,” in _IEEE International Conference on Robotics and Automation_ , 2019.
  * [13] P.-L. Bacon, J. Harb, and D. Precup, “The option-critic architecture,” in _AAAI_ , 2017.
  * [14] J. Hofbauer and W. H. Sandholm, “On the Global Convergence of Stochastic Fictitious Play,” _Econometrica_ , vol. 70, no. 6, pp. 2265–2294, 2002\.
  * [15] O. Khatib, “Real-time obstacle avoidance for manipulators and mobile robots,” in _Autonomous robot vehicles_. Springer, 1986, pp. 396–404.
  * [16] M. G. Park, J. H. Jeon, and M. C. Lee, “Obstacle avoidance for mobile robots using artificial potential field approach with simulated annealing,” in _ISIE 2001. 2001 IEEE International Symposium on Industrial Electronics Proceedings (Cat. No. 01TH8570)_ , vol. 3. IEEE, 2001, pp. 1530–1535.
  * [17] J. Dong, X. Zhang, and X. Jia, “Strategies of pursuit-evasion game based on improved potential field and differential game theory for mobile robots,” in _2012 Second International Conference on Instrumentation, Measurement, Computer, Communication and Control_. IEEE, 2012, pp. 1452–1456.

  *[CPG]: Central Pattern Generator
  *[RL]: reinforcement learning
  *[NN]: Neural Network