the international journal of robotics...

http://ijr.sagepub.com/Robotics Research

The International Journal of

http://ijr.sagepub.com/content/30/8/988The online version of this article can be found at:

DOI: 10.1177/0278364910394392

2011 30: 988 originally published online 3 June 2011The International Journal of Robotics ResearchRoss L Hatton and Howie Choset

choiceGeometric motion planning: The local connection, Stokes' theorem, and the importance of coordinate

Published by:

http://www.sagepublications.com

On behalf of:

Multimedia Archives

can be found at:The International Journal of Robotics ResearchAdditional services and information for

http://ijr.sagepub.com/cgi/alertsEmail Alerts:

http://ijr.sagepub.com/subscriptionsSubscriptions:

http://www.sagepub.com/journalsReprints.navReprints:

http://www.sagepub.com/journalsPermissions.navPermissions:

http://ijr.sagepub.com/content/30/8/988.refs.htmlCitations:

What is This?

- Jun 3, 2011 OnlineFirst Version of Record

- Jul 13, 2011Version of Record >>

at CARNEGIE MELLON UNIV LIBRARY on December 5, 2014ijr.sagepub.comDownloaded from at CARNEGIE MELLON UNIV LIBRARY on December 5, 2014ijr.sagepub.comDownloaded from

http://ijr.sagepub.com/

http://ijr.sagepub.com/content/30/8/988

http://www.sagepublications.com

http://www.ijrr.org/

http://ijr.sagepub.com/cgi/alerts

http://ijr.sagepub.com/subscriptions

http://www.sagepub.com/journalsReprints.nav

http://www.sagepub.com/journalsPermissions.nav

http://ijr.sagepub.com/content/30/8/988.refs.html

http://ijr.sagepub.com/content/30/8/988.full.pdf

http://ijr.sagepub.com/content/early/2011/04/13/0278364910394392.full.pdf

http://online.sagepub.com/site/sphelp/vorhelp.xhtml



Geometric motion planning: The localconnection, Stokes’ theorem, and theimportance of coordinate choice

The International Journal ofRobotics Research30(8) 988–1014© The Author(s) 2011Reprints and permission:sagepub.co.uk/journalsPermissions.navDOI: 10.1177/0278364910394392ijr.sagepub.com

Ross L Hatton and Howie Choset

AbstractThe locomotion of articulated mechanical systems is often complex and unintuitive, even when considered with the aid ofreduction principles from geometric mechanics. In this paper, we present two tools for gaining insights into the underlyingprinciples of locomotion: connection vector fields and connection height functions. Connection vector fields illustratethe geometric structure of the relationship between internal shape changes and the system body velocities they produce.Connection height functions measure the curvature of their respective vector fields and capture the net displacementover any cyclic shape change, or gait, allowing for the intuitive selection of gaits to produce desired displacements.Height function approaches have been previously attempted, but such techniques have been severely limited by their basisin a rotating body frame, and have only been useful for calculating planar rotations and infinitesimal translations. Wecircumvent this limitation by introducing a notion of optimal coordinates defining a body frame that rotates very little inresponse to shape changes, while still meeting the requirements of the geometric mechanics theory on which the vectorfields and height functions are based. In these optimal coordinates, the height functions provide close approximations ofthe net displacement resulting from a broad selection of possible gaits.

KeywordsGeometric mechanics, kinematics, locomotion, nonholonomic motion planning, underactuated robots, undulatory systems

1. Introduction

Locomotion—that is, self-powered motion through theworld—is one of the most basic actions performed by arobot or living organism. Driven wheels are the dominantparadigm for manmade locomotors, but their effectivenessis limited to relatively flat, smooth terrain. Nature providesmany alternative modes of locomotion that can handle morechallenging conditions: snakes crawl, fish swim, birds fly,and all manner of creatures walk. The facility with whichanimals use internal joint motions to move through theirenvironments far exceeds that which has been achievedin artificial systems; consequently there is much interestin raising the locomotion capabilities of such systems tomatch or surpass those of their biological counterparts. Thechallenge in this task is in finding effective means of plan-ning for these systems, where the controls are not neatlyseparable into ‘drive’ and ‘steering’ inputs.

A fundamental aspect of animal locomotion is that itis primarily composed of gaits: cyclic shape motions thatefficiently transport the animal through its environment.Examples of such gaits include a horse’s walking, trotting,and galloping, a fish’s translation and turning strokes, and asnake’s slithering and sidewinding. In all of these motions,the animals take advantage of an asymmetry in the reaction

forces they generate from the surrounding media to gaina net displacement over each cycle. The efficacy of suchgaits, along with the abstraction that they allow from inter-nal shape changes to position changes, suggests that theywill form an equally important part of artificial locomotion,raising the question of how to identify useful asymmetriesduring motion planning.

Here, we are specifically interested in producing gaitdesign tools for mechanical systems. Much prior work ingait design has taken the approach of choosing parameter-ized basis functions for gaits and simulating the motion ofthe system while executing the gaits, optimizing the inputparameters to find gaits that meet the design requirements.Such optimization with forward simulation is computation-ally expensive and sensitive to the choice of starting param-eters. Therefore, there is growing interest in finding gait

Robotics Institute and Department of Mechanical Engineering, CarnegieMellon University, Pittsburgh, PA, USA

Corresponding author:Ross L Hatton, Robotics Institute and Department of Mechanical Engi-neering, Carnegie Mellon University, 5000 Forbes Avenue, Pittsburgh, PA15213, USA.Email: [email protected]

at CARNEGIE MELLON UNIV LIBRARY on December 5, 2014ijr.sagepub.comDownloaded from


Hatton and Choset 989

Table 1. The three example systems.

Differential-drive car Floating snake Kinematic snake

The differential-drive car has indepen-dently driven wheels on either side of thebody. These wheels allow the system todrive in the direction in which it points,rotate around its center, or combine thesemotions to move along arcs of arbitrarycurvature.

The links on the floating snake are onlyconstrained by their joints. Because itdoes not interact with its environment,the translation and rotation of this sys-tem are dictated entirely by conservationof linear and angular momentum.

Passive wheelsets on the links of thekinematic snake prevent lateral transla-tion while freely allowing rotation andlongitudinal translation. The joint anglesare actively controlled and the systemcan locomote by ‘pulling’ or ‘pushing’against the wheel constraints.

design tools that replace the simulation stage with a gaitevaluation method more amenable to optimization.

Geometric mechanics offers a systematic means ofapproaching this optimization problem, providing rigor-ously defined quantities such as curvature for measuringthe net displacement a system can achieve over cyclicinternal shape motion. While methods based on these quan-tities are powerful, their dissemination has been unfortu-nately limited by the abstract nature of the differentialgeometry used to express them. Additionally, the curva-ture methods as previously developed have not been fullydeveloped for several interesting systems; in their presentstate, either they are restricted to designing small, inefficientmotions, or they provide incomplete information about theactual displacement of the system over the course of agait.

In this paper, we address these limitations on twofronts. First, we review the key elements of the geometricmechanics formulations, but do so in the language ofvector calculus rather than differential geometry and makebroad use of concrete examples based on the systems inTable 1. By taking this approach, we seek to present thismaterial to a broader audience. Second, using this simpli-fied formulation, we develop the concept of a body-velocityintegral (BVI), which provides an expanded and physicallymeaningful interpretation of previous curvature-basedapproaches to locomotion, and identify conditions underwhich the BVI is a good estimate of the true displacementresulting from a gait. We then introduce the notion thatrather than being intrinsic to the system, the presence ofthese conditions depends on the choice of parameterization,and demonstrate that this choice of parameterization canbe manipulated to ensure the conditions’ existence. We

finish by presenting a means for automatically finding theoptimal choice of parameterization for any given system,such that the BVI approximates the displacement withminimal error over the widest domain of possible gaits. Thetheoretical developments are supported by experimentalresults, in which the BVI is shown to be a significantlymore accurate approximation of the displacement underthe new parameterization than those previously describedin the literature.

2. Prior work

Our work builds on the body of locomotion literaturethat uses geometric mechanics to separate internal shapechanges from the external motions they produce. The appli-cation of geometric mechanics to locomotion, pioneeredby Shapere and Wilczek (1989) and further developed byMurray and Sastry (1993) and Kelly and Murray (1995),provides a powerful mathematical framework for analyzinglocomotion. A key product of this work is the developmentof the reconstruction equation for non-holonomic systems,which relates body velocity to changes in internal shape fora broad class of locomoting systems. We will not rederivethe reconstruction equation here; for a thorough treatment,see Bloch et al. (2003), Ostrowski and Burdick (1998), andShammas et al. (2007a).

This reconstruction equation has been used in a vari-ety of locomotion contexts. In Ostrowski and Burdick(1998) and Ostrowski et al. (2000) the reconstructionequation is combined with Lie-bracket theory to generatesinusoidal gaits that translate and rotate a variety of snake-like systems. Bullo and Lynch (2001) used the reconstruc-tion equation to decouple the locomotion of kinodynamic



990 The International Journal of Robotics Research 30(8)

systems and design kinematic gaits. More recently, therehas been interest in applying these techniques to swim-ming robots, such as McIsaac and Ostrowski’s (2003) workon anguilliform (eel-like) robots and the work by Mor-gansen et al. (2007) on fish, both of which combine thegeometric approach with biomimetic elements. In Hattonand Choset (2008), we introduced the connection vectorfield as a tool for visualizing the reconstruction equationdifferentially.

It is not generally possible to integrate the reconstructionequation in closed form, raising difficulties for the inverseproblem of finding shape changes that result in desiredtranslations. In special cases, however, Stokes’ theorem canbe used to find the net motion resulting from gaits (Kellyand Murray 1995). Mukherjee and Anderson (1993a,b)used this principle to analyze the motion of rolling disks,and Walsh and Sastry (1995) applied it to the case of anisolated three-link robot. Shammas et al. (2005, 2007a)combined this approach with the reconstruction equation todefine height functions on the shape space of their three-link robots that allowed the design of gaits resulting inspecified rotations. A similar technique was used by Melliet al. (2006) and later by Avron and Raz (2008) to generategaits for swimming robots.

The approaches in Shammas et al. (2007a), Melli et al.(2006), and Avron and Raz (2008) all shared the limita-tion that for general macroscopic (non-infinitesimal) gaits,the height functions could only be used to determine thenet rotations over the gaits, with the non-commutativityof translations and rotations precluding their use in deter-mining the net translation. Melli et al. and Avron andRaz were able to extend the techniques so as to find thenet translation over a limited set of macroscopic (but stillsmall magnitude) gaits, by using a higher-order definitionof the height functions. Shammas et al. took a differentapproach, using the height functions to generate coarseapproximations of the net displacement that disregard non-commutativity. These approximations were, in general, notclose enough to be used effectively in gait design, but inHatton and Choset (2009), we observed that we can signif-icantly reduce the error by making appropriate choices ofsystem parameterization, and in Hatton and Choset (2010)we took the first steps towards optimizing the parameterchoice.

3. Background

The present work makes use of several key techniques bor-rowed from geometric mechanics for representing the loco-motion of mechanical systems, the salient points of whichwe review here. In reviewing this material, our intention isto present it in terms accessible to a reader familiar withvector calculus, rather than go into the details of the under-lying differential geometry, which are thoroughly coveredin the prior work, particularly Kelly and Murray (1995) andBloch et al. (2003).

