Modelling a dynamic system as a manifold to calculate time optimal inputs.

[Michael Giglia]

Hello,

I stumbled upon pymanopt after I was trying to do some complex trajectory optimization using APMonitor(GEKKO in python). I was able to get pretty far such as generating time optimal trajectories "driving" in R2, and "flying" like a rocket in R3 (with orientation represented as a quaternion). But in my project the driving state of the system drives along a closed manifold (looks like a bread box where you can drive the car on the internals of box), not just in the R2 plane. As well, the flying state of the system is bounded by this bread box meaning I can only fly inside the bread box and when I fly into the breadbox the system moves back to the driving state. 

There are a ton of optimizations that I can see myself trying to implement, but essentially I want to first generate time optimal trajectories to get to some point either on the manifold surface (driving) or some point inside the manifold (flying) in minimum time given that I have some bounded control inputs (such as thrust, braking, steering, angular accel[flying]).

But before I even get to that I'd first like to get my hands dirty with a really simple example to help me understand how to use the library. 

So I finally get to my question: If I want to represent the following dynamic system in the regular form x_dot = Ax + Bu

x=[theta, theta_dot] //state of the system
u = [0; a] // -1 < a < 1; bounded control input
x_dot = [0 1; 0 -d]x + [0; 1]u //diff eqs., 'd' is drag component

How should I be thinking through the problem to represent it in pymanopt? Like how do I define the state-space as a Riemannian manifold (I think thats the manifold I want?) so that I can get from some initial state [x0] to some desired final state [xf] in the minimum amount of time?  My cost function could simply be ((xf-x0)^2 + (tf-t0)^2). t0 would probably always be zero. Also, how do I go about modelling this where let's say I have some maximum velocity "vmax" meaning that even if I put input 'u' to accelerate the system theta_dot couldn't increase above vmax (in both directions, |theta_dot| < vmax).

I'd like to keep in mind though that in the future my system has a lot of non-linearities, and possibly some discontinuities to be modeled as well, but I imagine it will take some time to get to that point. I started reading "An Introduction to Optimization on Smooth Manifolds by Nicolas Boumal" and am on chapter 4 so far. Any guidance or rudimentary examples are appreciated.

Thank You!

Best,

Michael Giglia

[Nicolas Boumal]

Dear Michael,

Thank you for your detailed message.

As I understand it, there are manifolds in your setup, but the problem you are facing isn't necessarily an optimization problem on a manifold, in the following sense:

It seems that your variables are the inputs to your dynamical system. Those inputs are likely in a linear space (real numbers corresponding to engine thrust, brakes etc?) Perhaps there is a manifold for the steering wheel position (that would be a circle, presumably; for 2D at least).

The manifolds seem to come up more as a constraint on acceptable places your system is allowed to go in state space.

Manopt (and PyManopt and Manopt.jl) are actually quite good for optimization on linear spaces as well, so, I'm not saying this isn't the right tool for you. I just want to ask/clarify what the role of the manifold is in the target application.

I think the starting point would be: when you write down the simplest possible optimization problem corresponding to a simplified version of your setup, what are your variables? what space do they live in? are they constrained in some way? Then also: what is the cost function?

Best,

Nicolas

Best,

Nicolas

[Michael Giglia]

Hi Nicolas!,

So I think you are right in that the manifolds would be setup as a constraint on acceptable states in the state space. Although in the future I could see probability outcome of a collision being modelled as a manifold of sorts and optimize on that manifold to find the optimal state to get a favorable outcome of the collision, but I digress.

I believe my state variables would be the following:

state = [car-position, car-velocity, car-orientation, car-angular_velocity, ball-pos, ball-velocity, ball-orientation, ball-angular_velocity]

car-position and ball-position are in R3, constrained by the shell of the manifold of the field and inside that manifold.

car-velocity is in R3, but constrained by { sqrt(vx^2 + vy^2 + vz^2) <= 2300 }, ball-velocity is in R3 constrained by {  sqrt(vx^2 + vy^2 + vz^2)  <= 6000 }

car-orientation is a unit quaternion, ball-orientation is irrelevant and doesn't affect dynamics of the system

car-angular_velocity is in R3, but constrained by { sqrt(wx^2 + wy^2 + wz^2) <= 5.5 }, similar for the ball-angular_velocity

The first cost function I'm trying to minimize is: [given an initial car and ball state, minimize{(car-position - ball-position)^2 + (tf - t0) } ]by changing the inputs over time. 

But I need to keep in mind that the system has two "dynamic states", one flying and one driving and specifically when transitioning from the driving dynamic state to the flying dynamic state, the velocity of the system increases immediately normal to the manifold it's driving on (this is referred to as a jump, there is also a double jump, but I think once I can model a jump a double jump should be fairly easy). And the system can also "land" back onto the manifold to change back to the driving state which has a different set of dynamical equations from the flying state. Also to make it clear the ball will undergo standard projectile motion and undergo elastic collision when it collides with the field manifold (the collision I would like to model, but I believe this comes up as a discontinuity so I think I'll start by not modelling this unless this type of discontinuity is easy to model).

