fb-jul02-p1 fb-jul02-p2 Acknowledgments

Geometric control of Lagrangian systems Main coworker: modeling, analysis, and design Andrew D. Lewis Dept of Mathematics & Statistics, Queen’s University Kingston, Ontario, K7L 3N6, Canada Francesco Bullo [email protected] Coordinated Science Lab, General Engineering Dept, ECE, AAE University of Illinois at Urbana-Champaign All papers and slides at http://motion.csl.uiuc.edu 1308 W. Main St, Urbana, IL 61801, USA and http://penelope.mast.queensu.ca [email protected], http://motion.csl.uiuc.edu This work has been supported by NSF grant CMS-0100162. SummerSchool on Modeling and Control of Mechanical Systems Dutch Institute for Systems and Control (http://www.disc.tudelft.nl/) Collaborators: Engineering Mechanics (http://www.em.tue.nl/) during my years at Caltech: Burdick, Leonard, Marsden, Murray, Zefranˇ • July 1st – July 4th 2002 during my years at University of Illinois: Cerven, Cort´es, Frazzoli, Karatas, Conference Center Woudschoten, Zeist, the Netherlands • Lynch, Mart´ınez, Zefranˇ http://www.woudschoten.nl

fb-jul02-p3 fb-jul02-p4

Table of Contents Lecture titles

A coordinate-free theory for controlled mechanical systems Lecture #1: From Linear Algebra to Mechanical Control Systems (i) modeling Lecture #2: Modeling Symmetries and Nonholonomic Constraints symmetries, nonholonomic constraints, impacts, kinematic systems Lecture #3: Perturbation Analyses of Affine Connection Control Systems (ii) analysis http://motion.csl.uiuc.edu/~bullo/papers/1999b-b.html averaging (series expansions under small-amplitude controls, averaging under Lecture #4: Kinematic Reductions and Configuration Controllability highly oscillatory controls), controllability (local controllability, configuration http://motion.csl.uiuc.edu/~bullo/papers/2002a-bll.html controllability, equilibrium and kinematic controllability), kinematic reductions (decoupling vector fields, fully reducible systems) Lecture #5: Stabilization and Tracking for fully actuated systems (iii) design Lecture #6: Trajectory Planning via Motion Primitives planning (inverse kinematics for kinematically controllable systems, power http://motion.csl.uiuc.edu/~bullo/papers/1997b-bll.html series inversion under small amplitude controls), stabilization and tracking http://motion.csl.uiuc.edu/~bullo/papers/2001a-bl.html fb-jul02-p5 fb-jul02-p6 1.3 Motion planning 1 Geometric Control of Lagrangian Systems Example systems 1.1 Scientific Interests (i) dexterous manipulation via minimalist robots (i) success in linear control theory is unlikely to be repeated for nonlinear (ii) real-time trajectory/path planning for autonomous vehicles systems. In particular, nonlinear system design. no hope for general theory (iii) locomotion systems (walking, swimming, diving, etc) mechanical systems as examples of control systems (ii) control relevance of tools from geometric mechanics Application contexts

(iii) geometric control past feedback linearization (i) guidance and control of physical systems (ii) prototyping and verification 1.2 Industrial Trends (iii) graphical animation and movie generation autonomous vehicles new concepts in design (iv) analysis of animal and human locomotion and prosthesis design in reconfigurable, reactive implementation on-line biomechanics sensing & computation cheap focus on actuators and algorithms exploit differential geometric structure

fb-jul02-p7 fb-jul02-p8 Research work reflected in these notes Very incomplete reference on geometric mechanics and geometric control of

[1] A. D. Lewis, “Simple mechanical control systems with constraints,” IEEE TAC, 45(8):1420–1436, 2000. mechanical systems [2] A. D. Lewis, “When is a mechanical control system kinematic?,” in Proc CDC, (Phoenix, AZ), [1] S. Arimoto. Control Theory of Non-linear Mechanical Systems: A Passivity-Based and Circuit-Theoretic pp. 1162–1167, Dec. 1999. Approach, volume 49 of OESS. Oxford University Press, Oxford, UK, 1996. [3] F. Bullo and M. Zefran,ˇ “On mechanical control systems with nonholonomic constraints and symmetries,” [2] A. M. Bloch and P. E. Crouch. Nonholonomic control systems on Riemannian manifolds. SIAM Journal of Syst. & Control L., 45(2):133–143, 2002. Control and Optimization, 33(1):126–148, 1995. [4] F. Bullo and M. Zefran,ˇ “Modeling and controllability for a class of hybrid mechanical systems,” IEEE [3] R. W. Brockett. Control theory and analytical mechanics. In R. Hermann and C. Martin, editors, TRA, Aug 2002. To appear. Geometric Control Theory, volume 7 of Lie Groups: History Frontiers and Applications, pages 1–46. Mathematical Scientific Press, Brookline, MA, 1977. [5] A. D. Lewis and R. M. Murray, “Configuration controllability of simple mechanical control systems,” SIAM JCO, 35(3):766–790, 1997. [4] P. S. Krishnaprasad and D. P. Tsakiris, “Oscillations, SE(2)-snakes and motion control: a study of the roller racer,” Dynamics and Stability of Systems, vol. 16, no. 4, pp. 347–397, 2001. [6] F. Bullo and K. M. Lynch, “Kinematic controllability for decoupled trajectory planning in underactuated mechanical systems,” IEEE TRA, 17(4)L402–412, 2001. [5] N. E. Leonard. Mechanics and nonlinear control: Making underwater vehicles ride and glide. In Nonlinear Control Systems Design (NOLCOS), volume 1, pages 1–6, Enschede, The Netherlands, July 1998. [7] F. Bullo, N. E. Leonard, and A. D. Lewis, “Controllability and motion algorithms for underactuated Lagrangian systems on Lie groups,” IEEE TAC, 45(8):1437–1454, 2000. [6] R. M. Murray. Nonlinear control of mechanical systems: a Lagrangian perspective. In Nonlinear Control Systems Design (NOLCOS), pages 378–389, Lake Tahoe, CA, June 1995. [8] F. Bullo, “Series expansions for mechanical control systems,” SIAM JCO, 40(1):166–190, 2001. [7] H. Nijmeijer and A. J. van der Schaft. Nonlinear Dynamical Control Systems. Springer Verlag, New York, [9] F. Bullo, “Averaging and vibrational control of mechanical systems,” SIAM JCO, Submitted 1999. To NY, 1990. appear 2002. [8] R. Ortega, A. Loria, P. J. Nicklasson, and H. Sira-Ramirez. Passivity-Based Control of Euler-Lagrange [10] J. Cort´es, S. Mart´ınez, and F. Bullo, “On nonlinear controllability and series expansions for Lagrangian Systems: Mechanical, Electrical and Electromechanical Applications. Communications and Control systems with dissipative forces,” IEEE TAC, Aug. 2002. To appear. Engineering. Springer Verlag, New York, NY, 1998. [11] F. Bullo, “Series expansions for analytic systems linear in controls,” Automatica, 38(9):1425-1432, 2002. [9] A. J. van der Schaft, “Port-controlled Hamiltonian systems: towards a theory for control and design of [12] F. Bullo and A. D. Lewis, “Kinematic controllability and motion planning for the snakeboard,” IEEE TRA, nonlinear physical systems,” Journal of the Society of Instrument & Control Engineers, vol. 39, no. 2, Jan. 2002. Submitted. pp. 91–8, 2000. fb-jul02-p9 fb-jul02-p10

2 Linear algebra Lecture #1: From Linear Algebra to 2.1 Notation Linear space V , vectors v V Mechanical Control Systems • ∈ dual space V ∗ is the space of co-vectors w: • Francesco Bullo w , v R h i ∈ Coordinated Science Lab, General Engineering Dept, ECE, AAE n in R , think of v as columns (V is space of column vectors), and w as rows (V ∗ University of Illinois at Urbana-Champaign • 1308 W. Main St, Urbana, IL 61801, USA is space of row vectors) [email protected], http://motion.csl.uiuc.edu construction is possible on any vector space! •

fb-jul02-p11 fb-jul02-p12

2.3 Addendum on linear algebra and multi-variable calculus

2.2 Vector versus indicial notation i (i) vectors: v = v ei , is natural pairing between dual spaces • h· ·i (ii) covectors, dual elements v V = column vectors , w V ∗ = row vectors : • ∈ { } ∈ { } (iii) on Rn, use variables (q1, . . . , qn) – notation useful for “summation convention” w v = w , v n R (iv) given a function f : R R, recall its directional derivative · h i ∈ → n ∂f ∂f other example, f(x1, . . . , xn) and v = (v1, . . . , vn) R (column): (v) the differential df is a covector field with components 1 , . . . n so that • ∈ ∂q ∂q ∂f n ∂f ∂f ∂f , v = v df = ( , . . . , ) h ∂x i ∂x i ∂q1 ∂qn i=1 i X L that is, we mean (vi) X is a vector field, and we can define X f = df , X h i ∂f ∂f ∂f = . . . (vii) planar body example: Vx, Vy are example vector fields ∂x ∂x1 ∂xn h i (viii) infinitesimal work in mechanical system is a pairing (not an inner product) fb-jul02-p13 fb-jul02-p14

“a matrix is a matrix is not a matrix” (xii) maps between linear space: A: V V has components Aj → i n n i j i (ix) a curve γ : I R has a velocity γ : I R , which is a vector field along the v = v ei Av = Ai v ej → → 7→ curve (xiii) bilinear maps: B : V V R has components Bij (x) a vector field X is an ODE and an ODE is a vector field × → (v, w) = (vie , wje ) B(v, w) = B viwj ∂ ∂ i j 7→ ij (xi) vector fields are written in terms of the canonical basis , . . . , , and { ∂q1 ∂qn } (xiv) associate linear map: B : V V ∗ has components Bij co-vector fields in terms of dq1, . . . , dqn → { } v = vie B viej ∂ ∂f ∂f i ij X(q) = Xi(q) ω = ω (q)dqi df = dqi = dqi 7→ ∂qi i ∂qi ∂qi i X (xv) an inner product , is a bilinear map, need a symbol G: V V R hh· ·ii × → 1 (xvi) since G: V V ∗ is non-singular, we can invert it, G− : V ∗ V is now an → → inner product on V ∗

fb-jul02-p15 fb-jul02-p16

(xvii) Lie derivatives do not commute ∂ ∂ ∂ ∂ (a) f = f ∂qi ∂qj ∂qj ∂qi (b) however [g1, g2]

−u2 L L f = L L f Consider the controlled ODE x˙ = g1(x)u1 + g2(x)u2 X1 X2 6 X2 X1 g +u 2 ∂g2 ∂g1 1 −u1 (c) correct formula is: define Lie bracket: [g1(x), g2(x)] = g1 g2 ∂x − ∂x +u2 g1 L L f L L f = L f X1 X2 − X2 X1 [X1,X2] where Lie bracket (in indicial notation) Properties of Lie brackets: (a) skew symmetry: [X, Y ] = [Y, X] ∂Y i ∂Xi − [X, Y ]i = Xj Y j ∂qj − ∂qj (b) linearity: [X, Y + Z] = [X, Y ] + [X, Z] (c) derivation: [X, fY ] = f[X, Y ] + (L f)Y in vector notation (where now ∂X/∂q is an n n matrix): X × (d) Jacoby identity: [X, [Y, Z]] + [Z, [X, Y ]] + [Y, [Z, X]] = 0 ∂Y ∂X [X, Y ] = X Y ∂q · − ∂q · fb-jul02-p17 fb-jul02-p18

3 A primer in Riemannian geometry (iv) q f(q) R is scalar function 7→ ∈ n 3.1 Notation (v) As on R , vector fields and covector fields attached to each point on Q: ∂f ∂f the differential df is a covector field with components , . . . so that (i) assume every object is real analytic • ∂q1 ∂qn (ii) Q is a manifold, that is, a locally Euclidean space ∂f i ∂f i df = i dq = i dq

¢ ¤ ∂q ∂q i X 1 n ¡ ¢ X is a vector field with components X , . . . X so that ¦ ¥ • i ∂ i ∂ X = X i = X i ¡

£ ∂q ∂q ¤ £ i X ¢ ¤ £ Lie derivative of a function (X, f are both functions of q): • ∂f L f := Xi = df , X X ∂qi h i i X ¦ ¥ (vi) Last equality is the natural pairing between tangent TQ and cotangent bundle (iii) q Q is point on manifold, in coordinates q = (q1, . . . , qn) T∗Q ∈

fb-jul02-p19 fb-jul02-p20

3.2 Affine Connections

An affine connection on maps two vector fields X, Y into a third vector field • ∇ (vii) γ : I Q is a curve on Q. Its velocity is a vector field along γ with X Y , satisfying the following properties: → ∇ components (i) fXY = f X Y i ∇ ∇ dγ (t) ∂ i ∂ L γ0(t) = = γ˙ (t) (ii) X fY = ( X f)Y + f X Y dt ∂qi ∂qi ∇ ∇ Given the basis ∂ , . . . , ∂ , determines and is uniquely determined by the • { ∂q1 ∂qn } ∇ Christoffel symbols: γ˙ (t) ∂ ∂ = Γij ∇ ∂ ∂qj k ∂qk i γ(t) ∂q In coordinates • ∂ Y = L Y k + Γk XiY j ∇X X ij ∂qk ³ ´ ∂Y k ∂ = Xi + Γk XiY j ∂qi ij ∂qk µ ¶ fb-jul02-p21 fb-jul02-p22

3.3 Covariant derivatives of vector fields along curves

Given a curve γ : I Q, and its velocity γ0 : I TQ is a curve on TQ. • → → 3.4 Property of covariant derivatives along curves γ0 : I TQ is an example of a vector field along a curve on Q • → Recall: An affine connection on maps two vector fields X, Y into a third vector ∇ Given a vector field η : I TQ along γ, define its covariant derivative along γ as field Y , satisfying the following properties: • → ∇X η = Y (i) fXY = f X Y ∇γ0 ∇γ0 ∇ ∇ (ii) fY = (L f)Y + f Y where Y is a smooth extensions of η to Q ∇X X ∇X In coordinates: • Given a function of time f, and a vector field η along γ: γ(t) = (γ1(t), . . . , γn(t)) γ (t) = (γ˙ 1(t), . . . , γ˙ n(t)) 0 d f(t)η(t) = f(t) η(t) + f(t) η(t) η(t) = (η1(t), . . . , ηn(t)) ∇γ0 dt ∇γ0 µ ¶ ³ ´ i η = η¨i + Γi (γ)γ˙ jηk ∇γ0 jk ³ ´

fb-jul02-p23 fb-jul02-p24 3.6 Collection of vector fields, distributions, and operations between 3.5 Geometric acceleration and geodesic curves vector fields

