Simulating Nonholonomic Dynamics

# Simulating Nonholonomic Dynamics

Marin Kobilarov M. Kobilarov: California Institute of Technology, Control and Dynamical Systems, Pasadena, CA 91125, USA David Martín de Diego D. Martín de Diego: Instituto de Ciencias Matemáticas, CSIC-UAM-UC3M-UCM, Serrano 123, 28006, Madrid, Spain  and  Sebastián Ferraro S. Ferraro: Departamento de Matemática e Instituto de Matemática, Universidad Nacional del Sur, Av. Alem 1253, 8000 Bahía Blanca, Argentina
###### Abstract.

This paper develops different discretization schemes for nonholonomic mechanical systems through a discrete geometric approach. The proposed methods are designed to account for the special geometric structure of the nonholonomic motion. Two different families of nonholonomic integrators are developed and examined numerically: the geometric nonholonomic integrator (GNI) and the reduced d’Alembert-Pontryagin integrator (RDP). As a result, the paper provides a general tool for engineering applications, i.e. for automatic derivation of numerically accurate and stable dynamics integration schemes applicable to a variety of robotic vehicle models.

## 1. Introduction

Nonholonomic constraints have been the subject of deep analysis since the dawn of Analytical Mechanics. Hertz, in 1894, was the first to use the term “nonholonomic system”, but we can even find older references in the work by Euler in 1734, who studied the dynamics of a rolling rigid body moving without slipping on a horizontal plane. Many authors have recently shown a new interest in that theory and also in its relation to the new developments in control theory, subriemannian geometry, robotics, etc (see, for instance, [24]). The main characteristic of this period is that Geometry was used in a systematic way (see L.D. Fadeev and A.M. Vershik [28] as an advanced and fundamental reference, and also, [1, 2, 5, 9, 18, 20] and references therein).

In the case of nonholonomic mechanics, these constraint functions are, roughly speaking, functions on the velocities that are not derivable from position constraints. Traditionally, the equations of motion for nonholonomic mechanics are derived from the Lagrange-d’Alembert principle which restricts the set of infinitesimal variations (or constrained forces) in terms of the constraint functions.

Recent works, such as [8, 10, 14, 23], have introduced numerical integrators for nonholonomic systems with very good energy behavior and properties such as the preservation of the discrete nonholonomic momentum map. In this paper, we will review and compare two new methods for nonholonomic mechanics, the Geometric Nonholonomic Integrator (GNI) [12] and the Reduced d’Alembert-Pontryagin Integrator (RDP) [17], examining their behavior in the numerical simulation of some of the most typical examples in nonholonomic mechanics: the Chaplygin sleigh and the snakeboard.

Finally, the developed algorithms are packaged as a general computational tool for automatic derivation of nonholonomic integrators given the system constraints and Lagrangian. It is available for download from
http://www.cds.caltech.edu/~marin/index.php?n=nhi

## 2. Introduction to Discrete Mechanics

Discrete variational integrators appear as a special kind of geometric integrators (see [13, 27]). These integrators have their roots in the optimal control literature in the 1960’s and 1970’s. In the sequel we will review the construction of this specific type of geometric integrators (see [22] for an excellent survey about this topic).

A discrete Lagrangian is a map , where is a finite-dimensional configuration manifold. For the construction of numerical integrators for a continuous Lagrangian system given by a Lagrangian , the discrete Lagrangian may be considered as an approximation of the integral action

 Ld(q0,q1)≊∫h0L(q(t),˙q(t))dt

where is a solution of the Euler-Lagrange equations corresponding to , that is,

 (1) ddt(∂L∂˙q(q(t),˙q(t)))−∂L∂q(q(t),˙q(t))=0,

additionally satisfying and , where is the time step. Observe that this solution always exists if the Lagrangian is regular and is small enough (see [25]).

Define the action sum corresponding to the Lagrangian by

 Sd=N∑k=1Ld(qk−1,qk),

