How to use

getPose

method

in

us.ihmc.humanoidRobotics.footstep.Footstep

private void checkFootMeetsAngleRequirement(Footstep stance, Footstep swingEnd)
{
 FramePose3D stancePose = new FramePose3D();
 stance.getPose(stancePose);
 FramePose3D swingEndPose = new FramePose3D();
 swingEnd.getPose(swingEndPose);
 FrameOrientation2D stanceOrientation = new FrameOrientation2D(stancePose.getOrientation());
 FrameOrientation2D swingOrientation = new FrameOrientation2D(swingEndPose.getOrientation());
 double yawDifference = swingOrientation.difference(stanceOrientation);
 double allowedDifference = (Math.abs(yawDifference) - footTwistLimitInRadians) - 3e-16;
 if (allowedDifference > 0)
   valid = false;
}

private void assertFootstepInAngleRange(String testDescription, Footstep footstep, double startYaw, double endYaw)
{
 FramePose3D footPose = new FramePose3D();
 footstep.getPose(footPose);
 FrameOrientation2D footOrientation = new FrameOrientation2D(footPose.getOrientation());
 footOrientation.changeFrame(WORLD_FRAME);
 double footYaw = footOrientation.getYaw();
 double angleBetweenStartAndEnd = Math.abs(AngleTools.computeAngleDifferenceMinusPiToPi(endYaw, startYaw));
 double angleBetweenStartAndFoot = Math.abs(AngleTools.computeAngleDifferenceMinusPiToPi(footYaw, startYaw));
 double angleBetweenFootAndEnd = Math.abs(AngleTools.computeAngleDifferenceMinusPiToPi(endYaw, footYaw));
 //if start->foot + foot-> end > start->end, then the foot angle is outside the range between the two
 boolean outsideFan = (angleBetweenStartAndFoot + angleBetweenFootAndEnd) > angleBetweenStartAndEnd + 1e-14;
}

private void assertNoRepeatedSquareUpSteps(String message, List<Footstep> footsteps)
{
 int numFootsteps = footsteps.size();
 if (numFootsteps < 4) // True step in place should only have two footsteps and that should be OK.
   return;
 // Assuming last steps are squared up: shouldn't match steps two before.
 for (int i = numFootsteps - 1; i > numFootsteps - 3; i--)
 {
   Footstep lastFootstep = footsteps.get(i);
   Footstep compareToLastFootstep = footsteps.get(i - 2);
   FramePose3D lastFootstepPose = new FramePose3D();
   lastFootstep.getPose(lastFootstepPose);
   FramePose3D compareToLastFootstepPose = new FramePose3D();
   compareToLastFootstep.getPose(compareToLastFootstepPose);
   FramePoint2D lastFootstepPosition2d = new FramePoint2D(lastFootstepPose.getPosition());
   FramePoint2D compareToLastFootstepPosition2d = new FramePoint2D(compareToLastFootstepPose.getPosition());
   double eps = 1e-14;
   assertTrue(
      message + " step " + i + " and " + (i - 2),
      (compareToLastFootstepPosition2d.distance(lastFootstepPosition2d) > eps)
         || (Math.abs(lastFootstepPose.getYaw() - compareToLastFootstepPose.getYaw()) > eps));
 }
}

@Override public boolean getUpcomingFootstepSolution(Footstep footstepToPack)
{
 if (icpOptimizationController.getNumberOfFootstepsToConsider() > 0)
 {
   footstepToPack.getPose(footstepPose);
   icpOptimizationController.getFootstepSolution(0, footstepPositionSolution);
   footstepPose.setXYFromPosition2d(footstepPositionSolution);
   footstepToPack.setPose(footstepPose);
 }
 return icpOptimizationController.wasFootstepAdjusted();
}

private FootstepDataListMessage createFootstepDataList(ArrayList<Footstep> desiredFootsteps)
{
 FootstepDataListMessage ret = new FootstepDataListMessage();
 for (int i = 0; i < desiredFootsteps.size(); i++)
 {
   Footstep footstep = desiredFootsteps.get(i);
   FramePoint3D position = new FramePoint3D();
   FrameQuaternion orientation = new FrameQuaternion();
   footstep.getPose(position, orientation);
   RobotSide footstepSide = footstep.getRobotSide();
   FootstepDataMessage footstepData = HumanoidMessageTools.createFootstepDataMessage(footstepSide, position, orientation);
   ret.getFootstepDataList().add().set(footstepData);
 }
 return ret;
}

