/** 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();
}
