Mathematical Modeling of Robotic Locomotion Systems

Abstract

This article deals with the presentation of an alternative approach that uses methods of geometric mechanics, which allow one to see into the geometrical structure of the equations and can be useful not only for modeling but also during the design of symmetrical locomotion systems and their control and motion planning. These methods are based on extracting the symmetries of Lie groups from the locomotion system in order to simplify the resulting equations. In the second section, the special two-dimensional Euclidean group $\mathcal{S}E\left(2\right)$ and its splitting into right and left actions are derived. The physical interpretation of the local group and spatial velocities is investigated, and by virtue of the fact that both of these velocities represent the same velocity from a physical point of view, the dependence between them can be found by means of the adjoint action. The third section is devoted to the modeling and analysis of the planar locomotion of the symmetrical serpentine robot; the positions and local group velocities of its links are derived, the vector fields for the local connections are given, and the trajectories of the individual variables in the lateral movement of the kinematic snake are shown. At the end of the article, the overall benefits of the scientific study are summarized, as is the comparison of the results from the simulation phase, while the most significant novelty compared to alternative publications in the field can be considered the realization of this study with a description of the relevant methodology at a detailed level; that is, the locomotion results confirm the suitability of the use of geometric mechanics for these symmetrical locomotion systems with nonholonomic constraints.

1. Introduction

Modeling biologically inspired symmetric mechanisms in mobile robotics is relatively problematic. The reason is the complexity of such mechanisms and the overall complexity of systems at multiple levels. Primary modeling of symmetric locomotion systems, where the movement or structural solution is characterized by symmetry, brings new challenges, such as the reduction of the overall dimension of the mechanism, generation of optimal gait functions, analysis of stability due to external perturbations, and identification of parameters representing appropriate biological patterns and is partly related to the overall biorealism of a complex solution. In the future, however, there will be important challenges regarding the overall energy efficiency or the adaptability of the mechanism to the changing environment and to the overall conditions affecting locomotion. In the past, several methods were developed for the description of mechanical systems, but the overall complexity of the mechanical system prevented the simple use of conventional methods. Currently, quite a lot of interest is devoted to the applications of geometric mechanics in robotics, while it has a high ability to describe a given mechanical system in a more elegant way, taking into account its geometric parameters, which can show a symmetrical character.

If we look in more detail at the concept of symmetry in combination with a mechanical solution, we can define the concept of a symmetrical mechanical system. This type of system is characterized by its geometry and design. This means that the overall mechanical system acquires a symmetrical character, which means that it is structurally symmetrical. At the same time, a part of this system is the symmetrical structure of the subsystem layout, which is mechanically balanced at the level of force action and overall structural stress.

Geometric mechanics has been used in scientific practice for decades. Its origin dates back to the sixties and seventies, when Vladimir Arnold (1966) [1], Stephen Smale (1970) [2], and Jean-Marie Souriau (1970) [3], laid the foundations of this discipline. Currently, geometric mechanics is used in a wide range of scientific fields, such as computer graphics, control theory, magnetohydrodynamics, and nonlinear stability. It is also used in the design of mechanisms for environments with active flow, such as aircraft systems, submarines, and the like. The motivation for its use in locomotion systems was probably due to the good results it achieved in solving the problems of positioning satellite systems in orbit, a falling cat, parking a car, and others. With a more detailed focus on robotic locomotion systems, we can encounter the application of geometric mechanics in several scientific works. For example, during the 1990s, in the work titled “Symmetry, Stability, Geometric Phases, and Mechanical Integrators—Part I” Marsden et al. (1991) [4], the authors presented the development of modern methods of mathematical analysis and their use for describing dynamics and overall progress in mechanics. In the work titled “Geometric Phases and Robotic Locomotion”, Kelly and Murray (1995) [5] deal with specific robotic applications, where they describe the mechanisms of a two-wheeled robot, an inchworm robot, a serpentine robot with sidewinding motion, and a six-legged robot with a tripod gait application. In the mentioned work, the authors developed and applied the theory of principal fiber bundles and the theory of connections. Geometric mechanics itself could be defined in several ways, but for our application, the most suitable definition is that geometric mechanics is a field of mechanics that deals with the study of the movement and iteration of bodies based on geometric and, at the same time, algebraic methods, with the aim of overall analysis of locomotion trajectories and identification of configuration parameters within defined spaces while using the language of differential geometry for formal description.

A more detailed view of the theory of geometric mechanics can also be obtained from the older works of Chaplygin (1897), Cartan (1928), Kobayashi and Nomizu (1963), Neimark and Fufaev (1972), Rosenberg (1977), Weber (1986), Bloch and Crouch (1992), Yang, Krishnaprasad, and Dayawansa (1993), and others. The work titled “Nonholonomic Mechanical Systems with Symmetry” by Bloch, Marsden, Krishnaprasad, and Murray (1995) [6] is considered another important work in the field. In this work, the authors deal with the development of the dynamics of mechanical systems with non-holonomic constraints, taking into account the geometric principle. This work is considered one of the basic sources for the study of the given issue. As an example, it shows the suitability of using the given approach In robotic locomotion for the entire class of possible solutions, such as legged robots, snake-like robots, and wheeled mobile robots. Within movement modeling, the theory of connection on a principal bundle is applied. An important part is the geometric phase, which generates a specific form of robot movement. Another application can be seen in the work titled “Nonholonomic Mechanics and Locomotion: The Snakeboard Example” by Ostrowski et al. (1994) [7]. The writers of this work concentrate on the design and description of the snake board-type mechanism for a better understanding of the undulating locomotion of snakes. Snake board has no biological counterpart and is an artificial imitation of the desired mechanism performing a snake-like symmetrical movement.

Important works at the beginning of the millennium include the work titled “Nonholonomic mechanics” by Bloch (2003) [8], which discusses nonholonomic mechanical systems and their control, and the work titled “Optimal Gait Selection for Nonholonomic Locomotion Systems” by Ostrowski et al. (2000) [9]. This work concentrates on the optimum control and selection of gaits in a category of nonholonomic locomotion systems that demonstrate group symmetry. Other works include, for example, A Framework for Steering Dynamic Robotic Locomotion Systems by McIsaac and Ostrowski (2003) [10], where the authors address the challenge of controlling robot motion in various dynamic environments, such as uneven terrain, obstacles, or unstable conditions, and modeling, analyzing, and controlling motion in various situations. They also present successful experimental results. The next work, titled “A method for determination of optimal gaits with application to a snake-like serial-link structure” by Hicks and Ito (2005) [11] introduces a strategy for identifying the most efficient gaits for serpentine robotic systems. The authors use optimization and simulation algorithms to identify the best walking parameters and confirm the success of the proposed method through simulation results. In the work titled “Geometric motion planning analysis for two classes of underactuated mechanical systems” by Shammas, Choset, and Rizzi (2007) [12], the authors develop gaits for two different types of underactuated mechanical systems: primarily kinematic and purely mechanical systems. They define inputs as gaits, which are a series of regulated shape changes in a multi-bodied mechanical system. A similarly important work is the article called Geometric motion planning: The local connection, Stokes’ theorem, and the importance of coordinate choice by Hatton and Choset (2011) [13], where the authors introduce two tools for understanding the fundamental principles of movement: connection vector fields and connection height functions. Connection vector fields depict the geometric link between internal shape changes and the resulting body velocities of a system.

The last decade in the field of application of geometric mechanics in the description of biologically inspired locomotion systems was also rich in interesting works and solutions. For example, the work titled “Locomotive reduction for snake robots” by Xiao et al. (2015) [14] introduces locomotive reduction, a simplifying approach that simplifies the control of a redundant snake robot to that of maneuvering a differential-drive vehicle. The next work titled “Geometric motion planning for systems with toroidal and cylindrical shape spaces” by Gong et al. (2018) [15] focuses on understanding the topology of the form space and develops geometric tools for systems with toroidal and cylindrical shape spaces. In the work titled “A hierarchical geometric framework to design locomotive gaits for highly articulated robots” by Chong et al. (2019) [16], the authors implement a hierarchic framework that decomposes a high-dimensional system into subsystems; they primarily focus on the movement of mechanical systems in two dimensions, in pairs, to achieve the desired movement of the robot. Similarly, in the work titled “Geometric phase predicts locomotion performance in undulating living systems across scales” by Rieser et al. (2019) [17], the authors focus on the geometric framework and the overall simplification of the calculations of the movement of biologically inspired robotic systems without limbs in different scales. The authors conclude that undulating locomotion in a damp environment can be applied to small organisms in viscous substances and animals navigating in viscous liquids like sand. It is therefore easy to describe the dynamics of these systems through sinusoidal modes. In the article titled “Guided Motion Planning for Snake-like Robots Based on Geometry Mechanics and HJB Equation” by Guo et al. (2018) [18], a method is proposed, through which the movement planning of a snake-like robot is realized, which is based on the re-composition of the multi-dimensional configuration space into low-dimensional fiber bundle topological space and gait space. The work thus derives a kinematic connection that maps the movement very well. In the work titled “Moving sidewinding forward: optimizing contact patterns for limbless robots via geometric mechanics” by Chong et al. (2021) [19], the authors deal with contact planning and the subsequent design of a framework for the design and optimization of contact patterns for generating efficient locomotion in the desired directions. The work is based on previous works in the field of geometric mechanics and introduces the optimization of the function that connects the contact state space and the position space. Another important work is “Coordinating tiny limbs and long bodies: Geometric mechanics of lizard terrestrial swimming” by Chong et al. (2022) [20], where the authors use the theory of geometric mechanics and its possibilities to explain observations in biological experiments. In this work, it was found that movement using body undulation with an advancing wave refers to the situation when this wave is generated by the whole body and not from the limbs (in the case of a static wave), and therefore, lizards living in the soil move in the “terrestrial swimming” manner defined by the authors. The interaction between the body and the environment during movement plays a crucial role.

Within our service robotics and mechatronics laboratory, geometric modeling techniques are used. Since 2014, our team has been evaluating and researching the geometric mechanics-based locomotion principles of biologically inspired robotic systems. As shown by several publications on the subject, we concentrate on locomotion based on body shape changes utilizing gradual cyclic waves as well as locomotion of articulated systems where a change in the limb position is applied in accordance with a predetermined movement function [21,22,23].

The suitability of the use of a geometric framework thus consists of its application in solving the problems of mobile robotics, in which the solution of mechanical problems is left to classical mechanics. It is especially suitable for use in locomotion situations with cyclic symmetric movement functions. In the following section, we will introduce basic concepts from the field of geometric mechanics that must be understood for the correct interpretation of mathematical notation. In geometric mechanics, there are many mathematical terms that represent, in most cases, abstract forms. The part itself is the description of appropriate basic forms using a special mathematical formalism. It expresses their mutual relations and subsequently, more complex constructions.

The geometric mechanics method allows one to see into the geometric structure of the equations and is used in the design, modeling, and control of motion planning. It can be used on a variety of locomotion systems, whether they are wheeled robots, legged robots, or underwater robots. It provides a deeper insight into the overall behavior of the system. Advances in the use of geometric mechanics have enabled more realistic models to be created, simulated, and then analyzed in a realistic environment.

Therefore, the aim of this study is to create a conceptual summary of the method of solving mobile non-holonomic symmetric robotic mechanisms, such as a snake-like robot, and to verify the suitability of using geometric mechanics.

2. Materials and Methods