given a curve γ, the second time derivative γ¨i is not a vector (i) = X1, . . . , X` a the collection or family of vfs • X { } (ii) X = span X , . . . , X is called the distribution, i.e., the point-wise C(Q){ 1 `} Given a curve γ, define the geometric acceleration of γ as the vector field sub-space of T Q. In other words, X = span X (q), . . . , X (q) • q q R 1 ` along γ { } (iii) the Lie bracket between Xi and Xj is [Xi, Xj] γ0(t) γ0(t) ∇ (iv) The distribution X is said to be involutive if it is closed under operation of in coordinates (with respect to the respective bases): X • Lie bracket, i.e., if for all vector fields X and Y taking values in , the vector 1 n 1 n field [X, Y ] also takes value in X . The involutive closure of the γ(t) = (γ (t), . . . , γ (t)) γ0(t) = (γ˙ (t), . . . , γ˙ (t)) distribution X is the smallest involutive distribution containing X , and is 1 1 i j n n i j γ0(t) = (γ¨ + Γ γ˙ γ˙ , . . . , γ¨ + Γ γ˙ γ˙ ) γ0(t) ij ij denoted Lie X . ∇ { } A curve with zero geometric acceleration is a geodesic (v) the symmetric product between Xi and Xj is the vector field •

geodesic curves enjoy various properties: constant point-wise energy, Xi : Xj = Xi Xj + Xj Xi • h i ∇ ∇ homogeneity, existence and uniqueness. One then can define the notion of symmetric closure and geodesic invariance. fb-jul02-p25 fb-jul02-p26 3.7 Riemannian metric

Metric is inner product on tangent space • , : TQ TQ R hh· ·ii × → 3.8 Associated linear maps between TQ and T∗Q inner product is positive definite, symmetric, bilinear form G. • (i) G: TQ T∗Q: → T In coordinates Gij Given a vector field X, ([G]X) is the co-vector field such that • i j X , Y = Gij (q)X (q)Y (q) hh ii T T ij ([G]X) Y = X [G]Y X · G as a matrix (in vector notation): X , Y = X T [G]Y . GX , Y X , Y • hh ii h i hh ii | {z } | {z } 1 (ii) : T∗Q TQ: Summary: G− → 1 T • Given a co-vector field F , G− F is the vector field such that (i) there is a pairing between functions and vector fields (i.e., LX f), and 1 1 T T similarly between vector fields and co-vector fields (i.e., df , X ) M − F , Y = (G− F ) [G]Y = F , Y h i hh ii h i (ii) G is a pairing between two vector fields where in vector notation “a pairing := combine two vectors to obtain a scalar”

NB: in mechanical systems, metric is usually denoted M. In Riemannian geometry g.

fb-jul02-p27 fb-jul02-p28 3.10 Levi-Civita (or metric) connection

Theorem 1 (Levi Civita). A metric , induces a unique G hh· ·ii ∇ 3.9 Gradient of a function such that (i) L Y , Z = G Y , Z + Y , G Z Given a function f, its gradient is the vector field X hh ii hh ∇X ii hh ∇X ii • (ii) G X Y G Y X = [X, Y ] 1 ∇ − ∇ grad f = G− df

or alternatively (i) Its symbols are: grad f , X df , X hh ii ≡ h i k 1 mk ∂Gmj ∂Gmi ∂Gij Γ = G + In indicial notation: ij 2 ∂qi ∂qj − ∂qm µ ¶ n mk 1 i 1 ij ∂f ij ∂f where G is m, k component of G− (grad f) = (G− ) = G ∂qj ∂qj j=1 X (ii) Proof based on equality: 2 Z , G Y = X Y , Z + Y X , Z Z Y , X hh ∇X ii hh ii hh ii − hh ii [X, Z] , Y [Y, Z] , X [X, Y ] , Z − hh ii − hh ii − hh ii fb-jul02-p29 fb-jul02-p30

4 Models of Mechanical Systems 4.1 Planar body example

Simple mechanical control system is composed of:

© ¨

 (i) the configuration space Q (manifold) ¡ £ ¥

¤ ¤ ¦ ¢ (ii) the kinetic energy G (metric) §

(iii) the potential energy V (function on Q) (iv) the input forces F 1, . . . , F m (co-vectors)

q = (θ, x, y)

J 0 0 V (q) = mgy [G] = 0 m 0  Total energy (Hamiltonian, sum of kinetic and potential) is: 0 0 m   1 2 (q, vq) = vq + V (q)   E 2k k We shall discuss F i in a few slides

fb-jul02-p31 fb-jul02-p32 4.2 Planar two links manipulator example 4.3 Kinematics

Only necessary variables to describe system are configuration variables, e.g.       q = (θ1, θ2)  

 Write (θi, xi, yi) in terms of q by means of kinematic analysis.         (q, q˙) := (θ , x , y , θ˙ , x˙ , y˙ ) /. (θ , x , y ) (θ , x , y )(q) E E i i i i i i i i i → i i i After simplification:

2 I1 + (l1(m1 + 4m2))/4 (l1l2m2 cos[θ1 θ2])/2 = 1 + 2 [G] = − E E E  2  1 1 (l1l2m2 cos[θ1 θ2])/2 I2 + (l2m2)/4 K (θ , x , y ) = I θ˙2 + m(x˙ 2 + y˙2) − 1 1 1 1 2 1 1 2 1 1   V1(θ1, x1, y1) = m1g y1

Therefore easy to write as function of all variables General study of single and multi-body kinematics. E fb-jul02-p33 fb-jul02-p34 4.4 Forces as co-vectors ¢ £

£ in this example only pure torques: Joint motor T acts on angle θ . Joint motor • 1 1 T acts on angle θ θ : 2 2 − 1

¡ ¢ ¡ T1 = dθ1 = 1 0 h i T2 = d(θ1 θ2) = dθ1 dθ2 = 1 1 − − − h i Why are forces co-vectors? Assume curve γ : I Q is solution to controlled Note: force is a co-vector, for example, F = df for some function f. But not → • equations, then always F is the differential of a function (Poincar´e lemma)

Infinitesimal Work = F , γ0 h i where γ0 T Q and hence F T∗ Q. ∈ γ ∈ γ forces as generalized forces, i.e., • both pure forces and pure torques are ok

fb-jul02-p35 fb-jul02-p36

4.5 Generalized force = pure force + pure torque 4.6 Lagrange-D’Alembert principle If force is pure torque on angle α, then F = dα. If force is pure force on distance The solution γ : I Q to the simple mechanical control system satisfies the x, then F = dx. Write a generalized force as linear combination of pure force and → pure torque. variational principle

1 2 δ γ0 V (γ) dt + F (γ, t) , δq = 0 ¤

¥ 2 k k − h i ZI µ ¶ ZI ¤¦ § ¨ where the variation δq is an arbitrary vector field along γ § © Systems subject to no force follow geodesic flow: •

2 δ γ0 dt = 0 0 γ0 = 0 k k ∇γ ZI 1 Systems subject to force follow forced geodesic flow: F = cos θdx + sin θdy = 0 cos θ sin θ • 1 2 h i γ0 γ0 = G− F F = hdθ sin θdx + cos θdy = h sin θ cos θ ∇ − − − − h i fb-jul02-p37 fb-jul02-p38 Given this data, we derive

(i) G is the Levi-Civita connection associated to G ∇ 1 a (ii) we define the input vector fields Ya = G− F , for a 1, . . . , m ∈ { } 5 Simple Mechanical Control Systems (SMCS) (iii) Coordinate-free formulation of the equations of motion:

m A simple mechanical control system: G 0 γ0 = Y (γ)u ∇γ a a 1 n a=1 (i) An n-dimensional configuration manifold Q, coordinates (q , . . . , q ) X the input functions ua are assumed Lebesgue measurable (ii) An inertia tensor G describing the kinetic energy 1 n G defines an inner product , between vector fields on Q (iv) In coordinates (q , . . . , q ), Christoffel symbols: hh· ·ii (iii) the potential energy V (function on Q) k 1 `k ∂G`j ∂G`i ∂Gij Γ = G + ij 2 ∂qi ∂qj − ∂q` (iv) m one-forms F 1, . . . , F m, describing m external control forces µ ¶ (v) Equations of motion in coordinates for trajectory γ : I Q: → m k k i j kj a γ¨ + Γijγ˙ γ˙ = G (F )jua a=1 X

fb-jul02-p39 fb-jul02-p40 5.1 Conservative and dissipative forces 6 Satellites and vehicles - systems on groups (i) potential energy V due to gravity gives rise to a force F = dV and a vector − (i) configuration is rotation matrix R field grad V . More generally, we shall assume an arbitrary vector field Y0(q) 3 3 T − R SO(3) = R R × R R = I, det R = +1 in the equations of motion ∈ { ∈ | } (ii) define operator as: ω y = ωy (ii) damping or dissipation force is of the form F = R(vq). R stands for Rayleigh · × dissipation function (i.e., a linear dissipation function. (iii) kinematic equation R˙ = Rω b b T The tensor R: TQ TQ is dissipative if follows from differentiating identity: R R = I3 → ω body velocity in body-frameb R(v ) , v 0 hh q qii ≤ 1 T (iv) Kinetic energy: K = 2 ω Jω Strict inequality for strictly dissipative forces remarkable because R is not present! In summary, a simple mechanical control system with dissipation and potential (v) no potential, and torques τ expressed body frame energy satisfied (vi) Euler Poincar`e equations of motion: G γ0 = Y0(γ) + R(γ0) + Y (γ)u(t) ∇γ0 R˙ = Rω 1 1 ω˙ = J− (Jω ω) + J− τ b × fb-jul02-p41 fb-jul02-p42

6.1 Mechanical control systems on matrix groups

(vii) if, for example, J = diag J1, J2, J3 (i) g G is configuration on n-dimensional matrix group { } ∈ local coordinates via x = log(g) ω˙ = ((J J )/J )ω ω + τ /J 1 2 − 3 1 2 3 1 1 ω˙ 2 = ((J3 J1)/J2)ω1ω3 + τ2/J2 (ii) kinetic energy = 1 vT v with > 0 − 2 I I n KE ω˙ 3 = ((J1 J2)/J3)ω1ω2 + τ3/J3 v R velocity in body frame − ∈ these are also called the Euler equations 1 m n (iii) body-fixed forces f , . . . , f (R )∗. ∈ (viii) (ω1, ω2, ω3) are pseudo-velocities, not the time derivative of any quantity on SO(3)

φ T Example: log(R) = 2 sin φ (R − R ), 2 cos φ = tr(R) − 1

fb-jul02-p43 fb-jul02-p44

6.3 Equations of motion, II

i i a b −1 k i γ¨ + Γab(γ)γ˙ γ˙ = (G F ) uk 6.2 Equations of motion, I Euler-Poincar´e eqns:

Kinematic eqns: g˙ = gv g˙ = g v i i j k 1 a i v˙ + Γ v v = (I− f ) ua(t) n n n jk b where v v is isomorphism R R × . a ←→ ←→ b X i where the Γjk are constants determined by G and I. Lie bracket is bmatrix commutator: [[v, w] = (vw wv) − Symmetric product: v : w i = Γi (vawb + vbwa) b b bb h i − ab R˙ = Rω Example: [ω, y] = ω × y Example: b −1 Ω˙ + J (Ω × JΩ) = 0 −1 hΩ : Ξi = J (Ω × JΞ + Ξ × JΩ) fb-jul02-p45 fb-jul02-p46 6.4 Satellite with Thrusters 6.5 Hovercraft configuration is rotation matrix R • kinematic equation: (i) Configuration:

• R˙ = RΩ 

cos θ sin θ x  © where ¨  b f2 P =  sin θ cos θ y  0 Ω3 Ω2 − − § 3 0 0 1    Ω R  Ω3 0 Ω1   ∈ 7→ −  

¥ ¦ ¡ £ ¤ £ 1 2 2 2 ¢  Ω2 Ω1 0  (ii) KE = (Jω + mvx + mvy) −  f1 2   kinetic energy: • 1 T (iii) f1 = e2, f2 = he1 + e3 KE = 2 Ω JΩ − two torques: f = e , f = e • 1 1 2 2 Equations of Motion:

Equations of Motion: 0 ω v Jω˙ = hu • − x − 2 ˙ R˙ = RΩ P = P ω 0 vy ,  mv˙x = mωvy + u1   Ω˙ = Ω Ω + e u (t) + e u (t). 0 0 0  mv˙y = mωvx + u2 J J b 1 1 2 2   − ×    

fb-jul02-p47 fb-jul02-p48 6.7 Planar underwater vehicle, cont’d

(i) to model ideal fluid, include added masses into kinetic energy: 1 1 K = (m v2 + m v2) + Jω2 2 x x y y 2 6.6 Planar underwater vehicle Notice θ, x, y are not present in energy

 (ii) generalized forces in body coordinates F = [fθ fx fy]  !

  (iii) Euler Poincar`e equation for planar underwater vehicle: "

 0 ω v − x # ˙ P = P ω 0 vy        0 0 0    Same kinematic description as hovercraft. However, effects of fluid. Jω˙ = (m m )v v + f x − y x y θ mxv˙x = myvyω + fx m v˙ = m v ω + f y y − x x y fb-jul02-p49 fb-jul02-p50 6.9 Proof of Euler Poincar`e equation for satellite, page 1/3 6.8 Underwater Vehicle in Ideal Fluid Let us consider geodesic equation without forces:

G γ0γ0 = 0 3D rigid body with three forces: ∇ (i) (R, p) SE(3), (Ω, V ) R6 ∈ ∈ The geodesic equation is written on a generic manifold. To write it with respect to ∂ ∂ 1 T 1 T coordinates ( ∂q1 , . . . , ∂qn ) on TQ, follow the steps: (ii) KE = 2 Ω JΩ + 2 V MV , = diag m1, m2, m3 , ∂ M γ0 = γ˙ { } i ∂qi J = diag J1, J2, J3 { } ∂ ∂ ∂ ∂ ∂ G 0 G 0 G ∂ γ γ˙ i i = γ¨i i + γ˙ k γ k = γ¨i i + γ˙ kγ˙ j k (iii) f1 = e4, f2 = he3 + e5, f3 = he2 + e6 ∇ ∂q ∂q ∇ ∂q ∂q ∇ ∂qj ∂q − µ ¶ µ ¶ where the last two steps exploit the properties of affine connections. Equations of Motion: At this point, the Christoffel symbols are computed by using:

G 2 Z , X Y = X Y , Z + Y X , Z Z Y , X R˙ = RΩˆ JΩ˙ = JΩ Ω + MV V hh ∇ ii hh ii hh ii − hh ii , × × [X, Z] , Y [Y, Z] , X [X, Y ] , Z (1) p˙ = RV MV˙ = MV Ω. − hh ii − hh ii − hh ii × where X, Y, Z take values in ∂ , and hence all Lie brackets [ ∂ , ∂ ] vanish. { ∂qi } ∂qi ∂qj

fb-jul02-p51 fb-jul02-p52

6.10 Proof of Euler Poincar`e equation for satellite, page 2/3 6.11 Proof of Euler Poincar`e equation for satellite, page 3/3

We here perform the same procedure, but with respect a basis of invariant vector Assume X, Y, Z take values in the basis E , and prove that { i} fields (i.e., all vector fields are expressed in the body-fixed frame) 1 1 G 1 1 Rej Rek = R ej ek + J− (ej Jek) + J− (ek Jej) . Think of γ as a curve on group of matrices, and write ∇ × 2 × 2 × µ ¶ b γ0(t) = ωi(t)Ei(γ(t)), This is a consequenceb of equation (1) and of the fact that the Lie brackets [Ei(R), Ej(R)] = R(e\i ej) and that the metric is invariant. Ei(R) = Rei, × Therefore, the geodesic equation becomes: where R SO(3) and e = [1, 0, 0] and accordingly e and e . We can do this ∈ 1 b 2 3 because of 1 1 1 1 0 = γ ω˙ iei + ωkωj ej ek + J− (ej Jek) + J− (ek Jej) × 2 × 2 × µ µ ¶¶ TR(SO(3)) = span Re1, Re2, Re3 { } and, using the fact that γ is an invertible matrix and a few simplification, we get According to the same steps as above, the geobdesicbequationb is: the right equation:

1 1 0 = ω˙ E (γ) + ω ω G E (γ) 0 = ω˙ + J− (ω Jω) i i k j ∇Ej (γ) k 2 × ¡ ¢ fb-jul02-p53 fb-jul02-p54

Lecture #2: Modeling Symmetries and Incomplete List of References on Systems with Constraints [1] J. L. Synge. Geodesics in nonholonomic geometry. Mathematische Annalen, 99:738–751, 1928.

[2] A. M. Vershik. Classical and non-classical dynamics with constraints. In Y. G. Borisovich and Yu. E. Nonholonomic Constraints Gliklikh, editors, Global analysis: studies and applications, volume 1108 of Lecture Notes in Mathematics, pages 278–301. Springer Verlag, New York, NY, 1984. Francesco Bullo [3] N. Sarkar, X. Yun, and V. Kumar. Control of mechanical systems with rolling constraints: Application to dynamic control of mobile robots. International Journal of Robotics Research, 13(1):55–69, 1994. Coordinated Science Lab, General Engineering Dept, ECE, AAE [4] A. J. van der Schaft and B. M. Maschke. On the Hamiltonian formulation of nonholonomic mechanical University of Illinois at Urbana-Champaign systems. Reports in Mathematical Physics, 34(2):225–233, 1994.

1308 W. Main St, Urbana, IL 61801, USA [5] A. M. Bloch and P. E. Crouch. Nonholonomic control systems on Riemannian manifolds. SIAM Journal on [email protected], http://motion.csl.uiuc.edu Control and Optimization, 33(1):126–148, 1995. [6] A. M. Bloch, P. S. Krishnaprasad, J. E. Marsden, and R. M. Murray. Nonholonomic mechanical systems This lecture based on the following references with symmetry. Archive for Rational Mechanics and Analysis, 136(1):21–99, 1996.

[1] A. D. Lewis, “Simple mechanical control systems with constraints,” IEEE TAC, 45(8):1420–1436, 2000. [7] J. P. Ostrowski and J. W. Burdick. The geometric mechanics of undulatory robotic locomotion. International Journal of Robotics Research, 17(7):683–701, 1998. [2] F. Bullo and M. Zefran,ˇ “On mechanical control systems with nonholonomic constraints and symmetries,” Syst. & Control L., 45(2):133–143, 2002. [8] P. S. Krishnaprasad and D. P. Tsakiris. Oscillations, SE(2)-snakes and motion control: a study of the roller racer. Dynamics and Stability of Systems, 16(4):347–397, 2001. [3] F. Bullo and M. Zefran,ˇ “Modeling and controllability for a class of hybrid mechanical systems,” IEEE TRA, Aug 2002. To appear.

fb-jul02-p55 fb-jul02-p56 7.2 Coordinate-free modelling: II 7 Essential review (i) given functions Γi , and curve γ : I R jk → n o 7.1 Coordinate-free modelling: I i i i j k ( 0 γ0) = γ¨ + Γ γ˙ γ˙ = 0 . . . geodesic flow ∇γ jk manifold Q, metric G • vector fields are written in terms of the canonical basis ∂ , . . . , ∂ , and ∂q1 ∂qn (ii) Given two vector fields X, Y , the covariant derivative of Y with respect to X • 1 n { } co-vector fields in terms of dq , . . . , dq is the third vector field Y defined via { } ∇X given a function ϕ: ∂Y i • ( Y )i = Xj + Γi Xj Y k. ∂ϕ ∇X ∂qj jk dϕ = dqi ∂qi

ij ∂ϕ ∂ grad ϕ = G (iii) symmetric product ∂qj ∂qi µ ¶ Ya : Yb = Ya Yb + Yb Ya q˙ = grad ϕ(q) . . . (negative) gradient flow h i ∇ ∇ − i i i ∂Ya j ∂Yb j i j k k j metric gives rise to connection with certain properties Ya : Yb = Y + Y + Γ Y Y + Y Y • h i ∂qj b ∂qj a jk a b a b ¡ ¢ fb-jul02-p57 fb-jul02-p58 8 Introduction to systems subject to constraints 

¡

7.3 Coordinate-free modelling: III   ¤

¨ §



affine connection control system ¥    £

  ¦ m © ¢   γ0 = Y0(γ) + R(γ0) + Ya(γ)ua(t) ∇γ0 a=1 X Ex #1: robotic manipulators with kinetic energy and forces at joints Constraints can be of two types: simple systems with conservative forces (i) constraints on q are called integrable Ex #2: aerospace and underwater vehicles invariant systems on Lie groups (ii) constraints on vq are sometimes called non-integrable Ex #3: systems subject to nonholonomic constraints from the greek roots: locomotion devices with drift, e.g., bicycle, snake-like robots integrable = holonomic nonintegrable = nonholonomic

fb-jul02-p59 fb-jul02-p60 8.1 Integrable constraints   Sometimes, an integrable constraints appears as: •

w , γ0 = 0, h i where, if w = dϕ, one writes   d dϕ , γ0 = ϕ(γ(t)) ϕ(γ(t)) = constant h i dt   Problem: given an arbitrary co-vector w, when is it w = dϕ ? • constraint on the configuration, such as clamping. It is given by • Locally, construct annihilator distribution D. If D is involutive, then ϕ(q) = 0 w is a holonomic constraint.

where ϕ : Q R → easy case, analyse on smaller space • fb-jul02-p61 fb-jul02-p62 8.2 Nonintegrable constraints I: kinematic systems Examples of kinematic systems

(x2, y2, θ2) (x1, y1, θ1)

¤ £

y L2 ¡ § ¨ ¨ ©

¦ ¢

L1 ¥ (x0, y0, θ0)

x [Car with trailer can be parked anywhere.]

x˙ = v cos φ nonintegrable constraints are constraints on velocity, that cannot be x˙ r cos θ 0 • y˙ = v sin φ written as constraints on configurations y˙r   sin θ  0 φ˙ = ω = v + ω  θ˙   1 tan φ 0 classic example is rolling without sliding    `    • (wheeled robot dynamics)        φ˙   0  1 If system has full control over all feasible velocities,       •       then kinematic analysis suffices Test: set all control inputs to zero, does the mechanical systems still move? driftless systems

fb-jul02-p63 fb-jul02-p64 9 Simple Mechanical Control Systems with constraints 8.3 Nonintegrable constraint II: dynamic systems Nonholonomic constraint described by constraint one-form ω        ω , γ0 = 0 h i      

 A simple mechanical control system subject to constraints   1 m 

 (i) A simple mechanical control system (Q, , V = 0, = F , . . . , F )  G F { }   (ii) A collection of constraint one-forms ω , . . . , ω . { 1 p} general case is a dynamic case, i.e., system can move with input at zero • The annihilator of span ω , . . . , ω is the constraint distribution D { 1 p} i.e., the distribution of feasible velocities basic example: bicycle • Orthogonal projections:

P : TQ D TQ and P ⊥ : TQ D ⊥ TQ → ⊂ → ⊂ fb-jul02-p65 fb-jul02-p66 9.1 Equations of motion 9.2 Expressions in coordinates The solution to the mechanical control system subject to the constraint distri- (i) design = X1, . . . , Xn p an orthogonal basis for feasible velocities D bution D is the curve γ : I Q solution to X { − } → k 1 m G (ii) compute (XΓ)ij = 2 Xi Xj , Xk G 1 a Xk hh ∇ ii γ0(t) = λ(t) + (G− F )ua k k ∇γ0(t) a=1 1 X k a (iii) compute Ya = 2 F , Xk P (γ ) = 0 Xk h i ⊥ 0 k k

where t λ(t) D ⊥ is the Lagrange multiplier, and γ0(0) D. Then the constrained equations of motion are 7→ ∈ ∈ Theorem: Constrained equations of motion (Synge 1928) i m γ0(t) = v (t)Xi(γ(t)) D 1 a γ0 γ0 = (P G− F )ua ∇ m a=1 k k i j k X v˙ (t) + (XΓ)ij v (t)v (t) = Ya (γ)ua(t) a=1 with respect to the constrained affine connection (Lewis 2000) X D Y = G Y + G P ⊥ (Y ) ∇X ∇X ∇X ¡ ¢ kinematic + dynamic equations

fb-jul02-p67 fb-jul02-p68 9.3 Comments

Constrained equations of motion

i γ0 = v Xi(γ) m k k i j k v˙ + (XΓ)ijv v = Ya ua Invariance under group action If a system is invariant under a group action and a=1 X the basis for D consists of invariant vectors, the generalized Christoffel symbols i ( Γ)k and the coefficients of the control vector fields Y k are invariant. (i) v components of γ0 are pseudo-velocity X ij a

k D (ii) (XΓ) are generalized Christoffel symbols for with respect to ij ∇ Key examples easily handled see next pages. X1, . . . , Xn { } Missing work Still to work out: bicycle, plate-and-ball systems, omni-directional, D k X Xj = (XΓ) Xk redundant, variable-geometry vehicles ∇ i ij however, no need to compute the projection P , nor its covariant

derivative G P ⊥ ∇ k (iii) Ya is the projection of the control vector fields onto Xk. If conservative forces, a k 1 i.e., F = dϕ , then Y = 2 L ϕ a a Xk Xk a k k fb-jul02-p69 fb-jul02-p70

10 The snakeboard example Inertia tensor: m 0 0 0 0  0 m 0 0 0  ¡ ¢

¡ 2 ¢ £ [ ] =  0 0 ` m J 0  G  r   

¢  0 0 Jr Jr 0  ¡ ¢      0 0 0 0 Jw   Constraints:   ¤ ¥ x˙ sin(θ φ) y˙ cos(θ φ) = 0 front − − front −

¦ x˙ sin(θ + ψ) y˙ cos(θ + ψ) = 0 back − back Constraint forms: Configuration manifold: SE(2) S2 × ω = sin(φ θ)dx + cos(φ θ)dy + ` cos φdθ Coordinates: q = (x, y, θ, ψ, φ) 1 − − ω = sin(φ + θ)dx + cos(φ + θ)dy ` cos φdθ Input forces: dψ, dφ 2 − −

fb-jul02-p71 fb-jul02-p72

10.1 Application of the method 10.2 Kinematic and dynamic equations Step (i): Choice of basis for D:

Jr ∂ ∂ ∂ x˙ ` cos φ cos θ m` cos φ sin φ cos θ 0 X1 = ` cos φ cos θ + ` cos φ sin θ sin φ , ∂x ∂y − ∂θ      Jr    y˙ ` cos φ sin θ m` cos φ sin φ sin θ 0 ∂ ∂ θ˙ =  sin φ  v1 +  Jr (sin φ)2  v2 + 0 v3 X20 = , X30 = .      m`2    ∂ψ ∂φ    −   −    ψ˙  0   1  0         Using the Gramm-Schmitt procedure we can construct the orthogonal basis:         φ˙  0   0  1         Jr Jr 2 ∂ ∂ X = cos φ sin φV sin φ + , X = X0         2 m` x − m`2 ∂θ ∂ψ 3 3 J v˙ 1 + r (cos φ)v2v3 = 0 m`2 Step (ii): compute generalized Christoffel symbols 2 2 2 m` cos φ 1 3 Jr cos φ sin φ 2 3 m` 2 v˙ 2 2 v v 2 2 v v = 2 2 2 uψ 1 Jr 2 m` cos φ 2 Jr cos φ sin φ − m` + Jr(sin φ) − m` + Jr(sin φ) m` Jr + Jr (sin φ) (XΓ) = cos φ , (XΓ) = , (XΓ) = 32 2 31 2 2 32 2 2 m` −m` + Jr sin φ −m` + Jr sin φ 3 1 v˙ = uφ . Jw L L Step (iii): input coefficients: X2 ψ = 1, X3 φ = 1 fb-jul02-p73 fb-jul02-p74 10.3 Kinematic and dynamic equations 10.4 Software implementation Mathematica FullSimplify the kinematic equations are implementation; commands erased for readability (* CONNECTIONS AND OTHER OPERATIONS *) Jr x˙ ` cos φ cos θ m` cos φ sin φ cos θ LieDer[X_,h_,x_] := Sum[D[h,x[[i]]]X[[i]],{i,Length[x]}]; = v + Jr ψ˙ y˙ ` cos φ sin θ  m` cos φ sin φ sin θ ˙ Jr 2 θ  sin φ   2 (sin φ)  LieBracket[X_,Y_,x_]:=Module[{i,j,N=Length[x]},    −   − m`        Table[Sum[D[Y[[i]],x[[j]]]X[[j]]-D[X[[i]],x[[j]]]Y[[j]],{j,N}],{i,N}]]; and the dynamic equations are LeviCivita[\metric_,x_]:=Module[{Minv=Inverse[M],i,j,k,h, J v˙ + r (cos φ)φ˙ψ˙ = 0 N=Length[x]},Table[Sum[Minv[[h,k]](D[M[[h,j]],x[[i]]]+ m`2 D[M[[i,h]],x[[j]]] - D[M[[i,j]],x[[h]]])/2,{h,N}],{k,N},{j,N},{i,N}]]; 2 ¨ m` cos φ ˙ Jr cos φ sin φ ˙ ˙ ψ 2 2 vφ 2 2 φψ − m` + Jr(sin φ) − m` + Jr(sin φ) CovariantDer[X_,Y_,Nabla_,x_]:=Module[{i,j,k,N=Length[x]}, 2 m` Table[Sum[D[Y[[i]],x[[j]]]X[[j]]+ = 2 2 2 uψ m` Jr + Jr (sin φ) Sum[Nabla[[i,j,k]]X[[j]]Y[[k]],{k,N}],{j,N}],{i,N}]]; 1 φ¨ = uφ . Jw

fb-jul02-p75 fb-jul02-p76 (* CHRISTOFFEL SYMBOLS *) (* SNAKEBOARD EXAMPLE *) X = {X1, X2, X3}; norms = {X1X1, X2X2, X3X3}; q = {x,y,th,psi,phi}; M ={{m,0,0,0,0},{0,m,0,0,0}, Tnabla = Table[ CovariantDer [X[[i]],X[[j]],nabla,q].M.X[[k]]/norms[[k]] {0,0,m ell^2,Jr,0},{0,0,Jr,Jr,0},{0,0,0,0,Jw}}; ,{k,1,3} ,{i,1,3}, {j,1,3}]; nabla = LeviCivita[M, q];

(* INPUTS *) (* FEASIBLE VELOCITIES *) F = Table[ LieDer[X[[k]],psi,q]/norms[[k]]u1 Vx = {Cos[th],Sin[th],0,0,0}; Vth = {0,0,1,0,0}; + LieDer[X[[k]],phi,q]/norms[[k]]u2 ,{k,3}]; X1 = ell Cos[phi] Vx - Sin[phi] Vth; X2p = {0,0,0,1,0}; X3 = {0,0,0,0,1}; (* EQUATIONS OF MOTION *)

v={vel[t], psi’[t], phi’[t]}; EqMotion = Table[ (* ORTHOGONALIZE VECTORS VIA GRAMM-SCHMITT *) D[v[[k]],t]+Sum[Tnabla[[k,i,j]] v[[i]] v[[j]],{i,3},{j,3}]==F[[k]], {k,3}]; X1X1 = X1.M.X1; X2pX2p = X2p.M.X2p; X3X3 = X3.M.X3; X1X3 = X1.M.X3; X1X2p = X1.M.X2p; X2pX3 = X2p.M.X3; (* CONTROLLABILITY ANALYSIS *) X2 = X2p-X1(X1X2p/X1X1); X2X2 = X2.M.X2; X13 = LieBracket[X1,X3,q]; X113 = LieBracket[X1,X13,q]; Det[AppendColumns[{X1},{X2},{X3},{X13},{X113}]]; fb-jul02-p77 fb-jul02-p78 11 The roller racer example ¦ Inertia tensor: m 0 0 0  0 m 0 0  ¢ ¤ [G] = .  0 0 I + I I   1 2 2    0 0 I2 I2 ¥   Constraint one-forms:   ¡ ¢ £ ω = sin θdx cos θdy 1 − ω = sin(θ + ψ)dx cos(θ + ψ)dy 2 − (` + ` cos ψ)dθ ` dψ . − 2 1 − 2 Configuration manifold: SE(2) S × Coordinates: q = (x, y, θ, ψ) Input force: dψ

fb-jul02-p79 fb-jul02-p80

11.1 Application of the method Step (ii): compute generalized Christoffel symbols Step (i): Choice of basis for D: 1 `1 + `2 cos ψ (I1 + I2) sin ψ ∂ ∂ sin ψ ∂ (XΓ)21 = X1 = cos θ + sin θ + `2 + `1 cos ψ f1(ψ) ∂x ∂y `2 + `1 cos ψ ∂θ µ ¶ µ ¶ 1 m(`1 + `2 cos ψ)(`2 + `1 cos ψ)(`1I2 cos ψ `2I1) (XΓ) = − `2 ∂ ∂ 22 f (ψ)2 X20 = + 1 − `2 + `1 cos ψ ∂θ ∂ψ µ ¶ 2 `1 + `2 cos ψ m(`1I2 cos ψ `2I1) (XΓ) = − Using the Gramm-Schmitt procedure we can construct the orthogonal basis: 21 ` + ` cos ψ f (ψ) µ 2 1 ¶ 2 (` I ` I cos ψ) sin ψ m` (` + ` cos ψ) + I sin2 ψ ∂ ∂ 2 m(`1I2 cos ψ `2I1)(sin ψ)f3(ψ) 2 1 1 2 2 2 1 2 (XΓ)22 = − − X2 = − Vx + . f1(ψ)f2(ψ) f1(ψ) − f1(ψ) ∂θ ∂ψ

∂ ∂ where f3(ψ) = (`1I2 `2I1 cos ψ) + m`1`2(`2 + `1 cos ψ). where Vx = cos θ + sin θ − ∂x ∂y 1 f (ψ) f (ψ) = m(` + ` cos ψ)2 + (I + I ) sin2 ψ L L 1 1 2 1 1 2 Step (iii): input coefficients: X1 ψ = 0, 2 X2 ψ = X2 f2(ψ) 2 2 2 2 k k f2(ψ) = m`2I1 + `1I2m(cos ψ) + I1I2 sin ψ . fb-jul02-p81 fb-jul02-p82 12 Proofs

11.2 Kinematic and dynamic equations 12.1 Constrained affine connection the kinematic equations are Consider

(` I ` I cos ψ) sin ψ G 1 x˙ cos θ 2 1− 1 2 cos θ γ0 γ0 = λ(t) + G− F (2) f1(ψ) ∇ (` I ` I cos ψ) sin ψ y˙ =  sin θ  v +  2 1− 1 2 sin θ ψ˙ P ⊥(γ0) = 0. (3) f1(ψ) 2 θ˙  sin ψ   m`2(`2+`1 cos ψ)+I2(sin ψ)     `2+`1 cos ψ   f1(ψ)  Project equation (2) onto D ⊥, and covariantly differentiate equation (3):      −  G 1 and the dynamic equations are P ⊥( γ0 γ0) = λ(t) + P ⊥(G− F ) ∇ G G G 1 ˙ 1 ˙ 2 γ0 P ⊥(γ0) = 0 P ⊥( γ0 γ0) = γ0 P ⊥ (γ0). v˙ + (XΓ)21(ψ)ψv + (XΓ)22(ψ)ψ = 0 ∇ ∇ − ∇ ¡ ¢ ¡ ¢ ¨ 2 ˙ 2 ˙ 2 f1(ψ) Hence: ψ + (XΓ)21(ψ)ψv + (XΓ)22(ψ)ψ = uψ . G 1 f2(ψ) λ(t) = γ0 P ⊥ (γ0) P ⊥(G− F ) − ∇ − and ¡ ¢ G G 1 γ0 γ0 + γ0 P ⊥ (γ0) = P (G− F ), ∇ ∇ ¡ ¢

fb-jul02-p83 fb-jul02-p84 12.2 Constrained equations in coordinates

G G G Define: Definition 1 X P ⊥ (Y ) = X P ⊥(Y ) P ⊥( X Y ). ∇ ∇ − ∇ D G G X Y = X Y + X P ⊥ (Y ) ¡ ¢D ¡G ¢ ∇ ∇ ∇ Lemma 2 For Y D, X Y = P ( X Y ) ¡ ¢ ∈ ∇ ∇ Summarizing: D 0 D G G 1 Lemma 3 Expression for γ γ0, where Xi orthogonal family spanning : γ0 γ0 + γ0 P ⊥ (γ0) = P (G− F ), ∇ { } ∇ ∇ D D i i i D 0 γ0 = 0 (v X ) = v˙ X + v 0 X can be written as ¡ ¢ ∇γ ∇γ i i ∇γ i D 1 i i jD γ0 γ0 = P (G− F ) = v˙ X + v v X ¡ ¢ ∇ i ∇Xj i where D is the constrained connection. ∇ Inner product with Xk:

The Christoffel symbols of the constrained connection with respect to the basis D i i j D 0 ∂ ∂ Xk , γ γ0 = v˙ Xk , Xi + v v Xk , Xj Xi ∂q1 , . . . , ∂qn are hh ∇ ii hh ii hh ∇ ii { } k 2 i j D = v˙ Xk + v v Xk , X Xi P k k hh ∇ j ii (DΓ)k = Γk + kj + Γk P ΓmP ij ij ∂qi im mj − ij km Final simplification:

D X , X = P G X , X = G X , X hh ∇Xi j kii hh ∇Xi j kii hh ∇Xi j kii fb-jul02-p85 fb-jul02-p86

13.1 Definition of impact

(Q, G, = span F 1, . . . , F m ) is a simple mechanical system 13 Ideal impact models • F { } + D − and D are two set of feasible velocities (right before, right after impact) here only ideal case: no friction, plastic/elastic, holonomic/nonholonomic impact • + + • ( −, P − ) and ( , P ) give eqns of motion, impact entails • ∇ F ∇ F • (P is orthogonal projection onto feasible velocities) (i) impulsive force that causes a jump in γ 0 The system undergoes an impact at time t if (ii) switch in equations of motions + + (i) the dynamics switch from ( −, P − ) to ( , P ), ∇ F ∇ F Reference on impact models (ii) there exists a tensor field J : T Q T Q such that q q → q [1] B. Brogliato. Nonsmooth Impact Mechanics: Models, Dynamics, and Control, volume 220 of Lecture Notes + in Control and Information Sciences. Springer Verlag, New York, NY, 1996. q(t ) = q(t−) + γ0(t ) = Jq γ0(t−) . ¡ ¢

fb-jul02-p87 fb-jul02-p88

13.2 Classic impacts 13.3 Hybrid mechanical control systems Plastic impact from large to smaller space: The two sets of feasible velocities D D + D D + − and are distinct (for example − = TQ and = TR is the tangent given a mechanical control system (Q, G, ) with a given set of constraint F space of a submanifold R Q). The operator distributions D , where i belongs to an index set I. ⊂ i D Jq = PD+ For each constraint i, we consider the constrained mechanical control system Σi = [Q, G, , Di, U], with associated i and i. is the orthogonal projection onto D +. F ∇ Y We define the hybrid mechanical control system as Elastic impact against surface: The equations of motion do not change, as V connection and input forces do not change. There exist a submanifold R such HMCS = [I, Q, ΣQ, , ∆] (4) that where I index set, Q, ΣQ collection of constrained mech. sys., V = vij i,j I { } ∈ Jq = PTR + ( e)P ⊥ − TR discrete controls and ∆ jump transition maps (linear operators in γ0 parametrized where PTR is the orthogonal projection onto the tangent space to R and where by vij ). 0 < e < 1 is the coefficient of restitution. fb-jul02-p89 fb-jul02-p90

Summary of Modeling Methods (lectures #1 and #2) Given the metric G and the distribution D, we define the following objects. We let P : T Q T Q be the orthogonal projection onto the distribution D with respect to → Simple mechanical control systems with constraints the metric G. We let G be the Levi-Civita connection on Q induced by the metric ∇ G. We let be the constrained affine connection defined by the metric G and A simple mechanical control system with constraints is a quintuple ∇ the constraint distribution D according to (Q, G, V, D, F ) comprised of the following objects: G G (i) an n-dimensional configuration manifold Q, X Y = X Y X P (Y ), ∇ ∇ − ∇ (ii) a Riemannian metric G on Q describing the kinetic energy, for any vector fields X and Y . When the vecto¡ r field¢ Y takes value in D, we have (iii) a function V on Q describing the potential energy, Y = P (G Y ), ∇X ∇X (iv) a distribution D of feasible velocities describing the linear velocity constraints, and (v) a collection of m covector fields = F 1, . . . , F m , linearly independent at F { } each q Q, defining the control forces. ∈

fb-jul02-p91 fb-jul02-p92

A controlled trajectory for the mechanical control system with constraints 1 Given the Riemannian metric G, we let G: T Q T ∗Q and G− : T ∗Q T Q → → (Q, G, V, D, F ) is a pair (γ, u) with γ : [0, T ] Q and denote the musical isomorphisms associated with . For a 1, . . . , m , we define → G u = (u , . . . , u ): [0, T ] m satisfying the controlled geodesic equations 1 a ∈ { } 1 m R the input vector fields Ya = P (G− (F )), the family of input vector fields → m = Y1, . . . , Ym , and the input distribution Y with Y { } γ˙ (t)γ˙ (t) = P (grad V (γ(t))) + Ya(γ(t))ua(t). (5) Y = span Y (q), . . . , Y (q) . Let L f be the Lie derivative of a scalar function ∇ − q R{ 1 m } X a=1 f with respect to the vector field X. The gradient of the function V is the vector X Here we assume that γ˙ (0) D and comment that this implies that field grad V defined implicitly by ∈ γ(0) γ˙ (t) Dγ(t) for all t [0, T ]. Furthermore, we assume the input functions ∈ ∈ m (grad V, X) = LX V. u = (u1, . . . , um): [0, T ] R to be Lebesgue measurable functions, and we write G → u U m . ∈ dyn fb-jul02-p93 fb-jul02-p94 Coordinate representation #1 Coordinate representation #2

On an open subset U Q let = X1, . . . , Xn be a basis of vector fields, and ⊂ X { } Let (q1, . . . , qn) be a coordinate system for the open subset U Q. The curve set 1 n ⊂ k γ : I U has therefore components (γ , . . . , γ ). The coordinate system on U Xi Xj = (XΓ)ij Xk, (6) → ∇ induces the natural coordinate basis ∂ , . . . , ∂ for the tangent bundle TU. 3 k ∂q1 ∂qn where the n functions (XΓ) i, j, k 1, . . . , n are called the generalized { } { ij | ∈ { }} With respect to this basis, we write the velocity curve γ˙ : I TU as Christoffel symbols with respect to . Given vector fields Y and Z on U, we can → i i X ∂ write Y = Y Xi and Z = Z Xi. Accordingly, γ˙ (t) = γ˙ i(t) (γ). ∂qi L k i k i j Y Z = Xi Z Y + (XΓ)ij Z Y Xk. ∇ In the coordinate system (q1, . . . , qn), we write γ = (γ1, . . . , γn), ¡¡ ¢ ¢ Let the velocity curve γ˙ : I TU have components (v1, . . . , vn) with respect to γ˙ = (γ˙ 1, . . . , γ˙ n), and the equations of motion read → , i.e., m X i k k i j k k γ˙ (t) = v (t)Xi(γ(t)). γ¨ + Γ γ˙ γ˙ = (P grad V ) (γ) + Y u . (8) ij − a a a=1 The pair (γ, u) is a controlled trajectory for the controlled geodesic equations (5) if X and only if it solves the controlled Poincar´e equations Here, the Christoffel symbols Γk i, j, k 1, . . . , n and the terms in the { ij | ∈ { }} m right-hand side are computed with respect to the natural coordinate basis. We k k i j k k v˙ + (XΓ) (γ)v v = (P grad V ) (γ) + Y (γ)u . (7) ij − a a refer to these equations as the controlled Euler-Lagrange equations. a=1 X

fb-jul02-p95 fb-jul02-p96

Remarks

(i) If the distribution D has rank p < n, it is useful to construct a local basis for Lecture #3: Perturbation Analyses of TQ by selecting the first p vector fields to generate D, and the remaining k Affine Connection Control Systems n p to generate D ⊥. In this case, one can see that v (t) = 0 for all time t − and all k p + 1, . . . , n . ∈ { } Francesco Bullo (ii) Assume a Lie group G acts on the manifold Q, and assume the metric G, and Coordinated Science Lab, General Engineering Dept, ECE, AAE the distribution D are invariant. Then the constrained connection is ∇ University of Illinois at Urbana-Champaign invariant, and, selecting invariant vector fields X1, . . . , Xn , the generalized { } 1308 W. Main St, Urbana, IL 61801, USA Christoffel symbols are invariant functions. [email protected], http://motion.csl.uiuc.edu (iii) simple mechanical control systems can be modeled under the general framework of affine connection control systems This lecture based on the following references

m [1] F. Bullo, “Series expansions for mechanical control systems,” SIAM JCO, 40(1):166–190, 2001.

γ0 γ0 = Y0(γ) + R(γ0) + Ya(γ)ua(t) [2] F. Bullo, “Averaging and vibrational control of mechanical systems,” SIAM JCO, Submitted 1999. To ∇ appear 2002. a=1 X [3] F. Bullo, “Series expansions for analytic systems linear in controls,” Automatica, 38(9):1425-1432, 2002. fb-jul02-p97 fb-jul02-p98 13.5 Intro: oscillatory control 13.4 Intro: Perturbation methods for mechanical control systems Known: Oscillatory controls generate motion in Lie bracket directions Before design, analyse forced response of Lagrangian system from rest

1 t 1 t x˙ = f(x) + g (x) sin + g (x) cos I) High magnitude high frequency 1 t 1 √² ² 2 √² ² H = H(q, p) + ϕ q, p, u µ ¶ µ ¶ “oscillatory control & ² ² µ µ ¶¶ vibrational stabilization” p(0) = p0

1 x˙ = f(x) + [g1, g2](x) II) Small input from rest H = H(q, p) + ²ϕ(q, p, u(t)) 2 “small-time local controllability” p(0) = 0 Today’s objective: oscillatory controls in mechanical systems

γ0 γ0 = Y (q, t) III) Classical formulation H = H(q, p) + ²ϕ(q, p) ∇ integrable Hamiltonian systems T p(0) = p0 γ0(0) = 0, Y (q, t)dt = 0 Z0

