- Add the Codota plugin to your IDE and get smart completions
private void myMethod () {}
public void setSoleFrame(FramePoint3D position, FrameQuaternion orientation) { position.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); orientation.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); setSoleFrame(new FramePose3D(position, orientation)); }
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; }
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; }
public void setSoleFrame(FramePoint position, FrameOrientation orientation) { position.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); orientation.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); setSoleFrame(new FramePose(ReferenceFrame.getWorldFrame(), position.getPoint(), orientation.getQuaternionCopy())); }
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; }
startingPose.setToZero(worldFrame); startingPose.setY(robotSide.negateIfRightSide(0.20)); contactableFoot.setSoleFrame(startingPose); contactableFeet.put(robotSide, contactableFoot);
@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 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); }
startingPose.setY(robotSide.negateIfRightSide(0.15)); contactableFoot.setSoleFrame(startingPose); contactableFeet.put(robotSide, contactableFoot);
FramePose3D footPose = new FramePose3D(initialPose); footPose.appendTranslation(0.0, side.negateIfRightSide(stepWidth / 2.0), 0.0); foot.setSoleFrame(footPose);