The configuration of a symmetrical mechanical system is a set of variables q₁, q₂, …, q_n that uniquely specify the position of any point on the mechanical system in two-dimensional or three-dimensional Euclidean space. These variables are called configuration variables or generalized variables and are characterized by the fact that they uniquely define the configuration of the mechanical system, while their n is equal to the number of degrees of freedom of the mechanical system. In a simplified way, we can write that the configuration space defines a set of possible arrangements of configurations q in the previously unbounded space known to us. In this way, we can uniquely define every single point in the configuration space that is related to the described mechanical system during its movement. The specific definition also refers to the number of degrees of freedom as they allow us to achieve a specific type of movement, such as translational, rotational, and bending. We denote the configuration space by $\mathcal{Q}$. Its dimension is similar to the number of degrees of freedom of the mechanical system. The dimension, also known as the system dimension, represents the possibilities of movement in mechanical systems and is analogous to the dimensions in Euclidean space. We can also use generalized coordinates within this analogy.

In geometric mechanics, the configuration space of mechanical systems is also called a manifold and is geometrically most often represented by a curve, a circle, or a spherical surface. In order to have a better idea when dealing with the term manifold later, we will rigorously define it in the following subsection.

2.1. Definition of Manifold

The idea of manifold is one of the most fundamental concepts in modern mathematics, cropping up in almost all aspects of mathematics (especially geometry and topology), many parts of physics, and even some parts of engineering. Basically, manifolds are just a way to extend the idea of “surface” to more than three dimensions. Any curve that can be drawn by hand is a one-dimensional manifold, a surface such as a sphere, disc, or torus is a two-dimensional manifold, and the entire 3D space is an example of a three-dimensional manifold.

Variable P, put simply, the manifold $\mathcal{Q}$ of dimension n, is a set of points that is locally similar to the Euclidean space ${\mathbb{R}}^n$, that is, it is a continuous space that is locally Cartesian, and it is possible to perform differentiation on it. A manifold is therefore not a set of rational numbers. However, the rigorous definition of manifold says that the manifold $\mathcal{Q}$ is a set for which every point P lies in some open set $\mathcal{U}$, which is continuously uniquely mapped onto an open subset ${\mathbb{R}}^n$. The symbolic notation of this expression is as follows:

$$\forall P\in \mathcal{Q}\exists \mathcal{U}\subset \mathcal{Q} such that P\in \mathcal{U},$$

(1)

$$and, \exists continuously \varphi :\mathcal{U}\to {\mathbb{R}}^n such that \mathcal{U}⟷\varphi \left(\mathcal{U}\right) is uniquely,$$

(2)

Within this manifold definition, $\varphi$ is a coordinate representation, $\left(\mathcal{U},\varphi \right)$ is a map, and n is the manifold’s dimension. However, the mentioned representation $\varphi$ introduces a local coordinate system on the set $\mathcal{U}$, which is the inverse image ${\varphi }^{-1}$ of the Cartesian system ${\mathbb{R}}^n$ or:

$$\varphi \left(P\in \mathcal{U}\right)\equiv (x^1,\dots ,x^n)\in {\mathbb{R}}^n,$$

(3)

The entire manifold represents a complete set, usable for the given purpose of the application; for this reason, the entire manifold is covered by an atlas or a set of maps. However, local map overlays are required to be smooth enough, as shown in Figure 1.

The manifold is graphically illustrated on the example of a terrestrial globe, where the surface of the globe is an example of a simple manifold. From a geometric point of view, it is a sphere, i.e., a two-dimensional curved continuous surface, on which we can locally introduce specific maps of coordinates for individual regions. Together, the maps form an atlas that covers the entire surface of the globe. The advantage is that trajectories, as well as speeds and accelerations, can be studied on local maps. This means that derivative and integration operations can be performed locally. A manifold is therefore a space that looks like a piece of n-dimensional Cartesian space around each point. One piece of such an n-dimensional space around the selected point is called a map or local coordinates.

Each point x on this map clearly corresponds to n real numbers, i.e., its coordinates. Thanks to this, differential and integral calculus can be meaningfully developed on it. A manifold is a natural generalization of ${\mathbb{R}}^n$. Locally, it is the same as ${\mathbb{R}}^n$, but globally, it need not be. An n-dimensional manifold can be understood as being glued together from several pieces of ${\mathbb{R}}^n$, i.e., maps that make up an atlas. Mechanical systems, with the exception of pathological cases, “live” on manifolds.

2.2. Fundamental Configuration Blocks and Operations within the Configuration Space

The first fundamental configuration blocks that are applicable within the design description include a straight line and a curve. In their cases, the configuration space has one dimension. Mechanically, a straight line can be expressed using a prismatic joint, and in the case of a curve, it is possible to use a tracking ball whose movement along the curve is limited. The symbolic notation ${\mathbb{R}}^1$ makes it possible to designate such one-dimensional systems whose elements are real numbers. The second fundamental configuration block belongs to the circle denoted by the symbolic notation ${\mathbb{S}}^1$. In terms of application, it is represented by a cyclic movement. The set is represented by a spherical surface with one dimension. Structurally, such a block can be expressed by a wheel, a joint, or a closed loop. In these applications, a rotating circular space occurs, which is repeated cyclically. The main feature for identifying the configuration block labeled ${\mathbb{S}}^1$ is the space’s cyclicity, Figure 2. This means that this type of block can also include a system that does not physically have a circular shape but only has a cyclical course. For this reason, this category does not include mechanical joint systems that cannot fully rotate cyclically, i.e., their ability to rotate is only partial.

Another configuration block can be created by combining a straight line and a circular configuration block. This gives us an extended configuration block that enables the use of configuration spaces for much more complex mechanical systems. The simplest of these systems is the configuration block ${\mathbb{R}}^2$ represented by a plane. This configuration block was created by applying the Cartesian product ${\mathbb{R}}^2={\mathbb{R}}^1{\times \mathbb{R}}^1$ of two configuration blocks represented by lines. In reality, it can be a surface on which a mobile robot moves or a connection with a pair of prismatic joints. Similarly, as in the previous case, by applying the Cartesian product to two configuration blocks ${\mathbb{R}}^1\times {\mathbb{S}}^1$, which are represented by a line and a circle, we obtain a configuration block geometrically corresponding to the shape of a cylinder. Mechanically, such a block is represented by a combination of a prismatic and a rotary joint. By combining two configuration blocks in the shape of a circle, one obtains an extended configuration block ${\mathbb{T}}^2={\mathbb{S}}^1\times {\mathbb{S}}^1$, geometrically represented by a torus. In the mechanical application, it is a pair of rotary cyclic joints, as shown in Figure 3.

2.3. Lie Groups

In general, groups are important algebraic systems within mathematics. Formally, they have a good application, for example, in the description of symmetries. The group $\left(\mathcal{G},△\right)$ can be rigorously defined as an algebraic system with a binary operation $△$ on the set $\mathcal{A}$, where $\exists e\in \mathcal{G}$ is a neutral element of this binary operation. The associativity condition must also be fulfilled for the binary operation $△$:

$$\left(\forall x,y,z\in \mathcal{A}\right)\left(\left(x△y\right)△z=x△\left(y△z\right)\right),$$

(4)

Furthermore, it must be true that for every element $x\in \mathcal{G}$, there is an inverse element with respect to the operation $△$. If the binary operation $△$ on the given group is additionally commutative,

$$\left(\forall x,y\in \mathcal{A}\right)\left(\left(x△y\right)=\left(y△x\right)\right),$$

(5)

we call such a group an Abelian group.

However, if we operate on the set $\mathcal{R}$ using the binary operations of addition, multiplication, or composition of functions, the notation of such algebraic systems has the following form: $\left(\mathcal{R},+\right)$ for addition, $\left(\mathcal{R},\cdot \right)$ for multiplication, and $\left({\mathcal{R}}^R,\circ \right)$ for the composition of functions. Within algebraic systems, it is also possible to perform the so-called null operations. To clarify, for example, addition and multiplication are binary operations, and the numbers 0 and 1 are null operations on the set $\mathcal{R}$, where 0 is the additive identity, while 1 is the multiplicative identity. Analogously, we can write that we have a set $\mathcal{G}$, which we call a group if a group operation is defined as one that assigns exactly one element to each ordered pair of $\mathcal{G}$. It therefore applies to group $\mathcal{G}$:

$$x, y \in \mathcal{G}, x\circ y=z\in \mathcal{G},$$

(6)

For an algebraic system to be a group, the following conditions must be fulfilled:

• Associativity for the binary operation $\circ$

  $$\left(\forall x,y,z\in \mathcal{G}\right)\left(\left(x\circ y\right)\circ z=x\circ \left(y\circ z\right)\right),$$

  (7)
• The existence of a neutral (unit) element $e\in \mathcal{G}$ for the operation $\circ$ if and only if:

  $$\left(\forall x\in \mathcal{G}\right)\left(\left(x\circ e\right)=\left(e\circ x\right)=x\right),$$

  (8)
• The existence of an inverse element $x^{-1}\in \mathcal{G}$ for every $x\in \mathcal{G}$:

  $$\left(x\circ x^{-1}\right)=\left(x^{-1}\circ x\right)=e.$$

  (9)

In our case, we are mainly interested in algebraic systems from the point of view of geometry. For this specific view, there is a special class of groups called Lie groups. These are objects in which two different aspects, algebraic and geometric, coexist at the same time. In the main sense, they are groups, but they are also smooth varieties at the same time. If the group elements were not formed by a discrete set but by a continuum, it would be possible to introduce points, i.e., their coordinates in the geometric sense of the word. In calculations, there are often situations where we need to perform algebraic operations such as addition and multiplication within a certain configuration space while operating on a set of real numbers. We conduct these operations to determine the absolute configuration of a component positioned relative to a separate component or to identify the relative configuration of two separate components positioned based on an absolute expression. For example, a Lie group with an additive binary operation on a configuration block of one dimension, namely a line, has a symbolic notation of the form $\left({\mathbb{R}}^1,+\right)$. Another example is the special orthogonal group $\mathcal{S}\mathcal{O}\left(n\right)$, which is a rotation group in an $n$-dimensional space. If the space has two dimensions, the rotation is defined by matrix formal notation:

$$\mathcal{R}\in \mathcal{S}\mathcal{O}\left(2\right)=\left[\begin{array}{cc}\cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right],$$

(10)

$\theta$ represents the rotation angle in matrix notation. This matrix is cyclic, smooth, and unique with respect to the rotation angle; so, it naturally corresponds to the definition of the manifold ${\mathbb{S}}^1$.

2.4. Special Euclidean Group $\mathcal{S}E\left(2\right)$

The $\mathcal{S}E\left(2\right)$ group encompasses the operations of rotation and translation within its structure. The movements can be categorized as “clean,” showing no signs of irregularity. Part of the Euclidean group $\mathcal{S}E\left(2\right)$ is the orthogonal group $\mathcal{S}\mathcal{O}\left(2\right)$, which represents a rotation in the planar-plane within the entire algebraic-structure. The resulting shape can be expressed as a semi-direct product of the orthogonal two-dimensional group and the planar configuration space $\mathcal{S}\mathcal{O}\left(2\right)⋊{\mathbb{R}}^2$, showcasing the interaction between rotational and translational components of the mechanism. For planar systems, the elements of the Euclidean group $\mathcal{S}E\left(2\right)$ have four possible interpretations:

• The first interpretation refers to the position of the rigid body and its orientation at the same time.
• The second interpretation refers to the position of the coordinate frame.
• The third interpretation concerns the movement actions of both the rigid body and the coordinate frame.
• The fourth interpretation refers to the action of identifying a point in another coordinate system relative to the coordinate frame.

It is necessary to state that the first two interpretations are closely related to each other as the position and orientation of rigid bodies are essentially identified by local coordinate systems aligned with the longitudinal and transverse axes of individual links. So, for both of these interpretations, we can say that the group $\mathcal{S}E\left(2\right)$ describes all possible configurations of the body in 2-dimensional space. The third and fourth interpretations of the $\mathcal{S}E\left(2\right)$ group are not only related to space, but also to the Lie group, which creates a certain configuration with each specific action. The geometric meaning of these operations is in the form of rotation and displacement using a homogeneous matrix, where ${\left[\begin{array}{cc}x& y\end{array}\right]}^T$ is the displacement vector and the orthogonal group $\mathcal{S}\mathcal{O}\left(2\right)$ is the submatrix consisting of the first and second rows and first and second columns. The aforementioned actions are divided into right actions and left actions. We denote the left action as $L_{h}g=h\circ g$ and the right action as $R_{h}g=g\circ h$, where the element $h$ acts on the element $g$. Since the group $\mathcal{S}E\left(2\right)$ is not commutative like, for example, Abelian groups, the right and left group actions are also not equivalent. Expressing the action in matrix form, where $h,g\in \mathcal{S}E\left(2\right)$ is the following for the elements $h$ (represents the initial configuration of the body with respect to the origin of the coordinate system) and $g$ (represents the transformation):

$$g\left(x,y,\theta \right)=\left[\begin{array}{ccc}\cos \theta & -\sin \theta & x\\ \sin \theta & \cos \theta & y\\ 0& 0& 1\end{array}\right], h\left(u,v,\beta \right)=\left[\begin{array}{ccc}\cos \beta & -\sin \beta & u\\ \sin \beta & \cos \beta & v\\ 0& 0& 1\end{array}\right],$$

(11)

The realization of the left action is the multiplication of the given matrices in the specified order (the left action of the group element $h$ to the group element $g$).

$$hg=\left[\begin{array}{ccc}\cos \beta & -\sin \beta & u\\ \sin \beta & \cos \beta & v\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}\cos \theta & -\sin \theta & x\\ \sin \theta & \cos \theta & y\\ 0& 0& 1\end{array}\right],$$

(12)

$$hg=\left[\begin{array}{ccc}\cos \left(\theta +\beta \right)& -\sin \left(\theta +\beta \right)& x\cos \beta -y\sin \beta +u\\ \sin \left(\theta +\beta \right)& \cos \left(\theta +\beta \right)& x\sin \beta +y\cos \beta +v\\ 0& 0& 1\end{array}\right],$$

(13)

This resulting left action matrix represents the new rigid body position configuration. The notation of this new position takes the form of a line vector, as shown in Figure 4a.

$$\left[\begin{array}{ccc}x\cos \beta -y\sin \beta +u,& x\sin \beta +y\cos \beta +v,& \left(\theta +\beta \right)\end{array}\right],$$

(14)

The realization of the right action of group element $h$ on element $g$ is as follows:

$$gh=\left[\begin{array}{ccc}\cos \theta & -\sin \theta & x\\ \sin \theta & \cos \theta & y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}\cos \beta & -\sin \beta & u\\ \sin \beta & \cos \beta & v\\ 0& 0& 1\end{array}\right],$$

(15)

$$gh=\left[\begin{array}{ccc}\cos \left(\theta +\beta \right)& -\sin \left(\theta +\beta \right)& x+u\cos \theta -v\sin \theta \\ \sin \left(\theta +\beta \right)& \cos \left(\theta +\beta \right)& y+u\sin \theta +v\cos \theta \\ 0& 0& 1\end{array}\right],$$

(16)

This resulting right action matrix represents the new rigid body position configuration. The notation of this new position takes the form of a line vector:

$$\left[\begin{array}{ccc}x+u\cos \theta -v\sin \theta ,& y+u\sin \theta +v\cos \theta ,& \left(\theta +\beta \right)\end{array}\right],$$

(17)

By using the right action, we have thus achieved the placement of the body’s local coordinate system in a new position represented by the position vector. The graphic interpretation of the action is shown in Figure 4b. For the points p of the rigid body, which are characterized by the coordinates $\left[\begin{array}{cc}{p}_x^b& p_y^b\end{array}\right]$ against the moving coordinate system and if the configuration of the rigid body is $g$, a special calculation of the action on the group $\mathcal{S}E\left(2\right)$ is provided in the following form:

$$\left[\begin{array}{cc}\mathrm{\varnothing }& \begin{array}{c}{p}_x\\ p_y\end{array}\\ \begin{array}{cc}0& 0\end{array}& 1\end{array}\right]=\left[\begin{array}{ccc}\cos \theta & -\sin \theta & x\\ \sin \theta & \cos \theta & y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cc}\mathrm{\varnothing }& \begin{array}{c}{p}_x^b\\ p_y^b\end{array}\\ \begin{array}{cc}0& 0\end{array}& 1\end{array}\right],$$

(18)

$$\left[\begin{array}{c}{p}_x\\ p_y\\ 1\end{array}\right]=\left[\begin{array}{ccc}\cos \theta & -\sin \theta & x\\ \sin \theta & \cos \theta & y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{p}_x^b\\ p_y^b\\ 1\end{array}\right],$$

(19)

It is clear from the matrix (19) that only the positions of the points, but not the orientation, are not considered. This process of mapping from $g$ to global system coordinates is known as direct kinematics.

Using the Euclidean group $\mathcal{S}E\left(2\right)$ to make a mathematical model of a certain mechanism is helpful because it takes into account how the rigid bodies in the mechanism are positioned in relation to each other using the group’s action. For example, the individual displacements in the group $\left({\mathbb{R}}^2\times {\mathbb{S}}^1\right)$ have a reference to the global coordinate framework regarding the $\mathcal{S}E\left(2\right)$ group, and the displacements have a reference to the initial coordinate framework. If $h=g^{-1}$, then the result of the right and left actions of the group is the unit element of the group $\mathcal{S}E\left(2\right)$, i.e., the beginning of the $\mathcal{S}E\left(2\right)$ group configuration space.

$$g\circ g^{-1}=\left[\begin{array}{ccc}\cos \theta & -\sin \theta & x_1\\ \sin \theta & \cos \theta & y_1\\ 0& 0& 1\end{array}\right]\circ \left[\begin{array}{ccc}\cos \theta & \sin \theta & {-x}_1\cos \theta -y_1\sin \theta \\ -\sin \theta & \cos \theta & {x_1\sin \theta -y}_1\cos \theta \\ 0& 0& 1\end{array}\right],$$

(20)

$$g\circ g^{-1}=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right],$$

(21)

2.5. Function, Curve, and Vectors on the Manifold

For further work with the manifold structure, we will define basic concepts such as functions, curves, and vectors on the manifold, which will help better understand the interrelationships between the formal description and the physical interpretation on a rigid body. For the function $f$ on the manifold $\mathcal{Q}$, it is true that the representation $f:\mathcal{Q}⟶\mathbb{R}$ is also a situation where each point $P\in \mathcal{Q}$ is assigned a real number $f\left(P\right)$. In the local coordinates of the map $\left(\mathcal{U},\varphi \right)$, the function $f$ can be represented by its coordinate definition $f\left({\varphi }^{-1}\right)=f\left(x^1,x^2,\dots ,x^n\right)$. Working with functions on the manifold is intuitive as physical interpretation is relatively frequent. To define the curve $\mathsf{\gamma }\left(t\right)$ on the manifold $\mathcal{Q}$, it is necessary to emphasize that it is a differentiable representation $\mathsf{\gamma }:\mathbb{R}⟶\mathcal{Q}$. So, the parametric curve $\mathsf{\gamma }\left(t\right)$ is a smooth representation of $\mathsf{\Omega }⟶\mathcal{Q}$, where $\mathsf{\Omega }\subset \mathbb{R}$ is an open interval. In the local map $\left(\mathcal{U},\varphi \right)$, the surroundings of the point $P\in \mathcal{Q}$ is parameterized by the coordinates $\left(x^1,x^2,\dots ,x^n\right)$; so, the curve $\mathsf{\gamma }\left(t\right)$ is locally defined by the functions $\left(x^1\left(t\right),x^2\left(t\right),\dots ,x^n\left(t\right)\right)$. For our practical implementation, the curve on the manifold represents the possible trajectory of the center of gravity movement, joint coordinates, joint rotation, and a number of other interpretations. Another important concept is defining a vector on a manifold. The vector $\mathit{v}$ at the point $P$ located on the manifold $\mathcal{Q}$ is given by the tangent to the curve $\mathsf{\gamma }\left(t\right)$ located on the manifold $\mathcal{Q}$, while the curve passes through the point $P$. It is characteristic of the vector v that has a direction determined by the curve $\mathsf{\gamma }\left(t\right)$ and a magnitude corresponding to the magnitude of the change $\mathsf{\gamma }\left(t\right)$ at time $t$. If several different curves with different parametrization pass through the point $P$, then the set of all vectors determined by these curves is called the tangential space ${\mathcal{T}}_P\mathcal{Q}$. For the vectors $\mathit{v}$, it is true that they lie in the tangential space ${\mathcal{T}}_P\mathcal{Q}$, which is a linear space and not on the manifold $\mathcal{Q}$. The application of these vectors is, for example, in the description of kinematic quantities of speed. Dealing with the speeds of the mechanical system is an essential part of the overall analysis of the behavior of the mechanism. The configuration of the body thus changes not only on the basis of the position coordinate $q$, but also on the basis of the velocity coordinate $\dot{q}$.

2.6. Velocities of the Mechanical Systems and Tangent Spaces