fb-jul02-p99 fb-jul02-p100 Incomplete List of References on Series Expansion and Averaging related to Mechanical Systems

[1] A. A. Agrachevˇ and R. V. Gamkrelidze, The exponential representation of flows and the chronological calculus, Math. USSR Sbornik, 35 (1978), pp. 727–785. 13.6 Coordinate-free modelling: I [2] J. Baillieul, Stable average motions of mechanical systems subject to periodic forcing, in Dynamics and manifold Q, metric G Control of Mechanical Systems: The Falling Cat and Related Problems, M. J. Enos, ed., vol. 1, Field • Institute Communications, 1993, pp. 1–23. ∂ ∂ vector fields are written in terms of the canonical basis 1 , . . . , n , and [3] R. E. Bellman, J. Bentsman, and S. M. Meerkov, Vibrational control of nonlinear systems: ∂q ∂q • 1 n { } Vibrational controllability and transient behavior, IEEE TAC, 31 (1986), pp. 717–724. co-vector fields in terms of dq , . . . , dq { } [4] P. E. Crouch, Geometric structures in systems theory, IEE Proceedings, 128 (1981), pp. 242–252. given a function ϕ: [5] M. Kawski. Geometric homogeneity and applications to stabilization. In Nonlinear Control Systems Design • Symposium (NOLCOS), pages 251–256, Tahoe City, CA, July 1995. ∂ϕ dϕ = dqi [6] M. Kawski and H. J. Sussmann, Noncommutative power series and formal Lie-algebraic techniques in ∂qi nonlinear control theory, in Operators, Systems, and Linear Algebra, U. Helmke, D. Pratzel-Wolters, and E. Zerz, eds., Teubner, Stuttgart, Germany, 1997, pp. 111–128. ij ∂ϕ ∂ grad ϕ = G [7] M. Levi, “Geometry of Kapitsa’s potentials,” Nonlinearity, vol. 11, no. 5, pp. 1365–8, 1998. ∂qj ∂qi µ ¶ [8] W. Liu and H. J. Sussmann, “Continuous dependence with respect to the input of trajectories of control-affine systems,” SIAM Journal on Control and Optimization, vol. 37, no. 3, pp. 777–803, 1999. q˙ = grad ϕ(q) . . . (negative) gradient flow − [9] J. P. Ostrowski and J. W. Burdick, “The geometric mechanics of undulatory robotic locomotion,” International Journal of Robotics Research, vol. 17, no. 7, pp. 683–701, 1998. [10] J. A. Sanders and F. Verhulst, Averaging Methods in Nonlinear Dynamical Systems, Springer Verlag, New York, NY, 1985. fb-jul02-p101 fb-jul02-p102 13.7 Coordinate-free modelling: II

