zero velocity constraint violated

[lrchit]

Hi, I'm trying to do mpc on quadruped with full body model, where the states are generalized coordinate q and v, and the inputs are ground reaction force and joint torque. I use your cppad interface to make full body dynamics and constraints, and find something weird. The "zero velocity constraint" of supporting feet is always violated (they slide) whatever the robot is standing and troting, and sometimes it crash directly: Screencast with the constraint observer:

My constraint is defined as below: ZeroVelocityConstraintCppAd.h:

class ZeroVelocityConstraintCppAd final : public StateConstraintCppAd {
public:
  ZeroVelocityConstraintCppAd(
      const PinocchioInterface &pinocchioInterface, WholeModelInfo info,
      const SwitchedModelReferenceManager &referenceManager,
      size_t contactPointIndex, const std::string &modelName,
      const std::string &modelFolder, const bool &recompileLibraries,
      bool verbose);

  ~ZeroVelocityConstraintCppAd() override = default;
  ZeroVelocityConstraintCppAd *clone() const override {
    return new ZeroVelocityConstraintCppAd(*this);
  }

  bool isActive(scalar_t time) const override;

  size_t getNumConstraints(scalar_t time) const override { return 3; }

private:
  ZeroVelocityConstraintCppAd(const ZeroVelocityConstraintCppAd &rhs) = default;

  ad_vector_t constraintFunction(ad_scalar_t time, const ad_vector_t &state,
                                 const ad_vector_t &parameters) const override;

  PinocchioInterfaceCppAd pinocchioInterfaceCppAd_;
  WholeModelInfo info_;
  const SwitchedModelReferenceManager *referenceManagerPtr_;
  const size_t contactPointIndex_;
  const std::string endEffectorName_;
};

ZeroVelocityConstraintCppAd.cpp:

ZeroVelocityConstraintCppAd::ZeroVelocityConstraintCppAd(
    const PinocchioInterface &pinocchioInterface, WholeModelInfo info,
    const SwitchedModelReferenceManager &referenceManager,
    size_t contactPointIndex, const std::string &modelName,
    const std::string &modelFolder, const bool &recompileLibraries,
    bool verbose)
    : StateConstraintCppAd(ConstraintOrder::Linear),
      pinocchioInterfaceCppAd_(std::move(pinocchioInterface.toCppAd())),
      info_(std::move(info)), referenceManagerPtr_(&referenceManager),
      contactPointIndex_(contactPointIndex), endEffectorName_(modelName) {
  StateConstraintCppAd::initialize(info_.stateDim, 0,
                                   modelName + "_zeroVelocity", modelFolder,
                                   recompileLibraries, verbose);
}

bool ZeroVelocityConstraintCppAd::isActive(scalar_t time) const {
  return referenceManagerPtr_->getContactFlags(time)[contactPointIndex_];
}

ad_vector_t ZeroVelocityConstraintCppAd::constraintFunction(
    ad_scalar_t time, const ad_vector_t &state,
    const ad_vector_t &parameters) const {
  const auto &model = pinocchioInterfaceCppAd_.getModel();
  auto data = pinocchioInterfaceCppAd_.getData();
  const pinocchio::ReferenceFrame rf =
      pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED;
  const ad_vector_t q = state.head(info_.generalizedCoordinatesNum);
  const ad_vector_t v = state.tail(info_.generalizedCoordinatesNum);

  pinocchio::forwardKinematics(model, data, q, v);
  pinocchio::updateFramePlacements(model, data);
  const ad_vector_t velocity =
      pinocchio::getFrameVelocity(model, data,
                                  model.getBodyId(endEffectorName_), rf)
          .linear();

  const ad_vector_t constraint = velocity;

  return constraint;
}

And my dynamics is that:

ad_vector_t PinocchioWholeDynamicsAD::getValueCppAd(
    PinocchioInterfaceCppAd &pinocchioInterfaceCppAd,
    const WholeModelPinocchioMappingCppAd &mappingCppAd,
    const ad_vector_t &state, const ad_vector_t &input) {
  const auto &info = mappingCppAd.getWholeModelInfo();
  assert(info.stateDim == state.rows());
  assert(info.inputDim == input.rows());

  const auto &model = pinocchioInterfaceCppAd.getModel();
  auto &data = pinocchioInterfaceCppAd.getData();

  const ad_vector_t qPinocchio = mappingCppAd.getPinocchioJointPosition(state);
  const ad_vector_t vPinocchio =
      mappingCppAd.getPinocchioJointVelocity(state, input);
  const ad_vector_t tauPinocchio = mappingCppAd.getPinocchioTorque(input);

  updateWholeDynamics(pinocchioInterfaceCppAd, info, qPinocchio);
  pinocchio::container::aligned_vector<pinocchio::ForceTpl<ad_scalar_t>> fext(
      model.njoints, pinocchio::ForceTpl<ad_scalar_t>::Zero());
  for (size_t i = 0; i < info.numThreeDofContacts; i++) {
    const auto frameIndex = info.endEffectorFrameIndices[i];
    const auto jointIndex = model.frames[frameIndex].parent;
    const Eigen::Matrix<ad_scalar_t, 3, 1> translationJointFrameToContactFrame =
        model.frames[frameIndex].placement.translation();
    const Eigen::Matrix<ad_scalar_t, 3, 3> rotationWorldFrameToJointFrame =
        data.oMi[jointIndex].rotation().transpose();
    const Eigen::Matrix<ad_scalar_t, 3, 1> contactForce =
        rotationWorldFrameToJointFrame *
        mappingCppAd.getPinocchioContactForce(input).segment(3 * i, 3);
    fext[jointIndex].linear() = contactForce;
    fext[jointIndex].angular() =
        translationJointFrameToContactFrame.cross(contactForce);
  }
  for (size_t i = info.numThreeDofContacts;
       i < info.numThreeDofContacts + info.numSixDofContacts; i++) {
    const auto frameIndex = info.endEffectorFrameIndices[i];
    const auto jointIndex = model.frames[frameIndex].parent;
    const Eigen::Matrix<ad_scalar_t, 3, 1> translationJointFrameToContactFrame =
        model.frames[frameIndex].placement.translation();
    const Eigen::Matrix<ad_scalar_t, 3, 3> rotationWorldFrameToJointFrame =
        data.oMi[jointIndex].rotation().transpose();
    const Eigen::Matrix<ad_scalar_t, 3, 1> contactForce =
        rotationWorldFrameToJointFrame *
        mappingCppAd.getPinocchioContactForce(input).segment(
            3 * info.numThreeDofContacts + 6 * (i - info.numThreeDofContacts),
            3);
    const Eigen::Matrix<ad_scalar_t, 3, 1> contactTorque =
        rotationWorldFrameToJointFrame *
        mappingCppAd.getPinocchioContactForce(input).segment(
            3 * info.numThreeDofContacts + 6 * (i - info.numThreeDofContacts) +
                3,
            3);
    fext[jointIndex].linear() = contactForce;
    fext[jointIndex].angular() =
        translationJointFrameToContactFrame.cross(contactForce) + contactTorque;
  }

  ad_vector_t stateDerivative(info.stateDim);
  ad_vector_t floatingBaseVelocity(6);
  const Eigen::Matrix<ad_scalar_t, 3, 1> eulerAngles = qPinocchio.segment(3, 3);
  const Eigen::Matrix<ad_scalar_t, 3, 1> localLinearVelocity =
      vPinocchio.segment(0, 3);
  const Eigen::Matrix<ad_scalar_t, 3, 1> localAngularVelocity =
      vPinocchio.segment(3, 3);
  floatingBaseVelocity.head(3) =
      getRotationMatrixFromZyxEulerAngles(eulerAngles) * localLinearVelocity;
  floatingBaseVelocity.tail(3) =
      getEulerAnglesZyxDerivativesFromLocalAngularVelocity(
          eulerAngles, localAngularVelocity);
  stateDerivative.head(6) = floatingBaseVelocity;
  stateDerivative.segment(6, info.generalizedCoordinatesNum - 6) =
      vPinocchio.tail(info.generalizedCoordinatesNum - 6);
  stateDerivative.tail(info.generalizedCoordinatesNum) =
      pinocchio::aba(model, data, qPinocchio, vPinocchio, tauPinocchio, fext);

  return stateDerivative;
}

The source code that could run is here: https://github.com/lrchit/ocs2_whole_body_mpc.git. And I changed fun.optimize(); in your CppAdInterface.cpp to fun.optimize("no_compare_op"); so that the codegen of dynamics is allowed. Can anyone tell me what's wrong with it and how to fix this problem? I've also tried penalty, barrier function and augment Lagrangian, but the results are almost the same. Is there some extra things needed to do to deal with state constraint?

[FenglongSong]

Hi,

I've worked on something similar about 2 years ago. Since you are using $x = [q, \nu]$ as states, the zero end-effector velocity constraint $v_{EE}=0$ is a pure-state equality constraint. OCS2 cannot handle pure-state equality constraint in its SQP solver (in DDP solver I don't know), at least this was the case 2 years ago. You can check again if there is any update (although I doubt it).

Therefore, if you want to do whole-body NMPC using OCS2, you can formulate acceleration-level end-effector constraints (check this paper: https://arxiv.org/pdf/2203.07429). This is a state-input equality constraint and OCS2 can handle it pretty well.

You should not use penalty methods to deal with equality constraints, which is pretty bad numerically :(

Another small tip: by my experience the gradient computation takes more than 50% of the computation time in each SQP iteration. You can consider using analytic derivative in Pinocchio, which is faster.

Hope this will be helpful!

Best, Fenglong

[lrchit]

Hi,

I've worked on something similar about 2 years ago. Since you are using x=[q,ν] as states, the zero end-effector velocity constraint vEE=0 is a pure-state equality constraint. OCS2 cannot handle pure-state equality constraint in its SQP solver (in DDP solver I don't know), at least this was the case 2 years ago. You can check again if there is any update (although I doubt it).

Therefore, if you want to do whole-body NMPC using OCS2, you can formulate acceleration-level end-effector constraints (check this paper: https://arxiv.org/pdf/2203.07429). This is a state-input equality constraint and OCS2 can handle it pretty well.

You should not use penalty methods to deal with equality constraints, which is pretty bad numerically :(

Another small tip: by my experience the gradient computation takes more than 50% of the computation time in each SQP iteration. You can consider using analytic derivative in Pinocchio, which is faster.

Hope this will be helpful!

Best, Fenglong

Hi,

Thanks for your reply, it helps a lot. I will go and learn about the things you mentioned!

Best, Ruochen