How to use

getCustomPositionWaypoints

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 static FootstepDataMessage createFootstepDataMessage(Footstep footstep)
{
 FootstepDataMessage message = new FootstepDataMessage();
 message.setRobotSide(footstep.getRobotSide().toByte());
 FramePoint3D location = new FramePoint3D();
 FrameQuaternion orientation = new FrameQuaternion();
 footstep.getPose(location, orientation);
 footstep.getFootstepPose().checkReferenceFrameMatch(ReferenceFrame.getWorldFrame());
 message.getLocation().set(location);
 message.getOrientation().set(orientation);
 packPredictedContactPoints(footstep.getPredictedContactPoints(), message);
 message.setTrajectoryType(footstep.getTrajectoryType().toByte());
 message.setSwingHeight(footstep.getSwingHeight());
 message.setSwingTrajectoryBlendDuration(footstep.getSwingTrajectoryBlendDuration());
 if (footstep.getCustomPositionWaypoints().size() != 0)
 {
   for (int i = 0; i < footstep.getCustomPositionWaypoints().size(); i++)
   {
    FramePoint3D framePoint = footstep.getCustomPositionWaypoints().get(i);
    framePoint.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame());
    message.getCustomPositionWaypoints().add().set(framePoint);
   }
 }
 return message;
}