where for . For any covector , we have a decomposition where . Therefore,

 dLd(q0,q1)=D1Ld(q0,q1)+D2Ld(q0,q1).

The discrete variational principle states that the solutions of the discrete system determined by must extremize the action sum given fixed points and .

Extremizing over , , we obtain the following system of difference equations

 (2) D1Ld(qk,qk+1)+D2Ld(qk−1,qk)=0.

These equations are usually called the discrete Euler-Lagrange equations.

The geometrical properties corresponding to this numerical method are obtained defining two discrete Legendre transformations associated to by

 F−Ld:Q×Q⟶T∗Q(q0,q1)⟼(q0,−D1Ld(q0,q1))
 F+Ld:Q×Q⟶T∗Q(q0,q1)⟼(q0,D2Ld(q0,q1))

and the 2-form , where is the canonical symplectic form on . We will say that the discrete Lagrangian is regular if is a local diffeomorphism. We will have that:

 F−Ld is a local diffeomorphism ⇔ F+Ld is a local diffeomorphism ⇔ ωd is symplectic

Under this regularity condition, this implicit system of difference equations (2) defines a local discrete flow , by . The discrete algorithm determined by preserves the symplectic form , i.e., . Moreover, if the discrete Lagrangian is invariant under the diagonal action of a Lie group , then the discrete momentum map defined by is preserved by the discrete flow. Here, denotes the fundamental vector field determined by :

 ξQ(q)=ddt∣∣t=0(exp(tξ)⋅q).

Therefore, these integrators are symplectic-momentum preserving integrators.

In [21] we have obtained a geometric derivation of variational integrators that is also valid for reduced systems (on Lie algebras, quotient of tangent bundles by a Lie group action, etc.)

## 3. Description of the nonholonomic dynamics

The presence of nonholonomic (or holonomic) constraints gives rise to forces. Nonholonomic systems are described by the Lagrange-D’Alembert’s principle which prescribes the constraint forces induced by the given nonholonomic constraints. In the following we will describe the equations of motion of a nonholonomic system in terms of Riemannian geometric tools (see [5]).

Let be an -dimensional differentiable manifold, with local coordinates , . Consider a mechanical Lagrangian system defined by , or, locally

 (3) L(q,˙q)=12gij(q)˙qi˙qj−V(q).

Here is a Riemannian metric on (locally defined by the symmetric, positive definite matrix ) and represents a potential function. We know that the equations of motion for a Lagrangian system are (1) which, in the case of a mechanical Lagrangian system of the form (3), admits a nice expression in terms of standard Riemmanian geometric tools:

where is the Levi–Civita connection associated to and, in coordinates, where is the inverse matrix of .

Assume that the system is subjected to nonholonomic constraints, defined by a regular distribution on , with . Locally the nonholonomic constraints are described by the vanishing of independent functions

 ϕa=μai(q)˙qi,1≤a≤m  (the constraint functions'').

The Lagrange–d’Alembert principle states that the equations of motion for a nonholonomic system determined by the two data are:

 (4) ddt(∂L∂˙qi(q(t),˙q(t)))−∂L∂qi(q(t),˙q(t)) =λaμai(q(t)), μai(q(t))˙qi(t) =0

where , are Lagrange multipliers to be determined. Using the Levi-Civita connection we find an intrinsic equation for the nonholonomic equations:

where is a section of along . Here stands for the orthogonal complement of with respect to the metric .

In coordinates, defining the functions (Christoffel symbols for ) by

 ∇∂∂qi∂∂qj=Γkij∂∂qk,

we may rewrite the nonholonomic equations of motion as

 ¨qk(t)+Γkij(c(t))˙qi(t)˙qj(t) =−gki(c(t))∂V∂qi+¯λa(t)gki(c(t))μai(c(t)), μai(c(t))˙qi(t) =0.

## 4. Geometric Nonholonomic Integrator – Gni

Given a nonholonomic system where is a Lagrangian system of mechanical type (3), using the metric , we may consider the complementary projectors

 P: TQ→D↪TQ Q: TQ→D⊥↪TQ

and their duals considered as mappings from to .

The Geometric Nonholonomic integrator (GNI, in the sequel) for a nonholonomic system only needs to fix a discrete Lagrangian to derive a numerical scheme, that is, it is not necessary to discretize the nonholonomic constraints for this type of integrator. The discrete nonholonomic equations proposed in [12] are

 (5a) P∗|qk(D1Ld(qk,qk+1))+P∗|qk(D2Ld(qk−1,qk)) =0 (5b) Q∗|qk(D1Ld(qk,qk+1))−Q∗|qk(D2Ld(qk−1,qk)) =0.

The first equation is the projection of the discrete Euler–Lagrange equations to the dual of the constraint distribution , while the second one can be interpreted as an elastic impact of the system against . This defines a unique discrete evolution operator if and only if the Lagrangian is regular, in the sense of Section 2.

Define the pre- and post-momenta using the discrete Legendre transformations:

 p+k−1,k =F+Ld(qk−1,qk)=(qk,D2Ld(qk−1,qk))∈T∗qkQ p−k,k+1 =F−Ld(qk,qk+1)=(qk,−D1Ld(qk,qk+1))∈T∗qkQ.

In these terms, equation (5b) can be rewritten as

 Q∗|qk(p−k,k+1+p+k−1,k2)=0

which means that the average of post- and pre-momenta satisfies the nonholonomic constraints.

We can also rewrite the discrete nonholonomic equations as a jump of momenta:

 (6) p−k,k+1=(P∗−Q∗)∣∣qk(p+k−1,k).

#### Reversibility.

Note that the map is an involution, that is . Therefore, it acts equivalently in both directions, i.e. it creates a reversible and symmetric flow. Furthermore, it can be expressed as

 S(q)=U(q)DU−1(q),

where is a diagonal matrix with elements corresponding to the eigenvalues of while is an invertible matrix with columns the eigenvectors of . Thus, the update (6) can be written as

 (7) U−1(qk)p−k,k+1=DU−1(qk)p+k−1,k

based on which one can regard the momentum as either remaining unchanged (corresponding to eigenvalues) or being reflected (corresponding to eigenvalues) with respect to the basis defined by the mapping .

#### Preservation Properties.

Suppose that is a manifold on which a Lie group acts. Define for each

 (8) gq={ξ∈g|ξQ(q)∈Dq},

where is the infinitesimal generator vector field corresponding to at the point . The bundle over whose fiber at is is denoted by . Define the discrete nonholonomic momentum map as in [8] by

 Jnhd(qk−1,qk):gqk →R ξ ↦⟨D2Ld(qk−1,qk),ξQ(qk)⟩.

For any smooth section of we have a function , defined as .

If is -invariant and is a horizontal symmetry (that is, for all ), then the GNI preserves (see [12] for a proof).

In some cases of interest, it is possible to obtain an integrator preserving energy applying the following theorem (see [12]):

###### Theorem 1.

Let the configuration manifold be a Lie group with a bi-invariant Lagrangian and with an arbitrary distribution , and take a discrete Lagrangian that is left-invariant. Then the GNI (5) is energy-preserving.

### 4.1. Nonholonomic version of the RATTLE and SHAKE methods

Consider a continuous nonholonomic system determined by the mechanical Lagrangian :

 L(q,˙q)=12˙qTM˙q−V(q)

(with a constant, invertible matrix) and the constraints determined by where is a matrix with .

Consider now the symmetric discretization

 Ld(qk,qk+1) =12hL(qk,qk+1−qkh)+12hL(qk+1,qk+1−qkh) =12h(qk+1−qk)TM(qk+1−qk)−h2(V(qk)+V(qk+1)).

After some straightforward computations we obtain that equations (5a) and (5a) for the proposed nonholonomic discrete system are

 (9a) qk+1−2qk+qk−1 =−h2M−1(Vq(qk)+μT(qk)λk) (9b) 0 =μ(qk)(qk+1−qk−12h),

where are Lagrange multipliers. We recognize this set of equations as an obvious extension of the SHAKE method proposed by [26] to the case of nonholonomic constraints.

The momentum is approximated by . Denoting , equations (9a) and (9b) are now rewritten in the form

 pk+1/2 =pk−h2(Vq(qk)+μT(qk)λk), qk+1 =qk+hM−1pk+1/2, 0 =μ(qk)M−1pk.

The definition of requires the knowledge of and, therefore, it is is natural to apply another step of the algorithm (9a) and (9b) to avoid this difficulty. Then, we obtain the new equations:

 pk+1 =pk+1/2−h2(Vq(qk+1)+μT(qk+1)λk+1), 0 =μ(qk+1)M−1pk+1.

The interesting result is that we obtain a natural extension of the RATTLE algorithm for holonomic systems to the case of nonholonomic systems. Unifying the equations above we obtain the following numerical scheme

 (10a) pk+1/2 =pk−h2(Vq(qk)+μT(qk)λk), (10b) qk+1 =qk+hM−1pk+1/2, (10c) 0 =μ(qk)M−1pk, (10d) pk+1 =pk+1/2−h2(Vq(qk+1)+μT(qk+1)λk+1), (10e) 0 =μ(qk+1)M−1pk+1.

These equations allow us to take a triple satisfying the constraint equations (10c), compute using (10a) and then using (10b). Then, equations (10d) and (10e) are used to compute the remaining components of the triple . It is clear, applying Theorem 1 that, in the case , the numerical method is energy preserving.

###### Remark 1.

From this Hamiltonian point of view, we have shown that the initial conditions for this numerical scheme are constrained in a natural way ( with ), that is, the initial conditions are exactly the same as those for the continuous system. Additionally, we select (see [12]).

In [11], the following theorem is proven.

###### Theorem 2.

The nonholonomic RATTLE method is globally second-order convergent.

### 4.2. Projected Version of the Nonholonomic RATTLE

The proposed nonholonomic RATTLE method can be expressed without the use Lagrangian multipliers by projecting the equations of motion onto the constraint distribution through the projection defined in §4.

Assuming that the Lagrangian is regular and that matrix is full rank (i.e. rank ) (9) can be reformulated as

 (11a) P(qk)TM(qk+1−2qk+qk−1) =−h2P(qk)TVq(qk) (11b) Q(qk)TM(qk+1−qk−12h) =0,

where the matrices and represent both orthogonal projectors and have rank and , respectively, and are defined by

 (12a) Q(q) =M−1μ(q)T(μ(q)M−1μ(q)T)−1μ(q), (12b) P(q) =Id−Q(q),

where is the identity matrix.

Eqs. (11) correspond to (5) for the case and furthermore can be put in the “momentum jump” form by adding (11a) and (11b) to get

 (13) qk+1 =qk+(Id−2M−1Q(qk)TM)(qk−qk−1)−h2M−1P(qk)TVq(qk).

For a more realistic example, we can add control inputs acting in the basis defined by the matrix to obtain the following discrete equations:

 qk+1 =qk+(Id−2M−1Q(qk)TM)(qk−qk−1)+h2M−1P(qk)Tf(qk,uk),

where the forces are given by .

In terms of momentum variables the integrator can be equivalently expressed as

 (14a) pk+1/2 =(Id−2Q(qk)T)pk−1/2+hP(qk)Tf(qk,uk) (14b) qk+1 =qk+hM−1pk+1/2

providing an update scheme .

A remaining critical step in completing the algorithm is to establish the link between the discrete variables for used in (14) and the continuous curve . In that respect one can regard as an approximation to the continuous momentum at time , i.e. . The pair satisfies the nonholonomic constraint by definition and is related, following from (14), to the “midpoint” momenta through

 (15a) pk =P(qk)Tpk+1/2−h2P(qk)Tf(qk,uk), (15b) pk =P(qk)Tpk−1/2+h2P(qk)Tf(qk,uk).

