Composable Geometric Motion Policies using Multi-Task Pullback Bundle Dynamical Systems

Andrew Bylard, Riccardo Bonalli, Marco Pavone

Abstract— Despite decades of work in fast reactive plan- ning and control, challenges remain in developing reactive motion policies on non-Euclidean manifolds and enforcing constraints while avoiding undesirable potential function local minima. This work presents a principled method for designing and fusing desired robot task behaviors into a stable robot motion policy, leveraging the geometric structure of non- Euclidean manifolds, which are prevalent in robot configuration and task spaces. Our Pullback Bundle Dynamical Systems (PBDS) framework drives desired task behaviors and prioritizes tasks using separate position-dependent and position/velocity- dependent Riemannian metrics, respectively, thus simplifying individual task design and modular composition of tasks. For enforcing constraints, we provide a class of metric-based tasks, eliminating local minima by imposing non-conflicting potential functions only for goal region attraction. We also provide a geometric optimization problem for combining tasks inspired by Riemannian Motion Policies (RMPs) that reduces to a simple least-squares problem, and we show that our approach is geometrically well-defined. We demonstrate the 2 PBDS framework on the sphere S and at 300-500 Hz on a manipulator arm, and we provide task design guidance and an open-source Julia library implementation. Overall, this work Fig. 1: Example tree of PBDS task mappings designed to move a ball along presents a fast, easy-to-use framework for generating motion the surface of a sphere to a goal while avoiding obstacles. Depicted are policies without unwanted potential function local minima on manifolds representing: (black) joint configuration for a 7-DoF robot arm general manifolds. with two fully revolute joints, (purple) ball pose, (red) distances to obstacles, (blue) position on the sphere, and (green) distance to the goal. Note that damping can be defined on 2 × 5, SE(3), and/or 2 as desired. I.INTRODUCTION T R S within the free submanifold (e.g. in narrowing passages or Fast reactive planning and control is often crucial in when a constraint is near the goal) [14], [15]. dynamic, uncertain environments, and related techniques An alternative coming from differential geometry is to have a long history [1]–[5]. However, key challenges remain encode constraints not with forces, but with metrics. For in applying these techniques to general manifolds. Well- example, correctly designed Riemannian metrics [16] defined known non-Euclidean manifolds are ubiquitous in robotics, on the robot configuration manifold have been proposed to often representing part of the configuration manifold (e.g., curve the manifold to prevent constraint violation not due SO(3) for aerial robot attitude). They can also be essential 2 to forces pushing the robot away, but due to the space for representing robotic task spaces (e.g., the sphere S for stretching infinitely in the direction of constraints [17], [18]. painting or moving a fingertip around an object surface, such Such a reliance on curvature rather than competing potential as in Fig. 1). Further, constraints may restrict the robot to a functions may also eliminate traps due to potential function free submanifold which is difficult to represent explicitly. local minima [19]. For example, an obstacle in the workspace can produce However, correctly designing such metrics directly on the an additional topological hole that geodesics or “default” configuration manifold can be difficult, motivating the use of arXiv:2101.01297v1 [cs.RO] 5 Jan 2021 unforced trajectories should avoid [6], [7]. pullback metrics, i.e. metrics are designed on task manifolds Often such constraints are handled by constrained opti- where constraints are naturally defined and then “pulled mization or by repulsive potentials. However, constrained back” through the task map onto the configuration manifold. optimization can scale poorly with the number and com- One recent work advocating this approach is RMPflow [17], plexity of nonconvex constraints, often becoming impractical which develops a tree of subtask manifolds with metrics for real-time MPC without careful initialization [8]–[10]. and forces defined on the leaf manifolds which are pulled On the other hand, repulsive potentials can be difficult to back and combined into a single acceleration policy on design without producing incorrect behavior. For example, the configuration manifold. However, [17] uses potentials in it is well known that artificial potential function (APF) addition to metrics to represent constraint-enforcing subtasks, techniques [1], [2] without assumptions about the robot and increasing design complexity and leading to the same local workspace geometry [11]–[13] can produce many incorrect minima issues as in other APF approaches. local minima and unnatural behavior such as oscillations Additionally, it is often useful to perform task prioritiza- A. Bylard, R. Bonalli, and M. Pavone are with the Department of tion dependent on robot velocity. For example, when a robot Aeronautics and Astronautics, Stanford University, Stanford, CA 94305. is moving away from an obstacle, the obstacle can often be {bylard, rbonalli, pavone} @stanford.edu. This work was safely ignored. To this end, [17] introduces Geometric Dy- supported in part by NSF Cyber-Physical Systems (CPS) Grant No. 1931815 and by KACST. Thanks to Fransesco Bullo, Benoit Landry, Thomas Lew, namical Systems (GDSs) weighted by Riemannian Motion and Jean-Jacques Slotine for their helpful discussions and insights. Policies (RMPs) [20], which allows velocity-dependent met- f ∗df f ∗Id rics used both to specify subtask behavior (such as constraint TM f ∗TN TN TN avoidance) and to prioritize tasks. However, this approach ∗ presents two difficulties: First, it is very difficult to design f πN πN task metrics which both specify desirable task behavior πM f and prioritize tasks correctly. This has led to investigating M N learning the task metrics from demonstrations [21], but in Fig. 2: Commutative diagram of the key manifolds used to form a pullback any case, it is unclear what task behavior within a single task bundle dynamical system. Note that the pullback bundle f ∗TN is an n- could benefit from velocity-dependent Riemannian metrics, vector bundle over the robot configuration manifold M. as we discuss in Sec. IV-B. assigning coordinates to locally-Euclidean patches. M has a Second, a GDS with velocity-dependent metrics does smooth tangent bundle TM containing tangent spaces T M not meet the key property of geometric consistency, as p at each point p ∈ M, and it has a cotangent bundle T ∗M, we demonstrate in Sec. IV-A. Geometric consistency1 (i.e. which is a natural place to define forces [23]. We will also de- invariance to changes in coordinate representations) is an note (p, v) ∈ T M as v or v depending on desired emphasis essential property of any differential geometric method which p p of the base point p. Given a Riemannian metric g, the couple ensures that the quantities defined, in this case acceleration (M, g) is a Riemannian manifold. The metric provides a policies, are well-defined on the robot and task manifolds smoothly-varying inner product g : T M × T M −→ at invoked, thus correctly capturing and leveraging the structure p p p R each point and thus induces norms || · || . The metric g also of these manifolds (for example, see Fig. 5). gp gives a “sharp” operator ] : T ∗M −→ TM and generalized To summarize, a large gap remains for producing a fast, gradient gradf : M −→ TM, with f ∈ C∞(M), which easy-to-use, and geometrically-consistent approach for gen- are useful for applying forces. A choice of connection ∇ erating motion policies on general robot and task manifolds, in TM (e.g. the standard Levi-Civita connection) assigns a achieving velocity-dependent task-weighting and leveraging unique acceleration operator Dσ to each curve σ in M. Thus metric-based enforcement of constraints to eliminate un- 0 0 0 we can compute acceleration Dσσ = ∇σ0(t)σ , where σ is wanted potential function local minima. the velocity along σ (used interchangeably with σ˙ notation, Statement of Contributions: To this end, we provide the which can also indicate a time derivative). Then by choosing following contributions: generalized forces F : TM −→ T ∗M, one can specify a 1) We present the Pullback Bundle Dynamical Systems 0 ] dynamical system on M satisfying Dσσ (t) = F(σ(t)) . (PBDS) framework for combining multiple geometric task This can be written in local coordinates as the second-order behaviors into a single robot motion policy, while main- differential equations taining geometric consistency and stability. In doing so, k i j k kj we provide a geometrically well-defined formulation of a σ¨ (t)+σ ˙ (t)σ ˙ (t)Γij(σ(t)) = g (σ(t))Fj(σ(t), σ˙ (t)), (1) weighting scheme inspired by RMPs, which reduces to a where we make use of Einstein index notation and where simple least-squares problem. We also remove the tension k Γij are the Christoffel symbols associated with ∇. We will between single-task behavior and inter-task weighting by use this type of dynamical system to design desired behavior introducing separate velocity-dependent weighting pseudo- on task manifolds that corresponding robot motion policies metrics for each task to handle task prioritization. should aim to replicate. 2) We apply the PBDS framework to tangent bundles Note also that we will use bold symbols when convenient to show limits to the practical use of velocity-dependent to denote local matrix or vector representations of objects. Riemannian metrics for task behavior design and to show For example, given Ξk σ˙ iσ˙ jΓk , we can write (1) as why GDSs do not maintain geometric consistency. , ij 3) We provide a class of constraint-enforcing tasks en- σ¨(t) + Ξ(σ(t), σ˙ (t)) = g−1(σ(t))F(σ(t), σ˙ (t)). coded solely via simple, analytical Riemannian metrics that stretch the space, rather than via traditional barrier function We may also abbreviate such expressions, for example as potentials, eliminating potential function local minima. σ¨ + Ξ = g−1F. 4) We demonstrate PBDS policy behavior in numerical III.PULLBACK BUNDLE DYNAMICAL SYSTEMS experiments and at 300-500 Hz on a 7-DoF arm, and we provide a fast open-source Julia library called PBDS.jl. Given k tasks, let M and Ni be smooth m- and ni- Paper Organization: The paper is organized as follows. In dimensional robot configuration and task manifolds, respec- Sec. II we recall relevant concepts from Riemannian geome- tively, where {fi : M −→ Ni}i=1,...,k is a set of smooth task try and establish some notation. Then Sec. III, builds a multi- maps. These tasks could represent a variety of desired behav- task PBDS, extracts a motion policy, and provides stability iors such as attraction to a point/region, velocity damping, results. Sec. IV unpacks key advantages over RMPflow’s or avoidance of a constraint. To illustrate, consider a running GDS/RMP framework. Finally, Sec. V provides implemen- pedagogical example of a point robot on the surface of a ball, tation details and a robot arm demonstration. aiming to reach a goal while avoiding obstacles. A simple design for the relevant tasks is shown in Table I. As shown, II.GEOMETRIC PRELIMINARIES the desired task behaviors can be encoded by designing on each task manifold Ni a potential function Φi : Ni −→ R+, Here we recall some results in Riemannian geometry and ∗ dissipative forces FD,i : TNi −→ T Ni, and a Riemannian establish notation that will be used throughout the paper. metric g (which we can refer to as the behavior metric), M i For a more detailed introduction, see [16], [22]. Let all of which are smooth. We also choose the standard Levi- be a smooth m-dimensional manifold, equipped with charts Civita connection ∇i [22]. As mentioned, these components give dynamical systems 1Geometrically-consistent objects are also known of as being “global” in differential geometry [22]. Thus we will also refer to such objects as being on the task manifolds Ni, but we want to form a robot geometrically or globally well-defined. motion policy from a dynamical system on the configuration Task Task Map Behavior Metric Dissipative Forces Potential Weight pseudometric 2 2 Goal Attraction f1 : S → R : p 7→ dist(p, pgoal) g1 = 1 F D,1 = 0 Φ1 = kxk2 wi = I2 2 2 ¯ Damping f2 : S → S : p 7→ p g¯2 = I3 F D,2 = −4x¯˙ Φ2 = 0 w¯2 = I6 2 2 Obstacle Avoidance fi : S → R+ : p 7→ dist(p, Xobs) gi = exp(1/(2x )) F D,i = 0 Φi = 0 See (8) TABLE I: Simple task design summary for running example of a robot on a sphere surface navigating to a goal point while avoiding obstacles. Note that each obstacle has an avoidance tasks (hence the indices i). For the damping task, the overbars denote representations in ambient Euclidean space, which 2 2 are easily pulled back through standard embedding maps to induce corresponding objects on S and TS (see Appx. A.II for details). manifold M. One way to proceed is by using pullbacks, i.e. where πVB denotes the projection onto the vertical bundle. “pulling back” the necessary objects through the task maps In Appx. C.III, we show that this map is geometrically well- fi to operate over M. First, we recall the definition of the defined. In particular, a curve α(p,v) forming a valid local pullback bundle of TN, omitting task indices for simplicity: PBDS always exists, and the map S does not depend on the ∗ a −1 particular choices of curves α(p,v). Given this fact, we will f TN = πN (f(p)), (2) 0 omit further mention of α and use the shorthand γv (0) p∈M p , γ0 (0) for (p, v) ∈ TM. ` α(p,v) where is a disjoint union and πN is the standard projection Now for each individual task, we can form a map Si as on TN. This is itself a well-defined vector bundle, and above to retrieve corresponding desired task pullback accel- in particular is an (m + n)-dimensional manifold. The erations. However, our goal is to combine these tasks into relationships between these manifolds are depicted in Fig 2, a single robot motion policy. To do this, one approach is to ∗ ∗ including a pullback differential f df : TM −→ f TN : define a robot acceleration policy using a geometrically well- (p, v) 7→ (p, π2(dfp(v))), where π2 denotes a projection onto defined optimization problem designed to strike a weighted the velocity component. balance between these pullback task acceleration policies. ∗ Next we define the pullback connection f ∇, whose This is not as straightforward as it may appear since the Christoffel symbols are given locally by operative accelerations are each in different spaces and are ` thus difficult to compare (i.e. σ¨ is in TTM and γ0 is ∗ k ∂f k σ˙ (t) f Γ (p) = (p)Γ (f(p)) ∗ ij ∂xi `j in T (fi TNi) for each task). To resolve this, we form a TTM for all p ∈ M. In Lemma C.1 we show this connection map relating robot accelerations on to their resulting T (f ∗TN ) is globally well-defined and compatible with a pullback pullback task accelerations in i i : ∗ ∗ metric f g. This connection gives a corresponding pullback Zi : TTM −→ T (fi TNi) ∗ acceleration operator f Dγ for each curve γ in M. We can
 ∗  (4) also denote the total acceleration given by the pullback forces (p, v), a 7→ πVB d(fi dfi)(p,v)(a) . ∗ ] ∗ ∗ ∗ ] by f F(·) : f TN −→ f TN :(p, v) 7→ f FD(p, v) − Also, for each task we define a Riemannian pseudometric f ∗grad Φ(p), where we define the pullback dissipation forces ∗ ∗ wi on TNi which will provide its weighting against other f FD and pullback gradient f grad in Appx. C.I. tasks, and we impose the following assumptions: We are now equipped to construct a key building block (A1) The pseudometrics wi are at every point in TNi either for forming a full Pullback Bundle Dynamical System: positive-definite or zero. Definition III.1 (Local Pullback Bundle Dynamical System). (A2) The Jacobian of the product map of the task maps Let f : M −→ N be a smooth task map. Then for each associated with nonzero weights has rank m. (p, v) ∈ M, we can choose a curve α(p,v) :(−ε, ε) −→ M In practice, these weighting pseudometrics are often trivial ∗ resulting in γα(p,v) :(−ε, ε) −→ f TN for some ε > 0 to design (e.g., see our running example in Table I, where such that (f, g, Φ, FD, α(p,v)) forms a local Pullback Bundle the attractor task weight is the Euclidean metric, and the Dynamical System (PBDS) satisfying damping task weight is induced by a Euclidean metric).  f ∗D γ (s) = f ∗F(γ (s))] However, they can also be used to switch tasks on and  α(p,v) α(p,v) α(p,v)  ∗ 0 off, which is useful for enforcing constraints as shown PBDS γα (0) = f df (α (0)), α(p,v) (p,v) α(p,v)(0) (p,v) in Sec. IV-C. Also, as in the running example, (A1) and  0 α(p,v)(0) = (p, v). (A2) are easy to satisfy. In particular, it is typical to use one or more damping tasks which together always provide This local PBDS construction is local in the sense that in dissipation along all degrees of freedom of the robot. Now to practice, we define a PBDSα(p,v) for each (p, v) ∈ TM and ∗ conclude, each wi also has a pullback pseudometric Fi wi on only evaluate it at (p, v). However, it is geometrically well- T (f ∗TN ) defined using the natural higher-order task map defined and is required to ensure that we have well-defined i i Fi : TM → TNi :(p, v) 7→ (p, (dfi)p(v)) (Appx. C.III). dynamics at each point on the pullback bundle independent We can now form the desired ODE on our robot manifold: of the robot curve σ : [0, ∞) −→ M that we ultimately follow. In particular, these dynamics must be well-posed at Definition III.2 (Multi-Task Pullback Bundle Dynamical ∗ System). Let {fi : M −→ Ni}i=1,...k be smooth task maps. t = 0, (i.e. where f Dσ is not well-defined, requiring another ∗ Then the set {(fi, gi, Φi, FD,i, wi)}i=1,...,k forms a multi- curve α 0 to produce f D ) and in cases where the σ (0) ασ0(0) task PBDS with curves σ : [0, ∞) −→ M satisfying 0 curve ασ (t) defining a valid PBDSασ0(t) for some t ∈ [0, ∞)  Pk 1 2 is not unique. For example, the latter may occur when m > σ¨(t) = arg min kZi(a) − Si(σ ˙ (t))k ∗  i=1 2 Fi wi n, which is often the case in practice. a∈Dσ˙ (t) (5) From these local PBDS dynamics, we can extract the σ˙ (0) = (p0, v0). corresponding desired task pullback acceleration associated Here, D is the globally well-defined affine distribution of with each robot position and velocity: v TTM such that the subspace D(p,v) ⊆ T(p,v) satisfies a = S : TM −→ T (f ∗TN):(p, v) 7→ π (γ0 (0)), (3) v a VB α(p,v) v componentwise for each a = ((p, v), (a , a )) ∈ D(p,v). ∗ F ∗Id M TTM F dF F ∗TTN TTN TTN f1 f3 f2 ∗ F πTN πTN πTM N1 N2 N3 F f1,1 f1,3 f3,2 TM TN f1,2 f3,1 πM πN N N N N N 1,1 1,2 1,3 3,1 3,2 f M N Fig. 3: Tree of manifolds and task maps in a multi-task PBDS policy. At the root is the robot configuration manifold, and task manifolds are at the leaves. Fig. 4: Commutative diagram for a PBDS system applied to a task map This structure can be exploited to parallelize computation of contributions between tangent bundles. from various tasks and reuse computation from parent nodes. GDSs could be combined through weighted optimization, in Under assumptions (A1)-(A2), as we show in Appx. C.III, that case by using Riemannian Motion Policies (RMP) [20]. the multi-task PBDS is geometrically well-defined and has In this section, we will explain the key difficulties with the a unique smooth solution. This gives us a dynamical system RMPflow approach (which we will refer to as GDS/RMP) on M that provides our robot acceleration policy: and how they are resolved by our multi-task PBDS policies. k !† k ! X > a X > a A. Geometric Consistency σ¨ = Jfi wi Jfi Jfi wi Ai , (6) i=1 i=1 The main shortcoming of the GDS framework is that it −1 ˙ Ai = g (F D,i − ∇Φi) − (Jf − Ξi)σ˙ is in general not geometrically consistent. Geometric con- i i (7) (Ξ ) = (Jf ) (Γ )k (Jf ) σ˙ r sistency is an essential property for any geometric method, i kj i `j i `h i hr ensuring its properties hold on non-Euclidean manifolds (i.e. where ∇ in this context is the Euclidean gradient operator, outside a single locally-Euclidean patch) and are invariant to a ni×ni and where wi ∈ R is the lower-right quadrant of changes in coordinates. To see where geometric consistency 2ni×2ni the local matrix wi ∈ R . In practice, this is the is lost, we can derive the GDS framework as a modification only quadrant necessary to design due to the vertical bundle of a PBDS applied to tangent bundles as follows. projections in (3) and (4). For a given task map f : M −→ N, we define a higher- Next, we use LaSalle’s invariance principle to demonstrate order task map F : TM −→ TN as in Sec. III. Since tangent stability about a set of robot equilibrium states. This requires bundles also have a manifold structure, we can build a PBDS a Lyapunov function, which we build adapting known results on F using the process in Sec. III, now operating on the in Lagrangian mechanics to the multi-task PBDS. In partic- higher-order manifolds shown in Fig. 4. However, now it is ular, we rely on one more key assumption: difficult to extract a robot acceleration policy corresponding (A3) The combined dissipative forces corresponding to to a curve on the robot manifold M because the PBDS tasks having nonzero weights are strictly dissipative. curves σ have moved to TM. In particular, σ(t) ∈ TM now Like the others, (A3) can be naturally satisfied (see the contains both robot positions and velocities, so σ¨(t) contains both acceleration and jerk. The GDS tackles this problem discussion of (A2)). Now consider the Lyapunov function ∗ i v by projecting the geometric acceleration F Dσγ = a ∂i + candidate on the robot tangent bundle TM: i a v κ ∂i onto the first half of the basis vectors ∂i to remove k X 1 ∗ 2 components that would correspond to jerk. Unfortunately, V (p, v) = (fi dfi)p(v) (f ∗g ) + Φi ◦ fi(p). this is a projection onto a horizontal bundle, and horizontal 2 i i p i=1 bundles are necessarily dependent on the choice of chart. In In Appx. C.III, we show that V˙ (σ0(t)) < 0 outside equilib- other words, it is impossible to define this projection globally. rium states, leading to the following stability results: In particular, a clear problem arises when considering a change of coordinates on the projected Riemannian task Theorem III.1 (Global Stability of Multi-Task PBDS). Let metric for the GDS. Consider a metric g on TN which can {(fi, gi, Φi, FD,i, wi)}i=1,...,k be a multi-task PBDS where ˜ v i j a i j be represented in chart C as g˜ijdv˜ dv˜ +g ˜ijda˜ da˜ , so that Φi ◦ fi are proper maps. Then given the Lyapunov function v a V above, the multi-task PBDS satisfies these: g˜ = blockdiag(g˜ , g˜ ). Let φ be the transition function from a new chart Cˆ to C˜. The Jacobian of φ has the form • The sublevel sets of V are compact and positively invariant for (5), so that every solution curve σ is  Jφv(ˆp, vˆ) 0  Jφ(ˆp, vˆ) = , defined in the whole interval [0, +∞). Jφav(ˆp, vˆ) Jφa(ˆp, vˆ) • For every β > 0, every solution curve σ in M starting j −1 −1 v a ∂p˜ at σ˙ (0) ∈ V ([0, β]) converges in V ([0, β]) as t → Jφij(ˆp, vˆ) = Jφij(ˆp, vˆ) = (ˆp), +∞ to an equilibrium state (p, 0) ∈ TM such that ∂pˆi grad Φ (p) = 0 if w (p, 0) 6= 0. ∂2p˜j i i Jφav(ˆp, vˆ) =v ˆk (ˆp). ij ∂pˆi∂pˆk IV. COMPARISON TO RIEMANNIAN MOTION POLICIES ˆ ON GEOMETRIC DYNAMICAL SYSTEMS The metric coordinates in C can then be found by As mentioned, prior work proposed a framework called gˆ = Jφ>gJφ˜ RMPflow which presented Geometric Dynamical Systems Jφv>g˜vJφv + Jφav>g˜aJφav Jφav>g˜aJφa = (GDS) for designing dynamical systems on task manifolds Jφa>g˜aJφav Jφa>g˜aJφa that could be “pulled back” onto the robot manifold to gener- ate a robot motion policy [17]. Similar to this work, multiple so that gˆv = Jφv>g˜vJφv + Jφav>g˜aJφav. Since g˜a 6= 0 by the properties of a metric, this shows that a projection onto equations for a set of uniformly weighted fi = Id tasks: v R g cannot be maintained through coordinate transformations. P (1 + Ξa)((ga)−1(F − ∂ Φ ) − Ξvσ˙ ) Since the projected geometric acceleration depends on gv, the i i i D,i xi i i σ¨ = P a 2 policy will change depending on the choice of coordinates i(1 + Ξi ) and will thus exhibit unnatural behavior (e.g., undesirable v 1 a −1 a a 1 a −1 a Ξi = 2 (gi ) ∂xi gi σ,˙ Ξi = 2 (gi ) ∂vi gi σ.˙ geodesics) on non-Euclidean manifolds. This shows that a velocity-dependent metric modifies both To make this concrete, consider our Table I example on the “force” of the task (its contribution to the numerator a sphere, omitting obstacle avoidance. To see the effect of of the policy equation) and the total “mass” of the system coordinate choice, we run both PBDS and GDS policies (the denominator). However, while modifying a task force is using different schemes for choosing coordinate charts on desirable, modifying the total mass is not. 2. The resulting trajectories can be see in Fig. 5a and b. S For example, consider using a velocity-dependent metric Here, the suboptimal and inconsistent behavior of the GDS to enforce constraints. For position constraints, if a task policy demonstrates the importance of geometric consistency. metric increases as the robot approaches the constraint with With such consistency, the policy can accurately capture and velocity towards it, the total mass increases. This indeed leverage the geometry of the manifold, as shown in the PBDS makes it hard for the robot to accelerate further towards example. Without it, the policy instead introduces artifacts the constraint, but likewise it becomes hard for the robot to that disturb natural motion on the manifold and can lead to accelerate away. Similarly for velocity constraints, a barrier- erratic behavior. In some cases it is possible for the GDS type velocity metric can enforce velocity limits, but it then policy designer to engineer tasks within a given coordinate becomes difficult for other tasks to remove the high inertia choice that suppresses such behavior, but for non-Euclidean of the robot near the velocity limit. metrics this will often be a struggle against the locally- Thus rather than applying the PBDS framework to tasks defined curvature terms introduced by the GDS formalism. mapping from the robot configuration tangent bundle TM, B. Task Prioritization and Velocity-Dependent Metrics we instead nominally consider tasks mapping from the base robot manifold M (i.e., using only position-dependent met- The main motivation for building GDSs on tangent bun- rics to design individual task behaviors), greatly simplifying dles is to define velocity-dependent metrics on task tangent the framework with little practical loss in expressiveness of bundles which can be used both to drive the GDS dynamics tasks. Additionally, leveraging (O2), we lose no expressive- and enable velocity-dependent prioritization of tasks in the ness in defining velocity-dependent prioritization of tasks RMP framework. However, this introduces difficulty for the by defining separate pseudometrics on the TNi to perform designer in forming task metrics which both give correct the weighting. This also removes the difficulty of achieving single-task behavior and weight each task correctly against desired task behavior and task prioritization with a single all other tasks. Indeed, rather than allowing incremental metric, thus resulting in a more modular framework. design and modular combination of tasks, it can require jointly redesigning tasks for every new task combination. C. Metric-based Constraints To resolve this issue, the PBDS framework leverages two As mentioned, constraints imposed solely by Riemannian observations: (O1) There are few practical scenarios where a metrics rather than repulsive potentials can eliminate the spu- velocity-dependent Riemannian metric is useful for driving rious local minima in combined potential fields characteristic task dynamics, and (O2) there is no need to use a single of many APF methods. However, this has not been consid- metric for both task dynamics and task prioritization. ered in works employing the GDS/RMP framework [21], We can gain intuition for (O1) by considering a mod- [24], [25]. The notion of constraint-enforcing Riemannian ified PBDS applied to tangent bundle tasks. For brevity, metrics was recently considered in [18]. However, rather than we outline the main modifications here and leave further constructing such metrics explicitly, the approach numeri- details to Appx. D. Let Fi : TM −→ TNi be our task cally computes a distance field from the goal considering maps. To avoid projecting onto the horizontal bundle as in obstacles, interpreting this as a geodesic distance field whose the GDS formulation, we instead wish to project onto the gradient is then used to guide trajectory optimization. Such vertical bundle, an operation that is globally well-defined. a technique is computationally intensive and may not be However, to continue recovering an acceleration policy rather ˆ practical for complex, dynamic scenarios. than a jerk policy, we consider permuted task maps Fi , Instead, we propose a class of simple analytical Rieman- 2di ˆ σTNi ◦ Fi : TM −→ R defined locally as Fi(p, v) = nian behavior metrics that provide tunable enforcement of ¯ ¯ ((dfi)p(v), fi(p)). This essentially swaps the position and constraints within the PBDS framework. To motivate the velocity coordinates, an operation which is globally well- form of these metrics, consider again a set of fi : R −→ R defined on parallelizable manifolds (e.g., Lie groups) having tasks, this time using the PBDS motion policy of (6): an embedding ϕ¯ : N −→ di , giving f¯ =ϕ ¯ ◦ f . Then i i R i i i P a −1 ˙ by considering curves in the corresponding pullback bundles i Jfiwi ((gi) (FD,i − ∂xi Φi) − (Jf i + Ξi)σ ˙ ) σ¨ = P 2 a . satisfying a system analogous to (5) with the appropriate i Jfi wi global projections applied, we arrive at the following ODE For constraint tasks, we let f be a distance function to for a curve σ in configuration manifold M: i the constraint boundary, so that xi ∈ R+ is the distance  †   1 −1 P > a P > a ¯ v to the constraint and Ξi = (gi) ∂x giσ˙ . Since we add σ¨ = Ci w Ci Ci w Ai , Ci = Jfi + Ξ 2 i i i i i i no repulsive potential and offload dissipative forces to other −1 a ˙¯ a Ai = (gi ) (F D,i − ∇Φi) − (Jfi − Ξi )σ˙ tasks, constraints are enforced through Ξi. In short, designing Ξv = JFˆ Γi+d Jf¯ σ˙ `, Ξa = JFˆ Γi+d Jf¯ σ˙ `. Ξi as a negative barrier function at the constraint boundary ij kj k(h+m) h` ij k(j+d) k(h+m) h` will slow negative velocities as desired. However, due to the It is now instructive to see the corresponding local policy requirements of gi and the form of Ξi, a simple logarithmic 2 (a) PBDS geometric consistency test (b) GDS/RMP geometric consistency test (c) PBDS attractor and obstacle avoidance policy on S 2 Fig. 5: (a) and (b) show a geometric consistency test using a point attractor on S and three schemes for choice of chart: (Red) Always using south pole stereographic projection, (Blue) Always using north pole stereographic projection. (Purple) Switching stereographic projections depending on the sphere hemisphere. The blue and green dots show the start and goal points, respectively. Note that using PBDS, all coordinate choices result in the same trajectory. (c) shows resulting trajectories from a PBDS attractor policy with obstacles added, where the curve colors represent different starting velocity directions. or inverse barrier gi is insufficient. In particular, gi = log xi Algorithm 1: Multi-Task PBDS Policy is negative for x < 1, and g = a/xb always results in Ξ = i i i i 1: Input: Robot position and velocity (σ, σ˙ ) −b/x for all a, b ∈ , which has minimal tuning capability i R 2: Output: Robot acceleration command σ¨ a/xb x (i.e. i cannot decrease fast enough with decreasing i). 3: Data: Task PBDSs (f , g , Φ , F ), weights w There are many suitable options, but a good candidate is i i i D,i i 4: Compute Ai using (7) for each task b gi(xi) = exp(a/(bxi )) 5: Compute combined acceleration policy σ¨ using (6) a > 0 b > 1 for , , which results in a familiar and flexible such as in Fig. 3 allows significant reuse of computation. Ξ = −a/xb−1 barrier function i i . This requires careful segmenting and recombination of the Additionally, we must weight constraint tasks such that main equations (6) and (7) (details in Appx. B). x˙ < 0 they are only active when i , both to save computa- For demonstration of PBDS on a complex robotic task, tion and because such metric-based constraints can produce we considered a 7-DoF Franka Panda arm grasping a mug an undesired acceleration away from the constraint when in a dynamic, cluttered environment (see video at bit.ly/pbds- x˙ > 0 i . This can be accomplished using a pseudometric vid). For mechanism modelling and visualization, we used 1 if x˙ > 0 and x < β, packages from JuliaRobotics [26]. For the PBDS policy, we wa(x , x˙ ) = i i (8) i i i 0 otherwise, used attractor, obstacle avoidance, and joint-limit policies on Euclidean task manifolds and considered damping on S2 β ∈ 1 where R+ activates the constraint avoidance within and S for gripper location around the mug and along the some proximity. This works well numerically, but smooth mug rim. Along with providing natural movement through approximations can be used to retain smoothness guarantees. obstacles, the fast Julia implementation allowed the robot to The effectiveness of this metric and pseudometric combi- react quickly to changes in the environment by computing nation can be seen in our full Table I running example with policy outputs at a rapid 300-500 Hz, a speed which could results shown in Fig. 5c, where each obstacle has a task with be accelerated further by exploiting the clear opportunities a task map fi giving the Euclidean distance to the obstacle for parallelization offered by the PBDS framework. 3 2 in the ambient R , metric gi = exp(1/xi ), and weighting pseudometric wi as defined in (8) with β = ∞. VI.CONCLUSIONS In conclusion, we have provided a fast, easy-to-use, and V. IMPLEMENTATION AND ARM EXPERIMENTS geometrically consistent framework for generating motion We implemented the PBDS framework and the GDS/RMP policies on non-Euclidean robot and task spaces. We have framework for comparison in a fast Julia package called also shown how PBDS can leverage Riemannian metrics PBDS.jl2. Despite the apparent complexity of its geomet- for simple constraint enforcement without generating unde- ric formulation, the PBDS algorithm in implementation sirable potential function local minima. We highlight that is quite simple and is summarized in Alg. 1. The main although designing tasks on non-Euclidean manifolds can challenge in handling non-Euclidean manifolds within the appear daunting, in practice the design is often trivial (Table PBDS framework is transitioning coordinate representations I), and part of the strength of PBDS lies in its ability of the necessary objects between different coordinate charts to seamlessly integrate different tasks that are designed in and embeddings. However, PBDS.jl is designed to handle isolation. We also stress that a rich, diverse set of complex these transitions automatically. To increase performance in behaviors can be produced from combinations of a handful complex tasks, PBDS.jl also implements a computational tree of common task design patterns. Future work for PBDS inspired by [17] for cases where a task map tree structure includes extensions to a broader class of constraints (e.g., velocity, acceleration, and control limits) and to tasks defined 2Available at https://github.com/StanfordASL/PBDS.jl on discrete structures such as manifold triangle meshes. REFERENCES

[1] O. Khatib, “Real-time obstacle avoidance for manipulators and mobile robots,” in Proc. IEEE Conf. on Robotics and Automation, 1985. [2] P. Khosla and R. Volpe, “Superquadric artificial potentials for obstacle avoidance and approach,” in Proc. IEEE Conf. on Robotics and Automation, 1988. [3] N. Hogan, “Impedance control: An approach to manipulation,” in American Control Conference, 1984. [4] S. M. Khansari-Zadeh and A. Billard, “A dynamical system approach to realtime obstacle avoidance,” Autonomous Robots, vol. 32, no. 4, pp. 433–454, 2012. [5] A. J. Ijspeert, J. Nakanishi, H. Hoffmann, P. Pastor, and S. Schaal, “Dynamical Movement Primitives: Learning attractor models for mo- tor behaviors,” Neural Computation, vol. 25, no. 2, pp. 328–373, 2013. [6] N. Ratliff, M. Toussaint, and S. Schaal, “Understanding the geometry of workspace obstacles in motion optimization,” in Proc. IEEE Conf. on Robotics and Automation, 2015. [7] J. Agirrebeitia, R. Aviles,´ I. F. de Bustos, and G. Ajuria, “A new APF strategy for path planning in environments with obstacles,” Mechanism and Machine Theory, vol. 40, no. 6, pp. 645–658, 2005. Fig. 6: Example experimental scenario for Franka Panda arm reaching [8] J. Schulman, J. Ho, A. Lee, I. Awwal, H. Bradlow, and P. Abbeel, through clutter to grasp a mug. The blue and black boxes are obstacles, and “Finding locally optimal, collision-free trajectories with sequential the green and red spheres on the arm represent the collisions for different convex optimization,” in Robotics: Science and Systems, 2013. robot links. [9] S. Kuindersma, R. Deits, M. Fallon, A. Valenzuela, H. Dai, F. Per- APPENDIX A menter, T. Koolen, P. Marion, and R. Tedrake, “Optimization-based locomotion planning, estimation, and control design for the Atlas FURTHER EXPERIMENTAL DETAILS humanoid robot,” Autonomous Robots, vol. 40, no. 3, pp. 429–455, In this section, we provide further details of the design of 2015. [10] W. Merkt, V. Ivan, and S. Vijaykumar, “Continuous-time collision the PBDS policy driving our robot arm grasping experiments, avoidance for trajectory optimization in dynamic environments,” in including details of task design on S1 and S2, which may IEEE/RSJ Int. Conf. on Intelligent Robots & Systems, 2019. serve as a useful reference for practitioners. [11] E. Rimon and D. E. Koditschek, “Exact robot navigation wsing Since all seven joints of the Franka Panda arm have artificial potentia functions,” IEEE Transactions on Robotics and 7 Automation, vol. 8, no. 5, pp. 501–518, 1992. joint limits, the robot configuration manifold was M = R [12] J.-O. Kim and P. K. Khosla, “Real-time obstacle avoidance using (note that for revolute joints without joint limits, the circle harmonic potential functions,” IEEE Transactions on Robotics and 1 should be used instead). For collision avoidance, we Automation, vol. 8, no. 3, pp. 338–349, 1992. S [13] L. Huber, A. Billard, and J.-J. Slotine, “Avoidance of convex and modelled a conservative collision hull around each link using concave obstacles with convergence ensured through contraction,” a set of spheres, as shown in Fig. 6. IEEE Robotics and Automation Letters, vol. 4, no. 2, pp. 1462–1469, 2019. I. Task Set for Grasping a Mug [14] Y. Koren and J. Borenstein, “Potential field methods and their inherent The full list of tasks for this PBDS policy and their limitations for mobile robot navigation,” in Proc. IEEE Conf. on Robotics and Automation, 1991. corresponding task spaces is as follows. [15] D. H. Kim, “Escaping route method for a trap situation in local path Damping: planning,” Int. Journal of Control, Automation and Systems, vol. 7, • R : Robot arm joint damping no. 3, pp. 495–500, 2009. 2 [16] J. M. Lee, Introduction to Riemannian Manifolds, 2nd ed. Springer, • S × R : Distance-toggled damping of gripper around 2018. mug 1 [17] C.-A. Cheng, M. Mukadam, J. Issac, S. Birchfield, D. Fox, B. Boots, • S × R : Distance-toggled damping of gripper along and N. Ratliff, “RMPflow: A computational graph for automatic motion policy generation,” in Workshop on Algorithmic Foundations mug rim of Robotics, 2018. Constraints: [18] J. Mainprice, N. Ratliff, M. Toussaint, and S. Schaal. (2020) An • : Joint limits (one for each joint) interior point method solving motion planning problems with narrow R+ passages. Available at https://arxiv.org/abs/2007.04842. • R+ : Obstacle avoidance (one for each pair of obstacle [19] N. D. Ratliff, K. Van Wyk, M. Xie, A. Li, and M. A. Rana. (2020) and link collision hull sphere) Optimization fabrics. Available at https://arxiv.org/abs/2008.02399. • R+ : Self-collision avoidance (one for each pair of link [20] N. D. Ratliff, J. Issac, D. Kappler, S. Birchfield, and D. Fox. (2018) collision hull spheres having the potential to collide) Riemannian Motion Policies. Available at https://arxiv.org/abs/1601. 04037. Attractors: 2 [21] M. A. Rana, A. Li, H. Ravichandar, M. Mukadam, S. Chernova, • R : Distance-toggled attractor above mug D. Fox, B. Boots, and N. Ratliff, “Learning reactive motion policies in 2 multiple task spaces from human demonstrations,” in Conf. on Robot • R : Distance-toggled gripper axis alignment attractor 2 Learning, 2020. • R : Distance-toggled grasping angle alignment attrac- [22] J. M. Lee, Introduction to Smooth Manifolds, 2nd ed. Springer, 2013. tor 2 [23] F. Bullo and A. D. Lewis, Geometric Control of Mechanical Systems. • : Distance-toggled grasp completion attractor Springer-Verlag, 2004. R [24] A. Li, C.-A. Cheng, B. Boots, and M. Egerstedt, “Stable, concurrent To prevent premature grasping and handle dynamic move- controller composition for multi-objective robotic tasks,” in Proc. ment of the mug, we implement a simple two-state machine IEEE Conf. on Decision and Control, 2019, Available at https://arxiv. which uses the above-mug attractor task above while the org/abs/1903.12605. [25] M. Mukadam, C.-A. Cheng, D. Fox, B. Boots, and N. Ratliff, “Rie- mug is moving faster than some threshold velocity and mannian Motion Policy fusion through learnable Lyapunov function until the end-effector nears the attractor goal. Otherwise reshaping,” in Conf. on Robot Learning, 2020. the other three attractor tasks are used instead. Note that [26] T. Koolen and R. Deits, “Julia for robotics: Simulation and real-time control in a high-level programming language,” in Proc. IEEE Conf. although in this state we have multiple potentials, one for on Robotics and Automation, 2019. each attractor task, they are easily designed to be always non- [27] M. W. Hirsch, Differential Topology. Springer, 1976. conflicting (i.e. the inner product of their resulting desired Rn+1 constraint tasks, but as in the grasping scenario, it can also be useful for attractor and damping tasks. ϕ¯ To do this, one can simply form a new task map which is ϕ¯ ϕ¯ the product of the original task map and the distance function + − used for toggling. Thus, the new task manifold becomes the Sn product of the original task manifold and R, i.e. R becomes R2 and S becomes S2 × R. (Note that in PBDS.jl, we ϕ+ ϕ− have implemented product task maps to conveniently handle n n heterogeneous manifolds like S2 × R). Given, one can form R −1 R ϕ− ◦ ϕ+ weights w for the task which are a function of the new task variable to toggle the task as desired. 2 Fig. 7: Commutative diagram for the two stereographic charts of S and its 2 embedding in Euclidean space. (Here we omit the subsets U+,U− ⊂ S APPENDIX B which are the domains of the respective chart maps ϕ , ϕ ). + − COMPUTATIONAL TREEFOR MULTI-TASK PBDS robot accelerations is never negative), and thus they produce Here we give a derivation and details for implementation no undesirable local minima. of the PBDS computational tree for reusing computation II. Tasks on Sn in multi-level trees of task maps. Consider a set of task n maps forming a task map tree such as in Fig. 3. In such For designing tasks on S for some n, it is often natural a case, multiple task maps are composite maps which share to consider 2 as being embedded in an ambient Euclidean S components (e.g., f1,1 ◦ f1 and f1,2 ◦ f1 share f1). Thus space and design the necessary components (g, FD, Φ, w) in cases where applying component maps and computing there, as we also do in the example in Table I. These Jacobians and Jacobian derivatives is expensive, it is useful components in the ambient space then induce corresponding n to exploit this tree structure to reuse computation while components on S . Thus, it is important to be able to computing the multi-task PBDS acceleration policy output. compute the induced versions of these components. The same Thus we propose quantities denoted K, A, B, F, and reasoning applies to other manifolds which have a natural n ξ which can be numerically pulled up the tree from the embedding in Euclidean space, but here we will consider S leaves and combined into equation (6). Noting that each leaf as it is a good example used in both the grasping and Table I corresponds to a different task, we initialize these quantities scenarios. n for each leaf as follows. The n-sphere S can be naturally embedded as the unit Leaf task manifold nodes: sphere in n+1, using an embedding ϕ¯ : n −→ n+1. We R S R > a > a ˙ can consider two stereographic projection charts on Sn with K = B = Jf w Jf, A = Jf w Jf n n n chart maps ϕ+ : U+ ⊂ S −→ R and ϕ− : U− ⊂ S −→ F = Jf >wag−1(F − ∇Φ) n representing the south and north pole charts, respectively. D R q a η Then we can define corresponding maps ϕ¯+ and ϕ¯− from ξsr = JfτqwτηJfαsΓαβJfβr, chart coordinates to embedded coordinates as follows: where f is the map to the task manifold from the immediate n 2xi X parent, and we locally compute the task metric g, Christoffel ϕ¯i (x) =ϕ ¯i (x) = , a = 1 + (xi)2 + − a symbols Γ, generalized force FD, potential gradient ∇Φ, and i=1 block wa of the weighting pseudometric w as usual. Next, 2 − a ϕ¯n+1(x) = −ϕ¯n+1(x) = . for parent nodes representing intermediate manifolds in the + − a composite task maps, we combine the child quantities as There is also a chart transition map between the chart follows. −1 coordinate representations defined by ϕ+ ◦ ϕ− (x) = ϕ− ◦ Intermediate manifold nodes: −1 2 X  X  ϕ+ (x) = x/kxk2. These maps and the associated spaces K = Jf > K Jf, B = Jf > B Jf are summarized in Fig. 7. We can construct similar maps for c c 2 2n+2 T S embedded in R . > X  X  ˙  A = Jf Ac Jf + Bc Jf Now given a behavior metric g¯, dissipative forces F¯D, n+1 q and potential function Φ defined on R and weighting > X k X  2n+2 F = Jf F c, ξh` = JfqkJfsh ξc Jfr`, pseudometric w defined on defined on R , the pullbacks sr of these objects through ϕ¯+ and ϕ¯− give their coordinate rep- where the c subscripts indicating quantities from the child resentations in the south and north pole charts, respectively. nodes, summations are over all child nodes, and f again In particular, we can compute the relevant components in the denotes the map to the node from the immediate parent. south pole chart by: Finally at the root node containing the robot configuration > > ¯ g+ = Jϕ¯+gJ¯ ϕ¯+, F D+ = Jϕ¯+F D manifold, we combine as follows. Root robot manifold node: ¯ ¯ a ¯> ¯a ¯ Φ+ = Φ ◦ ϕ+, w+ = Jϕ+w Jϕ+. X X X K = K , A = A , F = F Components in the north pole chart representation can be c c c k computed similarly. X  h ξk` = ξc σ˙ . III. Distance-Toggled Tasks h` We can now compute the multi-task PBDS acceleration In some scenarios it is useful to switch tasks on or off policy output using the root node quantities as based on some distance using the weighting pseudometrics. We have already seen an example of this in Sec. IV-C for σ¨ = K†(F − (A + ξ)σ˙ ). (9) b b  ci  M X X > X F = F i = Jfi Fi,j f1 fb i=1 i=1 j=1

b ci X X > > a −1
N1 ... Nb = Jfi Jfi,jwi,jgi,j F D,i,j − ∇Φi,j , i=1 j=1 f1,2 f1,c1 fb,1 fb,cb  b k X h N1,2 ... N1,c1 Nb,1 ... Nb,cb ξk` = ξi σ˙ i=1 h` Fig. 8: Task map tree for an example multi-task PBDS. b  ci q X X h We now demonstrate that this strategy of splitting and reusing = (Jfi)qk(Jfi)sh ξi,j (Jfi)r`σ˙ computation correctly reconstructs (6). i=1 j=1 sr

Lemma B.1 (Computational Tree). Given a multi-task PBDS b ci X X a and a corresponding tree of task maps, computing K, A, F, = (Jfi)qk(Jfi,j)τq(wij)τη(Jfi)sh(Jfi,j)αs and ξ at the root node gives i=1 j=1 † η h K (F − (A + ξ)σ˙ ) (Γi,j)αβ(Jfi,j)βr(Jfi)r`σ˙ † ! ! b ci X > a X > a X X > > a
= Jfi wi Jfi Jfi wi Ai = Jfi Jfi,jwi,j kη(Jfi,jJfi)αh i i i=1 j=1 η h (Γi,j) (Jfi,jJfi)β`σ˙ Proof. Consider the two-level task tree in Fig. 8, having αβ b ci b intermediate manifold nodes and ci children for the ith X X > > a
Pb = Jfi Jfi,jwi,j J(fi,j ◦ fi)αh intermediate node, for a total of i=1 ci leaf nodes and kη corresponding task maps. For such a multi-task PBDS, we i=1 j=1 η h can expand (6) as (Γi,j)αβJ(fi,j ◦ fi)β`σ˙ † b c  b c  i X Xi X X > > a
h > > a = Jfi Jfi,jwi,j (Ξi,j)ηhσ˙ σ¨ =  Jfi Jfi,jwi,jJfi,jJfi kη i=1 j=1 i=1 j=1 b ci b c i   X X > > a X X > > a −1 = Jfi Jfi,jwi,jΞi,jσ˙ . Jfi Jfi,jwi,j gi,j F D,i,j − ∇Φi,j i=1 j=1 i=1 j=1 ! !! Combining these in (9), we see that we indeed recover the d   − Jf Jf + Ξ σ˙ , correct robot acceleration policy from (6). dt i,j i i,j APPENDIX C where we use the i subscript to denote quantities associated MATHEMATICAL DETAILS FOR PULLBACK BUNDLE to the intermediate nodes and use the i, j subscript for those DYNAMICAL SYSTEMS associated to the leaf nodes and their corresponding tasks. In this section, we give a detailed derivation of Pullback Now unpacking each of the terms in (9) at the root robot Bundle Dynamical Systems and extraction of multi-task poli- manifold node, we have cies, showing that each of the key objects are geometrically b b  ci  well-defined. X X > X K = Ki = Jfi Ki,j Jfi Let M and N be smooth m- and n-dimensional robot con- i=1 i=1 j=1 figuration and task manifolds, having canonical projections

b ci πM and πN , respectively. Let f : M −→ N be a smooth X X > > a task map and let ∇ be the Levi-Civita connection in TN = Jf Jf w Jfi,jJfi, i i,j i,j g N i=1 j=1 corresponding to metric on with Christoffel symbols k 1 kh b Γij = g (∂igjh + ∂jgih − ∂hgij). X 2 A = Ai We form a pullback bundle f ∗TN as defined in (2), which i=1 is a smooth n-vector bundle over M and itself an (m + n)- b c c !  i   i  dimensional manifold. X > X ˙ X = Jfi Ai,j Jfi + Bi,j Jfi i=1 j=1 j=1 I. Constructions on the Pullback Bundle ∗ b ci Note that since f TN is not a tangent bundle, we must X X >  > a ˙ > a ˙  = Jfi Jfi,jwi,jJfi,jJfi + Jfi wi,jJfi,jJfi reconstruct all of the standard objects and operators normally i=1 j=1 used for defining mechanical systems on tangent bundles. We first denote the pullback bundle projection as f ∗π , and for b ci N X X > > a d   convenience we define a pullback differential = Jfi Jfi,jwi,j Jfi,jJfi , dt ∗ ∗ i=1 j=1 f df : TM −→ f TN :(p, v) 7→ (p, π2(dfp(v))) and the pullback of the identity map on TN a smooth curve, and denote the space of pullback bundle ∗ ∗ vector fields over σ f IdTN : f TN −→ TN :(p, v) 7→ (f(p), v), f ∗X(σ) {V : I −→ f ∗TN | V both of which are smooth and globally well-defined. , is smooth, ∗ We now construct a global pullback connection on f ∗TN: V (t) ∈ f Eσ(t), ∀t ∈ I}. Lemma C.1 (Pullback Connection). For every p ∈ M, As a classical result, for every curve σ in M, the pullback choose Christoffel symbols defined locally as connection f ∗∇ defines a unique acceleration operator ` ∗ ∗ ∗ ∗ k ∂f k f Dσ : f X(σ) −→ f X(σ), f Γij(p) = (p)Γ`j(f(p)). ∂xi which locally takes the following form: These functions are globally extendable and form a global ∗  ˙ k i j ∗ k  ∗ connection (which we call the pullback connection), f DσV (t) = V (t) +σ ˙ (t)V (t)f Γij(σ(t)) f ∂k|σ(t), ∗ ∗ ∗ (10) f ∇ : Γ(TM) × Γ(f TN) −→ Γ(f TN). ∗ ∗ ∗ where (f ∂i) is the frame for f TN defined by f ∂i(p) = Proof. The chosen Christoffel symbols are locally smooth. (p, π2(∂i(f(p))). To show they are global, we must check whether they satisfy Now we construct a pullback metric on f ∗TN. Note that the chart transition formula for Christoffel symbols on vector Lyapunov-type results for classical mechanical systems rely bundles. on the fact that the Levi-Civita connection is compatible Consider two charts at p ∈ M denoted by C and C˜ M M with the related Riemannian metric. Indeed, this allows one and two charts at f(p) ∈ N denoted by CN and C˜N . Then ∂ ∂ to differentiate the norm of some energy, obtaining such TM and TN have corresponding local frames ( ∂xi ), ( ∂x˜i ) ∂ ∂ variations as a function of the evolutionary equation of the and ( ∂yi ), ( ∂y˜i ) respectively. Suppose we also have a vector associated mechanical system. Thus, it is critical to reproduce bundle E over N (note that in our case we have E = TN). such a property in our framework to achieve stability. Given local frames (Ei) and (E˜i) for E,e there exists a smooth, non-singular matrix of functions (Aij) such that Lemma C.2. Let g be a Riemannian metric on N which is compatible with connection ∇. Then the pullback metric locally E˜i = AijEi. ∗ ∗ ∗ Now given a connection ∇ : Γ(TN) × Γ(E) −→ Γ(E), f g : f TN × f TN −→ R the related Christoffel symbols can be denoted by Γk and ij ((p, v1), (p, v2)) 7→ gf(p)(v1, v2) ˜k Γij The chart transition formula for such Christoffel symbols ∗ is is compatible with the pullback connection f ∇. ∂yq  ∂A  Γ˜k = Apk A Γp + jp . ij ∂y˜i jr qr ∂yq ∗ k We must now show a similar formula holds for f Γij. Given the local frames for E above, we can construct local frames Proof. Since g and ∇ are compatible, we know they satisfy ∗ ∗ for the pullback bundle f E as (f Ei) , (Ei ◦ f) and the compatibility condition ∗ ˜ ˜ ∗ ˜ ∗ (f Ei) , (Ei ◦ f). Then for f Ei = Bijf Ej, it is easily d B = A ◦ f gρ(t)(X(t),X(t)) = 2gρ(t)(DρX(t),X(t)) (11) seen that ij ij . We can now compute dt ˜` ∗ k ∂f k for any smooth vector field X along any smooth curve ρ in f Γ˜ = (p)Γ˜ (f(p)) ∗ ∗ ij i `j N. To show the same for f g and f ∇, let σ : I −→ M ∂x˜ ∗ ∂yq ∂f˜` be a smooth curve and let V ∈ f X(σ). Then ρ : I −→ N : pk t 7→ f(σ(t)) and W : I −→ TN : t 7→ f ∗Id (V (t)) are = ` (f(p)) i (p)A (f(p)) TN ∂y˜ ∂x˜ well-defined and smooth. Now using (11) we obtain   p ∂Ajp A (f(p))Γ (f(p)) + (f(p)) d ∗ jr qr q f g (V (t),V (t)) ∂y dt σ(t) ∂x` ∂f q d = (p) (p)Bpk(p) = g (f ∗Id (V (t)), f ∗Id (V (t))) ∂x˜i ∂x` dt f(σ(t)) TN TN  ∂A  d B (p)Γp (f(p)) + jp (f(p)) = g (W (t),W (t)) jr qr ∂yq dt ρ(t) ∂x` = 2gρ(t) (DρW (t),W (t)) . (12) = (p)Bpk(p) ∂x˜i Applying (10) adapted for Dσ gives  q q    ∂f p ∂Ajp ∂f ˙ k ` j k B (p) (p)Γ (f(p)) + (f(p)) (p) DρW (t) = W (t) +ρ ˙ (t)W (t)Γ (ρ(t)) ∂k|ρ(t) jr ∂x` qr ∂yq ∂x` `j  `  `   k ∂f i j k ∂x pk ∗ p ∂Bjp = W˙ (t) + (σ(t))σ ˙ (t)W (t)Γ (ρ(t)) ∂ | = (p)B (p) B (p)f Γ (p) + (p) i `j k ρ(t) ∂x˜i jr `r ∂x` ∂x  ˙ k i j ∗ k  ∗ k = W (t) +σ ˙ (t)W (t)f Γ (σ(t)) ∂k|ρ(t) We therefore find that the Christoffel symbols f Γij can be ij extended globally and the result follows. ∗  ˙ k i j ∗ k   = f IdTN V (t) +σ ˙ (t)V (t)f Γij(σ(t)) ∂k|σ(t) This naturally leads to a geometric acceleration operator ∗ ∗ for curves with velocity in f ∗TN. Let σ : I −→ M be = f IdTN (f DσV (t)). Thus continuing (12) finally provides forces to accelerations), we can also define the shorthand ∗ ] ∗ ∗ d ∗ f F(·) : f TN −→ f TN f gσ(t)(V (t),V (t)) dt ∗ ] ∗ ∗ ∗ ∗ (p, v) 7→ f FD(p, v) − f grad Φ(p) = 2gρ(t) (f IdTN (f DσV (t)), f IdTN (V (t))) ∗ ∗ = 2f gσ(t)(f DσV (t),V (t)). II. Local Pullback Bundle Dynamical Systems We finally come to the definition of an important build- ing block for Pullback Bundle Dynamical Systems (PBDS) called a local PBDS. Next we develop the sharp operator on f ∗TN, useful for handling forces. First we define the dual pullback bundle Definition C.1 (Local Pullback Bundle Dynamical System). Let f : M −→ N be a smooth task map for a Riemannian ∗ ∗ a ∗ −1 f T N = (πN ) (f(p)), task manifold (N, g), let Φ: N −→ R+ be a smooth ∗ p∈M potential function, and let FD : TN −→ T N be dissipative ∗ forces. Then for each (p, v) ∈ M, we can choose a curve where πN is the canonical projection for the cotangent ∗ α(p,v) :(−ε, ε) −→ M resulting in γα(p,v) :(−ε, ε) −→ bundle T N. We can now define the flat operator ∗ f TN for some ε > 0 such that (f, g, Φ, FD, α ) forms a ∗ ∗ ∗ ∗ (p,v) [ : f TN −→ f T N :(p, v) 7→ f gp(v, ·), local Pullback Bundle Dynamical System (PBDS) satisfying  which is easily seen to be a smooth bundle isomorphism. f ∗D γ (s) = f ∗F(γ (s))]  α(p,v) α(p,v) α(p,v) The sharp operator is naturally defined to be the inverse of  ∗ 0 PBDS γα (0) = f dfα (0)(α (0)), the flat operator, i.e., α(p,v) (p,v) (p,v) (p,v) α0 (0) = (p, v). ] : f ∗T ∗N −→ f ∗TN :(p, ω) 7→ (p, ω)[. (p,v) ] i ij The reason for this local PBDS definition is to ensure that In particular, locally we have (ω(p) ) = g (f(p))ωj(p). PBDS is well-posed at t = 0 (i.e. where a curve ασ0(0) is Now consider dissipative forces FD : TN −→ ∗ required to define f Dα 0 ) and in cases where the curve T ∗N, which we define satisfying the dissipative property σ (0) 0 ασ (t) defining a valid PBDSασ0(t) for some t ∈ [0, ∞) is hFD(p, v), vi ≤ 0 for all (p, v) ∈ TN, where h·, ·i is not unique. For example, the latter may occur when m > n, the natural pairing between a covector and a vector. For which is often the case in practice. F convenience, we associate to D corresponding pullback Now we must show that a local PBDS exists at each point dissipative forces as in TM. In particular, we must show that for any (p, v) ∈ ∗ ∗ ∗ ∗ f FD : f TN −→ f T N :(p, v) 7→ (p, π2(FD(f(p), v)), TM, there exists a curve α(p,v) satisfying PBDSα(p,v) . If ∗ ] ∗ ∗ (U, ϕ) is a local chart for M centered at p let α (s) such that we have f FD(·) : f TN −→ f TN. From this (p,v) , ∗ −1 m definition, it follows that we have hf FD(p, v), vi ≤ 0 for ϕ (sv), using local coordinates v ∈ R . This curve is all (p, v) ∈ f ∗TN so that these pullback forces inherit the well-defined and smooth for small times around zero, so the dissipative property. Note that the dissipative forces FD can desired curve α(p,v) always exist, meaning the corresponding also be time-dependent and similar results to those shown in local PBDS always exists. this work will hold, but we keep them time-independent for To begin the process of extracting a robot motion policy, simplicity. we can define a map giving the desired pullback task The last object we must construct on f ∗TN is a gradient accelerations from local PBDSs corresponding to each point operator for handling potentials. Given any smooth function in the robot configuration tangent bundle TM: Φ: N −→ , we locally define the smooth map ∗ R G : TM −→ T (f TN):(p, v) 7→ γ˙ α(p,v) (0). (14) ∗ ∗ ∂Φ ∗ i For this map to be well-defined, we must show that it BΦ : M −→ f T N : p 7→ i (f(p))f dy |p, ∂y does not depend on the choice of curves α(p,v). Let α(p,v) ∗ i ∗ ∗ β M where (f dy ) is the coframe for f T N defined by and (p,v) be two curves in satisfying PBDSα(p,v) ∗ i i f dy (p) = (p, π (dy (f(p))) and PBDSβ(p,v) , respectively. These result in corresponding 2 . This map is intentionally ∗ designed such that for every (p, v) ∈ TM we have curves γα(p,v) and γβ(p,v) in f TN as provided in the local PBDS definition, which exist as a consequence of ∗ ∂Φ ∗ i ∗ BΦ(p)(f dfp(v)) = (f(p))f dy |p(f dfp(v))) standard Cauchy-Lipschitz arguments. In particular, since ∂yi every quantity defining a local PBDS is smooth, these curves ! 1 k are at least C , allowing us to compute γ˙ α and γ˙ β ∂Φ ∗ i j ∂f ∂ (p,v) (p,v) = (f(p))f dy |p v (p) pointwise. Thus using (10) we can compute ∂yi ∂xj ∂yk f(p) k ∗ ] k γ˙ (0) = (f F(γα (0)) ) ∂f i ∂Φ ∂(Φ ◦ f) α(p,v) (p,v) j j i j ∗ k = v j (p) i (f(p)) = v i (p) − α˙ (0)γ (0)f Γ (α (0)) ∂x ∂y ∂y (p,v) α(p,v) ij (p,v) ∗ ∗ ] k = d(Φ ◦ f)(p, v) (13) = (f F(f df(α ˙ (p,v)(0))) ) i ∗ j ∗ k which is crucial for our stability analysis. Now we can define − α˙ (p,v)(0)(f df(α ˙ (p,v)(0))) f Γij(α(p,v)(0)) the pullback gradient operator such that ∗ ] k = (f F(γβ (0)) ) ∗ ∗ ] (p,v) f grad Φ: M −→ f TN : p 7→ BΦ(p) . ˙i j ∗ k − β(p,v)(0)γβ (0)f Γij(β(p,v)(0)) To denote the total acceleration contributed by the pullback (p,v) =γ ˙ k (0). forces (using the sharp operator to indicate converting from β(p,v) Therefore we have γ˙ α(p,v) (0) =γ ˙ β(p,v) (0) by the chart every (p, v) ∈ TM, we derive solutions to (15) locally. In invariance of the derivative operator. This shows that G is doing so, we also prove that (15) is smooth. First, in order to ∗ well-defined and, in particular, smooth. unpack Zi(a), we note that the differential d(fi dfi) locally γ0 (0) takes the form Given this result, we will adopt the shorthand vp , γ0 (0) for (p, v) ∈ TM, knowing that suitable choices of   α(p,v) ∗ Jfi(p) 0 J(fi dfi)(p, v) = ˙ , α(p,v) exist. Jf i(p, v) Jfi(p) III. Multi-Task Pullback Bundle Dynamical Systems ˙ where for (p, v) ∈ TM we define Jfi locally as Next, we continue with a detailed derivation for the 2 j extraction of multi-task PBDS policies. For k tasks, consider ˙ ` ∂ fi (Jfi)kj(p, v) = v ` k (p). task maps {fi : M → Ni}i=1,...,k. We equip every ni- ∂x ∂x dimensional task manifold Ni with a Riemannian metric gi, Now for a ∈ D(p,v) we have a potential function Φi, and dissipative forces FD,i. ∗
We can associate to every robot position and velocity Zi(a) = πVB d(fi dfi)(p,v)(a) ∗ k combination vp ∈ TM a smooth section γvp,i : [0, 1] −→ = πVB ((fi dfi)p(v), ((Jfi)jk(p)v , ∗ f TNi satisfying the single-task dynamics of the PBDS ˙ k a k
(Jf i)jk(p, v)v + (Jfi)jk(p)(a ) )) specified by (fi, gi, Φi, FD,i). Extending the construction of ∗ ˙ k a k (14) to an operator Gi for each task, we collect the vectors = ((fi dfi)p(v), (0, (Jf i)jk(p, v)v + (Jfi)jk(p)(a ) ). γ0 (0) = ((p, v), (γ ˙ v (0), γ˙ a (0))) into a globally well- vp,i vp,i vp,i Thus for a ∈ D we can locally compute defined and smooth operator (p,v) 2 kZ (a) − S (p, v)k ∗ ∗
i i F wi (16) Si : TM −→ T fi TNi i a ˙ a 2 = kJfi(p)a + Jf (p, v)v − γ˙ (0)k a . (p, v) 7→ πVB(Gi(p, v)) = πVB(γ ˙ vp,i(0)) i vp,i wi (Fi(p,v)) ∗ a 2 > a = ((fi dfi)p(v), (0, γ˙ v ,i(0))), kxk = x Bx γ˙ (0) p using the notation B . Additionally for vp,i where πVB denotes the globally well-defined projection onto we can locally compute the vertical bundle. Recall that the vertical bundle of a bundle a k ∗ ] k ` j ∗ k (γ ˙ ) (0) = (f Fi(γv ,i(0)) ) − v (t)γ (0)f (Γi) (p) is the subbundle formed by the kernel of the differential of its vp,i i p vp,i i `j ∗ ] ∗ standard projection, in this case ker(π ∗ ). In particular, = f FD,i(γvp,i(0)) − f grad Φi(p) fi TNi i i i v i a i a ∗ ` j ∗ k locally we have πVB(b ∂i + a ∂i ) = a ∂i for T (fi TNi). − v (t)γ (0)f (Γi) (p) Next, we form a map relating robot accelerations on TTM vp,i i `j ∗  ∂Φ  to their resulting task accelerations in T (f TNi): kj j i = gi (fi(p)) FD,i((dfi)p(v)) − j (fi(p)) ∗ ∂y Zi : TTM −→ T (fi TNi) i ` h k
 ∗  − v (t)(Jfi)jh(p)v (t)(Jfi)r`(p)(Γi)rj(fi(p)). (p, v), a 7→ πVB d(fi dfi)(p,v)(a) . Thus, we can compute solutions to (15) locally by ∗ Since the pullback differential fi dfi is a smooth map be- † ∗ k ! tween smooth manifolds M and fi TNi, the differential of X > a ∗ ζ(p, v) = Jfi(p) w (Fi(p, v))Jfi(p) the pullback differential d(fi dfi) is globally well-defined. i i=1 Thus Zi is globally well-defined and smooth. k ! We also assign a weighting pseudometric wi to each task X > a tangent bundle TNi. Given the higher-order task maps Fi : Jfi(p) wi (Fi(p, v))Ai(p, v) , TM −→ TNi :(p, v) 7→ (fi(p), (dfi)p(v)), we can define i=1 globally well-defined pullback pseudometrics ˙ a where Ai(p, v) = Jf i(p, v)v(t) − γ˙ v ,i(0). We can now ∗ ∗ ∗ p Fi wi : T (fi TNi) × T (fi TNi) −→ R see that that for ζ to be smooth and always have a unique ((p, v), a1), ((p, v), a2) 7→ (wi)F (p,v)(a1, a2). solution, the sum within the pseudoinverse must be full rank. i To achieve this, we make some key assumptions about the Now we define a function which will choose a robot joint design of the task maps f and weighting pseudometrics acceleration for each robot position and velocity in TM and i wi which will also be important for the stability results. First will thus drive our PBDS dynamics: for convenience, we define a function giving the indices of ζ : TM −→ D ⊂ TTM tasks having nonzero weights: k I : TM −→ P({1, . . . , k}) X 1 2 (15) (17) (p, v) 7→ arg min kZi(a) − Si(p, v)kF ∗w , 2 i i (p, v) 7→ {i ∈ {1, . . . , k} | wi((dfi)p(v)) 6= 0}. a∈D(p,v) i=1 This allows us to build a family of product task maps f where D is the globally well-defined affine distribution of Q (p,v) , fi giving for each (p, v) ∈ TM the product map TTM such that the subspace D(p,v) ⊆ T(p,v)TM satisfies i∈I(p,v) av = v componentwise for each a = ((p, v), (av, aa)) ∈ of the task maps associated to nonzero weights. D(p,v). Because all the involved quantities are globally Now we make the following assumptions: defined, as soon as (15) is well-defined (i.e., the minimization (A1) The pseudometrics wi are at every point in TNi either problem has a unique solution for every (p, v) ∈ TM) it is positive-definite or zero. automatically globally defined. (A2) The differential (df )p is always of rank m. To investigate conditions that guarantee existence and (p,v) Pk uniqueness of solutions to this minimization problem for Note that (A2) implies that m ≤ i=1 ni. Now for all ∗ (p, v) ∈ TM we can compute forces FD,i : TNi −→ T Ni, and weighting pseudomet- k rics wi on TNi. Then the set {(fi, gi, Φi, FD,i, wi)}i=1,...,k X Jf (p)>wa(F (p, v))Jf (p) forms a multi-task PBDS with curves σ : [0, ∞) −→ M i i i i satisfying i=1  X > a σ¨(t) = ζ(σ ˙ (t)) = = Jfi(p) wi (Fi(p, v))Jfi(p)   Pk 1 2 arg min kZ (a) − S (σ ˙ (t))k ∗ i∈I(p,v) i=1 2 i i F wi (18) a∈D i > a  σ˙ (t) = Jf (p)w (F(p,v)(p, v))Jf(p,v)(p),  (p,v) (p,v) σ˙ (0) = (p0, v0). Q a where F(p,v) , i∈I(p,v) Fi and w(p,v)(F(p,v)(p, v)) , Assuming (A1) and (A2) to ensure smoothness of ζ and a blockdiag({wi (Fi(p, v))}i∈I(p,v)). Given (A1) and (A2), the existence of unique solutions to (15), we see that we have the final local matrix is positive definite, so the original existence, uniqueness, and smoothness of solution curves σ sum is indeed full rank, giving us unique solutions and for a multi-task PBDS. smoothness of ζ. Thankfully, these assumptions are easily satisfied in practice as explained in Sec III. IV. Proof of Multi-Task PBDS Stability However, it is useful to show that in general, with proba- This section proves the stability results summarized in bility one the local matrix Jf(p,v)(p) has full rank at every the main body of the paper. First, we make another key point p ∈ M if the size of I(p, v) is large enough (in assumption related to the design of weighting pseudometrics. particular, if card(I(p, v)) ≥ 2m for every (p, v) ∈ TM). Using the convenience function in (17), for dissipative forces This may be inferred as a consequence of the Whitney Q we define FD,(p,v) , i∈I(p,v) FD,i. We now assume that immersion theorem. However, by leveraging transversality for every (p, v) ∈ TM we have the following: theory we can directly prove that, i.e., for a sufficiently high (A3) The dissipative force FD,(p,v) is strictly dissipative, number of tasks, the required full-rank properties are satisfied
pointwise with probability one. meaning hFD,(p,v) (df(p,v))p(v) , (df(p,v))p(v)i < 0 v 6= 0 To provide a more precise statement, we proceed by steps. for , Our objective is achieved by selecting at least 2m tasks, if Our Lyapunov stability results are as follows: we can shown that the complementary of the set Proposition C.1 (Lyapunov Function for Multi-Task PBDS). n ∞ Let {(fi, gi, Φi, FD,i, wi)}i=1,...,k be a multi-task PBDS. S , (f1, . . . , f2m) ∈ C (M; N1)× Then given the function V : TM −→ [0, ∞) defined by ∞ 2m o k · · · × C (M; N2m): rank(f ) < m X 1 ∗ 2 V (p, v) = (fi dfi)p(v) (f ∗g ) + Φi ◦ fi(p) 2 i i p is dense with respect to the Whitney topology (see, e.g., i=1 [27]). We prove that the complementary of the set S is dense ∞ ∞ we have that in C (M; N1) × · · · × C (M; N2m) with respect to the k Whitney topology by the jet transversality theorem (see, e.g., dV X (σ ˙ (t)) = hF ((df ) (σ ˙ (t))), (df ) (σ ˙ (t))i < 0 [27]). For this, we denote dt D,i i σ(t) i σ(t)   i=1 ˜ f1 f2m f1 f2m θ(f1, . . . , f2m) , θ, θ0 , . . . , θ0 , θ1 , . . . , θ1 for σ˙ (t) 6= 0 along solution curves σ to the multi-task PBDS. the jet of order one of f1, . . . , f2m. Let us define the d 0 Proof. Taking one element of the sum in dt V (σ (t)) from following smooth mapping in local jet coordinates the first term we have  f f  1 2m d 1 ∗ 2  ρ(θ) , θ1 , . . . , θ1 . (fi dfi)σ(t)(σ ˙ (t)) (f ∗g ) dt 2 i i σ(t) It is clear that ρ is a local summersion. It is also clear that d 1  = (f ∗g ) (f ∗df ) (σ ˙ (t)), (f ∗df ) (σ ˙ (t))
locally we have dt 2 i i σ(t) i i σ(t) i i σ(t) [ −1 ∗ ∗ S = ρ (L(2m, m; i)) = (fi gi)σ(t)(fi Dσ,iγσ,i(0), γσ,i(0)) (19) ∗ ∗ ] 1≤i≤m−1 = (fi gi)σ(t)(fi FD,i(γσ,i(0)) , γσ,i(0)) ∗ ∗ where L(2m, m; i) denotes the Stiefel manifold of linear − (fi gi)σ(t)(f grad Φi(σ(t)), γσ,i(0)) mappings from m to 2m of rank 1 ≤ i ≤ m. Therefore, ∗ R R = hfi FD,i(γσ,i(0)), γσ,i(0)i in terms of codimension, we have ∗ [ − hf gradΦi(σ(t)) , γσ,i(0)i codim(S) ≥ codimρ−1(L(2m, m; m − 1))
i
= hFD,i((dfi)σ(t)(σ ˙ (t))), (dfi)σ(t)(σ ˙ (t))i = codim L(2m, m; m − 1) = m + 1. ∗
− BΦi (σ(t)) (fi dfi)σ(t)(σ ˙ (t)) Then, the conclusion comes from a direct application of the = hF ((df ) (σ ˙ (t))), (df ) (σ ˙ (t))i (20) jet transversality theorem (see, e.g., [27]). D,i i σ(t) i σ(t) Now using the above constructions, we finally give the − d(Φi ◦ fi)(σ ˙ (t)), definition of a multi-task PBDS: where for (19) we use the compatibility of the pullback Definition C.2 (Multi-Task Pullback Bundle Dynamical metric and the pullback connection, and for (20) we use System). Let {fi : M −→ Ni}i=1,...,k be smooth task maps (13). Likewise, for the second term we simply have for a Riemannian task manifolds (N, g), with corresponding d   Φ ◦ f (σ(t)) = d(Φ ◦ f )(σ ˙ (t)). smooth potential functions Φi : Ni −→ R+, dissipative dt i i i i Thus, we have Thus, there must be t¯ > 0 such that σ˙ (p,0)(t¯) 6= 0, in k contradiction with σ(p,0)(t) ∈ A for every t > 0. d X V (σ ˙ (t))
= hF ((df ) (σ ˙ (t))), (df ) (σ ˙ (t))i. Now using the LaSalle Invariance Principle, we can con- dt D,i i σ(t) i σ(t) i=1 clude that for solution curves σ to the multi-task PBDS having initial condition σ˙ (0) ∈ V −1([0, β]), we have Then from assumption (A2) and the fact that all forces FD,i lim dist(σ ˙ (t),B) = 0, and the conclusion follows. are dissipative, we have that V˙ (σ ˙ (t)) < 0 for σ˙ (t) 6= 0. t→+∞

We now provide global stability results: APPENDIX D Theorem C.1 (Global Stability of Multi-Task PBDS). Let MATHEMATICAL DETAILS FOR PBDSPOLICIESON {(fi, gi, Φi, FD,i, wi)}i=1,...,k be a multi-task PBDS where TANGENT BUNDLE TASKS Φi ◦ fi are proper maps. Then given the Lyapunov function V of Proposition C.1, the multi-task PBDS satisfies the Here we provide further details for the extension of following: the PBDS framework to tangent bundle tasks, as used in Sec. IV-B. This framework is mainly instructive to show the • The sublevel sets of V are compact and positively invariant for ζ : TM −→ TTM, so that every solution challenges both in performing such an extension in a way curve σ is defined in the whole interval [0, +∞). that is geometrically well-defined and in finding practical application for using velocity-dependent Riemannian metrics • For every β > 0, every solution curve σ in M starting to drive task behavior. Indeed, in this work we were not able at σ˙ (0) ∈ V −1([0, β]) converges in V −1([0, β]) as to find useful robotic tasks whose behavior could not be t → +∞ to an equilibrium configuration (p, 0) ∈ replicated using the basic multi-task PBDS framework. V −1([0, β]) such that grad Φ (p) = 0 if w (p, 0) 6= 0. i i Given a task map f : M −→ N, consider the higher- Proof. First, we aim to show that the sublevel sets of V order task map F : TM −→ TN :(p, v) 7→ (f(p), dfp(v)). are compact. Following the argument in the proof of [23, It would be convenient if we could follow the same steps we Theorem 6.47], since we know Φi ◦ fi are proper, it is used to extract a robot acceleration policy from task maps sufficient to show that mapping from M. However, task maps mapping from TM introduce two major new challenges. f ∗g : TM × TM −→ R First, the previous procedure applied in this case would k produce curves on TM rather than on M as we desire. X ∗ ∗ ∗
((p, v1), (p, v2)) 7→ (fi gi)p (f dfi)p(v1), (f dfi)p(v2) . Unfortunately, not every curve in TM correctly represents i=1 a curve in M. In particular, given a curve µ : I −→ TM, defines a metric. Symmetry and bilinearity follow easily from the velocity curve µ˙ takes points in TTM, which have the ∗ the properties of the task pullback metrics f gi. For positive- form ((p, v), (b, a)), where (p, v) is the base point from definiteness, we first take assumption (A2), which also gives TM and (b, a) is the vector portion. If µ is to correctly that the differential dfp is always of rank m, for the product represent a curve σ : I −→ M, we must have that Qk ∗ µ˙ (t) = ((σ(t), σ˙ (t)), (σ ˙ (t), σ¨(t))) for every t ∈ I. This map f , i=1 fi. Now note that if f gp(v, v) = 0, then we must have (f ∗g ) (f ∗df ) (v), (f ∗df ) (v)
= 0 for each clearly imposes a velocity constraint on admissible curves in i i p i p i p TM that we can consider: namely, points ((p, v), (b, a)) in task. With dfp of full rank, this implies that v = 0. Thus it is clear that f ∗g is positive-definite and is a metric, which the velocity of admissible curves curves in TM must satisfy in turn implies that V is proper. In addition, since we have v = b (in local coordinates). The way we choose to handle V˙ (σ ˙ (t)) < 0 from Proposition C.1, the level sets of V are this is through a subbundle constraint, as we will see. positively invariant. Second, since velocities in this case are on TTM, an Now, let β > 0 and define the set acceleration operator applied to a curve µ in TM gives n twice as many acceleration vectors as an acceleration op- A = (p, v) ∈ V −1([0, β]) : erator applied to a curve in M. In particular, if µ should represent a curve σ in M, the acceleration operator gives
o hFD,(p,v) (df(p,v))p(v) , (df(p,v))p(v)i = 0 vectors that should represent both the second and third time n o derivatives of σ, i.e. both the acceleration on the jerk. Thus = (p, v) ∈ V −1([0, β]) : v = 0 . to meet our goal of an acceleration policy, it is desirable to project away the jerk components. The GDS framework where the last equality follows from (A3). First, we wish to in RMPflow accomplished this by applying a projection onto show that the set the horizontal bundle [20]. However, as discussed previously, n B = (p, v) ∈V −1([0, β]) : the horizontal bundle is chart-dependent, so this projection is not globally well-defined. o v = 0, gradΦi(p) = 0, i ∈ I(p, v) Instead, we choose to handle this by applying a permuta- tion to swap the position of the jerk and acceleration vectors, is the largest positively invariant set in A for the multi-task an operation which is globally well-defined for parallelizable PBDS. By its definition, we have B ⊆ A. Now suppose manifolds that are embedded in the Euclidean space, such by contradiction that we have a point (p, 0) in the largest as Lie groups. Then the global vertical bundle projection positively invariant set of A such that (p, 0) ∈/ B. Then will correctly remove the jerk components, leaving us with grad Φ(p) 6= 0 for some i ∈ I(p, v). Let the multi-task the acceleration components that we can use to build an PBDS system evolve starting from (p, 0), and let σ(p,0) be acceleration policy. Note that an alternative is to apply the its solution. If σ˙ (p,0)(t) = 0 for all t ∈ [0, +∞), then vertical bundle projection without a permutation and recover σ˙ (p,0)(0) = 0, and σ(p,0)(t) = p does not satisfy the system. a jerk policy, but leave that alternative open to future work. With this motivation and roadmap, we begin by consider- Now suppose we have a robot acceleration σ00 = ing enforcement of velocity constraints. ((p, v), (b, a)) ∈ TTM, with corresponding point ˆ∗ 00 ˘∗ 2d I. Enforcing Velocity Constraints F L(σ ) ∈ F T (R × M) in the pullback bundle. ∗ 00 ∗ If we also have Fˆ L(σ ) ∈ F˘ G 2d , this implies that We will enforce the velocity constraints discussed above R (dπ ) (b, a) − v = 0, or componentwise b − v = 0. This using subbundle constraints. For this, we first consider the M (p,v) is exactly the velocity constraint we aim to achieve. Thus, augmented task map we will enforce this constraint it by ensuring our dynamics ∗ F × πM : TM −→ TN × M F˘ G 2d evolve on the subbundle R . (p, v) 7→ (F (p, v), πM (p, v)) = ((p, dfp(v)), p) This would suffice if we aimed to recover a jerk policy. In order to do so, we need to develop a connection on ∗ 2d However, now consider the permutation map σTN : TN −→ the pullback bundle F˘ T (R × M) which is guaranteed to 2d ∗ R :(p, v) 7→ (dϕ¯p(v), ϕ¯(p)), where N has an embedding provide curves that stay in the subbundle F˘ G 2d if they ϕ¯ : N −→ d. Note that this map is globally well-defined R R start in the subbundle. First we define a projection P ˘∗ ⊥ F G 2d for parallelizable manifolds that are embedded in Euclidean R ˘∗ 2d ˘∗ ⊥ from F T ( × M) onto F G 2d , which is easily seen to space, such as all Lie groups (though unfortunately, a notable R R non-parallelizable manifold is S2). Using this and the embed- be globally well-defined. Now consider the tensorization of ¯ d the P ˘∗ ⊥ operator: ded task map f : M −→ R , we can form the composition F G 2d R 2d Fˆ σTN ◦ F : TM −→ T ˘∗ 2d ˘∗ ∗ 2d , R P ˘∗ ⊥ : TM −→ F T (R × M) ⊗ F T (R × M) F G 2d ¯ ¯ R (p, v) 7→ (dfp(v), f(p)) ˘∗ ∗  ˘∗  (p, v) 7→ F Ei (vp) P ˘∗ ⊥ (F Ej(vp)) F G 2d We now construct the full augmented permuted task map R which we will use: ˘∗ ˘∗ ∗ F Ei(vp) ⊗ F Ej (p), F˘ Fˆ × π : TM −→ 2d × M , M R where (F˘∗E∗) is the dual local frame of F˘∗T ∗( 2d × M) ¯ ¯ i R (p, v) 7→ (dfp(v), f(p), p). ˘∗ ˘∗ 2d associated to the local frame (F Ei) of F T (R × M). Now given the modified task tangent bundle T (R2d×M), we ∗ 2d can construct a pullback bundle F˘ T ( × M). Note that 2d R Now let ∇ 2d and ∇ be connections over and this is a vector bundle which is isomorphic to the pullback R M R M, respectively (e.g., ∇ 2d can be the pushforward of a direct sum vector bundle Fˆ∗(T 2d) ⊕ π∗ TM. R R M connection on TN through σTN ). Then we can combine Thus using straightforward identifications, we can define 2d ∇ 2d and ∇ to form a connection ∇˘ over × M. the map R M R From here, we can build a pullback connection F˘∗∇˘ over ˆ∗ ˆ∗ 2d ∗ F L : TTM −→ F T R ⊕ πM TM TM using the usual definition, having associated Christoffel ˘∗ ˘k ((p, v), (b, a)) 7→ ((p, v), symbols F Γij. ˆ dF(p,v)(b, a) + ((dπM )(p,v)(b, a) − v)). Then for every vector field X over TM and section V of We can also define the two vector subbundles of the pullback ˘∗ 2d ∗ 2d F T (R × M) we can build a well-defined operator bundle F˘ T (R × M): n F˘∗∇˘ P T (·, ·): TM → F˘∗T ( 2d×M)⊗F˘∗T ∗( 2d×M) ˘∗ 1 i 2 i 3 i F˘∗G⊥ R R F G = ((p, v), c ∂x 2d + c ∂v 2d + c ∂x ) 2d VB i R i R i M R ˆ∗ 2d ∗ such that we may consider the identification ∈ F T R ⊕ πM TM ˘∗ ˘ T ˘∗ 2d 1 3 o F ∇P ˘∗ ⊥ (·,V ): TM → F T (R × M). F G 2d | ci = 0 for i = 1, . . . , d, ci = 0 for i = 1, . . . , m , R From this we can compute the coordinate representation ˘∗ n 1 i 2 i 3 i F G 2d = ((p, v), c ∂x 2d + c ∂v 2d + c ∂x ) R i R i R i M ˘∗ ˘ T 1 F ∇P ˘∗ ⊥ (·,V ) = ( {h>2d,k≤2d}(h, k) o F G 2d ∈ Fˆ∗T 2d ⊕ π∗ TM | c3 = 0 for i = 1, . . . , m , R R M i 1 ˘∗ ˘k j h − {h≤2d,k>2d}(h, k))F ΓjhX V nk, which are globally well-defined and smooth, in the first case where 1 is an indicator function, and (ni) is an orthonormal due to its vertical bundle structure and in the second case ˘∗ ∗ 2d due to standard projection arguments exploiting the direct basis for F T (R × M). This allows us to define a new ˘∗ ∗ 2d sum structure of the pullback bundle described earlier. Note connection over the pullback bundle F T (R × M) as ˘∗ ˘∗ ∗ that we have F GVB ⊆ F G 2d as vector bundles. We also F˘ G 2d R R use these to define a further subbundle of the pullback bundle ˘∗ ˘ ˘∗ ˘ ˘∗ T F ∇X V , F ∇X V + F ∇P ˘∗ ⊥ (·,V ), ˘∗ ˘∗ ˘∗ ⊥ ˘∗ ⊥ F G 2d F G = F GVB ⊕ F G 2d , where F G 2d is the orthogonal R R R ∗ ∗ 2d F˘ G 2d F˘ T ( × whose Christoffel symbols take the form complement of R in the pullback bundle R M) ∗ . This orthogonal complement can be defined by F˘ G 2d R ˘∗ ⊥ ˘∗ 2d ˘∗ ˘k 1 F G 2d = {(p, v) ∈ F T ( × M) F Γjh = (1+ {h>2d,k≤2d}(h, k) R R (21) ∗ ∗ ˘∗ ˘k : F˘ g (v, w) = 0, ∀w ∈ F˘ G 2d }, − 1 (h, k))F Γ . p R {h≤2d,k>2d} jh ∗ ∗ 2d ∗ F˘ g F T ( × M) V (F˘(p, v)) ∈ F˘ G 2d given some pullback metric for R . In particular, notice when that when R 2di for every (p, v) ∈ TM, we have ΦM (p). Likewise on each T (R ×M) we form the weight product pseudometic w˘ = w 2d ⊕ w and dissipative F˘∗∇˘ P T (·,V )(F˘(p, v)) i T R i TM F˘∗G⊥ ˘ 2d forces F = F 2di ⊕ F . R D,i D,R D,M 2d+m 2m 2d   ˘∗ 2di ∼ X X X ˘∗ ˘k j h ˘∗ > Then we define two distributions of T (Fi T (R ×M)) = = − F Γ X V nk ∈ F G 2d . jh R ˆ∗ 2di ∗ T (F T R ) ⊕ T (πM TM) analogous to the subbundles k=2d+1 j=1 h=1 ˘∗ ˘∗ F GVB and F G 2d : ∗ R F˘ G 2d R n j j j ∗ 1 2 3 ˘ ˘ DVB,i = ((p, v), a), cj ∂x + cj ∂v + cj ∂a 2d In short, this new connection F ∇ effectively removes M M,1 R i the connection terms which may cause a curve to leave the 4 j 5 j
ˆ∗ 2di ∗ ∗ + cj ∂v 2di + cj ∂vM,2 ∈ T (Fi T R ) ⊕ T (πM TM) subbundle F˘ G 2d where our desired curves should stay. For R R o brevity, we omit the full proof of this result which generalizes 1 2 3 | cj = cj = 0 for j = 1, . . . , m, cj = 0 for j = 1, . . . , d , [23, Prop. 4.85]. n 1 j 2 j 3 j II. Multi-Task Acceleration Policy Optimization D 2di = ((p, v), a), cj ∂x + cj ∂v + cj ∂a 2d R M M,1 R i Now using this connection, we can build local pullback 4 j 5 j
ˆ∗ 2di ∗ + cj ∂v 2di + cj ∂vM,2 ∈ T (Fi T R ) ⊕ T (πM TM) bundle dynamical systems having curves that satisfy our R 5 o desired velocity constraints: | cj = 0 for j = 1, . . . , m , Definition D.1 (Local PBDS on Tangent Bundle Task). Let and define projections P and P from the tangent DVB,i D 2di f : M −→ N be a smooth task map, where N is embedded ∗ 2d R d pullback bundle T (F˘ T ( i ×M)) onto these distributions. in R , and associate to f the augmented, permuted task map i R 2d As with the subbundles, these distributions are smooth and F˘ : TM −→ R ×M. Also let g˘ be a Riemannian metric on 2d ˘ 2d globally well-defined due to their vertical bundle structures. R × M, let Φ: R × M −→ R+ be a smooth potential ˘ 2d ∗ 2d function, and let FD : T (R × M) −→ T (R × M) Now as before, we use the local PBDS construction above be dissipative forces. Then for each (p, v) ∈ M, we can to form for each task a section of the tangent pullback choose a curve α(p,v) :(−ε, ε) −→ TM resulting in bundle associating each robot position/velocity pair with the ˘∗ d corresponding desired task pullback acceleration: γα(p,v) :(−ε, ε) −→ F T (R × M) for some ε > 0 such that (f, g, Φ, F , α ) forms a local Pullback Bundle ˘∗ 2di D (p,v) Si : TM −→ T (Fi T (R × M)) Dynamical System (PBDS) satisfying (p, v) 7→ P ◦ P (γ0 (0)) D 2di DVB,i (p,v)   ˘∗  R F G 2d 0a R = (((p, v), a), (0, (0, γ (0), 0))).  ˘∗
(p,v) PF˘∗G F Dα(p,v) PF˘∗G(γα(p,v) (s)    Next we map robot accelerations and jerks to their corre- PBDS  ∗ ] α(p,v) = P F˘ F˘(γ (s)) sponding task tangent pullback bundle accelerations:  F˘∗G α(p,v)  ∗ 0 ˘∗ 2di γ (0) = F˘ dF˘ (α (0)), Zi : TTTM −→ T (Fi T (R × M))  α(p,v) α(p,v)(0) (p,v)  ˘∗ ˘ α(p,v)(0) = (p, v), (((p, v), a), κ) 7→ PD 2d ◦ PDVB,i (d(Fi dFi)((p,v),a)(κ)). R i where we denote total pullback acceleration F˘∗F˘(·)], anal- Now given a weighting pseudometric w˘ over T ( 2di × ogous to Appx. C. As before, given (p, v) ∈ TM, we have i R 0 M) for each task, we can use a further higher-order task that γα (0) from a corresponding local PBDS does not 2di (p,v) map Fi : TTM −→ T (R × M) : ((p, v), a) 7→ depend on the choice of curve α(p,v), so we will again use ˘ ˘ (Fi(p, v), (dFi)(p,v)(a)) to form the pullback pseudometric the shorthand γ0 (0) γ0 (0). ∗ vp , α(p,v) Fi w˘i over TTM. Now, in order to extract a single robot acceleration policy from a set of tasks, we trace the constructions in Appx. Then we can form our function returning the robot accel- C.III with some modifications to form the required geometric eration policy output for a given robot position and velocity: optimization problem. For k tasks, consider task maps {fi : ζ : TM −→ D ⊂ TTTM di M → Ni}i=1,...,k, where each Ni is embedded in R , k ¯ X 1 2 (22) giving corresponding embedded task map fi. We equip each (p, v) 7→ arg min kZ (κ) − S (p, v)k ∗ , i i F w˘i 2di 2 i associated embedding space R for TNi with a Rieman- κ∈D(p,v) i=1 nian metric g 2d and a potential function Φ 2d depending R i R i where D is the globally well-defined affine distribution of only on v for (p, v) ∈ T R2di . We also equip each T R2di TTTM such that the subspace D(p,v) ⊆ T((p,v),(v,a))TTM with a weighting pseudometric w 2d and dissipative forces T R i v v b 2di satisfies κ = v and κ = κ = a componentwise FD,R2di , both dependent only on the base points of T R , p v b a as will be important for defining our geometric optimization for each (((p, v), (v, a)), (κ , κ , κ , κ )) ∈ D(p,v). Note that because wT 2di depends only on the base points of problem. We also set the first d components of FD,R2di to R T R2di , given a point (p, v) ∈ TM, the coordinates of the be zero. Next, we define similar objects gM , ΦM on M and ∗ pullback pseudometric Fi w˘i do not depend on the particular wM , FD,M on TM, although their design is inconsequential ∗ as their contributions are ultimately projected away. ((p, v), a) ∈ T(p,v)TM at which Fi w˘i is evaluated. Thus in From these we can form on each augmented, permuted practice we can simply evaluate it at ((p, v), 0) ∈ TTM. 2di task space × M the product metric g˘ = g 2d ⊕ g To derive the local form of this acceleration policy, notice R i R i M ∗ and potential forces Φ˘ such that Φ˘ (v, q, p) = Φ 2d (v, q)+ that the differential d(F˘ dF˘ ) can be written locally as the i i R i i i ˆ  ˙¯ a Jacobian − ∇Φ 2di (Fi(vp)) − (Jfi(p, v) − Ξi (p, v))v   R Im 0 0 0 −1 a −1 where (g 2d ) is the lower-right quadrant of g 2d and  0 Im 0 0  R i R i ˘∗ ˘  ¨¯ ˙¯ ˙¯ ¯ the Euclidean gradient ∇Φ 2di is taken over the last di J(F dFi) = Jfi Jfi Jfi Jfi , R i   components. Additionally we have J˙f¯ 0 Jf¯ 0  i i  v j+di r (Ξ ) (p, v) = (JFˆ ) (v )(Γ 2d ) (Fˆ (v ))Jf¯ (p)v 0 0 0 Im i j` i s` p R i s(h+m) i p hr a ˆ j+di ˆ ¯ r ˙¯ (Ξ )j`(p, v) = (JFi)s(`+d )(vp)(Γ 2di ) (Fi(vp))Jfhr(p)v . where we denote Jfi analogous to Appx. C. From this we i i R s(h+m) can see that given a point having componentwise identifi- This leads to the multi-task PBDS policy for tangent cations q = (((p, v), (v, a)), (v, a, a, κ)) ∈ D(p,v) we can bundle tasks: locally compute Definition D.2 (Multi-Task PBDS on Tangent Bundles). Let 2 kZ (q) − S (p, v)k ∗ {f : M −→ N } be smooth task maps, where N i i F w˘i (23) i i i=1,...,k i i is embedded in d. Then given corresponding Riemannian ¯ ˙¯ κ 2 R = kJfi(p)a + Jfi(p, v)v − γ˙ v ,i(0)kw˘ a(F (v ,0))), 2di ˘ p i i p metrics g˘i on R × M, smooth potential functions Φi : 2di ˘ 2di v a b κ M a R × M −→ R+, dissipative forces FD,i : T (R × where γ˙ vp = (γ˙ v , γ˙ v , γ˙ v , γ˙ v , γ˙ v ), w˘i is the lower-right p p p p p ∗ 2di 0 2di M) −→ T (R ×M), and weighting pseudometrics w˘i on quadrant of w 2di , and we define F : TM −→ T ( × T R i R 2di T ( ×M), the set {(fi, gi, Φ˘ i, F˘D,i, w˘i)}i=1,...,k forms a M):(F˘i(p, v), (0, v)). Then adapting (10) and using the R computed Christoffel symbols of (21), we can compute the multi-task PBDS with curves σ : [0, ∞) −→ M satisfying  components of γ˙ v ,i along the indices of interest j = 2m + σ¨(t) = ζ(σ ˙ (t)) = p  di + 1,..., 2m + 2di. First for forces, we have  Pk 1 2 arg min kZi(κ) − Si(σ ˙ (t))k ∗ i=1 2 Fi w˘i (24)   j κ∈Dσ˙ (t) ˘∗ ˘ ]  P ˘∗ Fi Fi(γvp,i(0))  Fi G σ˙ (0) = (p0, v0).  ˘  jh ˆ ˘h ∂Φi ˘ Similar existence, uniqueness, smoothness, and stability = g 2d (Fi(vp)) FD,i(γvp,i(0)) − (Fi(vp)) . R i ∂xh guarantees can be given for this dynamical system as for that of Appx. C, but these are outside the scope of this appendix. Then for the Christoffel symbols term, denoting γvp = (γv , γa , γM ) α˙ = (α ˙ v , α˙ a ) vp vp vp and vp vp vp we have h  `   ˘∗ ˘j−2m  P ˘∗ α˙ v ,i(0)P ˘∗ γvp,i(0) Fi Γ (vp) Fi G p Fi G `h  `  ˘∗ ˘ r  = P ˘∗ α˙ v ,i(0)P ˘∗ (Fi JFi)hrα˙ v ,i(0) Fi G p Fi G p ˘ ˘j−2m ˘  (JFi)s`(vp)Γsh (Fi(vp)) = (α ˙ v )`(0)(Jf¯) (p)(α ˙ v )r(0) vp,i i hr vp,i ˆ ˘j−2m ˆ (JFi)s`(vp)Γs(h+d)(Fi(vp)) + (α ˙ a )`(0)(Jf¯) (p)(α ˙ v )r(0) vp,i i hr vp,i ˆ ˘j−2m ˆ (JFi)s(`+d)(vp)Γs(h+d)(Fi(vp)) = (Ξv) (v )(α ˙ v )`(0) i (j−2m−di)` p vp,i + (Ξa) (v )(α ˙ a )`(0), i (j−2m−di)` p vp,i ˘∗ ˘ where we notice the Jacobian Fi JFi takes the local form  ˙¯ ¯  Jfi Jfi 0 ˘∗ ˘ ¯ Fi JFi = Jfi 0 0  . 0 0 Im From this can compute solutions to (22) locally as k !† X > a ζ(p, v) = Ci (p, v)w˘i (Fi(vp, 0))Ci(p, v) i=1 k ! X > a Ci (p, v)w˘i (Fi(vp, 0))Ai(p, v) i=1

¯ v Ci(p, v) = Jfi(p) + Ξi (p, v)

−1 a ˆ  ˆ Ai(p, v) = (g 2d ) (Fi(vp)) F D, 2di ((dFi)vp (vp, 0)) R i R