Equality Constraints in Hierarchical Solver

[nilslandin-png]

Hi!

I'm trying to use the hierarchical solver of DAQP with 9 simple bounds on the variables and 3 equality constraints of the type Ax=b. Only the 3 equality constraints are hierarchical. I'm encountering infeasibility as soon as any of the hierarchical equality constraints cannot be fulfilled. Then I have a few questions:

1. The break point array should then look like {9, 10, 11, 12} if the rows in Ax=b is arranged in falling order of hierarchy?
2. Are equality constraints supported in the HIQP-solver? How should I specify them? Sense=5? ub=lb?
3. In single precision arithmetics, are there any specific solver parameters I need to be careful with? I'm using
   #define DAQP_DEFAULT_PRIM_TOL 1e-4
   #define DAQP_DEFAULT_DUAL_TOL 1e-5
   #define DAQP_DEFAULT_ZERO_TOL 1e-5
   #define DAQP_DEFAULT_PROG_TOL 1e-5
   #define DAQP_DEFAULT_LP_PROG_TOL 1e-5
   #define DAQP_DEFAULT_PIVOT_TOL 1e-4
   #define DAQP_DEFAULT_CYCLE_TOL 12
   #define DAQP_DEFAULT_ETA 1e-3
   //#define DAQP_DEFAULT_ETA 0
   #define DAQP_DEFAULT_ITER_LIMIT 10000
   #define DAQP_DEFAULT_RHO_SOFT 1e-3
   #define DAQP_DEFAULT_REL_SUBOPT 0
   #define DAQP_DEFAULT_ABS_SUBOPT 0
   #define DAQP_DEFAULT_SING_TOL (4e-4)
   #define DAQP_DEFAULT_REFACTOR_TOL 1e-4
   #define DAQP_DEFAULT_EPS_PROX 3e-3

[darnstrom]

Hi!

1. Breakpoints look ok
2. Equality constraints should be supported. Both sense=5 or and/or ub=lb should work.
3. I haven't experimented a lot with the hierarchical solver, so I don't have clear guidelines. But overall, the values seems reasonable.

Do you have an example of a problem in which you experience the infeasibility? If you could provide one I could help debugging things!

[darnstrom]

Actually for the breakpoints, you might want to use {8,9,10,11}. It should include the indices (0-indexed) where each hierarchy level ends. The constraints in the first level is seen as hard.
With you current breakpoints, the first equality constraint will be in the first hierarchy level, and hence be hard.

Edit: sorry I confused myself. Your breakpoints are probably correct.

[nilslandin-png]

Thanks for looking at it. I’ll provide example data as soon as I can.

But actually, I thought your stricken comment made sense. Violating what I intend to be 2nd hierarchical equality constraint seems to work - it’s when I force violation of the first equality constraint that i run into infeasibility. Edit: Or actually, the first hierarchical constraint does not seem to be allowed to induce violation in the second hierarchical constraint.

Could it be true that the first hierarchical constraint is actually treated as a hard constraint and that I should treat the first 9 (simple) constraints as level 1 priority constraints?

[darnstrom]

The breakpoints {9,10,11,12} leads to the following priority levels:

• Constraints 1-9 are interpreted as being hard constraints
• Constraint 10 has the second highest priority
• Constraint 11 has the third highest priority
• Constraint 12 has the fourth highest priority

I think this will be easier if there is a concrete problem which leads to an unexpected infeasibility, so if you can provide one it would be great!

[nilslandin-png]

The example data is provided in the appended text files

problemDefinition.txt

s-function definition.txt

. I should say that I suspect the simulink-integration to be the culprit at this point. If you could peek at the s-function as well, that would be highly appreciated! Obviously nothing thorough.

[darnstrom]

Hi again,
Thank you for provide the problem data!
You problem trivially has the origin as a solution, which I get when I solve it with daqp (below is Python code)

import numpy as np
import daqp

Q = np.eye(9, dtype=np.float64)
q = np.zeros(9, dtype=np.float64)

A = np.array([
    [ 1.0, -1.0,  0.0, -5.0, -5.0,  0.0,  0.0,  0.0,  0.0],
    [-1.0,  1.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0],
    [-0.5, -0.5,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0]
], dtype=np.float64)

# Lower and Upper Bounds (12x1)
lb = np.array([0, 0, 0, -1.8, -1.8, -1.8, -1.8, -1.8, -1.8, 0, 0, 0], dtype=np.float64)
ub = np.array([4.0, 4.0, 4.0, 1.8, 1.8, 1.8, 1.8, 1.8, 1.8, 0, 0, 0], dtype=np.float64)

# Constraint senses: 0 for standard inequality, 5 for HQP soft constraints
sense = np.array([0, 0, 0, 0, 0, 0, 0, 0, 0, 5, 5, 5], dtype=np.int32)

break_points = np.array([9, 10, 11, 12], dtype=np.int32)

d = daqp.Model()
d.setup(Q, q, A, ub, lb, sense, break_points=break_points)

x, fval, exitflag, info = d.solve()

print(f"Solver Exit Flag: {exitflag} (1 = Success / Optimal)")
if exitflag > 0:
    print("\nOptimal Decision Vector (x):")
    for i, val in enumerate(x):
        print(f"  x[{i}]: {val:8.4f}")
    
    print(f"\nObjective Value: {fval:.4f}")
    
    # Verify the hierarchical constraints (A * x)
    Ax = A @ x
    print("\nHierarchical Constraints (A * x) vs Bounds [lb, ub]:")
    for i in range(3):
        print(f"  Level {i+1}: lb={lb[9+i]} <= Ax={Ax[i]:.4f} <= ub={ub[9+i]}")