(i) given functions Γi , and curve γ : I R jk → n o 13.8 Coordinate-free modelling: III i i i j k ( γ0 γ0) = γ¨ + Γjkγ˙ γ˙ = 0 . . . geodesic flow ∇ affine connection control system

m (ii) Given two vector fields X, Y , the covariant derivative of Y with respect to X γ0 = Y0(γ) + R(γ0) + Ya(γ)ua(t) ∇γ0 is the third vector field Y defined via a=1 ∇X X ∂Y i ( Y )i = Xj + Γi Xj Y k. Ex #1: robotic manipulators with kinetic energy and forces at joints ∇X ∂qj jk simple systems with conservative forces Ex #2: aerospace and underwater vehicles (iii) symmetric product invariant systems on Lie groups Y : Y = Y + Y h a bi ∇Ya b ∇Yb a Ex #3: systems subject to nonholonomic constraints

i i locomotion devices with drift, e.g., bicycle, snake-like robots i ∂Ya j ∂Yb j i j k k j Ya : Yb = Y + Y + Γ Y Y + Y Y h i ∂qj b ∂qj a jk a b a b ¡ ¢

fb-jul02-p103 fb-jul02-p104 14 Perturbation Analysis I: 14.1 Averaging for general mechanical systems the “oscillatory control & vibrational stabilization” setting 1 t γ0 = Y0(γ) + R(γ0) + v Y (γ) (Bentsman et al, ’86 – present) vibrational stabilization ∇γ0 ² ² (Baillieul ’93 – present) discovery, study, apps of averaged potential µ ¶

γ0 = Y0(γ) + R(γ0) + Y (γ)u(t) γ0 ∇ γ0 = Y0(γ) + R(γ0) + λ Y : Y (γ) 1 t ∇γ0 h i u(t) = v ² ² µ ¶ where forcing v is T -periodic (i) approximation valid as ² 0 on the time scale t [0, 1] → ∈ T T s1 (ii) approximation valid as ² 0 on the time scale t [0, ), v(s )ds = v(s )ds ds = 0 → ∈ ∞ 1 1 2 1 2 if (γ, γ ) = (0, 0) is an hyperbolically stable critical point Z0 Z0 Z0 0 and let 1 T s1 2 λ = v(s )ds ds 2T 2 2 1 Z0 µZ0 ¶ fb-jul02-p105 fb-jul02-p106 14.2 Ex #1: a 2-link manipulator 14.3 Ex #2: the roller racer PSfrag replacements PSfrag replacements

π/2 £

0 π/2 20 (rad) 2 ¦ ¨

40 θ ,

1 0 θ

60 60 ¡ ¢ time (sec) © 20 40 θ1, θ2 (rad) time (sec) ¥ ¦ § ¤ 1 t u = θ + cos − 1 ² ² µ ¶ (i) recall X1, X2 two vector fields describing feasible velocities of racer

(ii) racer has single input Y = X2 Two-link damped manipulator with oscillatory control at first joint. The averaging (iii) symmetric product Y : Y has component along X1 analysis predicts the behavior. (the gray line is θ1, the black line is θ2). See later h i explanation for stability of (0, π/2). (iv) hence, racer moves forward (or backward?) using zero mean input

fb-jul02-p107 fb-jul02-p108 14.4 Extension: Two-time scales result PSfrag replacements PSfrag replacements

π/2  0 1 t π/2 γ0 = Gravity + Damping + v Y (t, γ) 20 ∇γ0 ² ² (rad)

µ ¶ 2

40 θ ,

1 0 θ v(t) is T -periodic and cyclic 60 60

time (sec) 20 40 θ1, θ2 (rad) time (sec)

γ0 = Gravity + Damping + λ Y : Y (t, γ) ∇γ0 h i 1 T s1 2 1 t λ = v(s )ds ds u = θ1 + cos 2T 2 2 1 − ² ² Z0 µZ0 ¶ µ ¶

why stable? as ² 0 on appropriate time scale → fb-jul02-p109 fb-jul02-p110

15 Simplified averaging analyses for SMCS with 15.1 Analysis I: averaging energy conservative forces In the open loop, integrable forces in the sense of conservative forces: 1 (q, v ) = v 2 + V (q) E q 2k qk i ij ∂ϕ Y (q, t) = grad ϕ(q, t), (grad ϕ) = G ∂qj but for controlled geodesic equations with input vector field m 1 t Symmetric product restricts va grad ϕ (q) ² ² a a=1 X µ ¶ grad ϕa : grad ϕb grad ϕa : ϕb h i ≡ h i Averaged potential and energy where Beltrami bracket (Crouch ’81): 1 2 averaged(q, p) = vq + Vaveraged(q) ab ∂ϕi ∂ϕj E 2k k ϕi : ϕj = dϕi , dϕj = G h i hh ii ∂qa ∂qb V (q) = V (q) + Λab ϕ : ϕ (q) averaged h a bi

Relationship between: (i) certain Lie brackets between vector fields on TQ, (ii) T s1 s1 ab 1 a b symmetric products of vector fields on Q, Beltrami bracket of functions (and, Λ = v (s2)ds2 v (s2)ds2 ds1 2T 0 0 0 averaged potential) Z µZ ¶ µZ ¶

fb-jul02-p111 fb-jul02-p112 PSfrag replacements PSfrag replacements 16 Proofs

π/2 £

0 π/2 16.1 Theorem statement 20 (rad)

2 Consider a control system described by an affine connection

40 θ ,

1 0 θ 60 60 1 ¡ a ¢ 0 γ0 = Y (q) + R(γ0) + Y (γ) v (t/²) (9) time (sec) ∇γ 0 a ² 20 40 θ1, θ2 (rad) time (sec) 1 m where γ0(0) = v , and where v , . . . , v are T -periodic functions st: 0 { } T T s2 a a v (s1)ds1 = 0 = v (s1)ds1ds2 = 0 0 0 0 1 t Z Z Z u = θ + cos − 1 ² ² Define the matrix Λ according to: µ ¶ 1 T s1 s1 Λab = va(s )ds vb(s )ds ds . 2T 2 2 2 2 1 Two-link damped manipulator with oscillatory control at first joint. (the gray line is Z0 µZ0 ¶ µZ0 ¶

θ1, the black line is θ2). Define the time-varying vector field t Despite the superimposed oscillatory behavior the variables (θ1, θ2) converge to the a Ξ(t, q) = v (s)ds Ya(q), global minimum of the averaged controlled potential energy. µZ0 ¶ fb-jul02-p113 fb-jul02-p114 Theorem 2 (Averaging under oscillatory control). Let γ : I Q be the 16.2 Fact #1: Coordinate-free Averaging → solution to the initial value problem in equation (9) and let r : I Q be the n n n → Let x, y, x0 R , let ² (0, ²0] with ²0 1. Let f, g : R+ R R be smooth solution to ∈ ∈ ¿ × → time-varying vector fields. Consider the initial value problem in standard form: ab 0 r0 = Y (r) + R(r)r˙ Λ Y : Y (r) ∇r 0 − h a bi dx = ²f(t, x), x(0) = x0. r(0) = q0, r˙(0) = v0. dt Assume f(t, x) is a T -periodic function in t, and define the averaged system: There exist a positive ²0, such that for all 0 < ² ²0 ≤ dy = ²f 0(y), y(0) = x , γ(t) = r(t) + O(²) dt 0 T γ0(t) = r0(t) + Ξ(t/², γ(t)) + O(²) 1 f 0(y) = f(t, y)dt. T as ² 0 on the time scale 1. Z0 → Theorem 3 (First order averaging). There exists ² , such that for 0 < ² ² , 0 ≤ 0 x(t) y(t) = O(²) − as ² 0 on the time scale 1/². → F. Bullo, “Averaging and vibrational control of mechanical systems,” SIAM JCO, Recall: an estimate is on the time scale δ(²), if it holds for all t such that • 1 Submitted November 1999. Appeared, Jul 2002. 0 < δ− (²)t < L with L independent of ².

fb-jul02-p115 fb-jul02-p116 Fact #1: Coordinate-free Averaging – continued Fact #1: Coordinate-free averaging – the variation of constants formula dx 1 t = f(x) + g , x , x(0) = x , dx dt ² ² 0 = f(x) + g(x) g is nominal, f is perturbation µ ¶ dt where g(t, x) is a T -periodic function in t. Define T g 0 1 flow along X + Y F (t, x) = (Φ )∗f (x) F (x) = F (τ, x)dτ. X+Y Y 0,t T Φ0,T (q0) = Φ0,T (δq0) Z0 Finally, let z and y be solutions¡ to¢ the initial value problems flow along Y z˙ = F (t/², z), z(0) = x0, 0 y˙ = F (y), y(0) = x0. Theorem 4 (First order averaging for oscillatory controls). Let F be a q0 δq0 T -periodic function in t. For t R+, we have ∈ g f+g g ∆ g x(t) = Φ0,t/²(z(t)). Φ0,t (q0) = Φ0,t(δq0), δq0 = Φ0,t(q0), ∆ = (Φ0,t)∗f

t sn−1 As ² 0 on the time scale 1, we have k ¡ ¢ → ∞ t k ∞ ∆ = ad f = f + . . . adg . . . adg f dsn . . . ds1 g k! g sn s1 x(t) = Φ0,t/²(y(t)) + O(²) k=0 n=1 Z Z X X 0 0 ¡ ¢ fb-jul02-p117 fb-jul02-p118

16.3 Fact #3: Homogeneity properties and Lie algebraic structure of affine connection control systems Lie algebraic & homogeneous structure: cont’d 1 n Given γ = (γ , . . . , γ ), write second order ODE on Q as first order ODE on TQ: The sets enjoy various interesting properties. Pj (i) [ , ] , that is, the Lie bracket between a vector field in and a Pi Pj ⊂ Pi+j Pi γ˙ i γ˙ i 0 vector field in belongs to . = + Pj Pi+j  i  i j k  i  γ¨ Γ (γ)γ˙ γ˙ Yt (γ) − jk (ii) k = 0 for all k 2,       P { } ≤ − Z Y lift (iii) for all X with k 1, X(0 ) = 0, ∈ Pk ≥ q | {z } | {z } (iv) every X 1 is the lift of a vector field on Q, i.e., ∈ P− Lie algebraic & homogeneous structure 0 X = Y lift = homogeneous polynomial of degree i in γ˙ 1, . . . , γ˙ n Y  i = 1 n P homogeneous polynomial of degree (i + 1) in γ˙ , . . . , γ˙   ½· ¸¾ where X is vector field on TQ and Y is vector field on Q

lift Z 1 . . . Y 1 ∈ P ∈ P−

fb-jul02-p119 fb-jul02-p120 Lie bracket diagram

#f 1 0 1 P P P− Coordinated independent treatment 2 (i, j) {0} (i) Geometric homogeneity, Kawski ’95:

1 (1, 1) (2, 1) {0} {0} given a Euler v.f. XE, Y is homogeneous of degree ν if [XE, Y ] = νY

(ii) Liouville vector field X (q, v) = vi ∂ ; key identities on TQ: #g E ∂vi

1 2 3 4 [XE, Z] = (+1)Z [X , Y lift] = ( 1)Y lift. E − lift lift Hence, degree of Z is +1, degree of Y lift is 1 [Y1 , [Z, Y2 ]] 1 ∈ P− −

lift lift 0 lift [Y1 , [Z, Y2 ]] = = Y1 : Y2  Y : Y  h i h 1 2i   fb-jul02-p121 fb-jul02-p122 16.4 Fact #4: putting it all together

Write second order equation (9) as first order —let x = (q, q˙) and Some bookkeeping: lift lift f(x) = Z(x) + Y0 (x) + R (x), lift a m Ya (y)v (t) ∗ lift lift Φ0,t Z(y) + Y0 (y) + R (y) g(t, x) = Y lift(x)va(t). P a ³ ´ t a=1 lift¡ lift a ¢ lift lift lift X = Z + Y0 + R + v (s1)ds1 [Ya , Z + Y0 + R ] Define the vector field F µZ0 ¶ ¡ t sb ¢ ¡ ¢ lift a b a lift lift lift lift g Ya (y)v (t) ∗ lift lift + v (s )v (s )ds ds [Y , [Y , Z + Y + R ]] F (t, y) = (Φ0,t)∗f (y) = Φ0,t (Z(y) + Y0 (y) + R (y)). b a a b b a 0 P 0 0 ³ ´ µZ Z ¶ and compute it¡according¢ to the series expansion ¡ ¢ t t sk−1 lift lift a lift lift g ∞ = Z + Y0 + R + v (s1)ds1 [Ya , Z + R ] (Φ0,t)∗f = f + . . . adg(sk) . . . adg(s1) f dsk . . . ds1. 0 0 0 µZ ¶ k=1 ¡ ¢ t sb ¡ ¢ X Z Z ¡ ¢ vb(s )va(s )ds ds Y : Y lift. The Lie algebraic structure implies − b a a b h a bi µZ0 Z0 ¶ k lift lift ad lift (Z(y) + Y (y) + R (y)) = 0, k 3, Ya 0 ∀ ≥ lift lift lift adY lift adY lift (Z(y) + Y0 (y) + R (y)) = Ya : Yb . b a −h i

fb-jul02-p123 fb-jul02-p124

An integration by parts and the symmetry of the symmetric product:

t sb vb(s )va(s )ds ds Y : Y b a a b h a bi 17 Perturbation Analysis II: µZ0 Z0 ¶ 1 t t the “small-time local controllability” setting = vb(s )ds va(s )ds Y : Y , 2 b b a a h a bi µZ0 Z0 ¶ In summary H = H(q, p) + ²ϕ(q, u(t)) t Small input from rest lift lift a lift lift F (t, y) = Z + Y0 + R + v (s1)ds1 [Ya , Z + R ] p(0) = 0 µZ0 ¶ ¡ t ¢ t ¡ ¢ 1 b a lift m v (sb)dsb v (sa)dsa Ya : Yb . − 2 h i γ0 = Ya(γ)ua(t) µZ0 Z0 ¶ ∇γ0 a=1 X F is T -periodic —compute its average F 0 as Objective: characterize forced flow via series expansion F 0(y) = Z + Y lift + Rlift Λab Y : Y lift. 0 − h a bi This is what we wished to sho¡ w. ¢ fb-jul02-p125 fb-jul02-p126 17.1 Series expansions for polynomial systems 17.2 Series expansion for affine connection control systems x˙ = P (x, x) + Ax + Bu(t) x(0) = 0 γ0 = kγ0 + Y (γ, t) ∇γ0 − γ0(0) = 0

+ ∞ x = xk k=1 + X ∞ γ0 = Vk(γ, t) absolute, uniform convergence t k=1 A(t τ) X x1(t) = e − Bu(τ)dτ 0 Z t k 1 t k(s t) − A(t τ) V1(q, t) = e − Y (q, s)ds xk(t) = e − P (xj(τ), xk j (τ))dτ, k 2. 0 0 − ≥ Z j=1 Z k 1 t X 1 − k(s t) Vk(q, t) = e − Vj(q, s) : Vk j (q, s) ds −2 h − i j=1 Z0 convergence radius: β2 u < 1, where β = 2 eAt P X ∞ k kL L1 k k∞ ° ° ° °

fb-jul02-p127 fb-jul02-p128

17.3 Series: comments 17.4 Analysis II: a forces geodesic flow written as gradient flow

