Issue when the constant term in the dynamics equation is involved

[shbang91]

Hello,

Thanks for maintaining this nice repo!
While using the HPIPM solver for my QP formulation, I encountered an issue related to the contact term (symbol b in the HPIPM paper) in the dynamics equation. In order to replicate my issue for a simple case, here, I use the 1D double integrator. The first scenario is that I commanded the double integrator to go to x_goal = (x, x_dot) = (-5, 0) from x_initial = (x, x_dot) = (5, 0) with only the cost and dynamics. As expected, it succeeded in stabilizing the double integrator with the HPIPM QP solver as shown below:

However, when adding the gravity term with the double integrator and trying to stabilize it, it diverges around the final node like below:

Here, I just added the gravity term with -g * dt as a constant term (b) in the dynamics equation (x_{k+1} = A x_k + B u_k + b) and commanded it to stay at the initial state. Also, the plots represent the solution trajectories (x and u) of the optimal control problem over 100 nodes with the dt = 0.01. As I played with the other higher dimensional examples, I noticed that only when I introduced some constant terms in the dynamics equation the HPIPM solver gives the diverging solution around the final node.
Have you ever seen this issue or do you know how to resolve this issue?

Thank you.

[shbang91]

The above example uses the quadratic cost both in the states and inputs, where I didn't set the reference for the input. From the solver's perspective, this intends to regularize the input to zero, which I think drives the input solution to zero around the final node. In another test I performed, when I set the reference for the input as mass * gravitational acceleration, it can properly stabilize the double integrator. Clearly, from my experience, the HPIPM solver needs the reference for both the states and inputs. Do you have any comments on this?

Thank you.

[giaf]

It's difficult to say without seeing the problem formulation you are using.
I assume that the plots above are the solution of a single QP, and not the results of a closed-loop simulation.

Anyway, looking at the second set of plots, the final "divergence" in the states in the final nodes is numerically very small, e.g. the first state is still very close to 5.0.
I don't know if you have it in your formulation, but this may be due to the lack of a proper terminal cost term, summarizing the cost from the end of the prediction horizon to infinity.
If this term is not there or not correct, the optimal solution minimizing the overall cost (as you said quadratic on both inputs and states) may be to accept a small deviation on the state from reference if this gives a larger decrease of the cost of the deviation of the input form its reference (that is zero as you reported).

[shbang91]

Sorry for not being able to state the detail of my problem formulation.

I am using the following formulation.

As you can see at the final node where i = N-1, I do have a quadratic terminal state cost. Even if I crank up the terminal cost term by 1000 or so, the states of both the position and velocity tend to deviate around the final node. If you look at the second set of plots, the second state (velocity) dropped a lot (reference is 0). Also, although there is no terminal cost of the input, the input still tries to go Zero if I don't give any reference. Do you have any thoughts on that?

Thanks.

[giaf]

Is the amplitude of the states deviation around the final node affected by how much you crank up the terminal cost, right?

In theory to avoid such effect you need the correct quadratic terminal state cost (both the quadratic and the linear terms), that in your case of a time-invariant OCP QP without inequality constraints can be computed using the Riccati equations (both the one computing the quadratic term and the one computing the linear term, this last is also necessary since you have a constant term b in the dynamics).

In practice in a closed loop experiment the terminal state cost as negligible influence as long as the control horizon is long enough, since only the first control is applied before the problem is solved all over again for a new initial state.