Commit 47c93e3c authored by Stefan Ecklebe's avatar Stefan Ecklebe
Browse files

Fix issue with to few setpoint steps on re-init in unlimited mode

parent f4a0bc6e
......@@ -521,16 +521,22 @@ class MPController:
if self.ocp.e_func is not None:
dims["e"] = np.prod(self.ocp.e_func.sx_out()[0].shape)
# adept setpoint dimensions
x_sp = self.ocp.x_ref.T[n:n + self.N + 1]
if (xn := x_sp.shape[0]) < self.N + 1:
x_sp = np.vstack([x_sp] + [x_sp[-1]] * (self.N + 1 - xn))
u_sp = self.ocp.u_ref.T[n:n + self.N]
if (un := u_sp.shape[0]) < self.N:
u_sp = np.vstack([u_sp] + [u_sp[-1]] * (self.N + 1 - un))
# create solver object
self.nmpc_solver = mpctools.nmpc(
N=dims,
x0=x0,
f=self.ocp.sys.x_dot_fun,
Delta=self.ocp.T,
sp=dict(x=self.ocp.x_ref.T[n:n+self.N+1],
u=self.ocp.u_ref.T[n:n+self.N]),
guess=dict(x=self.guess["x"].T,
u=self.guess["u"].T),
sp=dict(x=x_sp, u=u_sp),
guess=dict(x=self.guess["x"].T, u=self.guess["u"].T),
l=self.ocp.l_func,
Pf=self.ocp.lf_func,
e=self.ocp.e_func,
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment