/** is the foot in question within the switchZThreshold of the lowest foot */
  @Override
  public boolean hasFootHitGround() {
    double thisFootZ = getPointInWorld(foot.getSoleFrame()).getZ();
    double lowestFootZ = getLowestFootZInWorld();

    hitGround.set((thisFootZ - lowestFootZ) < switchZThreshold.getDoubleValue());
    soleZ.set(thisFootZ);
    ankleZ.set(getPointInWorld(foot.getFrameAfterParentJoint()).getZ());
    return hitGround.getBooleanValue();
  }
  /**
   * Quadruped version, assumes flat ground
   *
   * @param footName name for yovariable registry and yovariable names
   * @param quadrupedFeet all the feet
   * @param switchZThreshold difference in z between the lowest foot and the foot inquestion before
   *     assuming foot is in contact
   * @param totalRobotWeight
   * @param quadrant the foot in question
   * @param parentRegistry
   */
  public KinematicsBasedFootSwitch(
      String footName,
      QuadrantDependentList<? extends ContactablePlaneBody> quadrupedFeet,
      double switchZThreshold,
      double totalRobotWeight,
      RobotQuadrant quadrant,
      YoVariableRegistry parentRegistry) {
    registry = new YoVariableRegistry(footName + getClass().getSimpleName());
    foot = quadrupedFeet.get(quadrant);

    ContactablePlaneBody acrossBodyFrontFoot =
        quadrupedFeet.get(quadrant.getAcrossBodyFrontQuadrant());
    ContactablePlaneBody acrossBodyHindFoot =
        quadrupedFeet.get(quadrant.getAcrossBodyHindQuadrant());
    ContactablePlaneBody sameSideFoot = quadrupedFeet.get(quadrant.getSameSideQuadrant());
    otherFeet = new ContactablePlaneBody[] {acrossBodyFrontFoot, acrossBodyHindFoot, sameSideFoot};

    this.totalRobotWeight = totalRobotWeight;
    hitGround = new BooleanYoVariable(footName + "hitGround", registry);
    fixedOnGround = new BooleanYoVariable(footName + "fixedOnGround", registry);
    soleZ = new DoubleYoVariable(footName + "soleZ", registry);
    ankleZ = new DoubleYoVariable(footName + "ankleZ", registry);
    this.switchZThreshold = new DoubleYoVariable(footName + "footSwitchZThreshold", registry);
    this.switchZThreshold.set(switchZThreshold);

    yoResolvedCoP = new YoFramePoint2d(footName + "ResolvedCoP", "", foot.getSoleFrame(), registry);

    parentRegistry.addChild(registry);
  }
 @Override
 public boolean getForceMagnitudePastThreshhold() {
   // a more liberal version of hasFootHitGround
   double thisFootZ = getPointInWorld(foot.getSoleFrame()).getZ();
   double lowestFootZ = getLowestFootZInWorld();
   fixedOnGround.set((thisFootZ - lowestFootZ) < switchZThreshold.getDoubleValue() * 2);
   return fixedOnGround.getBooleanValue();
 }
  @Override
  public Footstep predictFootstepAfterDesiredFootstep(
      RobotSide supportLegSide, Footstep desiredFootstep) {
    RobotSide futureSwingLegSide = supportLegSide;
    ReferenceFrame futureSupportAnkleFrame = desiredFootstep.getPoseReferenceFrame();

    ReferenceFrame futureSupportAnkleZUpFrame =
        new ZUpFrame(ReferenceFrame.getWorldFrame(), futureSupportAnkleFrame, "ankleZUp");
    futureSupportAnkleZUpFrame.update();

    computeDistanceAndAngleToDestination(
        futureSupportAnkleZUpFrame, futureSwingLegSide, desiredDestination.getFramePoint2dCopy());
    Matrix3d footToWorldRotation =
        computeDesiredFootRotation(
            angleToDestination.getDoubleValue(), futureSwingLegSide, futureSupportAnkleFrame);

    FramePoint footstepPosition =
        getDesiredFootstepPositionCopy(
            futureSupportAnkleZUpFrame,
            futureSupportAnkleFrame,
            futureSwingLegSide,
            desiredDestination.getFramePoint2dCopy(),
            footToWorldRotation);
    FrameOrientation footstepOrientation = new FrameOrientation(ReferenceFrame.getWorldFrame());
    double[] yawPitchRoll = new double[3];
    RotationTools.convertMatrixToYawPitchRoll(footToWorldRotation, yawPitchRoll);
    footstepOrientation.setYawPitchRoll(yawPitchRoll);

    FramePose footstepPose = new FramePose(footstepPosition, footstepOrientation);
    footstepPose.changeFrame(ReferenceFrame.getWorldFrame());
    PoseReferenceFrame poseReferenceFrame =
        new PoseReferenceFrame("poseReferenceFrame", footstepPose);

    ContactablePlaneBody foot = contactableBodies.get(futureSwingLegSide);
    boolean trustHeight = true;
    return new Footstep(
        foot.getRigidBody(),
        futureSwingLegSide,
        foot.getSoleFrame(),
        poseReferenceFrame,
        trustHeight);
  }
  public KinematicsBasedFootSwitch(
      String footName,
      SideDependentList<? extends ContactablePlaneBody> bipedFeet,
      double switchZThreshold,
      double totalRobotWeight,
      RobotSide side,
      YoVariableRegistry parentRegistry) {
    registry = new YoVariableRegistry(footName + getClass().getSimpleName());
    foot = bipedFeet.get(side);
    ContactablePlaneBody oppositeFoot = bipedFeet.get(side.getOppositeSide());
    otherFeet = new ContactablePlaneBody[] {oppositeFoot};
    this.totalRobotWeight = totalRobotWeight;
    hitGround = new BooleanYoVariable(footName + "hitGround", registry);
    fixedOnGround = new BooleanYoVariable(footName + "fixedOnGround", registry);
    soleZ = new DoubleYoVariable(footName + "soleZ", registry);
    ankleZ = new DoubleYoVariable(footName + "ankleZ", registry);
    this.switchZThreshold = new DoubleYoVariable(footName + "footSwitchZThreshold", registry);
    this.switchZThreshold.set(switchZThreshold);

    yoResolvedCoP = new YoFramePoint2d(footName + "ResolvedCoP", "", foot.getSoleFrame(), registry);

    parentRegistry.addChild(registry);
  }
 @Override
 public ReferenceFrame getMeasurementFrame() {
   return foot.getSoleFrame();
 }