Stanford AA274A Principles of Robotic Autonomy | Autumn 2019 | Advanced Trajectory Optimization

斯坦福大学 AA274A 机器人自主原理 | 2019秋季 | 高级轨迹优化 在本讲座中，Marco Pavone 教授探讨了连续时间轨迹优化的间接方法，将“先优化后离散”的理念与直接非线性规划方法进行了对比。他从有限维拉格朗日乘子和一个几何圆示例切入，引入了连续时间哈密顿量、协态以及由此形成的两点边值问题（TPBVP）。随后，Pavone 在双积分器基准问题上演示了如何利用归一化时间重标度技巧来求解边界条件并处理自由终端时间问题。最后，他证明了直接法与间接法在结构上的一致性，指出在极限情况下 Karush-Kuhn-Tucker（KKT）条件会精确收敛至连续必要条件。 由于决策变量是不可数的控制轨迹 u(t)，连续时间最优控制作用于无限维函数空间，这使得计算求解方案分化为间接法（“先优化后离散”）和直接法（“先离散后优化”）两种框架。 (4:04) 间接最优控制通过构建哈密顿量并引入连续协态函数 p(t)，将动态优化问题转化为两点边值问题，把静态拉格朗日乘子理论直接推广至微分等式约束。 (32:46) 自由终端时间最优控制问题可通过在 [0, 1] 区间上进行无量纲时间归一化 tau = t / tf，并引入导数为零的常数参数状态 r = tf 扩充状态空间，从而适配现成的数值边值问题（BVP）求解器。 (57:13) 直接与间接轨迹优化在理论上具有一致性：随着时间离散步长趋近于零，离散 Karush-Kuhn-Tucker（KKT）条件精确收敛至连续的欧拉-拉格朗日方程和哈密顿边值条件。 (74:31)

Professor Marco Pavone frames robotic trajectory optimization within continuous-time optimal control theory, contrasting indirect variational derivations with direct numerical discretization. He demonstrates that indirect methods yield coupled two-point boundary value problems governed by the Hamiltonian and costates, which formally converge to direct nonlinear programming formulations in the continuous limit. Optimal Control and Function-Space Optimization (0:05) Trajectory optimization requires actively minimizing performance costs—such as elapsed transit time, actuator effort, or a weighted combination of both—subject to differential equations of motion and actuation limits. Pavone contrasts this with differential flatness, which he categorizes as a dynamic feasibility mapping technique rather than an arbitrary cost functional optimizer. Because optimal control optimizes a continuous control input history over time, it operates over an infinite-dimensional function space with uncountably many decision variables. Computationally, practitioners approach this infinite-dimensional problem through two distinct philosophies: indirect methods ('first optimize, then discretize') and direct methods ('first discretize, then optimize'). Pavone notes that while optimal control computes open-loop trajectories parameterized by time and initial state, physical robotics applications must ultimately combine these open-loop trajectories with feedback tracking controllers to prevent unmodeled disturbances and state drift from causing system failures. Extending Static Multipliers to the Dynamic Hamiltonian (32:46) To build mathematical intuition for indirect methods, Pavone draws a structural parallel to equality-constrained finite-dimensional optimization. In static nonlinear programming, constraints are absorbed into an augmented Lagrangian via Lagrange multipliers, where stationary points of the gradient identify candidate local extrema. In optimal control, the governing differential equations of motion act as continuous equality constraints. Consequently, the problem is augmented using the Hamiltonian, combining running stage costs with system dynamics scaled by time-varying costate variables. These costates serve as the dynamic counterpart to static Lagrange multipliers. Dimension counting confirms the mathematical consistency of the continuous necessary optimality conditions: for an n-dimensional state vector and m control inputs, Pontryagin's Minimum Principle yields n state differential equations, n costate differential equations, and m control stationarity conditions. This creates a system of 2n coupled first-order ordinary differential equations alongside m algebraic equations to solve for exactly 2n + m unknown functions of time. Transversality Conditions and Boundary Value Problems (42:09) Integrating the system's 2n first-order differential equations introduces 2n constants of integration that require boundary conditions. While initial system states supply n known conditions at the start time, the remaining conditions are derived from terminal variations via the master transversality boundary equation. This equation relates terminal cost derivatives, costates, the Hamiltonian, and admissible variations in final state and final time. Because the required constraints are split between the initial and terminal times, the necessary conditions form a Two-Point Boundary Value Problem (TPBVP) rather than a standard initial value problem. In numerical implementations—such as SciPy's boundary value problem solvers—practitioners specify the differential dynamics and a boundary residual function that measures candidate deviations from endpoint constraints, which the solver iteratively drives to zero. Time Rescaling for Free Final Time Optimization (57:13) Off-the-shelf boundary value problem solvers operate over a fixed, predetermined integration domain. However, minimum-time optimal control problems treat the final arrival time as an unconstrained decision variable. To resolve this mismatch, Pavone introduces a dimensionless time variable defined by physical time divided by the final time, mapping the integration domain onto a normalized interval from zero to one. Under this transformation, time derivatives are scaled by the unknown final horizon, which is incorporated as an auxiliary state variable with zero dynamic derivative. Pavone demonstrates this workflow on a displaced double integrator balancing duration against control effort. Evaluating the Hamiltonian transversality condition at the terminal boundary provides the necessary fifth constraint to solve for the four dynamic state-costate variables and the optimal transit duration simultaneously. Limiting Equivalence of Direct and Indirect Approaches (74:31) Pavone concludes by establishing the formal theoretical link between direct and indirect optimization. Direct methods convert infinite-dimensional continuous control problems into finite-dimensional nonlinear programs by discretizing time into finite intervals and enforcing dynamics at discrete collocation points, producing necessary optimality conditions governed by Karush-Kuhn-Tucker (KKT) multipliers. In the continuous limit as the discretization step size approaches zero and the number of nodes approaches infinity, the discrete KKT necessary conditions converge exactly to the continuous Euler-Lagrange equations and Hamiltonian boundary value conditions. The discrete Lagrange multipliers associated with system dynamics directly mirror the continuous costate functions, demonstrating that direct and indirect methods are mathematically consistent representations of the same underlying optimization problem. While indirect optimal control establishes exact analytical boundary value formulations for unconstrained state spaces, practical robotics requires extending these formulations to handle state inequalities, such as obstacle avoidance, which transitions trajectory optimization into algorithmic motion planning.

[4:04] Prof. Marco Pavone: which might indeed result in disaster [4:04] Prof. Marco Pavone: first optimize, then discretize [4:04] Prof. Marco Pavone: first discretize, then optimize [8:47] Prof. Marco Pavone: hallmark of any optimal control problem [18:21] Prof. Marco Pavone: You didn't know that one, eh? You thought he was French [18:21] Prof. Marco Pavone: legacy of Italy, courtesy of Italy [18:21] Prof. Marco Pavone: hiding under the rug [49:08] Prof. Marco Pavone: difficulty on steroids [70:44] Prof. Marco Pavone: alpha larger means that you care more about time than control effort [70:44] Prof. Marco Pavone: a little bit less esoteric [74:31] Prof. Marco Pavone: tightly coupled, as it should be because we're solving the same problem [74:31] Prof. Marco Pavone: bonus information

