Sorry! I'm hoping to make more videos someday... but yeah, it was much easier to find time for this sort of thing in grad school.

In general, you can add just about anything you want as a decision variable in a trajectory optimization problem. One common example would be slack variables for a constraint, or a parameter in the dynamics model. Specifically in my OptimTraj Matlab package there is some limitation on the types of decision variables, which I did mostly to keep the user interface simple enough to use in an educational context. The GPOPS-II optimization package (also in Matlab) has a bit more flexibility, for example, allowing model parameters to be decision variables. I'm sure other packages have more flexibility as well. Or you could code up your own transcription, and then the sky is the limit. The key thing to keep in mind is that you will need to be careful that you still have a "well behaved" problem. For example, you'll still need consistent / continuous gradients of those new decision variables.

Mostly just different warm-up routines. The one “real” song near the end is a an excerpt from “simple gifts”. Glad that you like it!

Excellent question. Yes, you can make the duration of the trajectory a decision variable, and in many cases that allows the solver to find a "more optimal" solution. One detail is that the total duration then couples nearly every constraint in the optimization problem, making the overall problem more challenging to solve. You can also end up with some counter-intuitive behaviors if you don't set up the objective function and constraints carefully. One related topic: it is generally a bad idea to make the duration of all segments into decision variables, as the solver will often get stuck (Bett's discusses this in his book in some detail). Generally you fix the "mesh fraction" allocated to each segment, and then allow a small number of "phase durations" to be decision variables (see GPOPS-II documentation for more on this).

You can add a constraint on an intermediate point in either method, although the gradients are much nicer in the direct collocation formulation. The approach is to select one or more knot points at which to apply the constraint, and then append them to the optimization problem. You always need to watch out for "tunneling". For example, if you have widely spaced collocation points, you can compute a "feasible" trajectory that passes directly through a thin wall if each of the collocation points is "not in collision" with the wall. There are various heuristics for avoiding this, most either based on "padding" the constraint (making the wall wider than it really is) or adding extra collocation points near obstacles.

@@MatthewKelly2 thanks for the answer! I am thinking on how to implement this intermediate constraint though. Maybe using a conditional statement (in the tree example one could do something like this: IF x=obstacle_coordinate AND y<= tree_height THEN Ceq=1 ) ? It doesn't seem very efficient to me though..

@@emanuel4516 - Correct. The 'if' statement would be no good, as it causes a discontinuity in the sparsity pattern of the constraint jacobian. You'll want to add a constraint something like: 'distance_to_obstacle(state(i)) > 0', and then apply that that constraint for every single point on the trajectory that could come in contact with the tree. This allows the NLP solver itself to figure out if the constraint is active or not. The 'distance_to_obstacle' function should be continuous (no if statements).

If you follow the first link (www.matthewpeterkelly.com/tutorials/trajectoryOptimization/cartPoleCollocation.svg), you'll discover a big SVG file that has all of the slides in it. I created this file using Inkscape. There is a LaTeX plugin for inkscape, although it was pretty unreliable at the time of writing this presentation. The animations (really just links to different views onto each slide) were auto-generated using Sozi, and animation front-end for Inkscape.

Sorry - I don't have much experience using Scipy for trajectory optimization! Let me know if you find a good library.

@@sambrothie Wow thank you this is very neat as well. Currently I am also trying to solve it a longer way so I can better learn about how this all works. Your package is a great tool for me to check. Do you happen to know what the equality constraints (final/initial state) would look like if I were to use scipy minimize?

It depends what you mean by "input law is discontinuous". Let's be more specific. If the objective or constraint function has a discontinuity, then you've got trouble. This will cause a jump in the gradients that will break most continuous optimization solvers. The general "trick" is to reformulate your problem in such a way as to inform the solver about the discontinuity and let it manage it via switching the set of active constraints. This is often done using slack variables. A common example is to replace an absolute value function with two positive slack variables and an equality constraint, as is described in Bett's book (see references at end of presentation or in my journal paper linked in the description). I If the discontinuity occurs in the solution of the trajectory optimization, which can happen for some smooth optimization problems, then you can also have problems. The NLP will often converge, but your discretization will often be bad. The solution here is to iteratively adjust your mesh, this can be done manually, or through adaptive meshing. See the research by Anil Rao et al. on HP adaptive meshing that is used in GPOPS for a concrete example and thorough analysis.

