Dynamic car model

[rst-tu-dortmund]

Dynamic car model

[soldierofhell]

Hi @croesmann,

I'd like to start moving towards dynamic model with extending states with linear velocity and control with acceleration, like in kinematic model in Kong.

I'm not sure how to set proper xref in Controller::step(), because we don't have target velocity. I don't know if I should omit this term or maybe exclude it by setting small weight in Q matrix?
By default it is zeroed now:

mpc_local_planner/mpc_local_planner/include/mpc_local_planner/systems/base_robot_se2.h

Line 89 in 8a5980a

if (x.size() > 3) x.tail(x.size() - 3).setZero();

And as for discretization grid, is it enought to add dimension for velocity in /grid/xf_fixed:

mpc_local_planner/mpc_local_planner/src/controller.cpp

Line 282 in 8a5980a

std::vector<bool> xf_fixed = {true, true, true};

And just to confirm: velocity bounds could be set with StructuredOptimalControlProblem:: setStateBounds() and acceleration limits replaced with jerk limits?

mpc_local_planner/mpc_local_planner/src/controller.cpp

Line 794 in 8a5980a

_inequality_constraint->setControlDeviationBounds(ud_lb, ud_ub);

As for the setStateBounds() as I understand the input bounds have to be for all states so I wonder what to set for (x,y,theta). It would be good to have getStateBounds() and just modify velocity bounds, but there's no getter (but maybe through getNlpFunctions()?)
https://github.com/rst-tu-dortmund/control_box_rst/blob/363e0a818931f1df6a5d1564378b454e6c0e1b19/src/optimal_control/include/corbo-optimal-control/structured_ocp/structured_optimal_control_problem.h#L152

Regards,

[soldierofhell]

Also I noticed that currently xref in step() is only from goal. Shoudn't we include intermediate points of path in cost too? (smoothed?)

mpc_local_planner/mpc_local_planner/src/controller.cpp

Line 129 in 8a5980a

_dynamics->getSteadyStateFromPoseSE2(goal, xf);

mpc_local_planner/mpc_local_planner/src/controller.cpp

Line 169 in 8a5980a

corbo::StaticReference xref(xf); // currently, we only support point-to-point transitions in ros

I'd expect cost term similar to via-points

mpc_local_planner/mpc_local_planner/src/controller.cpp

Line 634 in 8a5980a

ocp->setStageCost(std::make_shared<MinTimeViaPointsCost>(via_points, position_weight, orientation_weight, via_points_ordered));

[amakarow]

Hi, you can add your dynamics smilar to this example:
https://github.com/rst-tu-dortmund/mpc_local_planner/blob/master/mpc_local_planner/include/mpc_local_planner/systems/kinematic_bicycle_model.h

Just overwrite getStateDimension() to return four. Add one simple integrator component to the state space model such that the new model inputs are the acceleration and the steering angle. Also overwrite the method twistFromControl() to return your desired twist message. If you need to access the state vector from this method, feel free to change the API and we are happy to accept this as a PR.

Regarding your second question. We cannot include intermediate points of the path yet because we don't have a timing profile for the path. It would be possible to generate reference trajectories but this is not done yet.

[soldierofhell]

Hi @amakarow,
Thanks for the answer. I feel like everything is clear about adding extended model (I'll make a PR when finish).
As for the global plan I realized that via points work similar to teb, so path following is available with MinTimeViaPointsCost, am I right? But could you clarify what you mean by "generating reference trajectories"? Typically in lane following lateral mpc one construct polynomial interpolating reference points (let's say f(x_i) = y_i) and then include f(x)-y error in cost (called cross-track error). In fact it's quite similar to via-points term in MinTimeViaPointsCost. Can we do this better?

[soldierofhell]

One more question. If I want to add to cost term that minimize change of steering angle is it possible?

[amakarow]

The viapoint feature should work similiar, but we have not tested it extensevely. The global plan from the navigation stack is just the sequence of 2.5D way points without any temporal information. How do you want to match f(x(t_i)) = y(t_i)? To do lane following MPC one must construct polynomials with respect to time for each state. Via points are different such that we associate them with the spacially (not temporally!) closest state prior to each open-loop optimization step. Regarding your second question, you can specify bounds on the control input deviation. See for example: https://github.com/rst-tu-dortmund/mpc_local_planner/blob/master/mpc_local_planner/src/controller.cpp#L752

[soldierofhell]

@amakarow, we can do it also spatially. Let's say (x^p_i, y^p_i) is our plan (in body frame). Then we fit poly f(x), such that y^p_i = f(x^p_i).
Then for a given state (x_t, y_t) our cost is the distance |f(x_t) - y_t|

Just one additional question about handling latency. In my melex I've got huge latency between control command and actuators. Let's say that this latency is \delta seconds. I think I should either (1) modify the dynamics that during first \delta seconds previous control(s) are being executed (= control is known and fixed) or (2) "manually" calculate the dynamics for \delta seconds and start optimization with "extrapolated" states. Could you advice me which one is better and doable in your framework. Thanks in advance,

[amakarow]

Many thanks for the pointer.

Regarding your latency issue there is already a compensator respectively a predictor in our control toolbox. Please, check out the following code snippet:
https://github.com/rst-tu-dortmund/control_box_rst/blob/master/src/tasks/src/task_closed_loop_control.cpp#L129-L249
In this closed-loop task the compensator is used in combination with a cpu time filter to estimate the latency. You can also use the compensator to predict with respect to manually specified delta T. This delta T can be added to the parameter list. It would be awesome if you ever implement this and then share your modification via a new PR. You can integreate this into controller.cpp. https://github.com/rst-tu-dortmund/control_box_rst/blob/master/src/systems/include/corbo-systems/one_step_predictor.h This is just the compensator you can invoke.