(a) World velocity (b) Body velocity

Fig. 1. Two representations of the velocity of a robot. The robot,represented by the triangle, is translating up and to the right, whilespinning counterclockwise. In (a), the world velocity, g, is mea-sured with respect to the global frame. The body velocity, ξ , in(b) is the velocity represented in the robot’s instantaneous localcoordinate frame.

3.1. The reconstruction equation

When analyzing a multi-body locomoting system, it is con-venient to separate its configuration space Q into a positionspace G and a shape space M , such that the position g ∈ Glocates the system in the world, and the shape r ∈ M givesthe relative arrangements of its bodies,1 and then considerhow manipulating the shape affects the position. The geo-metric mechanics community (Murray and Sastry 1993;Kelly and Murray 1995; Ostrowski and Burdick 1998;Bloch et al. 2003; Shammas et al. 2007a) has addressed thisquestion with the development of the reconstruction equa-tion and the local connection, tools for relating the bodyvelocity of the system, ξ , to its shape velocity r, and accu-mulated momentum p. The general reconstruction equationis of the form

ξ = −A( r) r + �( r) p, (1)

where ξ is the body velocity of the system, A( r) is thelocal connection, a matrix that relates joint to body veloc-ity, �( r) is the momentum distribution function, and p isthe generalized non-holonomic momentum, which captureshow much the system is ‘coasting’ at any given time (Blochet al. 2003). The body velocity, illustrated for a planar sys-tem in Figure 1(b), is the position velocity as expressed inthe instantaneous local coordinate frame of the system, thatis its forward, lateral, and turning velocities. For systemsthat translate and rotate in the plane, with an SE( 2) posi-tion space, the body and world velocities are related to eachother as

ξ =⎡⎣ξx

ξy

ξθ

⎤⎦ =

⎡⎣ cos θ sin θ 0

− sin θ cos θ 00 0 1

⎤⎦ g, (2)

where θ is the system’s orientation.For systems that have as many velocity or momentum

constraints as dimensions in the position space, the gen-eralized momentum disappears (Shammas et al. 2007a),




and the system’s behavior is dictated by the kinematicreconstruction equation,

ξ = −A( r) r, (3)

in which the local connection thus acts as a kind of Jaco-bian, mapping from velocities in the shape space to thecorresponding body velocity.

The geometric mechanics literature upon which thepresent work is based includes a wide variety of modelsfor multi-body locomoting systems that provide concreteexamples of the reconstruction equation and local connec-tion. As we develop the concepts in this paper, we will drawupon three of these models: the common differential-drivecar, the floating snake or planar space robot (Kelly andMurray 1995; Shammas et al. 2005, 2007a), and the kine-matic snake (Shammas et al. 2005, 2007a; Ishikawa 2009).Each of these systems moves in the plane, is kinematic, andhas two internal configuration variables. The two snake-like models are simple enough to be tractable, but com-plex enough to pose interesting motion-planning problems.In contrast, the differential-drive car has been extensivelystudied through conventional means; we take advantageof its familiarity to provide an intuitive link between ournew results and the prior work in this area. Table 1 illus-trates these three systems and provides some notes on theprinciples of their locomotion, which are encoded in theirreconstruction equations as follows:

3.1.1. Differential-drive car The coordinates for thedifferential-drive car are g = ( x, y, θ ) ∈ SE( 2), and r =(α1,α2) ∈ S × S. The reconstruction equation for this sys-tem is based on the no-slip and no-slide assumptions for itswheels, which provide three independent constraints relat-ing the body velocity of the vehicle to the angular velocitiesof the wheels (Kelly and Murray 1995). Normalized forwheel radius and body width, the reconstruction equationfor the differential-drive car is

ξ = −⎡⎣−1 −1

0 01 −1

⎤⎦[

α1

α2

]. (4)

This equation matches an intuitive understanding of how adifferential-drive vehicle works: when the wheels are turnedtogether, the vehicle moves forward; when they are turnedoppositely, the vehicle rotates; and no combination of inputscan move the system laterally.

3.1.2. Floating snake For the floating snake, we take theposition g = ( x, y, θ ) ∈ SE( 2) as the position of the centerof mass and the orientation of the middle link and the shaper =(α1,α2) ∈ R

2 as the interlink joint angles. If the sys-tem starts at rest, conservation of linear momentum ensuresthat the velocity of the center of mass remains zero for alltime, so the first two rows of the local connection (corre-sponding to translation) are zero. The third row of the local

connection identifies rotational velocities that preserve a netangular momentum of zero in response to specified jointvelocities (Shammas et al. 2007a). Taking the rows together,the reconstruction equation for a floating snake with equallink lengths and inertias is

ξ = − 1

D

⎡⎣ 0 0

0 0a31 a32

⎤⎦[

α1

α2

], (5)

where

a31 = 5 + 3 cos (α2) + cos (α1 − α2)

a32 = 5 + 3 cos (α1) + cos (α1 − α2)

D = 19 + 6( cos (α1) + cos (α2)) +2 cos (α1 − α2).

3.1.3. Kinematic snake The position and orientation of thekinematic snake are similar to those of the floating snake,with g = ( x, y, θ ) ∈ SE( 2) and r = (α1,α2) ∈ R

2, exceptthat here we take the position as that of the middle link,rather than the center of mass. Outside of singularities, theno-slide conditions on the passive wheelsets constitute threeindependent constraints, equal in number to the positioncoordinates, so the system has no direction in which to coastand behaves kinematically. As detailed in Shammas et al.(2007a), the constraints define the local connection for thesystem, such that the reconstruction equation, normalizedfor link length, is

ξ = − 1

D

⎡⎣1 + cos (α2) 1 + cos (α1)

0 0− sin (α2) − sin (α1)

⎤⎦[

α1

α2

], (6)

where D = sin (α1) − sin (α2) + sin (α1 − α2).

3.2. Shape changes, gaits, and image-families

To facilitate discussion of motion plans for locomoting sys-tems, we define a vocabulary describing operations in theirshape spaces.

Definition 3.1 (Shape change). A shape change ψ is a tra-jectory in the shape space M of the robot over an interval[0, T], that is, the set of all shape changes is

� = {ψ ∈ C1 | ψ : [0, T] → M} (7)

where ψ( 0) ,ψ( T) ∈ M are, respectively, the start and endshapes.

Definition 3.2 (Gait). A gait φ is a cyclic shape change,that is, the set of all gaits is

� = {φ ∈ � | φ( 0) = φ( T) }. (8)

Note that a gait has a defined start shape φ( 0); two gaitswhose images in M are the same closed curve, but withdifferent start points, are distinct.




Definition 3.3 (Image-family). The image-family φ of agait φ is the set of all gaits that share its image (i.e. traceout the same closed curve) in M,

φ = {ϕ ∈ � | Im(ϕ) = Im(φ) }. (9)

The image-family includes all continuous time-reparameterizations and changes in starting point of agait.

3.3. Stokes’ theorem

In the second half of this paper, we use Stokes’ theorem(Boothby 1986) to approximate the net displacement of ourrobot over specified gaits. As a brief review, Stokes’ the-orem equates the line integral along a closed curve on avector field V on a space U to the integral of the curl of thatvector field2 over a surface bounded by the curve,

‰∂�

V ( u) ·du =¨�

curl V ( u) du, (10)

where � is a surface on U bounded by ∂�.The special case of Stokes’ theorem for vector fields on

R2 is also known as Green’s theorem. In this case, the curl

acts as a scalar derivative of the vector field that measureschange in both magnitude and orientation; the curl of avector field V on a space U ⊂ R

2 is

curl V = ∂v2

∂u1− ∂v1

∂u2. (11)

The surface � is just the region of U enclosed by ∂�, and(10) specializes to the simple area integral

ˆ∂�

V ( u) ·du =¨�

(∂v2

∂u1− ∂v1

∂u2

)du1 du2. (12)

4. Connection vector fields

On their own, the reconstruction equation and local con-nection provide a simple, generalized process model foroptimization-based approaches to motion planning (Sfakio-takis and Tsakiris 2007). They also provide a useful frame-work for applying Lie-bracket theory to the design ofsmall-amplitude gaits (Ostrowski et al. 2000) (i.e. small-amplitude oscillations in the shape variables) that propelthe systems in desired net directions. Unfortunately, theprocess-model approaches do not take advantage of the geo-metric structure of the reconstruction equation and the Lie-bracket techniques produce inefficient motions for whichmuch of the input energy is expended in high-frequencyoscillations.

We are interested here in developing motion-planningtechniques that make efficient use of the structure of thelocal connection, both in the sense that the structure guidesthe optimization, and that the resulting gaits efficiently con-vert internal motions into net displacements. As the firststep in developing these techniques, we have introduced

connection vector fields (Hatton and Choset 2008). Thesevector fields comprehensively illustrate the structure of thelocal connection in a form that intuitively captures the rela-tionship between shape and position changes. The connec-tion vector fields can be used directly to identify com-ponents of a given gait, such as the ‘power stroke’ and‘windup’ phases, or, as in the following sections, as a basisfor designing further motion-planning tools.

4.1. Definition

Each row A( r)i of the local connection can be consideredas defining a vector field on the shape space whose dotproduct with the shape velocity produces the correspondingcomponent of the body velocity,

ξi = �Aξi( r) ·r (13)

where the elements of �Aξi are taken from the ith row ofthe local connection and, for convenience, we wrap in thenegative sign when extracting this row as a vector field.3

Considering the local connection as a set of vector fieldswith the dot product operator provides a strong geometricintuition for understanding the relationship between shapeand position motions. The geometric interpretation of thedot product in Equation (13) is

ξi = �Aξi( r) ·r = ‖�Aξi ( r) ‖ ‖r‖ cos�, (14)

where � is the angle between the vectors. Taking the cos�term as a measure of the alignment of �Aξi ( r) and r, thebody velocity ξi is positive, negative, or zero when thetwo vectors have correspondingly positive, negative, or zeroalignment, and is scaled by the magnitudes of �Aξi and r.

For example, the two representative r vectors a and bin Figure 2 are, respectively, aligned with or perpendicu-lar to �Aξx (the ‘longitudinal connection vector field’) andperpendicular to or anti-aligned with �Aξθ (the ‘rotationalconnection vector field’). The ‘lateral connection vectorfield’ �Aξy is null, and thus has zero dot product with allvectors. Consequently, we can tell by inspection that theinput vector a produces a forward velocity with no lateralor rotational velocity, and that the input vector b producesa negative rotational velocity with no longitudinal or lateralmovement.

4.2. Applied examples

Beyond allowing for the characterization of the effect ofinput shape velocities, the connection vector fields alsofacilitate decomposing the displacements caused by shapechanges and gaits into their constituent motions, as demon-strated in the examples below.

4.2.1. Differential-drive car As an introduction to workingwith connection vector fields, consider the differential-drivecar, which has simple kinematics and two non-zero connec-tion vector fields. These connection vector fields are plotted




Fig. 2. Connection vector fields for the three-link kinematic snake shown in Table 1. The �Aξy field is null at all points in the shapespace, as the middle wheelset constrains ξy to zero under all conditions. Due to singularities in the vector fields at the lines α1 = ±π ,α2 = ±π , and α1 = α2, the magnitudes of the vector fields have been scaled by their arctangents for readability. The input shapevectors at a and b produce pure forward translation and pure negative rotation, respectively.

