Dynamical modeling and control of modular snake robots with series elastic actuators for pedal wave locomotion on uneven terrain

This paper introduces the equations of motion of modular 2D snake robots moving in the vertical plane employing Series Elastic Actuators (SEAs). The kinematics of such 2D modular snake robot is presented in an eﬃcient matrix form and Euler–Lagrange equations are constructed to model the robot. Moreover, using a spring-damper contact model, external contact forces, necessary for modeling pedal wave motion (undulation in the vertical plane) are taken into account, which unlike existing methods can be used to model the eﬀect of multiple contact points. Using such a contact model, pedal wave motion of the robot is simulated and the torque signal measured by the elastic element from the simulation and experimentation are used to show the validity of the model. Moreover, pedal wave locomotion of such robot on uneven terrain is also modeled and an adaptive controller based on torque feedback with optimized control gain is proposed. The simulation and experimental results show the eﬃciency of the proposed controller as the robot successfully climbs over a stair-type obstacle without any prior knowledge about its location with at least 24.8% higher speed compared with non-adaptive motion. [DOI: 10.1115/1.4044691]


Introduction
There are many life forms with incredibly effective locomotion mechanisms, sensing, and computation capabilities, which are invaluable sources of inspiration for researchers. One of these bioinspired designs is snake-like robots [1], which with their small body cross section, intrinsic stability, maneuverability, and hyperredundancy make them ideal for locomotion in challenging environments, such as pipes [2], tankers [3], and collapsed buildings [4].
Generally speaking, biological snakes can perform several locomotion patterns based on environmental conditions, to effectively traverse challenging environments. Among such motion patterns, lateral undulation [5] and its counterpart in the vertical plane (rectilinear [6] or pedal wave motion [7]) are of special interest due to their intrinsic characteristics enabling the snake to move in unstructured environments with different features [8].
Lateral undulation, which is the most common locomotion pattern among biological snakes, has received considerable attention since studies on snake robots started [9]. Most of these studies consider installing wheels to mimic the anisotropic friction properties of real snakes [10]. However, such wheeled snake robots suffer from the same limitations as wheeled mobile robots have. Hence, they might not be suitable for locomotion in unstructured environments. Moreover, although it has been shown that the anisotropic friction property of the snake belly scales and existence of push points in the environment are the major factors for generating such forward motion in Ref. [11], the development of snake-like skin, such as the one proposed in Ref. [12], has not yet generated practical results. 1 Corresponding Author To overcome the aforementioned issues, some recent works, such as Ref. [13], have focused on designing a robot capable of pushing against the irregularities of the terrain to move forward. Although such obstacle-aided locomotion patterns have been shown to be very effective and do not require the robot to be covered by a snakelike sleeve [14], the existence of a sufficient number of pegs for the robot to push against is essential to generate enough propulsive force [15]. Hence, such locomotion mechanisms are effective only for locomotion in confined spaces, such as pipes, where the robot can always push against the walls to move forward [16].
On the other hand, pedal wave motion of the snake robots, which is similar to caterpillar motion [17], is an effective locomotion pattern very similar to lateral undulation but performed in the vertical plane. In this type of motion, the robot lifts some of its links and pushes against the ground to move forward making it very effective for locomotion on uneven terrains, where lateral undulation is no longer effective [7]. Using such a locomotion pattern, one can take advantage of the small cross section of the snake body to move in confined spaces and because the robot pushes against the ground to move forward, the existence of sideways push points no longer have an effect on the generation of such movement.
Although pedal wave locomotion patterns can be used in challenging environments, dynamical modeling of such locomotion mechanism, necessary for in-depth investigation of such motion, is challenging due to the contact between the robot links and the environment. Some recent works, such as Ref. [6] have proposed a modeling framework based on a series of connected masses and springs to model such motion. However, such a model is only suitable for biological snakes modeling, where the contraction and relaxation of the muscles can be modeled with the same method.
More recently, in Ref. [18] based on the well-known Newton principle and in Ref. [19] using the Euler-Lagrange method, the dynamical equations of pedal wave motion suitable for modeling modular snake robots are presented. However, in both of these works, it is assumed that the number of contact points remains constant during the motion, and the normal forces are obtained based on the force and moment balance. Hence, such models are not suitable for modeling a snake robot with multiple contact points in unstructured environments, where the robot might be in contact with multiple contact points in different planes.
In addition to the lack of a generalized modeling framework, the number of works focused on adaptive pedal wave motion on uneven terrain is limited. In Ref. [7] a shape-based control scheme has been proposed for the pedal wave locomotion of snakes to enable them to climb a stair-type obstacle. However, the proposed method requires prior information about the size and location of the obstacle. Moreover, in Ref. [20], a control mechanism based on torque control strategy is proposed for a snake-like robot. However, the controller requires information from a pressure sensor installed on the surface of the links.
To address the aforementioned issues regarding modeling and control of snake robots with stiff joints on uneven terrain, we proposed a general modeling framework and an adaptive control strategy in Ref. [21]. In this work, we introduced the equations of motion of modular 2D snake robots in the vertical plane using the well-known spring-damper contact model and proposed an adaptive controller based on external force feedback resulting in the robot to successfully climb over a stair-type obstacle in the simulation environment without experimentation results.
On the other hand, more recently in Ref. [22], we proposed a cost-effective snake robot design equipped with series elastic actuators (SEAs), where an elastic element is attached between the motor and the links to reliably measure the servomotor output torque and regulate the motor torque (force) for adaptive motion [23]. Such a snake robot with SEAs and similar ones, such as Ref. [24] are less complicated and more robust compared with existing snake robots with torque measurement mechanisms, such as Ref. [25] with force sensor resistors, Ref. [26] with strain gauges and Ref. [27] with a cam mechanism (see Ref. [22] for more details), which makes them ideal for locomotion on uneven terrain. However, the introduction of an elastic element between the robot servos and the links adds a new level of complexity to the snake robot model [28], which to best of our knowledge is not considered in any snake robot model presented in the literature. Moreover, although it is shown in Refs. [29] and [22] (by conducting experimentation on the robot) that adaptive lateral undulation and pedal wave motion can be achieved by incorporating a torque feedback signal (by measuring the deflection of the elastic element) into the joint controller, no formal modeling framework exist to be employed for investigation of such control methods.
In this paper, a generalized dynamical model of modular 2D snake robots performing pedal wave locomotion in the vertical plane is presented, which extends our recent work in Ref. [21] by taking into account the effect of the flexible element embedded in each joint of the snake robot. The kinematics of such robot has been presented in a matrix form and using the Euler-Lagrange method, the equations of motion of the robot have been derived. Additionally, employing the well-known spring-damper contact model, the effect of external forces on the robot is taken into account, which unlike existing models, such as Ref. [19], can be used to model the robot in contact with the environment at multiple points in different planes. Moreover, using this dynamical model, the robot is simulated in an environment with a stair-type obstacle, and an adaptive controller in the gait parameters space with the optimized gain is designed. The effectiveness of such a controller for climbing over an obstacle without prior knowledge about its location is shown in the simulation and experimentation, which shows that at least 24.8% faster motion can be achieved with the use of the proposed controller, compared with nonadaptive gait-based motion generation strategy.
The organization of the rest of this paper is as follows. In Section 2, the problem definition is given. The pedal wave motion mechanism of modular snake robots is explained and the design of a snake robot with flexible elements at the joint is briefly discussed. In Section 3, the kinematics of a 2D snake robot with SEAs is presented, and in Section 4, the generalized equations of motion of the robot are obtained. Finally, in Section 5, the validity of the simulation model is examined and in Section 6 the modeling of the robot in unstructured environments is discussed. An adaptive controller based on the servomotor output torque feedback (measured by the elastic element) is designed and the effectiveness of such a controller is demonstrated by the simulation and experimental results.

