fix doc and add resize

Author: mayataka

ocs2_ipm/include/ocs2_ipm/IpmSolver.h

@@ -160,7 +160,8 @@ class IpmSolver : public SolverBase {
   /** Constructs the primal solution based on the optimized state and input trajectories */
   PrimalSolution toPrimalSolution(const std::vector<AnnotatedTime>& time, vector_array_t&& x, vector_array_t&& u);
 
-  /** Decides on the step to take and overrides given trajectories {x(t), u(t)} <- {x(t) + a*dx(t), u(t) + a*du(t)} */
+  /** Decides on the step to take and overrides given trajectories {x(t), u(t), slackStateIneq(t), slackStateInputIneq(t)}
+   * <- {x(t) + a*dx(t), u(t) + a*du(t), slackStateIneq(t) + a*dslackStateIneq(t), slackStateInputIneq(t) + a*dslackStateInputIneq(t)} */
   ipm::StepInfo takePrimalStep(const PerformanceIndex& baseline, const std::vector<AnnotatedTime>& timeDiscretization,
                                const vector_t& initState, const OcpSubproblemSolution& subproblemSolution, vector_array_t& x,
                                vector_array_t& u, scalar_t barrierParam, vector_array_t& slackStateIneq,

ocs2_ipm/src/IpmSolver.cpp

@@ -542,6 +542,8 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated
               ipm::initializeSlackVariable(result.ineqConstraints.f, settings_.initialSlackLowerBound, settings_.initialSlackMarginRate);
           dualStateIneq[i] = ipm::initializeDualVariable(slackStateIneq[i], barrierParam, settings_.initialDualLowerBound,
                                                          settings_.initialDualMarginRate);
+          slackStateInputIneq[i].resize(0);
+          dualStateInputIneq[i].resize(0);
         }
         performance[workerId] += ipm::computePerformanceIndex(result, barrierParam, slackStateIneq[i]);
         dynamics_[i] = std::move(result.dynamics);