(a) �Aξx (left) and �Aξθ (right) for the differential-drive vehicle, alongwith an example shape change ψ .

(b) Position change of the differential-drive vehicle while executing ψ .

Fig. 3. Connection vector fields for the differential-drive vehicle,along with an example shape change ψ and the resulting positionchange.

with a shape change ψ in Figure 3. The connection vectorfields are constant over the shape space and match our intu-itive understanding of how the system works: turning thewheels together in the +α1, +α2 direction moves the car

forward, while turning them oppositely in the −α1, +α2

direction rotates the car.The shape change ψ in Figure 3(a) is divided into three

stages, ψ1, ψ2, and ψ3. Over the first stage, ψ1 is alignedwith �Aξx and orthogonal to �Aξθ ; the robot thus drives for-ward while executing ψ1. In the second stage, ψ2 is alignedwith �Aξθ and perpendicular to �Aξx ; the robot correspond-ingly rotates in place while executing ψ2. In the third andfinal stage, ψ3 has positive alignment with �Aξθ and negativealignment with �Aξx ; the resulting position motion is thatthe robot rotates positively while reversing. The resultingposition changes are illustrated in Figure 3(b).

4.2.2. Floating snake The differential-drive car is a specialcase, with constant connection vector fields. Most systemshave connection vector fields that vary with the system’sshape. The floating snake provides a simple example ofhow this variation can provide information about the sys-tem’s behavior. As the first two rows of its local connectionin Equation (5) are null, all the information regarding thissystem’s position change in response to a shape change isencoded in the third, rotational row of the local connection.The plot of the corresponding connection vector field, �Aξθ ,in Figure 4(a) matches an intuitive understanding of howa system governed by conservation of angular momentumshould behave: the general trend to point in the +α1, −α2

direction corresponds to the central body counter-rotatingagainst the outer links. The exact heading of the field varieswith how much leverage the links exert on the system; themore extended an outer link is, the faster the central bodyneeds to counter-rotate to nullify the outer link’s angularmomentum.

Figure 4(a) also overlays �Aξθ with a plot of a sample gaitφ( t) =(α1( t) ,α2( t) ), with

⎡⎣α1( t)

α2( t)

⎤⎦ =

⎡⎣π

4

π4

⎤⎦ + R( −π/4)

⎡⎣ 9

4 cos ( t + 1716π )

98 sin ( t + 17

16π )

⎤⎦ , (15)




(a) �Aξθ for the floating snake, along with a gait φ.

(b) Orientation θ of the snake while executing φ.

Fig. 4. Sample gait φ for the floating snake. In the first seg-ment, the shape velocity φ is positively aligned with �Aξθ , so thesnake rotates positively. In the second segment, φ is negativelyaligned with the connection vector field, resulting in negativerotation.

where R( γ ) is a rotation by γ and the representation usedis chosen to highlight the position, orientation, scale, andphase of the elliptical path. The shape velocity φ( t) =[α1( t) α2( t) ]T at time t is a tangent vector to φ at φ( t).It is clear that φ( t) has positive alignment with �Aξθ (φ( t) )over the first segment of the gait and negative align-ment over the second segment, with zero alignment at thestart, end, and transition between segments; we can thusimmediately conclude that the robot rotates first positively,then negatively while executing the gait, as illustrated inFigure 4(b).

Closer observation of Figure 4(a) reveals that �Aξθ forthe floating snake has approximately constant magnitudeand that its curvature results in the negatively aligned Seg-ment 2 being longer than the positively aligned Segment 1,with the former acting as the ‘power stroke’ of the motionand the latter serving in a ‘windup’ role. This propertyresults in the net negative rotation over the gait, and wewill examine this property more closely in the followingsections.

4.2.3. Kinematic snake The kinematic snake’s connectionvector fields combine the interesting features from boththe differential-drive car and the floating snake: there aremultiple non-zero fields, and each varies richly with theshape of the robot. The vector fields also exhibit anotherinteresting feature not seen in the previous examples: sin-gularities. At shapes on the lines α1 = ±π , α2 = ±π ,and α1 = α2, the wheelset constraints are not indepen-dent and the local connection loses rank. For example, inTable 1, the kinematic snake is shown in a singular config-uration with α1 = α2. The normal lines to the constraintsall meet at a common center, allowing the system to rotatefreely around that center. In such configurations, the mag-nitudes of the connection vector fields approach infinity,the constraints lock the joints, and the system is unable tochange shape through internal torque. The system does gaina ‘coasting’ degree of freedom, but this state is uncontrol-lable, and the impracticality of reaching the singular statemeans that we need not consider gaits that pass through thesingularities.

The vector fields for the kinematic snake are plotted inFigure 5 along with two example gaits φ1 and φ2. The sin-gularities in the connection vector fields also pose a displayproblem, as the large magnitudes near the singularities sat-urate the resolution of the vector magnitudes in the plot andobscure the field in the regions we are more interested in. Toaddress this issue, we use a scaled arctangent normalizationon the vector magnitudes while maintaining their headings.The non-linearity of the arctangent function preserves thedifferences between small values, while pulling large valuesdown to a ceiling level. Along with the vector field plots, wepresent two gaits based on those in Shammas et al. (2007a),defined as

⎡⎣α1( t)

α2( t)

⎤⎦ =

⎡⎣−π

2

π2

⎤⎦ + R( −π/4)

⎡⎣ 5

4 cos ( t + τi)

1720 sin ( t + τi)

⎤⎦ , (16)

with τ1 = 0 for φ1 and τ2 = 4964π for φ2. We note for future

reference that they trace out the same closed curve in theshape space but from different starting points, and so areboth in the same image-family (i.e. φ2 ∈ φ1 or, equivalently,φ1 ∈ φ2).

Unlike the differential-drive vehicle, the kinematic snakecannot be simply ‘driven forward’ and it is not immedi-ately clear (outside of direct calculation) how executing theexample gaits changes the position of the robot. With theaid of the connection vector fields, however, we can gainadditional insight into the system’s behavior and identifycharacteristics of the position change during the gait. Inφ1, the robot starts with φ1 positively aligned with �Aξx andorthogonal to �Aξθ , and thus moves straight forward. As thegait moves from state 1 to state 2, φ1 becomes progres-sively anti-aligned with �Aξθ and orthogonal to �Aξx , untilat 2 the position change is purely negative rotation, gen-erating a cusp in the position trajectory. From 2 to 3, φ1




Fig. 5. Connection vector fields for the kinematic snake robot, along with the second example gait φ2 and the resulting positionchange.

becomes negatively aligned with �Aξx and perpendicular to�Aξθ generating a short stretch of backwards translation.As the robot reaches stage 4, φ1 again becomes orthog-onal to �Aξx , but this time is aligned with �Aξθ , and theresulting cusp returns the robot to its initial orientation.Finally, at stage 5, φ1 is once more aligned with �Aξx andorthogonal to �Aξθ , producing a second stretch of forwardmotion.

The second gait, φ2, contains all the same segments ofmotion as φ1, but in a different order. The initial config-uration of φ2 is at a point of pure rotation, so the result-ing position motion starts at what was the first cusp of φ1

and follows the short backward translation into the sec-ond cusp. Coming out of the cusp, it has rotated by π/2radians from its original starting orientation as it startsthe long translation segment from 3 to 4, sending thismotion into the −y direction. The cusp at 5 serves toreturn the snake to its original orientation with no accruedx motion.

If we consider the cusps as turning maneuvers, thedifference between the two gaits can roughly be charac-terized as φ1 having a ‘move–turn–turnback–move’ pat-tern, while φ2 has a ‘turn–move–turnback’ pattern. Inputsthat fall between these two patterns (i.e. start at differ-ent points on the ellipse) produce net displacements thatfall between the pure x and pure y displacements shownhere.

5. Connection height functions

In Section 4.2.2, we noted that the ‘curvature’ of the vectorfield in Figure 4(a) resulted in the example gait opposingthe ‘flow’ of the floating snake’s �Aξθ field more than itfollowed the flow. We then inferred a relationship betweenthis property and the net negative rotation experienced bythe system. To further examine this phenomenon, we turnto an approach based on Stokes’ theorem (Boothby 1986).By applying this theorem to the connection vector fields,we can generate scalar connection height functions on theshape space that directly capture the net motion resultingfrom arbitrary choices of gaits. These height functionshave been investigated in several prior works (Walsh andSastry 1995; Melli et al. 2006; Shammas et al. 2007a),but in each work were generated by direct mathematicalprocessing of the local connection, with no physicalintuition attached. Additionally, while the height functionswere useful for designing gaits to produce desired netrotations, limitations on their applicability prevented themfrom accurately capturing system translations. Here, weincorporate intuition from the connection vector fields intothe derivation and interpretation of the height functions;this both provides additional insight into their operationand lays the groundwork for the following section, in whichwe use the connection vector fields to circumvent previouslimitations on the height functions.




(a) Path followed by the system

(b) Accrued displacement

(c) Accrued BVI

Fig. 6. Example motion showing the difference between displace-ment and BVI. Because the BVI is calculated in the body frame,the ‘backward’ motion in stage 4 cancels out the ‘forward’ motionin stage 2.

5.1. Displacement and the body-velocityintegral

Before we can discuss tools for calculating the net motionover a gait at any technical level, we need to define what wemean by ‘net motion.’ While the net motion should clearlybe an integral of the body velocity, there are two senses inwhich the phrase ‘integrating the body velocity over time’can be interpreted. In the first, most natural sense, we inte-grate to find the resulting displacement.4 This quantity isfound by the non-linear iterative integral

g( t) =ˆ t

0

⎡⎣cos θ ( τ ) − sin θ ( τ ) 0

sin θ ( τ ) cos θ ( τ ) 00 0 1

⎤⎦

⎡⎣ξx( τ )ξy( τ )ξθ ( τ )

⎤⎦ dτ ,

(17)

in which ξx and ξy, the translational components of the bodyvelocity, are rotated into the world frame at each time.

The second sense of integrating ξ is to take its simplevector integral ζ ,

ζ ( t) =⎡⎣ζx( t)ζy( t)ζθ ( t)

⎤⎦ =

ˆ t

0

⎡⎣ξx( τ )ξy( τ )ξθ ( τ )

⎤⎦ dτ , (18)

where we term ζ the body-velocity integral, or BVI. Physi-cally, the BVI corresponds to the raw odometry for the robot(i.e. the net forwards minus backwards motion in each bodydirection) and it does not account for changes in the align-ment between body and world directions, that is, the relativeorientation of the body and world frames. While the integralin Equation (18) has appeared previously (Shammas et al.2007a), its result ζ has not previously been named, and itsphysical meaning has not been explored.

On first appearance, the BVI does not seem a particularlyuseful measure of the net motion of a system. For example,consider the path taken by the system in Figure 6. Over thecourse of the motion, the system clearly accrues a non-zeronet displacement. However, because the ‘backward’ motionin stage 4 cancels out the ‘forward’ motion in stage 2, thenet BVI is zero in all components: the same as if the systemhad not moved at all.