Background
Today's world of robotics is completely dominated by wheeled robots. However, this popularity stems from simplicity in modeling, control, and manufacture of such robots, and not their capabilities to overcome locomotion challenges. In contrast, snake-like robots have proved to be very effective in unstructured environments, where for wheeled robotic systems, it is impossible to operate without considering additional means of maneuverability [8].
Among snake-like locomotion patterns, pedal wave motion has several advantages over other gaits in unstructured environments. This type of locomotion, which is similar to extended caterpillar locomotion with multiple contact points [17], is exhibited mostly in heavy snakes [6]. Early studies on this type of locomotion suggested that in pedal wave (rectilinear) locomotion snake ribs act as legs, similar to walking. However, the snake does not "travel on its ribs" [30], or in the other words, the snake ribs are not the main means of locomotion.
In pedal wave motion, similar to earthworms [6], the snake travels in a straight line, and unlike other types of locomotion, sideways interaction with the environment is not essential. Instead, contraction and relaxation waves pass over the ventral muscles along with the lifting body parts are the main cause of locomotion [6]. Such motion has some similarities with sidewinding locomotion pattern, which the snake uses two separate parts of the body as static contact points to lift and bend the remaining parts and move forward [31]. However, unlike in sidewinding motion, where the robot is moving sidewise, in pedal wave motion the robot moves along a straight line, which makes it more suitable for locomotion in confined spaces, where there is not enough space to move sideways.
To mimic pedal wave motion, different snake robot designs, such as the one presented in Ref. [32] are proposed. However, the most common snake-like mechanism, capable of performing pedal wave motion, is presented in Ref. [22] (see Fig. 1), where a snake robot with six links and five servos rotating about parallel axes has been developed. This robot is capable of generating pedal wave motion by controlling the joints angles based on the following gait pattern, inspired by the body shape of biological snakes: where ; = 1, 2, . . . , −1 are the desired relative joint angles, is the temporal frequency, is the spatial frequency (phase shift), and is the amplitude of the sinusoidal wave. Figure 1 shows the snake robot with six links performing this type of motion with = 4 rad, = rad/s, and = − 2.2 5 rad with the average velocity obtained to be 2.40 cm/s.
In the rectilinear motion of biological snakes, the amplitude of the vertical wave, i.e., propagating along the snake body is very small, hence such motion is usually slow and not very efficient [33]. However, in a snake robot, the maximum amplitude of such vertical waves is not as limited as in biological snakes. Hence, a snake robot can achieve faster motion with the use of such locomotion pattern.