When focusing on the analysis of a symmetric mechanical system, it is important to focus not only on the current position coordinates $q$ but also on their first derivatives representing the velocity coordinates $\dot{q}$. Their change simultaneously changes the overall configuration of the investigated mechanism, which actually defines its current state. After choosing a suitable configuration space for the mechanical system, the next step in modeling its behavior is determining the velocities, i.e., the speed and direction in which the mechanical system moves. In general, velocities can be interpreted geometrically as tangent vectors to the configuration of the system, corresponding to infinitesimal changes in the configuration over time. With regard to geometric mechanics, velocity vectors are elements of tangential space, which can be imagined as a tangent plane to a specific manifold. If we focus on the concept of space, we will find that it is closely connected to the definition of its dimension; that is, it depends on the dimension of the manifold. The difference is that if we consider the manifold ${\mathbb{S}}^1$, then, the geometric representation of such a configuration space is in the shape of a circle. In this case, the velocity vector on this manifold is geometrically represented by a tangent to the circle. The tangent space thus has one dimension. If we work with a manifold with a higher number of dimensions, such as the torus ${\mathbb{T}}^2={\mathbb{S}}^1\times {\mathbb{S}}^1$, then, in this case, the velocity vector is located in the tangential space, which is represented by the contact plane. The set of all such n-dimensional tangent spaces is a new configuration space, i.e., a new manifold, which, however, is called a tangent manifold “tangent bundle” $\mathcal{T}\mathcal{Q}$. If we understand the configuration of the coordinate system $q$ as a point on the manifold $\mathcal{Q}$, then its velocity $\dot{q}$ is the direction, and the speed with which this point moves through the manifold is the vector that we can think of as “connected” to the manifold in the current configuration. The velocity vectors that a mechanical system can have at a given configuration $q$ are parts of a vector space named the tangent space at $\mathcal{Q}$ in that configuration, ${\mathcal{T}}_q\mathcal{Q}$, which generally contains all possible differential changes in configuration from point $q$. Tangent spaces on an n-dimensional manifold are ${\mathbb{R}}^{\mathrm{n}}$ vector spaces and can be conceptualized of as “linearization” of the manifold or vector spaces that come closest to the manifold at each point. The term “tangent space” reflects the idea that small changes in configuration are always in the direction contained on the manifold and are thus “tangent lines” to it. Like manifolds, tangent spaces and the vectors they contain exist as geometric objects that are independent of any coordinates used to describe them. However, to perform calculations on these vectors, it is useful to assign a basis to the tangent space so that each vector is parameterized by a set of real numbers. In the case of Rn varieties, the velocity calculation is simpler. Rigorously, we can characterize the tangent bundle $\mathcal{T}\mathcal{Q}$ as the union of all tangent spaces ${\mathcal{T}}_P\mathcal{Q}$ at all points $P$ on the manifold $\mathcal{Q}$, that is, $\mathcal{T}\mathcal{Q}\equiv \bigcup _{P\in \mathcal{Q}}{\mathcal{T}}_P\mathcal{Q}$. Each point on the new manifold $\mathcal{T}\mathcal{Q}$ represents a specific tangent vector ${\mathit{v}\in \mathcal{T}}_P\mathcal{Q}$. For a tangent bundle, there exists a point $W\in \mathcal{T}\mathcal{Q}$, which determines the vector ${\mathit{v}\in \mathcal{T}}_P\mathcal{Q}$ and a point $P\in \mathcal{Q}$, where the vector $\mathit{v}$ is tangent to the manifold $\mathcal{Q}$. There is also a representation $\pi :\mathcal{T}\mathcal{Q}⟶\mathcal{Q}$, while $\pi \left(W\right)=P$ holds. Furthermore, for the tangent bundle, the point $P\in \mathcal{Q}$ has the components of local coordinates in the form $x^i$ and the components of the vector $\mathit{v}$ have the form $v^i$ in the basis $\left\{\frac{\partial }{\partial x^i}\right\}$. For an existing point $W\in \mathcal{T}\mathcal{Q}$, it holds that it has natural components of coordinates in the form $\left(x^1,x^2,\dots ,x^n,v^1,v^2,\dots ,v^n\right)$, while the representation $\pi$ is in coordinates defined simply as $\pi \left(x^1,x^2,\dots ,x^n,v^1,v^2,\dots ,v^n\right)=\left(x^1,x^2,\dots ,x^n\right)$.

2.7. Lifted Actions with Vectors in the Tangent Manifold

During operations of addition and comparison of velocity vectors in individual tangent spaces and at the same time, in the tangent manifold, we will introduce a new type of method, the so-called “lifted actions”. These are directly associated with transformations. Similar to how the left and right actions apply to the Lie groups for the initial configuration $g$, it is analogously true that the lifted actions also have a left and right variant. For example, on a Lie group, a left action $L_h$ applied to the initial configuration $g$ using a left-lifted action $T_g{L}_h$ will allow a vector from the initial tangent space $T_{g}G$ to be transformed into equivalent vectors in the final tangent space $T_{hg}G$.

$$T_g{L}_h:T_{g}G⟶T_{hg}G,$$

(22)

Similarly, for the right-lifted action:

$$T_g{R}_h:T_{g}G⟶T_{gh}G,$$

(23)

Lifted actions can be calculated as differentials corresponding to the respective actions:

$$T_g{L}_h=\frac{\partial \left(L_{h}g\right)}{\partial g},$$

(24)

$$T_g{R}_h=\frac{\partial \left(R_{h}g\right)}{\partial g},$$

(25)

$$\frac{\partial \left(L_{h}g\right)}{\partial g}=\frac{\partial \left(R_{h}g\right)}{\partial g}=\frac{\partial \left(\left[h_1,h_2\right]+\left[g_1,g_2\right]\right)}{\partial \left[g_1,g_2\right]}=\left[\begin{array}{cc}\frac{\partial \left(h_1+g_1\right)}{\partial g_1}& \frac{\partial \left(h_1+g_1\right)}{\partial g_2}\\ \frac{\partial \left(h_2+g_2\right)}{\partial g_1}& \frac{\partial \left(h_2+g_2\right)}{\partial g_2}\end{array}\right],$$

(26)

2.8. Velocities of a Rigid Body

In general, velocities can be interpreted geometrically as tangent vectors to the configuration of the system, corresponding to infinitesimal changes in the configuration over time. The special Euclidean group $\mathcal{S}E\left(2\right)$ describes the configuration of a rigid body. As already mentioned, the speed of a mechanical system can be defined by how fast the group’s operations are performed. Let the inertial coordinate system and the local coordinate system be given, and let the motion of the rigid body be described by a trajectory on the $\mathcal{S}E\left(2\right)$ group, i.e., $t\to g\left(t\right)=\left\{x\left(t\right), y\left(t\right), \theta \left(t\right)\right\}$, where $t$ is time. In a group statement, this can be expressed as:

$$t\to g\left(t\right)=\left[\begin{array}{ccc}\cos \theta & -\sin \theta & \mathrm{x}\left(\mathrm{t}\right)\\ \sin \theta & \cos \theta & \mathrm{y}\left(\mathrm{t}\right)\\ 0& 0& 1\end{array}\right],$$

(27)

where it is assumed that the curve $g\left(t\right)$ is differentiable.

The velocities on Lie groups are defined by the velocity with which the infinitesimal actions of the group are applied to the configuration. Let the group (body) move by infinitesimal time $\delta \left(t\right)$:

$$g_{\delta }=g^{-1}\left(t\right)g(t+\delta t),$$

(28)

Then the group velocity $\xi$ is calculated as the difference between $g_{\delta }$ and the unit element of the group $\mathit{e}$, which is divided by the time for which the displacement $g_{\delta }$ was performed:

$$\xi =\underset{\mathit{\delta t}\to 0}{\lim }\frac{g_{\delta }-\mathit{e}}{{\delta }_t},$$

(29)

If there is a difference between the left and right actions of the group $g(t+\delta t)$, then the relation $g^{-1}\left(t\right)g(t+\delta t)$ is the right shift from $g\left(t\right)$ to $g(t+\delta t)$ and then:

$$\xi =\underset{\mathit{\delta t}\to 0}{\lim }\frac{g_{\delta }-\mathit{e}}{{\delta }_t}=\underset{\mathit{\delta t}\to 0}{\lim }\frac{{g^{-1}\left(t\right)g\left(t+\delta t\right)-g^{-1}g}_{\delta }}{{\delta }_t}=$$

(30)

$$=g^{-1}\left(t\right)\underset{\mathit{\delta t}\to 0}{\lim }\frac{{g\left(t+\delta t\right)-g}_{\delta }}{{\delta }_t}=g^{-1}\left(t\right)\dot{g}\left(t\right)=\overrightarrow{\xi },$$

(31)

defines the right group velocity $\overrightarrow{\xi }$.

Substituting the left displacement $g(t+\delta t)g^{-1}\left(t\right)$ into Equation (29) produces the left group velocity $\overleftarrow{\xi }$:

$$\xi =\underset{\mathit{\delta t}\to 0}{\lim }\frac{g_{\delta }-\mathit{e}}{{\delta }_t}=\underset{\mathit{\delta t}\to 0}{\lim }\frac{{g\left(t+\delta t\right)g^{-1}\left(t\right)-g}_{\delta }{g}^{-1}\left(t\right)}{{\delta }_t}=$$

(32)

$$=\underset{\mathit{\delta t}\to 0}{\lim }\frac{{g\left(t+\delta t\right)-g}_{\delta }}{{\delta }_t}{g}^{-1}\left(t\right)={\dot{g}\left(t\right)g}^{-1}\left(t\right)=\overleftarrow{\xi },$$

(33)

The group velocities $\overrightarrow{\xi }$ and $\overleftarrow{\xi }$ are an element of the tangent space in the unit group e, which is called the Lie algebra. This vector space is a linearization of a group around its unit $\mathit{e}$ in a very similar way as a tangent space is a linearization of a manifold around a given point. The natural basis can thus be obtained from members of the 1st order of the Taylor expansion of the group $g\left(t\right)$, which means that the group element representing the infinitesimal displacement is as follows:

$${\mathit{g}}_{\delta }=\mathit{e}+{\sum }_i\left.\frac{\partial g}{\partial g^i}\right|{\mathit{q}}_{\delta }^i,$$

(34)

The generation of the basis for the group velocities thus results from adjusting Equation (34) after substituting into Equation (29), removing the group unit $\mathit{e}$ and dividing the remainder by $\delta \left(t\right)$:

$$\xi ={{\sum }_i\left.\frac{\partial g}{\partial g^i}\right|}_e\left(\underset{\mathit{\delta t}\to 0}{\lim }\frac{q_{\delta }^i}{{\delta }_t}\right)={\sum }_i\frac{\partial g}{\partial g^i},$$

(35)

The resulting matrix $\xi$ in Appendix A represents an element of the Lie algebra. Its three parameters ${\xi }_x$, ${\xi }_y$, and ${\xi }_{\theta }$ describe the group velocity of a planar rigid body in the $\mathcal{S}E\left(2\right)$ group unit. Further, the left group velocity $\overleftarrow{\xi }$ will be called the spatial group velocity and will be denoted by the symbol ${\xi }^s$, $\overleftarrow{\xi }{=\xi }^s$, and the right group velocity $\overrightarrow{\xi }$ will be called the local group velocity ${\xi }^b$, i.e., $\overrightarrow{\xi }={\xi }^b$, and their expression will be the following:

$${\xi }^s=\dot{g}\left(t\right)g^{-1}\left(t\right),$$

(36)

$${\xi }^b=g^{-1}\left(t\right)\dot{g}\left(t\right),$$

(37)

Considering that the movement of a rigid body imitating the movement of a biological model is studied and is not acted upon by any external forces, the spatial ${\xi }^s$ and local ${\xi }^b$ velocities are constant during the movement. This results in two matrix differential equations: one with spatial and the other with local velocity.

$$\dot{g}\left(t\right)={\xi }^{s}g\left(t\right),$$

(38)

$$\dot{g}\left(t\right)=g\left(t\right){\xi }^b,$$

(39)

To obtain the solution of Equation (38), assuming that $g\left(t\right)$, $\dot{g}\left(t\right)$, $\xi$ are scalars, we will proceed with the following relations. We obtain and modify the differential equation of the first order, where the initial condition is $g\left(0\right)=g0$.

$$\dot{g}\left(t\right)=\xi g\left(t\right),$$

(40)

$$g\left(t\right)=e^{\xi t}g0,$$

(41)

Matrix equations also have analogous solutions of Equations (40) and (39):

$$g\left(t\right)=\mathrm{e}\mathrm{x}\mathrm{p}\left({\xi }^{s}t\right)g\left(0\right),$$

(42)

$$g\left(t\right)=g\left(0\right)\mathrm{e}\mathrm{x}\mathrm{p}\left({\xi }^{b}t\right),$$

(43)

