How to use

getPosition

method

in

us.ihmc.humanoidRobotics.footstep.Footstep

private void assertMaxDisplacementStanceToSwing(String message, ArrayList<Footstep> footsteps, double distTest)
{
 if (distTest == Double.POSITIVE_INFINITY)
   return;
 Footstep footstep1;
 Footstep footstep2;
 for (int i = 0; i < footsteps.size() - 1; i++)
 {
   footstep1 = footsteps.get(i);
   footstep2 = footsteps.get(i + 1);
   FramePoint3D position2 = new FramePoint3D();
   footstep2.getPosition(position2);
   FramePoint3D position1 = new FramePoint3D();
   footstep1.getPosition(position1);
   double dis = position2.distance(position1);
   assertTrue(String.format(message + " %.2f<=%.2f? step %d to %d", dis, distTest, i, i + 1), dis < distTest + 1e-14);
 }
}

private void assertMaxDisplacementSwingToSwing(String message, ArrayList<Footstep> footsteps, double distTest)
{
 if (distTest == Double.POSITIVE_INFINITY)
   return;
 Footstep footstep1;
 Footstep footstep2;
 for (int i = 0; i < footsteps.size() - 2; i++)
 {
   footstep1 = footsteps.get(i);
   footstep2 = footsteps.get(i + 2);
   FramePoint3D position2 = new FramePoint3D();
   footstep2.getPosition(position2);
   FramePoint3D position1 = new FramePoint3D();
   footstep1.getPosition(position1);
   double dis = position2.distance(position1);
   //       assertTrue(true);
   assertTrue(String.format(message + " %.2f<=%.2f? step %d to %d", dis, distTest, i, i + 2), dis < distTest + 1e-14);
 }
}

public double getSwingHeight(Footstep initialFootstep, Footstep stanceFootstep, Footstep endFootstep, HeightMapWithPoints heightMap)
{
 FramePose startPose = new FramePose();
 initialFootstep.getSolePose(startPose);
 FramePose endPose = new FramePose();
 endFootstep.getSolePose(endPose);
 Point3d position = new Point3d();
 stanceFootstep.getPosition(position);
 return getSwingHeight(startPose, endPose, position.getZ(), heightMap);
}

public static RobotSide getFrontFootRobotSideFromFootsteps(SideDependentList<Footstep> footSteps, FramePoint3D destination)
{
 FramePoint3D destinationInWorld = new FramePoint3D(destination);
 destinationInWorld.changeFrame(worldFrame);
 SideDependentList<Double> distances = new SideDependentList<Double>();
 for (RobotSide side : RobotSide.values)
 {
   FramePoint3D footstepPosition = new FramePoint3D();
   footSteps.get(side).getPosition(footstepPosition);
   footstepPosition.changeFrame(worldFrame);
   distances.set(side, new Double(footstepPosition.distanceSquared(destinationInWorld)));
 }
 RobotSide frontSide = (distances.get(RobotSide.LEFT) <= distances.get(RobotSide.RIGHT)) ? RobotSide.LEFT : RobotSide.RIGHT;
 return frontSide;
}

transferFromFootstep.getPosition(transferFromFootFramePoint);
FrameQuaternion transferFromFootOrientation = new FrameQuaternion();
transferFromFootstep.getOrientation(transferFromFootOrientation);
transferToFootstep.getPosition(transferToFootFramePoint);
FrameQuaternion transferToFootOrientation = new FrameQuaternion();
transferToFootstep.getOrientation(transferToFootOrientation);