Ackermann constraints

[SteveMacenski]

Support maximum curvature contraint on controls so that the system is able to work with ackermann vehicles

[SteveMacenski]

Looking into this I see a few options:

• In applyControlConstraints, we could apply a constraint for curvature of the noise-generated controls
• In integrateStateVelocities, we could apply a constraint for curvature of the output paths
• In motion model predict, we could restrict the change to curvature when being propagated

Perhaps to some degree my lack of familiarity with xtensor / views, but I'm not exactly sure predict in the motion models is doing what we want with xt::view(state, xt::all(), xt::range(idx.cbegin(), idx.cend()));. It seems to me that it is just taking the current state and returning the sub-portion of the tensor that is the control effort at a given timestep.

I'm not seeing how that adds into the last iteration's velocity to get the propogated state velocity from initials that the function propagateStateVelocitiesFromInitials which calls it implies. I don't see a next_velocities += controls here, just setting equal to with next_velocities = motion_model_->predict(curr_state, state.idx);.

---

Passing by that question, the next thing to ask is where makes the most sense to apply the curvature limitation constraint to the ackermann motion model we'd like to support. Both predict and applyControlConstraints give us access to control efforts (e.g. velocities) while integrateStateVelocities gives us path poses.

Path poses are by far the easiest way to find the curvature between points in to threshold, but then "fixing" up the trajectory that violates it would be less straight forward. So I think it would be adventitious to apply this constraint as early as possible. While projecting poses would be fine, but then we'd have to break down the process into a for loop for each timestep so if we need to adjust the velocity of one timestep, recompute the poses, so it can be propagated to all future time steps.

Given that the propagation occurs in tandem with the motion model's predict and is already separated with a for loop for timestep, that makes a great deal of sense. That leads me back to predict, as it probably should. But always good to take a principled approach to come back to the obvious answer 😆

I've worked on ackermann path planners in the past so they way I typically think about them is in the limitation of curvature of paths it can generate (e.g. minimum radius circle the car can create, between 3 path points, compute the curvature of a circle that they could make up). However, if we're working in velocity-space, that's not the right way to think about it.

The 2 constraints we need are turning rate and curvature that would occur if completing that command at that state.