These expressions can be used to determine proper variables to initialize the update (14) given continuous initial conditions . Since there is a set of solutions satisfying (15) for a given the most natural choice is to pick satisfying the constraints at . Therefore, the condition becomes

 p1/2 =p0+h2P(q0)Tf(q0,u0).

In summary, given initial conditions satisfying the constraints, the dynamics is evolved forwards to reach the final state , also in the constraint submanifold, after time steps through

 (16) p1/2=p0+h2P(q0)Tf(q0,u0),pk+1/2=(Id−2Q(qk)T)pk−1/2+hP(qk)Tf(qk,uk),qk+1=qk+hM−1pk+1/2,pN=P(qN)TpN−1/2+h2P(qN)f(qN,uN),

for .

## 5. Reduced d’Alembert-Pontryagin integrator-(RDP)

In this section we consider a class of mechanical systems which, in addition to nonholonomic constraints, also possess symmetries of motion arising from conservation laws. The interplay between the constraints and symmetries is linked to an intrinsic structure of the state space associated with important properties of the dynamics. Our goal in this section is to develop integrators that respect this structure and lead to more faithful numerical representation.

In §4 we introduced the action of a symmetry group and its relation the evolution of the system momentum. Additional structure arises whenever the dynamics and constraints are -invariant that permits the construction of reduced nonholonomic integrators [14, 17].

Following [1], define the subspaces and according to

 Vq={ξQ(q) | ξ∈g},Sq=Dq∩Vq.

Practically speaking, the vertical space represents the space of tangent vectors parallel to symmetry directions while is the space of symmetry directions that satisfy the constraints. Equivalently, can be regarded as the space generated by elements in , as defined in (8). The group is chosen so that the Lagrangian and distribution are -invariant. In addition, we make the standard assumption (see [1, 7]) that , for each .

Since our main interest is in a configuration space that is by construction of the form we will restrict any further derivations to the trivial bundle case. Using coordinates a basis for can be chosen as , for . Since is -invariant these elements can be expressed as , where is the body-fixed basis. We denote the space spanned by at each . Lastly, the system is subject to control force restricted to the shape space.

#### Nonholonomic Connection

With these definitions we can define a principal connection with horizontal distribution that coincides with at the point , where . This connection is called the nonholonomic connection and is constructed according to , where is the kinematic connection enforcing the nonholonomic constraints and is the mechanical connection corresponding to symmetries satisfying the constraints. These maps are defined according to

where is called the locked angular velocity, i.e. the velocity resulting from instantaneously locking the joints described by the variables . Intuitively, when the joints stop moving the system continues its motion uniformly along a curve (with tangent vectors in ) with body-fixed velocity and a corresponding spatial momentum that is conserved.

By definition the principal connection can be expressed as

where is the local form and the two components in (17) can be added to obtain

 g−1˙g+A(r)˙r=Ω.

#### Numerical Formulation

Since the Lagrangian is -invariant, we can define the reduced Lagrangian

 (18) ℓ(r,˙r,ξ)=L(r,˙r,e,g−1˙g).

In [17] a nonholonomic integrator was derived using a discrete variational d’Alembert-Pontryagin principle based on the reduced Lagrangian , the connection and a chosen trajectory discretization. In particular, a discrete trajectory with points and respective velocities and was constructed so that

 rk+1−rk=huk,τ−1(g−1kgk+1)=hξk,

where , with for a chosen . The map represents the difference between two configurations in the group by an element in its algebra and can be selected as:

• Exponential map , defined by , with is the integral curve through the identity of the left invariant vector field associated with (hence, with );

• Canonical coordinates of the second kind , , where is the Lie algebra basis.

A third choice for , valid only for certain quadratic matrix groups [6] (which include the rigid motion groups , , and ), is the Cayley map , (See App. A for more details).

