Pontryagin s maximum principle

Transcription

1 Pontryagin s maximum principle Emo Todorov Applied Mathematics and Computer Science & Engineering University of Washington Winter 2012 Emo Todorov (UW) AMATH/CSE 579, Winter 2012 Lecture 5 1 / 9

2 Pontryagin s maximum principle For deterministic dynamics ẋ = f (x, u) we can compute extremal open-loop trajectories (i.e. local minima) by solving a boundary-value ODE problem with given x (0) and λ (T) = x q T (x), where λ (t) is the gradient of the optimal cost-to-go function (called costate). Emo Todorov (UW) AMATH/CSE 579, Winter 2012 Lecture 5 2 / 9

3 Pontryagin s maximum principle For deterministic dynamics ẋ = f (x, u) we can compute extremal open-loop trajectories (i.e. local minima) by solving a boundary-value ODE problem with given x (0) and λ (T) = x q T (x), where λ (t) is the gradient of the optimal cost-to-go function (called costate). Definition (deterministic Hamiltonian) H (x, u, λ), ` (x, u) + f (x, u) T λ Theorem (continuous-time maximum principle) If x (t), u (t), 0 t T is the optimal state-control trajectory starting at x (0), then there exists a costate trajectory λ (t) with λ (T) = x q T (x) satisfying ẋ = H λ (x, u, λ) = f (x, u) λ = H x (x, u, λ) = `x (x, u) + f x (x, u) T λ u = arg min eu H (x, eu, λ) Emo Todorov (UW) AMATH/CSE 579, Winter 2012 Lecture 5 2 / 9

4 Derivation from the HJB equation (continuous time) For deterministic dynamics ẋ = f (x, u) the optimal cost-to-go in the finite-horizon setting satisfies the HJB equation n o v t (x, t) = min ` (x, u) + f (x, u) T v x (x, t) u = min H (x, u, v x (x, t)) u If the optimal control law is π (x, t), we can set u = π and drop the min : 0 = v t (x, t) + ` (x, π (x, t)) + f (x, π (x, t)) T v x (x, t) Now differentiate w.r.t. x and suppress the dependences for clarity: 0 = v tx + `x + π T x `u + f T x + π T x f T u v x + v xx f Using the identity v x = v tx + v xx f and regrouping yields 0 = v x + `x + f T x v x + π T x `u + f T u v x = v x + H x + π T x H u Since u is optimal we have H u = 0, thus λ = Hx (x, π, λ) where λ = v x. Emo Todorov (UW) AMATH/CSE 579, Winter 2012 Lecture 5 3 / 9

5 Derivation via Largrange multipliers (discrete time) Optimize total cost subject to dynamics constraints x k+1 = f (x k, u k ). Define the Lagrangian L (x, u, λ ) as L = q T (x N ) + N 1 k=0 ` (x k, u k ) + (f (x k, u k ) x k+1 ) T λ k+1 = q T (x N ) x T N λ N + x T 0 λ 0 + N 1 k=0 H (x k, u k, λ k+1 ) x T k λ k Setting L x = L λ = 0 and explicitly minimizing w.r.t. u yields Theorem (discrete-time maximum principle) If x k, u k, 0 k N is the optimal state-control trajectory starting at x 0, then there exists a costate trajectory λ k with λ N = x q T (x N ) satisfying x k+1 = H λ (x k, u k, λ k+1 ) = f (x k, u k ) λ k = H x (x k, u k, λ k+1 ) = `x (x k, u k ) + f x (x k, u k ) T λ k+1 u k = arg min H (x k, eu, λ k+1 ) eu Emo Todorov (UW) AMATH/CSE 579, Winter 2012 Lecture 5 4 / 9

6 Gradient of the total cost The maximum principle provides an efficient way to evaluate the gradient of the total cost w.r.t. u, and thereby optimize the controls numerically. Theorem (gradient) For given control trajectory u k, let x k, λ k be such that x k+1 = f (x k, u k ) λ k = `x (x k, u k ) + f x (x k, u k ) T λ k+1 with x 0 given and λ N = x q T (x N ). Let J (x, u ) be the total cost. Then u k J (x, u ) = H u (x k, u k, λ k+1 ) = `u (x k, u k ) + f u (x k, u k ) T λ k+1 Note that x k can be found in a forward pass (since it does not depend on λ), and then λ k can be found in a backward pass. Emo Todorov (UW) AMATH/CSE 579, Winter 2012 Lecture 5 5 / 9

