How to use

setPredictedContactPoints

method

in

us.ihmc.humanoidRobotics.footstep.Footstep

public Footstep(RobotSide robotSide, FramePose3D footstepPose, boolean trustHeight, boolean isAdjustable, List<Point2D> predictedContactPoints,
        TrajectoryType trajectoryType, double swingHeight)
{
 this.robotSide = robotSide;
 this.trustHeight = trustHeight;
 this.isAdjustable = isAdjustable;
 this.footstepPose.setIncludingFrame(footstepPose);
 setPredictedContactPoints(predictedContactPoints);
 this.trajectoryType = trajectoryType;
 this.swingHeight = swingHeight;
}

@Override
public void receivedPacket(FootstepDataListMessage packet)
{
  boolean adjustable = packet.getAreFootstepsAdjustable();
  List<FootstepDataMessage> footstepDataList = packet.getFootstepDataList();
  for (int i = 0; i < footstepDataList.size(); i++)
  {
   FootstepDataMessage footstepData = footstepDataList.get(i);
   FramePose3D footstepPose = new FramePose3D(ReferenceFrame.getWorldFrame(), footstepData.getLocation(), footstepData.getOrientation());
   Footstep footstep = new Footstep(robotSide, footstepPose, true, adjustable);
   footstep.setPredictedContactPoints(HumanoidMessageTools.unpackPredictedContactPoints(footstepData));
        footstep.setTrajectoryType(TrajectoryType.fromByte(footstepData.getTrajectoryType()));
   footstep.setSwingHeight(footstepData.getSwingHeight());
   reconstructedFootstepPath.add(footstep);
  }
}

@Override
public Footstep.FootstepType snapFootstep(Footstep footstep, HeightMapWithPoints heightMap)
{
 // can only snap footsteps in world frame
 footstep.getFootstepPose().checkReferenceFrameMatch(ReferenceFrame.getWorldFrame());
 FootstepDataMessage originalFootstep = HumanoidMessageTools.createFootstepDataMessage(footstep);
 //set to the sole pose
 FramePoint3D position = new FramePoint3D();
 FrameQuaternion orientation = new FrameQuaternion();
 footstep.getPose(position, orientation);
 originalFootstep.getLocation().set(position);
 originalFootstep.getOrientation().set(orientation);
 //get the footstep
 Footstep.FootstepType type = snapFootstep(originalFootstep, heightMap);
 if (type == Footstep.FootstepType.FULL_FOOTSTEP && originalFootstep.getPredictedContactPoints2d().size() > 0){
   throw new RuntimeException(this.getClass().getSimpleName() + "Full Footstep should have null contact points");
 }
 footstep.setPredictedContactPoints(HumanoidMessageTools.unpackPredictedContactPoints(originalFootstep));
 footstep.setFootstepType(type);
 FramePose3D solePoseInWorld = new FramePose3D(ReferenceFrame.getWorldFrame(), originalFootstep.getLocation(), originalFootstep.getOrientation());
 footstep.setPose(solePoseInWorld);
 footstep.setSwingHeight(originalFootstep.getSwingHeight());
 footstep.setTrajectoryType(TrajectoryType.fromByte(originalFootstep.getTrajectoryType()));
 return type;
}

@Override
public Footstep.FootstepType snapFootstep(Footstep footstep, HeightMapWithPoints heightMap)
{
 // can only snap footsteps in world frame
 footstep.getFootstepPose().checkReferenceFrameMatch(ReferenceFrame.getWorldFrame());
 FootstepDataMessage originalFootstep = HumanoidMessageTools.createFootstepDataMessage(footstep);
 //set to the sole pose
 FramePoint3D position = new FramePoint3D();
 FrameQuaternion orientation = new FrameQuaternion();
 footstep.getPose(position, orientation);
 originalFootstep.getLocation().set(position);
 originalFootstep.getOrientation().set(orientation);
 //get the footstep
 Footstep.FootstepType type = snapFootstep(originalFootstep, heightMap);
 if (type == Footstep.FootstepType.FULL_FOOTSTEP && originalFootstep.getPredictedContactPoints2d().size() > 0 )
 {
   throw new RuntimeException(this.getClass().getSimpleName() + "Full Footstep should have null contact points");
 }
 footstep.setPredictedContactPoints(HumanoidMessageTools.unpackPredictedContactPoints(originalFootstep));
 footstep.setFootstepType(type);
 FramePose3D solePoseInWorld = new FramePose3D(ReferenceFrame.getWorldFrame(), originalFootstep.getLocation(), originalFootstep.getOrientation());
 footstep.setPose(solePoseInWorld);
 footstep.setSwingHeight(originalFootstep.getSwingHeight());
 footstep.setTrajectoryType(TrajectoryType.fromByte(originalFootstep.getTrajectoryType()));
 return type;
}

@Override
public Footstep.FootstepType snapFootstep(Footstep footstep, HeightMapWithPoints heightMap)
{
 // can only snap footsteps in world frame
 footstep.getFootstepPose().checkReferenceFrameMatch(ReferenceFrame.getWorldFrame());
 FootstepDataMessage originalFootstep = HumanoidMessageTools.createFootstepDataMessage(footstep);
 //set to the sole pose
 FramePoint3D position = new FramePoint3D();
 FrameQuaternion orientation = new FrameQuaternion();
 footstep.getPose(position, orientation);
 originalFootstep.getLocation().set(position);
 originalFootstep.getOrientation().set(orientation);
 //get the footstep
 Footstep.FootstepType type = snapFootstep(originalFootstep, heightMap);
 footstep.setPredictedContactPoints(HumanoidMessageTools.unpackPredictedContactPoints(originalFootstep));
 footstep.setFootstepType(type);
 FramePose3D solePoseInWorld = new FramePose3D(ReferenceFrame.getWorldFrame(), originalFootstep.getLocation(), originalFootstep.getOrientation());
 footstep.setPose(solePoseInWorld);
 footstep.setSwingHeight(originalFootstep.getSwingHeight());
 footstep.setTrajectoryType(TrajectoryType.fromByte(originalFootstep.getTrajectoryType()));
 return type;
}

public Footstep.FootstepType snapFootstep(Footstep footstep, List<Point3D> pointList, double defaultHeight)
{
 // can only snap footsteps in world frame
 footstep.getFootstepPose().checkReferenceFrameMatch(ReferenceFrame.getWorldFrame());
 FootstepDataMessage originalFootstep = HumanoidMessageTools.createFootstepDataMessage(footstep);
 //set to the sole pose
 FramePoint3D position = new FramePoint3D();
 FrameQuaternion orientation = new FrameQuaternion();
 footstep.getPose(position, orientation);
 originalFootstep.getLocation().set(position);
 originalFootstep.getOrientation().set(orientation);
 //get the footstep
 Footstep.FootstepType type = snapFootstep(originalFootstep, pointList, defaultHeight);
 footstep.setFootstepType(type);
 footstep.setPredictedContactPoints(HumanoidMessageTools.unpackPredictedContactPoints(originalFootstep));
 FramePose3D solePoseInWorld = new FramePose3D(ReferenceFrame.getWorldFrame(), originalFootstep.getLocation(), originalFootstep.getOrientation());
 footstep.setPose(solePoseInWorld);
 footstep.setSwingHeight(originalFootstep.getSwingHeight());
 footstep.setTrajectoryType(TrajectoryType.fromByte(originalFootstep.getTrajectoryType()));
 return type;
}