The set of all these solutions $g\left(t\right)$ to the differential equation $\dot{g}\left(t\right)={\xi }^{s}g\left(t\right) \bigvee \dot{g}\left(t\right)=g\left(t\right){\xi }^b$ for all possible initial conditions $g\left(0\right)$ is called the flow of the vector field. We can interpret these solutions as a representation that any point $g\left(0\right)$ moves along the integral curve $g\left(t\right)$ from the point $g\left(0\right)$ to the point $\exp \left({\xi }^{s}t\right)g\left(0\right) \bigvee g\left(0\right)\mathrm{e}\mathrm{x}\mathrm{p}\left({\xi }^{b}t\right)$. Formally, we can express it as ${\Phi }^{\xi }:RxG\to G$ and $(t,g)\to \Phi \left(\exp \left(t\xi \right),g\right)$. The trajectories $g\left(t\right)$ induced by the vector field’s flow are called integral curves (i.e., curves that do not leave the unit group $\mathit{e}$ at time $t^{,}0$). Only one integral curve starts from the point $\mathit{e}$, and it is called a one-parameter subgroup. The manifold of the group $\mathcal{S}E\left(2\right)$ is thus filled with an infinite system of curves $g\left(t\right)$, while these curves do not intersect anywhere, and the velocity of movement along them is determined by the group velocity ξ, which is constant. The integral curve $g\left(t\right)$, which starts from the unit of the group $\mathit{e}$, is called a one-parameter subgroup of the group $\mathcal{S}E\left(2\right)$. Its entire course is fully determined by where and how quickly it leaves the unit of group $\mathit{e}$ at time $t=0$, i.e., by its tangent vector $\dot{g}\left(t\right)=\mathit{\xi }$ in the unit group e. Its solution has the following form:

$$g\left(t\right)=\exp \left(\xi t\right)=I+\xi t+\frac{{\xi }^2}{2!}{t}^2+\frac{{\xi }^3}{3!}{t}^3+,$$

(44)

The overall expression in $x\left(t\right)$ and $y\left(t\right)$ coordinates can be found in Appendix B. The final form of these equations is as follows:

$$x\left(t\right)=\left(\cos {\xi }_{\theta }t-1\right)\frac{{\xi }_y}{{\xi }_{\theta }}+\frac{{\xi }_x}{{\xi }_{\theta }}\sin {\xi }_{\theta }t,$$

(45)

$$y\left(t\right)=\left(\sin {\xi }_{\theta }t\right)\frac{{\xi }_y}{{\xi }_{\theta }}+\frac{{\xi }_x}{{\xi }_{\theta }}(1-\cos {\xi }_{\theta }t),$$

(46)

$$\theta \left(t\right)={\xi }_{\theta }t,$$

(47)

From Equations (45) and (47), it is possible to notice that it is actually a representation of the group velocities ${\xi }_x$, ${\xi }_y,$ and ${\xi }_{\theta }$ in the Lie algebra to the configurations (positions) $x\left(t\right)$, $y\left(t\right),$ and $\theta \left(t\right)$ on the Lie group. This representation is called the exponential representation and is defined as:

$$exp:se\left(2\right)\to \mathcal{S}E\left(2\right),$$

(48)

$$\xi \to \mathit{exp}\left(\xi \right)==e^{\xi },$$

(49)

The image of the element $\xi$ on the one-parameter subgroup $g\left(t\right)$ is assigned a point to which we arrive at time $t=1$ if at time $t=0$, we start from point $\mathit{e}$ with the initial velocity $\xi$ and for the entire time we go uniformly in a straight-line geodetics, as shown in Figure 5.

2.9. Left- and Right-Lifted Actions

It is characteristic of the left-lifted action that it preserves the expression of the velocity of the rigid body regarding the local coordinate system, the velocity of which is expressed in two different approaches. One possible way is to describe the velocity of the body in the plane using a forward, rotational, or lateral component. For the right-lifted action $T_g{R}_h$, in turn, it is true that it preserves the expression of the spatial velocity and its simplification of the calculation at fixed points relative to the rigid body.

$$T_g{L}_h=\frac{\partial \left(hg\right)}{\partial g}=\left[\begin{array}{ccc}\frac{\partial \left(x\cos \beta -y\sin \beta +u\right)}{\partial x}& \frac{\partial \left(x\cos \beta -y\sin \beta +u\right)}{\partial y}& \frac{\partial \left(x\cos \beta -y\sin \beta +u\right)}{\partial \theta }\\ \frac{\partial \left(x\sin \beta +y\cos \beta +v\right)}{\partial x}& \frac{\partial \left(x\sin \beta +y\cos \beta +v\right)}{\partial y}& \frac{\partial \left(x\sin \beta +y\cos \beta +v\right)}{\partial \theta }\\ \frac{\partial \left(\theta +\beta \right)}{\partial x}& \frac{\partial \left(\theta +\beta \right)}{\partial y}& \frac{\partial \left(\theta +\beta \right)}{\partial \theta }\end{array}\right],$$

(50)

$$T_g{L}_h=\frac{\partial \left(hg\right)}{\partial g}=\left[\begin{array}{ccc}\cos \beta & -\sin \beta & 0\\ \sin \beta & \cos \beta & 0\\ 0& 0& 1\end{array}\right],$$

(51)

The rigid body is moved from the g configuration to the center of the inertial coordinate system by the left lift action. Velocity vectors on a group are said to be equivalent if their configuration velocities are equal to the configuration velocity of the group velocity ${\xi }^b$ or ${\xi }^s$. The use of this fact will make it possible to find the relationship between the vectors on the group and consequently, the lifted action. Let the group velocity ${\xi }^b$ and the velocity vector $\dot{g}$ be in the configuration $g=(x,y,z)$. Applying the left action $h=(u,v,\beta )$ to the configuration $g$ leads to the new configuration $L_{h}g=hg$. The velocity vector in this configuration is denoted as $\left(h\dot{g}\right)$. It is necessary to find the relationship (lifted action) between the velocity vector $\left(h\dot{g}\right)$ and the velocity vector $\dot{g}$. By comparing the group velocities ${\xi }^b$ from both configurations, we obtain the following:

$$\dot{g}=g{\xi }^b\Rightarrow {\xi }^b=g^{-1}\dot{g},$$

(52)

$$(\dot{hg)}=hg{\xi }^b\Rightarrow {\xi }^b{\left(hg\right)}^{-1}(h\dot{g}),$$

(53)

$${\left(hg\right)}^{-1}(\dot{hg)}=g^{-1}\dot{g},$$

(54)

$$(\dot{hg)}=\left[\left(hg\right)g^{-1}\right]\dot{g},$$

(55)

$$\left(hg\right)g^{-1}=\left[\begin{array}{ccc}\cos \beta & -\sin \beta & \mathrm{u}\\ \sin \beta & \cos \beta & \mathrm{v}\\ 0& 0& 1\end{array}\right],$$

(56)

$$(\dot{hg)}=\left[\left(hg\right)g^{-1}\right]\dot{g}=\left[\begin{array}{ccc}\cos \beta & -\sin \beta & \mathrm{u}\\ \sin \beta & \cos \beta & \mathrm{v}\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}-\dot{\theta }\sin \theta & -\dot{\theta }\cos \theta & \dot{x}\\ \dot{\theta }\cos \theta & -\dot{\theta }\sin \theta & \dot{y}\\ 0& 0& 0\end{array}\right],$$

(57)

$$\begin{array}{l}\left[\left(hg\right)g^{-1}\right]\dot{g}\\ =\left[\begin{array}{ccc}-\dot{\theta }\cos \beta \sin \theta -\dot{\mathit{\theta }}\sin \beta \cos \theta & -\dot{\theta }\cos \beta \cos \theta +\dot{\theta }\sin \beta \sin \theta & -\dot{x}\cos \beta -\dot{y}\mathrm{s}\mathrm{i}\mathrm{n}\beta \\ -\dot{\theta }\sin \beta \sin \theta +\dot{\theta }\cos \beta \cos \theta & -\dot{\mathit{\theta }}\sin \beta \cos \theta -\dot{\theta }\cos \beta \sin \theta & \dot{x}\sin \beta +\dot{y}\mathrm{c}\mathrm{o}\mathrm{s}\beta \\ 0& 0& 0\end{array}\right],\end{array}$$

(58)

$$(\dot{hg)}=\left[\left(hg\right)g^{-1}\right]\dot{g}=\left[\begin{array}{ccc}-\dot{\theta }\sin \left(\theta -\beta \right)& -\dot{\theta }\cos \left(\theta -\beta \right)& -\dot{x}\cos \beta -\dot{y}\mathrm{s}\mathrm{i}\mathrm{n}\beta \\ \dot{\theta }\cos \left(\theta -\beta \right)& -\dot{\theta }\sin \left(\theta -\beta \right)& \dot{x}\sin \beta +\dot{y}\mathrm{c}\mathrm{o}\mathrm{s}\beta \\ 0& 0& 1\end{array}\right]$$

(59)

The dependence between velocities ${\dot{\left(hg\right)}}_{\dot{x}}$, ${\dot{\left(hg\right)}}_{\dot{y}}$, ${\dot{\left(hg\right)}}_{\dot{\theta }}$ a $\dot{x}, \dot{y}, a \dot{\theta }$ can be expressed in the following way:

$${\dot{\left(hg\right)}}_{\dot{x}}=-\dot{x}\cos \beta -\dot{y}\mathrm{s}\mathrm{i}\mathrm{n}\beta ,$$

(60)

$${\dot{\left(hg\right)}}_{\dot{y}}=\dot{x}\sin \beta +\dot{y}\mathrm{c}\mathrm{o}\mathrm{s}\beta ,$$

(61)

$${\dot{\left(hg\right)}}_{\dot{\theta }}=1,$$

(62)

In matrix form, the relationship between velocities is expressed as:

$$\left[\begin{array}{c}{\dot{\left(hg\right)}}_{\dot{x}}\\ {\dot{\left(hg\right)}}_{\dot{y}}\\ {\dot{\left(hg\right)}}_{\dot{\theta }}\end{array}\right]=\left[\begin{array}{ccc}\cos \beta & -\sin \beta & 0\\ \sin \beta & \cos \beta & 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\theta }\end{array}\right],$$

(63)

or by symbolic notation:

$$\dot{\left(hg\right)}=T_g{L}_h\dot{g},$$

(64)

Any left action $L_h$ that rotates the local coordinate system by an angle $\beta$ accompanies the lifted action $T_g{L}_h$ by rotating the velocity vector by the same value. In addition, the left-lifted action has an interesting property where $h$ is equal to $g^{-1}$. By combining $L_{g^{-1}}$ and $T_g{L}_{g^{-1}}$, a solid body is taken from $g$ and placed at the origin of the coordinate system with an equivalent velocity, thus aligning the local coordinate frame with the global frame. For the velocity of such a body:

$$\xi =T_g{L}_{g^{-1}}\dot{g},$$

(65)

After inverting relation (65), we can obtain the world velocities:

$$\dot{g}={\left(T_g{L}_{g^{-1}}\right)}^{-1}\xi =T_e{L}_g\xi ,$$

(66)