Fig. 1 Snake robot progression with pedal wave motion with
A p = π/4 rad, ω = π rad/s, and η = −2.2π/5 rad first on a high friction and then on a low friction surface

Fig. 2 The components of each robot module: (a) is the servomotor (DSR-0101), (b) is the magnet, (c) is the elastic element, (d) is the magnetic encoder, and (e) is the sensor holder
Moreover, one of the most important characteristics of pedal wave motion is that the snake can lift its body part and push against the ground to move forward, resulting in a very suitable locomotion pattern for uneven terrain. However, this also makes modeling of such motion challenging, compared with lateral undulation, in which the robot body is constantly in contact with the ground [34]. Consequently, any promising dynamical model of pedal wave motion should deal with contact forces in an efficient manner to obtain a suitable simulation model to be used for investigating such a complex motion.
In addition to the complexity of modeling the contact forces, the snake robot joint as shown in Fig. 2 is equipped with an embedded elastic element to reliably measure the motor torque. However, the effect of the elastic element on the robot motion as also mentioned in Ref. [35] cannot be neglected and should be considered to obtain a reliable model of the robot.
In the next section, we will mainly focus on obtaining the kinematics of the snake robot with SEAs in plane in a matrix form to be used for obtaining the Euler-Lagrange equations of motion of the robot.

Kinematics of the Snake Robot
A modular snake robot with identical links ( ; = 1, 2, . . . , ) and − 1 actuators ( ; = 1, 2, . . . , − 1) with the same gear ratio of attached in series with a spring to the corresponding link is illustrated in Fig. 3, where and are the absolute link and rotor angles, respectively, = − +1 is the angle between the consecutive links and + 1, = − is the th rotor angle relative to its stator reflected through the link side, , and , are the position of the center of mass of link and rotor , respectively, in the global coordinate frame and , denotes the center of mass of the robot. It should be noted that rotor angles and the flexible elements at each joint are not shown in Fig. 3. The reason is that considering the relative link angles , the body shape of the robot is independent of rotor angles , hence only relative angles s are used to show the body shape of the robot in the vertical plane. Figure 4 better shows the defined parameters for a single joint of a snake robot equipped with SEAs.
To obtain the expression for the kinematics and dynamics of the snake robot in a matrix form, it is convenient to define several matrices and vectors as follows: where ×( −1) is an × ( − 1) null matrix with the specified dimension. Considering these matrices, the kinematics relation for the rotors and the links can be obtained in a matrix form.
The Robot Links Velocity. Considering the body shape of the robot in Fig. 3 and assuming that 1 = 2 = . . . = = 2 for which with little effort can be written in matrix form as below and replacing and 1 from Eq. (2) and Eq. (3), the velocity of center of mass of each link can be obtained to be where = where is a matrix with all zero elements.
The Robot Rotors Velocity. In addition to the linear velocity of the center of mass and the angular velocity of each link, the angular velocity of the rotors and the linear velocity of the center of mass of the rotors should also be obtained to calculate the expression for the total kinetic energy of the system. Considering the body shape of the robot in Fig. 3, the position of the center of mass of rotor can immediately be obtained via which uses a similar method as the previous section can be written as Obtaining the derivative of the vector and with respect to time, the velocity of center of mass of each rotor can be obtained to be where and P is a diagonal matrix containing the identical gear ratio of the − 1 motors. It should be noted that the presented procedure for obtaining the kinematics of the links and the rotors enabled us to obtain the position and velocity of the center of mass of each link and rotor as a function of joint angles , rotor angle , the absolute angle of the head module, and the position of center of mass of the robot in matrix form. Hence, the implementation of such equations does not require symbolic computation, making the implementation of equations of motion to be presented in Section 4 simpler. It should also be mentioned that although the kinematic relations obtained for the snake robot in the vertical plane , the same equations can describe the kinematic relations of lateral undulation locomotion in horizontal plane by only replacing axis with axis. Hence, the obtained relations can be used to describe the kinematics of any modular 2D snake robot with an arbitrary number of links in an easy to implement matrix form.

Dynamical Equations
Considering the number of degrees of freedom of the snake robot, the Euler-Lagrange method is a straightforward approach, which can be employed to obtain the equations of motion of the snake robot. Using this method, the external contact forces can be incorporated into the equations, the effect of the springs at each joint can be taken into account through the expression for the potential energy of the system and the gravitational forces can easily be handled. Thus, compared with Newton's method, Euler-Lagrange method can be used to derive the dynamical equations of the snake robot motion in the vertical plane with less complexity.
Choosing generalized coordinates, i.e., the minimum number of variables required to uniquely describe the system, to be , the Euler-Lagrange equations of motion can immediately be constructed as follows: where 1 and 2 are the kinetic energy of the links and the rotors, respectively, 1 is the potential energy stored in the flexible elements (torsional spring) at the joints, 2 and 3 are the potential energy of the links and the rotors due to the gravitational force, = , is a zero column vector with ( + 2) rows, is the vector of − 1 control inputs (motor torques) and is the vector of non-conservative contact forces.
Kinetic Energy. Considering the chosen generalized coordinates , it can be seen that the expression for the kinetic energy of the links, necessary for constructing the Euler-Lagrange equation is independent of rotor angles s, i.e., the body shape of the robot is independent of . Hence, 1 can be obtained to be only the sum of the kinetic energy of the links due to their own linear and angular velocities as follows: where M and I are by diagonal matrices of mass and moment of inertial of the links, respectively. It is worthwhile to mention that, in the case of a 2D snake robot with stiff joints, the kinetic energy of the links as presented in Eq. (19) will be the only terms required to calculate the kinetic energy of the system and obtain the equations of motion of the robot. The only consideration is that the mass of each link should be the sum of the mass of the rotors and the links.
To fully obtain the expression for the kinetic energy of the system, the kinetic energy of the rotors should also be taken into account. Similar to the method used to obtain Eq. (19), the kinetic energy of the rotors can be obtained as where and are the linear velocities of the rotors at each joint, independent of the rotor angle , is the angular velocity of the rotor and M and I are − 1 by − 1 diagonal matrices of mass and moment of inertia of the rotors.
Potential Energy. The first part of the potential energy of the snake robot as shown in Fig. 4 is due to the springs attached between the actuators and links. Hence, the potential energy of the system due to the torsional spring at the joints can be obtained as where is the − 1 by − 1 diagonal matrix containing stiffness of the flexible elements of the joint, = 1 . . . −1 , and = 1 . . . −1 . This expression can also be represented as a function of generalized coordinates as follows: where is defined to be where 3×( −1) , ( −1)×3 , and 3×3 are null matrices with the specified dimensions.
On the other hand, considering the body shape of the robot in the plane, the robot lifts some parts of its body from the ground. Thus, the expression for the potential energy of the system due to the mass of the robot should also be included. Considering the body shape of the robot, the expression for the potential energy of the system due to the mass of the links ( 2 ) can be obtained to be Knowing that = 1