With these definitions in place the resulting reduced d’Alembert-Pontryagin (RDP) integrator can be stated [17]. For numerical convenience it is given in terms of vector-matrix notation, by treating the Lie algebra variables and as vectors of coordinates with respect to a chosen canonical basis (see App. B for an example).

The discrete flow satisfies the reduced discrete dynamics

 (19)

where and . The map is the right-trivialized tangent of defined by and is its inverse (see App. A).

Equation (19) along with the reconstruction equations

 (20) gk+1=gkτ(hξk),rk+1=rk+huk,

constitute the complete RDP discrete evolution.

## 6. Examples

### 6.1. The Chaplygin Sleigh

The Chaplygin Sleigh [1] is a planar rigid body making a contact with the ground through a skate mounted at the central axis of the body at a distance from its center of mass (Fig. 1). The configuration space is the group with coordinates describing the orientation and the position of the center of mass. The body has rotational inertia and mass and, therefore, its Lagrangian is defined by

 (21) L(q,˙q)=12I˙θ2+12m(˙x2+˙y2).

At the point of the skate contact the body must slide in the direction in which it is pointing. This condition is encoded by the nonholonomic constraint

 a˙θ+sinθ˙x−cosθ˙y=0.

A structure-preserving integrator was developed in [10] based on the discrete Lagrange-d’Alembert (DLA) principle with discrete momentum and measure preservation properties. Exploring this direction further, in this section we develop two alternative methods based on the GNI and RDP schemes.

#### GNI Integrator.

From the mass matrix and the constraint , the projector can be computed using (12a) as

 (22) Q(q)=1I+a2m⎡⎢⎣a2mamsinθ−amcosθaIsinθIsinθ2−Isinθcosθ−aIcosθ−IsinθcosθIcos2θ⎤⎥⎦.

Since the mass matrix is constant, the GNI integrator can be derived according to . In terms of the coordinates , the discrete update becomes

 vθk+12=(1−2a2mI′)vθk−12+amI′(−2sinθkvxk−12+2cosθkvyk−12),vxk+12=−2aII′sinθkvθk−12+(1−2II′sin2θk)vxk−12+2II′sinθkcosθkvyk−12,vyk+12=2aII′cosθkvθk−12+2II′sinθkcosθkvxk−12+(1−2II′cos2θk)vyk−12,

where . It is straightforward to verify that the resulting update rule is energy-preserving, i.e. . This property is inherent to the GNI construction as explained in [12].

#### RDP Integrator.

The sleigh has no internal joints and therefore no shape space. Since the Lagrangian (21) is left-invariant to group action, the reduced Lagrangian (18) can be expressed as

 ℓ(ξ)=L(e,g−1˙g),

where describes the angular, forward, and sideways velocities with respect to the body frame fixed at the center of mass. The constrained symmetry space (8) of the sleigh can be identified as

 gq=span{e1(g),e2(g)},

where and form the constant basis in the body-fixed frame with , for . The two components of the nonholonomic momentum become

 p1=(J+a2m)ω,p2=mv,

corresponding to angular and forward momenta, respectively. The group trajectory can be reconstructed from the momentum according to

 g−1˙g=(1I+a2mp1, 1mp2, aI+a2mp1).

The momentum components themselves evolve according to (see  [2]), or equivalently

 ˙p1=−aI+a2mp1p2,˙p2=ma(I+a2m)2p21.

Since the shape space consists of a single point, the discrete dynamics includes only the momentum equations (19) which become

 ⟨(dτ−1hξk)∗∂ξℓk−(dτ−1−hξk−1)∗∂ξℓk−1,ei⟩=0,

for . A simple form of these equations can be derived by choosing and truncating its tangent to first order, i.e. . Using the notation the update becomes

 (p1)k−(p1)k−1=−ha2(J+a2m)[(p1)k(p2)k+(p1)k−1(p2)k−1], (p2)k−(p2)k−1=hma2(J+a2m)2[(p1)k2+(p