hpipm icon indicating copy to clipboard operation
hpipm copied to clipboard

Equality Constraints not working with dense qp

Open dlcfort opened this issue 10 months ago • 1 comments

The optimization that I set up does not work when I set the number of general inequality constraints "ng" to zero. However when I set "ng" to a value of 1 it works fine. Is there something wrong with my formulation or is there a bug in the code? You can see from the printed vector (Av) at the end that it does not match up with the vector (b)

from hpipm_python import *
from hpipm_python.common import *
import numpy as np
import time

nv = 8 #number of variables
ne = 8 # number of equality constraints
ng = 0 
dim = hpipm_dense_qp_dim()
dim.set('nv', nv)
dim.set('ne', ne)
dim.set('ng', ng)
H = np.array([[ 2, -2,  0,  0,  0,  0,  0,  0],
                [-2,  4, -2,  0,  0,  0,  0,  0],
                [ 0, -2,  4, -2,  0,  0,  0,  0],
                [ 0,  0, -2,  2,  0,  0,  0,  0],
                [ 0,  0,  0,  0,  2, -2,  0,  0],
                [ 0,  0,  0,  0, -2,  4, -2,  0],
                [ 0,  0,  0,  0,  0, -2,  4, -2],
                [ 0,  0,  0,  0,  0,  0, -2,  2]])
g = np.array([[0.],[0.],[0.],[0.],[0.],[0.],[0.],[0.]])
A = np.array([[ 1,  0,  0,  0,  0,  0,  0,  0],
    [ 0,  0,  0,  0,  1,  0,  0,  0],
    [ 0,  0,  0,  1,  0,  0,  0,  0],
    [ 0,  0,  0,  0,  0,  0,  0,  1],
    [-3,  3,  0,  0,  0,  0,  0,  0],
    [ 0,  0,  0,  0, -3,  3,  0,  0],
    [ 0,  0, -3,  3,  0,  0,  0,  0],
    [ 0,  0,  0,  0,  0,  0, -3,  3]])
b = np.array([[ 1. ],[ 2. ],[10. ],[12. ],[ 1.5],[ 3. ],[ 0. ],[ 1.5]])
qp = hpipm_dense_qp(dim)
qp.set('H', H)
qp.set('g', g)
qp.set('A', A)
qp.set('b', b)
qp_sol = hpipm_dense_qp_sol(dim)
mode = 'robust'
arg = hpipm_dense_qp_solver_arg(dim, mode)
arg.set('mu0', 1e4)
arg.set('iter_max', 30)
arg.set('tol_stat', 1e-4)
arg.set('tol_eq', 1e-5)
arg.set('tol_ineq', 1e-5)
arg.set('tol_comp', 1e-5)
arg.set('reg_prim', 1e-12)
solver = hpipm_dense_qp_solver(dim, arg)
optimization_start_time = time.time()
solver.solve(qp, qp_sol)
optimization_end_time = time.time()
v = qp_sol.get('v')

print("A @ v[:,None]: " , np.dot(A , v))

A @ v[:,None]: [[-6.32751741] [-9.52767775] [40.07520835] [40.07520835] [ 1.5 ] [ 3. ] [68.85408864] [72.90432914]]

dlcfort avatar Aug 22 '23 19:08 dlcfort