=1
and substituting 0 from the expression of center of mass of the robot, Eq. (24) can be written as follows: Moreover, the center of mass of the rotors are located at , and not at the center of mass of the links, thus similar procedure should be repeated to calculate the potential energy of the rotors, which will result in the following expression for the potential energy of the rotors

Fig. 5 The force diagram showing the contact forces exerted to the robot module
Hence, one can find the total potential energy of the system to be the sum of the potential energy due to the mass of the links, mass of the rotors, and the elastic elements of the joints.

Non-Conservative Forces.
To obtain the effect of nonconservative forces , on the th link of the robot expressed in the global coordinate frame, it is enough to consider the point of effect of such forces. Assuming that these forces will be exerted on the center of mass of each link, can be obtained as follows: which can be written in matrix form as . . . are the vectors of all nonconservative forces along the global and directions, respectively.
The above procedure can be used to take into account any nonconservative forces such as friction and/or contact forces, necessary to model both pedal wave motion and lateral undulation in confined spaces. For example, Fig. 5 shows a general case where a single link of the robot is in contact with an obstacle and and are the normal and tangential forces exerted on the robot at the point by the environment. Assuming that is in contact with the obstacle, i.e., ≤ 0, and considering a spring-damper contact model, can be calculated as follows: where is the coordinate of the point of contact along the ( axis of the attached coordinate to the obstacle), is the spring, and is the damping constant of the environment. Once is obtained, it is straightforward to calculate as follows: where is the velocity of the point along the direction tangent to the surface and is the friction coefficient. Having derived and , it will be straightforward to incorporate these forces into the equations of motion. However, as it is shown in the general case of Fig. 5, and are expressed in the stationary coordinate frame , which is not necessarily aligned with the global coordinate frame. Thus, first, such forces should be expressed in the global coordinate frame to construct and and then Eq. (28) should be used to incorporate these forces into the dynamical model.
Another important consideration about such contact model for the snake robot is that unlike friction forces, which are assumed to be exerted on the center of mass of each link, the contact forces might be exerted on any point located on the robot links. However, due to computational limitations, it is not possible to check every points on the robot links for a possible contact with the environment. Hence, for the simulation purpose, we only consider the center of mass of the links, robot joints, tip of the head and the tail to be checked at each step-time to calculate the contact forces. This means that for a snake robot with links and − 1 joints, 2 + 1 points on the robot should be tested at each time-step to see if a contact has occurred or not. Once the set of contact points, i.e., ; = 1, . . . are obtained, the tangential and normal forces at these points can be calculated based on Eq. (29) and Eq. (30). It is critical to mention that, to incorporate these forces into the equations of motion, the derivative of the velocity vector of the contact points with respect to needs to be obtained and then a similar relation as in Eq. (28) should be used, which is straight forward and not included here for the sake of brevity.

