Passing Splines to the Acados S-Function

User Questions
,
1

Hi @FreyJo :wave:,

thank you very much for your help, I appreciate!

As you suggested, I open up a new topic refering to this one https://discourse.acados.org/t/evaluate-a-piecewise-polynomial-spline/

What I’m trying to do is the following:

My path planner that uses A* for example, computes an optimal path on a grid with a certain cell size and returns a vector of waypoints. Based on those waypoints I have to generate a trajectory (for position and orientation) that can be used as a reference for my MPC Controller. In my case, this is a simple Interpolation of the waypoints using splines.

My cost on the state would look like ||x_pred(k|t) - x_ref||²_Q with k = 0,…,N+1 and
x_ref being the evaluation of my predicted state using the Spline: x_ref = spline_fct(x_pred(k|t)).
Thats what I meant in the previous post with “… to get the tracking error”.

The question I am struggling with is, where do I create the splines? Creating it outside my S-Function would require the S-Function block to have an Input for functions. Right now, the y_ref and y_ref_e port of the S-Function only takes double values if I’m not mistaken. On the other hand, I don’t really know how to create the Spline inside the S-Function.
Coming back to your first question, the cost function I used so far was of type linear least square.

I hope that makes my problem more understandable :sweat_smile:

Best,
Miri

2

Hi Miri,

I am not sure what you mean with x_pred(k|t).
On the one hand, spline_fct(x_pred(k|t)) makes me think that this is the output of your path planner (A*).
On the other hand, |x_pred(k|t) - x_ref||²_Q makes me think that these are discrete states in the multiple shooting discretization of acados.

Can you clarify that?

Best,
Jonathan

3

Hi Jonathan,

so, with x_pred(k|t) I mean the discrete states over the prediction horizon N, predicted at time t, (k = 0,…,N-1, not ‘+’ sry!).
I’m sorry for the confusion!
Anyway, I’m trying to do the following:

A* returns vectors of waypoints, lets call them x_wp and y_wp and let them be of size 10.
Those waypoints are passed to the s-functions via paramters.
Within the S-Function I want to generate a spline of those waypoints like this
spline_fct = casadi.interpolant(‘pp’, ‘bspline’, {x_wp}, y_wp);

That should make it pretty easy to evaluate my predictions.
The Dynamics of my Vehicle to be controlled includes its X and Y Position.
So, applying the predicted (!) X Position to Spline_fct results in “where my Vehicle should be driving” wheras the predicted Y Position is “where my Vehicle will actually drive”.

The reason I want to do this, is because the Path Planner updates the path after certain time steps.

However, when I do

x_wp = p(1:10); 
y_wp = p(11:20);

with the parameters p = [p1; p2] and
p1 = MX.sym(‘p1’, 10, 1); %for X
p2 = MX.sym(‘p2’, 10, 1); %for Y

followed by:
spline_fct = casadi.interpolant(‘pp’, ‘bspline’, {x_wp}, y_wp);

I get the error that this is not possible since the p variables are free…

So, the ultimate Question would be how to create such a Spline_fct that is able to update.

Best,
Miri

4

Little correction, this is the error I get:

No matching function for overload function ‘interpolant’. Possible prototypes are:
INTERPOLANT(char,char,{[double]},int,struct)
INTERPOLANT(char,char,[int],int,struct)
INTERPOLANT(char,char,{[double]},[double],struct)
INTERPOLANT(char,char,[int],[double],struct)
You have: char, char, {MX}, MX

When I do,
spline_fct = casadi.interpolant(‘pp’, ‘bspline’, {full(evalf(x_wp))}, full(evalf(y_wp)));
I get

Cannot evaluate “f:()->(o0[10]) MXFunction” since variables [p1] are free.

5

Hi @Miri!
I have a similar issue to the one you have, however I am using the python interface to generate C code. I also get discrete points that define a trajectory. I tried implementing the solution from this topic. I think it got it working by modifying race_cars example. I modfied line 58 in this file and .

# old spline, can't update after compile
kapparef_s = interpolant("kapparef_s_old", "bspline", [s0], kapparef)

# new spline, can update after compile by setting parameters p.
model.p = MX.sym("spilne_p", len(s0), 1)
interp_path = interpolant("kapparef_s_new", "bspline", [s0])
kapparef_s = Function("interp_func", [s], [interp_path(s, p)])

# specifying the parameters later for new solution:
ocp.parameter_values = np.array(kapparef)

Here s0 is the distance along the trajectory, for example [0, 0.5, 1, … , 0.5*n]. Kapparef is the curvature of the trajectory at each point.
The problem is that it is horribly slow. When I use a hard coded spline (the spline is known at compilation) it uses 6ms with SQP RTI, however with the new method it uses 4s. I think this is because it does not interpolate once and then use the result. It does perhaps call interpolant every time I call kapparef_s?
Did you find a solution that works better? Is there perhaps a better method?

best,
Tor