List<FramePoint2D> contactFramePoints = contactableFoot.getContactPoints2d(); double coefficientOfFriction = contactableFoot.getCoefficientOfFriction(); YoPlaneContactState yoPlaneContactState = new YoPlaneContactState(sidePrefix + "Foot", foot, soleFrame, contactFramePoints, coefficientOfFriction, parentRegistry);
@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); } }
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,
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);
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; }
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(); this.contactStates.put(side, contactState);