Decoupled limbs yield differentiable trajectory outcomes through intermittent contact in locomotion and manipulation  Andrew M. Pace   Samuel A. Burden ∗  Abstract  When limbs are decoupled, we find that trajectory outcomes in mechanical systems subject to unilateral constraints vary differentiably with respect to initial conditions, even as the contact mode sequence varies.  1   Introduction  Locomotion with legs entails intermittent contact with terrain; manipulation with digits entails intermittent contact with objects. Since legged locomotion   is   self–manipulation [JK13; Joh+16], mathematical models for intermittent contact between limbs and environments apply equally well to both classes of behaviors.   Parsimonious models for the dynamics of intermittent contact are piecewise-defined, with transitions between contact modes summarized by abrupt changes in sys- tem velocities. Such models are   hybrid   dynamical systems whose state evolution is governed by continuous-time   flow   (generated by a vector field) punctuated by discrete-time   reset   (specified by a map). Trajectory outcomes are the resulting state of the system after   flowing   and undergoing nec- essary resets for a specified period of time. Trajectory outcomes in hybrid systems generally vary discontinuously as the discrete mode sequence varies as in Fig. 1 ( left ). The point of this paper is to provide sufficient conditions that ensure trajectories in mechanical systems subject to unilateral constraints vary (continuously and) differentiably through intermittent contact, even as the contact mode sequence varies as in Fig. 1 ( right ). Since scalable algorithms for optimization [Pol97] and learning [SB98] rely on differentiability, conditions ensuring existence of derivatives are of practical importance in robotic locomotion and manipulation.  1.1   Organization  We begin in Sec. 2 by specifying the class of dynamical systems under consideration, namely,  mechanical   systems subject to   unilateral   constraints.   Sec. 3 imposes conditions on the system dynamics and trajectories that enable us in Sec. 4 to report that trajectories vary differentiably with respect to initial conditions, even as the contact mode sequence varies.  ∗ Department of Electrical Engineering, University of Washington, Seattle, WA, USA ( apace2,sburden@uw.edu ). This material is based upon work supported by the U. S. Army Research Laboratory and the U. S. Army Research Office under contract/grant number W911NF-16-1-0158.  arXiv:1609.04056v2 [cs.RO] 23 May 2017
1.2   Relation to prior work  The technical content in Sec. 2 and Sec. 3 appeared previously in the literature and is (more– or–less) well–known; we collate the results here to contextualize and streamline our contributions in Sec. 4. -0.3   -0.2   -0.1   0.0   0.1   0.2   0.3 initial rotation   θ (0)   (rad)  -0.4  -0.2  0.0  0.2  0.4 final rotational velocity    ̇ θ ( t )   (rad/sec)  -0.3   -0.2   -0.1   0.0   0.1   0.2   0.3 initial rotation   θ (0)   (rad)  -0.15  -0.1  -0.05  0.0  0.05  0.1  0.15  Figure 1: Trajectory outcomes after flowing for a uniform time from the initial conditions away from impacts in mechanical systems subject to unilateral constraints. ( left ) In general, trajectory outcomes depend discontinuously on initial conditions. In the pictured model for rigid–leg trotting (adapted from [Rem+10]), discontinuities arise when two legs touch down:   if the legs impact simultaneously (corresponding to rotation   θ (0) = 0), then the post–impact rotational velocity is zero; if the rear leg impacts before the front leg ( θ (0)   >   0) or vice–versa ( θ (0)   <   0), then the post– impact rotational velocities are bounded away from zero. ( right ) When limbs are decoupled (e.g. through viscoelasticity), trajectory outcomes depend continuously on initial conditions.   In the pictured model for soft–leg trotting (adapted from [Bur+15a]), trajectory outcomes (solid lines) are continuous and differentiable. These figures were generated using simulations of the depicted models.  2   Mechanical systems subject to unilateral constraints  In this paper, we study the dynamics of a mechanical system with configuration coordinates  q   ∈   Q   =   R d   subject to unilateral constraints   a ( q )   ≥   0 specified by a differentiable function
a   :   Q   →   R n   where   d, n   ∈   N   are finite.   We are primarily interested in systems with   n >   1 constraints, whence we regard the inequality   a ( q )   ≥   0 as being enforced componentwise.   Given any   J   ⊂ { 1 , . . . , n } , and letting   | J |   denote the number of elements in the set   J , we let   a J   :   Q   →   R | J |  denote the function obtained by selecting the component functions of   a   indexed by   J , and we regard the equality   a J   ( q ) = 0 as being enforced componentwise. It is well–known (see e.g. [Bal00, Sec. 3] or [Joh+16, Sec. 2.4, 2.5]) that with   J   =   { j   ∈ { 1 , . . . , n }   :   a j   ( q ) = 0 }   the system’s dynamics take the form  M   ( q ) ̈ q   =   f   ( q,    ̇ q ) +   c ( q,    ̇ q )  ̇ q   +   Da J   ( q ) > λ J   ( q,    ̇ q ) ,   (1a)  ̇ q +   = ∆ J   ( q,    ̇ q − ) ,   (1b) where   M   :   Q   →   R d × d   specifies the mass matrix for the mechanical system in the   q   coordinates,  f   :   T Q   →   R d   is termed the   effort map   [Bal00] and specifies 1   the internal and applied forces,  c   :   T Q   →   R d × d   denotes the   Coriolis matrix   determined 2   by   M   ,   Da J   :   Q   →   R | J |× d   denotes the (Jacobian) derivative of the constraint function   a J   with respect to the coordinates,   λ J   :   T Q   →   R | J |  denotes the reaction forces generated in contact mode   J   to enforce the constraint   a J   ( q )   ≥   0,  λ J   ( q ) =   ( Da J   ( q ) M   ( q ) − 1 Da J   ( q ) > ) − 1   ,   (2) ∆ J   :   T Q   →   R d × d   specifies the collision restitution law that instantaneously resets velocities to ensure compatibility with the constraint   a J   ( q ) = 0,  ̇ q +   = ∆ J   ( q,    ̇ q − ) =   I d   −   (1 +   γ ( q,    ̇ q − )) P J   ( q )  ̇ q − ,   (3) where   I d   is the ( d   ×   d ) identity matrix,   γ   :   T Q   →   [0 ,   ∞ ) specifies the   coefficient of restitution ,  P J   :   Q   →   R d × d   is the projection onto the constraint surface,  P J   =   M   − 1 Da >  J  ( Da J   M   − 1 Da >  J  ) − 1   Da J   ,   (4) and  ̇ q +   (resp.    ̇ q − ) denotes the right– (resp. left–)handed limits of the velocity vector with respect to time.  Definition 1   (contact modes) .   The constraint functions   { a j   } n j =1   partition the set of   admissible  configurations   A   =   { q   ∈   Q   :   a ( q )   ≥   0 }   into a finite collection 3   { A J   } J ∈ 2 n   of   contact modes :  ∀ J   ∈   2 n   :   A J   =   { q   ∈   Q   | a J   ( q ) = 0 ,  ∀ i   6   ∈   J   :   a i ( q )   >   0 }   .   (5)  For each   J   ∈   2 n : we let   T A   =   { ( q,    ̇ q )   ∈   T Q   :   q   ∈   A }   and   T A J   =   { ( q,    ̇ q )   ∈   T Q   :   q   ∈   A J   } ; if   q   ∈   A J  then we say constraints in   J   are   active   at   q .  Remark 1.   In Def. 1 (contact modes),   J   =   { 1 , . . . , n }   indexes the maximally constrained contact mode and   J   =   ∅   indexes the unconstrained contact mode.  1 We let   T Q   =   R d   ×   R d   denote the   tangent bundle   of the configuration space   Q ; an element ( q,    ̇ q )   ∈   T Q   can be regarded as a pair containing a vector of generalized configurations   q   ∈   R d   and velocities  ̇ q   ∈   R d ; we write  ̇ q   ∈   T q   Q .  2 For each   `, m   ∈ { 1 , . . . , d }   the ( `, m ) entry   c `m   is determined from the entries of   M   by the formula  c `m   =   −   1 2  ∑ d k =1   ( D k M `m   +   D m M `k   −   D ` M km ) [JK13, Eqn. 30].  3 We let 2 n   =   { J   ⊂ { 1 , . . . , n }}   denote the   power set   (i.e. the set containing all subsets) of   { 1 , . . . , n } .
3   Assumptions  The point of this paper is to provide conditions that ensure trajectories of (1) vary differentiably as the contact mode sequence 4   varies. Without imposing additional conditions, the seemingly benign equations in (1) admit a range of dynamical phenomena that preclude differentiability. This section contains the conditions that will enable us to obtain differentiable trajectory outcomes in Sec. 4.  3.1   Existence and uniqueness of trajectories  In the present paper, we will assume that appropriate conditions have been imposed to ensure trajectories of (1) exist on a region of interest in time and state.  Assumption 1   (existence and uniqueness) .   There exists a   flow   for   (1) , that is, a function   φ   :  F   →   T A   where   F   ⊂   [0 ,   ∞ )   ×   T A   is an open subset (in the subspace topology) containing   { 0 } ×   T A  and for each   ( t,   ( q,    ̇ q ))   ∈   F   the restriction   φ | [0 ,t ] ×{ ( q,    ̇ q ) }   : [0 , t ]   →   T Q   is the unique left–continuous trajectory for   (1) .  Remark 2.   The problem of ensuring trajectories of   (1)   exist and are unique has been studied extensively; we refer the reader to [Bal00, Thm. 10] for a specific result, [Bro16, Thm. 5.3] for a setup using constrained complementarity problems, and [Joh+16] for a general discussion of this problem.  3.2   Differentiable vector field and reset map  Since we are concerned with differentiability properties of the flow, we assume the elements in (1) are differentiable.  Assumption 2   (differentiable vector field and reset map) .   The vector field   (1a)   and reset map   (1b)  are continuously differentiable.  Remark 3.   If we restricted our attention to the continuous–time dynamics in   (1) , then Assump. 2 would suffice to provide the local existence and uniqueness of trajectories imposed by Assump. 1; as illustrated by [Bal00, Ex. 2], Assump. 2 is insufficient when the vector field   (1a)   is coupled to the reset map   (1b) .  3.3   Decoupled limbs  Since continuity is necessary for differentiability, we must impose a condition that yields continuous outcomes for trajectories of (1). A general condition that is known 5   to provide continuity is that constraint surfaces intersect orthogonally relative to the mass matrix. Formally,  ∀ i, j   ∈ { 1 , . . . , n }   , i   6   =   j, q   ∈   a − 1  i   (0)   ∩   a − 1  j   (0) :  Da i ( q ) M   ( q ) − 1 Da j   ( q ) >   = 0 .   (6) Physically, this condition implies that any limb or body segments that can undergo impact si- multaneously must be inertially decoupled. Although this condition ensures trajectory outcomes are continuous [Bal00, Thm. 20], they generally remain nonsmooth [PB17, Thm. 1].   Thus we introduce a stronger condition that entails decoupling limb forces through a body.  4 See Def. 4 (contact mode sequence).  5 We refer to [Bal00, Thm. 20] for a detailed exposition of this result.
Assumption 3   (limbs decoupled through body) .   The configuration decouples into   ( n +1)   segments, hence   2 n   possible contact modes,   q   = ( q j   ) n j =0   ∈   Q   =   ∏ n j =0   Q j   where   Q j   =   R d j   so that: 1. the mass matrix is block diagonal,   M   ( q ) = diag ( M j   ( q j   )) n j =0 , where   M j   :   Q j   →   R ( d j   × d j   ) ; 2. for limb   j   ∈ { 1 , . . . , n }   the constraint   a j   only depends on   q j   ,   a j   :   Q j   →   R , the coefficient of restitution   γ j   only depends on the limb states,   γ j   :   T Q j   →   R , and the effort   f j   only depends on the states of the limb and the body,   f j   :   T Q 0   ×   T Q j   →   R d j   ; 3. the effort   f 0   applied to the body depends additively on the states of the limbs and the body,  f 0   =   g 0   +   ∑ n j =1   g j   , where for   j >   0 ,   g j   :   T Q 0   ×   T Q j   →   R d 0   , and   g 0   :   T Q 0   →   R d 0   .  Remark 4.   In the decoupled structure described in the preceding assumption, the variable   q 0   ∈  Q 0   =   R d 0   contains the “body” degrees–of–freedom, i.e. all coordinates that cannot undergo impact (and are not inertially coupled to those that can). A limb may contain several links and as such have several bilateral constraints corresponding to it. For instance in [Ken+16, Fig. 1(middle)], one limb contains four rigid bars. Each limb can be coupled through forces with the body, but can only influence other limbs indirectly through the body. Note that series compliance [Spr+13; Odh+14] and/or backdrivability [Hyu+14; Ken+16] contribute to inertial decoupling, but conditions (1) and (2) of Assump. 3 (limbs decoupled through body) require inertial decoupling in all degrees–of–freedom between body and limbs.  Remark 5   (discontinuous outcomes in locomotion) .   The analysis of a saggital–plane quadruped in [Rem+10] provides an instructive example of the behavioral consequences of coupling limbs in legged locomotion. As summarized in [Rem+10, Sec 3.1], the model possesses 3 distinct but nearby trot gaits, corresponding to whether two legs impact simultaneously or at distinct time instants; the simultaneous–impact trot is unstable due to discontinuous dependence of trajectory outcomes on initial conditions.  3.4   Differentiable constraint activation/deactivation times  Trajectories of (1) are not continuous functions of time due to intermittent impacts that trigger the reset map (1b).   However, it has been known for some time 6   that trajectory outcomes can nevertheless depend differentiably on initial conditions away from impact times, so long as the contact mode sequence is fixed.   For this result to hold, the time when constraints activate (or deactivate) must depend differentiability on initial conditions.   We now develop definitions used to state an   admissibility   condition at the end of the section that yields differentiable time–to– activation (and time–to–deactivation).  Definition 2   (admissible constraint activation/deactivation) .   A trajectory initialized at   ( q,    ̇ q )   ∈  T A J   ⊂   T Q   activates constraints   I   ∈   2 n   at time   t >   0   if (i) no constraint in   I   was active immediately before time   t   and (ii) all constraints in   I   become active at time   t ; this activation is   admissible   if the constraint velocity 7   for all activated constraints is negative.   Formally, with  ( ρ,    ̇ ρ − ) = lim s → t −   φ ( s,   ( q,    ̇ q ))   denoting the left–handed limit of the trajectory at time   t ,  ∀ i   ∈   I   :   D t   [ a i   ◦   φ ] (0 ,   ( ρ,    ̇ ρ − )) =   Da i ( ρ )  ̇ ρ −   <   0 .   (7)  6 The earliest instance of this result we found in the English literature is [AG58]. Subsequently, many authors (ourselves included) have re–proven this result; a partial list includes [HP00; Gri+02; WA12; Bur+15b]. The result follows via a straightforward composition of smooth flows with smooth time–to–impact maps; we refer the interested reader to [Bur+15b, App. A1] for details.  7 Formally, the   Lie derivative   [Lee12, Prop. 12.32] of the constraint along the vector field specified by (1a).
Similarly, the trajectory   deactivates constraints   I   ∈   2 n   at time   t >   0   if (i) all constraints in   I  were active at time   t   and (ii) no constraint in   I   remains active immediately after time   t ; this deactivation time is   admissible   if, for all deactivated constraints:   (i) the constraint velocity or constraint acceleration 8   is positive, or (ii) the time derivative of the contact force is negative. Formally, with   ( ρ,    ̇ ρ + ) = lim s → t +   φ ( s,   ( q,    ̇ q ))   denoting the right–handed limit of the trajectory at time   t , for all   i   ∈   I   :  (i)   D t   [ a i   ◦   φ ] (0 ,   ( ρ,    ̇ ρ + ))   >   0   or  D 2  t   [ a i   ◦   φ ] (0 ,   ( ρ,    ̇ ρ + ))   >   0 ,  or (ii)   D t   [ λ i   ◦   φ ] (0 ,   ( ρ,    ̇ ρ + ))   <   0 .  (8)  Remark 6.   The conditions for admissible constraint deactivation in case (i) of   (8)   can only arise at constraint activation times; otherwise the trajectory is continuous, whence active constraint velocities and accelerations are zero.  Definition 3   (admissible trajectory) .   The trajectory initialized at   ( q,    ̇ q )   is   admissible on [0 , t ]   ⊂   R  if (i) it has a finite number of constraint activation (hence, deactivation) times on   [0 , t ] , and (ii) every constraint activation and deactivation is admissible; otherwise the trajectory is   inadmissible .  Definition 4   (contact mode sequence) .   The   contact mode sequence 9   associated with an admissible trajectory   φ ( q,    ̇ q )   on   [0 , t ]   ⊂   R   that has   m   activation and/or deactivation times   t 1 , . . . , t m   is the unique function   ω   :   { 0 , . . . , m } →   2 n   such that there exists a finite sequence of times   { t ` } m +1  ` =0   ⊂   [0 , t ]  for which   0 =   t 0   < t 1   <   · · ·   < t m +1   =   t   and  ∀ `   ∈ { 0 , . . . , m }   :   φ (( t ` , t ` +1 ) ,   ( q,    ̇ q ))   ⊂   T A ω ( ` ) .   (9)  Remark 7.   In Def. 4 (contact mode sequence), the sequence   ω   is easily seen to be unique by the admissibility of the trajectory; indeed, the associated time sequence consists of start, stop, and constraint activation/deactivation times.  Assumption 4   (admissible trajectories) .   The trajectory of   (1)   initialized at   ( q,    ̇ q )   is admissible on   [0 , t ]   for all   ( t,   ( q,    ̇ q ))   ∈   F .  4   Differentiability through contact  Under Assumptions 1–4 from Sec. 3, previous work has shown that, when the contact mode se- quence is fixed, trajectory outcomes vary continuously [Bal00, Thm. 20] and differentiably [AG58] with respect to variations in initial conditions (i.e.   initial states and parameters).   This enables the use of scalable algorithms for optimal control [Pol97] and reinforcement learning [SB98] to improve the performance of a given behavior (corresponding to the fixed contact mode sequence) using gradient descent. However, these algorithms cannot be relied upon to select among differ- ent behaviors (corresponding to different contact mode sequences) since trajectory outcomes are known to depend nonsmoothly on initial conditions [PB17, Thm. 1].   In this section we report that decoupled limbs yield classically differentiable trajectory outcomes   even as the contact mode sequence varies , enabling the use of scalable algorithms to select behaviors.  8 Formally, the second Lie derivative of the constraint along the vector field specified by (1a).  9 This definition differs from the   word   of [Joh+16, Def. 4] in that a contact mode is included in the sequence only if nonzero time is spent in the mode; this definition is more closely related to the   words   of [Bur+16, Eqn. 72]
Theorem 1   (differentiability through intermittent contact) .   Under Assumptions 1–4 from Sec. 3, if   t   is not a constraint activation time for   ( q,    ̇ q ) , then the flow   φ   :   F   →   T A   for   (1)   is continuously differentiable at   ( t,   ( q,    ̇ q ))   ∈   F .  Remark 8   (proof sketch) .   We provide an illustration of the result in Fig. 2, and a sketch of the proof strategy in what follows. For the complete proof, see Sec. 6.   Given a contact mode sequence  ω   for a trajectory initialized near   ( q,    ̇ q ) , we construct a continuously differentiable ( C 1 ) function  φ ω   defined on an open set containing   ( t,   ( q,    ̇ q ))   by composing the sequence of flow–to–activation and flow–to–deactivation functions specified by   ω .   Without loss of generality, we only consider constraint activations. 10   Near   ( q,    ̇ q )   in Fig. 2, there are two activation sequences, corresponding to whether constraint   1   activates before constraint   2   activates, or vice–versa. For each   I   ⊂ { 1 ,   2 }  we let   φ I   denote the   C 1   flow for   (1a) , 11   and define the   C 1   function   Γ I   ( u,   ( p,    ̇ p )) = ( u,   ( p,   ∆ I   ( p )  ̇ p )) . By Assump. 4 (admissible trajectories), there exist   C 1   time–to–activation functions   τ   2  { 1 } , τ   2  ∅   for constraint   2   defined over open neighborhoods of   ( ρ,    ̇ ρ − )   and  (  ρ,    ̇ ρ +  { 1 }  )  and similarly there exists   C 1  time–to–activation functions   τ   1  { 2 } , τ   1  {∅}   for constraint   1   defined over open neighborhoods of   ( ρ,    ̇ ρ − )  and  (  ρ,    ̇ ρ +  { 2 }  )  . For each contact mode   I   ⊂ { 1 ,   2 }   and constraint   j   ∈ { 1 ,   2 }   undergoing activation ( j   6   ∈   I ), we let   φ j I   denote the flow–to–activation,  φ j I   ( u,   ( p,    ̇ p )) =   ( u   −   τ   j I   ( p,    ̇ p ) , φ I   ( u   −   τ   j I   ( p,    ̇ p ) ,   ( p,    ̇ p )) )   ;   (10)  since   φ j I   is obtained via composition from   C 1   functions, it is a   C 1   function. For   ω 1   = ( ∅ ,   { 1 }   ,   { 1 ,   2 } ) , the function   φ ω 1   is given by the composition  φ ω 1   =   φ { 1 , 2 }   ◦   Γ { 2 }   ◦   φ 2  { 1 }   ◦   Γ { 1 }   ◦   φ 1  ∅ ;   (11)  for   ω 2   = ( ∅ ,   { 2 }   ,   { 1 ,   2 } ) , the function   φ ω 2   is given by the composition  φ ω 2   =   φ { 1 , 2 }   ◦   Γ { 1 }   ◦   φ 1  { 2 }   ◦   Γ { 2 }   ◦   φ 2  ∅ .   (12)  Since both   φ ω 1   and   φ ω 2   are obtained via composition from   C 1   functions, they are   C 1   functions. The generalization of this procedure to arbitrary contact mode sequences is given in   Sec. 6.   As illustrated in Fig. 2, the trajectory outcome near   φ ( t,   ( q,    ̇ q ))   ∈   T A { 1 , 2 }   is differentiable with respect to the initial condition near   ( q,    ̇ q )   ∈   T A ∅ , even as the contact mode sequence changes from   ω 1   to  ω 2 . Formally, we can show that   Dφ ω 1   ( t,   ( q,    ̇ q )) =   Dφ ω 2   ( t,   ( q,    ̇ q ))   by computing these derivatives via the Chain Rule; this entails taking products of matrices with the general form 12  D Γ I   ( u,   ( p,    ̇ p )) =     1   0   0 0   I d   0 0   D p (∆ I   ( p,    ̇ p )  ̇ p )   D    ̇ p   (∆ I   ( p,    ̇ p )  ̇ p )      ,   (13)  Dφ j I   ( u,   ( p,    ̇ p )) =  [   1   1  Dh j   ( p,    ̇ p ) F I   ( p,    ̇ p )   Dh j   ( p,    ̇ p ) 0   I 2 d   −   1  Dh j   ( p,    ̇ p ) F I   ( p,    ̇ p )   F I   ( p,    ̇ p ) Dh j   ( p,    ̇ p )  ]  .   (14)  10 Admissible constraint deactivations do not alter the flow to first order since the state and vector field are continuous during these transitions.  11 These flows are guaranteed to exist over an open subset of   R   ×   T Q   containing   { 0 } ×   A I   by Assump. 2 (differ- entiable vector field and reset map).  12 For the definition of   h j   , see (18); for the definition of   F I   , see (38).
( q,    ̇ q )    ̇ ρ +   = ∆ { 1 , 2 } ( ρ,    ̇ ρ − )   φ ( t,   ( q,    ̇ q ))  T A ∅   T A { 1 , 2 }  T A { 1 }  ( ρ,    ̇ ρ − )  { a 1   = 0 } T A { 2 }  ( ρ,    ̇ ρ + )  { a 1   = 0 }  { a 2   = 0 }  { a 2   = 0 }  Figure 2: Illustration of trajectory undergoing two simultaneous constraint activations: the tra- jectory initialized at ( q,    ̇ q )   ∈   T A ∅   ⊂   T Q   flows via (1a) to a point ( ρ,    ̇ ρ − )   ∈   T A ∅   where both constraint functions   a 1 ,   a 2   are zero, instantaneously resets velocity via (1b) to  ̇ ρ +   = ∆ { 1 , 2 } ( ρ,    ̇ ρ − ), then flows via (1a) to   φ ( t,   ( q,    ̇ q ))   ∈   T A { 1 , 2 }   ⊂   T Q .   Nearby trajectories undergo activation and deactivation at distinct times: trajectories initialized in the red region activate constraint 1 and flow through contact mode   T A { 1 }   before activating constraint 2—their contact mode sequence is  ω 1   = ( ∅ ,   { 1 }   ,   { 1 ,   2 } )—while trajectories initialized in the blue region activate 2 and flow through  T A { 1 , 2 }   before deactivating 1—their contact mode sequence is   ω 2   = ( ∅ ,   { 2 }   ,   { 1 ,   2 } ).   Differen- tiability of trajectory outcomes is illustrated by the fact that red outcomes lie along the same submanifold as blue.  5   Discussion  We conclude by discussing implications and routes to generalizing the theoretical results reported above.  5.1   Implications for optimization and learning  Optimization and learning algorithms have emerged in recent years as powerful tools for synthesis of dynamic and dexterous robot behaviors [Mom+05b; Tod11; Kui+15; Lev+16; Kum+16]. Since scalable algorithms leverage derivatives of trajectory outcomes, their applicability to the dynamics in (1) has previously (i) been confined to a fixed contact mode sequence [Mom+05b; Mom+05a] or (ii) relied on approximations or relaxations of the dynamics [Tod11; Kui+15; Lev+16; Kum+16]. Neither of these approaches is entirely satisfying: (i) prevents the algorithm from automatically selecting the behavior (corresponding to the contact mode sequence) in addition to extremizing its performance; (ii) implies the model under consideration is no longer a mechanical system subject to unilateral constraints. The results we report in Sec. 4 provide an analytical and computational framework within which derivative–based algorithms can be rigorously and directly applied to the dynamics of mechanical systems subject to unilateral constraints (1) to select between permutations of constraint (de)activations.  5.2   Decoupled limbs  Assump. 3 (limbs decoupled through body) can be interpreted physically as asserting that robot segments that can undergo impact simultaneously (i.e. limbs) must be decoupled through another segment not undergoing impact (i.e.   the body).   Crucially, this condition is required to ensure
trajectory outcomes vary continuously with respect to initial conditions [Bal00, Thm. 20]; since continuity is a precondition for differentiability, this condition is equally necessary for the result reported in Thm. 1 (differentiability through intermittent contact). We note that this condition is violated by conventional robots constructed from rigid serial chains and non–backdrivable actua- tors [Mur+94]. In contrast, design methodologies that incorporate direct–drive actuators [Hyu+14; Ken+16] or series compliance [Spr+13; Odh+14] tend to produce robot locomotors and manip- ulators with limbs that are (approximately) decoupled.   How approximately the limbs are de- coupled is the determining factor on whether Assump. 3 (limbs decoupled through body) holds, and hence whether the trajectories are differentiable with respect to initial conditions away from (de)activations.  5.3   Grazing contact  Def. 2 (admissible constraint activation/deactivation) precludes   grazing   trajectories, i.e. those that activate constraints with zero constraint velocity, or deactivate constraints with zero instantaneous rate of change in contact force. The key technical challenge entailed by allowing constraint activa- tion (resp. deactivation) we termed   inadmissible   lies in the fact that the time–to–activation (resp. time–to–deactivation) function is not differentiable. This fact has been shown by others [DB+08, Ex. 2.7], and is straightforward to see in an example. Indeed, consider the trajectory of a point mass moving vertically in a uniform gravitational field subject to a maximum height (i.e. ceiling) constraint. The grazing trajectory is a parabola, whence the time–to–activation function involves a square root of the initial position.  5.4   Zeno phenomena  Def. 2 (admissible constraint activation/deactivation) precludes   Zeno   trajectories, i.e. those that undergo an infinite number of constraint activations (hence, deactivations) in a finite time interval. The key technical challenge entailed by allowing Zeno lies in the fact that evaluating the flow requires composing an infinite number of flow–and–reset functions.   Composing a finite number of smooth functions yields a smooth function, but the same is not generally true for infinite compositions.   Thus although it is possible to show that the infinite composition results in a differentiable flow in simple examples like the   rocking block   [Hou63] and   bouncing ball   [Bal00, Sec. 6.1], we cannot at present draw any general conclusions regarding differentiability of the flow along Zeno trajectories.  5.5   Friction  Friction is a microscopic phenomenon that eludes first–principles understanding [GM01].   Phe- nomenological models of friction are macroscopic approximations; one popular model 13   posits a transition from   sticking   to   sliding   when the ratio of normal to tangential force drops below a parameterized threshold. The system’s flow is discontinuous at this threshold, as some trajecto- ries   slide   away from their   stuck   neighbors. Even if such transitions are avoided, the introduction of simple friction models into mechanical systems subject to unilateral constraints is known to produce pathologies including nonexistence and nonuniqueness of trajectories [Ste00].  13 Usually attributed to Coulomb, but also due to Antomons [GM01].
5.6   Non–Euclidean configuration spaces  We restricted the configuration space to   Q   =   R d   starting in Sec. 2 to simplify the exposition and lessen the notational overhead. Nevertheless, the preceding results apply to configuration spaces that are complete Riemannian manifolds. 14  5.7   Contact–dependent effort  The dynamics in (1) vary with the contact mode   J   ⊂ { 1 , . . . , n }   due to intermittent activation of unilateral constraints   a J   ( q )   ≥   0, but the (so–called [Bal00]) effort map   f   was not allowed to vary with the contact mode.   Contact–dependent effort can easily introduce nonexistence or nonuniqueness.   Indeed, consider a planar system with   q   ∈   R 2   undergoing plastic impact with the constraint surface specified by   a ( q ) =   q 1   subject to contact–dependent effort that satisfies  f ∅ ( q ) = ( − 1 ,   +1) if   q 1   >   0 and   f { 1 } ( q ) = (+1 ,   − 1) if   q 1   = 0. Every trajectory eventually activates the constraint. Once the constraint is active, the trajectory becomes ill–defined.  5.8   Massless limbs  To accommodate massless limbs, one must specify their unconstrained dynamics.   If the uncon- strained dynamics differ from the constrained dynamics, then in effect one has introduced contact– dependent effort, whence we refer to the preceding section. If the unconstrained dynamics do not differ from the constrained dynamics, then in effect one has introduced bilateral constraints the massless limbs must satisfy, whence we refer to the subsequent section. The constrained dynamics of massless limbs are derived in [BG15].  5.9   Bilateral constraints  The preceding results hold in the presence of bilateral (i.e. equality) constraints so long as they do not couple limbs. Formally, if the bilateral constraints   b ( q ) = 0 are specified by a differentiable function   b   :   Q   →   R m , there must exist an assignment   β   :   { 1 , . . . , m } → { 1 , . . . , n }   such that for all bilateral constraints   k   ∈ { 1 , . . . , m } , unilateral constraints   i, j   ∈ { 1 , . . . , n } ,   i   6   =   j , and configurations   q   ∈   b − 1 (0)   ∩   a − 1  i   (0)   ∩   a − 1  j   (0):  〈 Da i ( q ) , Da j   ( q ) 〉 M   − 1   = 0 ,  〈 Db β ( i ) ( q ) , Da j   ( q ) 〉 M   − 1   = 0 .   (15)  5.10   Non–autonomous dynamics  One may wish to allow the continuous and/or discrete dynamics in (1) to vary with time or an external input.   Some common cases can easily be handled.   If the dynamics are time–varying, but time could be incorporated as a state variable so that the preceding assumptions hold for the augmented system determined by    ̃ q   = ( t, q )   ∈    ̃ Q   =   R   ×   Q ,   ̃ M   (  ̃ q ) = diag (1 , M   ( q ))   ,    ̃ f  (  ̃ q,    ̇  ̃ q  )  = (0 , f   ( t, q,    ̇ q )) ,   (16)  14 Since the preceding results are not stated in coordinate–invariant terms, they are formally applicable only after passing to coordinates.
then the preceding results apply directly to the augmented system; a similar observation holds when the value of an external input is determined by time and state in such a way that the closed– loop system (possibly augmented as above to remove the time dependence) satisfied the preceding assumptions.  6   Appendix:   Proof of Thm. 1 (differentiability through intermittent contact)  Theorem 1   (differentiability through intermittent contact) .   Under Assumptions 1–4 from Sec. 3, if   t   is not a constraint activation time for   ( q,    ̇ q ) , then the flow   φ   :   F   →   T A   for   (1)   is continuously differentiable at   ( t,   ( q,    ̇ q ))   ∈   F . Proof.   We begin with an apology to the reader. The notation used in this proof is nonstandard; it is our hope that though it is nonstandard the notation clairifies the steps more than it confuses the reader. Before begining with the proof we introduce some notation: 1.   x [ i ] denotes the   i th entry into variable   x ; 2.   q   is the vector containing all limb positions.   q   = [ q [ i ]] n i =0   ∈   Q   = Π n i =0 Q i ; 3. similarly  ̇ q   is the vector containing all limb velocities.    ̇ q   = [  ̇ q [ i ]] n i =0 . ( q,    ̇ q )   ∈   T Q ; 4.  ̄ a i   :   Q   →   R   and   h i   :   T Q   →   R   where  ∀ q   ∈   Q    ̄ a i ( q ) =   a i ( q i ) ,   (17)  ∀ ( q,    ̇ q )   ∈   T Q   h i ( q,    ̇ q ) =   a i ( q i ) ,   (18) the logical extensions of   a i   to the corresponding domains; 5. let    J   :   T Q   →   R d × d   where    J   ( q,    ̇ q ) =   D q (∆ J   ( q,    ̇ q )  ̇ q ), the derivative of the post-impact velocity with respect to position; 6. let   ♦ J   :   T Q   →   R d × d   where   ♦ J   ( q,    ̇ q ) =   D    ̇ q (∆ J   ( q,    ̇ q )  ̇ q ), the derivative of the post-impact veloc- ity with respect to position; 15  7.  [   q [ i ]  ̇ q [ i ]  ] n i =0  =   [   q [0] T   , q [1] T   ,   · · ·   , q [ n ] T   ,    ̇ q [0] T   ,   · · ·   ,    ̇ q [ n ] T   ] T   is the deinterleaving of the position and velocity components of the individual limbs into a vector where the first half corresponds to position components and the later half corresponds to velocity values. What follows are some helpful identies based upon the above notation and the given assumptions. 1. Given orthogonality of constraints: 16  15 Given the coefficient of restitution   γ   is dependent upon  ̇ q ,   ♦ J   ( q,    ̇ q ) might differ from ∆ J   ( q,    ̇ q ).  16   Properties (a)-(c) are included here for completeness and (d)-(e) may also be seen more succinctly using the limb decoupling assumption.
(a) the reset map has a block diagonal form with ∆ j J   :   T Q j   →   R d j   × d j   , the reset map for limb   j   with   J   constraints active: ∆ J   ( q [ j ] ,    ̇ q [ j ]) = diag   ( ∆ j J   ( q [ j ] ,    ̇ q [ j ]) ) n j =0   ,   (19) where ∆ j J   ( q [ j ] ,    ̇ q [ j ]) =   I d j   −   (1 +   γ j   ( q [ j ] ,    ̇ q [ j ]))   M j   ( q [ j ]) − 1 Da T j   ( q [ j ])   ( Da j   ( q [ j ]) M   ( q [ j ]) − 1 Da T j   ( q [ j ]) )   Da j   ( q [ j ]); (20) (b) the velocity of a given limb is not affected by a reset if the constraint corresponding to that limb is not active: ∆ k J   ( q [ k ] ,    ̇ q [ k ]) =   I d k   if   k / ∈   J ;   (21) (c) the reset map for constraint mode   J   is equal to the product of the reset maps for each active constraint:   ∏  j ∈ J  ∆ { j } ( q,    ̇ q ) = ∆ J   ( q,    ̇ q );   (22) (d)    J   ( q,    ̇ q ) has a block diagonal structure with    j J   :   T Q j   →   R d j   × d j   :   J   ( q,    ̇ q ) = diag   (  j J   ( q [ j ] ,    ̇ q [ j ]) ) n j =0   ,   (23) where   j J   ( q [ j ] ,    ̇ q [ j ]) =   D q j  ( ∆ j J   ( q [ j ] ,    ̇ q [ j ] )    ̇ q [ j ]);   (24) (e) if a given limb is not in the active constraint set, the block corresponding to the limb in    J   is zero:   k J   ( q [ k ] ,    ̇ q [ k ]) = 0 if   k / ∈   J.   (25) (f)    J   ( q,    ̇ q ) is the summation of    { j }   for each active constraint:  ∑  j ∈ J   { j } ( q,    ̇ q ) =    J   ( q,    ̇ q ) .   (26) 2. Given limb decoupling: (a) the acceleration of each limb is only dependent upon the given limb’s and the body’s current position and velocity;   α J   :   T Q J   →   R d :  α J   ( q,    ̇ q )[ j ] =            M   − 1  j   ( q [ j ]) ( f j   ( q [0] ,    ̇ q [0] , q [ j ] ,    ̇ q [ j ]) +   c j   ( q [ j ] ,    ̇ q [ j ])  ̇ q )   if   j / ∈   J   and   j   6   = 0  M   − 1  j   ( q [ j ]) ( f j   ( q [0] ,    ̇ q [0] , q [ j ] ,    ̇ q [ j ]) +   c j   ( q [ j ] ,    ̇ q [ j ])  ̇ q +  Da j   ( q [ j ]) T   λ j   ( q [ j ] ,    ̇ q [ j ]) )   if   j   ∈   J M   − 1 0   ( q [0]) ( f 0 ( q,    ̇ q ) +   c 0 ( q [0] ,    ̇ q [0])  ̇ q [0])   if   j   = 0; (27) (b)   ♦ J   ( q,    ̇ q ) has a block diagonal form with   ♦ i J   :   T Q i   →   R d j   × d j   :  ♦ j J   ( q [ j ] ,    ̇ q [ j ]) =   D    ̇ q [ j ] ∆ j J   ( q [ j ] ,    ̇ q [ j ])  ̇ q [ j ]   (28)  D    ̇ q (∆ J   ( q,    ̇ q )  ̇ q ) =   D    ̇ q  ( diag[∆ j J   ( q [ j ] ,    ̇ q [ j ])] n j =0  )   ,   (29) = diag   [ D    ̇ q [ j ] ∆ j J   ( q [ j ] ,    ̇ q [ j ])  ̇ q [ j ] ] n j =0   (30) = diag[ ♦ j J   ( q [ j ] ,    ̇ q [ j ])] n j =0 ;   (31)
(c) if a given limb is not in the active constraint set, the corresponding block in   ♦ J   is the identity:  ♦ k J   ( q [ k ] ,    ̇ q [ k ]) =   I d k   if   k / ∈   J ;   (32) (d)   ♦ J   ( q,    ̇ q ) is the product of   ♦ { j }   for each active constraint:  ∏  j ∈ J  ♦ { j } ( q,    ̇ q ) =   ♦ J   ( q,    ̇ q ) .   (33) We now proceed with the proof. 1. We repeat some of the notations found in the proof of [PB17, Thm. 1] here. (a) For any given perturbation, there is a finite set of selection functions corresponding to a sequence of (de)activating constraints. (b) These selection functions will be indexed by a pair of functions ( ω, η ) where:   ω   :   { 0 , . . . , m } →  2 n   is a contact mode sequence, i.e.   ω   ∈   Ω;   η   :   { 0 , . . . , m   −   1 } → { 1 , . . . , n }   indexes con- straints that undergo admissible activation or deactivation. (c) Let   μ   :   { 0 , . . . , m } →   2 n   be defined as   μ ( k ) =   ⋃ k − 1  i =0   { η ( i ) } , where we adopt the conven- tion that   ⋃ − 1  i =0   { i }   =   ∅ ; note that   μ   is uniquely determined by   η . As in [PB17, Thm. 1], we suppress notation indicating dependence on   ω   and   η   until (59). (d) Let ( ρ,    ̇ ρ − ) = lim u ↑ s   φ ( u,   ( q,    ̇ q )).   For all   k   ∈ { 0 , . . . , m } , define  ̇ ρ k   = ∆ μ ( k ) ( ρ )  ̇ ρ − , once again using the convention   ⋃ − 1  i =0   =   ∅ . 2. Since assumptions 1–4 from Sec. 3 satisfy the hypotheses of [PB17, Thm. 1], 17   φ   is piecewise– differentiable. 3. Let   K a   be the set of constraints undergoing activation and   K d   be the set of constraints undergoing deactivation. 18  4. Assume, without loss of generality, the trajectory begins in the unconstrainted mode   ∅ . 19  5. Let   R J   denote the reset map into constraint mode   J ;   R J   :   T Q   →   T Q J   ,  ∀ ( q,    ̇ q )   ∈   T Q   :   R J   ( q,    ̇ q ) =  [   q  ∆ J   ( q,    ̇ q )  ̇ q  ]  .   (34) 6. Let   DR J   denote the (Jacobian) derivative of   R J   with respect to ( q,    ̇ q ).  ∀ ( q,    ̇ q )   ∈   T Q   :   DR J   ( q,    ̇ q ) =  [   I d   0   J   ( q,    ̇ q )   ♦ J   ( q,    ̇ q )  ]  .   (35) 7. Let    ̃ R L J   denote the reset map from constraint mode   J   into constraint mode   J \ L ;    ̃ R L J   :   T Q J   →   T Q J \ L ,  ∀ ( q,    ̇ q )   ∈   T Q J   :    ̃ R L J   ( q,    ̇ q ) =  [ q   ̇ q  ]  .   (36)  17 Assump. 3 (limbs decoupled through body)is stronger than the orthoganality of constraints assumption.  18 Activating constraints may instantaneously deactivate, e.g. a bean bag hitting a ceiling.  19 This does not imply   K d   =   ∅ .
8. Let ˆ R `   :   T Q   →   T Q,  ∀ ( q,    ̇ q )   ∈   T Q   :   ˆ R ` ( q,    ̇ q ) =  { R { η ( ` ) } ( q,    ̇ q )   if   η ( ` )   ∈   K a   ̃ R { η ( ` ) }  ω ( ` )   ( q,    ̇ q )   if   η ( ` )   ∈   K d .   (37) 9. Let   F J   denote the vector field in constraint mode   J .   F J   :   T Q J   →   R 2 d ,  ∀ ( q,    ̇ q )   ∈   T Q J   :   F J   ( q,    ̇ q ) =  [    ̇ q α J   ( q,    ̇ q )  ]  ,   (38) where   α J   ( q,    ̇ q ) is defined in (27). 10. Let  S `   = 1  Dh ` ( ρ,    ̇ ρ ` ) F ` ( ρ,    ̇ ρ ` )  (  F ` +1 ( ρ,    ̇ ρ ( ` +1) )   −   D   ˆ R ` ( ρ,    ̇ ρ ` ) F ` ( ρ,    ̇ ρ ` )  )  Dh ` ( ρ,    ̇ ρ ` ) ,   (39) where   F `   =   F ω ( ` ) , and   Dh `   =   Dh η ( ` ) ;   S `   ∈   R 2 d × 2 d . 11. The saltation matrix for a given word   ω   at ( ρ,    ̇ ρ ) is Ξ η ω   =  | K a | + | K d |− 1  ∏  ` =0  (  D   ˆ R ω ( ` )   ⋃ { η ( ` ) } ( ρ,    ̇ ρ ` ) +   S `  )  (40) where   S `   is defined in (39) [Bur+16, Eq. 66], [Iva98, Eq. 2.5]. 12. Given that the vector field associated with a constraint undergoing deactivation is continuous, the corresponding saltation matrix is   I 2 d . That is   S `   = 0 and   D   ˆ R ` ( ρ,    ̇ ρ ( ` − 1) ) =   I 2 d   when   η ( ` ) is a deactivation;   η ( ` )   ∈   K d . In what follows, the calculations are performed only for activating constraints. 20  13. The inner product   Dh ` ( ρ,    ̇ ρ ` ) F ` ( ρ,    ̇ ρ ` ) in the computation of   S `   is independent of the word   ω .  Dh ` ( ρ,    ̇ ρ ` ) F ` ( ρ,    ̇ ρ ` ) =   D  ̄ a η ( ` ) ( ρ )  ̇ ρ `   =   Da η ( ` )   ( ρ [ η ( ` )])  ̇ ρ [ η ( ` )] .   (41) 14. The reset map into contact mode   ω ( ` )   ⋃   { η ( ` ) }   from contact mode   ω ( ` ) is the same as the reset map into contact mode   { η ( ` ) }   from contact mode   ω ( ` ). 21  ∀ ( q,    ̇ q )   ∈   T Q ω ( ` )   :   R ω ( ` )   ⋃ { η ( ` ) } ( q,    ̇ q ` ) =  [   q  ∆ ω ( ` )   ⋃ { η ( ` ) } ( q,    ̇ q ` )  ̇ q `  ]  (42) =  [   q  ∆ { η ( ` ) } ( q,    ̇ q ` )  ̇ q `  ]  (43) =   R { η ( ` ) } ( q,    ̇ q ` ) .   (44)  20 This exclusion does not apply to the case of deactivations caused by an activation, e.g. a bouncing ball or a bean bag hitting the ceiling.  21 It is important to note that it is not always the case   ω ( `   + 1) =   ω ( ` )   ⋃   { η ( ` ) }   as in the case of instantaneous constraint deactivation dependent upon a constraint activation. In this case   ω ( `   + 1) =   ω ( ` ) and   η ( ` )   6   =   ∅ .
15. From the chain rule for total derivatives [Lee12, Prop C.3] and Assump. 3 (limbs decoupled through body), the first order approximation of the reset map into constraint mode   J   is the same as the product of the reset maps into constraint mode   { j } ∈   J   at a given point ( q,    ̇ q ).  DR J   ( q,    ̇ q ) =   D   ∏  j ∈ J  R { j } ( q,    ̇ q ) =  (∏  j ∈ J  DR { j }  )  ( q,    ̇ q ) .   (45) 16. Given (45) and identities (33) and (31),  DR J   ( q,    ̇ q ) =  [   I d   0 ∑  j ∈ J    { j } ( q )   ∏  j ∈ J   ♦ { j } ( q,    ̇ q )  ]  .   (46) 17. The saltation matrix equation (40) can then be written as Ξ η ω   =  | K a | + | K d |− 1  ∏  ` =0  (  D   ˆ R ` ( ρ,    ̇ ρ ` ) +   S `  )  .   (47) 18. By computing the acceleration of each limb using (27) and   ♦ η ( ` )   has a block diagonal structure given by (31), clearly  α w ( ` +1) ( ρ,    ̇ ρ ( ` +1) )   −   ♦ η ( ` ) ( ρ,    ̇ ρ ` ) α ω ( ` ) ( ρ,    ̇ ρ ` ) =  {  α { η ( ` ) } ( ρ,   ∆ { η ( ` ) } ( ρ,    ̇ ρ )  ̇ ρ )   −   ♦ { η ( ` ) } ( ρ,    ̇ ρ ) α ∅ ( ρ,    ̇ ρ )   if   ω ( `   + 1) =   ω ( ` )   ⋃   { η ( ` ) }  α ∅ ( ρ,   ∆ { η ( ` ) } ( ρ,    ̇ ρ )  ̇ ρ )   −   ♦ { η ( ` ) } ( ρ,    ̇ ρ ) α ∅ ( ρ,    ̇ ρ )   if   ω ( `   + 1) =   ω ( ` )   ⋃   { η ( ` ) } .  (48) 19. From (45), the vector field difference in the calcuation   S `   for only an activation reduces to  F ( ` +1) ( ρ,    ̇ ρ ( ` +1) )   −   DR ω ( ` )   ⋃ { η ( ` ) } ( ρ,    ̇ ρ ` ) F ` ( ρ,    ̇ ρ ` ) =   F ` +1 ( ρ,    ̇ ρ ( ` +1) )   −   DR ` ( ρ,    ̇ ρ ` ) F ` ( ρ,    ̇ ρ ` ) =  [[    ̇ ρ ( ` +1) [ j ]  α ω ( ` +1) ( ρ,    ̇ ρ ( ` +1) )[ j ]  ]  −  [   I d   0   j  { η ( ` ) } ( ρ,    ̇ ρ ` )   ♦ j  { η ( ` ) } ( ρ,    ̇ ρ ` )  ] [    ̇ ρ ` [ j ]  α ω ( ` ) ( ρ,    ̇ ρ ` )[ j ]  ]] n j =0  ,  (49)
where  [    ̇ ρ ( ` +1) [ j ]  α ω ( ` +1) ( ρ,    ̇ ρ ` )[ j ]  ]  −  [   I d   0   j  { η ( ` ) } ( ρ,    ̇ ρ ` )   ♦ j  { η ( ` ) } ( ρ,    ̇ ρ ` )  ] [    ̇ ρ ` [ j ]  α ω ( ` ) ( ρ,    ̇ ρ ` )[ j ]  ]  =                       [  0 0  ]  if   j   6   =   η ( ` ) and  j   6   = 0 [  0  α ω ( ` +1) ( ρ,    ̇ ρ ( ` +1) )[0]   −   α ω ( ` ) ( ρ,    ̇ ρ ` )[0]  ]  if   j   = 0  [   ̇ ρ ( ` +1) [ j ]   −    ̇ ρ ` [ j ]  α ω ( ` +1) ( ρ,    ̇ ρ ( ` +1) )[ j ]   −    j  { η ( ` ) } ( ρ,    ̇ ρ )  ̇ ρ ` [ j ]   −   ♦ j  { η ( ` ) } ( ρ,    ̇ ρ ` ) α ω ( ` ) ( ρ,    ̇ ρ ` )[ j ]  ]  if   j   =   η ( ` ) =                       [  0 0  ]  if   j   6   =   η ( ` ) and  j   6   = 0 [  0  α { η ( ` ) }  ( ρ,   ∆ { η ( ` ) } ( ρ,    ̇ ρ )  ̇ ρ )   [0]   −   α ∅ ( ρ,    ̇ ρ )[0]  ]  if   j   = 0  [   ( ∆ { η ( ` ) } ( ρ,    ̇ ρ )  ̇ ρ )   [ j ]   −    ̇ ρ [ j ]  α { η ( ` ) }  ( ρ,   ∆ { η ( ` ) } ( ρ,    ̇ ρ )  ̇ ρ )   [ j ]   −    j  { η ( ` ) } ( ρ,    ̇ ρ )  ̇ ρ [ j ]   −   ♦ j  { η ( ` ) } ( ρ,    ̇ ρ ) α ∅ ( ρ,    ̇ ρ )[ j ]  ]  if   j   =   η ( ` ) .  (50) The equality in the last step for the   j   =   η ( ` ) case can be seen from the block diagonal structure of    η ( ` )   and (48). Thus, for the case of only an activation,  F ( ` +1) ( ρ,    ̇ ρ ( ` +1) ) − DR ω ( ` )   ⋃ { η ( ` ) } ( ρ,    ̇ ρ ` ) F ` ( ρ,    ̇ ρ ` ) =   F { η ( ` ) } ( ρ,    ̇ ρ η ( ` ) )   −   DR { η ( ` ) } ( ρ,    ̇ ρ ∅ ) F ∅ ( ρ,    ̇ ρ ∅ ) .  (51) Clearly, it can be shown by algebraic manipulation similar to (50), the equality  F ` +1 ( ρ,    ̇ ρ ( ` +1) ) − DR ω ( ` )   ⋃ { η ( ` ) } ( ρ,    ̇ ρ ` ) F ` ( ρ,    ̇ ρ ` ) =   F ∅ ( ρ,    ̇ ρ )   −   DR { η ( ` ) } ( ρ,    ̇ ρ ∅ ) F ∅ ( ρ,    ̇ ρ ∅ )   (52) holds for the case of an activation instantly causing a deactivation. 20. The saltation matrix from (47) can be further simplified using the independence of the inner product in   S `   (41), along with the flow differences (51) and (52) to Ξ η ω   =  | K a | + | K d |− 1  ∏  ` =0  (  DR ` ( ρ,    ̇ ρ ) +    ̃ S `  )  ,   (53) where   ̃ S `   =   1  Da η ( ` ) ( ρ [ η ( ` )])  ̇ ρ [ η ( ` )]  ( F ω ( ` +1) \ ω ( ` ) ( ρ,    ̇ ρ )   −   DR ` ( ρ,    ̇ ρ ) F ∅ ( ρ,    ̇ ρ ) )   Dh η ( ` ) ( ρ,    ̇ ρ ) .   (54) 21. Given the constraints are only dependent upon position, clearly   ̃ S j   DR { i } ( ρ,    ̇ ρ ) =    ̃ S j   (55) for all   i, j   ∈ { 0 , . . . ,   | K | −   1 } .
22. Next we show that  DR { j } ( ρ,    ̇ ρ )    ̃ S i   =    ̃ S i   (56) for   j   6   =   i . Given the block structure of the corresponding matrices, we make the following observations: (a) only the columns associated with the indices for   q i   are nonzero in    ̃ S i ; (b) only the rows associated with   q 0   and   q i   are nonzero in    ̃ S i ; (c) since   j   6   =   i ,    i  { j } ( ρ,    ̇ ρ ) = 0 and   ♦ i  { j } ( ρ,    ̇ ρ ) =   I d i   . The nonzero elements of   S i   are thus multiplied by an identity like matrix. 23. Given (55) and (56), the saltation matrix (53) contains the matrix product    ̃ S `    ̃ S k , where   ` > k  and   k, `   ∈ { 0 , . . . ,   | K | −   1 } . Within this matrix product, lies the inner product  Dh ` ( ρ,    ̇ ρ )   ( F ω ( k +1) \ ω ( k ) ( ρ,    ̇ ρ )   −   DR k ( ρ,    ̇ ρ ) F ∅ ( ρ,    ̇ ρ ) )   =   Da ` ( ρ [ ` ])  ̇ ρ [ ` ]   −   Da ` ( ρ )  ̇ ρ [ ` ] = 0 .   (57) Hence   ̃ S `    ̃ S g   = 0 .   (58) 24. Given (55),(56), and (58), the saltation matrix (53) can be written as Ξ η ω ( ρ,    ̇ ρ ) =   DR K a   ( ρ,    ̇ ρ ) +   ∑  k ∈ K a   ̃ S k .   (59) 25. The Bouligand-derivative [Sch12, Chpt. 3] of   φ t ( q,    ̇ q ) in direction ( v,    ̇ v )   ∈   T Q   is given by  Dφ t ( q,    ̇ q ;   v,    ̇ v ) =   Dφ t − s ( ρ,    ̇ ρ )Ξ η ω ( ρ,    ̇ ρ ) Dφ s ( q,    ̇ q )  [ v   ̇ v  ]  ,   (60) where   s   ∈   R   is the time of simultaneous activation and/or deactivation of constraints, and  η, ω   are determined by ( v,    ̇ v ) [Bur+16, Eq. 65]. 26. As the saltation matrix calculation (59) is independent of the word   ω , (60) can be rewritten as  Dφ t ( q,    ̇ q ;   v,    ̇ v ) =   Dφ t − s ( ρ,    ̇ ρ )  (  DR K a   ( ρ,    ̇ ρ ) +   ∑  k ∈ K a   ̃ S k  )  Dφ s ( q,    ̇ q )  [ v   ̇ v  ]  .   (61) Then   Dφ t ( q,    ̇ q ;   . ) is a linear function and the flow is classically differentiable [Sch12, Chpt. 3].
References  [AG58]   M. A. Aizerman and F. R. Gantmacher. “Determination of Stability by Linear Ap- proximation of a Periodic Solution of a System of Differential Equations with Dis- continuous Right–Hand Sides”. In:   The Quarterly Journal of Mechanics and Applied Mathematics   11.4 (1958), pp. 385–398.   doi :   10.1093/qjmam/11.4.385   (cit. on pp. 5, 6). [BG15]   B. Brogliato and D. Goeleven. “Singular mass matrix and redundant constraints in unilaterally constrained Lagrangian and Hamiltonian systems”. In:   Multibody System Dynamics   35.1 (2015), pp. 39–61.   doi :   10.1007/s11044-014-9437-4   (cit. on p. 10). [Bal00]   P. Ballard. “The Dynamics of Discrete Mechanical Systems with Perfect Unilateral Constraints”. In:   Archive for Rational Mechanics and Analysis   154.3 (2000), pp. 199– 274.   doi :   10.1007/s002050000105   (cit. on pp. 3–6, 9, 10). [Bro16]   B. Brogliato.   Nonsmooth Mechanics . Springer, 2016 (cit. on p. 4). [Bur+15a]   S. A. Burden, H. Gonzalez, R. Vasudevan, R. Bajcsy, and S. S. Sastry. “Metrization and Simulation of Controlled Hybrid Systems”. In:   IEEE Transactions on Automatic Control   60.9 (2015), pp. 2307–2320.   doi :   10.1109/TAC.2015.2404231   (cit. on p. 2). [Bur+15b]   S. A. Burden, S. Revzen, and S. S. Sastry. “Model Reduction Near Periodic Orbits of Hybrid Dynamical Systems”. In:   IEEE Transactions on Automatic Control   60.10 (2015), pp. 2626–2639.   doi :   10.1109/TAC.2015.2411971   (cit. on p. 5). [Bur+16]   S. A. Burden, S. S. Sastry, D. E. Koditschek, and S. Revzen. “Event–Selected Vector Field Discontinuities Yield Piecewise–Differentiable Flows”. In:   SIAM Journal on Applied Dynamical Systems   15.2 (2016), pp. 1227–1267.   doi :   10.1137/15M1016588  (cit. on pp. 6, 14, 17). [DB+08]   M. Di Bernardo, C. J. Budd, P. Kowalczyk, and A. R. Champneys.   Piecewise–smooth dynamical systems: theory and applications . Springer, 2008 (cit. on p. 9). [GM01]   E. Gerde and M. Marder. “Friction and fracture”. In:   Nature   413.6853 (2001), pp. 285–288.   url :   http://dx.doi.org/10.1038/35095018   (cit. on p. 9). [Gri+02]   J. W. Grizzle, G. Abba, and F. Plestan. “Asymptotically stable walking for biped robots: Analysis via systems with impulse effects”. In:   IEEE Transactions on Auto- matic Control   46.1 (2002), pp. 51–64.   doi :   10.1109/9.898695   (cit. on p. 5). [HP00]   I. A. Hiskens and M. A. Pai. “Trajectory sensitivity analysis of hybrid systems”. In:  IEEE Transactions on Circuits and Systems I: Fundamental Theory and Applications  47.2 (2000), pp. 204–220.   doi :   10.1109/81.828574   (cit. on p. 5). [Hou63]   G. W. Housner. “The behavior of inverted pendulum structures during earthquakes”. In:   Bulletin of the Seismological Society of America   53.2 (1963), pp. 403–417.   url :  http://www.bssaonline.org/content/53/2/403.abstract   (cit. on p. 9). [Hyu+14]   D. J. Hyun, S. Seok, J. Lee, and S. Kim. “High speed trot–running: Implemen- tation of a hierarchical controller using proprioceptive impedance control on the MIT Cheetah”. In:   The International Journal of Robotics Research   33.11 (2014), pp. 1417–1445.   doi :   10.1177/0278364914532150   (cit. on pp. 5, 9).
[Iva98]   A. Ivanov. “The stability of periodic solutions of discontinuous systems that intersect several surfaces of discontinuity”. In:   Journal of Applied Mathematics and Mechanics  62.5 (1998), pp. 677 –685.   doi :   http://dx.doi.org/10.1016/S0021-8928(98)000 87-2   (cit. on p. 14). [JK13]   A. M. Johnson and D. E. Koditschek. “Legged Self–Manipulation”. In:   IEEE Access  1 (2013), pp. 310–334.   doi :   10.1109/ACCESS.2013.2263192   (cit. on pp. 1, 3). [Joh+16]   A. M. Johnson, S. A. Burden, and D. E. Koditschek. “A Hybrid Systems Model for Simple Manipulation and Self–Manipulation Systems”. In:   The International Journal of Robotics Research   35.11 (2016), pp. 1289–1327.   doi :   10.1177/0278364916639380  (cit. on pp. 1, 3, 4, 6). [Ken+16]   G. Kenneally, A. De, and D. E. Koditschek. “Design Principles for a Family of Direct-Drive Legged Robots”. In:   IEEE Robotics and Automation Letters   1.2 (2016), pp. 900–907.   doi :   10.1109/LRA.2016.2528294   (cit. on pp. 5, 9). [Kui+15]   S. Kuindersma, R. Deits, M. Fallon, A. Valenzuela, H. Dai, F. Permenter, T. Koolen, P. Marion, and R. Tedrake. “Optimization–based locomotion planning, estimation, and control design for the atlas humanoid robot”. In:   Autonomous Robots   (2015), pp. 1–27.   doi :   10.1007/s10514-015-9479-3   (cit. on p. 8). [Kum+16]   V. Kumar, E. Todorov, and S. Levine. “Optimal control with learned local mod- els: Application to dexterous manipulation”. In:   IEEE International Conference on Robotics and Automation (ICRA) . 2016, pp. 378–383.   doi :   10.1109/ICRA.2016.74 87156   (cit. on p. 8). [Lee12]   J. M. Lee.   Introduction to smooth manifolds . Springer–Verlag, 2012 (cit. on pp. 6, 15). [Lev+16]   S. Levine, C. Finn, T. Darrell, and P. Abbeel. “End–to–end Training of Deep Visuo- motor Policies”. In:   Journal of Machine Learning Research   17.1 (2016), pp. 1334– 1373.   url :   http://dl.acm.org/citation.cfm?id=2946645.2946684   (cit. on p. 8). [Mom+05a]   K. Mombaur, H. Bock, J. Schloder, and R. Longman. “Self-stabilizing somersaults”. In:   IEEE Transactions on Robotics   21.6 (2005), pp. 1148–1157.   doi :   10.1109/TRO.2 005.855990   (cit. on p. 8). [Mom+05b]   K. D. Mombaur, R. W. Longman, H. G. Bock, and J. P. Schl ̈ oder. “Open–loop Stable Running”. In:   Robotica   23.1 (2005), pp. 21–33.   doi :   10.1017/S026357470400058X  (cit. on p. 8). [Mur+94]   R. M. Murray, Z. Li, and S. S. Sastry.   A mathematical introduction to robotic ma- nipulation . CRC Press, 1994 (cit. on p. 9). [Odh+14]   L. U. Odhner, L. P. Jentoft, M. R. Claffee, N. Corson, Y. Tenzer, R. R. Ma, M. Buehler, R. Kohout, R. D. Howe, and A. M. Dollar. “A compliant, underactuated hand for robust manipulation”. In:   The International Journal of Robotics Research  33.5 (2014), pp. 736–752.   doi :   10.1177/0278364913514466   (cit. on pp. 5, 9). [PB17]   A. M. Pace and S. A. Burden. “Piecwise–Differentiable Trajectory Outcomes in Mechanical Systems Subject to Unilateral Constraints”. In:   Proceedings of the 20th International Conference on Hybrid Systems: Computation and Control . HSCC ’17. Pittsburgh, USA: ACM, 2017 (in press) (cit. on pp. 5, 7, 13).
[Pol97]   E. Polak.   Optimization: algorithms and consistent approximations . Springer–Verlag, 1997 (cit. on pp. 1, 6). [Rem+10]   C. D. Remy, K. Buffinton, and R. Siegwart. “Stability Analysis of Passive Dynamic Walking of Quadrupeds”. In:   The International Journal of Robotics Research   29.9 (2010), pp. 1173–1185.   doi :   10.1177/0278364909344635   (cit. on pp. 2, 5). [SB98]   R. S. Sutton and A. G. Barto.   Reinforcement Learning: An Introduction . The MIT Press, 1998.   url :   http://gen.lib.rus.ec/book/index.php?md5=A89A0A170F9E3 0ECECF319A9F0BB8041   (cit. on pp. 1, 6). [Sch12]   S. Scholtes.   Introduction to piecewise differentiable equations . Springer–Verlag, 2012.  doi :   10.1007/978-1-4614-4340-7   (cit. on p. 17). [Spr+13]   A. Spr ̈ owitz, A. Tuleu, M. Vespignani, M. Ajallooeian, E. Badri, and A. J. Ijspeert. “Towards dynamic trot gait locomotion: Design, control, and experiments with Cheetah-cub, a compliant quadruped robot”. In:   The International Journal of Robotics Research   32.8 (2013), pp. 932–950.   doi :   10.1177/0278364913489205   (cit. on pp. 5, 9). [Ste00]   D. E. Stewart. “Rigid–Body Dynamics with Friction and Impact”. In:   SIAM Review  42.1 (2000), pp. 3–39.   doi :   10.1137/S0036144599360110   (cit. on p. 10). [Tod11]   E. Todorov. “A convex, smooth and invertible contact model for trajectory opti- mization”. In:   IEEE International Conference on Robotics and Automation (ICRA) . 2011, pp. 1071–1076.   doi :   10.1109/ICRA.2011.5979814   (cit. on p. 8). [WA12]   E. D. Wendel and A. D. Ames. “Rank deficiency and superstability of hybrid sys- tems”. In:   Nonlinear Analysis: Hybrid Systems   6.2 (2012), pp. 787–805.   doi :   10.10 16/j.nahs.2011.09.002   (cit. on p. 5).