7 Proof by induction The cost accumulated from time k until the end can be written recursively as J k (x kn, u kn 1 ) = ` (x k, u k ) + J k+1 (x k+1n, u k+1n 1 ) Noting that u k affects future costs only through x k+1 = f (x k, u k ), we have We need to show that λ k = For k < N we have J u k = `u (x k, u k ) + f u (x k, u k ) T J k x k+1 k+1 x k J k. For k = N this holds because J N = q T. J x k = `x (x k, u k ) + f x (x k, u k ) T J k x k+1 k+1 which is identical to λ k = `x (x k, u k ) + f x (x k, u k ) T λ k+1. Emo Todorov (UW) AMATH/CSE 579, Winter 2012 Lecture 5 6 / 9

8 Enforcing terminal states The final state x (T) is usually different from the minimum of the final cost q T, because it reflects a trade-off between final and running cost. We can enforce x (T) = x as a boundary condition and remove the boundary condition on λ (T). Once the solution is found, we can construct a function q T such that λ (T) = x q T (x (T)). However if λ (T) 6= 0 then x (T) is not the minimum of this q T. We can also define the problem as infinite horizon average cost, in which case it is usually suboptimal to have an asymptotic state different from the minimum of the state cost function. The maximum principle does not apply to infinite horizon problems, so one has to use the HJB equations. Emo Todorov (UW) AMATH/CSE 579, Winter 2012 Lecture 5 7 / 9

9 More tractable problems When the dynamics and cost are in the restricted form ẋ = a (x) + Bu ` (x, u) = q (x) ut Ru the Hamiltonian can be minimized analytically, which yields the ODE ẋ = a (x) BR 1 B T λ λ = q x (x) + a x (x) T λ with boundary conditions x (0) and λ (T) = x q T (x). If B, R depend on x, the second equation has additional terms involving the derivatives of B, R. Emo Todorov (UW) AMATH/CSE 579, Winter 2012 Lecture 5 8 / 9

10 More tractable problems When the dynamics and cost are in the restricted form ẋ = a (x) + Bu ` (x, u) = q (x) ut Ru the Hamiltonian can be minimized analytically, which yields the ODE ẋ = a (x) BR 1 B T λ λ = q x (x) + a x (x) T λ with boundary conditions x (0) and λ (T) = x q T (x). If B, R depend on x, the second equation has additional terms involving the derivatives of B, R. We have H u = R (x) u + B (x) T λ and H uu = R (x) 0. Thus the maximum principle here is both a necessary and a sufficient condition for a local minimum. Emo Todorov (UW) AMATH/CSE 579, Winter 2012 Lecture 5 8 / 9

11 Pendulum example Passive dynamics: x a (x) = 2 k sin (x 1 ) 0 1 a x (x) = k cos (x 1 ) 0 Optimal control: u = r 1 λ 2 ODE (with q = 0): ẋ 1 = x 2 ẋ 2 = k sin (x 1 ) r 1 λ 2 λ 1 = k cos (x 1 ) λ 2 λ 2 = λ 1 Emo Todorov (UW) AMATH/CSE 579, Winter 2012 Lecture 5 9 / 9

12 Pendulum example Passive dynamics: x a (x) = 2 k sin (x 1 ) 0 1 a x (x) = k cos (x 1 ) 0 Cost-to-go and trajectories: Optimal control: u = r 1 λ 2 ODE (with q = 0): Control law (from HJB): ẋ 1 = x 2 ẋ 2 = k sin (x 1 ) r 1 λ 2 λ 1 = k cos (x 1 ) λ 2 λ 2 = λ 1 Emo Todorov (UW) AMATH/CSE 579, Winter 2012 Lecture 5 9 / 9

13 Trajectory-based optimization Emo Todorov Applied Mathematics and Computer Science & Engineering University of Washington Winter 2012 Emo Todorov (UW) AMATH/CSE 579, Winter 2012 Lecture 6 1 / 13

14 Using the maximum principle Recall that for deterministic dynamics ẋ = f (x, u) and cost rate ` (x, u) the optimal state-control-costate trajectory (x (), u (), λ ()) satisfies ẋ = f (x, u) λ = `x (x, u) + f x (x, u) T λ u = arg min eu n ` (x, eu) + f (x, eu) T λ with x (0) given and λ (T) = x q T (x (T)). Solving this boundary-value ODE problem numerically is a trajectory-based method. o Emo Todorov (UW) AMATH/CSE 579, Winter 2012 Lecture 6 2 / 13

15 Using the maximum principle Recall that for deterministic dynamics ẋ = f (x, u) and cost rate ` (x, u) the optimal state-control-costate trajectory (x (), u (), λ ()) satisfies ẋ = f (x, u) λ = `x (x, u) + f x (x, u) T λ u = arg min eu n ` (x, eu) + f (x, eu) T λ with x (0) given and λ (T) = x q T (x (T)). Solving this boundary-value ODE problem numerically is a trajectory-based method. We can also use the fact that, if (x (), λ ()) satisfies the ODE for some u () which is not a minimizer of the Hamiltonian H (x, u, λ) = ` (x, u) + f (x, u) T λ, then the gradient of the total cost J is given by J (x (), u ()) = q T (x (T)) + J u (t) Z T 0 o ` (x (t), u (t)) dt = H u (x, u, λ) = `u (x, u) + f u (x, u) T λ Thus we can perform gradient descent on J with respect to u () Emo Todorov (UW) AMATH/CSE 579, Winter 2012 Lecture 6 2 / 13

16 Compact representations Given the current u (), each step of the algorithm involves computing x () by integrating forward in time starting with the given x (0), then computing λ () by integrating backward in time starting with λ (T) = x q T (x (T)). Emo Todorov (UW) AMATH/CSE 579, Winter 2012 Lecture 6 3 / 13

17 Compact representations Given the current u (), each step of the algorithm involves computing x () by integrating forward in time starting with the given x (0), then computing λ () by integrating backward in time starting with λ (T) = x q T (x (T)). One way to implement the above methods is to discretize the time axis and represent (x, u, λ) independently at each time step. This may be inefficient because the values at nearby time steps are usually very similar, thus it is a waste to represent/optimize them independently. Emo Todorov (UW) AMATH/CSE 579, Winter 2012 Lecture 6 3 / 13

18 Compact representations Given the current u (), each step of the algorithm involves computing x () by integrating forward in time starting with the given x (0), then computing λ () by integrating backward in time starting with λ (T) = x q T (x (T)). One way to implement the above methods is to discretize the time axis and represent (x, u, λ) independently at each time step. This may be inefficient because the values at nearby time steps are usually very similar, thus it is a waste to represent/optimize them independently. Instead we can splines, Legendre or Chebyshev polynomials, etc. u (t) = g (t, w) Gradient: Z J T w = g w (t, w) T J u (t) dt 0 u t w Emo Todorov (UW) AMATH/CSE 579, Winter 2012 Lecture 6 3 / 13

19 Space-time constraints We can also minimize the total cost J as an explicit function of the (parameterized) state-control trajectory: x (t) = h (t, v) u (t) = g (t, w) We have to make sure that the state-control trajectory is consistent with the dynamics ẋ = f (x, u). This yields a constrained optimization problem: Z T min q T (h (T, v)) + ` (h (t, v), g (t, w)) dt v,w 0 s.t. h (t, v) t = f (h (t, v), g (t, v)), 8t 2 [0, T] Emo Todorov (UW) AMATH/CSE 579, Winter 2012 Lecture 6 4 / 13

20 Space-time constraints We can also minimize the total cost J as an explicit function of the (parameterized) state-control trajectory: x (t) = h (t, v) u (t) = g (t, w) We have to make sure that the state-control trajectory is consistent with the dynamics ẋ = f (x, u). This yields a constrained optimization problem: Z T min q T (h (T, v)) + ` (h (t, v), g (t, w)) dt v,w 0 s.t. h (t, v) t = f (h (t, v), g (t, v)), 8t 2 [0, T] In practice we cannot impose the contraint for all t, so instead we choose a finite set of points ft k g where the constraint is enforced. The same points can also be used to approximate R `. There may be no feasible solution (depending on h, g) in which case we have to live with constraint violations. This requires no knowledge of optimal control (which may be why it is popular:) Emo Todorov (UW) AMATH/CSE 579, Winter 2012 Lecture 6 4 / 13

