ReferenceFrame ankleFrame = contactableFoot.getFrameAfterParentJoint(); ankleFrames.put(robotSide, ankleFrame); ankleZUpFrames.put(robotSide, new ZUpFrame(worldFrame, ankleFrame, robotSide.getCamelCaseNameForStartOfExpression() + "ZUp"));
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);
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.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(),