private void assertNoCourseRepeatedSquareUpSteps(String message, ArrayList<Footstep> footstepsOriginal, double startX, double startY, double startYaw,
   double eps, int shortSteps)
{
 SideDependentList<Footstep> startFeet = updatedStartFeet(startX, startY, startYaw);
 ArrayList<Footstep> footsteps = prependStanceToFootstepQueue(footstepsOriginal, startFeet);
 int numFootsteps = footsteps.size();
 for (int i = 0; i + 2 < numFootsteps; i++)
 {
   Footstep firstFootstep = footsteps.get(i);
   Footstep nextFootstep = footsteps.get(i + 2);
   FramePose3D firstFootstepPose = new FramePose3D();
   firstFootstep.getPose(firstFootstepPose);
   FramePose3D nextFootstepPose = new FramePose3D();
   nextFootstep.getPose(nextFootstepPose);
   FramePoint2D firstFootstepPosition2d = new FramePoint2D(firstFootstepPose.getPosition());
   FramePoint2D nextFootstepPosition2d = new FramePoint2D(nextFootstepPose.getPosition());
   double testeps = eps;
   String message2 = message + "course restep test " + i + "to" + (i + 2);
   boolean condition = (nextFootstepPosition2d.distance(firstFootstepPosition2d) > testeps)
      || (Math.abs(firstFootstepPose.getYaw() - nextFootstepPose.getYaw()) > testeps);
   Visualization visualize = (condition == true) ? Visualization.NO_VISUALIZATION : Visualization.VISUALIZE;
   showFootstepsIfVisualize(message2, footstepsOriginal, startFeet, visualize);
   assertTrue(message2, condition);
 }
}

private FrameVector3D offCenterVectorToSwingEnd(Footstep stance, Footstep swingEnd, FrameVector3D translationFromFootCenterToCircleCenter)
{
 FramePoint3D circleCenter;
 FramePose3D stanceSole = new FramePose3D();
 FramePose3D swingEndSole = new FramePose3D();
 stance.getPose(stanceSole);
 swingEnd.getPose(swingEndSole);
 circleCenter = new FramePoint3D(stanceSole.getPosition());
 circleCenter.add(translationFromFootCenterToCircleCenter);//change frame of translation first?
 FrameVector3D vectorToSwingEnd = new FrameVector3D(ReferenceFrame.getWorldFrame());
 vectorToSwingEnd.set(swingEndSole.getPosition());
 vectorToSwingEnd.sub(circleCenter);
 return vectorToSwingEnd;
}

public void set(ArrayList<Footstep> footsteps, double swingTime, double transferTime)
{
 FootstepDataListMessage footstepDataList = HumanoidMessageTools.createFootstepDataListMessage(swingTime, transferTime);
 for (int i = 0; i < footsteps.size(); i++)
 {
   Footstep footstep = footsteps.get(i);
   FramePoint3D position = new FramePoint3D();
   FrameQuaternion orientation = new FrameQuaternion();
   footstep.getPose(position, orientation);
   RobotSide footstepSide = footstep.getRobotSide();
   FootstepDataMessage footstepData = HumanoidMessageTools.createFootstepDataMessage(footstepSide, position, orientation);
   footstepDataList.getFootstepDataList().add().set(footstepData);
 }
 set(footstepDataList);
}

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

footstep.getPose(footstepPose);

FramePose3D stanceFramePose = new FramePose3D();
FramePose3D endingFramePose = new FramePose3D();
swingStart.getPose(initialFramePose);
stance.getPose(stanceFramePose);
swingEnd.getPose(endingFramePose);