I think showing a small video will maybe help visualize what I imagine. I've attached it as a gif (example traj.gif). In this case this is not an optimal trajectory by any means but just to show what the field looks like and the dynamics as well as the transition from driving to flying. In this case the ball is in the center of the field and not moving, but in reality the ball will be moving through the air in some way which the car is to intercept. Also you can see that there is an acceleration due to throttle as well as a separate acceleration due to "boost" which can be used both when driving or flying (i just happened to show an example where I didn't use boost while driving).

I hope I was able to provide you with what you were asking, if this is too complex to start, the first problem to tackle could be just driving to some point on the manifold in minimum time given some initial state and some desired final state (removing the ball from the equation and removing driving-flying state transition as well).

Best,

Michael Giglia

[Michael Giglia]

I realized that I didn't include my inputs into the state vector. I'll add those here.
input = [throttle, brake, boost, steering, angular_acceleration, jump]

throttle is in R1, 0 <= throttle <= 1 (only applies while driving)

brake is in R1, 0 <= brake <= 1 (only applies while driving)

boost is a binary variable 0 or 1, but I think this could be modelled as a continuous variable 0 <= boost <= 1, and use feedback control to approach the continuous model

steering is in R1, -1 <= steering <= 1, steering input changes the yaw_rate of the car, which is velocity dependent yaw_dot = f(||vmag||) (this only applies while driving)

angular_acceleration is in R3, -1 <= ax,ay,az <= 1 (only applies while flying

jump is also a binary variable 0 or 1, this i don't believe I can easily model as a continuous variable (ive thought about modelling this as some sort of charging/discharging electronic circuit and using the field as a threshold for charging/discharging this circuit, but i might be crazy)

Hopefully this isn't too much information haha!

[Michael Giglia]

So I've decided to really simplify the first optimization I want to succeed at with pymanopt. Unfortunately I'm new to differential geometry and my foundation in math is from a mechanical engineering background so quite a bit less abstract than most of the math I'm reading about to understand differential geometry and applying it to real world systems.

Here's the simplified problem. The system exists on the surface of a circle with a radius r possibly greater than 1. Given some initial state (s0) on the circle, drive to some desired state sf in minimum time given you can only change the acceleration of the system given by (-1<a<1).

I'm having trouble formulating this into an optimization problem using pymanopt and especially don't know how to constrain the position to this circle manifold. Do I define the gradient of the manifold as the systems velocity, meaning that the gradient along the manifold is dependent on a scalar value v? Then is the acceleration the gradient of the value v where v is a scalar bounded by some value |bound_v|>v and a is bounded as well by some other bound |bound_a| > a.

Previously in the other solver I used I defined a discretized time vector [0 to 1] and a manipulatable variable called tf that the solver could modify to minimize the objective function. Then my differential equations look as follows to allow the solver to minimize the tf variables:

x.dt()/tf = v 

v.dt()/tf = a [a is a manipulatable variable as well as tf] 

obj = tf + (final*(x-xf)^2) + (final*(v-vf)^2)

I've attached a picture drawn out of this simplified system as well as attached a file showing the 1D optimization done in gekko with the output of the solver.

Are there any examples out there using manopt that solve this type of optimization that I could look at to learn from? The optimal_rotations.py example I'm just not sure what it's optimizing for and I'm not sure how I can apply that example to my system. Sorry if this is a rudimentary optimization problem, I'm just having trouble wrapping my head around how to move my method of optimization over to a pymanopt format. 

----------------------

Crude drawing of system driving on a circle, I tried formatting this into some sort of vector/matrix notation but was having some trouble figuring out how to set it up so that only a couple of the variables of the state are manipulatable and affect the other variables.

[Nicolas Boumal]

Hello Michael,

I'm sorry that I haven't taken the time to look at your message.

You mentioned that you are learning some differential geometry with an engineering background: I wrote a book on that topic for that audience which you may find helpful, see: http://nicolasboumal.net/book. Comments / suggestions for improvement most welcome.

About your questions re gradient and velocity: Manopt is for optimization, so what it cares about is: give it a set M (a manifold) and a function f : M -> R. Then, Manopt can take care of (trying to) minimize it. The gradient Manopt expects is simply the gradient of f on that manifold. It's not a physical quantity such as the velocity of the car for example: that's a different concept.

It seems that there is a strong 'optimal control' aspect to your problem. Manopt is not tailored specifically for such applications, though of course as a generic optimization toolbox it could be used for it. On the other hand, there are toolboxes designed specifically for optimal control and which may offer more user-friendly tools.

For example, there is the ACADO toolkit: https://acado.github.io/

It goes back quite a few years, and if I remember correctly it's in C++ (with a Matlab interface aswell). So it's perhaps not the right tool for you. I'm mentioning it mostly because perhaps knowing about one such toolbox may make it easier to find other ones, possible in Python. I do not recall whether they handle manifolds in there.

Best of luck,

Nicolas