Robotics: Science and Systems 2019 Freiburg im Breisgau, June 22-26, 2019 A Hierarchical Geometric Framework to Design Locomotive for Highly Articulated Robots

Baxi Chong∗, Yasemin Ozkan Aydin∗, Guillaume Sartoretti†, Jennifer M Rieser∗, Chaohui Gong†, Haosen Xing†, Howie Choset † and Daniel I Goldman ∗ ∗Georgia Institute of Technology, †Carnegie Mellon Institution

Abstract—Motion planning for mobile robots with many degrees-of-freedom (DoF) is challenging due to their high- dimensional configuration spaces. To manage this curse of di- mensionality, this paper proposes a new hierarchical framework that decomposes the system into sub-systems (based on shared capabilities of DoFs), for which we can design and coordinate motions. Instead of constructing a high-dimensional configuration space, we establish a hierarchy of two-dimensional spaces on which we can visually design gaits using geometric mechanics tools. We then coordinate motions among the two-dimensional spaces in a pairwise fashion to obtain desired robot locomotion. Further geometric analysis of the two-dimensional spaces allows us to visualize the contribution of each sub-system to the locomotion, as well as the contribution of the coordination among the sub-systems. We demonstrate our approach by designing gaits for quadrupedal robots with different morphologies, and Fig. 1. Three link swimmer with four cubic legs CAD diagram of the experimentally validate our findings on a robot with a long robot (a) top and (b) side view. (c) Physical model. The upper back and lower back are actuated by two AX12 servos (labeled as 1 and 2) and the legs are actuated back and intermediate-sized legs. actuated by XL320 servos. The legs have rectangular shape (24x24x45 mm) and the body is always in contact with ground via blocks (55x12x45 mm) I.INTRODUCTION attached to the body segments. High degree-of-freedom (HDoF) mobile robots offer ad- vantages over conventional wheeled vehicles in difficult-to- convergence is attained. Once this process has converged, negotiate terrains [3, 17, 21, 25]. However, the benefit of many we further show how GM tools allow us to visualize the degrees of freedom also poses a challenge: how to coordinate contribution of each sub-system to locomotion, as well as all of the DoFs to produce purposeful motion. One approach that of the coordination of every pair of sub-systems. In this to address this so-called curse of dimensionality is to couple way, the proposed hierarchical framework provides a motion groups of the DoFs into one or two DOFs thereby producing a planning tool to visually and intuitively design gaits for HDoF lower dimensional configuration space [5, 10, 15, 16, 24, 2, 6]. systems. We term these lower dimensional systems as sub-systems. Sub- We apply our framework to study the motion of systems can be formed based on observations of biological a quadrupedal robot with a long articulated body and systems, empirical experiments, or intuition. The approach intermediate-sized legs (Fig. 1), a morphology incorporating described in this paper first plans for motions within every features from both lizards and . Here, we show how sub-system, and then for coordination among the sub-systems. our framework provides insight into the role of leg contacts Specifically, we propose a hierarchical framework to decom- and body undulation in quadruped locomotion. Previous work pose the motion planning problem in the full configuration has shown that the undulation of an elongated body improves space of a robot, into i) a series of simpler motion planning the stride length of animals and robots [11, 27, 8]. A more problems in the low-dimensional spaces associated with the comprehensive review on quadrupedal design can be different sub-systems, and ii) the coordination of these sub- found in [27]. There are two common modes of body un- systems. dulation: standing waves (lizard-like) [11, 27], and traveling Our framework relies on a motion planning approach, waves propagated from head to tail (-like) [8]. In the first namely geometric mechanics (GM), which offers powerful mode, the body curvature is maximized to increase the self- tools to design, visualize, and analyze a wide range of lo- propulsion generated by leg-ground interaction. In the second comotor behaviors [4, 7, 14]. We rely on GM tools to visually mode, the body undulation can generate self-propulsion even design the motion of each subsystem with respect to our without legs. Interestingly, we find that the optimized whole- locomotive goals (here, to maximize stride displacement), and body motion (i.e., motion in the full configuration space) for to design the coordination of pairs of sub-systems. By rely- our robot is a hybrid combination of the lizard- and snake-like ing on a coordinate-descent-like approach, we then optimize modes, such that the self-propulsion from leg-ground contact locomotion by iterating over every pair of sub-systems until and that from body-ground contact are properly coordinated.

1 maps the changes in internal shape variables (joint angles) to changes in group variables (position and orientation) of the robot. The interaction between a robot and an environment is often difficult to model, which is typically the case when the system locomotes on a granular medium. In such a case, calculating the local connection matrix A can be quite challenging. Instead, we numerically derive A using resistive force theory (RFT) [13, 22, 26] to model the contact. Prior work [8] has shown that numerically derived local connections using Fig. 2. Example height functions and gaits in different shape spaces granular RFT can effectively predict movements in granular (a) The height function in a Euclidean shape space corresponds to motions of an 8-link snake robot slithering in the forward direction on the surface media. In this paper, we model the robot-ground contact by of 6mm plastic particles. The purple circle represents the optimal gait in the poppy seed RFT equations [27], from which we numerically corresponding shape space. (b) The height function in a torus shape space derive the local connections. corresponds to the forward motion of a robot with a long actuated body and intermediate-sized legs on the surface of ∼ 1 mm poppy seeds. The 2) Connection vector fields and height functions: Each row solid purple curve represents a sample gait in the corresponding shape space. of local connection matrix A corresponds to a component Orange lines represent the assistive lines to form closed loops with the gait path in the unfolded torus shape space. The area in the solid purple shadow direction of the body velocity and therefore gives rise to a represents the area where the gait path and the assistive lines form a clockwise connection vector field. The body velocities in the forward, loop; the area in the dashed purple shadow represents the area where the gait lateral and rotational directions are respectively computed as path and the assistive lines form a counterclockwise loop. Red, white and black indicate positive, zero and negative values in height function respectively. the dot product of connection vector fields and the shape velocity r˙. A shape velocity r˙ along the direction of the vector field yields the largest possible body velocity, while a shape We validate our theoretical predictions through a series of velocity r˙ orthogonal to the field produces zero body velocity. experiments where a 10-DoF robot locomotes on granular A gait can be represented as a closed curve in the corre- material. sponding shape space. The displacement resulting from a gait, The paper is structured as follows: Section II introduces ∂χ, can be approximated by   geometric mechanics and presents our framework to design ∆x Z whole-body motion; Section III applies our methods to study   the motion design of a robot with a long articulated body and ∆y = A(r)dr. (2)   ∂χ four legs; Section IV discusses the significance and concludes ∆θ our work. According to Stokes’ Theorem, the line integral along a II.METHOD closed curve ∂χ is equal to the surface integral of the curl of A. Review of geometric mechanics A(r) over the area enclosed by ∂χ: Z ZZ In this section, we provide a concise overview of the A(r)dr = ∇ × A(r)dr1dr2, (3) geometric tools needed for this paper. For a more detailed ∂χ χ and comprehensive review, we refer readers to [4, 7, 14]. where χ denotes the area enclosed by ∂χ. The curl of the The gait design tools of geometric mechanics separate the connection vector field, ∇ × A(r), is referred to as the height configuration space of a system into two spaces: the position function (Fig. 2a). The three rows of the vector field A(r) can space and the shape space. The position space denotes the thus produce three height functions in the forward, lateral and location of a system relative to a world frame, while the shape rotational direction, respectively. space denotes the internal configuration (internal shape) of the system. Geometric mechanics seeks to establish a functional With the above derivation, we simplify the gait design prob- relationship between velocities in these spaces; this functional lem to drawing a closed path in Euclidean shape space. The relationship is often called a connection, and it shares many displacements can be approximated by the volume enclosed by properties with the Jacobian of a robotic manipulator. the gait path. For example, in Fig. 2a, we show an example 1) Kinematic reconstruction equation: Principally, in kine- height function in Euclidean shape space corresponding to matic systems where there is no drift in the system, the the motion in the forward direction of an 8-link snake robot equations of motion [14] reduce to slithering on the surface of 6mm plastic particles. The circle (solid purple circle in Fig. 2a) enclosing the most volume ξ = A(r)r ˙, (1) represents the gait with the largest forward stride displacement. T where ξ = [ξx ξy ξθ] ∈ g denotes the body velocity in Although the review of geometric mechanics sections are forward, lateral, and rotational direction; r denotes the internal limited to two dimensions, there has been recent work on shape variables (joint angles); A(r) is the local connection applying geometric mechanics to high-dimensional robot gait matrix that relates shape velocity r˙ to body velocity ξ. design [5, 20]. Ramasamy and Hatton [20] extended geometric Eq.(1) is also called the kinematic reconstruction equation and mechanics to high dimensional shape space and proposed a method to visualized the kinematics. Gong et al. [5] approxi- the displacement solely resulting from motions in φ1, while C mated the kinematics of complex locomotors using only two keeping φ2 constant at the initial position, i.e., Pa ∩ Pb . shape variables with the proper shape basis functions. The first two terms (right hand side) in Eq.(4) are constant 3) Torus shape space: Often, a shape space can have some and independent from any gait path we choose. The physical cyclic structure. With both shape variables being cyclic, the meaning of the last term (right hand side) in Eq.(4) is the shape space has a torus shape, (T 2) [12]. In Fig. 2b, we additional displacement resulting from the contribution from show an example height function in a torus shape space coordination of φ1 and φ2, i.e., Pa ∩ Pb. corresponding to the forward motion of a quadruped robot Finally, we refer readers to [7] for a detailed derivation and with the standing wave body undulation on the surface of ∼ 1 proof of motion planning in torus shape spaces. T 2 mm poppy seeds. The shape variable, Φ = [φ1, φ2] ∈ T , has a cyclic structure, where one axis represents the phase of B. Whole-body motion the leg movement, and the other axis represents the phase of 1) Sub-system motion phase variable: In this section, we the body undulation. While the gait path (solid purple curve construct a high-dimensional torus shape space, on which Fig. 2b) is a closed curve in the torus shape space, there is no we can design the whole-body coordination. Consider an obvious area enclosed by the gait path. M-dimensional motion system and its shape variable r = T M To form an enclosed area, we introduce the notion of two [ri, r2, ..., rM ] ∈ R . Assume there are N sub-systems. assistive lines in the Euclidean parameterization of the torus We term the motion of a sub-system sub-system motion. Each shape space. The assistive lines are defined as: ∂χ1 : φ1 = 0, sub-system motion will form a closed loop gait path in the M (the solid orange line l1 +l2 in Fig. 2b), and ∂χ2 : φ2 = 0 (the shape space R . The closed loop gait path in sub-system solid orange line l3 + l4 in Fig. 2b). Note that the dashed line motion i ∈ {1, 2, ..., N} can be described by a function 0 1 M 1 li is equivalent to the corresponding solid line li, according to fi : T → R such that r = fi(φi), where φi ∈ T is the cyclic property of the torus shape space. In Fig. 2b, the the gait phase variable of sub-system motion i. The whole- gait path (the solid purple curve) together with two assistive body motion can be formulated as the superposition of the 0 0 lines (l1, l2, l3, l4) encloses two areas in the torus shape sub-system motions gait paths: i=N space (solid purple shadow area and dashed purple shadow X area). The net displacement can be approximated by the line r = fi(φi). (5) integral along two assistive lines plus the surface integral of i=1 the enclosed area: We define the sub-system motion phase variable as Φ = Z Z Z [φ , φ , ..., φ ]T ∈ T N . We can relate the Φ˙ to the shape A(r)dΦ = A(Φ)dΦ + A(Φ)dΦ 1 2 N ∂χ ∂χ1 ∂χ2 velocity r˙ by the Jacobian matrix [23] Jr,Φ: ZZ h i + ∇ × A(Φ)dφ dφ , (4) df1(φ1) df2(φ2) dfN (φN ) 1 2 r˙ = ... Φ˙ = Jr,ΦΦ˙ , (6) χ−χ1−χ2 dφ1 dφ2 dφN where χ − χ1 − χ2 denotes the area enclosed by the assitive With Eq.(6) in hand, we can rewrite Eq.(1): i=N lines and gait path. Note that in Fig. 2b, the gait path and X ξ = A(r)r ˙ = A( f (φ ))r ˙ the assistive lines form a clockwise loop in the lower right i i i=1 corner (solid purple shadow area) and a counterclockwise loop i=N X 0 in the upper left corner (dashed purple shadow area). Taking = A( fi(φi))JrΦΦ˙ = A (Φ)Φ˙ (7) the handedness into account, the surface integral (third term i=1 on the right hand side) in Eq.(4) should be computed as the surface integral of the enclosed area in the clockwise loop where A0(Φ) is the new local connection matrix mapping the (solid purple shadow area in Fig. 2b) minus the surface integral differential of the sub-system motion phase variable, Φ˙ , to the of the enclosed area in the counterclockwise loop (dashed body velocity ξ. purple shadow area in Fig. 2b). A properly designed gait in the sub-system motion phase To interpret the physical meaning of each term (right hand variable space (N-dimensional torus shape space) will then side) in Eq.(4), we introduce the notation of two sets Pa lead to coordinated whole-body motion. That is, we simplify and Pb, where Pa and Pb denote the contribution from the the whole-body motion gait design problem in the original M- horizontal and vertical coordinate axes in the torus shape dimensional Euclidean space RM to the N-dimensional torus N space. In our example in Fig. 2b, Pa represents the contribution sub-system motion phase variable space T . from leg movement and Pb represents the contribution from 2) Gait design: In this section, we propose a method to standing wave body undulation. design a gait path in an N-dimensional torus shape space. The physical meaning of the first term (right hand side) The gait path in the N-dimensional sub-system motion phase in Eq.(4) is the displacement solely resulting from motions variable space can be described by a gait function H : T 1 → N T 1 in φ2, while keeping φ1 constant at the initial position, i.e., T , such that: Φ = [h1(φ0), h2(φ0), ..., hN (φ0)] . φ0 ∈ T C C Pa ∩Pb, where () denotes the complement set. The physical is the global gait phase of the motion in sub-system motion meaning of the second term (right hand side) in Eq.(4) is phase variable space. We assume that all the functions hi are homeomorphism local connection matrix. The superscript [i] denotes that we −1 mapping, such that h ◦h is the identity mapping. Under this separated hi from H in deriving the local connection matrix. assumption, all sub-system motions will operate at the same Following the method discussed in Section II-A, we con- [i] frequency (more formally, the winding number of the gait path struct the height function (given by ∇ × AH∗ ), which help must be [1, 1, ..., 1] in N-dimensional torus space). us to design the mapping hi. While there are N functions in H, there are only N − 1 3) Visualization: In this subsection, we develop the tools to independent functions in H (dimension of Φ ∈ T N space visualize the contribution from a specific sub-system motion 1 minus the dimension of φ0 ∈ T space), allowing one and the contribution from the coordination of two sub-system constraint equation on H. For simplicity, we fix h1(x) := x, motions. We define Pi as the set denoting the contribution i.e., φ1 := φ0. Note that our method is permutation invariant. from a sub-system motion i. The contribution from coordina- For simplicity, in this work we assume that all the functions tion between sub-system motion i and j can be expressed as in H are linear mapping, such that hi(x) = x + φi, where Pi ∩ Pj. φi is the phase offset of the sub-system motion i. In these In Section II-B2, we construct the local connection matrix ˙ ˙ cases, only the phase offsets between sub-system motions mapping the phase differentials φ0 and φi to the body velocity are subject to optimization in the whole-body coordination ξ. Following the approach introduced in Section II-A, we can gait design. For more complex systems where linear mapping approximate the net displacement as: cannot produce the effective whole-body coordination, the Z " ˙ # Z " ˙ # mapping function prescription can be extended to sigmoidal, [i] T φ0 [i] T φ0 A ([φ φ ] ) = A ∗ ([φ φ ] ) quadrutic, cubic or other homeomorphism mappings. H∗ 0 i H 0 i ∂χ φ˙i ∂χ :φ =0 φ˙i We start the optimization with a naive initial guess on the 1 0 Z " ˙ # gait function H that all functions in H are identity mapping, [i] T φ0 + A ∗ ([φ φ ] ) H 0 i ˙ i.e., hi(x) = x. We then decompose the N-dimensional torus ∂χ2:φi=0 φi ZZ shape space into N − 1 two-dimensional torus shape spaces [i] T + ∇ × A ∗ ([φ φ ] )dφ dφ (10) [φ , φ ]T , where i ∈ {2, 3, ... N}. Next, we optimize one H 0 i 0 i 0 i χ−χ1−χ2 independent function hi ⊂ H in the two-dimensional torus T 2 shape space composed of [φ0, φi] ∈ T . Then, we take an The physical meaning of each term (right hand side) in approach similar to coordinate descent methods, where at each Eq.(10) can be interpreted in a similar manner to our analysis step, we optimize and update one function in H and iterate in Section II-A. The physical meaning of the first term (right among all the independent functions in H until convergence. hand side) in Eq.(10) is the displacement solely from the mo- ∗ In the converged gait function H , every independent function tion in sub-system i, while keeping φ0 constant at initial posi- ∗ ∗ hi ⊂ H should be optimized in its corresponding two- tion. Note that all other sub-system motions have been coupled T ∗ [i] dimensional torus shape space composed of [φ0, φi] . to φ0 by H in our derivation of local connection matrix AH∗ . The most challenging part in our proposed gait optimization Keeping φ0 constant at initial position is equivalent to keeping tool is to find the optimized mapping hi in the two-dimensional all other sub-system motions stopped at initial position. Thus, T torus shape space [φ0, φi] . The detailed optimization steps the first term (right hand side) in Eq.(10) can be interpreted are presented in the remainder of this section. as the displacement solely from sub-system motion i, while To optimize each independent function hi ⊂ H, we keeping all other motion systems stopped at initial position, T C first relate the two-dimensional shape variable [φ0, φi] to i.e., Pi ∩P[i], where P[i] = P1 ∪P2 ∪...∪Pi−1 ∪Pi+1 ∪...∪PN . the sub-system motion phase variable Φ such that Φ = The subscript [i] in P[i] denotes that P[i] is the union of the T [h1(φ0), h2(φ0), ..., hi−1(φ0), φi, hi+1(φ0), ..., hN (φ0)] . contribution of all other sub-system motions except sub-system Next, we relate the differentials of φ0 and φi to the sub- motion i. Since only one sub-system motion is involved in system motion phase variable differential by a Jacobian matrix the first term (right hand side) in Eq.(10), it is then a pre-

T J Φ,[φ0, φi] :   computed constant, independent from the sub-system motion ˙ φ0 coordination. Φ˙ = J T (8) Φ,[φ0, φi]  ˙  The physical meaning of the third term (right hand side) in φi Eq.(10) is the additional displacement from coordination be- With Eq.(8) in hand, we can rewrite Eq.(7) as tween sub-system motion i and all other sub-system motions,   i.e., Pi ∩ P[i]. φ˙ C 0 ˙ 0 T 0 Since Pi = (Pi ∩ P[i]) ∪ (Pi ∩ P[i]), Pi can be visually ξ = A (Φ)Φ = A ([φ0 φi] )J T   Φ,[φ0, φi] ˙ analyzed by the surface integral over the height function φi [i]   (9) ∇ × A ∗ plus a pre-computed constant. ˙ H [i] φ0 Next, we develop the tool to visualize the contribution from = A ([φ φ ]T ) H 0 i  ˙  coordination of two specific sub-system motions i and j, i.e., φi Ci ∩ Cj. In order to construct Ci ∩ Cj, we first assign hj = 0 [i] where AH is the new local connection matrix. The subscript such that in this scenario, sub-system motion j is stopped. H denotes that we need the gait function H to derive the Following our approach in section II-B2, we construct a two- dimensional shape variable [φ0, φi] and relate it to the sub- system motion phase variable Φ[j] (the superscript [j] denotes [j] that φj is not involved in Φ ) as:

[j] ∗ ∗ ∗ Φ = [h1(φ0), ..., hi−1(φ0), φi, hi+1(φ0), ..., ∗ ∗ ∗ T ..., hj−1(φ0), 0, hj+1(φ0), hN (φ0)] (11)

Given Eq.(11), we can relate the differential of φ0 and φi to the sub-system motion phase variable differential Φ[j] by a

Jacobian matrix J [j] T : Φ ,[φ0, φi]   φ˙ ˙ [j] 0 Φ = J [j] T   (12) Φ ,[φ0, φi] ˙ φi

Note that motion associated with the sub-system motion phase variable Φ[j] in Eq.(12) does not include the motion in sub-system j. Similar to Eq.(7), we can then relate it to the shape velocity: k=j−1 k=N [j] X X ξ = A( fk(φk) + fk(φk))r ˙ k=1 k=j+1 " ˙ # 0 [j] ˙ [j] 0 T φ0 = A (Φ )Φ = A ([φ0 φi] )J [j] T Φ ,[φ0, φi] φ˙ Fig. 3. The model for our robot and its sub-system motions. (a) The i shape variables of a robot with a long articulated body and four legs. The " ˙ # body undulation joint angle, αi, and leg shoulder joint angle βi are labelled. [i][j] T φ0 = AH∗ ([φ0 φi] ) (13) Segments in solid ellipses are in contact with the ground; segments in open φ˙i ellipses are in the air. (b) Leg sub-system motion. The leg sub-system motion where ξ[j] denotes the body velocity (the superscript [j] in ξ[j] is prescribed by a periodic variable φ1. The row of eight boxes indicate the contact state of the leg during the phase of the gait, where filled gray means denotes that sub-system motion j is not coupled in the body ground contact and open white means in the air. The blue curves indicate [i][j] the leg shoulder joint angles. (c) Upper back sub-system motion. The upper velocity) and AH∗ denotes the new local connection. The [i][j] back sub-system motion is prescribed by a periodic variable φ2. (d) Lower superscript [i][j] in AH∗ denotes that we separated hi and back sub-system motion. The lower back sub-system motion is prescribed by ∗ [i][j] φ hj from H in deriving AH∗ . With Eq.(13), we can rewrite a periodic variable 3 Eq.(3) as:

" # " # P ∩ P Z φ˙0 Z φ˙0 In this way, we can visualize i j as the surface integral [i][j] T [i][j] T [i] [i][j] AH∗ ([φ0 φi] ) = AH∗ ([φ0 φi] ) ˙ ˙ over the constructed height function ∇ × (A ∗ − A ∗ ). ∂χ φi ∂χ1:φ0=0 φi H H Z " ˙ # [i][j] T φ0 III.EXAMPLE: GAITDESIGNFORAROBOTWITHALONG + A ∗ ([φ0 φi] ) H ˙ ACTUATED BODY AND INTERMEDIATE SIZEDLEGS ∂χ2:φi=0 φi - ZZ [i][j] T In this section, we apply our proposed hierarchical frame- + ∇ × AH∗ ([φ0 φi] )dφ0dφi, (14) χ−χ1−χ2 work to design gaits for a robot with a long actuated body and intermediate-sized legs. Existing approaches to design gaits for Similar to our analysis in Section II-B3, we can interpret the quadrupedal systems can be found in [27]. We validate our physical meaning of the third term (right hand side) in Eq.(14) proposed hierarchical framework by robot experiments using as the additional displacement resulting from the coordination a robophysical approach [1, 19]. We first test the whole-body between sub-system motion i and all other sub-system motions motion resulting from the two common approaches to design coupled to φ0 in Eq.(11), where we assume that sub-system quadrupedal body undulation. We next apply our proposed motion j is stopped. In this way, we can express the physical framework to design whole-body motion and visualize the meaning of the third term (right hand side) in Eq.(14) as Pi ∩ kinematics of whole-body coordination. Finally, we verify our theoretical predictions by robot experiments and RFT (P[i] − Pj). Given Eq.(10) and Eq.(14), we can construct Pi ∩ Pj. We simulation. subtract the third term (right hand side) in Eq.(14) from the third term (right hand side) in Eq.(10), giving us: A. Experimental set-up

Pi ∩ Pj = Pi ∩ P[i] − Pi ∩ (P[i] − Pj ) We built an open-loop, servo-driven, 3D-printed robot (450 ZZ " ˙ # " ˙ # g, ∼40 cm long) with a long actuated body and intermediate- [i] φ0 [i][j] φ0 = ∇ × (A ∗ ( ) − A ∗ ( ))dφ0dφi sized legs (Fig.1), and tested its performance on granular H φ˙ H φ˙ χ−χ1−χ2 i i media (∼ 1 mm diameter poppy seeds). Each leg (front Fig. 4. Height functions (a) The height function presenting the coordination of snake-like body undulation and leg movement. (b) The height function presenting the coordination of lizard-like body undulation and leg movement. (c1) The height function presenting the coordination of upper back and leg movement. (c2) The height function presenting the coordination of lower back and leg movement. In all illustrations, the solid blue curve represents the optimized gait path. right FR, front left FL, hind right HR and hind left HL) has two DoFs, with one controlling the planar shoulder joint Fig. 5. Robot experiment and RFT simulation results (a) The primary data of displacement from robot experiments (solid lines with error bars) and RFT angle βi and the other controlling the binary ground contact simulation (solid lines) plotted as a function of gait percentage over two gait δi. The body consists of three segments, connected by two cycles for snake-like body undulation (blue), optimal body undulation (red) independently-actuated joints (upper back joint angle α and and lizard-like body undulation (yellow). (b) The displacement is plotted as 1 a function of spatial phase. The red curve with error bars shows the robot lower back joint angle α2), and maintains continuous contact experiment results. The blue curve shows the RFT simulation results. with the ground (Fig. 3). The robot executed a programmed set of movements (fol- lowing the gait input) to walk on the loosely-packed poppy can improve the locomotion performance by coordinating with seeds. Throughout the experiment, four Optitrack Flex13 cam- the leg sub-system motion and the lower back sub-system eras recorded the positions of the infrared-reflective markers motion. on the robot. At the end of each experiment, the robot’s 3) Lower back sub-system motion: Similar to upper back final position was identified. Before each experiment, an air- sub-system motion, we prescribe the lower back joint angle, fluidized bed erased the footprints and allowed the seeds to be α2, as a sinusoidal function of the gait phase φ3, as illustrated reset into a loosely-packed state. in Fig. 3d. While the lower back motion system itself cannot generate effective locomotion, it can improve the locomotion B. Sub-system motions performance by coordinating with the leg sub-system motion We decompose the motion of the robot into three sub- and the upper back sub-system motion. system motions: the leg sub-system motion, the upper back sub-system motion and the lower back sub-system motion. C. Whole body motion gait design 1) Leg sub-system motion: We assume that the leg sub- Body undulation plays an important role in whole-body system motion follows a gait pattern, a typical type of leg motion. In some cases where the leg drag is dominating movements given from prior work [9]. The trot is a two-beat (lizard-like locomotion), the body undulation will form a gait with diagonally paired leg movements. The fore right leg standing wave to improve the self-propulsion by leg-ground is always in phase with the hind left leg while the fore left leg interaction [11, 27]. In other cases where the body drag is is always in phase with the hind right leg. In Fig. 3b, we show dominating (snake-like locomotion), the body undulation will that the shoulder joint angles and the leg contact patterns of form a traveling serpenoid wave to generate self-propulsion all legs are prescribed by a periodic variable φ1. by body-ground interaction [8]. In this section, we explore the 2) Upper back sub-system motion: We prescribe the upper role of body undulation for a quadrupedal robot with a long back joint angle, α1, as a sinusoidal function of the gait actuated body and intermediate-sized legs. phase φ2, as illustrated in Fig. 3c. While the upper back We assume all the mapping functions are linear, i.e., φi = motion system itself cannot generate effective locomotion, it φj +ψij, where ψij is the phase offset between the sub-system motion i and j. We define the spatial phase as the phase shift we can design the whole-body motion gait in the three- between the upper back and lower back, i.e., ψ23. The spatial dimensional sub-system motion phase space. Fig. 4c shows the phase characterizes the wave pattern of the body undulation. converged optimal gaits on their corresponding height func- optimal In our robot experiments, we found that the optimal spatial tions. In optimal body undulation, we have ψ12 = 1.13π, optimal optimal phase, intermediate between lizard-like and snake-like, max- ψ13 = 0.88π and ψ23 = 0.25π. We observe that optimal imizes the displacement. The optimized whole-body motion spatial phase in optimal undulation ψ23 is intermediate snake lizard will leverage the self-propulsion of leg-ground contact and the between ψ23 and ψ23 . self-propulsion of body-ground contact. The theory-predicted optimal body undulation for our robot 1) Snake-like body undulation: The long actuated body is neither traveling serpenoid wave nor standing wave. Instead, of the robot can generate self-propulsion from body-ground the intermediate spatial space in the spectrum, from lizard- interaction. Hatton et al. [8] studied the motion of a three- like to snake-like, will maximize the forward displacement. link swimmer on granular media, where the body undulation To verify our theory, we sampled the spectrum and tested the follows the traveling serpenoid wave. We call the traveling the resulting whole-body motion on the robot experiments and serpenoid wave body undulation snake-like body undulation. RFT simulation. Both robot experiments and RFT simulation In snake-like body undulation, we couple the upper and lower agree well with our theory prediction. back sub-system motions such that they form a traveling serpenoid wave propagated from head to tail. Specifically, the D. Visualization phase of lower back sub-system motion is π/2 ahead of the In order to visualize the contribution from each individual phase of upper back sub-system motion, i.e., the spatial phase sub-system motion, we present a Venn diagram for sub-system of snake-like body undulation is ψsnake = π/2. 23 motions in Table I. There are three major sets: P , contri- Next, we seek to coordinate the traveling serpenoid wave leg bution from the leg movement (blue circle), P , contribution body undulation with the leg movement, which is character- up from the upper back (red circle), and P , the contribution ized by ψsnake. Following the steps in section II-B2, we can low 12 from the lower back (yellow circle). The intersection of three calculate the height function on the shape space composed of sets creates 4 additional regions (for a total of 7). The physical [φ , φ ]T and then design the proper whole-body coordina- 1 2 meaning of each region is listed in Table I. tion gait. The height function and the optimized whole-body Note that regions 1, 3 and 6 are the contribution solely from coordination gait for snake-like body undulation is shown in upper back, lower back and leg sub-system motion respec- Fig 4a. To summarize, in snake-like body undulation, we have tively. The contribution solely from an individual sub-system ψsnake = 0.75π, ψsnake = 1.25π and ψsnake = 0.5π. 12 13 23 motion is a pre-computed constant, independent from any gait 2) Lizard-like body undulation: The long actuated body we prescribe. Other regions, presenting the contribution from can, on the other hand, help to improve the self-propulsion by coordination, are determined by the gait paths (characterize by leg-ground interaction. Zhong et al. [27] studied the motion the phase offset ψ , ψ and ψ ) we choose. In Table I, we of a quadrupedal robot with a single DoF in the back. Ijspeert 12 13 23 listed the dependence of each region on the phase offsets. et al. [11] showed that standing wave body undulation can be coordinated with leg movements during . We call the Following the steps in Section II-B3, we can visualize the standing wave body undulation lizard-like body undulation. contribution from each region in Fig. 6. Note that in a motion Although the lizard-like body undulation itself does not lead system with three sub-system motions, all the regions in the to effective motion, it can be coordinated with leg movement Venn diagram can be visualized from the basic set operations. to improve locomotion performance. From Fig. 6c and Fig. 6d, we observe that optimized body un- In lizard-like body undulation, we couple the upper and dulation indeed leads to additional displacement by increasing lower back such that they form a standing wave. Specifically, the self-propulsion from the leg-ground interaction, similar to lower back sub-system motion is always in phase with upper the role of lizard body undulation. On the other hand, Fig. 6a suggests that the slight phase offset between upper back and back sub-system motion, i.e., φ2 = φ3, such that spatial phase lizard lower back, ψ23, can lead to additional displacement by self- ψ23 = 0. Next, we seek to coordinate the standing wave body un- propulsion from body-leg interaction, similar to the role of dulation with the leg movement, which is characterized by snake body undulation. ψlizard. Following the steps in section II-B2, we can calcu- 12 E. Results late the height function on the two-dimensional shape space T composed of [φ1, φ2] and then design the proper whole- In our robot experiments, we recorded the displacement of body coordination gait. The height function and the optimized the robot as it implemented the different gaits. In Fig. 5a, we whole-body coordination gait for lizard-like body undulation is plotted the displacement as a function of gait percentage for shown in Fig 4b. To summarize, in lizard-like body undulation, the snake-like, optimal body and lizard-like body undulation lizard lizard lizard we have ψ12 = π, ψ13 = π and ψ23 = 0. gaits. In Fig. 5b, we plotted the displacement as a function of 3) Optimal body undulation: In this subsection, we con- spatial phase. Both simulation and robot experiments suggest sider the upper back and lower back as two independent sub- that our theoretically calculated optimal spatial phase will lead system motions. With the methods described in section II-B2, to the largest displacement. Venn Diagram Region Definition Physical meaning Determined by C 1 Pup ∩ (Pleg ∪ Plow) solely from UB - C 2 (Pup ∩ Plow) ∩ Pleg from coordination of UB and LB ψ23 C 3 Plow ∩ (Pleg ∪ Pup) solely from LB -

4 Plow ∩ Pleg ∩ Pup whole body coordination ψ12 and ψ13 C 5 (Pleg ∩ Plow) ∩ Pup from coordination of LB and legs ψ13 C 6 Pleg ∩ (Pup ∪ Plow) solely from legs - C 7 (Pleg ∩ Plow) ∩ Pup from coordination of UB and legs ψ12 TABLE I DEFINITION AND PHYSICAL MEANING OF EACH OF THE REGIONS IN THE VENNDIAGRAMFOROURROBOT.

Fig. 6. Visualization of the contribution from individual motion sub system and the contribution from the coordination. Each region in the Venn diagram can be visualized by the surface integral over the corresponding height function.

We observe that the optimal body undulation spatial phase body-leg interaction and leg-ground interactions to generate decreases from π/2 to 0 for a robot with a long actuated efficient locomotion. Our theory-predicted optimized whole- body and infinitesimal-sized legs (like a snake), a robot with body motion agrees well with the presented robot experiments a long actuated body and intermediate-sized legs, and a robot and RFT simulation. with a long actuated body and long legs (like a lizard). Such In future work, we will further explore the underlying link spatial phase decreases could be associated with the changes between our hierarchical gait design framework and the way in leg sizes. We hypothesize that the evolution of leg size animals have optimized their locomotion through evolution. (limblessness of snake) is associated with the changes in Specifically, we believe there could be interesting similarities their corresponding locomotion patterns. This framework may between our framework and the emergence of locomotion provide insight into the hypothesis on the correspondence patterns in certain quadrupeds (e.g., lizards, skinks), as well between form and function [18]. as centipedes and millipedes. In doing so, we will study how biological systems coordinate their HDoF motion systems and IV. CONCLUSION adapt to different environments and leg/body morphologies. In this paper, we presented a new framework for design- Although this paper only considered cases where the hi- ing locomotion by coordinating motions of different sub- erarchy had a depth of one (i.e., no sub-sub-systems), we systems, as well as visualizing the kinematics of the resulting believe that our approach can be used recursively to optimize whole-body motion. Our framework allows us to design, locomotion for a robot decomposed into a deeper hierarchy optimize, and visualize the whole-body motion. We applied of sub-systems, sub-sub-systems, etc. Additionally, this frame- our framework to design a whole-body motion for quadrupedal work can also be applied to complex high-dimensional motion robots. A quadrupedal robot with a long actuated body and systems where many sub-systems exist. For the future, we infinitesimally small legs (snake-like) mainly utilizes its body- will extend our work to study real-time motion planning in ground interaction to generate self-propulsion. In contrast, response to possible sub-system failures. That is, when some the quadrupedal robot with a long actuated body and long joints fail to be actuated, we envision that our framework will legs (lizard-like) mainly unitizes its leg-ground interaction be able to design a new whole-body motion in response to to generate self-propulsion. Our quadrupedal robot, with a these new constraints. Additionally, we will investigate how long actuated body and intermediate-sized legs, combines our framework can be used to analyze with both approaches to coordinate the self-propulsion from both numerous sub-systems in complex environments. REFERENCES Business Media, 2013. [15] Hamidreza Marvi, Chaohui Gong, Nick Gravish, Henry [1] Jeffrey Aguilar, Tingnan Zhang, Feifei Qian, Mark Astley, Matthew Travers, Ross L Hatton, Joseph R Kingsbury, Benjamin McInroe, Nicole Mazouchova, Mendelson, Howie Choset, David L Hu, and Daniel I Chen Li, Ryan Maladen, Chaohui Gong, Matt Travers, Goldman. Sidewinding with minimal slip: Snake and et al. A review on locomotion robophysics: the study of robot ascent of sandy slopes. Science, 346(6206):224– movement at the intersection of robotics, soft matter and 229, 2014. dynamical systems. Reports on Progress in Physics, 79 [16] Kamilo Melo and Laura Paez. Experimental determina- (11):110001, 2016. tion of control parameter intervals for repeatable gaits in [2] Gordon J Berman, Daniel M Choi, William Bialek, and modular snake robots. In Safety, Security, and Rescue Joshua W Shaevitz. Mapping the stereotyped behaviour Robotics (SSRR), 2014 IEEE International Symposium of freely moving fruit flies. Journal of The Royal Society on, pages 1–7. IEEE, 2014. Interface, 11(99):20140672, 2014. [17] Hiromi Mochiyama, Etsujiro Shimemura, and Hisato [3] Gregory S Chirikjian and Joel W Burdick. The kine- Kobayashi. Shape control of manipulators with hyper de- matics of hyper-redundant robot locomotion. IEEE grees of freedom. The International Journal of Robotics transactions on robotics and automation, 11(6):781–793, Research, 18(6):584–600, 1999. 1995. [18] John A Nyakatura, Kamilo Melo, Tomislav Horvat, [4] Jin Dai, Hossein Faraji, Chaohui Gong, Ross L Hatton, Kostas Karakasiliotis, Vivian R Allen, Amir Andik- Daniel I Goldman, and Howie Choset. Geometric Swim- far, Emanuel Andrada, Patrick Arnold, Jonas Laustroer,¨ ming on a Granular Surface. In Robotics: Science and John R Hutchinson, et al. Reverse-engineering the Systems, 2016. locomotion of a stem amniote. Nature, 565(7739):351, [5] Chaohui Gong, Daniel I Goldman, and Howie Choset. 2019. Simplifying Gait Design via Shape Basis Optimization. [19] Yasemin Ozkan-Aydin, Jennifer M. Rieser, Christian M. In Robotics: Science and Systems, 2016. Hubicki, William Savoie, and Daniel I. Goldman. Physics [6] Chaohui Gong, Matthew J Travers, Henry C Astley, approaches to natural locomotion: Every robot is an Lu Li, Joseph R Mendelson, Daniel I Goldman, and experiment. In Shawn M. Walsh and Michael S. Strano, Howie Choset. Kinematic gait synthesis for snake robots. editors, Robotic Systems and Autonomous Platforms, The International Journal of Robotics Research, 35(1-3): Woodhead Publishing in Materials, pages 109 – 127. 100–113, 2016. Woodhead Publishing, 2019. [7] Chaohui Gong, Julian Whitman, Jaskaran Grover, Baxi [20] Suresh Ramasamy and Ross L Hatton. Geometric Chong, Richard Ren, and Howie Choset. Geometric gait optimization beyond two dimensions. In American motion planning for systems with toroidal and cylindrical Control Conference (ACC), 2017, pages 642–648. IEEE, shape spaces. In Dynamic Systems and Control Confer- 2017. ence, 2018. [21] Shai Revzen and Daniel E Koditschek. Why we need [8] Ross L Hatton, Yang Ding, Howie Choset, and Daniel I more Degrees of Freedom. Procedia IUTAM, 20:89–93, Goldman. Geometric visualization of self-propulsion in 2017. a complex medium. Physical review letters, 110(7): [22] Sarah S Sharpe, Stephan A Koehler, Robyn M Kuckuk, 078101, 2013. Miguel Serrano, Patricio A Vela, Joseph Mendelson, [9] Milton Hildebrand. Symmetrical gaits of horses. science, and Daniel I Goldman. Locomotor benefits of being a 150(3697):701–708, 1965. slender and slick sand swimmer. Journal of Experimental [10] Shigeo Hirose. Biologically inspired robots: snake-like Biology, 218(3):440–450, 2015. locomotors and manipulators, volume 1093. Oxford [23] Angus Ellis Taylor and William Robert Mann. Advanced University Press Oxford, 1993. calculus. Wiley, 1972. [11] Auke Jan Ijspeert, Alessandro Crespi, Dimitri Ryczko, [24] Matthew Tesch, Kevin Lipkin, Isaac Brown, Ross Hatton, and Jean-Marie Cabelguen. From swimming to walking Aaron Peck, Justine Rembisz, and Howie Choset. Pa- with a salamander robot driven by a spinal cord model. rameterized and scripted gaits for modular snake robots. science, 315(5817):1416–1420, 2007. Advanced Robotics, 23(9):1131–1158, 2009. [12] Shoshichi Kobayashi and Katsumi Nomizu. Foundations [25] Matthew J Travers, Julian Whitman, Perrin Schiebel, of differential geometry, volume 1. Interscience publish- Daniel I Goldman, and Howie Choset. Shape-Based ers New York, 1963. Compliance in Locomotion. In Robotics: Science and [13] Chen Li, Tingnan Zhang, and Daniel I Goldman. A Systems, 2016. terradynamics of legged locomotion on granular media. [26] Tingnan Zhang and Daniel I Goldman. The effectiveness Science, 339(6126):1408–1412, 2013. of resistive force theory in granular locomotion a. Physics [14] Jerrold E Marsden and Tudor S Ratiu. Introduction to of Fluids, 26(10):101308, 2014. mechanics and symmetry: a basic exposition of classical [27] Baxi Zhong, Yasemin Ozkan Aydin, Chaohui Gong, mechanical systems, volume 17. Springer Science & Guillaume Sartoretti, Yunjin Wu, Jennifer Rieser, Haosen Xing, Jeffery Rankin, Krijn Michel, Alfredo Nicieza, et al. Coordination of back bending and leg movements for quadrupedal locomotion. In Robotics: Science and Systems, 2018.