+ t k(s t) ∞ V1(q, t) = 0 e − Y (q, s)ds, γ0 = Vk(γ, t)  1 t k(s t) γ0 = grad ϕ(γ, t) k=1 Vk+1(q, t) = R e − Va : Vk a ds γ0 X  − 2 0 h − i ∇ Error bounds: R γ0(0) = 0q0  P k 2k 1 V (q, t) = O( Y t − ). k k k k k

In abbreviated notation + ∞ 1 γ0(t) = grad ϕk(γ(t), t) γ(0) = q0 V1 = Y V2 = Y : Y −2h i kX=1 1 V = Y : Y : Y 3 2hh i i t ϕ1(q, t) = ϕ(q, s)ds so that Z0 k 1 t − 1 1 4 7 1 γ0(t) = Y (q, t) Y : Y (q, t) + Y : Y : Y (q, t) + O( Y t ). ϕk(q, t) = ϕj(q, s) : ϕk j (q, s) ds 2 2 −2 h − i − h i hh i i k k j=1 0 X Z fb-jul02-p129 fb-jul02-p130 17.5 Example of open-loop response: planar body 17.6 Conjecture PSfrag replacements simple example: body with one force γ0 = R(γ0) + Y (γ, t) through center of mass and one torque. ∇γ0 γ0(0) = 0

q(0) = (0, 0, 0), T = 2π

u1 = .5(sin t 2 sin 2t), u2 = .5 cos t − + exact solution ∞ γ0 = Vk(γ, t) kX=1

t R(q)(t s) V1(q, t) = e − Y (q, s)ds Z0 k 1 t 1 − R(q)(t s) Vk(q, t) = e − Vj (q, s) : Vk j (q, s) ds −2 h − i j=1 0 X Z PSfrag replacements PSfrag replacements PSfrag replacements

first order second order third order Positive answer for isotropic damping: R = kIn.

fb-jul02-p131 fb-jul02-p132

Lecture #4: Kinematic Reductions and Configuration Controllability 18 Summary Francesco Bullo Coordinated Science Lab, General Engineering Dept, ECE, AAE (i) innovative approach towards control of mechanical systems University of Illinois at Urbana-Champaign (homogeneity vs passivity) 1308 W. Main St, Urbana, IL 61801, USA (perturbation methods vs energy and Lyapunov functions) [email protected], http://motion.csl.uiuc.edu (ii) challenges: convergence & complexity This lecture based on the following references

(iii) applications to controllability, vibrational stabilization, analysis of [1] A. D. Lewis, “When is a mechanical control system kinematic?,” in Proc CDC, (Phoenix, AZ), locomotion gaits, motion planning, optimal control, normal forms, etc pp. 1162–1167, Dec. 1999. [2] A. D. Lewis and R. M. Murray, “Configuration controllability of simple mechanical control systems,” SIAM JCO, 35(3):766–790, 1997. [3] F. Bullo and K. M. Lynch, “Kinematic controllability for decoupled trajectory planning in underactuated mechanical systems,” IEEE TRA, 17(4)L402–412, 2001. [4] F. Bullo and A. D. Lewis, “Kinematic controllability and motion planning for the snakeboard,” IEEE TRA, Jan. 2002. Submitted. fb-jul02-p133 fb-jul02-p134 Incomplete List of References on Kinematic Modeling and Controllability 18.1 Preliminaries: Kinematic modeling

[1] J. T. Betts, “Survey of numerical methods for trajectory optimization,” AIAA Journal of Guidance,

Control, and Dynamics, vol. 21, no. 2, pp. 193–207, 1998.

¤ £ [2] R. W. Brockett, “Hybrid models for motion control systems,” in Essays in Control: Perspectives in the Theory and its Applications, pp. 29–53, Boston, MA: Birkh¨auser, 1993. ¡ § ¨ ¨ ©

[3] E. Frazzoli, M. A. Daleh, and E. Feron, “Real-time motion planning for agile autonomous vehicles,” AIAA ¦ ¢ Journal of Guidance, Control, and Dynamics, vol. 25, no. 1, pp. 116–129, 2002. ¥

[4] V. Manikonda, P. S. Krishnaprasad, and J. Hendler, “Languages, behaviors, hybrid architectures and motion control,” in Mathematical Control Theory (J. Baillieul and J. C. Willems, eds.), New York, NY: Springer Verlag, 1998.

[5] K. McIsaac, and J. P. Ostrowski, “Steering algorithms for dynamic robotic locomotion systems,” in x˙ = v cos φ x˙ cos θ 0 Algorithmic and Computational Robotics: New Directions (B. R. Donald and K. M. Lynch and D. Rus, y˙ = v sin φ r eds.), Natick, MA: A. K. Peters, pp. 221-231, 2001. y˙r   sin θ  0 [6] A. D. Lewis, “Affine connections and distributions with applications to nonholonomic mechanics,” in φ˙ = ω = v + ω 1 Reports on Mathematical Physics, vol. 42, no. 1/2. pp. 135-164, 1998.  θ˙   tan φ 0    `    (wheeled robot dynamics)       [7] K. M. Lynch, N. Shiroma, H. Arai, and K. Tanie, “Collision-free trajectory planning for a 3-DOF robot  φ˙   0  1 with a passive joint,” International Journal of Robotics Research, vol. 19, no. 12, pp. 1171–1184, 2000.             [8] A. J. van der Schaft, “Linearization of Hamiltonian and gradient systems,” IMA Journal of Mathematical Control & Information, vol. 1, pages 185-198, 1984

[9] H. J. Sussmann, “A general theorem on local controllability,” SIAM Journal on Control and Optimization, vol. 25, no. 1, pp. 158–194, 1987.

fb-jul02-p135 fb-jul02-p136 18.2 Preliminaries: Controllability theory 19 Kinematic reductions for simple mechanical control systems with constraints [g1, g2] (i) Objective: relationships between the given mechanical control system and an Given a driftless system x˙ = g (x)u + g (x)u −u2 1 1 2 2 appropriate low-complexity kinematic representation g +u 2 ∂g2 ∂g1 1 −u1 define Lie bracket: [g1(x), g2(x)] = g1 g2 (ii) treatment for simple mechanical control systems subject to no potential energy ∂x − ∂x +u g 2 1 (iii) we relate controlled trajectories for the (second-order) controlled geodesic equation system is controllable iff LARC m γ˙ (t) = Y (γ(t))u (t). ∇γ˙ (t) a a a=1 X to controlled trajectories for driftless control systems on Q.

not full rank full rank

Example: car parking problem when can a second order system follow the solution of a first order? fb-jul02-p137 fb-jul02-p138 19.1 Motivating example Nomenclature: simple example: body with (i) The controlled geodesic equation is a dynamic models of mechanical systems:

one force through center of mass and one torque m γ˙ (t) = Y (γ(t))u (t). (i) Can follow any straight ∇γ˙ (t) a a a=1 ¡ line and can turn (2 preferred velocity fields) X In dynamic models the control inputs u are accelerations, and assumed (ii) Controllable via these two motions Lebesgue measurable functions: u U m . (hence, interesting for planning problems) ∈ dyn 

 (ii) In contrast to this, we refer to first-order differential equations on Q as      ¤ kinematic models of mechanical systems. Let = V , . . . , V be a family ¥  ¢ 1 ` 

 