For the right-lifted action $T_g{R}_h$, in turn, it is true that it preserves the expression of the spatial velocity and its simplification of the calculation at fixed points relative to the rigid body. Just as the left-lifted action $T_g{L}_h$ was obtained, it is also possible to determine the right-lifted action $T_g{R}_h$ on the group $\mathcal{S}E\left(2\right)$. Let the group velocity ${\xi }^s$ and the velocity vector $\dot{g}$ be in the configuration $g=(x,y,\theta )$. Applying the right action $h=(u,v,\beta )$ to the configuration $g$ leads to the new configuration $R_{h}g=gh$. The velocity vector in this configuration is denoted as $(\dot{gh)}$. It is necessary to find the relationship (lifted action) between the velocity vector $(\dot{gh)}$ and the velocity vector $\dot{g}$. By comparing the group velocities ${\xi }^s$ of both configurations, we obtain the following:

$$\dot{g}={\xi }^{s}g\Rightarrow {\xi }^s=\dot{g}{g}^{-1},$$

(67)

$$\left(\dot{gh}\right)={\xi }^{s}gh\Rightarrow {\xi }^s={\left(gh\right)\left(g\dot{h}\right)}^{-1},$$

(68)

$$\left(\dot{gh}\right){\left(gh\right)}^{-1}=\dot{g}{g}^{-1},$$

(69)

For searched vector $\left(\dot{gh}\right)$, a $\dot{g}$ is valid:

$$\left(\dot{gh}\right)=\dot{g}\left[g^{-1}\left(gh\right)\right],$$

(70)

$$\left(\dot{gh}\right)=\left[\begin{array}{ccc}-\dot{\theta }\sin \theta & -\dot{\theta }\cos \theta & \dot{x}\\ \dot{\theta }\cos \theta & -\dot{\theta }\sin \theta & \dot{y}\\ 0& 0& 0\end{array}\right]\left[\begin{array}{ccc}\mathit{cos}\beta & -\mathit{sin}\beta & u\\ \mathit{sin}\beta & \mathit{cos}\beta & v\\ 0& 0& 1\end{array}\right]=$$

(71)

$$=\left[\begin{array}{ccc}-\dot{\dot{\theta }\sin \theta }\cos \beta -\dot{\theta }\cos \theta \sin \beta & \dot{\theta }\sin \theta \sin \beta -\dot{\theta }\cos \theta \cos \beta & -u\dot{\theta }\sin \theta -v\dot{\theta }\cos \theta +\dot{x}\\ \dot{\theta }\cos \theta \cos \beta -\dot{\theta }\sin \theta \sin \beta & -\dot{\mathit{\theta }}\cos \theta \sin \beta -\dot{\theta }\sin \theta \cos \beta & u\dot{\theta }\cos \theta -v\dot{\theta }\sin \theta +\dot{y}\\ 0& 0& 1\end{array}\right],$$

(72)

$$\left(\dot{gh}\right)=\left[\begin{array}{ccc}-\dot{\theta }\sin (\theta +\beta )& -\dot{\theta }\cos (\theta +\beta )& \dot{x}-\left(u\sin \theta +v\cos \theta \right)\dot{\theta }\\ \dot{\theta }\cos (\theta +\beta )& -\dot{\theta }\sin (\theta +\beta )& \dot{y}+\left(u\cos \theta -v\sin \theta \right)\dot{\theta }\\ 0& 0& 1\end{array}\right],$$

(73)

The resulting relationship of the right-lifted action by components is as follows:

$${\left(\dot{gh}\right)}_{\dot{x}}=\dot{x}-\left(u\sin \theta +v\cos \theta \right)\dot{\theta },$$

(74)

$${\left(\mathrm{g}\dot{h}\right)}_{\dot{y}}=\dot{y}+\left(u\cos \theta -v\sin \theta \right)\dot{\theta },$$

(75)

$${\left(\mathrm{g}\dot{h}\right)}_{\dot{\theta }}=1,$$

(76)

In matrix form, the relationship between velocities is expressed as:

$$\left[\begin{array}{c}{\left(\dot{gh}\right)}_{\dot{x}}\\ {\left(\dot{gh}\right)}_{\dot{y}}\\ {\left(\dot{gh}\right)}_{\dot{\theta }}\end{array}\right]=\left[\begin{array}{ccc}1& 0& \dot{x}-\left(u\sin \theta -v\cos \theta \right)\dot{\theta }\\ 0& 1& \dot{y}-\left(u\cos \theta -v\sin \theta \right)\dot{\theta }\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\theta }\end{array}\right],$$

(77)

$$T_g{R}_h=\left[\begin{array}{ccc}1& 0& \dot{x}-\left(u\sin \theta +v\cos \theta \right)\dot{\theta }\\ 0& 1& \dot{y}+\left(u\cos \theta -v\sin \theta \right)\dot{\theta }\\ 0& 0& 1\end{array}\right],$$

(78)

$$\left(\dot{gh}\right)=T_g{R}_h\dot{g},$$

(79)

The right-lifted action preserves the spatial velocity ${\xi }^s$. This group velocity is the velocity of an imaginary point of a rigid body, which, at a given moment, passes through the beginning of the inertial coordinate system, and its value is calculated according to the following equation:

$${\xi }^s=T_g{R}_{g^{-1}}\dot{g},$$

(80)

By inverting the equation, the spatial velocity becomes the world velocity for any local rigid body coordinate system:

$$\dot{g}={\left(T_g{R}_{g^{-1}}\right)}^{-1}{\xi }^s=T_e{R}_g{\xi }^s,$$

(81)

2.10. Spatial Velocity and Its Determination Using Adjoint Operators

After applying the right action $R_{h}G$ to the Euclidean group $\mathcal{S}\mathrm{E}\left(2\right)$, the local coordinate system is located in the position and orientation of the group element $h$ with respect to the group element $g$. When determining such a local coordinate system, for example, regarding articulated systems, it is subsequently appropriate to determine its velocity as a function of the group element $\dot{g}$. This speed can also be expressed using standard kinematic equations:

$$\left(\dot{gh}\right)=\left[\begin{array}{ccc}1& 0& -\left(u\sin \theta +v\cos \theta \right)\\ 0& 1& u\cos \theta -v\sin \theta \\ 0& 0& 1\end{array}\right]\dot{g},$$

(82)

We can also express this velocity using the right-lifted action as follows:

$$\begin{array}{l}{T}_g{R}_h=\frac{\partial \left(gh\right)}{\partial g}\\ =\left[\begin{array}{ccc}\frac{\partial \left(x+u\cos \theta -v\sin \theta \right)}{\partial x}& \frac{\partial \left(x+u\cos \theta -v\sin \theta \right)}{\partial y}& \frac{\partial \left(x+u\cos \theta -v\sin \theta \right)}{\partial \theta }\\ \frac{\partial \left(y+u\sin \theta +v\cos \theta \right)}{\partial x}& \frac{\partial \left(y+u\sin \theta +v\cos \theta \right)}{\partial y}& \frac{\partial \left(y+u\sin \theta +v\cos \theta \right)}{\partial \theta }\\ \frac{\partial \left(\theta +\beta \right)}{\partial x}& \frac{\partial \left(\theta +\beta \right)}{\partial y}& \frac{\partial \left(\theta +\beta \right)}{\partial \theta }\end{array}\right],\end{array}$$

(83)

$$T_g{R}_h=\left[\begin{array}{ccc}1& 0& -\left(u\sin \theta +v\cos \theta \right)\\ 0& 1& u\cos \theta -v\sin \theta \\ 0& 0& 1\end{array}\right],$$

(84)

As shown above, Equations (82) and (84) are the same. However, the difference is that by using the right-lifted action, its spatial velocity ${\xi }^s$ is preserved. The spatial velocity ${\xi }^s$ can be defined as the velocity of a rigid body of an imaginary point located on the rigid body at the moment it passes through the origin of the coordinate system. It is represented in the body’s local coordinate system, and we can calculate it using the following equation:

$${\xi }^s=T_g{R}_{g^{-1}}\dot{g},$$

(85)

If we perform the inversion of Equation (85) by simple processing, we arrive at a relation that expresses the world speed $\dot{g}$ for any local coordinate system, as follows:

$$\dot{g}={\left(T_g{R}_{g^{-1}}\right)}^{-1}{\xi }^s=T_e{R}_g{\xi }^s,$$

(86)

The spatial velocity of a body is a suitable mathematical construction connected together with the world velocity and the velocity of the body. Adjoint operators are used for transformations between the spatial velocity ${\xi }^s$ and the body’s own velocity $\xi$ and vice versa.

These adjoint operators consist of a pair of lifted actions, and together, they can form adjoint actions of Lie groups. For example, the adjoint operation ${Ad}_g$ is used to map the body velocity $\xi$ to the spatial velocity ${\xi }^s$:

$${\xi }^s=\stackrel{{Ad}_g}{\overbrace{\left(T_g{R}_{g^{-1}}\right)\left(T_e{L}_g\right)}}\xi ,$$

(87)

The inverse adjoint operation $A{d}_g^{-1}$ is used for the inverse mapping of the spatial velocity ${\xi }^s$ to the body velocity $\xi$:

$${\xi }^b=\stackrel{A{d}_g^{-1}}{\overbrace{\left(T_g{L}_{g^{-1}}\right)\left(T_e{R}_g\right)}}{\xi }^s,$$

(88)

The velocity vector $\dot{g}$ in configuration g has the same value in the relations $\dot{g}=g{\xi }^b$ and $\dot{g}={\xi }^{s}g$. By comparing these two relationships, we obtain the following:

$$g{\xi }^b={\xi }^{s}g,$$

(89)

Their mutual dependence is obtained, as follows:

$${\xi }^b=g^{-1}{\xi }^{s}g,$$

(90)

After subsequent modifications implemented in Appendix C, we obtained the final shape of the individual components of the vector ${\xi }^b$:

$$\left[\begin{array}{c}{\xi }_1^b\\ {\xi }_2^b\\ {\xi }_3^b\end{array}\right]=\left[\begin{array}{ccc}\cos \theta & \sin \theta & x\cos \theta -y\sin \theta \\ -\sin \theta & \cos \theta & x\sin \theta +\cos \theta \\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{\xi }_1^s\\ {\xi }_2^s\\ {\xi }_3^s\end{array}\right],$$

(91)

The same applies to the inverse relationship.

$${\xi }^s=g{\xi }^b{g}^{-1},$$

(92)

where the entire form is detailed in Appendix C.

$$\left[\begin{array}{c}{\xi }_1^s\\ {\xi }_2^s\\ {\xi }_3^s\end{array}\right]=\left[\begin{array}{ccc}\cos \theta & -\sin \theta & \mathrm{y}\\ \sin \theta & \cos \theta & -\mathrm{x}\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{\xi }_1^b\\ {\xi }_2^b\\ {\xi }_3^b\end{array}\right],$$

(93)

An adjoint operation is described by the following equation:

$${Ad}_g=\left(\begin{array}{ccc}\cos \theta & -\sin \theta & \mathrm{y}\\ \sin \theta & \cos \theta & -\mathrm{x}\\ 0& 0& 1\end{array}\right),$$

(94)

The adjoint action ${Ad}_g$ on the Euclidean group $\mathcal{S}\mathrm{E}\left(2\right)$ maps the velocity of the rigid body $\xi$ into the spatial velocity ${\xi }^s$ of the rigid body:

$${\xi }^s=\stackrel{T_g{R}_{g^{-1}}}{\overbrace{\left[\begin{array}{ccc}1& 0& y\\ 0& 1& -x\\ 0& 0& 1\end{array}\right]}}\underset{\dot{g}}{\underbrace{\stackrel{T_e{L}_g}{\overbrace{\left[\begin{array}{ccc}\cos \theta & -\sin \theta & 0\\ \sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right]}}\left[\begin{array}{c}{\xi }_x\\ {\xi }_y\\ {\xi }_{\theta }\end{array}\right]}},$$

