diff --git a/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblemImpl.cpp b/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblemImpl.cpp index 3cb4948..ccde566 100644 --- a/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblemImpl.cpp +++ b/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblemImpl.cpp @@ -519,11 +519,18 @@ void QPInverseProblemImpl::solveInverseProblem(double& objective, } } - problem.getPrimalSolution(lambda); + { + const qpOASES::returnValue primalResult = problem.getPrimalSolution(lambda); + msg_error_when(primalResult != qpOASES::SUCCESSFUL_RETURN, "QPInverseProblemImpl") << "getPrimalSolution failed"; + } + objective = problem.getObjVal(); real_t * slack = new real_t[nbVariables+nbConstraints]; // dual solution: slack[0:nV-1] => corresponds to lambda, slack[nV:nC+1] => corresponds to dual variables - problem.getDualSolution(slack); + { + const qpOASES::returnValue dualResult = problem.getDualSolution(slack); + msg_error_when(dualResult != qpOASES::SUCCESSFUL_RETURN, "QPInverseProblemImpl") << "getDualSolution failed"; + } dual.resize(nbConstraints); for (int i=0; i