The BVI does have three saving graces that make it wor-thy of study. First, while the displacement integral in Equa-tion (17) must be solved individually for any given gait, theBVI integral in Equation (18) can be replaced by an equiv-alent area integral via Stokes’ theorem, providing heightfunctions that encode its solution over all possible gaits.Second, unlike the translational components, the rotationalcomponents of the displacement and BVI are identicallyequal, so the net rotation over a gait can be directly deter-mined from the BVI. Third and most importantly, under theright conditions the translational components of the BVIcan be taken as close approximations of the displacement,and these conditions can be expressed as simple functionson the shape space.




5.2. The BVI, Stokes’ theorem, and heightfunctions

Starting from the definition of the BVI in Equation (18), wecan express the BVI over a shape change ψ as a set of lineintegrals on the connection vector fields,

ζ ( T) =ˆ T

0ξi( τ ) dτ (19)

=ˆ T

0

�Aξi (ψ( τ ) ) ·ψ( τ ) dτ (20)

=ˆψ

�Aξi ( r) dr. (21)

For shape changes that are gaits, we can further applyGreen’s form of Stokes’ theorem, Equation (12), to convertthis line integral into an area integral,

ζ ( T) =¨φa

curl �Aξi ( r) dr (22)

=¨φa

∂ �Aξi2

∂α1− ∂ �Aξi

1

∂α2dr, (23)

where φa is the area on M enclosed by the gait. Note thatas all gaits in an image-family enclose the same area, theintegral in Equation (23), and thus the BVI, is the same forall gaits in an image-family.

By plotting the integrand of Equation (23) as a heightfunction Hζj on the shape space, we can easily identify gaitsthat produce various displacements. Such height functionshave featured in prior works, and rules of thumb for design-ing curves that produce desired values of the integral inEquation (23) are given in Shammas et al. (2007a):

1. Non-zero net integral (I). A loop in a region where thesign of Hζj is constant produces a non-zero net integral.The sign of the integral depends on the sign of Hζj andthe orientation of the loop (the direction in which it isfollowed).

2. Non-zero net integral (II). A figure-eight across a lineHζj ( r) = 0, where each half is in a region where the signof Hζj is constant, produces a non-zero net integral.

3. Zero net integral. A loop that encloses equally positiveand negative regions of Hζj ( r), such that the integralsover the positive and negative regions cancel each otherout, produces a zero net integral.

In an automated gait design algorithm, these rules wouldserve to guide optimization of gait parameters, for exampleby indicating regions that the curve should grow to encloseor shrink to avoid.

Fig. 7. Rotational connection vector field �Aξθ and height functionHζθ for the floating snake. The gait image-family encircles a neg-ative region of the height function; correspondingly, any gait in theimage-family produces a net negative rotation.

5.3. Finding net rotation from the heightfunction

The floating snake is the classic example of using a con-nection height function (Kelly and Murray 1995; Sham-mas et al. 2005, 2007a). Taking the curl of �Aξθ producesthe height function Hζθ shown in Figure 7. For any givengait, integrating Hζθ over the area enclosed by that gaitgives the net rotation. For instance, the gait image-familyoverlaid on Hζθ in Figure 7 contains the gait shown inFigure 4(a). It positively encircles a negative region ofHζθ , and thus produces the net negative rotation seen inFigure 4(b).

Note that in Section 4.2.2, we attributed the net nega-tive rotation to the difference in length between the ‘pos-itive’ and ‘negative’ segments of the gait, but we are nowattributing it to the value of the height function in theregion encircled by the gait. Some intuition on the work-ings of the height functions can be gained by realizingthat these two reasons are in fact closely linked: the heightfunction measures the curl of the vector field that bends it




to align more closely to the second half of the gait thanthe first.

5.4. The BVI as an approximation ofdisplacement

Although the example in Figure 6 shows that we cannotalways take the BVI as an indication of the net displace-ment, it does not show that we can never do so. To inves-tigate the existence of gaits for which the BVI is a reliablemeasure of displacement, we consider the error εζ = ζ − gbetween the BVI and displacement over an arbitrary gait.By subtracting the displacement integral in Equation (17)from the BVI integral in Equation (18) and combining theintegrands, we can express this error as

εζ =ˆ t

0

⎡⎣1 − cos θ sin θ 0

− sin θ 1 − cos θ 00 0 0

⎤⎦

⎡⎣ξx

ξy

ξθ

⎤⎦ dτ , (24)

with an implicit dependence on τ for all variables in theintegrand. From Equation (24), we can easily identify a con-dition that induces small error terms: if θ ( t) remains smallfor all t, then the matrix in the integrand remains close to azero matrix for all t, and the error εζ is thus close to a zerovector. We will thus turn our attention to finding situationsfor which this small-angle condition holds true. Note thatboth the displacement and the BVI are calculated relativeto the system’s starting configuration, that is g( 0) =( 0, 0, 0)for the purposes of this calculation; any difference betweenthe starting position and a preferred origin of the spaceresults in a transform that applies equally to g and ζ anddrops out of the error calculation in Equation (24).

Trivially, small gaits (i.e. gaits described by small-amplitude loops in the shape space) offer θ no opportunityto grow large; these gaits have been studied extensively inthe controls literature with the aid of Lie-algebra analysis(Murray and Sastry 1993; Ostrowski and Burdick 1998;Melli et al. 2006; Morgansen et al. 2007). Small gaits, how-ever, spend more energy in high-frequency oscillations thanthey do in producing net motion and are thus inefficient; weare interested in designing larger, more efficient gaits. Tothis end, we observe that as the orientation of the systemover a gait is the integral of its rotational body velocity,

θ ( t) =ˆ t

0ξθ ( τ ) dτ =

ˆ φ(t)

φ(0)

�Aξθ ( r) dr, (25)

the magnitude of its value at any time is bounded to theline integral of the magnitude of the rotational connectionvector field,

|θ ( t) | ≤ˆ φ(t)

φ(0)| �Aξθ ( r) dr|. (26)

For gaits in regions where ‖�Aξθ ( r) ‖ is small, θ ( t) and εζ ( t)are thus guaranteed to be small, and the BVIs of those gaitsare thus good approximations of their displacements.

Unfortunately, this observation does not directly help usto apply the height function approach to translation gaits forsystems like the kinematic snake. The �Aξθ ( r) vector fieldfor the kinematic snake in Figure 5 clearly does not van-ish in any region, so we cannot expect the small-rotationcondition to hold over gaits placed anywhere in the shapespace. The most successful past attempt to address this lim-itation used an alternative definition of the height functionsthat includes second-order terms that account for rotation(Melli et al. 2006; Avron and Raz 2008). This higher-orderapproach allows for the design of gaits that are ‘small,’rather than ‘differentially small,’ but does not extend to‘large’ gaits. Another approach has been to withdraw fromthe goal of precise gait-evaluation via the height functionsand to design gaits that have large BVIs, in the antici-pation that they will produce correspondingly large dis-placements (Shammas et al. 2007a). This assumption thatthe magnitude of the displacement is correlated to that ofthe BVI has produced some usefully efficient gaits, but isunsatisfying as a provable motion-planning technique, asit relies on an assumption with direct counter-examples(such as in Figure 6). To bring the height functions into aform where they indicate displacement, a new approach isneeded.

6. The importance of coordinate choice

As the final stage in developing a height function approachfor systems like the kinematic snake, we now discuss theimportance of coordinate choice. This constitutes a signif-icant departure from previous work, in which the choiceof body frame was considered largely unimportant, exceptthat it facilitated expression of the system constraints. Wehave observed, however, that the choice of coordinate framesignificantly affects the structure of the connection vec-tor fields, including the presence or absence of the small-rotation condition described in Section 5.4. By carefullyselecting our coordinates, then, we can manipulate the con-nection vector fields to meet these conditions, and thusproduce useful height functions. Further, this selectioncan be accomplished by a simple change-of-coordinatesoperation after the local connection has been found in aconvenient frame, thus preserving the advantages of thecustomary frame while gaining the benefits of the newframe.

6.1. BVI and displacement for a sample gaitimage-family

To demonstrate the benefits of changing the system’s coor-dinates, we compare the performance of the connectionheight functions in approximating the displacement overa representative image-family of gaits φ under differentchoices of coordinates. As a baseline for comparison, wefirst consider the interaction of φ with the height func-tions corresponding to the original choice of coordinates.




Fig. 8. Representative gait image-family overlaid on the connection vector fields and height functions of the kinematic snake in theoriginal coordinate system depicted in Table 1. (Owing to singularities in the local connection, the magnitudes of the height functionsand vector fields are scaled by their arctangents.)

Figure 8 shows the positive (counterclockwise) elements ofφ overlaid on the connection vector fields and height func-tions. On the height functions, the gait family encircles apositive area on Hζx , a zero area on Hζy , and a net-zero areaon Hζθ . As the rotational BVI and displacement are equalto each other, we can use the rules from Section 5.2, to con-clude from the net-zero integral on Hζθ that the three-linkrobot undergoes no net rotation over the course of any gaitfrom this family. However, because the �Aξθ field is non-vanishing the system will undergo intermediate rotation,forcing the BVI to diverge from the displacement.

The BVI for this image-family is shown in Figure 9 alongwith the locus of all of the net displacements generated bygaits in the image-family. The BVI is a point on the posi-tive x-axis, with coordinates corresponding to the areas thatthe image-family encircles on the height functions, whilethe locus of net displacements is an arc with a significant−y component that is not captured by the BVI. Note thatthe arc of displacements is at constant radius from the ori-gin: for a gait with zero net rotation, changing the startingshape only changes the direction of net translation as com-pared to the starting frame, and does not affect the mag-nitude of translation. A proof of this property is given inAppendix A.

6.2. BVI and displacement in a mean-orientationframe

While the �Aξθ field for the kinematic snake in Figure 8 hadno region of small magnitude, our experience has shownthat changing the definition of the system’s orientation sub-stantially changes the structure of this field, to the degreethat a good choice of coordinates can induce a null regionin �Aξθ . For instance, instead of using the orientation of themiddle link as the θ component of g, we can derive the localconnection with θ representing the mean orientation of thethree links, as shown in Figures 10 and 11. The connec-tion vector fields and height functions corresponding to thiscoordinate choice are plotted with φ in Figure 12. With thisnew choice of coordinates, �Aξθ now has a region of smallmagnitude in the same region as φ, making the BVI a goodapproximation of the displacement for gaits in this image-family. The improvement of this estimate as compared tothe estimate under the previous coordinate choice is shownin Figure 13, with both a significant reduction in the vari-ance of net displacements and a much closer placement ofthe BVI to this locus. Figure 14 illustrates the underlyingprinciple of this reduction in variance: Under the originalcoordinate choice, the two gaits selected from the example




Fig. 9. The BVI and locus of displacements corresponding to theimage-family of gaits for the kinematic snake robot, depicted inFigure 8, are represented, respectively, by the circle on the x-axisand the arc section. The BVI only depends on an area integralover the area enclosed by the gait and is thus the same for allgaits in the family. The displacement, however, also depends onthe start/end shape of the gait, and there is, thus, a range of dis-placements, each corresponding to one starting point in the image-family. The displacements are of the order of one link length ofthe robot.

image-family produce displacements of the same magni-tude, but in different directions, whereas the new choiceof orientation realigns these displacements to point in thesame direction.