private static void assertStepIsPointingCorrectly(String message, Footstep footstep, FrameOrientation2D endOrientation)
{
 FramePose3D footPose = new FramePose3D();
 footstep.getPose(footPose);
 // System.out.println(Math.toDegrees(endOrientation.getYaw()) + "?=" + Math.toDegrees(footPose.getYaw()));
 // System.out.println(Math.toDegrees(footPose.getRoll()) + "?=" + Math.toDegrees(footPose.getPitch()) + "?=" + 0.0);
 assertEquals(message + " Foot roll should be 0.", 0.0, footPose.getRoll(), 1e-10);
 assertEquals(message + " Foot pitch should be 0.", 0.0, footPose.getPitch(), 1e-10);
 FrameOrientation2D footOrientation = new FrameOrientation2D(footPose.getOrientation());
 double yawDiff = endOrientation.difference(footOrientation);
 assertEquals(message + " Foot yaw and desired yaw difference should be 0.", 0.0, yawDiff, 1e-10);
}

private int checkForEndingOfAdjustment(double omega0)
{
 int numberOfFootstepsToConsider = clipNumberOfFootstepsToConsiderToProblem(this.numberOfFootstepsToConsider.getIntegerValue());
 if (timeRemainingInState.getDoubleValue() < remainingTimeToStopAdjusting.getDoubleValue() && localUseStepAdjustment && !isStanding.getBooleanValue() && !isInTransfer.getBooleanValue())
 {
   //record current locations
   for (int i = 0; i < numberOfFootstepsToConsider; i++)
   {
    upcomingFootstepLocations.get(i).set(footstepSolutions.get(i));
    Footstep footstep = upcomingFootsteps.get(i);
    footstep.getPose(footstepPose);
    footstepPose.setXYFromPosition2d(footstepSolutions.get(i).getFrameTuple2d());
    footstep.setPose(footstepPose);
    inputHandler.addFootstepToPlan(footstep);
   }
   localUseStepAdjustment = false;
   numberOfFootstepsToConsider = clipNumberOfFootstepsToConsiderToProblem(this.numberOfFootstepsToConsider.getIntegerValue());
   footstepRecursionMultiplierCalculator.resetTimes();
   footstepRecursionMultiplierCalculator.submitTimes(0, 0.0, singleSupportDuration.getDoubleValue());
   for (int i = 1; i < numberOfFootstepsToConsider + 1; i++)
    footstepRecursionMultiplierCalculator.submitTimes(i, doubleSupportDuration.getDoubleValue(), singleSupportDuration.getDoubleValue());
   footstepRecursionMultiplierCalculator.submitTimes(numberOfFootstepsToConsider + 1, doubleSupportDuration.getDoubleValue(), singleSupportDuration.getDoubleValue());
   footstepRecursionMultiplierCalculator.computeRecursionMultipliers(numberOfFootstepsToConsider, isInTransfer.getBooleanValue(), localUseTwoCMPs, omega0);
 }
 return numberOfFootstepsToConsider;
}

private void calculateInitialFootPoseAndOffsets()
{
 SideDependentList<Footstep> currentFootsteps;
 if (priorStanceFeetSpecified)
   currentFootsteps = priorStanceFeet;
 else
   currentFootsteps = createFootstepsFromBipedFeet();
 Footstep left = currentFootsteps.get(RobotSide.LEFT);
 Footstep right = currentFootsteps.get(RobotSide.RIGHT);
 FramePose3D leftPose = new FramePose3D();
 FramePose3D rightPose = new FramePose3D();
 left.getPose(leftPose);
 right.getPose(rightPose);
 FramePose2D leftPose2d = new FramePose2D(leftPose);
 FramePose2D rightPose2d = new FramePose2D(rightPose);
 startPose.interpolate(leftPose2d, rightPose2d, 0.5);
 Pose2dReferenceFrame startFramePose = new Pose2dReferenceFrame("StartPoseFrame", startPose);
 leftPose.changeFrame(startFramePose);
 rightPose.changeFrame(startFramePose);
 FrameOrientation2D leftOrientation = new FrameOrientation2D(leftPose.getOrientation());
 FrameOrientation2D rightOrientation = new FrameOrientation2D(rightPose.getOrientation());
 initialDeltaFeetYaw = leftOrientation.difference(rightOrientation);
 initialDeltaFeetY = leftPose.getY() - rightPose.getY();
 initialDeltaFeetX = leftPose.getX() - rightPose.getX();
}

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

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

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