How to use

getTrustHeight

method

in

us.ihmc.humanoidRobotics.footstep.Footstep

private boolean areFoostepsEqual(Footstep sentFootstep, Footstep receivedFootstep, double epsilon)
{
 boolean arePosesEqual = sentFootstep.getFootstepPose().epsilonEquals(receivedFootstep.getFootstepPose(), epsilon);
 boolean sameRobotSide = sentFootstep.getRobotSide() == receivedFootstep.getRobotSide();
 boolean isTrustHeightTheSame = sentFootstep.getTrustHeight() == receivedFootstep.getTrustHeight();
 // FIXME the field is set in FootstepDataListMessage only, which makes impossible to properly convert messages to footsteps.
 boolean isAdjustableTheSame = true; //sentFootstep.getIsAdjustable() == receivedFootstep.getIsAdjustable();
 boolean sameWaypoints = sentFootstep.getCustomPositionWaypoints().size() == receivedFootstep.getCustomPositionWaypoints().size();
 if (sameWaypoints)
 {
   for (int i = 0; i < sentFootstep.getCustomPositionWaypoints().size(); i++)
   {
    FramePoint3D waypoint = sentFootstep.getCustomPositionWaypoints().get(i);
    FramePoint3D otherWaypoint = receivedFootstep.getCustomPositionWaypoints().get(i);
    sameWaypoints = sameWaypoints && waypoint.epsilonEquals(otherWaypoint, epsilon);
   }
 }
 boolean sameBlendDuration = MathTools.epsilonEquals(sentFootstep.getSwingTrajectoryBlendDuration(), receivedFootstep.getSwingTrajectoryBlendDuration(), epsilon);
 return arePosesEqual && sameRobotSide && isTrustHeightTheSame && isAdjustableTheSame && sameWaypoints && sameBlendDuration;
}

public void requestSwing(RobotSide upcomingSwingSide, Footstep footstep, double swingTime)
{
 if (!footstep.getTrustHeight())
 {
   FramePoint supportAnklePosition = new FramePoint(ankleZUpFrames.get(upcomingSwingSide.getOppositeSide()));
   supportAnklePosition.changeFrame(footstep.getParentFrame());
   double newHeight = supportAnklePosition.getZ();
   footstep.setZ(newHeight);
 }
 FootControlModule footControlModule = footControlModules.get(upcomingSwingSide);
 footControlModule.setFootstep(footstep, swingTime);
 setContactStateForSwing(upcomingSwingSide);
}