6.3. Identifying valid coordinate choices

The increased utility of the connection height functionsin the mean-orientation coordinate system raises twoimportant questions: First, what properties of the mean-orientation coordinate system led to the improvement? Sec-ond, what other coordinate sets are available, and might theyprovide even more nullification of the �Aξθ field? Startingwith the second question, we offer the notion of a validcoordinate set as one that induces a body frame for whichthe body velocity is produced by a reconstruction equa-tion of the form of Equation (1); for kinematic systems,this reconstruction equation should reduce to the form ofEquation (3). This notion is compatible with the derivationsof the reconstruction equation in Kelly and Murray (1995),Ostrowski (1998), and Shammas et al. (2007a), which alsoinclude a means of testing the validity of individual coor-dinate sets. The test used in those works is, unfortunately,only useful for validating given coordinate sets, and doesnot provide a means for finding new coordinate sets exceptby trial and error. However, if we have a valid coordinate setfrom which to start, such as one of the frames in Table 1,we can use the following lemma to directly find other validcoordinate sets.

Fig. 10. New orientation definition for kinematic snake. The ori-entation θ is the mean of the individual link orientations θ1, θ2,and θ3. Note that the body frame directions xb and yb are, respec-tively, aligned with and perpendicular to the orientation line, andnot to the central axis of the middle link.

(a) Original. (b) Mean orientation.

Fig. 11. A three-link system in various configurations. In both (a)and (b), the system is shown at five points on the shape spacewith reference orientation θ = 0. In (a), θ is the orientation ofthe middle link, while in (b), θ is the mean orientation of the links.

Lemma 6.1 (Relationship of valid coordinate sets). Inany valid coordinate set (i.e. one for which a local connec-tion can be derived), the body frame of the system describedby the coordinates is related by a shape-dependent trans-form to the body frame in a coordinate set known to bevalid.

Proof. Consider the two coordinate frames shown inFigure 15. The frame designated by ( xb

old, ybold) represents

an original, known-valid body frame for the system andthe frame designated by ( xb

new, ybnew) is a second frame

that is a candidate to be a body frame under a new choiceof coordinates. For any such frame, we can define itsposition and orientation in the original body frame to beβ =(βx,βy,βθ ), as shown in Figure 15. The body velocityξ new of this frame, that is, its velocity as measured along itsown instantaneous coordinates, is

ξ new = Brot · ( ξ + β + B×ξθ ) , (27)




Fig. 12. Representative gait image-family overlaid on the connection vector fields and height functions of the kinematic snake in themean-orientation coordinate system depicted in Figure 10. (Owing to singularities in the local connection, the magnitudes of the heightfunctions and vector fields are scaled by their arctangents.)

where β represents the relative velocity of the two frames,B× is the cross product term that gives the translationalvelocity of the new frame produced by rotation of the oldframe,

B× =⎡⎣−βy

βx

0

⎤⎦ , (28)

and Brot is the rotation matrix

Brot =⎡⎣ cosβθ sinβθ 0

− sinβθ cosβθ 00 0 1

⎤⎦ (29)

that handles the conversion of the velocity vector betweenthe two frames.

For the kinematic systems we are considering, if this newframe is a valid body frame for the system, then ξ new mustbe produced by a kinematic reconstruction equation of theform of Equation (3). We first address the sufficiency ofour condition on β: when β is a function of r, the relativevelocity between the frames is

β = ∂β

∂rr (30)

and the cross-product term is also a function of the shape,so Equation (27) thus becomes

ξ new = Brot( r) ·(ξ + ∂β

∂rr + B×( r) ξθ

). (31)

By substituting in the original kinematic reconstructionEquation (3) into Equation (31) and regrouping,

ξ new = Brot( r) ·(

−A( r) r + ∂β

∂rr − B×( r) Aξθ ( r) r

)(32)

= Brot( r) ·(

−A( r) +∂β∂r

− B×( r) Aξθ ( r)

)r (33)

= −Anew( r) r, (34)

we see that ξ new is produced by a matrix of the right formto be a local connection, and that having β as a function ofr is sufficient to define a valid body frame. To see that it isalso a necessary condition, consider the effect of definingβ as a function of anything other than r. In this situation,the expansion of Equation (27) into Equation (31) wouldcontain terms that cannot be collected into Anew( r) inEquation (34), precluding such a β from meeting ourdefinition of a valid coordinate system.




Fig. 13. The BVI and displacements corresponding to the image-family of gaits in Figure 12 for the kinematic snake robot withthe new measure of orientation depicted in Figure 10 are repre-sented, respectively, by the cross above the x-axis and the shortarc section, with the BVI and displacements as measured in theold coordinate choice presented for reference. With the new mea-sure, the orientation of the system remains almost constant overthe course of any of the gaits in the family, and the BVI ζ is thus agood approximation of the displacements.

6.4. Finding an optimal coordinate choice

Having defined the valid body frames, and thus the avail-able choices of coordinates, we can address the questionsof why the mean-orientation body frame was a good choiceand how to systematically find an optimal choice.

6.4.1. Optimal choice of orientation The key to finding theoptimal choice of orientation lies in the third row of the ∂β

∂r

term in Equation (31), ∂βθ∂r . As βθ is a function of r, ∂βθ

∂r isits gradient with respect to the shape, ∇rβθ . Because Brot

and B× only contribute to the translational components ofthe new local connection, any difference between �Aξθ and�Aξθ

new is due to the addition of ∇rβθ .When moving from the center-link frame for the kine-

matic snake to the mean-orientation frame, βθ = ( −α1 +α2) /3 and its gradient is ∇rβθ = [− 1

313

], as shown in

Figure 16. Figure 17 visually represents the addition of thegradient field to the original �Aξθ field. In the two circledregions, the fields cancel each other out, creating the nullregions that we observed in Figure 12. In these regions, thenew body frame counter-rotates with respect to the center-link frame, and thus rotates very little with respect to theworld in response to shape changes.

Based on this observation of what makes the mean-orientation frame an effective choice of coordinates, wenow have a criterion for an optimal choice of coordinates:It should induce a ∇rβθ field that most nullifies �Aξθ . Whilethe exact meaning of ‘most nullifies’ is open to interpreta-tion, for now we will take it as a minimization of the average

(a) Original coordinate choice.

(b) Mean-orientation coordinate choice.

Fig. 14. Position trajectories for two gaits taken from the exam-ple image-family in Figure 12, as measured in the original andmean-orientation coordinate frames. In the mean-orientation coor-dinate frame, the trajectories are reoriented such that their netdisplacements are in the same direction.

Fig. 15. An original, known-valid body frame, and a new framedisplaced from that frame by β




Fig. 16. The relative orientation βθ and its gradient ∇rβθ betweenthe mean-orientation frame and the original center-link frame forthe kinematic snake, as a function of the shape α.

squared magnitude �Aξθnew over the region of interest, that is,

minimizing D for

D =¨�

‖�Aξθ + ∇rβθ‖2 d� (35)

for� ⊂ M . Because βθ is a function of r, we can generate itdirectly from ∇rβθ (modulo an integration constant) and wecan shift attention to directly finding ∇rβθ that minimizesEquation (35).

Identifying optimal coordinate choices with minimiza-tions of D in Equation (35) provides the key to find-ing the best coordinate choices for arbitrary systems. Thecost function D is minimized when ∇rβθ is the gradientfield that most closely approximates −�Aξθ , that is, it isthe negative projection of �Aξθ onto the space of conser-vative vector fields. This projection is identical to that inthe Hodge–Helmholtz decomposition (Arfken 2005), whichseparates a vector field into a conservative component anda divergence-free remainder. Here, the conservative compo-nent is equal to −∇rβθ , while the remainder becomes �Aξθ

new.While an analytical solution for this projection requires that�Aξθ meet certain regularity conditions and is not gener-ally feasible, the discrete Hodge–Helmholtz decomposition(Guo et al. 2005) provides a numerical minimization ofEquation (35) and is relatively easy to compute for arbi-trary vector fields via finite-element methods, as explainedin Appendix C.

In practice, we have found that an extra refinement to thedecomposition is necessary when �Aξθ has singularities. Thelarge magnitudes of such fields in the vicinity of the singu-larities dominate the integral in Equation (35), producing a∇rβθ field that closely approximates −�Aξθ at the singulari-ties, but at the expense of any accuracy over the rest of thefield. We address this limitation by using the same arctan-gent scaling we use to display the vector fields, and insteadsolve the minimization problem with

D =¨�

∣∣∣∣∣∣∣∣∣∣arctan( k‖�Aξθ ‖)

k‖�Aξθ ‖�Aξθ + ∇rβθ

∣∣∣∣∣∣∣∣∣∣2

d�, (36)

where the scaling factor k = 0.2/median( ‖�Aξθ ‖) modi-fies the arctangent function such that the center of its linearregion corresponds to the median value of the range of inputvector magnitudes. The arctangent scaling acts as a selec-tive weighting function that emphasizes the quality of thefit in the low-control-force regions of the shape space, awayfrom the singularities. Details of this scaling function aregiven in Appendix B.

6.4.2. Optimal choice of reference position While thechoices of βx and βy do not affect the magnitude of �Aξθ , theydo contribute to the goal of minimizing the error betweenthe BVI and the displacement. Returning to Equation (24),we observe that the rotation matrix term is multiplied by thetranslational body velocity, [ξx ξy]T. It, therefore, followsthat minimizing this body velocity by choosing a coordi-nate system that minimizes �Aξx and �Aξy will further reducethe error beyond the reduction already accomplished by anoptimal choice of βθ . We have already seen an ideal exam-ple of this principle in the floating snake: using the centerof mass as the reference position induces zero-magnitudefields for �Aξx and �Aξy , and the translational BVI and dis-placement over a gait are both identically zero with no error.If another reference point, such as the center of the middlelink, had been used, �Aξx and �Aξy would not be zero, and theBVI would not match the displacement.

Finding the optimal choice of position coordinates hasa similar formulation to finding the optimal choice of ori-entation, in that we are looking for a change of coordinatesthat most nullifies the translational connection vector fields.The key difference from the orientation optimization liesin the B× term, which is now non-zero. Beyond adding athird term into the summations of vector fields that produce�Aξx

new and �Aξynew, B× also couples these two calculations, so

we must simultaneously solve for βx and βy to minimizeD for

D =¨�

‖�Aξx + ∇rβx − βy �Aξθ ‖2

+‖�Aξy + ∇rβy + βx �Aξθ ‖2 d�. (37)

As with the discrete Hodge–Helmholtz decomposition, wetake a finite-element approach to this minimization, thedetails of which appear in Appendix D. Additionally, whensingularities are present, we make use of the arctangentscaling approach in Equation (36) to emphasize the regionsof the shape space away from the singularities. Note that�Aξθ in this equation is for the original choice of coordi-nates, and that Brot does not appear in the optimization asit does not affect the magnitude of the connection vectorfields.

6.4.3. Summary of coordinate optimization In summary,the coordinate optimization process is as follows. First,we use the Hodge–Helmholtz decomposition to find the




Fig. 17. The effect of adding ∇rβθ for the mean-orientation coordinate choice to �Aξθ . In the circled regions, ∇rβθ ≈ −�Aξθ ( r), so‖�Aξθnew‖ ≈ 0 in these regions. Note that for visual clarity within each plot, the vectors in the different fields are not to scale.

