How to use

getFootstepPose

method

in

us.ihmc.humanoidRobotics.footstep.Footstep

private void updateNextFootsteps(int stepIndex)
{
 int numberOfStepToUpdate =
    (numberOfFootstepsForTest - stepIndex) < numberOfFootstepsToConsider ? (numberOfFootstepsForTest - stepIndex) : numberOfFootstepsToConsider;
 int nextStepIndex;
 for (nextStepIndex = 0; nextStepIndex < numberOfStepToUpdate; nextStepIndex++)
 {
   nextFootstepPoses.get(nextStepIndex).set(footstepList.get(stepIndex + nextStepIndex).getFootstepPose());
 }
 for (; nextStepIndex < numberOfFootstepsToConsider; nextStepIndex++)
 {
   nextFootstepPoses.get(nextStepIndex).setToNaN();
 }
}

public FootstepDataListMessage generateFootstepsFromLocationsAndOrientations(RobotSide[] robotSides, double[][][] footstepLocationsAndOrientations, double swingTime, double transferTime)
{
 FootstepDataListMessage footstepDataList = HumanoidMessageTools.createFootstepDataListMessage(swingTime, transferTime);
 for (int i = 0; i < robotSides.length; i++)
 {
   RobotSide robotSide = robotSides[i];
   double[][] footstepLocationAndOrientation = footstepLocationsAndOrientations[i];
   Footstep footstep = generateFootstepFromLocationAndOrientation(robotSide, footstepLocationAndOrientation);
   FootstepDataMessage footstepData = HumanoidMessageTools.createFootstepDataMessage(robotSide, footstep.getFootstepPose().getPosition(), footstep.getFootstepPose().getOrientation());
   footstepDataList.getFootstepDataList().add().set(footstepData);
 }
 return footstepDataList;
}

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 AdjustFootstepMessage createAdjustFootstepMessage(Footstep footstep)
{
 AdjustFootstepMessage message = new AdjustFootstepMessage();
 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);
 MessageTools.copyData(footstep.getPredictedContactPoints().stream().map(Point3D::new).collect(Collectors.toList()),
            message.getPredictedContactPoints2d());
 return message;
}

@Override
public void adjustFootstepWithoutHeightmap(Footstep footstep, double height, Vector3D planeNormal)
{
 // can only snap footsteps in world frame
 footstep.getFootstepPose().checkReferenceFrameMatch(ReferenceFrame.getWorldFrame());
 FramePose3D footstepPose = new FramePose3D();
 footstep.getPose(footstepPose);
 Point3D position = new Point3D(footstepPose.getPosition());
 double yaw = footstep.getFootstepPose().getYaw();
 Quaternion orientation = new Quaternion();
 RotationTools.computeQuaternionFromYawAndZNormal(yaw, planeNormal, orientation);
 position.setZ(height);
 footstepPose.set(new Point3D(position), orientation);
 footstep.setPose(footstepPose);
}

private void updateContactState(int currentStepCount, double percentageOfPhase)
{
 if (inDoubleSupport.getBooleanValue())
 {
   for (RobotSide side : RobotSide.values)
    contactStates.get(side).setFullyConstrained();
 }
 else
 {
   RobotSide swingSide = footstepList.get(currentStepCount).getRobotSide();
   contactStates.get(swingSide).clear();
   contactStates.get(swingSide.getOppositeSide()).setFullyConstrained();
   if (currentStepCount > 0)
   {
    swingFootPose.set(footstepList.get(currentStepCount - 1).getFootstepPose());
   }
   else
   {
    swingFootPose.set(initialPose);
    swingFootPose.appendTranslation(0.0, swingSide.negateIfRightSide(stepWidth / 2.0), 0.0);
   }
   FramePose3D endOfSwing = footstepList.get(currentStepCount).getFootstepPose();
   swingFootPose.interpolate(endOfSwing, percentageOfPhase);
   FootSpoof foot = feet.get(swingSide);
   foot.setSoleFrame(swingFootPose);
 }
 bipedSupportPolygons.updateUsingContactStates(contactStates);
}

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

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

for (int numberOfStepsFromTarget = 0; numberOfStepsFromTarget <= numberOfFootstepsBetweenStartAndTarget; numberOfStepsFromTarget++)
  FramePose2D currentFootstepPose = new FramePose2D(footsteps.get(numberOfFootsteps - numberOfStepsFromTarget - 1).getFootstepPose());
  assertEquals("Current footstep orientation does not match end orientation.", 0.0, currentFootstepPose.getOrientationDistance(targetMidFeetPose2d),
        ORIENTATION_THRESHOLD);

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

@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testConstructor()
{
 FootstepData testObject = new FootstepData();
 assertTrue(testObject != null);
 Footstep testFootstep = new Footstep(RobotSide.LEFT);
 FootstepTiming testFootstepTiming = new FootstepTiming(0.2, 0.1);
 testFootstepTiming.setAbsoluteTime(0.1, 0.5);
 testObject.set(testFootstep, testFootstepTiming);
 assertTrue(testObject.getFootstep() == testFootstep);
 assertTrue(testObject.getStepTime() == testFootstepTiming.getStepTime());
 assertTrue(testObject.getSwingSide() == testFootstep.getRobotSide());
 assertTrue(testObject.getSupportSide() == testFootstep.getRobotSide().getOppositeSide());
 assertTrue(testObject.getSwingTime() == testFootstepTiming.getSwingTime());
 assertTrue(testObject.getTransferTime() == testFootstepTiming.getTransferTime());
 assertTrue(MathTools.epsilonEquals(testObject.getTransferStartTime(), testFootstepTiming.getExecutionStartTime(), Epsilons.ONE_BILLIONTH));
 assertTrue(testObject.getSwingStartTime() == testFootstepTiming.getSwingStartTime());
 assertTrue(testObject.getFootstepPoseReferenceFrame() == testFootstep.getFootstepPose().getReferenceFrame());
 assertTrue(testObject.getFramePose() == testFootstep.getFootstepPose());
 testObject.setSwingTime(0.8);
 assertTrue(testObject.getSwingTime() == 0.8);
 Footstep newFootstep = new Footstep(RobotSide.LEFT);
 testObject.setFootstep(newFootstep);
 assertTrue(testObject.getFootstep() == newFootstep);
}

for (Footstep footstep : footsteps)
  FramePose2D currentFootstepPose = new FramePose2D(footstep.getFootstepPose());
  if (currentFootstepPose.getOrientationDistance(startTargetMidPose2dMean) < ORIENTATION_THRESHOLD)
   numberOfStepsAlignedWithMeanOrientation++;

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

FramePoint2D transferFromFootPosition = new FramePoint2D(transferFromFootstep.getFootstepPose().getPosition());
FramePoint2D transferToFootPosition = new FramePoint2D(transferToFootstep.getFootstepPose().getPosition());

for (Footstep footstep : walkToLocationBehavior.getFootSteps())
  FramePose2D currentFootstepPose = new FramePose2D(footstep.getFootstepPose());
  assertEquals("Current footstep orientation does not match start orientation.", 0.0, currentFootstepPose.getOrientationDistance(startMidFeetPose2d),
        ORIENTATION_THRESHOLD);