(95)

$${\xi }^s=\stackrel{{Ad}_g}{\overbrace{\left[\begin{array}{ccc}\cos \theta & -\sin \theta & y\\ \sin \theta & \cos \theta & -x\\ 0& 0& 1\end{array}\right]}}\left[\begin{array}{c}{\xi }_x\\ {\xi }_y\\ {\xi }_{\theta }\end{array}\right],$$

(96)

Similarly, the inverse adjoint action $A{d}_g^{-1}$ on the Euclidean group $\mathcal{S}\mathrm{E}\left(2\right)$ has the effect of mapping the spatial velocity ${\xi }^s$ of the rigid body to the velocity of the rigid body $\xi$:

$$\xi =\stackrel{T_g{L}_{g^{-1}}}{\overbrace{\left[\begin{array}{ccc}\cos \theta & \sin \theta & 0\\ -\sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right]}}\underset{\dot{g}}{\underbrace{\stackrel{T_e{R}_g}{\overbrace{\left[\begin{array}{ccc}1& 0& -y\\ 0& 1& x\\ 0& 0& 1\end{array}\right]}}\left[\begin{array}{c}{\xi }_x^s\\ {\xi }_y^s\\ {\xi }_{\theta }^s\end{array}\right]}},$$

(97)

$$\xi =\stackrel{A{d}_g^{-1}}{\overbrace{\left[\begin{array}{ccc}\cos \theta & \sin \theta & x\sin \theta -y\cos \theta \\ -\sin \theta & \cos \theta & x\cos \theta +y\sin \theta \\ 0& 0& 1\end{array}\right]}}\left[\begin{array}{c}{\xi }_x^s\\ {\xi }_y^s\\ {\xi }_{\theta }^s\end{array}\right],$$

(98)

3. Results

This section will be devoted to the modeling and analysis of the planar locomotion of a symmetric serpentine robot with lateral shear constraints provided by passive wheels. The snake robot, which is composed of three links, performs lateral movements. Its undulating locomotion movement will be ensured by changing the size of the angles $\left({\mathsf{\alpha }}_1, {\mathsf{\alpha }}_2\right)$, while it has fixed wheel axles perpendicular to the body, as shown in Figure 6. The kinematic snake’s location in the plane is denoted by the variables $\left(\mathrm{x}, \mathrm{y},\mathsf{\theta }\right)\in \mathcal{S}\mathrm{E}\left(2\right),$ and its configuration space is as follows:

$$Q=G\times M=SE\left(2\right)\times \left(S\times S\right),$$

(99)

The chosen kinematic snake has three degrees of freedom given by $\left(\mathrm{x}, \mathrm{y},\mathsf{\theta }\right)$ and the two shape variables $\left({\mathsf{\alpha }}_1, {\mathsf{\alpha }}_2\right)$.

3.1. Calculation of the Position of the Coordinate Systems of the Symmetric Three-Link Kinematic Snake

The coordinate systems will be marked on the simplified diagram of the kinematic snake in Figure 7, and the position calculation will be carried out from the middle link, in which the local coordinate system for the entire system with the origin in $g_2$ is determined, the shape of which is as follows:

$$g_2=\left[\begin{array}{ccc}\mathrm{c}\mathrm{o}\mathrm{s}\theta & -\mathrm{s}\mathrm{i}\mathrm{n}\theta & x\\ \mathrm{s}\mathrm{i}\mathrm{n}\theta & \mathrm{c}\mathrm{o}\mathrm{s}\theta & y\\ 0& 0& 1\end{array}\right],$$

(100)

In order to be able to calculate the individual positions of the first and third link, first, the position of the end of the second link h₂ with respect to $g_2$ is calculated by moving it in the direction of the x-axis to the right by a distance L/2:

$$h_{{2g}_2}=g_2\left[\begin{array}{ccc}1& 0& \frac{L}{2}\\ 0& 1& 0\\ 0& 0& 1\end{array}\right],$$

(101)

$$h_{{2g}_2}=\left[\begin{array}{ccc}\mathrm{c}\mathrm{o}\mathrm{s}\theta & -\mathrm{s}\mathrm{i}\mathrm{n}\theta & x\\ \mathrm{s}\mathrm{i}\mathrm{n}\theta & \mathrm{c}\mathrm{o}\mathrm{s}\theta & y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}1& 0& \frac{L}{2}\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]=\left[\begin{array}{ccc}\mathrm{c}\mathrm{o}\mathrm{s}\theta & -\mathrm{s}\mathrm{i}\mathrm{n}\theta & x+\frac{L}{2}\mathrm{c}\mathrm{o}\mathrm{s}\theta \\ \mathrm{s}\mathrm{i}\mathrm{n}\theta & \mathrm{c}\mathrm{o}\mathrm{s}\theta & y+\frac{L}{2}\mathrm{s}\mathrm{i}\mathrm{n}\theta \\ 0& 0& 1\end{array}\right],$$

(102)

The resulting position of the end of the first link is as follows:

$$h_{1g_2}=h_{{2g}_2}.f_{1h_2}.g_{1f_1}{h}_{1g_1},$$

(103)

$$h_{1g_2}=\left[\begin{array}{ccc}\mathrm{c}\mathrm{o}\mathrm{s}(\theta +{\alpha }_1)& -\mathrm{s}\mathrm{i}\mathrm{n}(\theta +{\alpha }_1)& x+L \mathrm{c}\mathrm{o}\mathrm{s}(\theta +{\alpha }_1)+\frac{L}{2}\mathrm{c}\mathrm{o}\mathrm{s}\theta \\ \mathrm{s}\mathrm{i}\mathrm{n}(\theta +{\alpha }_1)& \mathrm{c}\mathrm{o}\mathrm{s}(\theta +{\alpha }_1)& y+L \mathrm{s}\mathrm{i}\mathrm{n}(\theta +{\alpha }_1)+\frac{L}{2}\sin \theta \\ 0& 0& 1\end{array}\right],$$

(104)

We calculate the position at the other end of the second link in the same way as $h_{{2g}_2}$, but by moving it to the left in the direction of the $x_0$ axis L/2:

$$f_{{2g}_2}=g_2\left[\begin{array}{ccc}1& 0& -\frac{L}{2}\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]=\left[\begin{array}{ccc}\mathrm{c}\mathrm{o}\mathrm{s}\theta & -\mathrm{s}\mathrm{i}\mathrm{n}\theta & x-\frac{L}{2}\mathrm{c}\mathrm{o}\mathrm{s}\theta \\ \mathrm{s}\mathrm{i}\mathrm{n}\theta & \mathrm{c}\mathrm{o}\mathrm{s}\theta & y-\frac{L}{2}\mathrm{s}\mathrm{i}\mathrm{n}\theta \\ 0& 0& 1\end{array}\right],$$

(105)

The resulting position of the end of the second link is as follows:

$$f_{3g_2}=f_{{2g}_2}.h_{3f_2}.g_{3h_3}{f}_{3g_3},$$

(106)

$$f_{3g_2}=\left[\begin{array}{ccc}\mathrm{c}\mathrm{o}\mathrm{s}({\alpha }_2-\theta )& \mathrm{s}\mathrm{i}\mathrm{n}({\alpha }_2-\theta )& x-L \mathrm{c}\mathrm{o}\mathrm{s}\left({\alpha }_2-\theta \right)-\frac{L}{2}\mathrm{c}\mathrm{o}\mathrm{s}\theta \\ -\mathrm{s}\mathrm{i}\mathrm{n}({\alpha }_2-\theta )& \mathrm{c}\mathrm{o}\mathrm{s}({\alpha }_2-\theta )& y+L \mathrm{s}\mathrm{i}\mathrm{n}\left({\alpha }_2-\theta \right)-\frac{L}{2}\mathrm{s}\mathrm{i}\mathrm{n}\theta \\ 0& 0& 1\end{array}\right],$$

(107)

In the case of a multi-link serpentine robot, the same procedure would, of course, be followed, while the obtained expressions for the positions of the coordinate systems become more complex with each additional member added. The key point is that the configurations of any members can be readily represented as a sequence of relative positions of the coordinate systems that result in these expressions, as shown in Figure 7.

3.2. Calculation of Local Velocities of a Three-Link Kinematic Snake

When modeling these systems, it is important to consider the functional relationship between the local velocity of each member in the kinematic chain and the total local velocity $\xi$, and its shape velocity $\dot{r}$ is sought. This dependence on the three-member system is investigated. The local coordinate system for the entire system is fixed to the middle member with the origin at $g_2$. This choice of the local coordinate system of the system gives a trivial Jacobian at the point $g_2$:

$${\xi }_{g_2}=\stackrel{J_{g_2}^b}{\overbrace{\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 1& 0& 0& 0\\ 0& 0& 1& 0& 0\end{array}\right]}}\left[\begin{array}{c}{\xi }^x\\ {\xi }^y\\ \begin{array}{c}{\xi }^{\theta }\\ \begin{array}{c}{\dot{\alpha }}_1\\ {\dot{\alpha }}_2\end{array}\end{array}\end{array}\right]=\left[\begin{array}{cc}{I}^{3\times 3}& {\mathbf{0}}^{3\times 2}\end{array}\right]\left[\begin{array}{c}\xi \\ \dot{r}\end{array}\right],$$

(108)

This will ensure the calculation of the velocity of other members of the chain. In order to determine the local velocity of snake body member 1, the local velocity of the end of snake body member 2 must first be found and used to calculate the local velocity of the nearer end of snake body member 1, which has a rotational speed ${\mathsf{\alpha }}_1$ with respect to $h_2$:

$${\xi }_{h_2}={Ad}_{{h2,g}_2}^{-1}{\xi }_{g_2}=\left[\begin{array}{ccc}\cos \theta & \sin \theta & x\sin \theta -y\cos \theta \\ -\sin \theta & \cos \theta & x\cos \theta +y\sin \theta \\ 0& 0& 1\end{array}\right]{\xi }_{g_2}=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& \frac{L}{2}\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{\xi }^x\\ {\xi }^y\\ {\xi }^{\theta }\end{array}\right],$$

(109)

$${\xi }_{h_2}=\left[\begin{array}{c}{\xi }^x\\ {\xi }^y+{\xi }^{\theta }\frac{L}{2}\\ {\xi }^{\theta }\end{array}\right],$$

(110)

Finally, the local velocity in $h_1$ is calculated as:

$${\xi }_{h_1}={Ad}_{{h1,g}_1}^{-1}{\xi }_{g_1}=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& \frac{L}{2}\\ 0& 0& 1\end{array}\right]\left[-\begin{array}{c}{\xi }^x\cos {\alpha }_1+({\xi }^y+{\xi }^{\theta }\frac{L}{2})\sin {\alpha }_1\\ {\xi }^x\sin {\alpha }_1+\frac{L}{2}({\xi }^{\theta }+{\dot{\alpha }}_1)+({\xi }^y+{\xi }^{\theta }\frac{L}{2})\\ {\xi }^{\theta }+{\dot{\alpha }}_1\end{array}\cos {\alpha }_1\right],$$

(111)