else:
    print("Solver failed to find a feasible hierarchical solution.")

I suppose the problem is, as you mention, the Simulink interface. I have never tried the hierarchical interface in Simulink, and I wouldn't expect it to work (I am not even sure if it would respect the hierarchy at all.) Unfortunately I don't have a MATLAB license anymore, so I cannot really debug things myself for the Simulink interface to add support for HQPs...

[nilslandin-png]

Thank you for trying out the problem. Hah, yeah, you're right, of course. The right hand side of the three last constraints should be varying. I get a feasible solution for [200; 0; 0] but infeasible solution for [300; 0; 0], Instead of infeasibility I expected the first right hand side to produce non-zero elements at (1-based) indices 4 and 5 and for the second right hand side to produce non-zero elements at indices 1, 2, 4 and 5 (and the second and third equality to be violated).

Anyway, I'm using the workspace approach here. Can you forsee any obvious reasons why that wouldn't work?

[darnstrom]

Ok! Both [200;0;0] and [300;0;0] works fine for me in the example code.
One thing when using the workspace approach: you probably also need to add DAQP_UPDATE_hierarchy to the ùpdate_mask in case you haven't done that yet.

[nilslandin-png]

Thanks for the suggestion! I actually hadn't done that. This changed the outcome to "initial working set overdetermined", which I think might be a step in the right direction!

I know it's a lot to ask but is it possible for you to check your solution again while changing the datatype to using float32 and with the parameters I supplied in the first post? If not I'll try to do that a bit later!

[darnstrom]

I agree, that is progress!

Another thing: I think these lines in the S-function should be removed:

        // Reuse previous working set
        if (WARM_START == 1)
        {
            daqp_activate_constraints(work);
        }

This type of activation is already happening inside the daqp_update_ldpin newer version of daqp, so this might lead to "double activation" of constraint, which might yield an overdetermined active set.

[nilslandin-png]

Thanks a bunch for thinking about this! I commented out the lines you suggested. The full s-function is appended. I still don't get the expected result unfortunately.

1. I get the exit flag "soft optimal" for only zeros in the right hand side that we spoke of earlier. Also, a suboptimal solution seems to be found as elements 4 and 5 in the solution vector take on maximum and minimum values respectively, negating each others contribution the the first equality constraint. The equality constraint is fulfilled but not with the expected trivial solution. The Hessian is well conditioned and diagonal (with only 1's).
2. I can get an undocumented(?) exit flag = 3 when the first element in the 3-vector rhs is 50.

Sfunc_202607121124.txt

[darnstrom]

1. Is this for the "first" solve?. It is strange that this happens, because it does not for the Python example, and I am not sure what would be different compared with a "cold" solve in the Simulink interface. Maybe the single precision could play a role, but I doubt it.
2. This is expected. exitflag=3 signifies that all the degrees of freedom was "used up", which can happen for the hierarchical solver. Since the hierarchical solver is still a bit experimental I have not added this in the documentation yet (and in the end, I might report exitflag = 1, instead of exitflag =3 in this scenario).

[nilslandin-png]

Understood (exit flag = 3). Are there other values that the flag can take? I'm using https://darnstrom.github.io/daqp/parameters/ as a reference.

You mean "first" solve inside the hierarchical solver? I'm not sure. Can I see this in the interface somehow?

1. Replacing the equality constraint with tight inequality constraints made the solutions slightly more... predictable? However, now only the highest priority equality constraint seems to be fulfilled.
2. I can now get exit flag = 4294967290. I'm logging the exit flag as an int32 - this shouldn't cause any issues? Memory issues with my implementation or is it somehow int32 overflow?
3. One thing that I think I haven't mentioned is that in hierarchical.c changed the hard coded value next to "// For weakly active" to 1e-6 for adaption to single precision.

I'm also wondering if there's a more efficient way of going about this. Could we perhaps do some of this communication via email/chat at your convenience? My email is firstname.lastname@gmail.com.

If you prefer to keep it documented here that is fine too! I just thought it could be made a little more efficient.

[darnstrom]

Weird with the exitflag = 4294967290, this should not be possible unless it is somehow "uninitialized" somehow. Is there a reason why you need to log it as an int32 and not as a normal int?

I looked into the S-function more, and found some possible errors:

• `memset(work, sizeof(DAQPWorkspace), 0) cleared zero bytes. It must be memset(work, 0,
  sizeof(DAQPWorkspace)).
• `memcpy(..., sizeof(work->sense)) copied only the size of a pointer (typically 8 bytes) instead of all 12 senses.
• qp->nh was set to DAQP_NBP - 1, meaning only three of the four breakpoints were used. The last hierarchical constraint might therefore be omitted.
• Active-set arrays were sized as n + 1. Hierarchical DAQP needs n + ns_max + 1; here that is 9 + 9 + 1 = 19. The old arrays could therefore be overrun. (The packed LDL array consequently needed 190 elements rather than 55.)
• A single global initDone flag allowed terminating one solver instance to disable every instance.

I tried to fix these drawbacks in this version: Sfunc_202607121124.txt

[nilslandin-png]

Thank you so much for looking at the s-function. I’ve had a suspicion I’ve had memory issues throughout testing this. I’ll try this out tomorrow and get back to you!

Edit: and that makes a lot of sense! That’s why the third hierarchical constraint always seemed to be fulfilled and why the first one never induced violations in the third!