How to use

setSwingHeight

method

in

us.ihmc.humanoidRobotics.footstep.Footstep

@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);
  }
}

footstep.setSwingHeight(footstepData.swingHeight);
switch (footstepData.getOrigin())

@Override
public Footstep.FootstepType snapFootstep(Footstep footstep, HeightMapWithPoints heightMap){
 FootstepDataMessage originalFootstep = new FootstepDataMessage(footstep);
 //set to the sole pose
 Vector3d position = new Vector3d();
 Quat4d orientation = new Quat4d();
 RigidBodyTransform solePose = new RigidBodyTransform();
 footstep.getSolePose(solePose);
 solePose.get(orientation, position);
 originalFootstep.setLocation(new Point3d(position));
 originalFootstep.setOrientation(orientation);
 //get the footstep
 Footstep.FootstepType type = snapFootstep(originalFootstep, heightMap);
 if (type == Footstep.FootstepType.FULL_FOOTSTEP && originalFootstep.getPredictedContactPoints() != null){
   throw new RuntimeException(this.getClass().getSimpleName() + "Full Footstep should have null contact points");
 }
 footstep.setPredictedContactPointsFromPoint2ds(originalFootstep.getPredictedContactPoints());
 footstep.setFootstepType(type);
 FramePose solePoseInWorld = new FramePose(ReferenceFrame.getWorldFrame(), originalFootstep.getLocation(), originalFootstep.getOrientation());
 footstep.setSolePose(solePoseInWorld);
 footstep.setSwingHeight(originalFootstep.getSwingHeight());
 footstep.setTrajectoryType(originalFootstep.getTrajectoryType());
 return type;
}

@Override
public Footstep.FootstepType snapFootstep(Footstep footstep, HeightMapWithPoints heightMap)
{
 FootstepDataMessage originalFootstep = new FootstepDataMessage(footstep);
 //set to the sole pose
 Vector3d position = new Vector3d();
 Quat4d orientation = new Quat4d();
 RigidBodyTransform solePose = new RigidBodyTransform();
 footstep.getSolePose(solePose);
 solePose.get(orientation, position);
 originalFootstep.setLocation(new Point3d(position));
 originalFootstep.setOrientation(orientation);
 //get the footstep
 Footstep.FootstepType type = snapFootstep(originalFootstep, heightMap);
 if (type == Footstep.FootstepType.FULL_FOOTSTEP && originalFootstep.getPredictedContactPoints() != null)
 {
   throw new RuntimeException(this.getClass().getSimpleName() + "Full Footstep should have null contact points");
 }
 footstep.setPredictedContactPointsFromPoint2ds(originalFootstep.getPredictedContactPoints());
 footstep.setFootstepType(type);
 FramePose solePoseInWorld = new FramePose(ReferenceFrame.getWorldFrame(), originalFootstep.getLocation(), originalFootstep.getOrientation());
 footstep.setSolePose(solePoseInWorld);
 footstep.setSwingHeight(originalFootstep.getSwingHeight());
 footstep.setTrajectoryType(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);
 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;
}

footstep.setSwingHeight(footstepData.getSwingHeight());
switch (footstepData.getOrigin())

public Footstep.FootstepType snapFootstep(Footstep footstep, List<Point3d> pointList, double defaultHeight)
{
 FootstepDataMessage originalFootstep = new FootstepDataMessage(footstep);
 //set to the sole pose
 Vector3d position = new Vector3d();
 Quat4d orientation = new Quat4d();
 RigidBodyTransform solePose = new RigidBodyTransform();
 footstep.getSolePose(solePose);
 solePose.get(orientation, position);
 originalFootstep.setLocation(new Point3d(position));
 originalFootstep.setOrientation(orientation);
 //get the footstep
 Footstep.FootstepType type = snapFootstep(originalFootstep, pointList, defaultHeight);
 footstep.setFootstepType(type);
 footstep.setPredictedContactPointsFromPoint2ds(originalFootstep.getPredictedContactPoints());
 FramePose solePoseInWorld = new FramePose(ReferenceFrame.getWorldFrame(), originalFootstep.getLocation(), originalFootstep.getOrientation());
 footstep.setSolePose(solePoseInWorld);
 footstep.setSwingHeight(originalFootstep.getSwingHeight());
 footstep.setTrajectoryType(originalFootstep.getTrajectoryType());
 return type;
}

@Override
public Footstep.FootstepType snapFootstep(Footstep footstep, HeightMapWithPoints heightMap){
 FootstepDataMessage originalFootstep = new FootstepDataMessage(footstep);
 //set to the sole pose
 Vector3d position = new Vector3d();
 Quat4d orientation = new Quat4d();
 RigidBodyTransform solePose = new RigidBodyTransform();
 footstep.getSolePose(solePose);
 solePose.get(orientation, position);
 originalFootstep.setLocation(new Point3d(position));
 originalFootstep.setOrientation(orientation);
 //get the footstep
 Footstep.FootstepType type = snapFootstep(originalFootstep, heightMap);
 footstep.setPredictedContactPointsFromPoint2ds(originalFootstep.getPredictedContactPoints());
 footstep.setFootstepType(type);
 FramePose solePoseInWorld = new FramePose(ReferenceFrame.getWorldFrame(), originalFootstep.getLocation(), originalFootstep.getOrientation());
 footstep.setSolePose(solePoseInWorld);
 footstep.setSwingHeight(originalFootstep.getSwingHeight());
 footstep.setTrajectoryType(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;
}