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); }
contactableFeet.put(robotSide, contactableFoot); RigidBodyBasics foot = contactableFoot.getRigidBody(); ReferenceFrame soleFrame = contactableFoot.getSoleFrame(); List<FramePoint2D> contactFramePoints = contactableFoot.getContactPoints2d();
@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); } }
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); } }
RigidBodyBasics foot = contactableFoot.getRigidBody(); ReferenceFrame soleFrame = contactableFoot.getSoleFrame(); List<FramePoint2D> contactFramePoints = contactableFoot.getContactPoints2d();
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();
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; }
this.ankleZUpFrames.put(side, new ZUpFrame(worldFrame, foot.getFrameAfterParentJoint(), footName + "AnkleZUpFrame")); this.soleZUpFrames.put(side, new ZUpFrame(worldFrame, foot.getSoleFrame(), footName + "SoleZUpFrame")); YoPlaneContactState contactState = new YoPlaneContactState(footName + "ContactState", foot.getRigidBody(), foot.getSoleFrame(), foot.getContactPoints2d(), foot.getCoefficientOfFriction(), registry); contactState.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; }