Fix qpOASESWrapper when i == N.
@@ -70,6 +70,10 @@ bool qpOASESWrapper::solveStagewiseOptim(std::size_t i,
m_A.row(1) << 2 * delta, 1;
m_hA[1] = xNext[1];
m_lA[1] = -m_boundary;
+ } else {
+ m_A.topRows<2>().setZero();
+ m_lA.head<2>().setConstant(-1);
+ m_hA.head<2>().setOnes();
}
Eigen::Index cur_index = 2;
for (const Solver::LinearConstraintParams& lin : m_constraintsParams.lin)