The first is the constraint wz, which now would be either the turning rate of the wheels or the rotation rate of the body-fixed reference frame. For simplicity / consistency, I think the body-fixed reference frame (center of rear axle) is the way to go so that it is analog to the other motion models (and we don't have to do anything special about it). That deals with the turning rate constraint with what we have already.

With that assumption that we're going to be working in body-fix coordinates rather than wheel turning rates / constraints, we should use that also in the motion model constraints we want to have set up. So, the main constraint we need to add for Ackermann is the constraint for curvature.

To be continued... ran out of time to come to conclusive thoughts on this. But I think this is a decent starting point -- we know where to do it and have half the context for constraints required and thorough enough for documentation about what wz means w.r.t. ackermann vehicles

---

On another topic, I feel like the control constraints should actually be members of the motion model, as they're constraints to the motion of the vehicle. I'd like your opinion on that from an engineering standpoint if that would be sensible for us to abstract out into the motion model class or not.

[SteveMacenski]

Back on this ackermann constraint thing

So we have linear body-fixed frame velocities and angular velocities. Perhaps I'm overthinking this, but if we think about it in body-fixed angles, when we're driving "forward" whereas forward is not straight, there is a component of both X and Y velocity. So the single-valued velocity would need to be the velocity of the wheels?

There's the classical kinematics formula v = wr maybe I'm overthinking it, but this doesn't feel quite right to me for our situation. So I did a little thought experiment to convince myself of this

Rather than trying to think of some infinitesimally small slice of a circle made up of a single timestep's linear and angular velocity and trying to use some fancy math to find its curvature -- what if I zoom out and allow those velocities to be forward simulated such that they create a perfect circle (assuming v and w are constant).

In that situation, then our rotation rate defines the time it takes to complete a circle. Knowing there are 2PI radians, the time to complete a circle is 2PI/w = time. With the linear velocity, we know now the distance it would have traveled in that time, dist = v * time. If a circle has circumference of 2PIr, then we can back out r.

Perhaps unsurprisingly to those reading this at home, the reduction is r = v / w, so I think I've convinced myself that this does apply in our situation from dimensional analysis.

So this is EZ-PZ to find the curvature to know if we have exceeded the minimum turning radius to threshold via.

The open question now is that we can find r, but how exactly do we modify our velocities / controls to restrict it? We need only keep the ratio of v / w above a value, but we can modify either v or w or both to accomplish it. What do you think?

I would think we should reduce angular velocity, but it might be wise to scale linear as well to some degree.

[SteveMacenski]

Coming back on this Monday, I think the answer is to restrict w, I don't think its sensible to reduce v.

So in summary:

• In predict, compute r using r = fabs(v) / fabs(w) based on the state velocity + control effort's value at the current timestep
• If r < Rmin set w = sgn_of_w(v / Rmin) so that its always below the required value
• return

I'm definitely not an xtensor wizard -- @artofnothingness is this something you can add to predict for AckermannMotionModel and remove the runtime exception that its not implemented?

[artofnothingness]

Perhaps to some degree my lack of familiarity with xtensor / views, but I'm not exactly sure predict in the motion models is doing what we want with xt::view(state, xt::all(), xt::range(idx.cbegin(), idx.cend()));. It seems to me that it is just taking the current state and returning the sub-portion of the tensor that is the control effort at a given timestep.

Now predict just returns controls. i.e given time step i, we predict velocity at time_step i+1 as equal to current control (at time step i).

In motion model predict, we could restrict the change to curvature when being propagated

Even though we could do this, i'm not sure that it's the right place. predict must predict velocity, given current velocity, current control.

[artofnothingness]

Passing by that question, the next thing to ask is where makes the most sense to apply the curvature limitation constraint to the ackermann motion model we'd like to support. Both predict and applyControlConstraints give us access to control efforts (e.g. velocities) while integrateStateVelocities gives us path poses.

I think we must constraint only controls, integrate works with predicted velocities, so if we change something here in controls, we must re-predict whole batch again.

[artofnothingness]

I'm definitely not an xtensor wizard -- @artofnothingness is this something you can add to predict for AckermannMotionModel and remove the runtime exception that its not implemented?

Yeah, that looks quite easy

[artofnothingness]

In general, i agree that restrict W make more sense.

[SteveMacenski]

Even though we could do this, i'm not sure that it's the right place. predict must predict velocity, given current velocity, current control.

But this is where the motion model is implemented, if we don't do it here for ackermann, we'd have to redesign the motion model API to include another location for this restriction. If predict on the MotionModel's goal is to propagate a next velocity from a control (which right now just sets equal it seems), then this seems like exactly the right place to threshold the velocity based on unachievable control constraints. We can set to equal for diff/omni because there's no technically impossible angular velocity / linear velocity based on drive train (just kinematics that are set via limits in the generation process). Since we're not using more fancy dynamic models at the moment.

e.g. (inefficiently, but illustratively)

control = xt::view(state, xt::all(), xt::range(idx.cbegin(), idx.cend()))
velocity = control
if |velocity.v| / |velocity.w| > Rmin {velocity.w = sgn(velocity.v) / Rmin} 
return velocity

I suppose we could do it here instead but then we'd have to introduce a loop over all controls to threshold, which already exists for the predict function. But that would be another possible location. We'd have to have the motion models implement applyControlConstraints so that the motion models own the max velocity constraints and able to also apply this new curvature constraint for ackermann. In fact, that really would then be the motion model and the predict function could simply be removed and the direct setting of controls to velocities could just become part of the optimizer code since it would be identical everywhere (unless we plan to do some other modeling later?). If we did want to keep the ability for fancy dynamic models in the future, then may as well keep this API.

I think both options are fine, but the first involves the least change to existing code and wouldn't involve any more looping than already exists. It would though be problematic in the future if we did the second option and more complex dynamic models were added. Then the curvature constraint would again have to be applied within motion model's predict after applying the model.

^^ actually thinking that through, if we ever wanted to support more complex dynamic models, it must be done in predict, not on generation, to account for other irregularities in previous timesteps.

I revise my view, I think it should definitely be in predict, or else it could cause subtle bugs later for more complex dynamic models and it doesn't really matter to us at all for the trivial case that we set velocity = control like we have now. It would be future proofing us

[artofnothingness]

Let's make it simpler
We have

1. Control generation
2. Control restriction (now it's apply control constraints)
3. Iterative prediction of velocities over N time_steps given initial velocities, and controls over N time_steps

We have "Model" that predicts velocities, given initial vels and controls over time window. It could be simple Naive model (predicted velocities = controls) or neural network model in future.
Given as it is, probably MotionModel should have "Model" class in itself that does it.

If we know that using given motion model we can't achieve W more than some threshold, then, why not just restrict controls over that threshold (like you propose w_coontrol = sgn_of_w(v_control / Rmin) ) ?
From design perspective, we could place this function in MotionModel, and invoke it in applyControlConstraints

[SteveMacenski]

If later we have a more involved motion model based on a neural network (as opposed to a more 'normal' dynamic model), we don't want the generated controls into the model to be already cut off of curvature. If its already driving on a curvature too small than the kinematic limits of the vehicle because its drifting in a turn, then the neutral network literally couldn't generate a trajectory in the entire state space if previously limited, so much of its actual drivable options would be removed. Remember for the Ackermann model, we're working in body-fixed coordinates as discussed above, not steering angle coordinates.

So in order to make sure that this doesn't become an issue in the future, its up to the prediction model to update the control input based on what's possible at that timestep. Right now, our naive model is just passing it through. But if we think about it slightly differently, we can think about it as a kinematics model, where for omni / differential drive robots, anything is technically possible (within velocity and acceleration limits) so passing through is an OK first-order approximation. But for ackermann models, this is where invalid controls should be cut off - because for more complex models in the future, this is also where we would expect them to be cut off too or considered if valid in context with more complex effects the NN is modeling. If we cut them off too early, then future models (if any were to be added) dealing with slippage or high-speed drifting effects could be artificially limited. At least, that's my intuition about it.

For what we have right now, I agree, it doesn't really matter.

But I think for instance we could make our predict motion model apply our drivability model. In this case, right now, that's just kinematic limits of velocity. We could, in concept, actually remove applyControlConstraints and threshold the commands in the motion model predict so its not just a naive pass through and the motion models themselves have their kinematic limits. I'm not necessarily advocating for that (unless it would be faster computationally), but that would make sense to decouple the optimizer's noised control generation from the vehicle characteristics. For something as simple as maximums, this is fine, but if we were to do something later like accelerations or jerk, that might be something worth discussing again.

Or like I think we both talk about above, adding a new API to motion model in addition to predict for applying control contraints. I think its just a matter of which is more efficient as to which we should do.

[artofnothingness]

I'm not sure what you propose instead of cutting off controls right after their random generation

Given generated randomly controls

1. use Neural network model to predict real velocities,
2. If predicted velocities outside the limits of our model then correct controls

Something like that ?

[SteveMacenski]

Yeah, I imagine that the NN in (1) would do that itself, but (2) for non-NN (or post NN logical checks that output is rational) to deal with model constraints -- which would include curvature. We could do it in apply control constraints if it was going to have significant differences in runtime, but that might introduce some issues if we ever did add NN support

[artofnothingness]

Oh ok, i see.
if i understand you correctly, i see problems with constraining controls after real velocities prediction that based on real velocities and not controls.
First - how would you change controls, knowing that real velocities after prediction go outside model constraints ?
Second problem - you need to re-predict all velocities again, starting from changed controls.

[SteveMacenski]

i see problems with constraining controls after real velocities prediction that based on real velocities and not controls.

Exactly! We need the real last timestep's solutions to consider this one's in the propagation in case a more complex model starts to do non-naive behavior.

First - how would you change controls, knowing that real velocities after prediction go outside model constraints ?

Isn't that the equations I set up in a prior comment?

Second problem - you need to re-predict all velocities again, starting from changed controls.

No, because you would be correcting that timestep, then we get the next timestep which should be based on the last time step's state, and recursively so in the future. Its only a problem if the motion model cannot make those modifications and have it be the initial condition for the next step.

[artofnothingness]

No, because you would be correcting that timestep, then we get the next timestep which should be based on the last time step's state, and recursively so in the future.

Oh, i got it

Isn't that the equations I set up in a prior comment?

control = xt::view(state, xt::all(), xt::range(idx.cbegin(), idx.cend()))
velocity = control
if |velocity.v| / |velocity.w| > Rmin {velocity.w = sgn(velocity.v) / Rmin} 
return velocity

This one? I don't see here changing controls. I see this like only predicted velocities changed
Can you please describe your vision step by step ?

I see this idea like that:

1. Apply control constraints - restrict min-max random generated controls (or move this functional somewhere else)
2. NN or Naive model (velocities = controls) real velocities prediction
3. As you said, we could restrict controls in (2) while iterative prediction, to optimize process

Obscure part for me is 3.

Imagine an example - one iteration of prediction step:
Given
current velocity = 0.2 m/s
current_control = 0.3 m/s
NN prediction of real velocity at next time step = 0.25 m/s, which is too high for our constraints (let's say not more than 0.23 m/s)
Then how should we change controls at this step to get considerable velocity?

[SteveMacenski]

I suppose I thought that predict was the last time the controls were used, but even in the back of mind I knew that was wrong. So it looks like we'd need to update both the velocities and the controls.

I think some of what's tripping me up here is that this isn't the way I had thought about this problem, and I'm not sure how what's in code now could be made to work with a dynamics/NN model (e.g. if I can't actually modify velocities/controls in predict without screwing up future timesteps, something is wrong). Let me explain then how I thought about it and probably there's good reason for doing what you did, but explains my current view. Even if its different with how you think about it, I think it will help you see what operations I think need to be done and maybe its easy for you to see how that would translate into how you think about it.

I thought about propagateStateVelocitiesFromInitials's role was to take the set initial velocity conditions, apply notional control efforts to them over timesteps, and get in output a set of velocities based on those controls from a model. That would imply that we have some velocity v at timestep i, so at timestep i+1 vi+1 = vi + ci+1, for a naive pass-through model. So the control effort is applied to a velocity using the motion model so the model has access to its current speed vi and the control that we want to have applied ci+1 at the i+1th timestep. Then a NN, dynamics, or kinematics model could decide what to do with that information, f(current velocity/state, notional control effort to apply) to result in what the model says would happen if you applied a control effort at a given state.

However, the code has the v = v + c in the generateNoisedControls method (when is odd, given the other function's name is propagate), so the "control" after this point aren't really (at least how I think about controls) controls, they're full velocities after controls are applied to them. That's removing part of what a dynamics / motion model should be providing, don't you think? Its up to a model to decide what the outcome of a control would be on a velocity, its not simply addition (when not using a naive passthrough model like we have now).

As a result of this, the input the predict (e.g. motion model) is just a velocity, but we don't know what the actual control effort was that generated it, or what the initial state was (without accessing previous timestep information). So now we're talking about having to update the "controls" that already have previous velocities already applied to it, then in turn creating an offset if we have to update anything in c for all future timesteps that will grow. We wouldn't have any offset if we updated the velocity in timestep order based on raw controls and use previous timestep's velocity as the input with the new noised control the the current timestep (e.g. vi+1 = vi + ci+1, vi+2 = vi+1 + ci+2 and so forth, no offset because input to next is just the output of last, taking into account any model changes to c).

Then all we're doing in the model for naive is adding them and passing through like we do now (half in generateNoisedControls and half in propagateStateVelocitiesFromInitials) but for dynamic models, it has the initial state, the raw controls to apply to it, and then its output is the state at that point. For the Ackermann situation, we'd have to modify the controls + update the velocities, but then the next timestep would use that modified control / velocity state and wouldn't have any additional burden because we didn't pre-propagate the velocities into the controls at the generation stage.

I know that was alot, this is something much easier to talk about in front of a white board 😆

---

So when we're talking about updating controls for ackermann, if we have vi and ci+1 as inputs and adding them for a naive Ackermann model is outside of the valid solution space, we use the equation above to find from vi what the maximum allowable control is to remain in bounds.

So analyzing 3 cases:

• We're already turning at min radius in right direction and ci+1 says go more right --> this would make an adjusted ci+1 = 0 so we continue going at vi which is already the maximum
• We're already turning at min radius in right direction and ci+1 says go more left --> this is valid and the vi+1 = vi + ci+1 holds without needing adjustment
• We're driving in some non-limit situation and we hit the limit in either direction --> we use the equations above to back out the maximum of ci+1 component we can keep

[SteveMacenski]

In summary, since this is alot and it would be good to just get everything in 1 place:

• I think a NN or dynamics model f() should have the initial state, and the control effort to apply to the state as inputs
• But right now, what we call "controls" are pre-progagated by the velocity in generateNoisedControls, rather than raw controls to be applied to the current state i in the batches
• That creates problems if we need to modify the controls in the motion model (predict) with a stateful offset for each batch

For how I think about controls and to combat it

• The motion model's predict input should be the current state (e.g. velocity at i-1), the control (e.g. ci) and the output should be the new velocity (vi)
• that way, the model can modify ci and the changes in vi can be used in the next timestep without any weird effects for all future timesteps in each batch
• I'm not sure how that would impact runtime performance, I assume that concat process in generateNoisedControls is pretty efficient

Alternatively

• Instead we could pre-threshold for ackermann models in generateNoisedControls so we can propagate in generateNoisedControls as is done now, but then "controls" aren't what I think about as "controls", but that's fine I guess if everything else works out
• However, I wonder if that might cause some issues later for NN models artificially restricting its available options. That's just speculation though.
• Though, this would require a for loop to iterate over all timesteps for the batches, which is already being done in predict, which could conceptually cause another bottleneck in generateNoisedControls for applying the ackermann constraint. But if you don't think that would be too bad, that's also an option

[artofnothingness]

I thought about propagateStateVelocitiesFromInitials's role was to take the set initial velocity conditions, apply notional control efforts to them over timesteps, and get in output a set of velocities based on those controls from a model. That would imply that we have some velocity v at timestep i, so at timestep i+1 vi+1 = vi + ci+1, for a naive pass-through model

v(i+1) = c(i)

Naive model expects that if at time step i we have velocity v(i) equal 2 m/s, given current control c(i) 2.5 m/s after our delta t we would get v(i+1) = 2.5m/s ( equal to c(i) )

However, the code has the v = v + c in the generateNoisedControls method (when is odd, given the other function's name is propagate), so the "control" after this point aren't really (at least how I think about controls) controls, they're full velocities after controls are applied to them. That's removing part of what a dynamics / motion model should be providing, don't you think? Its up to a model to decide what the outcome of a control would be on a velocity, its not simply addition (when not using a naive passthrough model like we have now).

generateNoisedControls generates random control sequences from current (from previous time_step) best control sequence.
i.e we have time_steps x control_dim control sequence and generateNoisedControls outputs batch_size x time_steps x control_dim (random generated control batch of sequences near previous best control sequence)

so the "control" after this point aren't really (at least how I think about controls) controls, they're full velocities after controls are applied to them

It's literally what will be passed to the topic /cmd_vel
and for naive model we think that robot will reach this speed at the next time step. v(i +1) = what passed to cmd_vel at i time step

i'll back to this thread a little later

[artofnothingness]

so the "control" after this point aren't really (at least how I think about controls) controls, they're full velocities after controls are applied to them

We have
control_sequence - current best control sequence - i.e values that will be passed to cmd_vel topic at 1...n time steps and give us best trajectory
State - tensor of dim batch x time_steps x 5, where 5 stands for velocity_x, velocity_wz, control_x, control_wz, dt. (changed when we use omni, but let's focus on diff drive for simplicity)

Basically at time step 0 we have robot_speed which corresponds to 0 time step velocity_x, velocity_wz
Then we generate different cmd_vel's (controls) at each time step, for each batch
Then model should predict real velocities at all time steps using initial velocity_x, velocity_wz and generated random controls (cmd_vels at time step 1....n)
So the first iteration looks like
velocity(i+1) = MODEL( v(i), cmd_vel(i) )

For naive model we have velocity(i+1) = cmd_vel(i)

I'm explaining how it works now, if we have some misunderstanding at this point

[artofnothingness]

But right now, what we call "controls" are pre-progagated by the velocity in generateNoisedControls, rather than raw controls to be applied to the current state i in the batches

in generateNoisedControls we randomly sample controls from previous best control_sequence which is not in State, in propagateStateVelocitiesFromInitials we predict real velocities based on genereatedNoisedControls and velocities at time_step 0 given from odometry.

[artofnothingness]

The motion model's predict input should be the current state (e.g. velocity at i-1), the control (e.g. ci) and the output should be the new velocity (vi)

lets say i = current moment t
To predict velocities at next time step i + 1 = t + dt we need initial velocities for current moment v_init(t) from odometry, controls c for all batches and all time_steps starting from t (i.e t, t + dt, t + 2dt ...) and it should output real velocities for time steps starting form i+1 i.e t + dt, t + 2dt, t + 3dt...

The motion model's predict input should be the current state (e.g. velocity at i-1), the control (e.g. ci) and the output should be the new velocity (vi)

This input serves a different purpose as i think.
We need semantically establish definitions for this kinds of prediction

Though, this would require a for loop to iterate over all timesteps for the batches, which is already being done in predict, which could conceptually cause another bottleneck in generateNoisedControls for applying the ackermann constraint. But if you don't think that would be too bad, that's also an option

That's a good point. We need some benchmarking/profiling

[SteveMacenski]

v(i+1) = c(i)

I guess that's not how I think about it from the aerospace perspective, its the role of the motion model to tell you what the outcome v(i+1) is if you apply control c(i) at initial state v(i). If you had a dynamics model, MODEL(), you need both the initial state v(i) and the control c(i) to decide what v(i+1) would be v(i+1) = MODEL(v(i), c(i)), since how a car reacts to speeding up by 1m/s at 10m/s is different from 100m/s (part of why people use the NN to model weird effects at high speed or on odd types of surfaces).

I understand how the code is written from that perspective, but I don't think it provides the motion model the proper inputs to be able to apply a NN or dynamics model solution. For example another MPPI implementation using a NN for dynamics: state = self._dynamics(state, u, t) (how its used is here in the actual dynamics). Its given the initial state v(i) the control c(i) and the timestep to return the state after applying the control, v(i+1), which is a translation of what I'm suggesting. Just providing it with what you want it to do (c(i)) is insufficient for a dynamics model.

tl;dr: we should be giving the predict function the 'current' velocity from the last timestep and the current control requested as inputs.

[SteveMacenski]

Separately, because the random controls are already added to the last-best velocity (https://github.com/artofnothingness/mppic/blob/develop/src/optimizer.cpp#L186-L187), I think (right?) there is a problem if at some timestep N during the trajectory's prediction phase (https://github.com/artofnothingness/mppic/blob/develop/src/optimizer.cpp#L237) we need to modify c(N) to be within limits, would that impact future timesteps N+M where M is the number of sequences left in the time window?

I suppose we could make the controls immutable in the predict, but then we'd need a prior processing step to handle motion model limitations (e.g. max acceleration between steps, ackermann curvature constraints) in the apply control constraints section of the code, but that again brings up the bit below:

Though, this would require a for loop to iterate over all timesteps for the batches, which is already being done in predict, which could conceptually cause another bottleneck in generateNoisedControls for applying the ackermann constraint. But if you don't think that would be too bad, that's also an option

That's a good point. We need some benchmarking/profiling

But perhaps its unavoidable if we want to add acceleration constraints for smoothness anyway. And we maybe do? Though I'm surprised not to see that in another implementation, so maybe there's a reason not to (or they were research focused, not production focused). Because of random independent generation, its possible that kinematic constraints are impractical, due to roll out effects just like N+M above.

So I'm starting to come around to your thinking on where we should put the ackermann constraint on control (applyControlConstraints) and keep the controls as best-last control + perturbations, not to be changed in predict, but there's the comment above on the new for loop and bottlenecks. But either way, the motion model predict needs more information than its being provided right now for dynamics, NN, or non-naive models.

tl;dr: lets add the ackermann constraint to applyControlConstraints, but that inherits a new for loop potential bottleneck. That way we don't have to deal with mutable controls. I can't seem to find any implementation of MPPI or similar that have physical constraints like this, so its up to our best judgement.

[SteveMacenski]

In works in PR #59