Foot force difference control

[Saeed-Mansouri]

Hi there,

As far as I understood, in the foot force difference control, the target and measured forces are represented in the sole frames. However, the target and surface positions of the foot are in the inertial frame. I have the following questions:

1. The vertical drift compensation term is in the inertial frame. However, the respective velocities are in the sole frames. Does it make sense?
2. In the damping term (Eq 19 of the paper), the left and right feet' vertical forces are not in the same frame. How can we sum two variables represented in the different frames?
3. Does the current foot force difference control work for the non-planar contacts?

[stephane-caron]

Duplicate of https://github.com/stephane-caron/lipm_walking_controller/discussions/72 (I answered there). This is a big bug! Thank you for pointing it out :grinning:

[Saeed-Mansouri]

Based on Multi-Contact Stabilization of a Humanoid Robot for Realizing Dynamic Contact Transitions on Non-coplanar Surfaces, I have improved Stabilizer::updateFootForceDifferenceControl() to consider non-planar contacts. I have also modified the vertical drift term to have independent compensations for each foot.

void Stabilizer::updateFootForceDifferenceControl()
{
  if(contactState_ != ContactState::DoubleSupport || inTheAir_)
  {
    dfzForceError_ = 0.;
    dfzHeightError_ = 0.;
    vdcHeightError_ = 0.;
    leftFootTask->refVelB({{0., 0., 0.}, {0., 0., 0.}});
    rightFootTask->refVelB({{0., 0., 0.}, {0., 0., 0.}});
    return;
  }
  Eigen::Matrix<double, 3, 6> J;
  J.block<3,3>(0,0) = leftFootTask->targetPose().rotation();
  J.block<3,3>(0,3) = rightFootTask->targetPose().rotation();

  Eigen::Matrix<double, 6, 1> weight;
  weight << leftFootRatio_, leftFootRatio_, leftFootRatio_, (1 - leftFootRatio_), (1 - leftFootRatio_), (1 - leftFootRatio_);
  Eigen::DiagonalMatrix<double, 6> W = weight.asDiagonal();

  Eigen::Matrix<double, 6, 3> pinvJ = W * J.transpose() * (J * W * J.transpose()).inverse();
  Eigen::Matrix<double, 6, 6> Phi = Eigen::Matrix6d::Identity() - pinvJ * J;

  Eigen::Matrix<double, 6, 1> forceError;
  forceError.block<3,1>(0,0) = leftFootTask->targetWrench().force() - leftFootTask->measuredWrench().force();
  forceError.block<3,1>(3,0) = rightFootTask->targetWrench().force() - rightFootTask->measuredWrench().force();
  Eigen::Matrix<double, 6, 1> forceErrorCtrl = Phi * forceError;

  Eigen::Matrix<double, 6, 1> posErrorCtrl;
  posErrorCtrl.block<3,1>(0,0) = (leftFootTask->surfacePose() * leftFootTask->targetPose().inv()).translation();
  posErrorCtrl.block<3,1>(3,0) = (rightFootTask->surfacePose() * rightFootTask->targetPose().inv()).translation();

  sva::MotionVecd velLeft = {{0., 0., 0.}, {0., 0., - dfzAdmittance_ * forceErrorCtrl[2] - vdcFrequency_ * posErrorCtrl[2]}};
  sva::MotionVecd velRight = {{0., 0., 0.}, {0., 0., - dfzAdmittance_ * forceErrorCtrl[5] - vdcFrequency_ * posErrorCtrl[5]}};

  leftFootTask->refVelB(velLeft);
  rightFootTask->refVelB(velRight);
}