us.ihmc.humanoidRobotics.footstep.Footstep.getSoleReferenceFrame java code examples

Add the Codota plugin to your IDE and get smart completions

private void myMethod () {
 BufferedReader b =InputStream in;new BufferedReader(new InputStreamReader(in))
Reader in;new BufferedReader(in)
File file;new BufferedReader(new FileReader(file))
Smart code suggestions by Tabnine
}

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

Popular methods of Footstep

Popular in Java

Parsing JSON documents to java classes using gson
findViewById (Activity)
getOriginalFilename (MultipartFile)
Return the original filename in the client's filesystem.This may contain path information depending
getSystemService (Context)
Rectangle (java.awt)
A Rectangle specifies an area in a coordinate space that is enclosed by the Rectangle object's top-
Charset (java.nio.charset)
A charset is a named mapping between Unicode characters and byte sequences. Every Charset can decode
KeyStore (java.security)
KeyStore is responsible for maintaining cryptographic keys and their owners. The type of the syste
CountDownLatch (java.util.concurrent)
A synchronization aid that allows one or more threads to wait until a set of operations being perfor
ExecutorService (java.util.concurrent)
An Executor that provides methods to manage termination and methods that can produce a Future for tr
TimeUnit (java.util.concurrent)
A TimeUnit represents time durations at a given unit of granularity and provides utility methods to
Top 17 PhpStorm Plugins

How to use getSoleReferenceFramemethodin us.ihmc.humanoidRobotics.footstep.Footstep

Best Java code snippets using us.ihmc.humanoidRobotics.footstep.Footstep.getSoleReferenceFrame (Showing top 20 results out of 315)

How to use
getSoleReferenceFrame
method
in
us.ihmc.humanoidRobotics.footstep.Footstep