- Add the Codota plugin to your IDE and get smart completions
private void myMethod () {}
@Before public void setUp() { parentRegistry = new YoVariableRegistry("parentRegistryTEST"); for (RobotSide robotSide : RobotSide.values) { String sidePrefix = robotSide.getCamelCaseNameForStartOfExpression(); double xToAnkle = 0.0; double yToAnkle = 0.0; double zToAnkle = 0.0; List<Point2D> contactPointsInSoleFrame = new ArrayList<>(); contactPointsInSoleFrame.add(new Point2D(footLengthForControl / 2.0, toeWidthForControl / 2.0)); contactPointsInSoleFrame.add(new Point2D(footLengthForControl / 2.0, -toeWidthForControl / 2.0)); contactPointsInSoleFrame.add(new Point2D(-footLengthForControl / 2.0, -footWidthForControl / 2.0)); contactPointsInSoleFrame.add(new Point2D(-footLengthForControl / 2.0, footWidthForControl / 2.0)); FootSpoof contactableFoot = new FootSpoof(sidePrefix + "Foot", xToAnkle, yToAnkle, zToAnkle, contactPointsInSoleFrame, 0.0); FramePose3D startingPose = new FramePose3D(); startingPose.setToZero(worldFrame); startingPose.setY(robotSide.negateIfRightSide(0.20)); contactableFoot.setSoleFrame(startingPose); contactableFeet.put(robotSide, contactableFoot); RigidBodyBasics foot = contactableFoot.getRigidBody(); ReferenceFrame soleFrame = contactableFoot.getSoleFrame(); List<FramePoint2D> contactFramePoints = contactableFoot.getContactPoints2d(); double coefficientOfFriction = contactableFoot.getCoefficientOfFriction(); YoPlaneContactState yoPlaneContactState = new YoPlaneContactState(sidePrefix + "Foot", foot, soleFrame, contactFramePoints, coefficientOfFriction, parentRegistry); yoPlaneContactState.setFullyConstrained(); contactStates.put(robotSide, yoPlaneContactState); } }
private BipedSupportPolygons setupBipedSupportPolygons(SideDependentList<FootSpoof> contactableFeet, YoVariableRegistry registry) { SideDependentList<ReferenceFrame> ankleZUpFrames = new SideDependentList<>(); SideDependentList<YoPlaneContactState> contactStates = new SideDependentList<>(); for (RobotSide robotSide : RobotSide.values) { FootSpoof contactableFoot = contactableFeet.get(robotSide); ReferenceFrame ankleFrame = contactableFoot.getFrameAfterParentJoint(); ankleFrames.put(robotSide, ankleFrame); ankleZUpFrames.put(robotSide, new ZUpFrame(worldFrame, ankleFrame, robotSide.getCamelCaseNameForStartOfExpression() + "ZUp")); String sidePrefix = robotSide.getCamelCaseNameForStartOfExpression(); RigidBodyBasics foot = contactableFoot.getRigidBody(); ReferenceFrame soleFrame = contactableFoot.getSoleFrame(); List<FramePoint2D> contactFramePoints = contactableFoot.getContactPoints2d(); double coefficientOfFriction = contactableFoot.getCoefficientOfFriction(); YoPlaneContactState yoPlaneContactState = new YoPlaneContactState(sidePrefix + "Foot", foot, soleFrame, contactFramePoints, coefficientOfFriction, registry); yoPlaneContactState.setFullyConstrained(); contactStates.put(robotSide, yoPlaneContactState); } ReferenceFrame midFeetZUpFrame = new MidFrameZUpFrame("midFeetZupFrame", worldFrame, ankleZUpFrames.get(RobotSide.LEFT), ankleZUpFrames.get(RobotSide.RIGHT)); midFeetZUpFrame.update(); BipedSupportPolygons bipedSupportPolygons = new BipedSupportPolygons(midFeetZUpFrame, ankleZUpFrames, registry, null); bipedSupportPolygons.updateUsingContactStates(contactStates); return bipedSupportPolygons; }
private SideDependentList<FootSpoof> setupContactableFeet(double footLength, double footWidth, double totalWidth) { SideDependentList<FootSpoof> contactableFeet = new SideDependentList<>(); for (RobotSide robotSide : RobotSide.values) { String sidePrefix = robotSide.getCamelCaseNameForStartOfExpression(); double xToAnkle = 0.0; double yToAnkle = 0.0; double zToAnkle = 0.084; List<Point2D> contactPointsInSoleFrame = new ArrayList<>(); contactPointsInSoleFrame.add(new Point2D(footLength / 2.0, footWidth / 2.0)); contactPointsInSoleFrame.add(new Point2D(footLength / 2.0, -footWidth / 2.0)); contactPointsInSoleFrame.add(new Point2D(-footLength / 2.0, -footWidth / 2.0)); contactPointsInSoleFrame.add(new Point2D(-footLength / 2.0, footWidth / 2.0)); FootSpoof contactableFoot = new FootSpoof(sidePrefix + "Foot", xToAnkle, yToAnkle, zToAnkle, contactPointsInSoleFrame, 0.0); FramePose3D startingPose = footPosesAtTouchdown.get(robotSide); startingPose.setToZero(worldFrame); startingPose.setY(robotSide.negateIfRightSide(0.5 * (totalWidth - footWidth))); contactableFoot.setSoleFrame(startingPose); contactableFeet.put(robotSide, contactableFoot); } return contactableFeet; }
FootSpoof foot = new FootSpoof(testName + side.getCamelCaseName() + "Foot", 0.0, 0.0, 0.084, contactPoints, 0.1); getFootLocationFromCoMLocation(tempFramePoint1, side, currentLocation, walkingDirectionUnitVector, stepLength, stepWidth); foot.setPose(tempFramePoint1, robotOrientation); contactableFeet.put(side, foot); soleFrames.put(side, foot.getSoleFrame()); ankleFrames.put(side, foot.getFrameAfterParentJoint()); YoPlaneContactState yoPlaneContactState = new YoPlaneContactState(side.getCamelCaseName(), foot.getRigidBody(), foot.getSoleFrame(), foot.getContactPoints2d(), foot.getCoefficientOfFriction(), testRegistry); yoPlaneContactState.setFullyConstrained(); contactStates.put(side, yoPlaneContactState);
public void testFootstepAndPointsFromDataFile() throws NumberFormatException, InsufficientDataException, IOException { QuadTreeFootstepSnappingParameters snappingParameters = new AtlasFootstepSnappingParameters(); ConvexHullFootstepSnapper footstepSnapper = new ConvexHullFootstepSnapper(new SimpleFootstepValueFunction(snappingParameters), snappingParameters); double maskSafetyBuffer = 0.01; double boundingBoxDimension = 0.3; footstepSnapper.setUseMask(true, maskSafetyBuffer, boundingBoxDimension); String baseName = "footstepListsForTesting/"; String resourceName = baseName + "DataFromConvexHullSnapper1422988400956.txt"; InputStream resourceAsStream = getClass().getClassLoader().getResourceAsStream(resourceName); FootstepPointsDataReader dataReader = new FootstepPointsDataReader(resourceAsStream); FootstepDataMessage footstepData = new FootstepDataMessage(); footstepData.setRobotSide(RobotSide.LEFT.toByte()); FootSpoof spoof = new FootSpoof("basicSpoof"); FramePose2D desiredPose = new FramePose2D(ReferenceFrame.getWorldFrame()); List<Point3D> listOfPoints = new ArrayList<>(); while (dataReader.hasAnotherFootstepAndPoints()) { listOfPoints = dataReader.getNextSetPointsAndFootstep(footstepData); desiredPose.setIncludingFrame(ReferenceFrame.getWorldFrame(), footstepData.getLocation().getX(), footstepData.getLocation().getY(), footstepData.getOrientation().getYaw()); Footstep footstep = footstepSnapper.generateFootstepUsingHeightMap(desiredPose, spoof.getRigidBody(), spoof.getSoleFrame(), RobotSide.fromByte(footstepData.getRobotSide()), listOfPoints, 0.0); assertTrue(footstep.getFootstepType() != Footstep.FootstepType.BAD_FOOTSTEP); } }
private SideDependentList<Footstep> updatedStartFeet(double startX, double startY, double startYaw) { double leftXFactor = initialDeltaFeetLocalX / 2 * Math.cos(startYaw) - initialDeltaFeetLocalY / 2 * Math.sin(startYaw); double leftYFactor = initialDeltaFeetLocalX / 2 * Math.sin(startYaw) + initialDeltaFeetLocalY / 2 * Math.cos(startYaw); QuadTreeFootstepSnapper footstepSnapper = new SimpleFootstepSnapper(); Point2D leftFootStartPoint = new Point2D(startX + leftXFactor, startY + leftYFactor); FramePose2D leftFootPose2d = new FramePose2D(WORLD_FRAME, leftFootStartPoint, startYaw + initialDeltaFeetYaw / 2); Point2D rightFootStartPoint = new Point2D(startX - leftXFactor, startY - leftYFactor); FramePose2D rightFootPose2d = new FramePose2D(WORLD_FRAME, rightFootStartPoint, startYaw - initialDeltaFeetYaw / 2); FramePoint3D leftPosition = new FramePoint3D(WORLD_FRAME, leftFootStartPoint.getX(), leftFootStartPoint.getY(), 0); FramePoint3D rightPosition = new FramePoint3D(WORLD_FRAME, rightFootStartPoint.getX(), rightFootStartPoint.getY(), 0); FrameQuaternion leftOrientation = new FrameQuaternion(WORLD_FRAME, leftFootPose2d.getYaw(), 0.0, 0.0); FrameQuaternion rightOrientation = new FrameQuaternion(WORLD_FRAME, rightFootPose2d.getYaw(), 0.0, 0.0); leftContactableFoot.setSoleFrame(leftPosition, leftOrientation); rightContactableFoot.setSoleFrame(rightPosition, rightOrientation); Footstep leftStart = footstepSnapper.generateFootstepWithoutHeightMap(leftFootPose2d, feet.get(RobotSide.LEFT), contactableFeet.get(RobotSide.LEFT).getSoleFrame(), RobotSide.LEFT, GROUND_HEIGHT, PLANE_NORMAL); Footstep rightStart = footstepSnapper.generateFootstepWithoutHeightMap(rightFootPose2d, feet.get(RobotSide.RIGHT), contactableFeet.get(RobotSide.RIGHT).getSoleFrame(), RobotSide.RIGHT, GROUND_HEIGHT, PLANE_NORMAL); SideDependentList<Footstep> startFeet = new SideDependentList<Footstep>(leftStart, rightStart); return startFeet; }
private void setupConsistencyChecks() { newTestStartConsistency = true; copWaypointsFromPreviousPlan = new ArrayList<>(); CoPPointsInFoot copPointsInFoot = new CoPPointsInFoot(testClassName, 0, new ReferenceFrame[] {worldFrame, feet.get(RobotSide.LEFT).getSoleFrame(), feet.get(RobotSide.RIGHT).getSoleFrame()}, registry); copWaypointsFromPreviousPlan.add(copPointsInFoot); icpCornerPointsFromPreviousPlan = new RecyclingArrayList<>(FramePoint3D::new); comCornerPointsFromPreviousPlan = new RecyclingArrayList<>(FramePoint3D::new); for (int i = 0; i < numberOfFootstepsToTestForConsistency; i++) { copPointsInFoot = new CoPPointsInFoot(testClassName, i + 1, new ReferenceFrame[] {worldFrame, feet.get(RobotSide.LEFT).getSoleFrame(), feet.get(RobotSide.RIGHT).getSoleFrame()}, registry); copWaypointsFromPreviousPlan.add(copPointsInFoot); } }
public void setSoleFrame(FramePoint3D position, FrameQuaternion orientation) { position.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); orientation.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); setSoleFrame(new FramePose3D(position, orientation)); }
private void testAPoint(boolean assertPositionConditions, boolean assertPointConditions) throws InsufficientDataException { FootSpoof footSpoof = new FootSpoof("footSpoof"); testAPoint(assertPositionConditions, assertPointConditions, footSpoof); }
private void assertFootstepsValid(String testDescription, Visualization visualize, Point2D startWorldPoint, double startWorldYaw, Point2D endWorldPoint, SideDependentList<Footstep> startFeet, ArrayList<Footstep> footsteps) { showFootstepsIfVisualize(testDescription, footsteps, startFeet, visualize); double semiCircleOffset = 0.0; double validSideOffset = 2 * footSide; double radianFootTwistLimit = 1.1 * Math.max(Math.abs(pathType.getTurningCloseStepAngle()), Math.abs(pathType.getTurningOpenStepAngle())); double footReachLimitMeters = 1.1 * (pathType.getStepLength() + Math.max(pathType.getStepWidth(), pathType.getTurningStepWidth())); FootstepValidityMetric footstepValidityMetric = new SemiCircularStepValidityMetric(leftContactableFoot.getRigidBody(), semiCircleOffset, radianFootTwistLimit, footReachLimitMeters, validSideOffset); assertAllStepsLevelAndZeroHeight(testDescription, footsteps, zToAnkle); assertAllStepsValid(testDescription, footsteps, startFeet, footstepValidityMetric); assertNoRepeatedSquareUpSteps(testDescription + " Shouldn't restep at end", footsteps); assertNoInitialRepeatedSteps(testDescription, footsteps, startFeet); assertLastTwoStepsCenterAroundEndPoint(testDescription, footsteps, endWorldPoint); }
private SideDependentList<FootSpoof> setupContactableFeet(double footLength, double footWidth, double totalWidth) { SideDependentList<FootSpoof> contactableFeet = new SideDependentList<>(); for (RobotSide robotSide : RobotSide.values) { String sidePrefix = robotSide.getCamelCaseNameForStartOfExpression(); double xToAnkle = 0.0; double yToAnkle = 0.0; double zToAnkle = 0.084; List<Point2D> contactPointsInSoleFrame = new ArrayList<>(); contactPointsInSoleFrame.add(new Point2D(footLength / 2.0, footWidth / 2.0)); contactPointsInSoleFrame.add(new Point2D(footLength / 2.0, -footWidth / 2.0)); contactPointsInSoleFrame.add(new Point2D(-footLength / 2.0, -footWidth / 2.0)); contactPointsInSoleFrame.add(new Point2D(-footLength / 2.0, footWidth / 2.0)); FootSpoof contactableFoot = new FootSpoof(sidePrefix + "Foot", xToAnkle, yToAnkle, zToAnkle, contactPointsInSoleFrame, 0.0); FramePose3D startingPose = footPosesAtTouchdown.get(robotSide); startingPose.setToZero(worldFrame); startingPose.setY(robotSide.negateIfRightSide(0.5 * (totalWidth - footWidth))); contactableFoot.setSoleFrame(startingPose); contactableFeet.put(robotSide, contactableFoot); } return contactableFeet; }
@ContinuousIntegrationTest(estimatedDuration = 0.1) @Test(timeout = 30000) public void testAddAndSetIncludingFrameWithFramePoint() { FramePoint3D testLocation = new FramePoint3D(footSpoof.getSoleFrame(), Math.random(), Math.random(), Math.random()); assertTrue(copPointsInFoot.isEmpty()); copPointsInFoot.addWaypoint(CoPPointName.MIDFOOT_COP, 0.2, testLocation); assertEquals(1, copPointsInFoot.getCoPPointList().size()); testLocation.changeFrame(worldFrame); EuclidFrameTestTools.assertFramePoint3DGeometricallyEquals(testLocation, copPointsInFoot.get(0).getPosition(), epsilon); assertEquals(0.2, copPointsInFoot.get(0).getTime(), epsilon); }
public void setSoleFrame(FramePoint position, FrameOrientation orientation) { position.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); orientation.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); setSoleFrame(new FramePose(ReferenceFrame.getWorldFrame(), position.getPoint(), orientation.getQuaternionCopy())); }
double footHalfWidth = 0.05; double coefficientOfFriction = 0.0; ContactablePlaneBody leftContactableFoot = new FootSpoof("leftFoot", xToAnkle, yToAnkle, zToAnkle, footForward, footBack, footHalfWidth, coefficientOfFriction); ContactablePlaneBody rightContactableFoot = new FootSpoof("rightFoot", xToAnkle, yToAnkle, zToAnkle, footForward, footBack, footHalfWidth, coefficientOfFriction);
contactPointsInSoleFrame.add(new Point2D(-footLengthForControl / 2.0, -footWidthForControl / 2.0)); contactPointsInSoleFrame.add(new Point2D(-footLengthForControl / 2.0, footWidthForControl / 2.0)); FootSpoof contactableFoot = new FootSpoof(sidePrefix + "Foot", xToAnkle, yToAnkle, zToAnkle, contactPointsInSoleFrame, 0.0); FramePose3D startingPose = new FramePose3D(); startingPose.setToZero(worldFrame); startingPose.setY(robotSide.negateIfRightSide(0.20)); contactableFoot.setSoleFrame(startingPose); contactableFeet.put(robotSide, contactableFoot); RigidBodyBasics foot = contactableFoot.getRigidBody(); ReferenceFrame soleFrame = contactableFoot.getSoleFrame(); List<FramePoint2D> contactFramePoints = contactableFoot.getContactPoints2d(); double coefficientOfFriction = contactableFoot.getCoefficientOfFriction(); YoPlaneContactState yoPlaneContactState = new YoPlaneContactState(sidePrefix + "Foot", foot, soleFrame, contactFramePoints, coefficientOfFriction, parentRegistry); yoPlaneContactState.setFullyConstrained();
private BipedSupportPolygons setupBipedSupportPolygons(SideDependentList<FootSpoof> contactableFeet, YoVariableRegistry registry) { SideDependentList<ReferenceFrame> ankleZUpFrames = new SideDependentList<>(); SideDependentList<YoPlaneContactState> contactStates = new SideDependentList<>(); for (RobotSide robotSide : RobotSide.values) { FootSpoof contactableFoot = contactableFeet.get(robotSide); ReferenceFrame ankleFrame = contactableFoot.getFrameAfterParentJoint(); ankleFrames.put(robotSide, ankleFrame); ankleZUpFrames.put(robotSide, new ZUpFrame(worldFrame, ankleFrame, robotSide.getCamelCaseNameForStartOfExpression() + "ZUp")); String sidePrefix = robotSide.getCamelCaseNameForStartOfExpression(); RigidBodyBasics foot = contactableFoot.getRigidBody(); ReferenceFrame soleFrame = contactableFoot.getSoleFrame(); List<FramePoint2D> contactFramePoints = contactableFoot.getContactPoints2d(); double coefficientOfFriction = contactableFoot.getCoefficientOfFriction(); YoPlaneContactState yoPlaneContactState = new YoPlaneContactState(sidePrefix + "Foot", foot, soleFrame, contactFramePoints, coefficientOfFriction, registry); yoPlaneContactState.setFullyConstrained(); contactStates.put(robotSide, yoPlaneContactState); } ReferenceFrame midFeetZUpFrame = new MidFrameZUpFrame("midFeetZupFrame", worldFrame, ankleZUpFrames.get(RobotSide.LEFT), ankleZUpFrames.get(RobotSide.RIGHT)); midFeetZUpFrame.update(); BipedSupportPolygons bipedSupportPolygons = new BipedSupportPolygons(midFeetZUpFrame, ankleZUpFrames, registry, null); bipedSupportPolygons.updateUsingContactStates(contactStates); return bipedSupportPolygons; }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testAddAndSetIncludingFrameWithCoPTrajectoryPoint() { CoPTrajectoryPoint testLocation1 = new CoPTrajectoryPoint("TestLocation1", "", null, framesToRegister); CoPTrajectoryPoint testLocation2 = new CoPTrajectoryPoint("TestLocation2", "", null, framesToRegister); testLocation1.changeFrame(footSpoof.getSoleFrame()); testLocation1.setPosition(new FramePoint3D(footSpoof.getSoleFrame(), Math.random(), Math.random(), Math.random())); testLocation2.changeFrame(footSpoof.getSoleFrame()); testLocation2.setPosition(new FramePoint3D(footSpoof.getSoleFrame(), Math.random(), Math.random(), Math.random())); copPointsInFoot.addWaypoint(CoPPointName.ENTRY_COP, 0.87, testLocation1); copPointsInFoot.addWaypoint(CoPPointName.MIDFOOT_COP, 0.12, testLocation2); assertEquals(2, copPointsInFoot.getCoPPointList().size()); EuclidFrameTestTools.assertFramePoint3DGeometricallyEquals(testLocation1.getPosition(), copPointsInFoot.get(0).getPosition(), epsilon); assertEquals(0.87, copPointsInFoot.get(0).getTime(), epsilon); EuclidFrameTestTools.assertFramePoint3DGeometricallyEquals(testLocation2.getPosition(), copPointsInFoot.get(1).getPosition(), epsilon); assertEquals(0.12, copPointsInFoot.get(1).getTime(), epsilon); FramePoint3D tempFramePointForTesting = new FramePoint3D(footSpoof.getSoleFrame()); copPointsInFoot.getFinalCoPPosition(tempFramePointForTesting); EuclidFrameTestTools.assertFramePoint3DGeometricallyEquals(testLocation2.getPosition(), tempFramePointForTesting, epsilon); }
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); }
double footHalfWidth = 0.05; double coefficientOfFriction = 0.0; ContactablePlaneBody leftContactableFoot = new FootSpoof("leftFoot", xToAnkle, yToAnkle, zToAnkle, footForward, footBack, footHalfWidth, coefficientOfFriction); ContactablePlaneBody rightContactableFoot = new FootSpoof("rightFoot", xToAnkle, yToAnkle, zToAnkle, footForward, footBack, footHalfWidth, coefficientOfFriction);
contactPointsInSoleFrame.add(new Point2D(-footLengthForControl / 2.0, -footWidthForControl / 2.0)); contactPointsInSoleFrame.add(new Point2D(-footLengthForControl / 2.0, footWidthForControl / 2.0)); FootSpoof contactableFoot = new FootSpoof(sidePrefix + "Foot", xToAnkle, yToAnkle, zToAnkle, contactPointsInSoleFrame, 0.0); startingPose.setY(robotSide.negateIfRightSide(0.15)); contactableFoot.setSoleFrame(startingPose); contactableFeet.put(robotSide, contactableFoot); RigidBodyBasics foot = contactableFoot.getRigidBody(); ReferenceFrame soleFrame = contactableFoot.getSoleFrame(); List<FramePoint2D> contactFramePoints = contactableFoot.getContactPoints2d(); double coefficientOfFriction = contactableFoot.getCoefficientOfFriction(); YoPlaneContactState yoPlaneContactState = new YoPlaneContactState(sidePrefix + "Foot", foot, soleFrame, contactFramePoints, coefficientOfFriction, registry); ReferenceFrame ankleFrame = contactableFoot.getFrameAfterParentJoint(); ankleFrames.put(robotSide, ankleFrame); ankleZUpFrames.put(robotSide, new ZUpFrame(worldFrame, ankleFrame, robotSide.getCamelCaseNameForStartOfExpression() + "ZUp")); soleFrames.put(robotSide, contactableFoot.getSoleFrame());