$${\xi }_{h_1}=\left[\begin{array}{c}{\xi }^x\cos {\alpha }_1+({\xi }^y+{\xi }^{\theta }\frac{L}{2})\sin {\alpha }_1\\ {-\xi }^x\sin {\alpha }_1+\mathrm{L}({\xi }^{\theta }+{\dot{\alpha }}_1)+({\xi }^y+{\xi }^{\theta }\frac{L}{2})\\ {\xi }^{\theta }+{\dot{\alpha }}_1\end{array}\mathrm{c}\mathrm{o}\mathrm{s}{\alpha }_1\right],$$

(112)

Similarly, when the local velocity of snake body member 3 is calculated, first, the local velocity of the other end of snake body member 2 is calculated, and then, it is transformed into the local velocity of the nearer end of snake body member 3. The local velocity of the other end of snake body member 2 is equal to:

$${\xi }_{f_2}={Ad}_{{f2,g}_2}^{-1}{\xi }_{g_2}=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& -\frac{L}{2}\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{\xi }^x\\ {\xi }^y\\ {\xi }^{\theta }\end{array}\right]=\left[\begin{array}{c}{\xi }^x\\ {\xi }^y-{\xi }^{\theta }\frac{L}{2}\\ {\xi }^{\theta }\end{array}\right],$$

(113)

$${\xi }_{f_3}=\left[\begin{array}{c}{\xi }^x\cos {\alpha }_2-\left({\xi }^y-{\xi }^{\theta }\frac{L}{2}\right)\sin {\alpha }_2\\ {\xi }^x\sin {\alpha }_2-\mathrm{L}\left({\xi }^{\theta }-{\dot{\alpha }}_2\right)+\left({\xi }^y-{\xi }^{\theta }\frac{L}{2}\right)\\ {\xi }^{\theta }-{\dot{\alpha }}_2\end{array}\cos {\alpha }_2\right],$$

(114)

It applies to local velocities ${\xi }_{g_1},{\xi }_{g_2}$, and ${\xi }_{g_3}$ and is adjusted to the Jacobian form:

$${\xi }_{g_1}=\stackrel{J_{g_1}^b}{\overbrace{\left[\begin{array}{ccccc}\cos {\alpha }_1& \sin {\alpha }_1& \frac{L}{2}\sin {\alpha }_1& 0& 0\\ -\sin {\alpha }_1& \cos {\alpha }_1& \frac{L}{2}(1+\cos {\alpha }_1)& \frac{L}{2}& 0\\ 0& 0& 1& 1& 0\end{array}\right]}}\left[\begin{array}{c}{\xi }^x\\ {\xi }^y\\ \begin{array}{c}{\xi }^{\theta }\\ \begin{array}{c}{\dot{\alpha }}_1\\ {\dot{\alpha }}_2\end{array}\end{array}\end{array}\right] ,$$

(115)

$${\xi }_{g_2}=\stackrel{J_{g_2}^b}{\overbrace{\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 1& 0& 0& 0\\ 0& 0& 1& 0& 0\end{array}\right]}}\left[\begin{array}{c}{\xi }^x\\ {\xi }^y\\ \begin{array}{c}{\xi }^{\theta }\\ \begin{array}{c}{\dot{\alpha }}_1\\ {\dot{\alpha }}_2\end{array}\end{array}\end{array}\right],$$

(116)

$${\xi }_{g_3}=\stackrel{J_{g_3}^b}{\overbrace{\left[\begin{array}{ccccc}\cos {\alpha }_2& -\sin {\alpha }_2& \frac{L}{2}\sin {\alpha }_2& 0& 0\\ \sin {\alpha }_2& \cos {\alpha }_2& -\frac{L}{2}(1+\cos {\alpha }_2)& 0& \frac{L}{2}\\ 0& 0& 1& 0& 1\end{array}\right]}}\left[\begin{array}{c}{\xi }^x\\ {\xi }^y\\ \begin{array}{c}{\xi }^{\theta }\\ \begin{array}{c}{\dot{\alpha }}_1\\ {\dot{\alpha }}_2\end{array}\end{array}\end{array}\right],$$

(117)

For the three-link kinematic snake, three non-holonomic constraints are obtained without sideslip of the wheels. The individual y components of the Jacobians $J_{g_i}^y=0$ are expressed in the Pfaffian form:

$$\left[\begin{array}{c}0\\ 0\\ 0\end{array}\right]=\left[\begin{array}{ccccc}-\sin {\alpha }_1& \cos {\alpha }_1& \frac{L}{2}(1+\cos {\alpha }_1)& \frac{L}{2}& 0\\ 0& 1& 0& 0& 0\\ \sin {\alpha }_2& \cos {\alpha }_2& -\frac{L}{2}(1+\cos {\alpha }_2)& 0& \frac{L}{2}\end{array}\right]\left[\begin{array}{c}{\xi }^x\\ {\xi }^y\\ \begin{array}{c}{\xi }^{\theta }\\ \begin{array}{c}{\dot{\alpha }}_1\\ {\dot{\alpha }}_2\end{array}\end{array}\end{array}\right],$$

(118)

By modifying the previous equation, the resulting relationship between the positional and shape variables of the kinematic snake is obtained, as follows:

$$\xi =\underset{\mathbb{A}}{\underbrace{\frac{1}{D}\left[\begin{array}{cc}\frac{L}{2}(\cos {\alpha }_2+1)& \frac{L}{2}(\cos {\alpha }_1+1)\\ 0& 0\\ \sin {\alpha }_2& \sin {\alpha }_1\end{array}\right]}}\dot{r},$$

(119)

$$D=\sin {\alpha }_1-\sin {\alpha }_2+\sin \left({\alpha }_1-{\alpha }_2\right).$$

(120)

3.3. Depiction of Local Connections Using Vector Fields

The local connection of the three-link kinematic snake is displayed using vector fields. In addition to displaying the individual components of the local connection, the vector fields also contain a generated gait ϕ, which represents the association between the shape variables, i.e., the rotations of the snake’s joints. Gait, which generates the lateral movement of the snake robot Figure 8, has the following form:

$$\varphi :{\alpha }_1=40\sin t,{\alpha }_2=-40\sin (t+25) ,$$

(121)

The rotation speed of the joints is obtained by differentiating the rotation angles with respect to time, Figure 9:

$${\dot{\alpha }}_1=40\cos t,{\dot{\alpha }}_2=-40\cos (t+25),$$

(122)

After defining the gait, the curve can be displayed in individual vector fields. Figure 10, Figure 11 and Figure 12 show how the kinematic snake will move in the direction of the x, y, and θ axes. If the arrows of the vector field ${\overrightarrow{\mathbb{A}}}^{{\mathsf{\xi }}_{\mathrm{x}}}$ are in the opposite direction of the sense of the generated gait, then the kinematic snake will move in the negative direction, and in the opposite case, the snake will move in the positive direction. Arrows that land perpendicular to the gait curve only cause the snake to turn. The vector field of the local connection ${\overrightarrow{\mathbb{A}}}^{{\mathsf{\xi }}_{\mathrm{y}}}$ is a zero-vector field, which results from the conditions of non-holonomy. In the case of the arrows of the local connection vector field ${\overrightarrow{\mathbb{A}}}^{{\mathsf{\xi }}_{\mathsf{\theta }}},$ which are parallel to the gait curve, they only cause the snake’s joints to rotate, and the snake does not rotate in the direction perpendicular to the curve.

4. Conclusions

Based on the results from Section 3.3, where the simulation results were presented, we can conclude that when applying the serpentine lateral movement to the symmetrical kinematic snake, the individual vector fields, together with the drawn movement curves, give us information about the real behavior in individual sections of the movement. Individual sections of the curve are represented by points 1, 2, 3, 4, and 5. The key input in this case is the shape of the function ϕ. The gradual shape change of the robotic snake’s body is realized by simultaneously setting the rotation angles ${\alpha }_1$ and ${\alpha }_2$ in the snake’s joints. The results show a simplified diagram of the layout of individual links and their sizes and angles in the joints of the model. The current gradual change of rotation in time is interpreted in the output results figures. The green color identifies the total cycle of change of rotation of both angles, which corresponds to a time of 6.5 s for both angles simultaneously. Similarly, for the angular speeds of rotation of the joints, in the result figure, we can see the cyclic character of the change in the angular speeds $\dot{{\alpha }_1}$ and $\dot{{\alpha }_2}$. When comparing these shape parameters, it is clear that the magnitude of the speed in the initial phases of the movement is greater. At the same time, however, it is necessary to take into account the influence of non-holonomic restrictions on the movement of the snake, which will be manifested by the impossibility of direct movement in the perpendicular direction at the location of the individual wheels.

The total movement of the robot globally expresses the vector field of the connection in the direction of the x and y axes and the angle θ. Is presented in the results the vector field together with the gait curve, on which the positions of the points within one cycle are highlighted in black. It is the position of the points on the gait curve and the influence of the vectors at the location of the given point that represent the overall character of the symmetrical movement. The effect of the non-holonomic constraint can be seen in results too, where no vector field acts in the corresponding plane during the cycle of the gait curve. For a proper understanding, the overall rotation of the central link at the center of gravity, i.e., the overall orientation of the entire body of the snake and its gradual change during one cycle of the curve.

A three-link robotic snake with non-holonomic constraints, which has two action quantities of rotation changes ${\alpha }_1$ and ${\alpha }_2$ at the joints, moves in a lateral cyclic motion, while the changes of the shape of the body based on the change of the shape variables are equal to the respective phases of the movement in points 1 to 5, Figure 13. Finally, we can observe the change in the coordinates of the global variables x, y, and $\theta$ angle during the simulation verification. It can be seen in Figure 14 that the movement of turning the angle is repeated cyclically. Movement in the y-axis direction is forbidden precisely because of non-holonomic constraints. For this reason, the value Y of the transformation is equal to the constant 0.

The most important research findings can be summarized in the following points:

• To implement the locomotion of a three-link snake robot of the under-actuated type, the shape of the robot is changed in order to achieve the required movement function through the rotation of the joints.
• During the locomotion of the mechanism, there is also a cyclical change of an-gular velocities when it is found that the velocities are greater in the initial phases of the movement.
• From the results of the simulation of a three-link snake robot during the cyclic phases of locomotion, it follows that visualizing the local connection through a vector field clearly contributes to the body shape changes. Moreover, it is related to the overall control of the mechanism and depends on the input geometric parameters.
• The equation of the input movement curve is an important influencing parameter for the overall resulting shape of the movement of the mechanism.

Based on these findings, we can conclude that when modeling a mobile robotic mechanism with non-holonomic constraints, we must pay attention not only to non-holonomic constraints but also to the classification of shape variables, the shape of motion functions, and the overall geometric symmetry of the mechanism. Based on this, we can, to a certain extent, generalize the use of the given method to multi-element systems with non-holonomic constraints.

The advantages of using a geometric approach to describe the principle of locomotion of a biologically inspired snake robot are clearly visible in the graphic representation and in the simplification of the mathematical description of the symmetric mechanical system. Geometric mechanics provides a mathematical framework for accurately describing the motion and kinematic properties of a robot. Using geometric tools such as Lie groups and algebras, it is possible to mathematically model and analyze the movement of a robot in space, and it is also possible to analyze the movement of different types of robots, regardless of their specific construction and mechanical parameters. In addition, it allows for optimizing the design of the robot’s movement and searching for efficient trajectories. Using the principles of differential geometry and variation, it is possible to evaluate the energy requirements, stability, and other properties of the robot’s movement. All this makes geometric mechanics a powerful tool for describing locomotion and its physical understanding.