How to use

getSoleReferenceFrame

method

in

us.ihmc.humanoidRobotics.footstep.Footstep

private void computeFootstepCentroid(FramePoint2d centroidToPack, Footstep footstep)
{
 predictedSupportPolygon.clear(footstep.getSoleReferenceFrame());
 addPredictedContactPointsToPolygon(footstep, predictedSupportPolygon);
 predictedSupportPolygon.update();
 predictedSupportPolygon.getCentroid(centroidToPack);
}

private FrameVector3D getTranslationVector(Footstep stance, double circleCenterOffset)
{
 RobotSide stanceSide = stance.getRobotSide();
 double yCenterDisplacementTowardsExpectedOtherFoot = (stanceSide == RobotSide.LEFT) ? -circleCenterOffset : circleCenterOffset;
 FrameVector3D translationFromFootCenterToCircleCenter = new FrameVector3D(stance.getSoleReferenceFrame(), 0.0, yCenterDisplacementTowardsExpectedOtherFoot,
    0.0);
 translationFromFootCenterToCircleCenter.changeFrame(ReferenceFrame.getWorldFrame());
 return translationFromFootCenterToCircleCenter;
}

  public static List<FramePoint> calculateExpectedContactPoints(Footstep atFootstep, ContactablePlaneBody foot)
  {
   if (!atFootstep.getBody().getName().equals(foot.getRigidBody().getName()))
     throw new RuntimeException("The RigidBodies are not the same.");
   
   List<FramePoint> ret = foot.getContactPointsCopy();
   
   ReferenceFrame soleFrame = atFootstep.getSoleReferenceFrame();
   
   for (int i = 0; i < ret.size(); i++)
   {
     FramePoint contactPoint = ret.get(i);
     contactPoint.checkReferenceFrameMatch(foot.getSoleFrame());
     contactPoint.setIncludingFrame(soleFrame, contactPoint.getPoint());
   }

   return ret;
  }
}

private void addPredictedContactPointsToPolygon(Footstep footstep, FrameConvexPolygon2d convexPolygonToExtend)
{
 List<Point2d> predictedContactPoints = footstep.getPredictedContactPoints();
 if (predictedContactPoints != null && !predictedContactPoints.isEmpty())
 {
   int numberOfContactPoints = predictedContactPoints.size();
   for (int i = 0; i < numberOfContactPoints; i++)
   {
    tempFramePoint.setXYIncludingFrame(footstep.getSoleReferenceFrame(), predictedContactPoints.get(i));
    convexPolygonToExtend.addVertexByProjectionOntoXYPlane(tempFramePoint);
   }
 }
 else
 {
   ConvexPolygon2d defaultPolygon = defaultFootPolygons.get(footstep.getRobotSide());
   for (int i = 0; i < defaultPolygon.getNumberOfVertices(); i++)
   {
    tempFramePoint.setXYIncludingFrame(footstep.getSoleReferenceFrame(), defaultPolygon.getVertex(i));
    convexPolygonToExtend.addVertexByProjectionOntoXYPlane(tempFramePoint);
   }
 }
}

public void extractFootstepSolutions(ArrayList<YoFramePoint2d> footstepSolutionsToPack, ArrayList<YoFramePoint2d> referenceFootstepLocations,
   ArrayList<Footstep> upcomingFootsteps, int numberOfFootstepsToConsider,
   ICPOptimizationSolver solver)
{
 boolean firstStepAdjusted = false;
 for (int i = 0; i < numberOfFootstepsToConsider; i++)
 {
   solver.getFootstepSolutionLocation(i, locationSolution);
   upcomingFootsteps.get(i).getPosition2d(upcomingFootstepLocation);
   ReferenceFrame deadbandFrame = upcomingFootsteps.get(i).getSoleReferenceFrame();
   boolean footstepWasAdjusted = applyLocationDeadband(locationSolution, upcomingFootstepLocation, referenceFootstepLocations.get(i).getFrameTuple2d(), deadbandFrame,
      footstepDeadband.getDoubleValue(), footstepSolutionResolution.getDoubleValue());
   if (i == 0)
    firstStepAdjusted = footstepWasAdjusted;
   footstepSolutionsToPack.get(i).set(locationSolution);
 }
 this.footstepWasAdjusted.set(firstStepAdjusted);
}

public static FramePoint3D getCenterOfFootstep(Footstep stance, RobotSide side, double centimetersForwardFromCenter, double centimetersInFromCenter)
{
 FramePoint3D stanceCenter = new FramePoint3D(stance.getSoleReferenceFrame());
 stanceCenter.getReferenceFrame().checkReferenceFrameMatch(stance.getSoleReferenceFrame());
 stanceCenter.setX(stanceCenter.getX() + centimetersForwardFromCenter);
 stanceCenter.setY(stanceCenter.getY() + side.negateIfLeftSide(centimetersInFromCenter));
 stanceCenter.changeFrame(worldFrame);
 return stanceCenter;
}

