def solve_eqp1(H, f, A, ret_factorization=False):    
    n_vars = H.shape[0]
    assert H.shape[1] == n_vars
    assert f.shape[0] == n_vars
    assert A.shape[1] == n_vars
    n_cnts = A.shape[0]
    
    _u,_s,_vh = np.linalg.svd(A.T)
    N = _u[:,n_cnts:]
    # columns of N span the null space
    
    # x = Nz
    # then problem becomes unconstrained minimization .5 z'N'HNz + z'N'f
    # N'HNz + N'f = 0
    L = N.T.dot(H.dot(N))
    R = -N.T.dot(f)
    z = np.linalg.solve(L, R)
    x = N.dot(z)
    
    if ret_factorization:
        return x, (N, z)
    return x
solve equality-constrained qp
dataset/ETHPy150Open rll/lfd/lfd/registration/tps.py/solve_eqp1