@@MatthewKelly2 thank you so much for your complete answer!! However, I don’t know if I’m mixing things here, but the slack variables is part of the KKT conditions for optimality, right? And what if I cannot reformulate my NLP to make it continuous for my feasible set, this means that I’ll have to go for stochastic methods of optimization? Sorry for coming with more questions....

@@pedrocalorio1655 Some NLP solvers use slack variables internally, but you don't need to worry about that. The slack variables that I'm talking about would be added directly as decision variables while formulating the NLP, typically as extra state or control variables. The `minimumWork/MAIN_cstWork.m` example in my OptimTraj software shows a simple example of using slack variables to represent an absolute value function. Similar tricks can be used for min() and max(). There are also various ways to "smooth out" functions and interpolation from tables. Iterative methods can be made "smooth" by using a fixed number of iterations. You can use stochastic optimization, but I've never gotten that to work reliably. The problem is that trajectory optimization requires a large number of equality constraints for the defects, and stochastic optimizer have a really hard time with equality constraints. Put differently, the whole idea behind collocation methods (like I describe here) is to put the optimization into a format the makes it easy for sparse gradient-based solvers to optimize. If you switch to a stochastic solver, then you would probably want a different formulation entirely, something closer to single shooting (which has its own set of challenges).

This animation is playing back the solution to a trajectory optimization problem. If you follow the "source code" link in the description it will bring you to OptimTraj, a trajectory optimization library that I wrote which includes this problem as an example. There is also a link to a tutorial that explains some of the concepts behind trajectory optimization. You can definitely make a LQR controller stabilize this sort of trajectory. The trick is to linearize around the trajectory, not the final solution. Check out Russ Tedrake's Underactuated Robotics class at MIT, where he discusses this topic in depth: underactuated.mit.edu/lqr.html#example2

​@@MatthewKelly2 Thank you much for your information!

@@MatthewKelly2 Thank you so much again. Until now I didn't know what I have to study. Thank you very much for introducing about this field.

Good question! In the demo that I wrote, it is a hard-coded assumption that the pole length is fixed. That being said, you can definitely make a version of the dynamics function that supports a time-varying pole length.There are three ways that I could imagine doing it. (1) Add a passive element, such as a spring, that applies along the pole and allows the mass to move along the length. This would require writing out a new dynamics equation, but would not require adding new inputs to the function. (2) Explicitly make the length of the pole a function of time and then pass that function in as a parameter. Then take the time that is passed into the dynamics function and use it to evaluate the pole length (and its derivatives). Then you need to update the dynamics to properly account for accelerations related to changes in pole length. (3) Add an actuator that can control the length of the pole (easiest to use a force actuator here), and then update the dynamics accordingly. This would allow the optimization to "select" the correct length for the pole. In all cases you would need to update the dynamics of the system, but that shouldn't be too tricky as it is a relatively simple system. Good luck!

Good question! In the demo that I wrote, it is a hard-coded assumption that the pole length is fixed. That being said, you can definitely make a version of the dynamics function that supports a time-varying pole length.There are three ways that I could imagine doing it. (1) Add a passive element, such as a spring, that applies along the pole and allows the mass to move along the length. This would require writing out a new dynamics equation, but would not require adding new inputs to the function. (2) Explicitly make the length of the pole a function of time and then pass that function in as a parameter. Then take the time that is passed into the dynamics function and use it to evaluate the pole length (and its derivatives). Then you need to update the dynamics to properly account for accelerations related to changes in pole length. (3) Add an actuator that can control the length of the pole (easiest to use a force actuator here), and then update the dynamics accordingly. This would allow the optimization to "select" the correct length for the pole. In all cases you would need to update the dynamics of the system, but that shouldn't be too tricky as it is a relatively simple system. Good luck!

@@MatthewKelly2 I will try to tinker with these ideas. Thank you so much for your suggestions!

not really