public static FramePoint getCenterOfFootstep(Footstep stance, RobotSide side, double centimetersForwardFromCenter, double centimetersInFromCenter)
{
 FramePoint stanceCenter = new FramePoint(stance.getSoleReferenceFrame());
 stanceCenter.getReferenceFrame().checkReferenceFrameMatch(stance.getSoleReferenceFrame());
 stanceCenter.setX(stanceCenter.getX() + centimetersForwardFromCenter);
 stanceCenter.setY(stanceCenter.getY() + side.negateIfLeftSide(centimetersInFromCenter));
 stanceCenter.changeFrame(worldFrame);
 return stanceCenter;
}

private void assertLastTwoStepsCenterAroundEndPoint(String testDescription, ArrayList<Footstep> footsteps, Point2D endWorldPoint)
{
 int numSteps = footsteps.size();
 Footstep lastStep = footsteps.get(numSteps - 1);
 Footstep nextLastStep = footsteps.get(numSteps - 2);
 Vector3D lastStepPosition = new Vector3D();
 Vector3D nextLastStepPosition = new Vector3D();
 Vector3D positionInFrame = new Vector3D();
 lastStep.getSoleReferenceFrame().getTransformToWorldFrame().getTranslation(lastStepPosition);
 nextLastStep.getSoleReferenceFrame().getTransformToWorldFrame().getTranslation(nextLastStepPosition);
 positionInFrame.interpolate(nextLastStepPosition, lastStepPosition, 0.5);
 Point2D positionInFrame2d = new Point2D(positionInFrame.getX(), positionInFrame.getY());
 double endPositionOffset = endWorldPoint.distance(positionInFrame2d);
 assertEquals(testDescription + " footsteps must end near desired end", 0.0, endWorldPoint.distance(positionInFrame2d), endPositionTolerance);
}

private void computeEntryCMPForFootstep(FramePoint entryCMPToPack, Footstep footstep, FramePoint2d centroidInSoleFrameOfPreviousSupportFoot,
   YoFramePoint previousExitCMP)
{
 ReferenceFrame soleFrame = footstep.getSoleReferenceFrame();
 List<Point2d> predictedContactPoints = footstep.getPredictedContactPoints();
 RobotSide robotSide = footstep.getRobotSide();
 if (predictedContactPoints != null)
   tempSupportPolygon.setIncludingFrameAndUpdate(soleFrame, predictedContactPoints);
 else
   tempSupportPolygon.setIncludingFrameAndUpdate(soleFrame, defaultFootPolygons.get(robotSide));
 computeEntryCMP(entryCMPToPack, robotSide, soleFrame, tempSupportPolygon, centroidInSoleFrameOfPreviousSupportFoot, previousExitCMP);
}

ReferenceFrame soleReferenceFrame = footstep.getSoleReferenceFrame();
double increaseZSlightlyToSeeBetter = 0.001;
FramePose3D soleFramePose = new FramePose3D(soleReferenceFrame, new Point3D(0.0, 0.0, increaseZSlightlyToSeeBetter), new AxisAngle());

public void setNextFootstep(Footstep nextFootstep)
{
 isUsingNextFootstep.set(nextFootstep != null);
 if (isUsingNextFootstep.getBooleanValue())
 {
   ReferenceFrame footstepSoleFrame = nextFootstep.getSoleReferenceFrame();
   ConvexPolygon2d footPolygon = footPolygons.get(nextFootstep.getRobotSide()).getConvexPolygon2d();
   nextFootstepPolygon.setIncludingFrameAndUpdate(footstepSoleFrame, footPolygon);
   nextFootstepPolygon.changeFrameAndProjectToXYPlane(worldFrame);
 }
}

if (isUpcomingFootstepLast)
 predictedSupportPolygon.clear(currentFootstep.getSoleReferenceFrame());
 addPredictedContactPointsToPolygon(currentFootstep, predictedSupportPolygon);
 predictedSupportPolygon.update();

ReferenceFrame soleReferenceFrame = footstep.getSoleReferenceFrame();
double increaseZSlightlyToSeeBetter = 0.001;
FramePose soleFramePose = new FramePose(soleReferenceFrame, new Point3d(0.0, 0.0, increaseZSlightlyToSeeBetter), new AxisAngle4d());

predictedSupportPolygon.clear(upcomingFootsteps.get(0).getSoleReferenceFrame());
addPredictedContactPointsToPolygon(upcomingFootsteps.get(0), predictedSupportPolygon);
predictedSupportPolygon.update();