Equations of Motion.
Considering the expression obtained for the kinetic and potential energies of the snake robot, it is now possible to construct the equations of motion of pedal wave motion as follows: where ( 1 ) is a +2× +2 positive definite link inertia matrix, ( 1 ) is the rotor inertia matrix, ( 1 , 1 ) and ( 1 , 1 ) are Coriolis and centrifugal terms due to the velocity of the links and rotors, is a zero vector of size − 1, G is the column vector of gravitational forces, is the same as defined in Eq. (18) and 1 2 2 3 = L I L where 1 and 2 have appeared due to the angular velocity of the rotors depending on and 3 depends on the inertia and gear ratio of the rotors (see the Appendix for the structure of the matrices in Eq. (31). Equation (31) can be seen as two sets of equations. The first + 2 equations are under-actuated dynamics of the system containing the relative joint angles, the position of the center of mass and orientation of the robots head module, in which the friction force and other environmental forces will appear and the last − 1 equations are fully actuated motor-side equations.
It should also be noted that considering stiff joints, i.e., matrix with high values, one can assume there is no elastic element at the joint, i.e., the last − 1 equations can be neglected. Consequently, the equations of motion of a planar snake robot with stiff joints Fig. 6 Simulated pedal wave motion with A p = π 4 rad, ω = π rad/s, and η = − 2.2π

rad with the average velocity of 2.18 cm/s
can easily be obtained with little effort as follows: where the definition of ( 1 ), ( 1 ), ( 1 , 1 ), and ( 1 , 1 ) remain to be the same as in Eq. (31), = , is a zero vector with three elements, and 1 , G , and are the first + 2 elements of , G, and , respectively.

Pedal Wave Motion on Smooth Surfaces
To demonstrate the effectiveness of the proposed model in generating pedal wave motion, gait pattern Eq. (1) with = 4 rad, = rad/s, and = − 2.2 2 rad (same as the pedal wave motion of the robot shown in Fig. 1 has been chosen to be implemented on the simulation model,described by Eq. (31). For this purpose, a snake robot with six identical links each with the mass of 0.1 kg and length 0.07 m (same as the physical robot) and five motors with the rotor mass of 0.02 kg and gear reduction of 255 has been simulated via M R2017b, and the results demonstrating pedal wave motion are shown in Fig. 6. As shown in Fig. 6, the proposed dynamical model has been successfully used to simulate pedal wave motion when the environmental constant k and d are chosen to be 350 N/m and 15 Ns/m, respectively. The simulation step-time has been chosen to be 0.0001 s, which is sufficiently small to make sure the fast dynamic of the system (last − 1 rows of Eq. (31)) are being captured.
It should be noted that, to implement gait, described by Eq. (1), a joint level controller should be designed such that the error between the desired and measured relative joint angles converges to zero by only using motor-side measurements and . Generally speaking, designing such a tracking controller for flexible joint robots requires backstepping control techniques [36]. However, for such small values of and 1 , it can be concluded that and will be sufficiently small [37], hence a simple PD tracking controller as presented in Eq. (33) is used to control the simulated model in Fig. 6 and also the physical robot where and are positive definite gains chosen to be 30 and 15 respectively.
To validate the proposed model in Eq. (31), the torque signal obtained by measuring the deflection of the elastic element from the experiment shown in Fig. 1 has been compared with the simulation results for the fifth joint (head module) as shown in Fig. 7. As can be seen in Fig. 7, the experimental data resembles the simulation data with some expected inconsistency due to the simplified assumptions used to obtain model, described by Eq. (31). The main differences between the model and the actual robot could stem from the assumed linear model for the elastic element, difficulty in precisely modeling the contact forces and the very simplified friction model, described by Eq. (30).
Moreover, as it can be seen in Fig. 1, at first, the robot is moving on top of a high friction surface (black plate) with = 0.82 and then on a low friction surface with = 0.40 to examine the effect of friction on the forward velocity of the robot and investigate the relationship between the friction of the surface and the deflection of the elastic element. From the experiment, it could be seen that the forward velocity of the robot on the surface with higher friction was approximately 20.3% higher than the robot velocity on the lower friction surface. Considering (30), one may expect that the velocity of the robot on high friction to be almost two times higher than the velocity on the lower friction surface as the external force in direction linearly depends on . However, both the simulation results (13.7% increase) and experimental results suggested that increasing will cause the forward velocity to increase, but this relation is not linear.
Moreover, as can be seen in Fig. 7, after 7.5 s, when the entire head module is on the surface with low friction, the absolute value of the negative peaks of the torque signal have been increased by 16.3%. Although in the experimentation, the same phenomenon has been observed, the peaks were increased by only 4.3%. One reason for that could be using the very simplified model of friction in Eq. (30) for the simulation, which differs from the reality.
Consequently, one can see that model described by Eq. (31) is successfully used to capture the essentials of pedal wave motion. However, as expected, the simulation model cannot precisely capture the details of pedal wave motion of a real snake robot, due to the complexity of precisely modeling the contact forces and the friction. Hence, such simulation framework combined with real experimentation on the physical robot can generate more fruitful results.

Pedal Wave Motion on Uneven Terrain
Unlike locomotion on smooth surfaces, on uneven terrains, the environmental forces exerted on the robot along the forward direction might cancel each other. Hence, the robot average velocity could be affected adversely and in some cases, the robot might not be able to move forward. To address this issue, one can extend the presented model in Eq. (31) to simulate pedal wave motion on uneven terrains and investigate adaptive control strategies to achieve more agile snake-like motion on uneven terrains.
For this purpose, motivated by the controller proposed in Ref. [29] for lateral undulation, we propose an adaptive control strategy where , are constant gait parameters similar to Eq. (1) and is the spatial frequency of th joint to be controlled. To control the spatial frequencies, i.e., the vector = 1 , 2 . . . , −1 , we propose an adaptive controller shown in Fig. 8, where is the position controller, described by Eq. (33), is the vector of nominal spatial frequencies, = is the vector of the desired joint angles generated from Eq. (34). Moreover, the admittance controller is defined as follows: where , , and are the positive desired inertia, damping, and stiffness, respectively, is the vector of torque signal feedback measured by − 1 elastic elements embedded at each joint (first − 1 elements of ) and is the feedback gain to be designed. As it can be seen in Eq. (35), choosing the nominal gait parameters , if 0, converges to , because , , and are chosen to guarantee that Eq. (35) is a stable secondorder system. However, if ≠ 0 (mainly due to external contact forces), deviates from its nominal value based on the feedback signal and the dynamical model presented in Eq. (35). This is similar to the conventional admittance controllers, where the measured output torque (force) will be used in the outer loop to change the reference for the inner position control loop [38]. However, in the proposed controller in Eq. (35), the measured torque signal will be used to adaptively control the parameter of the gait pattern, i.e., , instead of directly controlling the joint angles.
To fully define controller, given by Eq. (35), the feedback gain should be designed to achieve the desired motion. Considering the complex, nonlinear, and under-actuated mathematical model of the robot, this task can be very challenging. However, one can use the model presented in Eq. (31) and run the simulation on uneven terrains and search for the desired value of kc. To achieve this goal, we used the model in Eq. (31) and simulated the motion based on gait pattern, given by Eq. (34), with = 4 rad, and the controller shown in Fig. 8 in an environment with a stair-type obstacle (6 cm high, 11 cm wide) located 5 cm away from the head module. We increased the feedback gain , incrementally by 0.1 from 0 to 15 and calculated the average velocity of the robot after 60 s. The average velocity of the snake robot with each value of the feedback gain is shown in Fig. 9. As it can be seen in Fig. 9, using Eq. (35) with = 10.1, the maximum forward speed is achieved, which is close to the average velocity obtained on the smooth surface. Moreover, the results showed that with = 10.1, the average forward speed of the robot is higher by 24.8% compared with = 0. This is an important result because as it can be seen from Eq. (35), with = 0, converges to independent of , similar to the case when the open loop gait pattern in Eq. (34) is being used. Hence, Fig. 9 shows that such closed-loop control strategy with = 10.1 results in a faster motion compared with gait pattern Eq. (34) with the same parameters but without feedback. Figure 10 shows the snake robot climbing over the stair located on its path without prior knowledge about its location with control Eq. It should be noted that to model the stair-type obstacle, same spring-damper contact model shown in Fig. 5 has been used. However, in addition to model the contact between the robot and the top side of the stair, contact forces exerted on the robot due to the collision to the side of the stair has also been included into the model. The reason is that there are cases that the robot cannot climb over the stair due to small amplitude of the wave, i.e., ,  Fig. 10, the model has successfully been used to simulate the robot in contact with the environment at three different points, which was not possible using the contact modeling framework proposed in Ref. [19].
To obtain the simulation results in Fig. 10, 2 + 1 points (the center of mass of the links, robot joints, tip of the head and the tail) on the robot body have been checked at each step-time to determine the contact points and calculate the external forces. Unlike on smooth surfaces, in environments with sharp edge obstacles, there is a high possibility that the links touch the obstacle at the points other than these 2 + 1 points. Such a problem can be addressed by considering more candidate contact points to be checked at each step-time, which increases the computation burden of the simulation model. However, because this problem happens mostly due to the sharp edges of the obstacle touching the links, checking the distance between the candidate contact points and the sharp points of the obstacle (vertices of the square-shape obstacle) or considering bounding a box around the obstacle and checking if the candidate contact points have touched the borders of the bounding box are other possible solutions (see Ref. [39] for more details about the collision detection problem in simulation environments).
To investigate the effectiveness of the proposed controller on the physical robot, the first experiment was conducted on the real robot based on Eq. (35) by setting = 0. However, the robot could not climb over the stair due to rolling over to one side. This issue could not be modeled by the presented modeling framework because the model only describes the motion in plane and lateral forces in Y direction cannot be taken into account. This means that the value of the measured motor torque and the simulation results are valid as long as the side stability of the robot is guaranteed. This is not a major issue in pedal wave motion on smooth surfaces, because as the robot moves in a straight line, the projection of the center of mass of the robot lies inside the convex hull of the contact points (all located on a straight line). Hence, the robot will not roll over to one side. However, when the robot is climbing over an obstacle, it could be in contact with four different surfaces, i.e., the ground, sides of the obstacle and top of the obstacle at the same time (see Ref. [40] which gives more information about the similar issue in stable walking on uneven terrains). Hence, the snake robot can roll over to one side.
Finally, to evaluate the performance of the physical robot using the optimized controller, the designed controller based on Eq. (35) with = 10.1 was implemented on the physical snake robot, and the recorded motion is shown in Fig. 11. As can be seen in Fig. 11, the robot could successfully climb over the stair with the same dimensions as of the stair in the simulation, with an average Fig. 11 The physical snake robot climbing over the obstacle with height 6 cm and width 11 cm, with an average velocity of 1.3 cm/s velocity of 1.3 cm/s. Moreover, as it can be seen in the experiment, unlike the case with = 0, the robot did not roll over to one side and successfully climbed over the obstacle due to the feedback signal , which modulates the phase shift . Hence, unlike the case with = 0, deviated from based on the feedback signal, producing an adaptive pedal wave motion.
To improve the performance of the snake robot with the use of the presented controller, one way is to use the obtained value of from the simulation as the initial search point for experimentation based optimization methods, such as Bayesian optimization [41]. One can also consider pedal wave motion with an added lateral motion by installing additional servo motors and extend the model, described by Eq. (31), to further investigate the side stability of the robot during locomotion on uneven terrains. Another method, which could improve the performance of the controller, is to include other sensors, such as IMUs in addition to the torque sensors to achieve more adaptive and stable pedal wave motion.

Conclusion
This paper introduced a general modeling framework for modeling modular snake robots with SEAs performing undulation in the vertical plane (pedal wave motion). The kinematics of such snake robot in vertical plane were obtained in a matrix form and the Euler-Lagrange equations of motion have been presented. Using the obtained dynamical model and the spring-damper contact model, pedal wave motion of the robot on smooth surfaces was simulated and the effect of changing friction was investigated, which, supported by the experimentation results, showed that the forward velocity of the pedal wave motion of the snake robot is higher on the surfaces with high friction. In addition, to validate the model, the torque signal measured by the elastic elements embedded at each joint of the physical robot was compared to the simulation results, which showed that they bear a considerable resemblance, with expected discrepancies due to the simplified friction and contact models. Comparing the theoretical predictions with a simulation-based physics engine can be considered as another method to verify the results in future works. Moreover, it is shown that this model is powerful enough to simulate snake locomotion on uneven terrains by means of simulating the robot motion climbing over a stair-like obstacle. Finally, an adaptive gait-based controller using the measured torque feedback of the elastic element was proposed and optimized using a search-based method. The controller was implemented on the physical robot, which showed that the robot can successfully climb over the obstacle without any prior knowledge about the exact location of the obstacle.

Nomenclature
= gear ratio of the servo motors at each snake robot joint = the environments damping constant = the environments spring constant = the number of snake robot links = diagonal matrix containing stiffness of the flexible elements = the kinetic energy of the system = the potential energy of the system = link of the snake robot = the point of contact between the robot and the ground = rotor of the snake robot = positive definite gains of the adaptive controller = the angle between the consecutive links and + 1 = the angle of rotor relative to its stator reflected through the link side = spatial frequency of joint reference angle = absolute link angle = absolute rotor angle = time-frequency of the joint reference angle . . .
and 1 is the first + 2 elements of the state vector .