Fig. 18. Optimal β (top) and ∇rβ (bottom) for the kinematic snake, using the arctangent metric in Equation (36).

∇rβθ field that minimizes �Aξθ . Second, we integrate thisfield to find βθ ( r), choosing the integration constant sothat βθ ( 0, 0) = 0; from βθ , we generate the Brot operatorthat converts the x and y components of the body velocityinto the new frame. Third, solving the coupled optimiza-tion problem in Equation (37) provides us with ∇rβx, ∇rβy,and B×. Finally, we insert these terms into Equation (33) togenerate the new, optimized local connection.

6.5. BVI vs. displacement in optimal frame

Applying the discrete Hodge–Helmholtz decompositionand the position optimization to the connection vector fieldsfor the kinematic snake produces the β and ∇rβ functionsshown in Figure 18. The first conclusion we can draw fromthese functions is that the mean-orientation coordinates areclose to optimal for orientation: the plots of βθ and ∇rβ




Fig. 19. Representative gait image-family overlaid on the connection vector fields and height functions of the kinematic snake in theoptimized coordinate system. Compared to the mean-orientation coordinate system, �Aξθ is nullified over a larger area, and Hζy showsmore variation. (Owing to singularities in the local connection, the magnitudes of the height functions and vector fields are scaled totheir arctangents.)

strongly resemble their mean-orientation counterparts inFigure 16. The chief difference between them is that theoptimization process warps βθ so that ∇rβθ more closelyapproximates −�Aξθ near the edges of the region under con-sideration. As show in Figure 19, this change in ∇rβθ leadsto the nullification of �Aξθ over a wider area than was thecase for the mean-orientation coordinate system. Figure 20shows the marked improvement of the BVI as an estimate ofdisplacement in the optimized coordinate system. The spanof the locus of displacements is reduced to the extent that itcan be considered as a point, and the BVI calls out the samelocation as this point.

It is worth noting that the degree to which the new coor-dinate system nullifies �Aξθ is inversely proportional to themagnitude of Hζθ , and that Hζθ itself does not change. Theformer effect is explained by considering that as Hζθ is thecurl of �Aξθ , it measures the non-conservativity of �Aξθ , sothat the smaller the value of Hζθ in a region, the more wecan nullify �Aξθ by the addition of a conservative field. Thelatter effect comes from the identity between the curl ofthe sum of two vector fields and the sum of their curls; theconservative field we added to �Aξθ during optimization haszero curl by definition, and so does not change the heightfunction.

The βx and βy functions found by the coordinate opti-mization move the center of the new coordinate framecloser to a ‘true center’ of the system. For example,as illustrated in Figure 18, βx has a saddle structurethat increases with the distance of α1 from 0 radiansand decreases as α2 moves away from 0 radians. Thisbehavior corresponds to the system’s center of constraint5

(roughly at the same location as the center of mass)being drawn forwards or backwards as it moves awayfrom a fully extended configuration. Similarly, βy increasesand decreases proportionally to both α1 and α2 as theouter links move away from the centerline of the middlelink.

6.6. Generality

Having demonstrated the accuracy of the BVI as an esti-mate of displacement for our example gait image-family,we now expand the scope of our analysis. To be an effectivetool for motion planning, the BVI must closely approxi-mate the displacement over a broad domain of gaits. Oneway to measure the size of the domain over which theapproximation holds is to consider the ‘largest’ gaits thatcan be executed while keeping the error within acceptable




Fig. 20. The BVI and displacements, corresponding to the image-family of gaits in Figure 19 for the kinematic snake robot with theoptimized measure of orientation from Figure 18, are represented,respectively, by the box above the x-axis and a very short arcsection at the same location, with the BVI and displacements asmeasured in the original and mean-orientation coordinate choicesshown for reference.

bounds. While it is naturally infeasible to find the BVIand displacement for all possible gaits, it is straightfor-ward to consider the set of all circular gaits. From this set,we can then find the maximum radius for a circular gaitimage-family centered at each point in the shape space forwhich the error percentage is less than a given value, that isfor which

maxφ ‖ζ − gφ‖minφ ‖gφ‖

< εmax, (38)

where gφ is the locus of net translations corresponding tothe image-family, max ‖ζ − gφ‖ is the maximum distancefrom the BVI to a point on the net translation locus, andmin ‖gφ‖ is the minimum magnitude of the net transla-tion in the locus. Figure 21 shows this maximum radiusfor an acceptable error of εmax = 0.1, both as a func-tion on the shape space and as a representative collectionof circular image-families. For reference, the gait used inthe previous examples is drawn along with the 10% errorgaits, showing it to be well within the range of allow-able radii for its location. Note that by calculating thismaximum radius, we are not suggesting that such calcu-lations should form a part of motion planning, but ratherare demonstrating the effects of the coordinate change onour example system. Also, the use of circular or ellipticalgaits here and elsewhere should not be taken as a restrictionon the applicability of the results; they appear here becausethey allow us to examine the underlying principles with-out adding in extra complicating factors in the form of gaitparameterizations.

(a)

(b)

Fig. 21. Maximum radius for a circular gait image-family forεmax = 0.1, (a) plotted as a function on the shape space, and(b) drawn as a representative collection of image-families. In (b),the dashed line denotes the α1 = α2 singularity, while the dottedbox marks the extent of the domain of the plot in (a). The thick-lined circle in the lower-right portion of the drawing is the gaitimage-family used in the main examples.

As expected, the regions of the shape space in whichthe BVI serves well as an approximation of the net dis-placement correspond to the regions in which �Aξθ hasbeen nullified by the change of coordinates. It is worthnoting that these regions are also inherently good regionsin which to search for translation gaits, both becausetheir distance from constraint singularities results in smallrequired control forces, and because the low magnitude ofHζθ in these regions produces a high density of gaits thatproduce pure net translation with zero net rotation, whichis important when designing gaits that can be repeatedto locomote the system in a straight line. The change ofcoordinates thus not only makes it possible to use the BVIto design translational gaits, but does so in the most usefulregions of the shape space.

Taking this observation one step further, we can roughlypartition the shape space into regions containing trans-lational and rotational gaits, as illustrated in Figure 22.The translational regions have the properties noted above,while the rotational regions have large magnitudes of Hζθ ,




Fig. 22. Partitioning of the shape space into regions that con-tain effective translational or rotational gaits. The translationalregions correspond to S-shaped configurations of the robot, whilethe rotational regions correspond to C-shapes.

allowing for gaits that efficiently turn shape motion intonet rotation. This partitioning bears strong similarity tothe findings by Melli et al. (2006) that an S-shape fortheir three-link swimmer was most effective for producingtranslation, while a C-shape was better for rotation.

7. Optimal coordinates for other systems

While our chief interest in developing the coordinate opti-mization approach has been to analyze non-traditional loco-motors such as the kinematic snake, applying coordinateoptimization to the two other systems discussed earlieryields some interesting insights.

7.1. Floating snake

As we noted previously, conservation of linear momentumdictates that the floating snake’s center of mass remains sta-tionary when the system starts at rest. Accordingly, we canconclude that the center of mass is an inherently optimalchoice of reference point according to Equation (37), andthus that optimizing the coordinate choice for the systemreduces to finding the optimal choice of orientation. Whileour initial motivation for optimizing coordinate choice,making the BVI an approximation of displacement, is lessrelevant for the floating snake (as any gait for the systemwill still of course generate zero net displacement and zerotranslational BVI, along with a change in orientation thatexactly matches the rotational BVI), working with an opti-mized choice of orientation still better captures the ‘truemotion’ of the system. In this case, we regard the ‘truemotion’ as the actual contribution of a shape change tothe net rotation produced by a gait containing that shapechange.

Fig. 23. The relative orientation βθ and ∇rβθ between the opti-mized orientation frame and the original center-link frame for thefloating snake, as a function of the shape α.

Similarly to the kinematic snake, the floating snake’soptimal orientation is a weighted average of the orienta-tions of the three links, with βθ and ∇rβθ as plotted inFigure 23. Figure 24 shows the example gait for the floatingsnake overlaid onto the original and optimized �Aξθ fields,along with the time history of the orientation of the systemusing both measures. Using the original definition of orien-tation, the system exhibits significant positive ‘windup’ fol-lowed by a ‘power stroke’ that brings it to its final negativeorientation. The new orientation, however, monotonicallyapproaches the final negative value. The difference betweenthe two plots highlights how the optimal orientation foundby minimizing Equation (35) separates out the gradientcomponent of �Aξθ : the ‘extra’ motion in the original coor-dinate choice corresponds to this (conservative) gradientcomponent, and thus by definition cancels with itself over agait. With the new choice of orientation, this self-cancelingmotion is stripped away, leaving only the actual contri-bution of a given change in shape to the net orientationchange.

7.2. Differential-drive car

The differential-drive car presents a second special case ofcoordinate optimization. For this system �Aξθ is a constant(and, thus, conservative) vector field, and can, therefore, beexactly nullified by the addition of ∇rβθ = −�Aξθ . In thecoordinates defined by the implied choice of

βθ = α1 − α2, (39)

θ , θ , and εζ are all identically zero, so there is no need tooptimize the choice of reference point. Figure 25 shows theoptimized connection vector fields and height functions forthe differential-drive car, along with a parallel-parking gaitthat produces the trajectory in Figure 26. The gait encirclesa net-zero region on the Hζx height function and a negativeregion on Hζy ; as ζx and ζy are, respectively, equal to the xand y displacements with no error, the resulting motion hasnet-lateral displacement.

The optimal coordinates for the differential-drive carreveal some interesting relationships between that system




(a) Example gait overlaid on the origi-nal �Aξθ for the floating snake.

(b) Example gait overlaid on the opti-mized �Aξθ for the floating snake.

(c) Time history of the original and optimizedorientations while executing the example gait.

Fig. 24. Optimizing the definition of θ for the floating snake strips out the gradient component from �Aξθ , leaving only the portion thatactively contributes to net rotation over a gait. For the example gait, this means that the optimized orientation changes monotonicallytowards its final value, without the large ‘windup’ and ‘power stroke’ seen in the original orientation.

Fig. 25. Connection vector fields and height functions for the differential-drive car in the optimized coordinate choice. For this choiceof coordinates, the �Aξθ field is identically zero, so ζx and ζy are, respectively, equal to the x and y displacements with no error. Theexample gait shown here encircles a net-zero region on Hζx and a negative region on Hζy , producing the net-lateral ‘parallel-parking’motion shown in Figure 26.

and two other systems that appear in the geometric mechan-ics literature: the south-pointing chariot (Santander 1992)and the rolling disk (Mukherjee and Anderson 1993a; Blochet al. 2003). The south-pointing chariot is a two-wheeledcart with a pointer that counter-rotates with respect tothe vehicle by an angle βp = −θ such that the pointermaintains a constant angle with respect to the global

coordinate frame, as illustrated in Figure 27. The pointeris implemented mechanically with a differential gear setthat takes the difference between the two wheels’ axleangles; for unit wheel spacing and radius, the pointerangle is

βp = α1 − α2. (40)




A comparison of Equation (39) with Equation (40) clearlyshows that βθ = βp, and, thus, that optimizing the coordi-nates for the differential-drive car produces a mathematicaldescription of the south-pointing chariot.

The rolling disk is an elementary non-holonomic sys-tem, often used in tutorial examples. Its kinematic modelis depicted in Figure 28, with position g =( x, y) ∈ R

2 and‘shape’ r =(αa,αb) ∈ S × S. The connection vector fieldsand height functions for the rolling disk are the same as forthe optimized differential-drive, but rotated clockwise by45 degrees; the reason for this becomes immediately clearwhen we consider that rolling the disk forward is equivalentto driving both wheels of the car together, while chang-ing the disk’s heading corresponds to driving the wheels inopposite directions to each other. This relationship betweenthe α parameters for the disk and car is illustrated inFigure 29.

While a deeper examination of the relationships amongthe differential-drive car, the south-pointing chariot, andthe rolling disk is beyond the scope of the current work,their equivalence under coordinate optimization suggeststhat they hold the seeds to additional insights into the coor-dinate optimization process. The differential-drive car inits classical coordinate system has zero-valued height func-tions and thus represents one end of the spectrum of height-function analysis, with no translation information availablefrom the functions, while the south-pointing chariot androlling disk represent the other extreme, with the heightfunctions providing an exact measure of the displacementover gaits. A key element of our future work will be anexamination of this spectrum, with an eye towards incorpo-rating it into a more fully differential geometric treatmentof the coordinate change.

8. Demonstration

We applied the gaits from the image-family in Figures 8and 19 to a physical instantiation of the kinematic snake,shown in Figure 30, and plotted the resulting displacementsin the original and optimized coordinates in Figure 31. Dur-ing the experiments, we observed some backlash in thejoints and slip in the constraints, to which we attribute thedifferences between the calculated and experimental lociin Figures 20 and 31. Even with this error, the BVI wasa considerably more effective estimate of the displacementwith the new coordinate choice than with the old choice.Under the old coordinate choice, the error in the the netdirection of travel between the BVI estimate and the actualdisplacement ranged from 0.72 to 0.25 radians. With thenew choice, the error in the estimate ranged from 0.08 to−0.05 radians, reducing the maximum error magnitude by89% and eliminating the minimum error.

9. Conclusions

In this paper, we have shown that the choice of coordi-nates used to represent a mobile system directly affects

Fig. 26. ‘Parallel-parking’ motion for the differential-drive carproduced by the gait in Figure 25.

Fig. 27. The south-pointing chariot’s pointer counter-rotates withrespect to the main body by an angle βp = −θ , such that itmaintains a constant angle with respect to the global coordinates.

αb

(x y)

αa

,

Fig. 28. Kinematic model for a rolling disk. The disk’s position isthe ( x, y) location of its contact point on the plane, and its ‘shape’is the roll angle αa and the heading angle αb.

Fig. 29. Relationship between α1 and α2 for the differential-drivecar and αa and αb for the rolling disk.




Fig. 30. Physical instantiation of the kinematic snake. Two opti-cal mice on the middle link provide odometry data, including anestimation of constraint slip.

Fig. 31. BVI and displacements for the physical kinematic snakewhile executing the example image-family of gaits. The boxesand diamonds show the expected displacements for the robot, cal-culated from the actual trajectories followed by the joints, whilethe crosses and triangles show the actual measured displacement,which differs from the expected displacement because of back-lash and constraint slip. As in Figure 20, the BVI is a muchbetter estimate of the displacement with the new, optimized coor-dinate choice than with the original choice, and the variation in thedisplacement for different starting shapes is much smaller.

the analytical tools that can be applied to the system. Bychoosing a set of coordinates that minimizes the rotationof the body frame in response to changes in system shape,we have advanced a technique for approximating the netdisplacement resulting from a gait by means of Stokes’ the-orem, rather than by direct integration of the equations ofmotion. In turn, this approximation provides both an intu-itive visualization of the system’s kinematics and a means ofdesigning gaits by encircling sign-definite regions of well-defined height functions. The technique constitutes a sig-nificant improvement over previous efforts in this area, asit is applicable to efficient macroscopic translational gaits,while previous work was limited to macroscopic rotationalgaits or inefficient small-amplitude translational gaits.

By considering the effect of coordinate choice, we haveadded a new dimension to analysis of the motion-planningproblem. Beyond the specific advances presented here, thiswidened perspective opens the door for new analysis out-side the scope of this paper. A key example of such futurework is the unification of our observations with the geomet-ric mechanics theory on which we have built. This founda-tional work has typically striven to be independent of thecoordinates used, so it is likely that new insights can beattained by lifting our coordinate-based approach from thesystems examined here to a more abstract level.

Developing a more formal geometric formulation of thecoordinate change will also provide the necessary structureto expand the classes of systems considered in our anal-ysis. Thus far, our examples have been limited to planarsystems with two shape inputs and governed by conserva-tion of momentum or a single set of non-holonomic con-straints. Concepts such as curl are inherently dimensionallylimited, but differential geometry encompasses their gen-eralization to higher dimensions, allowing us a route toinclude more shape parameters. Similarly, the more for-mal approach will inform the construction of objectivefunctions for optimizing coordinate choice for fully-spatialmotion in SE( 3). While extending our analysis to higherdimensions, we will also consider systems governed byother constraints that give rise to a fiber bundle struc-ture, such as swimming systems (Melli et al. 2006; Avronand Raz 2008), the mixed kinodynamic systems of Sham-mas et al. (2007b), and potentially to hybrid systems thatcycle through multiple sets of constraints over the course ofa gait.

A second area that the present work leaves open to fur-ther exploration is finding more refined objective functionsfor optimizing the coordinate choice. Weighting the Hodge–Helmholtz projection to emphasize the quality of the fitbetween ∇rβθ and −�Aξθ in targeted regions, as in Equa-tion (36), improves the accuracy of the height functions inregions of the shape space where useful gaits will exist.While the scaled arctangent scaling we use here servesthis purpose adequately by emphasizing areas of the shapespace away from singularities, there is still much room toinvestigate alternative weighting schemes. Such approachesmight, for example, de-emphasize regions of the shapespace that correspond to self-collision of the robot andwhich are thus unusable for gait design.

Notes

1. In the parlance of differential geometry, this separationassigns a (trivial, principal) fiber bundle structure Q = G ×Mto the system’s configuration space, with M the base space andG the fiber space.

2. A more formal differential geometric treatment of this sub-ject matter would, respectively, replace ‘vector field’ and ‘curl’with ‘one-form’ and ‘exterior derivative,’ but here (with-out loss of generality) we choose our language for broaderaccessibility.




3. In strict differential geometric language, each row Ai of A isa one-form over M acting on r, and �Aξi is the negative dual ofthat one-form.

4. In the geometric mechanics literature, the displacement over agait is often referred to as the ‘geometric phase.’

5. The intersection point of the lines perpendicular to thewheelsets. When there are multiple intersection points, weregard the center of constraint as their mean position.

Acknowledgements

The authors would like to thank Scott Kelly and MatthewTesch for many insightful discussions.

Funding

This research received no specific grant from any fundingagency in the public, commercial, or not-for-profit sectors.

Conflict of interest statement

The authors declare that they have no conflicts of interest.

References

Arfken GB (2005) Mathematical Methods for Physicists, 6thedition. Elsevier.

Avron J and Raz O (2008) A geometric theory of swimming: Pur-cell’s swimmer and its symmetrized cousin. New Journal ofPhysics 9: 063016.

Becker EB, Carey GF and Oden JT (1981) Finite Elements:An Introduction, volume 1. Texas Institute for ComputationalMathematics.

Bloch AM with Baillieul J, Crouch PE and Marsden JE (2003)Nonholonomic Mechanics and Control. Berlin: Springer.

Boothby WM (1986) An Introduction to Differentiable Manifoldsand Riemannian Geometry. New York: Academic Press.

Bullo F and Lynch KM (2001) Kinematic controllability fordecoupled trajectory planning in underactuated mechanicalsystems. IEEE Transactions on Robotics and Automation 17:402–412.

Guo Q, Mandal MK and Li MY (2005) Efficient Hodge–Helmholtz decomposition of motion fields. Pattern Recogni-tion Letters 26: 493–501.

Hatton RL and Choset H (2008) Connection vector fieldsfor underactuated systems. In: Proceedings of the IEEEBioRobotics Conference, Scottsdale, AZ, USA, October.

Hatton RL and Choset H (2009) Approximating displacementwith the body velocity integral. In: Proceedings of Robotics:Science and Systems, Seattle, WA, USA, June.

Hatton RL and Choset H (2010) Optimizing coordinate choicefor locomoting systems. In: Proceedings of the IEEE Inter-national Conference on Robotics and Automation, Anchorage,AK, USA, May.

Ishikawa M (2009) Iterative feedback control of snake-like robotbased on principal fibre bundle modelling. International Jour-nal of Advanced Mechatronic Systems 1: 175–182.

Kelly SD and Murray RM (1995) Geometric phases and roboticlocomotion. Journal of Robotic Systems 12: 417–431.

McIsaac KA and Ostrowski JP (2003) Motion planning foranguilliform locomotion. IEEE Transactions on Robotics andAutomation 19: 637–652.

Melli JB, Rowley CW and Rufat DS (2006) Motion planning foran articulated body in a perfect planar fluid. SIAM Journal ofApplied Dynamical Systems 5: 650–669.

Morgansen KA, Triplett BI and Klein DJ (2007) Geometric meth-ods for modeling and control of free-swimming fin-actuatedunderwater vehicles. IEEE Transactions on Robotics 23:1184–1199.

Mukherjee R and Anderson DP (1993a) Nonholonomic MotionPlanning Using Stokes’ Theorem. In: IEEE International Con-ference on Robotics and Automation, pp. 802–809.

Mukherjee R and Anderson DP (1993b) A surface integralapproach to the motion planning of nonholonomic systems. In:American Control Conference, 1993, pp. 1816–1823.

Murray RM and Sastry SS (1993) Nonholonomic motion plan-ning: steering using sinusoids. IEEE Transactions on Auto-matic Control 38: 700–716.

Ostrowski J (1998) Reduced equations for nonholonomicmechanical systems with dissipative forces. Reports on Mathe-matical Physics 42: 185–209.

Ostrowski J and Burdick J (1998) The mechanics and controlof undulatory locomotion. International Journal of RoboticsResearch 17: 683–701.

Ostrowski JP, Desai JP and Kumar V (2000) Optimal gait selec-tion for nonholonomic locomotion systems. The InternationalJournal of Robotics Research 19: 225–237.

Santander M (1992) The Chinese south-seeking chariot: A sim-ple mechanical device for visualizing curvature and paralleltransport. American Journal of Physics 60: 782–787.

Sfakiotakis M and Tsakiris DP (2007) Biomimetic centering forundulatory robots. International Journal of Robotics Research26: 1267–1282.

Shammas E, Schmidt K and Choset H (2005) Natural gait gen-eration techniques for multi-bodied isolated mechanical sys-tems. In: IEEE International Conference on Robotics andAutomation, April, pp. 3664–3669.

Shammas EA, Choset H and Rizzi AA (2007a) Geometric motionplanning analysis for two classes of underactuated mechanicalsystems. The International Journal of Robotics Research 26:1043–1073.