V { } ¤¦ ` § ¨ £ of vector fields. For curves γ : [0, T ] Q and w : [0, T ] R , consider the      § © → →

  kinematic model induced by

V

 ` Ok ? ? ? γ˙ (t) = Vb(γ(t))wb(t). Xb=1 search for decoupling vector fields describing 1st order ODEs In kinematic models, the control inputs are velocity variables, and are assumed whose time-scaled flow is solutions to (forced) 2nd order ODEs absolutely continuous: w U ` . ∈ kin

fb-jul02-p139 fb-jul02-p140

19.2 Kinematic reductions and decoupling vector fields

In short, is a kinematic reduction if any curve γ : I Q solving the V → (controlled) kinematic model can be lifted to a solution to a solution of the 19.3 Kinematic reductions and decoupling vector fields: cont’d (controlled) dynamic model. The kinematic model induced by V1, . . . , V` is a kinematic reduc- More accurately, the kinematic model induced by = V , . . . , V is a kinematic { } V { 1 `} tion of the mechanical control system (Q, G, V = 0, ) reduction of the dynamic model, if, for any control input w U ` and F ∈ kin if and only if corresponding controlled trajectory (γ, w) for the kinematic model, there exists a the distribution span Vi, Vj : Vk i, j, k 1, . . . , ` is a sub- control input u U m such that (γ, u) is a controlled trajectory for the dynamic { h i| ∈ { }} ∈ dyn distribution of the input distribution Y . model. The rank of a kinematic reduction is the rank of the distribution generated by • The vector field V is decoupling the vector fields . V if and only if Rank-one kinematic reductions are particularly interesting. We shall call a vector V Y and V : V Y . • ∈ h i ∈ field V decoupling if the rank-one kinematic system induced by = V is a V { } kinematic reduction. Hence, the second-order control system can be steered along any time-scaled integral curve of a decoupling vector field. fb-jul02-p141 fb-jul02-p142

19.4 Mechanical systems fully reducible to kinematic systems

when is a mechanical system kinematic? 19.5 Mechanical systems fully reducible to kinematic systems: cont’d That is, when will the largest possible kinematic reduction, i.e., will be X Y A distribution is said to be geodesically invariant if it is closed under attained? operation of symmetric product, i.e., if for all vector fields X and Y taking values in X , the vector field X : Y also takes value in X . The symmetric closure of The dynamic model for the system (Q, G, V = 0, ) is fully reducible to the h i F the distribution X is the smallest geodesically invariant distribution containing X . kinematic system induced by if, is a kinematic reduction of V V (Q, G, V = 0, ) and if, for any control input u U m , initial condition F ∈ dyn Theorem 5. A mechanical control system is fully reducible to a kinematic system γ˙ (0) span( ), and corresponding controlled trajectory (γ, u) for the dynamic ∈ V if and only if U ` model, there exists a control input w kin such that (γ, w) is a controlled ∈ Y trajectory for the kinematic model induced by . (i) the kinematic system is induced by the input distribution and V (ii) the input distribution Y is geodesically invariant. A dynamic system is fully reducible to a kinematic system is there exists one such collection of vector fields . V

fb-jul02-p143 fb-jul02-p144 Definition 6. The kinematic system induced by is 20 Accessibility and controllability notions V (i) locally accessible from q if there exists T > 0 so that int( V (q , t)) = 0 R 0 ≤ 6 ∅ 20.1 Controllable kinematic systems for t (0, T ], is ∈ Here we consider the family = V , . . . , V giving rise to the driftless / (ii) small-time locally controllable ( STLC) from q0 if there exists T > 0 so that V { 1 `} kinematic control system. For q Q we denote q0 int( V (q0, t)) for t (0, T ], and is 0 ∈ ∈ R ≤ ∈

V (q , T ) = γ(T ) (γ, u) is a controlled trajectory (iii) controllable if for every q1, q2 Q there exists a controlled trajectory (γ, u) R 0 { | ∈ defined on [0, T ] for some T > 0 with the property that γ(0) = q1 and for kinematic model defined on [0, T ] with γ(0) = q0 , } γ(T ) = q2. and V (q0, T ) = t [0,T ] V (q0, t). R ≤ ∈ R S ¡ ¡ ¡

Theorem 7. The kinematic system is STLC (and therefore accessible) from q0 if £ ¦ ¢ ¡ § ¥ ¨ ¤ and only if Lie span( ) q0 = Tq0 Q. Furthermore, if Q is connected and if ¦ ¦ ¢©£ ¢©£ ¡ ¡ § ¥ § ¥ { V } ¨ ¤ ¨ ¤ Lie span( ) = T Q for each q Q, then the kinematic mode is controllable. { V }q q ∈ fb-jul02-p145 fb-jul02-p146

20.2 Kinematically controllable dynamic systems

(i) A dynamic mechanical system described by (Q, G, V, D, F ) is kinematically controllable if there exists a sequence of kinematic reductions i 1, . . . , k , rank = ` so that for every q , q Q there are {Vi| ∈ { } Vi i} 1 2 ∈ corresponding controlled trajectories

`i Theorem 8. Consider a dynamic mechanical system. The system is locally (γi, wi) γi : [Ti 1, Ti] Q, wi : [Ti 1, Ti] R , i 1, . . . , k such that { | − → − → ∈ { }} kinematically controllable if and only if it possesses a collection of decoupling γ1(T0) = q1, γk(Tk) = q2, and γi(Ti) = γi+1(Ti) for all i 1, . . . , k 1 . ∈ { − } vector fields (i.e., rank-one kinematic reductions) whose involutive closure has (ii) In other words, any q Q is reachable from any q Q by concatenating 2 ∈ 1 ∈ maximal rank everywhere in Q. motions on Q corresponding to kinematic reductions of the dynamic system

(iii) The dynamic system is locally kinematically controllable from q0 if, for any

neighborhood of q0 on Q, the set of reachable configurations by trajectories remaining in the neighborhood and following motions of its kinematic

reductions contains q0 in its interior.

fb-jul02-p147 fb-jul02-p148 20.3 Controllable dynamic systems

Consider a dynamic mechanical system (Q, G, V, D, F ). For q0 Q, denote ∈ Notation: Consider iterated symmetric products in the vector fields Y , . . . , Y . { 1 m} (q , T ) = γ˙ (T ) (γ, u) is a controlled trajectory RTQ 0 { | (i) A symmetric product is bad if it contains an even number of each of the

of the dynamic model defined on [0, T ] and satisfying γ˙ (0) = 0q0 . vector fields Y , . . . , Y , and otherwise is good. Thus, for example, } 1 m Ya : Yb : Ya : Yb is bad for all a, b 1, . . . , m and Ya : Yb : Yc is Here 0q0 Tq0 Q is the zero vector. Also, TQ(q0, T ) = t [0,T ] TQ(q0, t). hh i h ii ∈ { } h h ii ∈ R ≤ ∈ R good for any a, b, c 1, . . . , m . S ∈ { } Definition 9. Consider a dynamic mechanical system (Q, G, V, D, F ) and let (ii) The degree of a symmetric product is the total number of input vector fields q0 Q. Suppose that the controls for the dynamic system are restricted to take comprising the symmetric product. For example, our given bad symmetric ∈ their values in a compact set of Rm which contains 0 in the interior of its convex product has degree 4 and the given good symmetric product has degree 3. hull. The dynamic system is (iii) If P is a symmetric product in the vector fields Y , . . . , Y and if σ S is { 1 m} ∈ m (i) locally accessible from q0 if there exists T > 0 so that int( TQ(q0, t)) = an element of the permutation group on 1, . . . , m , σ(P ) denotes the R ≤ 6 ∅ { } for t (0, T ], and is symmetric product obtained by replacing each occurrence of Y with Y . ∈ a σ(a) (ii) small-time locally controllable ( STLC) from q0 if there exists T > 0 so that 0 int( (q , t)) for all t (0, T ]. q0 ∈ RTQ 0 ≤ ∈ fb-jul02-p149 fb-jul02-p150 20.4 Configuration controllable dynamic systems

The preceding discussion concerned the set of reachable states for a dynamic mechanical system. Let us now restrict to descriptions of the set of reachable Theorem 10. Consider a dynamic mechanical system described by configurations. We define (Q, G, V, D, F ) and let q0 Q. The dynamic mechanical system is ∈ (q , T ) = τ( (q , T )), (q , T ) = (q , t). RQ 0 RTQ 0 RQ 0 ≤ RQ 0 (i) locally accessible from q if and only if Sym Y = T Q, and is t [0,T ] 0 { }q0 q0 ∈[ (ii) STLC from q if Sym Y = T Q and if for every bad symmetric product This gives the following notions of controllability relative to configurations. 0 { }q0 q0 P we have Definition 11. Consider a dynamic mechanical system described by D F σ(P )(q0) span P1(q0), . . . , Pk(q0) , (Q, G, V, , ) and let q0 Q. The dynamic mechanical system is ∈ R{ } ∈ σ Sm ∈ X (i) locally configuration accessible from q0 if there exists T > 0 so that where P , . . . , P are good symmetric products of degree less than P . 1 k int( (q , t)) = for all t (0, T ], and is RQ 0 ≤ 6 ∅ ∈ The condition stated for STLC is derived from a result of Sussmann ’87. (ii) small-time locally configuration controllable ( STLCC) from q0 if there exists T > 0 so that q int( (q , t)) for all t (0, T ] with the controls 0 ∈ RQ 0 ≤ ∈ restricted to take their values in a compact subset of Rm that contains the origin in its convex hull.

fb-jul02-p151 fb-jul02-p152

20.5 Controllability inferences

Theorem 12. Consider an analytic dynamic mechanical system described by STLC = small-time locally controllable (Q, G, V, D, F ) and let q0 Q. The dynamic mechanical system is STLCC = small-time locally configuration controllable ∈ (i) locally configuration accessible from q0 if and only if LKC = locally kinematically controllable Lie Sym Y = T Q, and is FR-LKC = fully reducible, locally kinematically controllable { { }}q0 q0 (ii) STLCC from q if Lie Sym Y = T Q and if for every bad symmetric 0 { { }}q0 q0 product P we have STLC LKC FR-LKC

σ(P )(q ) span P (q ), . . . , P (q ) , 0 ∈ R{ 1 0 k 0 } σ Sm X∈ STLCC where P1, . . . , Pk are good symmetric products of degree less than P .

There exist counter-examples for each missing implication sign. fb-jul02-p153 fb-jul02-p154 20.7 Graphical illustration 20.6 Controllability and Configuration Controllability m Y Locally controllable γ0 = kγ0 + Ya(q)ua rank(Sym q0 ) = n ∇γ0 − { } u a=1 bad symmetric products are (q0, 0) (af, vf) X −→ γ0(0) = 0 linear combination of lower can reach open set  order good products of velocities   given forces F a Configuration controllable { } u rank(Lie Sym Y q ) = n (q , 0) (q , 0) { { }} 0 0 −→ f  1 a good/bad as above can reach open set accessible accelerations Ya = G− F  { } of configurations  Simplifications: accessible velocities Y , Y : Y , . . . { a h b ci } (i) for systems on group: algebraic tests on the Lie algebra (ii) for systems with integrable forces: Beltrami brackets between functions accessible configurations Ya, Yb : Yc , [Yb, Yc], [ Ya : Yb , Yc], . . . { h i h i }M

fb-jul02-p155 fb-jul02-p156 20.8 An example controllability analysis: the snakeboard 20.9 An example controllability analysis: the roller racer

Symmetric products: Symmetric products:

1 2 X : X = 0 , X : X = 0 , X : X = 2(XΓ) (ψ)X + 2(XΓ) (ψ)X h 2 2i h 3 3i h 2 2i 22 1 22 2 D 1 Jr Jr(cos φ sin φ) span X2, X2 : X2 = if (XΓ)22(ψ) = 0 X2 : X3 = 2 (cos φ)X1 2 2 X2 , { h i} 6 h i m` − m` + Jr(sin φ) Lie brackets:

`2 `1 + `2 cos ψ ∂ span X2, X3, X2 : X3 = D if cos φ = 0. [X1, X2] = Vy 2 { h i} 6 `2 + `1 cos ψ − (`2 + `1 cos ψ) ∂θ

`2 sin ψ `1 + `2 cos ψ [X1, [X1, X2]] = − 2 Vx + 2 Vy , Lie brackets: (`2 + `1 cos ψ) (`2 + `1 cos ψ) ∂ [X , X ] = `(sin φ)V + (cos φ) 1 3 x ∂θ [X , [X , X ]] = `(sin φ)V , 1 1 3 − y span X , X , [X , X ], [X , [X , X ]] = TQ everywhere ` I cos ψ = ` I . { 1 2 1 2 1 1 2 } 2 1 6 1 2 span X , X , X , [X , X ], [X , [X , X ]] = TQ System is STLCC System is locally configuration accessible { 1 2 3 1 3 1 1 3 } fb-jul02-p157 fb-jul02-p158 20.10 A catalog of affine connection control systems

System Picture Reducibility & Controllability robotic leg two decoupling v.f., fully reducible and n = 3, m = 2 planar 2R robot (1, 0): no reductions, accessible LKC single torque at either joint: (0, 1): decoupling v.f., fully reducible, (1, 0), (0, 1) (1, 0, 1) and (1, 1, 0): two decoupling not accessible or STLCC n = 2, m = 1 planar 3R robot, two torques: v.f., LKC and STLC (0, 1, 1), (1, 0, 1), (1, 1, 0) (0, 1, 1): two decoupling v.f., fully re- roller racer n = 3, m = 2 no kinematic reductions, accessible, single torque at joint ducible and LKC not STLCC rolling penny n = 4, m = 1 fully reducible and LKC n = 4, m = 2 planar body with single force decoupling v.f., reducible, not accessi- or torque ble n = 3, m = 1 snakeboard two decoupling v.f., LKC, STLCC planar body with single gen- n = 5, m = 2 no kinematic reductions, accessible, eralized force not STLCC n = 3, m = 1 3D vehicle with 3 generalized forces three decoupling v.f., LKC, STLC planar body with two forces two decoupling v.f., LKC, STLC n = 6, m = 3 n = 3, m = 2

fb-jul02-p159 fb-jul02-p160

Summary of Analysis Methods Lecture #5: Stabilization and Tracking for (lectures #3 and #4) fully actuated systems Comprehensive, coherent body of work encompassing results on (i) perturbation methods Francesco Bullo (ii) kinematic reductions Coordinated Science Lab, General Engineering Dept, ECE, AAE University of Illinois at Urbana-Champaign (iii) controllability properties 1308 W. Main St, Urbana, IL 61801, USA [email protected], http://motion.csl.uiuc.edu

Open directions This lecture based on the following references Averaging higher order, 2-time scales, gait analysis [1] F. Bullo and R. M. Murray, “Tracking for fully actuated mechanical systems: A geometric framework,” Automatica, vol. 35, no. 1, pp. 17–34, 1999. Controllability gravity or generic dissipation fb-jul02-p161 fb-jul02-p162 Incomplete List of References on Lyapunov and passivity methods for stabilization and tracking

[1] S. Arimoto. Control Theory of Non-linear Mechanical Systems: A Passivity-Based and Circuit-Theoretic Approach, volume 49 of OESS. Oxford University Press, Oxford, UK, 1996. 20.11 Stabilization via the total energy as Lyapunov function [2] A. M. Bloch, N. E. Leonard, and J. E. Marsden, “Controlled Lagrangians and the stabilization of mechanical systems. I. the first matching theorem,” IEEE TAC, vol. 45, no. 12, pp. 2253–2270, 2000. Consider a simple mechanical control system (Q, G, V = 0, ) with F [3] D. E. Koditschek, “The application of total energy as a Lyapunov function for mechanical control equations systems,” in Dynamics and Control of Multibody Systems (J. E. Marsden, P. S. Krishnaprasad, and J. C. 1 γ0 γ0 = G− F Simo, eds.), vol. 97, pp. 131–157, AMS, 1989. ∇ [4] H. Nijmeijer and A. J. van der Schaft. Nonlinear Dynamical Control Systems. Springer Verlag, New York, Goal: Stabilize q Q NY, 1990. 0 ∈ [5] R. Ortega, A. Loria, P. J. Nicklasson, and H. Sira-Ramirez. Passivity-Based Control of Euler-Lagrange (i) fully actuated system: span( ) = T∗Q Systems: Mechanical, Electrical and Electromechanical Applications. Communications and Control F Engineering. Springer Verlag, New York, NY, 1998. (ii) ϕ: Q R with critical zero and positive definite Hessian [6] R. Ortega, A. J. van der Schaft, I. Mareels, and B. Maschke, “Putting energy back in control,” IEEE → Control Systems Magazine, vol. 21, no. 2, pp. 18–33, 2001. ϕ(q0) = 0, dϕ(q0) = 0, Hess ϕ(q0) > 0 [7] S. Stramigioli, Modeling and IPC Control of Interactive Mechanical Systems- A Coordinate - Free Approach, vol. 266 of Lecture Notes in Control and Information Sciences. Springer Verlag, 2001. (iii) Rayleigh dissipation function Kd : TQ T∗Q [8] A. J. van der Schaft, “Stabilization of Hamiltonian systems,” Nonl. Analysis, Theory, Methods & App.s, → vol. 10, no. 10, pp. 1021–1035, 1986. [9] A. J. van der Schaft, L2-Gain and Passivity Techniques in Nonlinear Control, vol. 218 of Lecture Notes in Control and Information Sciences. New York, NY: Springer Verlag, 1996.

fb-jul02-p163 fb-jul02-p164

21 Tracking for Fully Actuated Systems

Classic PD control: F (v ) = dϕ(q) K v PD q − − d q Objective: track reference trajectory γref Stability, local exponential stability, global convergence to critical points of ϕ Configuration and velocity errors: (assuming existence compact and invariant set) (i) “distance” between q and r ...... error function (i) Lyapunov function is positive definite, symmetric, quadratic Ψ: Q Q R • × → d 1 2 1 2 (ϕ + γ0 ) = 0 ϕ + 0 γ0 dt 2k k ∇γ 2∇γ k k (ii) “distance” between γ0 and γref0 ...... transport map = dϕ , γ0 + γ0 γ0 , γ0 h i hh∇ ii linear map : T Q T Q • T(q,r) r → q = dϕ , γ0 + dϕ(q) Kdγ0 , γ0 = Kdγ0 , γ0 . h i h− − i −h i velocity error is e˙ = γ0 γ0 • − T(γ,γref) ref Ψ˙ = d Ψ , e˙ (ii) Proof of exponential convergence rates: modify Lyapunov function with ²ϕ˙ ⇒ h 1 i term, or perform linearized analysis “compatibility:” d Ψ(q, r) = ∗ d Ψ(q, r) • 2 −T(q,r) 1 Examples: joint or Euler angle rates, body-fixed angular velocities fb-jul02-p165 fb-jul02-p166

21.2 Table of Examples 21.1 Tracking on Manifolds

1 Goal: Track a reference γref : I Q for γ0 γ0 = G− F → ∇ Device configuration error function transport map/

PD + Feedforward: Let F = FPD + FFF with space velocity error

n 2 FPD(γ0, t) = d1Ψ(γ, γref) Kd e˙ Rob. manipulator R q r In − − k − k d 2 3 T T F (γ0, t) = ( 0 w ) + γ0 Pointing device S R 1 q r (q r)I3 + (r q) FF G γ (γ,r) r 0 (q,γref) ref ∇ T wr =γ dt T q=γ(t) ⊂ − × µ ¯ ref ¯ ¶ ¯ ¡ ¢¯ T Satellite SO(3) tr K(I3 RR ) Ω Ωd b ¯ ¯ − d − T T tr ¡K(I3 R R)¢ Ω R RdΩd (i) Lyapunov stability with exponential convergence rates. − d − ¡ ¢ (ii) time-varying Lyapunov function 3 Submersible SE(3) [combination of R [change of reference 1 2 and SO(3)] frame] t Ψ(γ(t), γ (t)) + γ0(t) γ0 (t) 7→ ref 2 k − T(γ(t),γref(t)) ref k (iii) FFF has two terms: “curvature” and acceleration of γref Riemannian mfld Q geodesic distance parallel transport

fb-jul02-p167 fb-jul02-p168 21.3 Effects of Different Choices of Error Computations

Closed-loop trajectory on SO(3) with different feedforward Lecture #6: Trajectory Planning via

Left transport map: trajectory Right transport map: trajectory Motion Primitives Francesco Bullo

0 0 Coordinated Science Lab, General Engineering Dept, ECE, AAE 2.5 2.5 University of Illinois at Urbana-Champaign 5 5 time 7.5 time 7.5 1308 W. Main St, Urbana, IL 61801, USA 10 10 [email protected], http://motion.csl.uiuc.edu

Closed-loop trajectory on SE(2) with different feedback This lecture based on the following references [1] F. Bullo and K. M. Lynch, “Kinematic controllability for decoupled trajectory planning in underactuated mechanical systems,” IEEE TRA, 17(4)L402–412, 2001. [2] F. Bullo and A. D. Lewis, “Kinematic controllability and motion planning for the snakeboard,” IEEE TRA, Jan. 2002. Submitted. [3] F. Bullo, N. E. Leonard, and A. D. Lewis, “Controllability and motion algorithms for underactuated Lagrangian systems on Lie groups,” IEEE TAC, 45(8):1437–1454, 2000. [4] W. T. Cerven and F. Bullo, “Constructive controllability algorithms for motion planning and optimization,” IEEE TAC, Nov. 2001. Submitted. fb-jul02-p169 fb-jul02-p170

22 Motion planning for underactuated vehicles 22.1 Limited actuation provides for challenges

Real time motion planning – feedforward and 2 degree-of-freedom design for aggressive tracking – can compute feasible trajectory only via 2 pt. boundary value optimal control: iterative, off-line algorithms, convergence SCAMP project Cassini probe blimp SSL, U. Maryland – loss of controllability along minimum-time trajectories

Stabilization – accurate hovering/station keeping ( exponential stab.) – reconfiguration after actuator failure (not linearly controllable) hovercraft tail-less aircraft helicopter Honda biped

(i) vehicles, robotic manipulators, locomotion devices Locomotion (ii) nonlinearities (kinetic energy, forces, configurations/velocities) – analysis of gaits and of novel propulsion mechanisms – system design (iii) limited actuation (under-actuation, mag. & rate limits, ...)

fb-jul02-p171 fb-jul02-p172 22.3 Preliminaries: Numerical Optimal Control

22.2 Motion Planning Scenarios Optimal Control Transcription T S is submanifold of trim conditions, helices, rel. equilibria, hover N min u(t) 2dt u k k u(t) = d ψ (t) Z0 i i i=1 (i) Classic Point-to-Point Setting: on manifold subj x(0) = 0, x(T ) = xdesired X x(t) Ã x = x(t ) and linearly controllable x˙ = f(x, u) j j j = 1, . . . , N u 1, u˙ 1 x | | ≤ | | ≤ S (ii) Point-to-Point remaining on manifold and sys- tem is not linearly controllable (low velocity regime, internal actuation, actuator failure, ill Finite dimensional NLP conditioned linearization) Numerical (iii) Fast Point-to-Point via minimum-time trajec- implementation min d 2 k k S tory and system is not linearly controllable Your favorite solver: subj x1 = 0, xNx = xdesired Sequence of QP xj+1 = f(xj, d) (iv) Harder: Point-to-Point away from S Feasible SQP g(d) 1 Dimension: Nx + Nu | | ≤ fb-jul02-p173 fb-jul02-p174 22.4 Motion planning via primitives Incomplete List of References on Motion planning via low complexity models and via series expansions

Goal: reduce complexity & abstract dynamics [1] E. G. Al’brekht, “On the optimal stabilization of nonlinear systems,” PMM - Journal of Applied Mathematics and Mechanics, vol. 25, pp. 1254–1266, 1961. (i) quantize system dynamics into finite set of primitives P1, . . . , Pn { } [2] E. Frazzoli, M. A. Daleh, and E. Feron, “Real-time motion planning for agile autonomous vehicles,” AIAA system can evolve on primitive for arbitrary time Journal of Guidance, Control, and Dynamics, vol. 25, no. 1, pp. 116–129, 2002. [3] A. Halme and J. Orava, “Generalized polynomial operators for nonlinear systems analysis,” IEEE (ii) characterize switches/transitions between primitives Transactions on Automatic Control, vol. 17, no. 2, pp. 226–8, 1972. transition requires a fix duration and displacement [4] W. Magnus. On the exponential solution of differential equations for a linear operator. Communications on Pure and Applied Mathematics, VII: 649–673, 1954. [5] G. Lafferriere and H. J. Sussmann. A differential geometric approach to motion planning. In Z. Li and J. F. Canny, editors, Nonholonomic Motion Planning, pages 235–270. Kluwer Academic Publishers, Wheeled robot example Boston, MA, 1993. [6] N. E. Leonard and P. S. Krishnaprasad, Motion control of drift-free, left-invariant systems on Lie restrict search / abstract dynamics to straight lines and circles groups, IEEE Transactions on Automatic Control, 40 (1995), pp. 1539–1554. [7] V. Manikonda, P. S. Krishnaprasad, and J. Hendler, “Languages, behaviors, hybrid architectures and motion control,” in Mathematical Control Theory (J. Baillieul and J. C. Willems, eds.), New York, NY: Springer Verlag, 1998. Translate Rotate [8] R. M. Murray and S. S. Sastry, Nonholonomic motion planning: Steering using sinusoids, IEEE Transactions on Automatic Control, 5 (1993), pp. 700–726. [9] H. J. Sussmann, “New differential geometric methods in nonholonomic path finding,” in Systems, Models, and Feedback: Theory and Applications (A. Isidori and T. J. Tarn, eds.), pp. 365–384, Boston, MA: switch / transition Birkh¨auser, 1992.

fb-jul02-p175 fb-jul02-p176 23.1 Decoupling vector fields and kinematic controllability 23 Decoupled motion planning via kinematic Data structure

i controllability (i) given inertia tensor G, Christoffel symbols Γjk ∂Y i and covariant derivative ( Y )i = Xj + Γi Xj Y k Motion planning for underactuated robot system ∇X ∂qj jk (i) actuator failure (ii) given force co-vectors F 1, . . . , F m , (ii) lighter design with no actuators { } 1 a and input distribution Y = span Ya = G− F , a = 1, . . . , m { } controllable kinematic reduction: (i) Can follow any straight line

and can turn (2 preferred velocity fields) Theorems ¡ (ii) Controllable via these two motions The vector field V is decoupling if and only if V Y and V V Y . (iii) Planning via inverse kinematic ∈ ∇ ∈ System is kinematically controllable if LARC on decoupling v. fields fb-jul02-p177 fb-jul02-p178 23.2 Ex #1: A three-dimensional aerospace vehicle with three forces 23.3 Ex #2: Three link planar manipulator with passive link

u2 yb d x u b 1 kinematically controllable via u3 z F b b £

body-fixed constant velocity fields ¢ ¥ ¦ § F ¤ ¨

¡

5

y g Actuator Decoupling Kinematically 4 xg configuration vector fields controllable zg since invariant vector fields (0,1,1) 2 yes decoupled trajectory planning (1,0,1) 2 yes 0 6 via inverse kinematic z0 3 y0 (1,1,0) 2 yes

1 x0

2 Lynch, Shiroma, Arai, Tanie. “Collision-free trajectory planning for a 3-DOF robot with a passive joint” IJRR, 19(12):1171-1184, 2000

fb-jul02-p179 fb-jul02-p180 23.4 Ex #3: The snakeboard and the roller racer 24 Motion planning via series expansions

 Linear Control Systems Nonlinear Mechanical

©

Systems x˙ = Ax + bu(t)

©   x˙ = f0(x) + fi(x)ui(t) where x Rn, u Rm. ∈ ∈ X  1) Solution from x(0) = 0 is 1) Evolution is a series expansion,

    t with iterated integrals of u and iter- A(t s) x(t) = e − bu(s)ds. ated Lie brackets between fj .   Z0 2) Controllability: sufficient tests in- 2) Iff the system is controllable clude a full rank question. (i) snakeboard is kinematically controllable T A(T s) A0(T s) W = e − bb0e − ds. 3) Local constructive planning pro- (ii) roller racer is not: T Z0 cedure: truncate the series, find an (a) single input Y such that Y Y span Y 3) Open-loop control to reach x ∇ 6∈ { } d inverse (local motion primitives), (b) moves forward using zero mean (cyclic) input A0(T t) 1 combine in iterative fashion. u(t) = b0e − WT− xd. fb-jul02-p181 fb-jul02-p182 24.2 Reviewing various concepts

rewrite: • 24.1 Mechanical control systems on matrix groups 1 v˙ i + Γi vj vk = v˙ i + v : v jk 2h i ( 1f )u (t) = b u (t) =: β(t) (i) g G is configuration on n-dimensional matrix group I− k k k k ∈ local coordinates via x = log(g) X X

1 T (ii) kinetic energy = 2 v Iv with I > 0 KE Given the family of input vectors b1, . . . , bm , define v Rn velocity in body frame • { } ∈ Sym b1, . . . , bm { } 1 m n (iii) body-fixed forces f , . . . , f (R )∗. a symmetric product in Sym b1, . . . , bm is bad if it contains even number of ∈ • { } each b . Otherwise good. Generalized Christoffel symbols written with respect to a basis of left invariant i vector fields are constant. bad: b : b , b : b : b : b h 1 1i h 1 h 2 h 2 1iii good: b , b : b 1 h 1 2i definite time integral: β(t) = t β(τ)dτ • 0 R

fb-jul02-p183 fb-jul02-p184

24.3 Computing the “Force to Displacement” Map 24.4 Example 1: single input systems

g˙ = g · vˆ ¢ ¤

1 ¢ v˙ + 2 hv : vi = β(t) £ ¥ planar rigid body with only £ ¡ With ² 1, let • ¿ b = 1f m 2 I− 2 ¦ ¨ ¥ ¥ £ £ 1 2 2 1 2 2 § © β(t, ²) = ² β (t) + ² β (t) = bk ²uk(t) + ² uk(t) 1 2 set (β (t), β (t)) = ( ²ψ(t)b2, 0) k=1 • § X ¡ ¢ provided ψ(2π) = 0, we have: If x(0) = 0, v(0) = 0, then over finite interval • 2 2π 2 ² 1 1 1 2 v(2π) β : β (2π) = 2 ² ψ dt b2 : b2 1 2 2 1 1 1 3 2 0 v(t) = ²β (t) + ² β 2 β : β (t) + O(² ) ≈ − h i − h i − h i ¡ R ¢¡ ¢ ³ ´ independent of sign of ψ(t) (“energy integral” always positive) x(t) = ²β1(t) + ²2 β2(t) 1 β1 : β1 (t) + 1 [β1, β1](t) + O(²3) • − 2 h i 2 µ ¶ x(2π) behaves similarly • fb-jul02-p185 fb-jul02-p186 24.5 Simulation with “uni-directional” motion 24.6 Example 2: systems with two inputs ¡

With β2 = 0, and with β1(2π) = β1(2π) = 0 • red is force green is center of mass ²2 v(2π) β1 : β1 (2π) ≈ − 2 h i

Satellite with two thrusters • – b , b torques about first two axes { 1 2} b : b = 0, b : b torque about third axis h k ki h 1 2i 1 – If β (t) = ψ(t)(b1 + b2) then 2 β1 : β1 (2π) = 2 b : b 2π ψ dt h i h 1 2i 0 ³R ´ energy integral in-phase (classic area integral out-of-phase)

fb-jul02-p187 fb-jul02-p188

24.7 Interpretation 24.9 Inverting the Approximate Map

2 2 1 1 1 1 recall v(2π) ² β 2 β : β (2π) given accelerations bi = I− fi • ≈ − h i { } i i β = uk(t)bk ³ ´ P © “reachable” velocities bi, bj : bk , . . . assume “controllable” ¨ { h i } • rank bi, bj : bk = n,

{ h i} ¨ bi : bi span b1, . . . , bm “reachable” configurations b , b : b , [b , b ], . . . h i ∈ { } { i h j ki j k } Inverse(v ) : can design (β (t), β (t)) • desired 1 2 24.8 Examples of systems with “fully reachable” velocities β2 1 β1 : β1 (2π) = v − 2 h i desired ³ ´ (i) in-phase inputs generate motion along good symmetric product ¥ ¤ £ ¢ f2 ¦ ¤ (ii) uni-directional contribution due to bad symmetric products can be compensated for by lower order, good products § f1 i (iii) uk sinusoids (cyclic, in-phase or orthogonal) fb-jul02-p189 fb-jul02-p190

24.10 Primitives of Motion 24.11 Point to point problem via constant velocity algorithm

Goal drive system from (Id, 0) to (g , 0) use Inverse as building block for motion planning 1 Arguments (g1, σ)

Change-Vel (², vfinal) ...... steer velocity v(t) to ²vfinal Require log(g1) well defined

Initial state: v(0) = ²v0

Final state: v(2π) ²v 1: N Floor( log(g1) /(2πσ)) ≈ final ⇐ k k 2: v log(g )/(2πσN) nom ⇐ 1 Maintain-Vel (², v ) ...... keeps velocity v(t) at ²v 3: Change-Vel(σ, v ) start maneuver nom ref nom { } 4: for k = 1 to (N 1) do Initial state: v(0) = ²vref − Final state: v(2π) ²v 5: Maintain-Vel(σ, vnom) keep nominal velocity ≈ ref { } 6: end for 7: Change-Vel(σ, 0) stop maneuver (i) can compute change in g { }

1 2 2 (ii) expansions with low initial speed: v(0) = ²v0 + ² v0

(iii) “sum” contributions over finite and O(1/²) intervals N intervals × σvnom = total displacement

fb-jul02-p191 fb-jul02-p192

24.12 Stabilization problem via iterative steering 24.13 Simulation of Point-to-Point Problem

Goal drive system to the state (Id, 0) exponentially as t → ∞ Arguments σ 2 2 Require (log(g(0)), v(0)) σ. k k ≤ y y 1 1

1: for k = 1 to + do ∞ 0 0 2: t 4kπ t is the current time k ⇐ { k } 3: σ (log(g(t ), v(t ))) 0 1 0 1 k ⇐ k k k k x x 4: Change-Vel σ , log(g(t )) + πv(t ) /(2πσ ) k − k k k Properties of algorithms 5: Change-Vel³(σk, 0)¡ ¢ ´ 6: end for closed form, negligible computational load • 1 3/2 asymptotic behavior: time O(²− ), final error (² ) • series expansion approach leads to complete algorithms two primitives force final configuration and velocity to vanish • fb-jul02-p193 fb-jul02-p194 25 Motion planning for polynomial systems 24.14 Simulations for 3D vehicle Linear Control Systems Nonlinear Mechanical Systems x˙ = Ax + Bu(t) x˙ = f0(x) + fi(x)ui(t) 1) Solution from x(0) = 0 is X t 1) Characterize flow map A(t s) x(t) = e − Bu(s)ds Z0 x(T ) = Φ(u) 2) Iff the system is controllable

T A(T s) A0(T s) 2) Controllability: range Φ W = e − BB0e − ds > 0 motion primitive Z0 based on local inversion 3) Open-loop control to reach xd 3) Local planning:

global planning A0(T t) 1 u(t) = B0e − W − xd u = Φ†(xd)

fb-jul02-p195 fb-jul02-p196 25.1 Series for polynomial systems 25.2 Constructive controllability 25.3 Minimum energy control For low-dimensional models of aerospace and underwater vehicles, trigonometric Let x(0) = 0, choose base functions: Set up Hamilton’s equations: dependencies can be turned into polynomial: n [2] i n x˙ = Ax + f (x, x) BB0λ u(t) = ψ (t)ci c R − [2] ∈ [2] x˙ = Ax + f (x, x) + Bu, x(0) = x0, i=1 λ˙ = A0λ 2f (x)0λ X − − [2] n n n f : R R R is a symmetric tensor then x(T ) = Φ(u) = Φ(c) × →

xk(T ) = Φk(c, . . . , c) n This time no input, λ(0) = λ0 R k ∈ xk = O( c ) k k k k xk = Φk(λ0, . . . , λ0) + ∞ evolution via (Volterra) series x(t) = Φ(u) = xk(t) kX=1 t To have x(T ) = xd, solve For boundaries conditions, solve At A(t τ) x1(t) = e x0 + e − Bu(τ)dτ + + 0 ∞ ∞ Z x = Φ c + Φ (c, . . . , c) x = Φ λ + Φ (λ , . . . , λ ) t k 1 d 1 k d 1 0 k 0 0 − A(t τ) [2] k=2 k=2 xk(t) = e − f (xa(τ), xk a(τ)) dτ X X − 0 Ãa=1 ! Z X fb-jul02-p197 fb-jul02-p198 25.5 Inversion for linearly controllable systems

25.4 Expression for Φ tensors To solve ∞ In constructive controllability xd = Φ1c + Φk(c, . . . , c) k=2 t X i A(t τ) i i Φ1(t) = e − Bψ (τ)dτ Φ1 is full rank iff system is linearly controllable, and appropriate ψ (t) Z0 { } t i1i2 A(t τ) [2] i1 i2 Φ (t) = e − f Φ (τ), Φ (τ) dτ, 2 1 1 1: iterative numerical scheme lim ck cgoal Z0 k → t ¡ ¢ →∞ i1i2i3 A(t τ) [2] i1 i2i3 [2] i1i2 i3 Φ3 (t) = e − f (Φ1 (τ), Φ2 (τ)) + f (Φ2 (τ), Φ1 (τ)) dτ 1 1 ∞ 1 0 c1 = Φ− xd, ck+1 = Φ− xd Φ− Φk(ck, . . . , ck) Z ³ ´ 1 1 − 1 . k=2 . X

t k 1 ∞ − i1...ik A(t τ) [2] i1...ia i1...ik−1 2: inverse Taylor expansion cgoal = ck Φk (t) = e − f (Φa (τ), Φk a (τ)) dτ. − k=1 0 Ãa=1 ! X Z X 1 1 c = Φ− x , c = Φ− Φ c , , c To evaluate at t = T 1 1 d k − 1 m i1 · · · im i1+ +im=k i ,···X,im

fb-jul02-p199 fb-jul02-p200 25.7 Inversion for nonlinearly controllable systems 25.6 Simulations for linearly controllable systems

Solve

¥ ¨ © ¥ £ § ¦ ¤ ∞

xd = Φ2(c, c) + Φk(c, . . . , c)

¢ k=3 ¡ X for not linearly controllable system such as

¥ ¨ ©

£

¤ [2] x˙ = f (x, x) + Bu, x(0) = 0

planar vertical takeoff and landing aircraft model (PVTOL)

Desired motion is horizontal translation from left to right without any vertical or [2] Assume A = 0, rank Bi, f (Bj , Bk) = n rotational displacement. { } f [2](B , B ) span B , . . . , B , i i i ∈ { 1 m} ∀ 1 1

0 0 Z Z −1 −1

−2 −2 Can invert xd = Φ2(c, c) via “quadratic inversion” −3 −3 −1 0 1 2 3 4 5 6 −1 0 1 2 3 4 5 6 X X m first order second order u : [0, 2π] R = Inverse(xd) → fb-jul02-p201 fb-jul02-p202 25.8 Quadratic inversion (compare with linear case) 25.9 Simulations for nonlinearly controllable systems (i) Let N = m(m 1)/2, P = (j, k) 1 j < k m , 1 α N, and − { | ≤ ≤ } ≤ ≤ ψ (t) = 1 α sin(αt) (α + N) sin (α + N)t . α √2π − ³ ¡ ¢´ (ii) Compute (m + N) real numbers zi and zjk such that

[2] xd = ziBi + zjkf (Bj , Bk). 1 i m 1 j

1 2 global planning Bu(t) = b (t) + b (t) = Inverse(xd)

fb-jul02-p203

Summary of Design Methods (lectures #5 and #6)

Body of work encompassing results on (i) stabilization via energy methods for fully actuated systems (ii) motion planning via kinematic reductions (iii) motion planning via low amplitude oscillations (iv) talk by Jorge Cort´es on motion planning via high amplitude oscillations

Open directions

Motion control via low amplitude oscillations general manifold case Motion control via kinematic reductions numerical methods for inverse kinematics, time-varying feedback stabilizers