qiayuanl / legged_control

Nonlinear MPC and WBC framework for legged robot based on OCS2 and ros-controls
BSD 3-Clause "New" or "Revised" License
854 stars 217 forks source link

About the gravity term in EOM task #45

Closed Qiao-JC closed 9 months ago

Qiao-JC commented 1 year ago
Task WbcBase::formulateFloatingBaseEomTask() {
  auto& data = pinocchioInterfaceMeasured_.getData();

  matrix_t s(info_.actuatedDofNum, info_.generalizedCoordinatesNum);
  s.block(0, 0, info_.actuatedDofNum, 6).setZero();
  s.block(0, 6, info_.actuatedDofNum, info_.actuatedDofNum).setIdentity();

  matrix_t a = (matrix_t(info_.generalizedCoordinatesNum, numDecisionVars_) << data.M, -j_.transpose(), -s.transpose()).finished();
  vector_t b = -data.nle;

  return {a, b, matrix_t(), vector_t()};
}

May I ask why there is no gravity term in the b vector in the EOM task?

  vector_t b = -data.nle - data.g;
qiayuanl commented 9 months ago

data.nle corresponds to concatenation of the Coriolis, centrifugal and gravitational effects.