Shammas EA, Choset H and Rizzi AA (2007b) Towards a uni-fied approach to motion planning for dynamic underactu-ated mechanical systems with non-holonomic constraints. TheInternational Journal of Robotics Research 26: 1075–1124.

Shapere A and Wilczek F (1989) Geometry of self-propulsionat low Reynolds number. Journal of Fluid Mechanics 198:557–585.

Walsh G and Sastry S (1995) On reorienting linked rigid bod-ies using internal motions. IEEE Transactions on Robotics andAutomation 11: 139–146.

Appendix A: Magnitude of displacement forgaits in an image-family

In Section 6.1, we commented that the locus of net displace-ments resulting from an image-family of gaits that producesno net rotation will be an arc of constant radius, centered onthe origin. To see that this must be the case, consider the two




Fig. 32. Two shape changes ψa and ψb that form gaits whenconcatenated in either order.

shape changes

ψa : [0, Ta] → M (41)

ψb : [0, Tb] → M , (42)

arranged such thatψa( Ta) = ψb( 0) andψb( Tb) = ψa( 0), asillustrated in Figure 32. By concatenating these two shapechanges, we can generate two distinct gaits from a commonimage-family φ,

φ1 = ψaψb (43)

φ2 = ψbψa. (44)

The net displacements over these two gaits are found bycomposing the net displacements over each section,

gφ1 ( T) = gψa ( Ta) gψb ( Tb) (45)

gφ2 ( T) = gψb ( Tb) gψa ( Ta) , (46)

where T = Ta + Tb. Evaluating Equation (45) as gφ1 ( T) =( x1, y1, θ1) gives the net displacement over φ1 as

gφ1 ( T) = ( xa + xb cos θa − yb sin θa, ya + xb sin θa

+yb cos θa, θa + θb) . (47)

The squared magnitude of the xy displacement over φ1 isthus

‖gφ1 ( T) ‖2xy = x2

a + y2a + x2

b + y2b + cos θa( xaxb + yayb)

+ sin θa( xayb − xbya) ; (48)

by a similar calculation, the squared magnitude of the xydisplacement over φ2 is

‖gφ2 ( T) ‖2xy = x2

a + y2a + x2

b + y2b + cos θb( xaxb + yayb)

− sin θb( xayb − xbya) , (49)

and the difference between these two values is

‖gφ1 ( T) ‖2xy − ‖gφ2 ( T) ‖2

xy =( cos θa − cos θb)

( xaxb + yayb) +( sin θa + sin θb) ( xayb − xbya) . (50)

Fig. 33. Scaled arctangent function. For small kx, arctan ( kx)/k ≈x, but for larger values it is limited to π/2k. In this paper, wechoose k to place the median value of kx at 0.2, in the center ofthe distortion-free domain.

For image-families that produce no net rotation, that is,for which θa +θb = 0, the two trigonometric terms in Equa-tion (50) go to zero, and the magnitude of displacement isthe same for φ1 and φ2. As the start and end points of ψa

and ψb can be arbitrarily chosen along the curve defined byφ, the results of Equation (50) apply to all gaits in φ. Thisapproach can be generalized to SO( n) to show that in animage-family that produces no net rotations, the magnitudeof displacement for all gaits must be the same.

Appendix B: Scaled arctangent

When plotting and optimizing the connection vector fieldsfor the kinematic snake, we modify their magnitudes by ascaled arctangent function

y = arctan ( kx)

k(51)

to prevent the large magnitudes of the fields near the sin-gularities from saturating the plot or optimization process.Figure 33 illustrates the behavior of this function. For kx <0.5, y ≈ x, but for larger values of kx, y increases muchmore slowly, asymptotically approaching an upper limit ofπ/2k. The scaling parameter k can be chosen based on thevalues of x to minimize the distortion of ‘reasonable’ val-ues, while filtering out overly large outliers. In this paper,we use k = 0.2/median( x) to place median( kx) at 0.2, inthe center of the distortion-free domain.

Appendix C: Discrete Hodge–Helmholtzdecomposition

In Section 6.4.1, we use the discrete Hodge–Helmholtzdecomposition to find the optimal choice of orientation,following the algorithm in Guo et al. (2005). The central




operation in this decomposition is finding a potential func-tion E that minimizes the cost function

D( E) =ˆ�

‖∇E − V‖2 d� (52)

for an input vector field V . Here, we present an expandedderivation of the finite-element approach to solving thisminimization. This derivation contains some details ofthe formulation that were not presented in Guo et al.(2005) and serves as an introduction for the minimiza-tion in Appendix D, which has a similar form, but severaladditional terms.

For generality, the derivation here uses generic expres-sions for the potential function and input vector fields; whenwe apply the results to Section 6.4.1, we take E = −βθ andV = �Aξθ .

C1 Finite-element formulation

There is no general analytical solution for minimizingEquation (52), but a finite-element approach readily pro-vides a numerical solution. The first stage of this optimiza-tion is to take the output potential function E as beingthe weighted sum of a set of (as yet unspecified) basisfunctions φ,

E =∑

i

φiei. (53)

An optimal set of weights e that minimizes Equation (52)must result in D being stationary with respect to eachweight,

∀ i,∂D

∂ei= 0. (54)

The partial derivative of D with respect to the weightsis found by first expanding the right-hand side ofEquation (52),

D( E) =ˆ�

( ∇E − V ) · ( ∇E − V ) d� (55)

=ˆ�

( ∇E · ∇E) +( V · V ) −2( ∇E · V ) d�, (56)

and then differentiating under the integration sign,

∂D

∂ei=ˆ�

∂( ∇E · ∇E)

∂ei︸︷︷︸A

+ ∂( V · V )

∂ei︸︷︷︸0

− 2∂( ∇E · V )

∂ei︸︷︷︸B

d�.

(57)The second term of the summation is clearly zero, as theinput vector field does not depend on the weights, and thefirst and third terms evaluate to

A :∂( ∇E · ∇E)

∂ei= 2

∂∇E

∂ei· ∇E = 2∇φi · ∇E (58)

B : 2∂( ∇E · V )

∂ei= 2∇φi · V , (59)

allowing the consolidation of Equation (57) into

∂D

∂ei= 2ˆ�

∇φi · ( ∇E − V ) d�. (60)

Applying the necessary condition from Equations (54) to(60) produces an alternate condition on the weights e tominimize D: They should generate e such that

ˆ�

∇φi · ∇E d� =ˆ�

∇φi · V d�. (61)

It may interest a reader with knowledge of other finite-element approaches to note the resemblance between Equa-tion (61) and an alternate formulation of the problem, inwhich we seek to approximate V with an element of the setof gradient fields, using ∇φ as a test function.

C2 Piecewise-linear solution

To solve Equation (61), we need to specify the basis func-tions. While many classes of basis functions are available,here we use piecewise-linear bases on a triangular mesh.One such function is depicted in Figure 34; it has a valueof 1 at its central node and zero at all other nodes, withlinearly interpolated values in the interiors of the triangles.This is a canonical choice of basis, and as noted in Guo et al.(2005), several aspects make it especially convenient. First,the property of φi being 1 at the ith node and zero every-where else means that E at node i is equal to ei. Second, thisproperty also means that for each i, we need only evaluateEquation (61) over the six (or fewer) triangles containingnode i, making the minimization condition

∑k

ˆTk

∇φi · ∇E dTk =∑

k

ˆTk

∇φi · V dTk , (62)

where Tk is the kth triangle containing the ith node, as num-bered in Figure 34. From the quadrature rules for integrat-ing over linear triangular regions (Becker et al. 1981), theintegrals in Equation (62) are equal to the areas of the trian-gles multiplied by the integrands evaluated at the triangles’geometric centers,∑

k

∇φik · ∇Ek Ak =∑

k

∇φik · Vk Ak . (63)

For odd values of k, the terms in the integrands evaluate to

∇φik = [φid − φic,φia − φic]T (64)

∇Ek = [Ed − Ec, Ea − Ec]T (65)

Vk =( Va + Vc + Vd) /3, (66)

and for even values of k,

∇φik = [φib − φia,φib − φid]T (67)

∇Ek = [Eb − Ea, Eb − Ed]T (68)

Vk =( Va + Vb + Vd) /3, (69)




Fig. 34. Triangular mesh with piecewise-linear basis function.The basis function has a value of 1 at its central node and 0 atall other nodes, with linearly interpolated values in the interiors ofthe triangles.

Fig. 35. Geometry of odd- and even-numbered triangles in thegrid. The lettering a through d denotes a local coordinate systemfor the nodes in each triangle.

where the nodes are lettered as in Figure 35.As E and V each appear linearly in ∇Ek and Vk , Equation

(63) can be reformed into a linear equation

S1E = S2V , (70)

where the ith rows of S1 and S2 encode the summations overthe triangles containing the ith node. These two matricesdepend only on the grid spacing (and the definition of thebasis functions), and so can be precalculated as S = S−1

1 S2,such that

E = SV (71)

produces the potential function whose gradient best approx-imates the input vector field. In practice, one row and col-umn of S1 and one row of S2 must be eliminated whileprecalculating S; as ∇( E + C) = ∇E for any constant C,there is a whole family of potential functions that are solu-tions to Equation (71), and eliminating the row and columnlocks the problem down to a single solution.

Appendix D: Reference-point optimization

We take a similar approach to optimizing the reference-point position as we did for the orientation. The main differ-ence is that rather than a single input vector field and outputpotential function, there are now three input vector fields,

U , V , W , and two output potential functions, E, F, whichcorrespond, respectively, to �Aξx , �Aξy , �Aξθ , and βx, βy, and acost function

D( E, F) =ˆ�

‖U +∇E−FW‖2 +‖V +∇F +EW‖2. (72)

As in the Hodge–Helmholtz decomposition, we minimizeD by first expanding the squared vector norms,

D( E, F) =ˆ�

( U · U) +( ∇E · ∇E) +( F2W · W )

+ 2( U · ∇E) −2( FW · ∇E)

+ ( V · V ) +( ∇F · ∇F) +( E2W · W )

+ 2( V · ∇F) +2( EW · ∇F) d�, (73)

then differentiating by the basis coefficients e and f ,

∂D

∂ei= 2ˆ�

∇φi · ( ∇E − FW + U)

+ φiW ·( EW + V + ∇F) d� (74)

and

∂D

∂fi= 2ˆ�

∇φi · ( ∇F − EW + V )

+ φiW ·( FW − U − ∇F) d�. (75)

Applying the minimization conditions of ∂D/∂ei = 0 and∂D/∂fi = 0 produces the equated integrals

ˆ�

∇φi · ( ∇E − FW ) + φiW · ( EW + ∇F) d�

=ˆ�

∇φi · ( −U) −φi( V · W ) d� (76)

andˆ�

∇φi · ( ∇F + EW ) + φiW · ( FW − ∇E) d�

=ˆ�

∇φi · ( −V ) + φi( V · W ) d�, (77)

which can be operated on by the same piecewise-linearapproach as in Section C2, resulting in a matrix equation

[EF

]= S( W )

[UV

]. (78)

Unlike Equation (61), Equations (76) and (77) contain non-gradient terms, and so admit only a single solution. Conse-quently, the S matrix here is full rank and does not need tohave a row and column removed in order to find a solution.



the international journal of robotics...

Documents