leggedrobotics / ocs2

Optimal Control for Switched Systems
https://leggedrobotics.github.io/ocs2
BSD 3-Clause "New" or "Revised" License
836 stars 221 forks source link

How to get the derivatives of the state variables calculated in the systemFlowMap() function? #84

Open chenhuiYu00 opened 10 months ago

chenhuiYu00 commented 10 months ago

In function:

ad_vector_t LeggedBalanceSystemDynamics::systemFlowMap(ad_scalar_t /*time*/, const ad_vector_t& state, const ad_vector_t& input,
                                                       const ad_vector_t& parameters) const {
  // todo: add linear equation
  ad_scalar_t theta_l = state(1), theta_r = state(2), theta = state(3), psi = state(4), x_dot = state(5), theta_l_dot = state(6),
              theta_r_dot = state(7), theta_dot = state(8), psi_dot = state(9);

  ad_matrix_t A = generateA(parameters(0), parameters(1)), B = generateB(parameters(0), parameters(1));

  ad_vector_t result(10);
  result = A * state + B * input;

  return result;
}

I want to use the value of result in other places of the program, How can I do it?