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); } }
@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); }
@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); }
@ContinuousIntegrationTest(estimatedDuration = 0.1) @Test(timeout = 30000) public void testAddAndSetIncludingFrameWithYoFramePoint() { YoFramePoint3D testLocation1 = new YoFramePoint3D("TestLocation1", footSpoof.getSoleFrame(), null); YoFramePoint3D testLocation2 = new YoFramePoint3D("TestLocation2", footSpoof.getSoleFrame(), null); testLocation1.set(Math.random(), Math.random(), Math.random()); testLocation2.set(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()); FramePoint3D pointToTest = new FramePoint3D(testLocation1); pointToTest.changeFrame(worldFrame); EuclidFrameTestTools.assertFramePoint3DGeometricallyEquals(pointToTest, copPointsInFoot.get(0).getPosition(), epsilon); assertEquals(0.87, copPointsInFoot.get(0).getTime(), epsilon); pointToTest = new FramePoint3D(testLocation2); pointToTest.changeFrame(worldFrame); EuclidFrameTestTools.assertFramePoint3DGeometricallyEquals(pointToTest, copPointsInFoot.get(1).getPosition(), epsilon); assertEquals(0.12, copPointsInFoot.get(1).getTime(), epsilon); }
private void updateCurrentFootsteps() { for (RobotSide side : RobotSide.values) { YoFramePoseUsingYawPitchRoll footPose = currentFootLocations.get(side); if (contactStates.get(side).inContact()) { footPose.setFromReferenceFrame(feet.get(side).getSoleFrame()); } else { footPose.setToNaN(); } } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testGetToeOffContactPoint() { RobotSide trailingSide = RobotSide.LEFT; FramePoint3D exitCMP = new FramePoint3D(); FramePoint2D desiredCMP = new FramePoint2D(); FramePoint2D toeOffPoint = new FramePoint2D(); exitCMP.setToZero(contactableFeet.get(trailingSide).getSoleFrame()); desiredCMP.setToZero(contactableFeet.get(trailingSide).getSoleFrame()); toeOffPoint.setToZero(contactableFeet.get(trailingSide).getSoleFrame()); exitCMP.setX(0.05); desiredCMP.setX(0.05); generator = new WrapperForMultipleToeOffCalculators(toeOffCalculators, parentRegistry); generator.setExitCMP(exitCMP, trailingSide); generator.computeToeOffContactPoint(desiredCMP, trailingSide); generator.getToeOffContactPoint(toeOffPoint, trailingSide); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testGetToeOffContactPoint() { RobotSide trailingSide = RobotSide.LEFT; FramePoint3D exitCMP = new FramePoint3D(); FramePoint2D desiredCMP = new FramePoint2D(); FramePoint2D toeOffPoint = new FramePoint2D(); exitCMP.setToZero(contactableFeet.get(trailingSide).getSoleFrame()); desiredCMP.setToZero(contactableFeet.get(trailingSide).getSoleFrame()); toeOffPoint.setToZero(contactableFeet.get(trailingSide).getSoleFrame()); exitCMP.setX(0.05); desiredCMP.setX(0.05); toeOffCalculator = new CentroidProjectionToeOffCalculator(contactStates, contactableFeet, getToeOffParameters(), parentRegistry); toeOffCalculator.setExitCMP(exitCMP, trailingSide); toeOffCalculator.computeToeOffContactPoint(desiredCMP, trailingSide); toeOffCalculator.getToeOffContactPoint(toeOffPoint, trailingSide); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testComputeToeOffContactPoint() { RobotSide trailingSide = RobotSide.LEFT; FramePoint3D exitCMP = new FramePoint3D(); FramePoint2D desiredCMP = new FramePoint2D(); exitCMP.setToZero(contactableFeet.get(trailingSide).getSoleFrame()); desiredCMP.setToZero(contactableFeet.get(trailingSide).getSoleFrame()); exitCMP.setX(0.05); desiredCMP.setX(0.05); generator = new WrapperForMultipleToeOffCalculators(toeOffCalculators, parentRegistry); generator.setExitCMP(exitCMP, trailingSide); generator.computeToeOffContactPoint(desiredCMP, trailingSide); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testComputeToeOffContactPoint() { RobotSide trailingSide = RobotSide.LEFT; FramePoint3D exitCMP = new FramePoint3D(); FramePoint2D desiredCMP = new FramePoint2D(); exitCMP.setToZero(contactableFeet.get(trailingSide).getSoleFrame()); desiredCMP.setToZero(contactableFeet.get(trailingSide).getSoleFrame()); exitCMP.setX(0.05); desiredCMP.setX(0.05); toeOffCalculator = new CentroidProjectionToeOffCalculator(contactStates, contactableFeet, getToeOffParameters(), parentRegistry); toeOffCalculator.setExitCMP(exitCMP, trailingSide); toeOffCalculator.computeToeOffContactPoint(desiredCMP, trailingSide); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testSetExitCMP() { RobotSide trailingSide = RobotSide.LEFT; FramePoint3D exitCMP = new FramePoint3D(); exitCMP.setToZero(contactableFeet.get(trailingSide).getSoleFrame()); exitCMP.setX(0.05); generator = new WrapperForMultipleToeOffCalculators(toeOffCalculators, parentRegistry); generator.setExitCMP(exitCMP, trailingSide); }
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; }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testSetExitCMP() { RobotSide trailingSide = RobotSide.LEFT; FramePoint3D exitCMP = new FramePoint3D(); exitCMP.setToZero(contactableFeet.get(trailingSide).getSoleFrame()); exitCMP.setX(0.05); toeOffCalculator = new CentroidProjectionToeOffCalculator(contactStates, contactableFeet, getToeOffParameters(), parentRegistry); toeOffCalculator.setExitCMP(exitCMP, trailingSide); }
@ContinuousIntegrationTest(estimatedDuration = 0.1) @Test(timeout = 30000) public void testVisualization() { YoGraphicsList dummyGraphicsList = new YoGraphicsList("DummyGraphics"); ArtifactList dummyArtifactList = new ArtifactList("DummyArtifacts"); copPointsInFoot.setupVisualizers(dummyGraphicsList, dummyArtifactList, 0.05); assertEquals(dummyArtifactList.getArtifacts().size(), 10); assertEquals(dummyGraphicsList.getYoGraphics().size(), 10); copPointsInFoot.addWaypoint(CoPPointName.MIDFOOT_COP, 1.0, new FramePoint3D(footSpoof.getSoleFrame(), 1.0, 2.1, 3.1)); YoGraphicPosition graphic = (YoGraphicPosition) dummyGraphicsList.getYoGraphics().get(0); assertEquals(1.0 - xToAnkle, graphic.getX(), 1e-5); assertEquals(2.1 - yToAnkle, graphic.getY(), 1e-5); assertEquals(3.1 - zToAnkle, graphic.getZ(), 1e-5); }
ReferenceFrame soleFrame = contactableFoot.getSoleFrame(); List<FramePoint2D> contactFramePoints = contactableFoot.getContactPoints2d(); double coefficientOfFriction = contactableFoot.getCoefficientOfFriction();
@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); } }
FootSpoof contactableFoot = contactableFeet.get(robotSide); RigidBodyBasics foot = contactableFoot.getRigidBody(); ReferenceFrame soleFrame = contactableFoot.getSoleFrame(); List<FramePoint2D> contactFramePoints = contactableFoot.getContactPoints2d(); double coefficientOfFriction = contactableFoot.getCoefficientOfFriction(); ankleFrames.put(robotSide, ankleFrame); ankleZUpFrames.put(robotSide, new ZUpFrame(worldFrame, ankleFrame, robotSide.getCamelCaseNameForStartOfExpression() + "ZUp")); soleFrames.put(robotSide, contactableFoot.getSoleFrame());
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); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testChangeFrame() { copPointsInFoot.setFeetLocation(new FramePoint3D(worldFrame, 0.2, 0.1, 0.1), new FramePoint3D(worldFrame, 0.2, -0.1, 0.1)); copPointsInFoot.addWaypoint(CoPPointName.MIDFOOT_COP, 0.2, new FramePoint3D(worldFrame, 0.2, 0.15, 0.1)); copPointsInFoot.addWaypoint(CoPPointName.ENTRY_COP, 0.8, new FramePoint3D(worldFrame, 0.15, -0.05, 0.11)); copPointsInFoot.changeFrame(footSpoof.getSoleFrame()); FramePoint3D tempFramePoint = new FramePoint3D(); copPointsInFoot.getSupportFootLocation(tempFramePoint); assertEquals(tempFramePoint.getReferenceFrame(), footSpoof.getSoleFrame()); assertEquals(tempFramePoint.getX(), 0.2 + xToAnkle, epsilon); assertEquals(tempFramePoint.getY(), -0.1 + yToAnkle, epsilon); assertEquals(tempFramePoint.getZ(), 0.1 + zToAnkle, epsilon); assertEquals(copPointsInFoot.get(0).getReferenceFrame(), footSpoof.getSoleFrame()); copPointsInFoot.get(0).getPosition(tempFramePoint); assertEquals(tempFramePoint.getX(), 0.2 + xToAnkle, epsilon); assertEquals(tempFramePoint.getY(), 0.15 + yToAnkle, epsilon); assertEquals(tempFramePoint.getZ(), 0.1 + zToAnkle, epsilon); assertEquals(copPointsInFoot.get(1).getReferenceFrame(), footSpoof.getSoleFrame()); copPointsInFoot.get(1).getPosition(tempFramePoint); assertEquals(tempFramePoint.getX(), 0.15 + xToAnkle, epsilon); assertEquals(tempFramePoint.getY(), -0.05 + yToAnkle, epsilon); assertEquals(tempFramePoint.getZ(), 0.11 + zToAnkle, epsilon); }
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 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; }