21 Second-order methods More efficient methods (DDP, ilqg) can be constructed by using the Bellman equations locally. Initialize with some open-loop control u (0) (), and repeat: 1 Compute the state trajectory x (n) () corresponding to u (n) (). 2 Construct a time-varying linear (ilqg) or quadratic (DDP) approximation to the function f around x (n) (), u (n) (), which gives the local dynamics in terms of the state and control deviations δx (), δu (). Also construct quadratic approximations to the costs ` and q T. 3 Compute the locally-optimal cost-to-go v (n) (δx, t) as a quadratic in δx. In ilqg this is exact (because the local dynamics are linear and the cost is quadratic) while in DDP this is approximate. 4 Compute the locally-optimal linear feedback control law in the form π (n) (δx, t) = c (t) L (t) δx. 5 Apply π (n) to the local dynamics (i.e. integrate forward in time) to compute the state-control modification δx (n) (), δu (n) (), and set u (n+1) () = u (n) () + δu (n) (). This requires linesearch to avoid jumping outside the region where the local approximation is valid. Emo Todorov (UW) AMATH/CSE 579, Winter 2012 Lecture 6 5 / 13

22 Numerical comparison Conj. ODE DDP time(s) Log 10 ( Cost ) Steepest Conjugate Iteration iter 1 iter 10 iter 100 iter 200 Emo Todorov (UW) AMATH/CSE 579, Winter 2012 Lecture 6 6 / 13

23 Finding Locally- Op0mal, Collision- Free Trajectories with Sequen0al Convex Op0miza0on John Schulman, Jonathan Ho, Alex Lee, Ibrahim Awwal, Henry Bradlow, Pieter Abbeel Thursday, July 4, 13

24 Sampling- based methods like RRT Graph search methods like A* OpAmizaAon based methods MoAon Planning } slow down as dimensionality increases ReacAve control PotenAal- based methods for high- DOF problems (KhaAb, 86) OpAmize over the enare trajectory ElasAc bands (Quinlan & KhaAb, 93) CHOMP (Ratliff, et al. 09) & variants (STOMP, ITOMP) Industrial robot arm (6 DOF) Thursday, July 4, 13 Mobile manipulator (18 DOF) Humanoid (34 DOF) Finding Locally- OpAmal, Collision- Free Trajectories. Presenter: John Schulman

25 Trajectory OpAmizaAon min 1:T X t subject to k t+1 no collisions joint limits other constraints t k 2 + other costs non-convex Finding Locally- OpAmal, Collision- Free Trajectories. Presenter: John Schulman Thursday, July 4, 13

26 Trajectory OpAmizaAon min 1:T X t subject to k t+1 no collisions joint limits other constraints t k 2 + other costs non-convex SequenAal convex opamizaaon Repeatedly solve local convex approximaaon Challenge ApproximaAng collision constraint Thursday, July 4, 13 Finding Locally- OpAmal, Collision- Free Trajectories. Presenter: John Schulman

27 Collision Constraint as L1 Penalty Penalty 0 dsafe Signed Distance Thursday, July 4, 13 Finding Locally- OpAmal, Collision- Free Trajectories. Presenter: John Schulman

28 Collision Constraint as L1 Penalty Penalty r 0 0 dsafe Signed Distance Linearize w.r.t. degrees of freedom sd AB ( ) sd AB ( 0 )+ˆn T J pa ( 0 )( 0 ) Thursday, July 4, 13 Finding Locally- OpAmal, Collision- Free Trajectories. Presenter: John Schulman

29 ConAnuous- Time Safety A(t+1) A(t) T B Collision check against swept- out volume ConAnuous- Ame collision avoidance Allows coarsely sampling trajectory overall faster Finds becer local opama Finding Locally- OpAmal, Collision- Free Trajectories. Presenter: John Schulman Thursday, July 4, 13

30 OpAmizaAon: Toy Example Finding Locally- OpAmal, Collision- Free Trajectories. Presenter: John Schulman Thursday, July 4, 13

31 Benchmark: Example Scenes 7 DOF (one arm) 198 problems 18 DOF (two arms + base + torso) 96 problems example scene (taken from MoveIt collection) Thursday, July 4, 13 example scene (imported from Trimble 3d Warehouse / Google Sketchup) Finding Locally- OpAmal, Collision- Free Trajectories. Presenter: John Schulman

32 Benchmark Results Arm planning (7 DOF) 10s limit Trajopt BiRRT (*) CHOMP success time (s) path length 99% 97% 85% Full body (18 DOF) 30s limit Trajopt BiRRT (*) CHOMP (**) success time (s) path length 84% 53% N/A N/A N/A (*) Top-performing algorithm from MoveIt/OMPL (**) Not supported in available implementation Finding Locally- OpAmal, Collision- Free Trajectories. Presenter: John Schulman Thursday, July 4, 13

33 Trajectories with Sequential Convex Opti ch penalizes collisions with a hinge loss and increases the alty coefficients in an outer loop as necessary. (ii) An efficient mulation of the no-collisions constraint that directly considers John Schulman, Jonathan tinuous-time safety and enables the algorithm to reliably solve Ho, Alex Lee, Ibrahim Awwal, Henry Bradlow and Pie ion planning problems, including problems involving thin and mplex obstacles. We benchmarked ourabstract We algorithm against several other approach mopresent a novel for incorporating planning algorithms, solving a suite of 7-degree-of-freedom collision avoidance into trajectory optimization as a method of OF) arm-planning problems and 18-DOF full-body planning solving robotic motion planning problems. At the core of our blems. We compared against sampling-based planners from approachtoare (i) A sequential convex optimization procedure, MPL, and we also compared CHOMP, a leading approach which Our penalizes collisions with than a hinge trajectory optimization. algorithm was faster the loss and increases the penalty coefficients in an outer loop as necessary. (ii) An efficient rnatives, solved more problems, and yielded higher quality formulation of the no-collisions constraint that directly considers hs. continuous-time safetyadditional and enables the algorithm to reliably solve Experimental evaluation on the following problem motion problems, including problems involving thin and es also confirmed the speed planning and effectiveness of our approach: complex Planning foot placements withobstacles. 34 degrees of freedom (28 joints Fig. 1. Several problem settings were we have used our algorithm for motion We benchmarked ourasalgorithm against several mo- an arm trajectory for the PR2 in simulation, in a planning. Top other left: planning DOF pose) of the Atlas humanoid robot it maintains problem. Top right: PR2 opening a door with a full-body motion. ic stability and has to planning negotiate algorithms, environmental constraints. tion solving a suite ofbenchmark 7-degree-of-freedom Bottom left: industrial robot picking boxes, obeying an orientation constraint Industrial box picking. (iii)arm-planning Real-world motion planning for (DOF) problems and 18-DOF full-body planning effector. Bottom right: humanoid robot model (DRC/Atlas) ducking PR2 that requires problems. consideringwe all compared degrees of against freedom sampling-based at the on the endplanners from underneath an obstacle while obeying static stability constraints. me time. OMPL, and we also compared to CHOMP, a leading approach Other Experiments - - Videos at InteracAve Session Planning for 34- DOF humanoid (stability constraints) Box picking with industrial robot (orientaaon constraints) for trajectory optimization. Our algorithm was faster than the I. I NTRODUCTION alternatives, solved more problems, and yielded higher quality paths. Saturday, February 13 configuration space, which can be directly incorporated part2,of Trajectory optimization algorithms have two roles in robotic Experimental evaluation on the following additional problem optimization problem that is solved at each tion planning. First, they can be used to smooth and shorten into the convex types also confirmed the speed and effectiveness of our approach: of the optimization. ectories generated (i) byplanning some other method. Second, can iteration foot placements withthey 34 degrees of freedom (28 joints Fig. 1. Several problem settings were we have used to plan from +scratch: initializes a trajectory Theasfirst advantage ofplanning. our approach speed.anour Top left:isplanning arm impletrajectory 6 DOFone pose) of thewith Atlas humanoid robot it maintains contains collisions and perhaps violates constraints, and mentation solves typical arm planning problems in around static stability and has to negotiate environmental constraints. benchmark problem. Top right: PR2 opening a Bottom left: industrial robot picking hopes that the optimization converges to a (iii) high-quality (ii) Industrial box picking. Real-world100 motion200 planning ms andforsolves problems involving many boxes, more ob on the end effector. Bottom right: humanoid rob the PR2 that requires considering algoall degrees of freedom at the ectory satisfying constraints. Using an optimization degrees of freedom in under a second. This is largely enabled underneath an obstacle while obeying static st same time. m to plan from scratch is an especially attractive option by our novel formulation of the the collision penalty, which problems with many degrees of freedom (DOF), since the guarantees safety in continuous time by considering swepti NTRODUCTION mputation time scales favorably with the I. number of DOF. out volumes. This cost formulation has little computational Saturday, February part2,of configuration overhead in in collision checking and allowspresenter: us tospace, use a sparsely Two of the key ingredients in trajectory optimization Trajectory optimization algorithmsforhave twolocally- OpAmal, roles robotic Finding Collision- Free T13 rajectories. John Swhich chulmancan the advantage convex optimization problem tion planning (1) the planning. numerical First, optimization The into second of our approach they canmethod, be used tosampled smooth trajectory. and shorten Thursday, July are 4, 13motion Constant- curvature 3D needle steering (non- holonomic constraint)

34 Optimal, Collision-Free Try it ooptimal, ut yourself! Finding Locally Collision-Free ential Convex Optimization Trajectories with Sequential Convex Optimization Ibrahim Awwal, Henry Bradlow and Pieter Abbeel John Schulman, Jonathan Ho, Alex Lee, Ibrahim Awwal, Henry Bradlow and Pieter Abbeel Code and docs: rll.berkeley.edu/trajopt ting present a novel approach for incorporating d Abstract We of ollision avoidance into trajectory optimization as a method of our olving ure, robotic motion planning problems. At the core of our pproach are (i) A sequential convex optimization procedure, the which ient penalizes collisions with a hinge loss and increases the enalty coefficients in an outer loop as necessary. (ii) An efficient ders ormulation of the no-collisions constraint that directly considers olve ontinuous-time safety and enables the algorithm to reliably solve and Run our benchmark: github.com/joschu/planning_benchmark motion planning problems, including problems involving thin and omplex obstacles. mowe benchmarked our algorithm against several other modom ion planning algorithms, solving a suite of 7-degree-of-freedom ning DOF) arm-planning problems and 18-DOF full-body planning rom roblems. We compared against sampling-based planners from oach OMPL, and we also compared to CHOMP, a leading approach orthe trajectory optimization. Our algorithm was faster than the ality lternatives, solved more problems, and yielded higher quality aths. blem Experimental evaluation on the following additional problem ypes also confirmed the speed and effectiveness of our approach: ach: i) Planning placements with 34 degrees ofhave freedom (28algorithm joints for oints Fig.motion 1. Several problem settings were we have used our algorithm for motion Fig. foot 1. Several problem settings were we used our planning. +ains 6 DOF pose) Top of the humanoid robot as planning. left: Atlas planning an arm trajectory for itthemaintains PR2 in simulation, in atop left: planning an arm trajectory for the PR2 in simulation, in a benchmark tatic stability andproblem. has to Top negotiate environmental benchmark right: PR2 opening a doorconstraints. with a full-body motion. problem. Top right: PR2 opening a door with a full-body motion. ints. left: industrial robot picking boxes, obeying an orientation constraint ii)for Industrial motion planning for Bottom Bottombox left:picking. industrial(iii) robotreal-world picking boxes, obeying an orientation constraint on the end the requires end effector. Bottom right: robot model (DRC/Atlas) ducking effector. Bottom right: humanoid robot model (DRC/Atlas) ducking he that considering allhumanoid degrees of freedom at the thepr2 on underneath an obstaclecwhile obeyingtrajectories. static stability constraints. ollision- Free Presenter: John Schulman underneath an obstacle while obeying static stability constraints.finding Locally- OpAmal, ame time. Thursday, July 4, 13