I’m developing an MPC for an autonomous car. This is part of the student organization fuel fighter at NTNU. The goal is to make the most energy efficient autonomous car. We will compete in Shell Eco-Marathon this summer.
I’m using the python interface to define the MPC and then use the generated c-code together with my C++ code for ROS to run the MPC.
I want to use splines to describe the track. Since the track is not know before we start this spline needs to be updated as the system is running. I have not found a good way of doing this. I tried implementing the solution from this topic. I got it working by modifying race_cars example. I modfied line 58 in this file.
# 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. It works, however it is painfully slow. ~4000 times slower in my case… I think the issue is that instead of calculating the parameters for the spline once and the using them for the rest of the calculations, it’s calculating the parameters every time the function is called.
What I want is that if the track is updated then the I want to update the spline. Otherwise I want to use the spline from the previous calculation. I think that if it would be possible to send the spline parameters (the internal coefficients) to acados it would work. Then I would generate the interpolant function myself and pass this to acados. I could either pass the internal parameters or what would be very nice is that if I could pass a function pointer or something like that . Is this possible?
thanks for your detailed description. In my opinion, I would calculate it offline (not in acados) and pass the final coefficients as parameters in your OCP. Then you are also able to update them only whenever it makes sense for your problem.
Thanks @mss.
That makes sense. What I’m unsure about is whats the best way of passing these parameters. Since the number of coefficients are dependent on number of points I have. If I use a casadi.MX variable doesn’t that assume that the dimensions is know at compilation?
Also given that I’m able to pass the parameters to acados. How do I avoid writing my own spline code? Do you have any suggestions on how i could calculate the coefficients and also use them without doing it all from scratch? Would be nice if I could retrieve the parameters from casADi interpolant function in my code and then pass the internal coefficients to acados. Then I could give these to a casADi interpolant function in acados and use that. I have not found a way of doing this. Do you know if it’s possible or have something similar I could try?
Hi @torpedo,
I am facing the same exact problem. Did you manage to find a solution? If so, would you be so nice to tell me how you did that? I am very new to acados so any kind of help is welcome.
Thanks in advance for your time and for your patience!
Hi @jack!
No I was not able to find a good solution. I still have not had time to fully explore the possibilites, however none of the ideas I have seems like a good solution. What I ended up doing instead is using the least square method to calculate at third degree polynomial that fits a section of the track infront of the vehicle. This was accurate enough in my case, however I still have some problems with ensuring that it fits a large enough section. I was recommended to look at Lagrange polynomials, however I have not had time to explore that yet. Still the best would be to figure out how to use splines.
Sorry, that I don’t have a solution for you. Please let me know if you find a good solution!
Reviving this topic since I wonder if anyone found a nicer solution for this. I need this to update my dynamic distance function (distance from robot to nearest obstacle). This distance table/function changes over time, since obstacles are allowed to move. Any new updates?
If you follow the examples and let your BSpline coefficients be part of the parameter vector then you can update the BSpline coefficients between each solver call (i.e. online)
Based on the linked example the acados setup could use uSym as decission variables and pSym as parameters where the BSPline coefficients are contained in the parameter vector.
Hello Grunnet, I am interested in the approach you have used. I saw your example, but I am unable to replicate it in the 1D case. My idea was to calculate the knots and coefficients using MATLAB (spline function), but I am struggling to find the correct structure to pass these parameters to casadi.bspline.
I report here my code
clear all
close all
clc
import casadi.*
x = linspace(-3,3,1000);
y = x.^3;
matlab_spline = spline(x,y);
coeff = matlab_spline.coefs;
xKnots = matlab_spline.breaks;
% Casadi parametric spline
coeff_casadi = reshape(coeff',1,[]);
%order of polynomials
d=3;
%Mx symbol corresponding to the dimension of spline
vSym=MX.sym('v',1);
%Coefficient vector as an MX symbol
coeffSym=MX.sym('coeff',numel(coeff_casadi));
%Create MX graph parameterized with the spline coefficients
splineMx=casadi.bspline(vSym, coeffSym, {xKnots} , d, 1, struct());
%Create casadi function based on the Mx graph
splineFunc=casadi.Function('spline',{vSym,coeffSym},{splineMx},struct());
% Use the created spline casadi function
y_casadi=full(splineFunc(x,coeff_casadi));
figure("name","plot 3rd-order polynomial")
hold on,
plot(x,y),
plot(x,y_casadi)
legend("real","spline")
. Is this possible?
