OCS2::legged_robot::FrictionConeConstraint摩擦锥约束

公式：

\(h(F) = u(F_z + F_{gripper}) - \sqrt{F^2_x + F^2_y + \epsilon} >= 0\)

• \(F_{gripper}\)：抓力（如果有）
• \(\epsilon\)：正则化参数，用于避免梯度计算时分母为0

vector_t FrictionConeConstraint::getValue(scalar_t time, const vector_t& state, const vector_t& input,
                                          const PreComputation& preComp) const 
{
  //从input中找出力
  const auto forcesInWorldFrame = centroidal_model::getContactForces(input, contactPointIndex_, info_);
  //转换坐标
  const vector3_t localForce = t_R_w * forcesInWorldFrame;
  //返回h(F)的值
  return coneConstraint(localForce);
}

VectorFunctionLinearApproximation FrictionConeConstraint::getLinearApproximation(scalar_t time, const vector_t& state,
                                                                                 const vector_t& input,
                                                                                 const PreComputation& preComp) const {
  const vector3_t forcesInWorldFrame = centroidal_model::getContactForces(input, contactPointIndex_, info_);
  const vector3_t localForce = t_R_w * forcesInWorldFrame;

  //F_local对u求导，F_local = T * u,求导后是T
  const auto localForceDerivatives = computeLocalForceDerivatives(forcesInWorldFrame);
  //h(F)对F_local求导
  const auto coneLocalDerivatives = computeConeLocalDerivatives(localForce);
  //h(F)对u求导
  const auto coneDerivatives = computeConeConstraintDerivatives(coneLocalDerivatives, localForceDerivatives);
  
  VectorFunctionLinearApproximation linearApproximation;
  //h(F)的值和导数
  linearApproximation.f = coneConstraint(localForce);
  linearApproximation.dfdx = matrix_t::Zero(1, state.size());
  linearApproximation.dfdu = frictionConeInputDerivative(input.size(), coneDerivatives);
  return linearApproximation;
}

df/du

FrictionConeConstraint::LocalForceDerivatives FrictionConeConstraint::computeLocalForceDerivatives(
    const vector3_t& forcesInWorldFrame) const {
  LocalForceDerivatives localForceDerivatives{};
  localForceDerivatives.dF_du = t_R_w;
  return localForceDerivatives;
}

dh/df, dh/dff

FrictionConeConstraint::ConeLocalDerivatives FrictionConeConstraint::computeConeLocalDerivatives(const vector3_t& localForces) const {
  const auto F_x_square = localForces.x() * localForces.x();
  const auto F_y_square = localForces.y() * localForces.y();
  const auto F_tangent_square = F_x_square + F_y_square + config_.regularization;
  const auto F_tangent_norm = sqrt(F_tangent_square);
  const auto F_tangent_square_pow32 = F_tangent_norm * F_tangent_square;  // = F_tangent_square ^ (3/2)

  ConeLocalDerivatives coneDerivatives{};
  //对h(F) = u * (Fz + gripperForce) - sqrt(Fx * Fx + Fy * Fy + regularization)求一阶偏导
  coneDerivatives.dCone_dF(0) = -localForces.x() / F_tangent_norm;// dh/dx
  coneDerivatives.dCone_dF(1) = -localForces.y() / F_tangent_norm;// dh/dy
  coneDerivatives.dCone_dF(2) = config_.frictionCoefficient;// dh/du

  //二阶偏导
  coneDerivatives.d2Cone_dF2(0, 0) = -(F_y_square + config_.regularization) / F_tangent_square_pow32;
  coneDerivatives.d2Cone_dF2(0, 1) = localForces.x() * localForces.y() / F_tangent_square_pow32;
  coneDerivatives.d2Cone_dF2(0, 2) = 0.0;
  coneDerivatives.d2Cone_dF2(1, 0) = coneDerivatives.d2Cone_dF2(0, 1);
  coneDerivatives.d2Cone_dF2(1, 1) = -(F_x_square + config_.regularization) / F_tangent_square_pow32;
  coneDerivatives.d2Cone_dF2(1, 2) = 0.0;
  coneDerivatives.d2Cone_dF2(2, 0) = 0.0;
  coneDerivatives.d2Cone_dF2(2, 1) = 0.0;
  coneDerivatives.d2Cone_dF2(2, 2) = 0.0;

  return coneDerivatives;
}

H(F)计算

\(h(F) = u(F_z + F_{gripper}) - \sqrt{F^2_x + F^2_y + \epsilon} >= 0\)

vector_t FrictionConeConstraint::coneConstraint(const vector3_t& localForces) const {
  const auto F_tangent_square = localForces.x() * localForces.x() + localForces.y() * localForces.y() + config_.regularization;
  const auto F_tangent_norm = sqrt(F_tangent_square);
  const scalar_t coneConstraint = config_.frictionCoefficient * (localForces.z() + config_.gripperForce) - F_tangent_norm;
  return (vector_t(1) << coneConstraint).finished();
}

dh/du, dh/duu

FrictionConeConstraint::ConeDerivatives FrictionConeConstraint::computeConeConstraintDerivatives(
    const ConeLocalDerivatives& coneLocalDerivatives, const LocalForceDerivatives& localForceDerivatives) const {
  ConeDerivatives coneDerivatives;
  // First order derivatives
  coneDerivatives.dCone_du.noalias() = coneLocalDerivatives.dCone_dF.transpose() * localForceDerivatives.dF_du;

  // Second order derivatives
  coneDerivatives.d2Cone_du2.noalias() =
      localForceDerivatives.dF_du.transpose() * coneLocalDerivatives.d2Cone_dF2 * localForceDerivatives.dF_du;

  return coneDerivatives;
}