[0:05] All right, so I think we can get started. [0:07] Today, we are going to talk about advanced methods [0:10] for trajectory optimization. [0:13] So this is the last lecture on the topic [0:15] of trajectory optimization. [0:16] Next week, we'll look at a problem of motion planning [0:19] that in the language of this class [0:22] should be understood as a trajectory optimization [0:24] with a set of constraints that is with obstacles. [0:28] So today, what we're going to do is [0:30] to revisit the problem of trajectory planning [0:33] by casting it as an optimal control problem. [0:38] When we talked about differential flatness methods [0:41] for trajectory planning, what we said is that in that context, [0:45] we are interested in a feasible solution [0:49] with some nice properties. [0:51] But we're not trying to, in general, [0:54] optimize a cost function. [0:56] But in some cases, it's very important [0:58] to have a trajectory that not only is a good trajectory [1:01] but actually optimizes some cost function, [1:04] like, for example, minimizes time, minimizes control effort, [1:07] minimizes the combination of time and control effort, [1:10] and so on. [1:11] So this is really the topic of optimal control. [1:13] I teach a class on this topic in the spring. [1:16] Today, I'm going to share with you some key ideas. [1:20] And as in the style of this class, [1:22] I'm going to do a deep dive in a specific technique [1:26] by giving you some actionable insights [1:29] to solve some problems with the specific techniques. [1:34] In particular, we're going to focus the discussion [1:37] on indirect methods. [1:39] And toward the end of the class, I'm [1:41] going to shed some light on the connection [1:42] between indirect methods and direct methods [1:45] for optimal control. [1:48] The suggested reading for this lecture is this book by Kirk. [1:53] It's a little bit dated, but it's [1:54] a very accessible introduction to the field of optimal control. [1:58] The field of optimal control tends [1:59] to be extremely mathematically heavy. [2:02] It's a fairly sophisticated topic. [2:04] This book does a good job in being rigorous, [2:07] while at the same time being quite accessible. [2:11] All right, so as we said earlier this week, [2:15] the trajectory planning problem can [2:18] be cast as-- actually should be cast as an optimal control [2:21] problem, whereby the idea is that we are given [2:23] the equations of motion of our robotic system [2:27] in the form of a differential equation. [2:29] As usual, x is the state, u is the input. [2:32] The differential equation, for example, [2:34] could represent the equation of motion [2:37] of a unicycle, differential drive, simple car, and so on. [2:41] We are given input constraints in the form, for example, [2:45] of upper bounds on control authority, maximum speed, [2:49] maximum acceleration, maximum angular velocity, and so on. [2:55] We're possibly given the state constraints [2:57] in the form of obstacle avoidance. [2:59] But as we said, we're going to deal with obstacle avoidance [3:02] next week. [3:03] For this week, we consider the case [3:05] where we have no constraints. [3:06] Which means that the feasible set of states [3:08] is the entire Euclidean space. [3:11] And finally, we're given a cost function [3:13] that has an additive structure, terminal term plus a running [3:17] cost. [3:19] The integral term captures the notion of cost [3:22] that is accrued throughout the execution of the trajectory. [3:26] The terminal term captures the notion [3:29] of penalizing whether at the final time, tf, [3:34] we are in a position that is not exactly the position where [3:37] we want to be, for example. [3:42] So it's a very general formulation. [3:43] Usually, you're given the initial condition. [3:46] The task is to find a control history u that [3:53] with an associated state trajectory [3:55] x, obtained through the solution of differential [3:59] equation of motion, such that the cost is minimized. [4:04] Now, this is a complicated optimization problem [4:07] because we are optimizing over the space of functions. [4:11] Control input u is a function of time. [4:19] So you may be more familiar with optimization problem, where [4:21] you optimize over a set of finite decision variables, [4:25] say, x1, x2, x3. [4:26] Here, you are optimizing over a function, which, [4:29] if you think about, entails optimizing over [4:32] an uncountable set of decision variables, [4:36] each one for all possible value of t. t is a continuous number. [4:40] So you have one decision variable per t. [4:42] So you have uncountable number of decision variable. [4:45] So people refer to this class of problem [4:47] as infinite dimensional problems because the space of function [4:50] is infinite dimensional. [4:52] So that doesn't mean that this problem is infinitely more [4:55] complicated than standard optimization [4:57] but certainly is more complicated [4:59] than standard optimization. [5:01] So, as we discussed last time, the standard setup [5:06] for optimal control entails computing open loop control [5:13] trajectories, u, that only depend on time and possibly [5:17] the initial condition. [5:18] Remember that open loop means that your control action at time [5:24] t is the same, regardless of where [5:28] you are in the state space. [5:30] Which basically implies that if performance is nominal, [5:34] then you're doing the right thing. [5:36] But if, for whatever reason, due to model mismatches, [5:38] disturbances, and so on, you start drifting away, [5:41] you keep applying the same action [5:43] that you should have applied if performance [5:45] was nominal, which might indeed result in disaster. [5:49] And then we discuss why you want to augment open loop [5:52] control with the trajectory tracking and so on. [5:55] But for the sake of optimal control, [5:57] the standard setup is to optimize over [5:59] open loop function, meaning function of the time t. [6:02] Now, there are two main methods to do so. [6:04] First class of methods referred to as indirect methods, whereby [6:07] the idea is to first write down the necessary optimality [6:11] condition for the infinite dimensional optimization problem [6:16] that I presented in the previous slide [6:18] and then solve for those necessary optimality conditions. [6:24] The other approach, referred to as direct methods, [6:26] entails transcribing the problem into a finite dimensional [6:31] nonlinear optimization problem through discretization [6:34] and then solve for that optimization problem. [6:39] So to remember, indirect methods can be summarized [6:43] as we first optimize in terms of writing down [6:45] the necessary optimality conditions and then discretize. [6:48] Well, direct methods is the opposite. [6:50] We first discretize to do the transcription into a nonlinear [6:54] optimization problem and then we optimize. [6:57] So today we're going to discuss mostly indirect methods. [7:00] And then we are going to discuss direct methods, something [7:03] that actually we already discussed on Tuesday, [7:06] but in particular, in terms of what are the commonalities [7:10] and what is the link between these two classes of methods. [7:14] Any questions so far? [7:18] Yeah. [7:20] What is the methodology that you gave us [7:23] for differentially flat systems? [7:25] Which class will that fall under? [7:27] It doesn't fall under neither of those classes. [7:32] Philosophically, it's more related to direct methods [7:34] because we don't write down necessary optimality conditions. [7:38] But in reality, the point is that with differential flatness, [7:42] you are not trying to solve an optimal control problem. [7:45] You are trying to solve a feasibility problem, [7:47] whereby you just need to find a contraction control action, u, [7:52] that satisfy the differential equation [7:55] and additional constraints, like a initial condition and terminal [7:59] conditions and so on. [8:00] In reality, we said that we can also [8:02] include some notion of optimality [8:05] by setting up a cost function in the flat output [8:08] space, for example, in terms of Jack and snap. [8:10] But strictly speaking, differential flatness [8:12] looks more at the feasibility problem [8:14] rather than an optimality problem. [8:15] And we said that is more conceptually related [8:17] to direct methods because there is [8:19] no notion of writing down the necessary optimality condition. [8:23] The idea is to transform the problem [8:26] into a finite dimensional problem [8:28] or even into a linear system of equations, as we saw last time. [8:32] Does that make sense? [8:33] Yeah. [8:34] So if you will, it could be categorized [8:36] as a specific application of direct methods, where we are not [8:41] too strict in enforcing costing or optimizing [8:44] with respect to cost function. [8:47] So let's start with indirect methods. [8:51] So as I said, conceptually, indirect methods, [8:55] the procedures behind indirect methods [8:58] is fairly straightforward. [9:00] You should be familiar with the standard unconstrained [9:06] optimization setup, whereby you are given a nonlinear function f [9:10] of x. [9:12] x Is a vector that belongs to Rn. [9:16] So it's a vector with n components. [9:19] Each component is a real number. [9:22] You have a question? [9:24] All good? [9:26] All right. [9:27] And then you want to find the global minimum [9:30] or the global minima of this function, f. [9:34] So one way that you can solve this problem [9:36] is assuming that the function is differentiable. [9:41] You set the gradient of this function equal to 0. [9:45] That's a necessary condition of optimality [9:47] for unconstrained optimization. [9:49] You solve for those values of x, such that the gradient of f [9:53] is equal to 0. [9:54] And those are all candidates for optimality. [9:57] And then you pick the one that gives you the lowest cost. [10:01] So that's an example of approach whereby you first write down [10:06] the necessary optimality condition [10:08] and then you solve for them. [10:09] In the case of unconstrained nonlinear optimization, [10:12] the necessary optimality condition [10:13] is gradient of f equal to 0. [10:16] Solving for them entails solving a nonlinear algebraic equation. [10:22] With an indirect method for optimal [10:24] control is instantiated this framework. [10:27] In the same way, the only difference [10:30] is that of writing down the necessary optimality condition [10:34] for an optimal control problem is much more complicated. [10:39] I'm going to show you those necessary optimality conditions. [10:41] I'm not going to prove them. [10:43] But to justify them and to give you a little bit more [10:46] insight into what they mean, I want [10:48] to draw a parallel with the topic [10:52] of finite dimensional optimization. [10:54] But we want twist. [10:55] And the twist is that now we are also considering constraints. [11:01] So on top of minimizing a cost function f, [11:07] we have to consider a set of unconstrained m constraints [11:12] of the type h of x equals 0. [11:17] So what do you think this setup, finite dimensional optimization [11:22] means? [11:22] This x is a vector with n components. [11:25] So why do you think this setup is, in a way, [11:29] the finite dimensional equivalent and optimal control [11:31] problem? [11:32] What do we have to include constraints [11:34] to have something that is closer to an optimal control problem? [11:38] In other words, which is the equality constraint [11:42] that is the hallmark of any optimal control problem? [11:47] Every optimal control problem has an equality constraint. [11:49] Which constraint is that one? [11:54] Let's go back. [11:55] Well, here, there is only one equality constraint. [11:58] Any optimal control problem entails [12:02] finding controls and corresponding state that [12:05] satisfy the differential equation of motion. [12:08] Differential equation of motion is indeed an equality [12:10] constraint, is a differential equation, as opposed [12:13] to an algebraic equation, but still is an equality constraint. [12:18] So in terms of finite dimensional optimization, [12:20] equality constraint in the nonlinear optimization problem [12:24] are the class of problems that comes [12:25] closest to optimal control. [12:29] So in order to shed some light on how [12:32] indirect methods for optimal control problem work, [12:35] I'm going to tell you a little bit more [12:36] about the equality constraint nonlinear optimization problems. [12:40] And then I'm going to draw a parallelism [12:42] between the necessary optimality condition for equality [12:45] constrained optimization problem and necessary optimality [12:49] conditions for optimal control problems. [12:51] Yeah. [12:53] With constraints as a function equal to 0, [12:56] do you have some kind of constraint [12:57] that's not inequality? [12:59] For example, your velocity needs to be less than something, [13:02] how would you [INAUDIBLE]? [13:03] Yes, so of course, the marginal version [13:07] is a nonlinear optimization problem with equality [13:09] and inequality constraints. [13:11] It is possible to derive necessary optimality conditions [13:13] for those problems as well. [13:16] But that is beyond the scope of this class. [13:18] Here, for me, the reason why I'm going through the equality [13:21] constraint nonlinear optimization topic [13:26] is more from a pedagogical standpoint, [13:28] as opposed to teaching you how to solve general nonlinear [13:31] optimization problems. [13:32] But you are absolutely right. [13:33] In general, even for optimal control problems, [13:38] you might have a more sophisticated class [13:41] of constraints. [13:43] And that leads to more sophisticated, necessary [13:46] optimality conditions. [13:47] That, again, is beyond the scope of this class. [13:49] But if you're interested, I'll be happy to, of course, [13:51] talk to you about this, give you some readings. [13:53] And obviously, you can also take my class in the spring. [13:58] All right, so you may or you may not [14:00] familiar with how to write down the necessary optimality [14:03] conditions for equality constraint, [14:08] nonlinear optimization problems. [14:09] Just to make sure that everyone is on the same page, [14:12] I'm going to summarize to you what [14:15] the necessary conditions are. [14:17] So you know that if there are no constraints, [14:21] then the necessary optimality condition [14:23] is gradient of f is equal to 0. [14:26] The question is, what is now the necessary optimality conditions [14:30] for the case where we have equality constraints? [14:35] This boils down to a very elegant theory [14:38] in mathematics and optimization, referred to as Lagrange theory. [14:43] The bottom line is, define an augmented cost function. [14:49] So the idea is reasoning directly [14:52] in terms of these constraints, it is tricky. [14:55] So the idea is actually to move these constraints into the cost [15:02] function f and then optimize a function without constraints, [15:10] whereby we penalize the violation [15:13] of those constraints as well. [15:15] That's the idea. [15:16] So basically, including in the cost [15:18] function a penalty term that penalizes the violation [15:22] of equality constraints. [15:23] In doing so, we remove the constraints. [15:26] And we can then solve the problem [15:27] as an unconstrained optimization problem, for which we already [15:30] know how to write down a necessary optimality condition [15:33] in terms of gradient equal to 0. [15:37] Of course, there is a lot behind it. [15:40] But as I said, the idea is to set up an augmented cost [15:45] function x and lambda. [15:47] X are the previous decision variables. [15:50] L lambda are additional decision variables [15:54] defined as f of x plus the sum from i equal 1 to m of lambda i, [16:03] hix. [16:08] So this augmented cost function that now accounts [16:12] as an independent variables, both the decision variable x [16:16] and an additional vector of variables lambda-- [16:19] lambda 1, lambda 2, lambda m. [16:24] So this function is referred to as the Lagrangian. [16:37] So the main results in equality constraint non-linear [16:41] optimization problem is that a Necessary Optimality Condition, [16:46] NOC, for a point x star to be a minimum for the equality [17:00] constraint nonlinear optimization problem [17:02] is that the augmented cost function is a stationary point. [17:08] Which means that the gradient of the Lagrangian is equal to 0. [17:13] Which means that the partial derivative [17:14] of the Lagrangian with respect to x is equal to 0. [17:17] And the partial derivative of the Lagrangian with respect [17:19] to the lambdas is equal to 0. [17:25] Sorry, what is NOC [INAUDIBLE]? [17:27] Necessary Optimality Condition. [17:32] So here, of course, there is quite a bit of technical results [17:40] that I'm hiding under the rug. [17:42] But the main intuition, as I said, is as follows. [17:46] Solving directly a problem with the reasoning directly [17:49] in terms of the constraints is hard. [17:51] So the idea is to turn the problem [17:53] into an unconstrained optimization [17:55] problem, for which we already know [17:57] how to write down the necessary optimality condition. [17:59] And the way to do so is to move the constraints to the cost [18:03] function and penalize them through penalty [18:07] variables lambda i's referred to as Lagrange multipliers-- [18:15] Lagrange. [18:21] so something that is very important to note [18:24] is some of you may think that Lagrange is French. [18:27] It is he's not. [18:29] He was Italian. [18:33] So don't get mistaken by the fact [18:35] that-- so Lagrange was born Lagrangie. [18:43] He was born in the same city I'm from, Torino. [18:46] And at the age of about 40 moved to French. [18:50] And well, French people started calling him Lagrange. [18:53] That looks more French. [18:54] And now, he's known as Lagrange. [18:57] Everyone thinks he's French. [18:58] But he's Italian. [18:59] [LAUGHTER] So having said that-- you didn't know that one, eh? [19:02] You thought he was French. [19:04] Oh, I told you. [19:05] Yes, you see. [19:06] I told this to everyone. [19:08] [LAUGHTER] [19:08] OK, so having said that, so these legacy of Italy, [19:14] courtesy of Italy, all right. [19:15] So the idea is that now, we have this augmented cost function, [19:19] where we have a penalized constraint. [19:21] The magic of Lagrange optimality theory is that a point x star. [19:29] And this is the condition for a point [19:31] x star to be a minimum of the original equality constraint [19:35] and optimization problem. [19:36] A necessary condition is that it has [19:38] to be a minimum for the unconstrained optimization [19:41] problem with respect to an augmented cost function. [19:44] What is a little bit weird here is [19:47] that in order to compute the minimum for the augmented cost [19:51] function, we are solving, not only for x, but also [19:54] for the lambdas. [19:58] Another thing to consider here is [20:01] that when you take the derivative of the Lagrangian [20:05] with respect to the lambdas-- like, for example, [20:08] if you take the derivative of the Lagrangian with respect [20:10] to the i-th Lagrange multiplier, what do you get? [20:17] What's that? [20:20] Not 0. [20:21] If you take the derivative with respect [20:22] to the lambda i's, you get hi. [20:27] So this set of conditions basically [20:29] enforce that the point is feasible for the original [20:33] optimization problem. [20:34] Feasible means that it satisfies the equality constraints. [20:37] And the other part ensures that there is indeed a local minimum. [20:41] Practically, what do you do? [20:42] You set up the Lagrangian, you find a point of stationarity. [20:45] You solve for both the x's and the lambdas. [20:48] You then discard the lambdas. [20:50] Those were just auxiliary variables [20:51] that allowed you to crack the problem. [20:53] And then the x becomes a candidate [20:57] for optimality for the equality constraint problem. [21:01] Let's do an example just to clarify things. [21:05] By the way, here, I say a point x star is a local minimum. [21:13] On top of satisfying this equation, it has to be regular. [21:16] Regular means that, basically, the constraint gradients, [21:21] the gradients of the constraints have to be linearly independent. [21:23] So there is a mild technical condition for this to be true. [21:27] This is a little bit beyond the scope of this class [21:29] but just to give you a heads up that there is just [21:31] a mild condition for this to be true. [21:33] But again, here, my point is to give you [21:34] an intuition for the necessary optimality conditions [21:36] for optimal control problem, as opposed [21:39] to give a lecture on nonlinear optimization. [21:43] Let me give you an example. [21:45] So assume that you want to solve the problem, minimum [21:49] of x1 plus x2 subject to x1 squared plus x2 squared [21:56] equal to. [22:01] So geometrically, what does this mean? [22:06] So this y-axis-- oh no, this is the x2 axis. [22:13] This is the x1 axis. [22:18] This is the circle such that the x1 squared plus x2 [22:24] squared is equal to 2. [22:27] So this is the set of feasible points for the optimization [22:31] problem, meaning that the only points that can be considered [22:36] as a solution to that optimization problem [22:38] are those that lie on this circle. [22:41] That's what this equality constraint here means. [22:45] Now, among all those points, x1, x2, [22:47] we want to find those that minimize the linear combination. [22:55] In particular, from a geometric standpoint, [22:57] you can visualize the sum x1 plus x2 [23:02] as lines, which actually form a 45 degree angle. [23:09] So this will be x1 plus x2 equals c. [23:15] This will be x1 plus x2 equal. [23:19] What? [23:20] 0. [23:21] 0. [23:22] This would be x1 plus x2 equal minus c, [23:30] well, If it's more or less symmetric and so on. [23:33] So this basically are the level curves of the cost function [23:36] x1 plus x2. [23:37] So geometrically, if you were to find the minimum, [23:39] what would you do? [23:43] So C is greater than 0, which is greater than minus C. [23:47] So ideally, geometrically, what you want to do [23:50] is to move this line as much to the left as possible [23:54] but with the constraint that there [23:55] should be an intersection with the circle. [23:57] Because otherwise, you don't have [23:59] any feasible x1 plus x1, x2 for your optimization problem. [24:06] So you start from here, maybe here. [24:09] X1 plus x2 is equal to 10. [24:11] You start moving to the left. [24:13] Here, x1 plus x2 is equal to 0. [24:15] You keep moving to the left. [24:17] The more you move to the left, the more x1 plus x2 [24:19] is small until you reach a point of tangency here. [24:24] So by the symmetry of the problem, [24:27] it's not too hard to convince yourself that this is a minus 1, [24:31] minus 1. [24:33] So minus 1, minus 1 is indeed the solution to this nonlinear [24:37] optimization problem. [24:38] Well, let's see if we can find the solution [24:40] by applying the Lagrange multiplier theorem. [24:53] So what do we do to solve this problem? [24:55] Step number one, we form the Lagrangian. [24:59] The cost function is x1 plus x2. [25:07] This is f. [25:11] And to be consistent with our notation, [25:14] x1 squared plus x2 squared minus 2 equals 0. [25:19] So this is the function h. [25:22] The constraint function here, we only [25:24] have one equality constraint. [25:25] So i is equal to 1. [25:32] We have a number of Lagrange multipliers [25:34] that is equal to the number of equality constraints, one [25:37] Lagrange multiplier per constraint. [25:39] Each lambda I is a real. [25:45] So in this case, we have only one Lagrange multiplier [25:49] times the constraint, x1 squared plus x2 squared minus 2. [25:55] Two, we write down the necessary optimality condition [25:59] that, as I said, amount to setting [26:02] the gradient of the augmented function, [26:04] the Lagrangian function equal to 0. [26:06] Remember that now, the Lagrangian function [26:08] depends on both x and lambda? [26:09] So I have to take partial derivative with respect [26:11] to both x and lambda. [26:14] So partial derivative of lambda with respect to x1 [26:19] is equal to 1 plus 2 lambda x1. [26:25] And this has to be equal to 0 as a necessary optimality [26:28] condition. [26:29] Partial derivative of the Lagrangian with respect to x2 [26:33] is 1 plus 2 lambda x2 equal to 0. [26:37] Partial derivative of the Lagrangian [26:40] with respect to lambda 1, well, with respect to lambda [26:44] because we have only one constraint, [26:47] is equal to x1 squared plus x2 squared minus 2 equal to 0. [26:52] See how the partial derivative with respect to the Lagrange [26:55] multiplier gives you the equality constraint that [26:58] basically enforces that what you're solving for [27:01] is indeed a feasible point for the original equality constraint [27:04] optimization problem. [27:05] This is the link between the augmented problem [27:09] and the original problem. [27:11] OK, now, we have three unknowns-- x1, x2, [27:15] and lambda, three equations. [27:18] We solve for them. [27:19] The solution will be a candidate for optimality. [27:21] In particular, from this one, we see [27:25] that the x1 is equal to minus 1 over 2 lambda. [27:29] X2 is equal to 1 minus 1 over 2 lambda. [27:35] I plug this into here. [27:36] So 1 over 4 lambda squared plus 1 over 4 lambda [27:41] squared equal to? [27:44] So this implies that 1 over 2 lambda square is equal to 2. [27:58] So this means that lambda squared is equal to 1/4, [28:04] which means that lambda is equal to plus, minus 1/2. [28:15] So then I go back into these two equations, [28:19] I substitute for lambda. [28:21] So if lambda is equal to 1/2, then x1 is equal to minus 1, [28:31] and x2 is equal to minus 1. [28:34] If lambda is equal to minus 1/2, then x1 is equal to 1 and x2 is [28:42] equal to 1. [28:45] So these are the stationary points for the Lagrangians. [28:51] You throw away the Lagrange multiplier was just [28:54] a tool to get to the solution, but it's not [28:56] what you care about. [28:57] And the Lagrange multiplier actually [29:00] incorporates some interesting information [29:03] about the optimization problem, for example, [29:05] in terms of sensitivity. [29:07] But for the sake of this class, this [29:08] was just a tool to solve the problem. [29:10] We don't really care about it. [29:11] Throw it away-- so a candidate for optimality is minus 1, [29:14] minus 1. [29:15] And a candidate for optimality is 1, 1. [29:18] So then what you do, you go back to your cost function. [29:22] You plug in the first point, minus 1, minus 1. [29:25] We get the cost, minus 2. [29:28] You plug in the second candidate for optimality, 1, 1. [29:32] And you get the cost 2. [29:33] Since 2 is greater than minus 2, actually, [29:35] the minimum, in this case, the global minimum [29:38] is represented by this point. [29:45] Yeah. [29:46] What would happen if you plug-in those numbers [29:48] into Lagrange equation number [INAUDIBLE], the Lagrange? [29:54] Not the lambda, but the big L. If were to plug-in those-- [30:00] Yeah, so if you plug in, they're-- so the intuition [30:04] of the Lagrangian is that-- [30:06] I mean there are some more sophisticated interpretations, [30:09] but intuition, the Lagrangian is an augmented cost function, [30:11] whereby you penalize this constraint. [30:13] At the minimum, since you have a feasible point, this goes to 0. [30:16] And then you have the minimum for your function. [30:22] But for the sake of this class, just [30:23] think of it as basically an unconstrained penalized version [30:27] of your original equality constraint problem-- [30:31] so very important to understand that the procedure. [30:34] You first set up this generalized cost function [30:38] that makes the problem easier. [30:39] And that tells you what the solution of the original problem [30:45] is. [30:45] You write down the necessary optimality condition [30:48] for this augmented cost function and then you solve for them. [30:52] You throw away the Lagrange multipliers [30:54] that you use to augment the problem. [30:56] And you retain the information about the original decision [31:00] variables as your candidates for optimality. [31:04] is that enough to prove that this is the global minima. [31:07] In this case, yes. [31:10] Because since this is a continuous function [31:13] on a compact domain, so there is a minimum and maximum. [31:16] And we find that those are completely [31:20] characterized by the Lagrange stationary point. [31:25] Why wouldn't you try all the points on [INAUDIBLE]? [31:27] Yeah. [31:28] In this case, yeah. [31:29] But it's not always so. [31:30] For example, even in unconstrained optimization-- [31:41] so for example, think about this case. [31:50] So the points that-- [31:51] actually, to make it even simpler. [31:56] So the points that satisfy the gradient equal to 0 condition [32:00] are this point and this point here. [32:04] But indeed, that the global minimum is here. [32:09] So in order for the gradient equal to 0 condition [32:16] to be conditioned for necessary condition for local optimality, [32:19] it is important that the point is [32:22] in the interior of the domain. [32:24] Here, what went wrong is that it was at the boundary. [32:28] So there are some of those nuances. [32:29] But again, I don't want to give a lecture about nonlinear [32:32] optimization. [32:33] I just want to give you an example of some intuition [32:36] on nonlinear optimization to see how [32:39] to interpret the necessary optimality condition [32:42] for optimal control problems. [32:46] Cool! [32:47] Having said that, and here, of course, [32:50] I won't do the demonstration, but the idea of indirect methods [32:55] to solve optimal control problem is [32:58] very similar to the idea of solving [33:00] equality constraint linear optimization [33:03] problem on a finite dimensional set of variables. [33:07] In particular, the idea is to first construct [33:11] an augmented cost function, in the case of optimal control, [33:15] referred to as the Hamiltonian. [33:27] So the Hamiltonian has a similar meaning [33:30] of the Lagrangian function. [33:32] So it's given by your running cost [33:37] g, similar to what you had here with the f for equality [33:40] constraint optimization problem, plus p transpose of t [33:49] times a of x, u, t. [33:57] The function a is the one that was giving you the equality [33:59] constraint point p are additional variables [34:04] that play the same role of Lagrange multipliers. [34:12] But in optimal control, they are referred [34:17] to as the costate variables. [34:21] That is just terminology. [34:23] So in optimal control, typically, people [34:25] don't refer to these variables as Lagrange multipliers but as [34:28] costates. [34:30] Two, they are a function of time. [34:34] So the Lagrange multipliers or costate [34:38] itself are functions, Which makes sense. [34:43] Since the original set of variables or decision variables [34:50] were functions x and u, makes sense that Lagrange multipliers [34:53] also are functions. [34:58] So that would be step number one. [34:59] Step number two entails writing down the necessary optimality [35:03] condition. [35:04] In the case of optimal control problem, [35:06] the set of necessary optimality condition [35:09] is represented by a set of coupled differential [35:11] equations, ordinary differential equations. [35:16] So this is a little bit annoying because now, we [35:19] don't have a simple gradient of f equal to 0 conditions, [35:23] meaning that we have to solve nonlinear algebraic equations. [35:26] We actually have to solve differential equations. [35:30] But the idea will be the same. [35:32] We're given these equations. [35:34] I'm going to give an example. [35:36] We're given these equations. [35:37] We solve for the x, u, and p-- so for the original decision [35:41] variables, x and u and the Lagrange multipliers. [35:45] Then we throw away the Lagrange multipliers. [35:47] And what we get is a candidate for local optimality, [35:50] the same way that in Lagrange theorems, [35:52] we get candidates for local optimality. [35:59] Note the similarities. [36:03] First, the Hamiltonian looks extremely [36:06] similar to the Lagrangian. [36:07] One term is represented by the cost function. [36:14] And the other term is a penalization term [36:17] for the deviation from the equality constraint-- [36:21] in this case, the differential equations of motion, similarity [36:24] number one. [36:25] Similarity number two, one of the conditions, [36:32] in particular the x dot equal partial derivative h [36:37] with respect to p, gives rise to what? [36:41] If you take the partial derivative of the Hamiltonian [36:44] with respect to p, what do I get? [36:47] Partial constraint. [36:49] Yeah, So I get the a, x, u, t, which is really the differential [36:55] equation. [36:56] Same way that when I was taking the partial derivative [37:00] with respect to the Lagrange multiplier, [37:02] I was getting equality constraints. [37:05] That's neat. [37:08] Finally, in this setting, so a finite dimensional setting, [37:13] x is a vector with n components. [37:16] We are assuming that the decision [37:17] vector has n components. [37:20] Lambda is a vector with m components [37:22] because we're assuming we have m equality constraints. [37:25] So I have n plus m, decision variables. [37:32] And how many equations do I have? [37:35] So this equation gives me how many constraints? [37:42] 1.5 dimension. [37:43] Well, so n, this is what I have, n components. [37:47] And here, m. [37:49] So I have n plus m variables and n plus m equations. [37:54] Which means that I have a number of equations that is sufficient [37:56] in order to solve for all the variables, which is great. [38:00] What about here? [38:01] So here, I have here, n differential equation, [38:07] 1 per component of the state vector. [38:12] Here, I have how many equations? [38:18] How many costate function or Lagrange multiplier function [38:24] do I have? [38:28] Remember, one equality constraint, [38:31] since I have one equality constraint, [38:32] a per state variable, and I have n state variables? [38:41] Well, another way to see this is that I'm [38:44] taking the partial derivative with respect to x. [38:47] And x is n variables. [38:48] So what do I get as an output? [38:50] This is a vector, a scalar and matrix. [38:52] It's n. [38:54] n, so n differential equation from here [38:57] and differential equation from here, [38:59] I'm assuming that I have m inputs as usual. [39:02] So I have m algebraic equations from the last one. [39:06] Which means that I have 2n plus m equations 2n of them are ODEs, [39:15] ordinary Differential Equations, and the other M are algebraic. [39:23] How many decision variables do I have? [39:26] I have n variables because we're saying that the state vector is [39:34] n dimensional. [39:35] We have one Lagrange multiplier per constraint. [39:38] And I have n constraint because the differential equation [39:41] is n dimensional. [39:42] So P is n dimensional as well. [39:45] And I have n control. [39:46] So I have 2n plus m functions to solve for. [39:54] 2n plus m functions to solve for and 2n plus m equations, good. [39:58] Actually, I have a number of equations that is sufficient [40:01] in order to pinpoint all the decision variables [40:06] and Lagrange multipliers. [40:10] The known thing about this necessary optimality condition [40:14] is that these are differential equations. [40:17] So to solve for them, I need also boundary conditions. [40:22] We know that to solve differential equations, [40:27] I mean you need to have initial condition that the pinpoint, [40:30] the constants of integration. [40:32] In particular, how many constants of integration [40:35] do I have here? [40:40] I'm going to call out every ICME student here to answer. [40:43] I know there is one ICME student. [40:46] So roughly speaking, for each first order differential [40:50] equation, you have one constant of integration. [40:55] So for example, if I have the equation x dot equal x, [41:05] what's the solution? [41:09] [INAUDIBLE] x. [41:12] Well, yes, but then, of course, I also [41:14] have a constant in front, which depends [41:16] on the initial condition. [41:18] Let's say that the initial condition is x0 is equal to 0. [41:21] In that case, what is a equal to? [41:25] [INAUDIBLE] [41:26] 1. [41:27] Not 1. [41:28] Because if you set a is 0, this is equal a. [41:32] And this is equal to x of 0 that we're saying is equal to 0. [41:36] So now, what am I saying? [41:39] Yeah, OK, yeah. [41:42] All right, so here, you have n constants of integration. [41:51] So n of them come from the initial boundary condition, x, [41:54] t0 equal to t0. [42:09] The second one comes from this more convoluted expression-- [42:15] don't read too much into it, use it more as [42:18] a tool to solve these problems. [42:20] Here, delta xf captures the notion of admissible variation [42:27] at the final endpoint. [42:28] Whether we want to reach a specific point at time tf [42:32] or actually that is free, part of the optimization. [42:35] Delta tf captures the admissible variation [42:39] with respect to the final time whether the final time is fixed, [42:42] meaning I want to get somewhere in 10 seconds [42:44] or the final time is free, meaning [42:46] that I want to get somewhere, as soon as possible, [42:49] but I'm not specifying the final time. [42:55] Which basically means that assuming [42:57] that we can solve the set of differential equation, [43:00] in order to do so, we have a differential equation, [43:02] but we had to set up the boundary conditions correctly. [43:06] Actually, I'll skip this one. [43:07] This is the last annoying slide before doing the fun stuff. [43:11] So in particular, the way to interpret this slide [43:16] is as follows. [43:23] So small h here is the terminal cost. [43:31] So this reads as delta h partial derivative with respect [43:38] to x minus p delta xf plus-- of course, [43:46] I'm omitting the other independent variables [43:50] for brevity-- plus the Hamiltonian [43:56] plus the partial derivative of the final cost with respect [44:01] to time. [44:05] Delta tf has to be equal to 0. [44:08] So that's the master equation. [44:11] So now, the task is, how do I find the boundary conditions [44:16] in such a way that I can pinpoint [44:17] all constants of integration? [44:19] In all cases, we are assuming that we are [44:23] fixing the initial conditions. [44:24] So we already have n conditions. [44:26] But we need n more because we have a 2n differential [44:31] equations, which means that 2n constant of integration. [44:35] So let's see how to use that master equation in order [44:39] to pinpoint those boundary conditions. [44:42] In particular, assume as-- [44:52] yeah, let's consider this case first. [44:54] I think it's the easiest. [44:55] So assume that tf-- [44:58] OK, so the easiest is assume that the tf is fixed [45:04] and xf is fixed. [45:07] So the final time and final conditions are specified. [45:10] I want to get somewhere specifically at final time tf [45:14] equals 10, say. [45:16] In that case, how do I make substitutions for the delta xf [45:20] and delta tf? [45:21] This, again, has the interpretation [45:24] of admissible variation. [45:30] So if both the final time and the final state are fixed, [45:34] is there any admissible variation [45:37] for the final time and final state? [45:40] No, since they're fixed, every variation is inadmissible. [45:44] Which means that delta xf has to be considered equal to 0. [45:47] Delta tf has to be equal to 0. [45:48] This is more a mnemonic thing rather than a conceptual thing. [45:51] It means that this equation tells you nothing. [45:53] But on the other hand, if xf is fixed on top of the condition x, [45:58] t0 is equal to 0, we also have the condition x, tf equal xf. [46:02] And this is another set of n equations. [46:05] So in total, I have 2n equations that allow me to pinpoint the 2n [46:10] constant of integration. [46:14] So that was an easy case. [46:16] A slightly more complicated case is the case when tf is free-- [46:22] this is a very typical case by the way-- [46:25] and xf is fixed. [46:28] So you want to get to a final condition as soon as possible. [46:34] But a specific final time is not specified. [46:37] You want to minimize that time. [46:39] So actually, time becomes part of the optimization. [46:42] So in this case, we still have x, t0 equal to x0. [47:00] So tf is now free. [47:02] So since xf is fixed, what is the admissible variation for xf? [47:07] So this has to be 0. [47:09] So this term goes away. [47:11] But delta tf is free, which means that it [47:14] can take any possible value. [47:16] So for this equation to be satisfied [47:19] for every possible value of delta tf, [47:21] what should this term be equal to? [47:23] [INAUDIBLE] to 0. [47:25] This has to be equal to 0. [47:26] So this master equation is implying [47:29] that h plus partial derivative of a small h with respect [47:35] to time has is to be equal to 0. [47:40] And then we have the condition x, tf is equal to xf. [47:45] So I have n equations from here, n equations from here, [47:49] and one equation from here. [47:51] So in this case, we have a 2n plus 1 equations for 2n constant [47:57] of integration. [47:59] So why do we think we need an additional equation [48:02] to solve for everything? [48:04] So what do we have here that we have additionally [48:07] to determine that we did not have here? [48:14] We have the time is free. [48:16] So on top of the 2n constant of integration, [48:20] I need an additional equation that allows me to pinpoint [48:23] the optimal final time, which is given by this one. [48:28] So everything come together. [48:30] So at this point, you should understand the trick. [48:33] So when you want to break down the boundary conditions, [48:37] usually, you have an initial condition. [48:41] And then you go into the master equations [48:44] and you make the appropriate substitutions for delta xf [48:46] and delta tf, depending on whether the final condition [48:50] for the state and/or the final condition for the time [48:52] are free or not. [48:55] Can you remind me what little h is again? [48:57] Little h is the terminal cost for your optimal control [49:01] problem. [49:04] OK, so I'm going to do an example that [49:06] should clarify all of this. [49:08] But before we do that, something to consider [49:12] is that in your experience, you might [49:15] have been exposed primarily to initial value problems, [49:19] whereby you have differential equations, whereby [49:22] all the boundary conditions are enforced at the initial time, [49:29] so on one side-- [49:31] for example, initial position and initial velocity. [49:35] In the solution to the necessary optimality condition [49:40] for optimal control problem, you have some boundary conditions [49:44] that are specified at initial time [49:47] and some boundary conditions that are [49:48] satisfied at the final time. [49:52] So this class of problems are referred to [49:55] as the notorious two-point boundary value problems, [50:01] much more complicated than initial value problem. [50:03] So if you thought that the differential equations were [50:06] difficult, this is difficulty on steroids. [50:10] The difficulties, now, you have to find a function that [50:15] not only satisfy the differential equation, [50:17] but satisfies constraints both at initial time and final time. [50:21] So if you think a little bit, that is much trickier to do. [50:25] It is tricky. [50:27] Fortunately, there are some solvers out there to do so. [50:31] I'm going to give you an example in Python [50:34] that you're going to use for your last problem in the P set. [50:39] And the solver is referred to as CVP solver. [50:42] The CVP solver is a handy function in Matlab [50:45] that allows you to solve two-point boundary value [50:48] problem of this form, whereby you have a boundary [50:50] condition and initial time and the final time, exactly what we [50:53] have for optimal control problems. [50:55] If you're a matter of user, which you should not, [50:57] because you're taking this class, [50:59] there is also a boundary value problem solver [51:01] model called BVP4C. [51:04] There are a lot of interesting aspects [51:06] of numerical optimization behind them. [51:09] But again, in the sake of this class, [51:10] I want to give you the operational insights [51:13] about these tools. [51:14] So let's consider a simple example. [51:16] So this example has nothing to do with optimal control. [51:18] It's just an example to show you how [51:20] to solve a two-point boundary value problem. [51:23] I'll discuss it later, how to apply a two-point boundary value [51:26] problem solver to optimal control problem. [51:30] So here, I have a differential equation [51:31] whereby z dot 1 is equal to z2. [51:34] Z dot 2 is equal to minus absolute value of z1. [51:37] z1 is equal to 0. [51:38] And z1, 4 is equal to minus 2. [51:41] So the initial time is 0. [51:42] The final time is 4. [51:44] I have a boundary condition at initial time [51:48] and a boundary condition at the final time. [51:55] All right, so that's how the solver works. [52:02] And it is indeed pretty easy. [52:06] So first of all, you install it. [52:09] And we provide a link in the PC to do so. [52:12] So the way it works is that you have [52:14] to specify a couple of things and then it [52:16] works, in general, pretty well. [52:17] So the first thing that you specify [52:20] is the callback function that describes your-- [52:25] actually, let me write here. [52:36] So we have z dot 1 is equal to z2. [52:45] Z dot 2 is equal to minus z1. [52:53] Z1, 0, 0-- z1, 0 is equal to 0. [52:57] Z1 at 4 is equal to minus 2. [53:05] So the first thing that you do is [53:10] to set up the callback function that [53:13] specify the right-hand side of the differential equation. [53:16] So basically, you tell the solver what [53:18] the differential equation is. [53:19] In particular, the first element of the differential equation [53:26] is z2. [53:26] Remember that in Python the counter starts from 0? [53:29] So z2 here is z1. [53:32] And the second component of the differential equation [53:36] is minus absolute value of z1, which is 0. [53:41] Because again, in Python the counter starts from 0. [53:44] So that's a differential equation. [53:46] The second thing to set up is the function [53:51] that specifies the residuals of the boundary conditions. [53:55] The residuals means by how much your current boundary [54:00] values are not fulfilling the boundary conditions. [54:03] So basically, this is an error term [54:05] that the solver uses in order to eventually find [54:08] the solution to the two-point variable problem. [54:11] So in particular, the residual for the first boundary condition [54:16] is simply the value of z1 evaluated at 0 here. [54:27] And M second residual is z1, 4 plus 2. [54:34] So basically, you take everything [54:36] to the left-hand side. [54:38] What you get should be equal to 0. [54:40] What the solver wants to know is if that is actually [54:43] 0 or something else. [54:44] If it is something else, then it's [54:45] going to penalize it so that you get to a solution that satisfies [54:48] the boundary conditions. [54:50] Does this make sense? [54:52] This is for this solver, just the thing that [54:54] is a little bit less intuitive. [54:56] But if you think about, it's just [54:57] a way to specify by how much you're violating the boundary [55:00] conditions. [55:01] Once you do that, then setting up the problem is easy. [55:04] You define the problem by telling the solver [55:06] how many differential equations we have. [55:08] I'm going to pause this code so that you [55:10] have a template that you can use for your P set. [55:14] You specify the number of left boundary conditions, [55:17] so the number of conditions at initial time. [55:26] You thought it was over, but it was not. [55:28] So left boundary conditions, of course, [55:30] since you know that we have two differential equations, if you [55:33] have a one on the left, it means that you have one on the right. [55:36] The domain of integration between time [55:38] equals 0 at time equal 4. [55:40] What is the OD function, the callback function [55:42] for the differential equation? [55:44] And finally, what are the boundary conditions [55:46] in terms of the residuals? [55:48] The last step is to specify the initial guess, typically [55:53] a constant function-- so something [55:55] that you give the solver to help the solver. [55:58] In this case, we're saying that the guess [56:00] is that the first variable is equal to 1 throughout. [56:04] And the second variable z2 is equal to 0 throughout-- [56:09] just give something sensible, 0, 0, if you have no idea. [56:13] And then you call the solver. [56:17] And that's it. [56:19] So you do that over here. [56:26] So here, we have z1 and z2. [56:29] We see that, for example, yeah, z2, [56:37] its derivative is always negative [56:40] because it's minus absolute value. [56:42] So as you can see, it goes down. [56:44] For z1, the derivative has the same sign of z2. [56:48] So as long as z2 is positive, z1 is going up. [56:51] And when z2 is negative, then z1 goes down. [56:55] So the differential equation, at least from intuition standpoint, [56:58] is satisfied. [56:59] Boundary condition, z1, 0 is equal to 0. [57:03] And z1, 4 is equal to minus 2. [57:06] So this is to show that, actually, we [57:08] have solved the two-point variable problem-- [57:10] so pretty handy tool. [57:13] As it is, that's useful for optimal control becomes useful [57:19] once we apply it to the nested optimality condition. [57:21] Before doing so-- I already got to the key point [57:24] of this lecture-- we need to do one more thing. [57:28] The thing is that if you sit here, [57:34] this solver and pretty much any solver [57:36] out there assume that you know the final time. [57:41] By an optimal control, oftentimes, the final time [57:44] is up to optimization. [57:48] So what do you do? [57:51] What you do is that you look back to your slides and lectures [57:55] or video recordings and then you remember that there [57:58] is a very nice article that is basically a bunch of tricks [58:03] to take your two-point multivariable [58:05] problem with weird features, like, for example, [58:08] the final time is not specified, and so on. [58:11] And there are a bunch of tricks to recast the two-point boundary [58:15] value problem in standard form, meaning in this form, [58:19] meaning in a form where now you can call a solver. [58:23] So there are a number of tricks. [58:25] The most important one is the one [58:27] that allows you to handle a final time that is not specified [58:32] and up to optimization. [58:35] And that trick is, again, related to the notion [58:39] of time rescaling. [58:42] And the idea is fairly simple. [58:46] The problem is that assume that you have an optimal control [58:51] problem whereby tf is free. [58:53] So you write down a necessary optimality condition. [58:56] But once you try to call the solver, tf is free. [58:58] So you cannot really apply it directly. [59:03] So then you do time rescaling. [59:04] Then you introduce a new time, tau, is [59:07] equal to the true time over tf. [59:13] You do know tf. [59:14] But you know that at some point, you're [59:16] going to get at the end of the operation. [59:18] So when t is equal to tf, t-tau is equal to 1. [59:21] So with respect to tau, now, the domain of integration [59:26] is fully specified is between 0 and 1. [59:29] So now, what you do is to view x of t, as x of tau [59:38] t similar to what we did on Wednesday. [59:41] And then the derivative of x of t with respect to t [59:46] is equal to the derivative of x with respect to tau times [59:50] the derivative of tau with respect to t, [59:56] which is equal to 1 over tf. [59:59] Which means that the derivative with respect to tau [60:03] is equal to tf derivative with respect to t. [60:09] So then the idea is to introduce a time scaling. [60:15] tf now just becomes-- so we express [60:18] all the derivatives with respect to the new time, tau. [60:22] Tau is, regardless of the specific value of tf, [60:26] is always between 0 and 1. [60:29] You replace all the derivatives with respect [60:30] to t as derivative with respect to tau [60:34] by simply multiplying by tf. [60:36] Now, tf is a constant. [60:37] You don't know it. [60:38] It's something that you're solving for. [60:41] So you add a state variable with trivial dynamics, [60:45] r dot equal to 0, where r is tf basically. [60:52] And then the solver will pick up the right value of tf [60:55] in order to satisfy the boundary condition that [60:58] come from the master equation that I did. [61:02] Here, by solving the master equation, [61:04] I'll give you an example that makes this very clear. [61:07] And that's something that you have to do for your PC. [61:10] So let's assume we want to solve this optimal control problem. [61:13] It is an important one. [61:14] It's simple. [61:15] But it's actually an insightful one. [61:31] So x double dot is equal to u. [61:35] So I have a double integrator-- [61:38] so think of a particle on a line that has momentum. [61:46] So x double dot is equal to u. x0 is equal to 10. [61:52] x dot 0 is equal to 0. [61:58] X, tf is equal to 0. [62:02] And x dot tf is equal to 0. [62:08] So the idea is that you start-- so let's say that this [62:11] is the origin of your line. [62:13] So you start here, a coordinate at 10 with velocity 0. [62:19] And you want to get here. [62:23] But you want to do so in a way whereby you minimize [62:29] the time plus control effort. [62:44] So the reason why this is an interesting problem [62:47] is that you have dynamics. [62:49] It's a double integrator. [62:50] And you are considering a cost function [62:52] that is very common one-- a combination of time [62:57] and controlled effort, here represented as a U square. [63:00] tf is free. [63:03] And you are trying to optimize it. [63:05] So now, the task is we have this problem, [63:08] how do we use everything we learned so far to write down [63:10] the necessary optimality conditions to set up a two-point [63:13] [INAUDIBLE] problem that I can solve? [63:15] And then I just call the solver, and I'm [63:18] done, which is, by the way, what you [63:19] have to do for the problem in your last problem in the P set. [63:25] So let's do that. [63:27] So you had to be methodical. [63:29] So the first step is you write down the Hamiltonian. [63:33] So the Hamiltonian here is equal to what? [63:38] G. [63:39] G. So g is 1/2 bu squared plus the term penalizing [63:47] the dynamics. [63:49] Let me be very specific. [63:51] So x double dot is equal to u. [63:53] First of all, we had to put the problem in the same notation [63:57] of the optimal control problem. [63:58] Remember, in the optimal control problem, [64:00] we have a first order differential equation. [64:03] So what we do is a standard trick. [64:05] We denote by x1 is equal to x. [64:12] Then x2 is equal to x dot. [64:16] So I can express, equivalently, the dynamics as x1 equal to x2 [64:21] and x dot 2 is equal to u. [64:24] Do you know this trick to go from first order differential [64:26] equation-- sorry, second order differential equation [64:29] to a system of two first order differential equation. [64:33] So yeah, this is just an equivalence. [64:36] So how many differential equations do I have? [64:38] 2. [64:39] How many Lagrange multipliers I'm going to have? [64:42] 2, 1 per differential equation. [64:45] So this is a plus p1 times x2 plus p2 times u. [64:53] That's the first component of differential equation. [64:55] That's the second component of differential equation. [64:59] Good. [65:03] So we have set up the Lagrangian-- oh, sorry, [65:05] Hamiltonian. [65:07] We have to take the partial derivative. [65:10] So partial derivative of h with respect-- [65:14] so what are the necessary optimality conditions? [65:20] Here. [65:22] We have partial derivative h with respect to p. [65:24] We will get exact differential equation. [65:26] So I'm not going to write them again. [65:29] The interesting one is partial. [65:33] pidot. [65:37] One is equal to minus partial derivative of h [65:40] with respect to x1. [65:43] So-- go here, derivative with respect to x1 is 0. [65:47] Here, I don't have any dependency with respect to x1. [65:50] So this is 0 is equal to minus partial derivative h [65:55] relative to x2-- [65:58] 0, 0, 1. [66:02] So this is equal to minus p1. [66:07] And then you have the third one, the partial derivative [66:10] of h with respect to u, which is equal to bu plus p2. [66:24] This has to be equal to 0. [66:26] Which means that u is equal to minus p2 over b. [66:36] And now, you're pretty much done. [66:38] So you have the set of differential equation [66:42] x dot 1 equal to x2, x dot 2 equal to u. [66:49] But u is equal to minus p2 over b. [66:54] p dot 1 is equal to 0. [66:58] p dot 2 is equal to minus p1. [67:04] So that's your set of differential equation [67:05] you want to solve for. [67:07] But the final time is free. [67:10] So now, you cannot directly call the two-point boundary value [67:13] solver. [67:14] So you have to do the time rescaling trick. [67:17] Which means that now, you rescale with respect [67:19] to time tau. [67:21] Which means let's denote the derivative with respect [67:29] to tau as prime instead of dot. [67:34] All of this becomes derivative with respect to tau. [67:39] But when I take the derivative with respect to tau, [67:42] I have to multiply by tf, as we showed before, for consistency. [67:52] In order to represent everything in the same formalism [67:56] that the BVP solver wants, we add an additional variable r [68:00] that represents tf. [68:01] We give it the trivial dynamic 0. [68:05] So that will be r. [68:06] That will be r. [68:08] That will be r. [68:12] And now, I have to write down the boundary conditions. [68:16] So I have four boundary conditions already. [68:27] Yeah, I'll leave this one. [68:33] So what are the boundary conditions? [68:37] So I have x0 equal 10, x dot 0, so x2, equals 0. [68:53] And x, tf is equal to 0. [68:58] So x of tf is equal to 0. [69:07] And then x dot tf is equal to 0. [69:13] I still needed to write down the additional boundary [69:21] condition with respect to time. [69:22] I go into the master equation. [69:26] I don't have a dimensional variation with respect to x. [69:29] So the only equation that we have to satisfy is h. [69:36] So the Lagrangian plus the partial derivative of a small h [69:39] with respect to t, the t. [69:41] So what's the derivative of this with respect to if you view h [69:45] as a function of t. [69:46] What's the derivative of t? [69:48] [INAUDIBLE] [69:50] It's alpha tf, yeah. [69:53] Plus alpha tf equals 0. [70:10] Yeah, so now, r is a variable with the trivial dynamics. [70:19] What you do is in this equation, I just substitute the tf with r. [70:25] And now, I have a set of five differential equations with five [70:30] boundary conditions and the domain [70:32] integration between 0 and 1 because I've [70:34] done the time scaling. [70:37] Here, I've written the differential equation [70:39] in a more compact form-- [70:40] just to verify what if I find, this is straightforward. [70:44] But let me show you the code. [70:52] Yeah, so I went ahead and I implemented this stuff. [70:54] So u is equal to minus 1/b here times a p2. [71:04] I called the vector z is x1, x2, p1, p2, r. [71:13] So this is saying that the u-- oh, no, is y, sorry. [71:27] Y. So u is equal to minus 1 over y3. [71:32] This is y3, probably. [71:36] Consider. [71:38] This is Y3. [71:40] y3 And so we are encoding that one. [71:46] And so x dot, which will be x1, x [71:49] dot x2 dot is equal to y4, which is this one, times the dynamics. [71:57] You can easily verify the dynamics, [71:58] what I want to show that indeed I'm multiplying by the time [72:02] scaling factor. [72:04] And then, basically, encoding the dynamics [72:08] also for p and then the dynamics for the dummy state that [72:12] represents the final time tf. [72:16] So just take your time to go through this code, [72:19] but that's pretty much it. [72:20] So once you understand this, you can fairly easily [72:23] solve a large variety of optimal control problems [72:26] by applying these techniques. [72:32] The other thing would be just to set up the boundary conditions. [72:35] This is exactly what we have there. [72:37] We have the one on the left, the ones on the right. [72:40] And the weird one will be this one here, the one [72:45] that entails the Hamiltonian. [72:48] So what it did was to-- [72:57] what they did was to solve this problem for different values [73:01] of B and alpha. [73:07] So I keep B fixed and I change alpha. [73:11] Changing alpha means that, basically, you [73:12] give more weight on the fact that they are taking longer. [73:17] So alpha larger means that you care more about time [73:20] than control effort. [73:22] So I solve the problem for several combinations [73:24] of parameters. [73:26] And you can see that, as alpha is increased, [73:32] the final time, so the time I get to the destination, [73:35] decreases. [73:36] Blue is the analytical curve in terms of this problem [73:41] can be solved analytically. [73:42] You don't need numerical methods. [73:44] The red dots are the result of the numerical integration-- [73:48] so to show you that actually this method is quite accurate. [73:52] I'm going to pause this code as well. [73:57] So any questions here? [73:59] Yeah, so the [INAUDIBLE] method and the Hamilton method [74:04] is a type of constraint? [74:06] That's right. [74:07] And yeah, which in particular entails [74:08] solving differential equations, as opposed [74:10] to algebraic equations. [74:11] Yeah, because the constraints are variables. [74:13] Yeah, that's the third point. [74:15] That if I show you right away, the differential equation, [74:18] you will say, well, what is this stuff? [74:21] Instead, by showing first, equality constraint [74:25] and optimization, I hope that this seems a little bit [74:27] less esoteric. [74:31] All right, the last couple of minutes, the other very small [74:35] thing that I wanted to chat about [74:36] is the relation between direct methods and indirect methods. [74:40] So I won't go through the entire discussion [74:42] because it would take another 10 minutes. [74:45] But the idea here is, as I said, indirect methods-- [74:48] we already discussed this on Tuesday-- [74:51] the idea is to transcribe the problem into a nonlinear [74:53] optimization problem by doing a discretization of differential [74:57] equation and of the integral for the cost. [75:00] We already discussed this. [75:01] So we already know this. [75:03] Now, the question is, what is the relation [75:05] between the solution that a direct optimization [75:11] solver provides and the solution that you will [75:14] get with an indirect method. [75:16] So what's the connection between the two approaches? [75:18] Well, the connection is as follows. [75:21] So this is now a finite dimensional nonlinear [75:24] optimization problem-- finite dimensional [75:26] because you have a finite number of decision variable x of i, [75:29] where i is equal to 1 but dot capital N. Capital [75:33] n is the number of discretization points. [75:36] So the connection is that if you write down [75:39] the necessary optimality condition for this nonlinear [75:42] optimization problem-- [75:49] I'm not going through all the details. [75:50] This is more for your own-- it's bonus information. [75:53] I'm not going to ask you about this, neither in the pre-set [75:57] nor in the midterm 1 nor in the midterm 2. [76:00] It's just for your own edification. [76:02] If you write down the necessary optimality condition [76:04] for the nonlinear optimization problem for the transcribed one, [76:09] and if you take the time step, hi, [76:12] so basically the discretization in time, [76:14] if you make it smaller and smaller, [76:17] the necessary optimality condition [76:19] that arise from discretization and nonlinear optimization [76:21] problem converge to the necessary optimality condition [76:26] for the optimal control problem. [76:28] So that's the consistency check. [76:30] So if you go the direct route, so you first transcribe [76:33] and you write down the necessary condition, [76:35] but if the time step is very small, [76:38] those necessary conditions look like almost identical, [76:41] indeed are the discretized version [76:43] of the necessary optimality condition [76:45] for the optimal control problem that are [76:46] leveraged by indirect methods. [76:50] So oftentimes, this is not said. [76:52] So it is not mentioned. [76:53] So it seems that these two approaches [76:54] are completely different. [76:56] Indeed, they are tightly coupled, [76:59] as it should be because we're solving the same problem. [77:03] As I said, this is more information for you. [77:05] It's going to be presented in the lecture notes. [77:07] You have the slides here. [77:08] But the topic of indirect methods [77:11] is going to be covered in the midterm. [77:13] This relation between direct and indirect [77:15] is not going to be covered in the midterm. [77:18] All right, tools out there, I already [77:20] mentioned those on Tuesdays. [77:22] Next time, so next week, we'll focus now [77:24] on the problem of trajectory planning and optimization [77:26] with constraint, so where we have [77:28] to account for obstacle avoidance, [77:30] which will lead us to talk about motion planning algorithms. [77:34] OK, so have a nice weekend, and I'll see you on Tuesday.