- Add the Codota plugin to your IDE and get smart completions
private void myMethod () {BufferedReader b =
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; }