Merged in fix/modelData_dynamicsBias (pull request #683)

bugfix

Author: farbod-farshidian

ocs2_oc/src/approximate_model/LinearQuadraticApproximator.cpp

@@ -47,11 +47,11 @@ void approximateIntermediateLQ(OptimalControlProblem& problem, const scalar_t ti
   modelData.time = time;
   modelData.stateDim = state.rows();
   modelData.inputDim = input.rows();
-  modelData.dynamicsBias.setZero(state.rows());
 
   // Dynamics
   modelData.dynamicsCovariance = problem.dynamicsPtr->dynamicsCovariance(time, state, input);
   modelData.dynamics = problem.dynamicsPtr->linearApproximation(time, state, input, preComputation);
+  modelData.dynamicsBias.setZero(modelData.dynamics.dfdx.rows());
 
   // Cost
   modelData.cost = ocs2::approximateCost(problem, time, state, input);
@@ -95,10 +95,10 @@ void approximatePreJumpLQ(OptimalControlProblem& problem, const scalar_t& time,
   modelData.time = time;
   modelData.stateDim = state.rows();
   modelData.inputDim = 0;
-  modelData.dynamicsBias.setZero(state.rows());
 
   // Jump map
   modelData.dynamics = problem.dynamicsPtr->jumpMapLinearApproximation(time, state, preComputation);
+  modelData.dynamicsBias.setZero(modelData.dynamics.dfdx.rows());
 
   // Pre-jump cost
   modelData.cost = approximateEventCost(problem, time, state);