/**
* This function takes a footstep and calculates the touch-down polygon in the
* desired reference frame
*/
private void calculateTouchdownFootPolygon(Footstep footstep, ReferenceFrame desiredFrame, FrameConvexPolygon2d polygonToPack)
{
 footstep.getPositionIncludingFrame(centroid3d);
 centroid3d.getFramePoint2d(centroid2d);
 centroid2d.changeFrame(desiredFrame);
 polygonToPack.setIncludingFrameAndUpdate(footstep.getSoleReferenceFrame(), defaultSupportPolygons.get(footstep.getRobotSide()));
 polygonToPack.changeFrameAndProjectToXYPlane(desiredFrame);
 // shrink the polygon for safety by pulling all the corner points towards the center
 polygonToPack.scale(centroid2d, SHRINK_TOUCHDOWN_POLYGON_FACTOR);
}

private void computeExitCMPForFootstep(FramePoint exitCMPToPack, Footstep footstep, FramePoint2d centroidInSoleFrameOfUpcomingSupportFoot,
   boolean isUpcomingFootstepLast)
{
 if (useTwoCMPsPerSupport)
 {
   ReferenceFrame soleFrame = footstep.getSoleReferenceFrame();
   List<Point2d> predictedContactPoints = footstep.getPredictedContactPoints();
   RobotSide robotSide = footstep.getRobotSide();
   if (predictedContactPoints != null)
    tempSupportPolygon.setIncludingFrameAndUpdate(soleFrame, predictedContactPoints);
   else
    tempSupportPolygon.setIncludingFrameAndUpdate(soleFrame, defaultFootPolygons.get(robotSide));
   computeExitCMP(exitCMPToPack, robotSide, soleFrame, tempSupportPolygon, centroidInSoleFrameOfUpcomingSupportFoot, isUpcomingFootstepLast);
 }
 else
 {
   exitCMPToPack.setToNaN(worldFrame);
 }
}

public void visualizeFootstep(ContactablePlaneBody bipedFoot, Footstep footstep)
{
 bagOfSingleFootstepVisualizers.visualizeFootstep(footstep, bipedFoot);
 nullRobot.setTime(nullRobot.getTime() + scs.getDT());
 FramePoint solePositon = new FramePoint(footstep.getSoleReferenceFrame());
 solePositon.changeFrame(worldFrame);
 updateFocus(solePositon);
 scs.setCameraFix(focusX, focusY, 0.0);
 scs.setCameraPosition(focusX, focusY - 1.5, 6.0);
 scs.tickAndUpdate();
}

public void visualizeFootstep(ContactablePlaneBody bipedFoot, Footstep footstep)
{
 bagOfSingleFootstepVisualizers.visualizeFootstep(footstep, bipedFoot);
 nullRobot.setTime(nullRobot.getTime() + scs.getDT());
 FramePoint3D solePositon = new FramePoint3D(footstep.getSoleReferenceFrame());
 solePositon.changeFrame(worldFrame);
 updateFocus(solePositon);
 scs.setCameraFix(focusX, focusY, 0.0);
 scs.setCameraPosition(focusX, focusY - 1.5, 6.0);
 scs.tickAndUpdate();
}

public static FramePoint getCenterOfPredictedContactPointsInFootstep(Footstep footstep, RobotSide side, double centimetersForwardFromCenter, double centimetersInFromCenter)
{
 Point2d footstepCenter;
 
 List<Point2d> predictedContactPoints = footstep.getPredictedContactPoints();
 if (predictedContactPoints != null)
 {
   ConvexPolygon2d contactPolygon = new ConvexPolygon2d(predictedContactPoints);
   footstepCenter = contactPolygon.getCentroid();
 }
 else
 {
   footstepCenter = new Point2d();
 }
 
 footstepCenter.setX(footstepCenter.getX() + centimetersForwardFromCenter);
 footstepCenter.setY(footstepCenter.getY() + side.negateIfLeftSide(centimetersInFromCenter));
 
 FramePoint footstepCenter3d = new FramePoint(footstep.getSoleReferenceFrame(), footstepCenter.getX(), footstepCenter.getY(), 0.0);
 footstepCenter3d.changeFrame(worldFrame);
 return footstepCenter3d;
}

public static FramePoint3D getCenterOfPredictedContactPointsInFootstep(Footstep footstep, RobotSide side, double centimetersForwardFromCenter, double centimetersInFromCenter)
{
 Point2D footstepCenter;
 List<Point2D> predictedContactPoints = footstep.getPredictedContactPoints();
 if (predictedContactPoints != null)
 {
   ConvexPolygon2D contactPolygon = new ConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(predictedContactPoints));
   footstepCenter = new Point2D(contactPolygon.getCentroid());
 }
 else
 {
   footstepCenter = new Point2D();
 }
 footstepCenter.setX(footstepCenter.getX() + centimetersForwardFromCenter);
 footstepCenter.setY(footstepCenter.getY() + side.negateIfLeftSide(centimetersInFromCenter));
 FramePoint3D footstepCenter3d = new FramePoint3D(footstep.getSoleReferenceFrame(), footstepCenter.getX(), footstepCenter.getY(), 0.0);
 footstepCenter3d.changeFrame(worldFrame);
 return footstepCenter3d;
}