private void checkFootMeetsAngleRequirement(Footstep stance, Footstep swingEnd) { FramePose3D stancePose = new FramePose3D(); stance.getPose(stancePose); FramePose3D swingEndPose = new FramePose3D(); swingEnd.getPose(swingEndPose); FrameOrientation2D stanceOrientation = new FrameOrientation2D(stancePose.getOrientation()); FrameOrientation2D swingOrientation = new FrameOrientation2D(swingEndPose.getOrientation()); double yawDifference = swingOrientation.difference(stanceOrientation); double allowedDifference = (Math.abs(yawDifference) - footTwistLimitInRadians) - 3e-16; if (allowedDifference > 0) valid = false; }
private void assertFootstepInAngleRange(String testDescription, Footstep footstep, double startYaw, double endYaw) { FramePose3D footPose = new FramePose3D(); footstep.getPose(footPose); FrameOrientation2D footOrientation = new FrameOrientation2D(footPose.getOrientation()); footOrientation.changeFrame(WORLD_FRAME); double footYaw = footOrientation.getYaw(); double angleBetweenStartAndEnd = Math.abs(AngleTools.computeAngleDifferenceMinusPiToPi(endYaw, startYaw)); double angleBetweenStartAndFoot = Math.abs(AngleTools.computeAngleDifferenceMinusPiToPi(footYaw, startYaw)); double angleBetweenFootAndEnd = Math.abs(AngleTools.computeAngleDifferenceMinusPiToPi(endYaw, footYaw)); //if start->foot + foot-> end > start->end, then the foot angle is outside the range between the two boolean outsideFan = (angleBetweenStartAndFoot + angleBetweenFootAndEnd) > angleBetweenStartAndEnd + 1e-14; }
private void assertNoRepeatedSquareUpSteps(String message, List<Footstep> footsteps) { int numFootsteps = footsteps.size(); if (numFootsteps < 4) // True step in place should only have two footsteps and that should be OK. return; // Assuming last steps are squared up: shouldn't match steps two before. for (int i = numFootsteps - 1; i > numFootsteps - 3; i--) { Footstep lastFootstep = footsteps.get(i); Footstep compareToLastFootstep = footsteps.get(i - 2); FramePose3D lastFootstepPose = new FramePose3D(); lastFootstep.getPose(lastFootstepPose); FramePose3D compareToLastFootstepPose = new FramePose3D(); compareToLastFootstep.getPose(compareToLastFootstepPose); FramePoint2D lastFootstepPosition2d = new FramePoint2D(lastFootstepPose.getPosition()); FramePoint2D compareToLastFootstepPosition2d = new FramePoint2D(compareToLastFootstepPose.getPosition()); double eps = 1e-14; assertTrue( message + " step " + i + " and " + (i - 2), (compareToLastFootstepPosition2d.distance(lastFootstepPosition2d) > eps) || (Math.abs(lastFootstepPose.getYaw() - compareToLastFootstepPose.getYaw()) > eps)); } }
@Override public boolean getUpcomingFootstepSolution(Footstep footstepToPack) { if (icpOptimizationController.getNumberOfFootstepsToConsider() > 0) { footstepToPack.getPose(footstepPose); icpOptimizationController.getFootstepSolution(0, footstepPositionSolution); footstepPose.setXYFromPosition2d(footstepPositionSolution); footstepToPack.setPose(footstepPose); } return icpOptimizationController.wasFootstepAdjusted(); }
private FootstepDataListMessage createFootstepDataList(ArrayList<Footstep> desiredFootsteps) { FootstepDataListMessage ret = new FootstepDataListMessage(); for (int i = 0; i < desiredFootsteps.size(); i++) { Footstep footstep = desiredFootsteps.get(i); FramePoint3D position = new FramePoint3D(); FrameQuaternion orientation = new FrameQuaternion(); footstep.getPose(position, orientation); RobotSide footstepSide = footstep.getRobotSide(); FootstepDataMessage footstepData = HumanoidMessageTools.createFootstepDataMessage(footstepSide, position, orientation); ret.getFootstepDataList().add().set(footstepData); } return ret; }
private void assertNoCourseRepeatedSquareUpSteps(String message, ArrayList<Footstep> footstepsOriginal, double startX, double startY, double startYaw, double eps, int shortSteps) { SideDependentList<Footstep> startFeet = updatedStartFeet(startX, startY, startYaw); ArrayList<Footstep> footsteps = prependStanceToFootstepQueue(footstepsOriginal, startFeet); int numFootsteps = footsteps.size(); for (int i = 0; i + 2 < numFootsteps; i++) { Footstep firstFootstep = footsteps.get(i); Footstep nextFootstep = footsteps.get(i + 2); FramePose3D firstFootstepPose = new FramePose3D(); firstFootstep.getPose(firstFootstepPose); FramePose3D nextFootstepPose = new FramePose3D(); nextFootstep.getPose(nextFootstepPose); FramePoint2D firstFootstepPosition2d = new FramePoint2D(firstFootstepPose.getPosition()); FramePoint2D nextFootstepPosition2d = new FramePoint2D(nextFootstepPose.getPosition()); double testeps = eps; String message2 = message + "course restep test " + i + "to" + (i + 2); boolean condition = (nextFootstepPosition2d.distance(firstFootstepPosition2d) > testeps) || (Math.abs(firstFootstepPose.getYaw() - nextFootstepPose.getYaw()) > testeps); Visualization visualize = (condition == true) ? Visualization.NO_VISUALIZATION : Visualization.VISUALIZE; showFootstepsIfVisualize(message2, footstepsOriginal, startFeet, visualize); assertTrue(message2, condition); } }
private FrameVector3D offCenterVectorToSwingEnd(Footstep stance, Footstep swingEnd, FrameVector3D translationFromFootCenterToCircleCenter) { FramePoint3D circleCenter; FramePose3D stanceSole = new FramePose3D(); FramePose3D swingEndSole = new FramePose3D(); stance.getPose(stanceSole); swingEnd.getPose(swingEndSole); circleCenter = new FramePoint3D(stanceSole.getPosition()); circleCenter.add(translationFromFootCenterToCircleCenter);//change frame of translation first? FrameVector3D vectorToSwingEnd = new FrameVector3D(ReferenceFrame.getWorldFrame()); vectorToSwingEnd.set(swingEndSole.getPosition()); vectorToSwingEnd.sub(circleCenter); return vectorToSwingEnd; }
public void set(ArrayList<Footstep> footsteps, double swingTime, double transferTime) { FootstepDataListMessage footstepDataList = HumanoidMessageTools.createFootstepDataListMessage(swingTime, transferTime); for (int i = 0; i < footsteps.size(); i++) { Footstep footstep = footsteps.get(i); FramePoint3D position = new FramePoint3D(); FrameQuaternion orientation = new FrameQuaternion(); footstep.getPose(position, orientation); RobotSide footstepSide = footstep.getRobotSide(); FootstepDataMessage footstepData = HumanoidMessageTools.createFootstepDataMessage(footstepSide, position, orientation); footstepDataList.getFootstepDataList().add().set(footstepData); } set(footstepDataList); }
public static AdjustFootstepMessage createAdjustFootstepMessage(Footstep footstep) { AdjustFootstepMessage message = new AdjustFootstepMessage(); message.setRobotSide(footstep.getRobotSide().toByte()); FramePoint3D location = new FramePoint3D(); FrameQuaternion orientation = new FrameQuaternion(); footstep.getPose(location, orientation); footstep.getFootstepPose().checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); message.getLocation().set(location); message.getOrientation().set(orientation); MessageTools.copyData(footstep.getPredictedContactPoints().stream().map(Point3D::new).collect(Collectors.toList()), message.getPredictedContactPoints2d()); return message; }
footstep.getPose(footstepPose);
private static void assertStepIsPointingCorrectly(String message, Footstep footstep, FrameOrientation2D endOrientation) { FramePose3D footPose = new FramePose3D(); footstep.getPose(footPose); // System.out.println(Math.toDegrees(endOrientation.getYaw()) + "?=" + Math.toDegrees(footPose.getYaw())); // System.out.println(Math.toDegrees(footPose.getRoll()) + "?=" + Math.toDegrees(footPose.getPitch()) + "?=" + 0.0); assertEquals(message + " Foot roll should be 0.", 0.0, footPose.getRoll(), 1e-10); assertEquals(message + " Foot pitch should be 0.", 0.0, footPose.getPitch(), 1e-10); FrameOrientation2D footOrientation = new FrameOrientation2D(footPose.getOrientation()); double yawDiff = endOrientation.difference(footOrientation); assertEquals(message + " Foot yaw and desired yaw difference should be 0.", 0.0, yawDiff, 1e-10); }
private int checkForEndingOfAdjustment(double omega0) { int numberOfFootstepsToConsider = clipNumberOfFootstepsToConsiderToProblem(this.numberOfFootstepsToConsider.getIntegerValue()); if (timeRemainingInState.getDoubleValue() < remainingTimeToStopAdjusting.getDoubleValue() && localUseStepAdjustment && !isStanding.getBooleanValue() && !isInTransfer.getBooleanValue()) { //record current locations for (int i = 0; i < numberOfFootstepsToConsider; i++) { upcomingFootstepLocations.get(i).set(footstepSolutions.get(i)); Footstep footstep = upcomingFootsteps.get(i); footstep.getPose(footstepPose); footstepPose.setXYFromPosition2d(footstepSolutions.get(i).getFrameTuple2d()); footstep.setPose(footstepPose); inputHandler.addFootstepToPlan(footstep); } localUseStepAdjustment = false; numberOfFootstepsToConsider = clipNumberOfFootstepsToConsiderToProblem(this.numberOfFootstepsToConsider.getIntegerValue()); footstepRecursionMultiplierCalculator.resetTimes(); footstepRecursionMultiplierCalculator.submitTimes(0, 0.0, singleSupportDuration.getDoubleValue()); for (int i = 1; i < numberOfFootstepsToConsider + 1; i++) footstepRecursionMultiplierCalculator.submitTimes(i, doubleSupportDuration.getDoubleValue(), singleSupportDuration.getDoubleValue()); footstepRecursionMultiplierCalculator.submitTimes(numberOfFootstepsToConsider + 1, doubleSupportDuration.getDoubleValue(), singleSupportDuration.getDoubleValue()); footstepRecursionMultiplierCalculator.computeRecursionMultipliers(numberOfFootstepsToConsider, isInTransfer.getBooleanValue(), localUseTwoCMPs, omega0); } return numberOfFootstepsToConsider; }
private void calculateInitialFootPoseAndOffsets() { SideDependentList<Footstep> currentFootsteps; if (priorStanceFeetSpecified) currentFootsteps = priorStanceFeet; else currentFootsteps = createFootstepsFromBipedFeet(); Footstep left = currentFootsteps.get(RobotSide.LEFT); Footstep right = currentFootsteps.get(RobotSide.RIGHT); FramePose3D leftPose = new FramePose3D(); FramePose3D rightPose = new FramePose3D(); left.getPose(leftPose); right.getPose(rightPose); FramePose2D leftPose2d = new FramePose2D(leftPose); FramePose2D rightPose2d = new FramePose2D(rightPose); startPose.interpolate(leftPose2d, rightPose2d, 0.5); Pose2dReferenceFrame startFramePose = new Pose2dReferenceFrame("StartPoseFrame", startPose); leftPose.changeFrame(startFramePose); rightPose.changeFrame(startFramePose); FrameOrientation2D leftOrientation = new FrameOrientation2D(leftPose.getOrientation()); FrameOrientation2D rightOrientation = new FrameOrientation2D(rightPose.getOrientation()); initialDeltaFeetYaw = leftOrientation.difference(rightOrientation); initialDeltaFeetY = leftPose.getY() - rightPose.getY(); initialDeltaFeetX = leftPose.getX() - rightPose.getX(); }
@Override public Footstep.FootstepType snapFootstep(Footstep footstep, HeightMapWithPoints heightMap) { // can only snap footsteps in world frame footstep.getFootstepPose().checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); FootstepDataMessage originalFootstep = HumanoidMessageTools.createFootstepDataMessage(footstep); //set to the sole pose FramePoint3D position = new FramePoint3D(); FrameQuaternion orientation = new FrameQuaternion(); footstep.getPose(position, orientation); originalFootstep.getLocation().set(position); originalFootstep.getOrientation().set(orientation); //get the footstep Footstep.FootstepType type = snapFootstep(originalFootstep, heightMap); if (type == Footstep.FootstepType.FULL_FOOTSTEP && originalFootstep.getPredictedContactPoints2d().size() > 0){ throw new RuntimeException(this.getClass().getSimpleName() + "Full Footstep should have null contact points"); } footstep.setPredictedContactPoints(HumanoidMessageTools.unpackPredictedContactPoints(originalFootstep)); footstep.setFootstepType(type); FramePose3D solePoseInWorld = new FramePose3D(ReferenceFrame.getWorldFrame(), originalFootstep.getLocation(), originalFootstep.getOrientation()); footstep.setPose(solePoseInWorld); footstep.setSwingHeight(originalFootstep.getSwingHeight()); footstep.setTrajectoryType(TrajectoryType.fromByte(originalFootstep.getTrajectoryType())); return type; }
@Override public Footstep.FootstepType snapFootstep(Footstep footstep, HeightMapWithPoints heightMap) { // can only snap footsteps in world frame footstep.getFootstepPose().checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); FootstepDataMessage originalFootstep = HumanoidMessageTools.createFootstepDataMessage(footstep); //set to the sole pose FramePoint3D position = new FramePoint3D(); FrameQuaternion orientation = new FrameQuaternion(); footstep.getPose(position, orientation); originalFootstep.getLocation().set(position); originalFootstep.getOrientation().set(orientation); //get the footstep Footstep.FootstepType type = snapFootstep(originalFootstep, heightMap); if (type == Footstep.FootstepType.FULL_FOOTSTEP && originalFootstep.getPredictedContactPoints2d().size() > 0 ) { throw new RuntimeException(this.getClass().getSimpleName() + "Full Footstep should have null contact points"); } footstep.setPredictedContactPoints(HumanoidMessageTools.unpackPredictedContactPoints(originalFootstep)); footstep.setFootstepType(type); FramePose3D solePoseInWorld = new FramePose3D(ReferenceFrame.getWorldFrame(), originalFootstep.getLocation(), originalFootstep.getOrientation()); footstep.setPose(solePoseInWorld); footstep.setSwingHeight(originalFootstep.getSwingHeight()); footstep.setTrajectoryType(TrajectoryType.fromByte(originalFootstep.getTrajectoryType())); return type; }
@Override public void adjustFootstepWithoutHeightmap(Footstep footstep, double height, Vector3D planeNormal) { // can only snap footsteps in world frame footstep.getFootstepPose().checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); FramePose3D footstepPose = new FramePose3D(); footstep.getPose(footstepPose); Point3D position = new Point3D(footstepPose.getPosition()); double yaw = footstep.getFootstepPose().getYaw(); Quaternion orientation = new Quaternion(); RotationTools.computeQuaternionFromYawAndZNormal(yaw, planeNormal, orientation); position.setZ(height); footstepPose.set(new Point3D(position), orientation); footstep.setPose(footstepPose); }
public static FootstepDataMessage createFootstepDataMessage(Footstep footstep) { FootstepDataMessage message = new FootstepDataMessage(); message.setRobotSide(footstep.getRobotSide().toByte()); FramePoint3D location = new FramePoint3D(); FrameQuaternion orientation = new FrameQuaternion(); footstep.getPose(location, orientation); footstep.getFootstepPose().checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); message.getLocation().set(location); message.getOrientation().set(orientation); packPredictedContactPoints(footstep.getPredictedContactPoints(), message); message.setTrajectoryType(footstep.getTrajectoryType().toByte()); message.setSwingHeight(footstep.getSwingHeight()); message.setSwingTrajectoryBlendDuration(footstep.getSwingTrajectoryBlendDuration()); if (footstep.getCustomPositionWaypoints().size() != 0) { for (int i = 0; i < footstep.getCustomPositionWaypoints().size(); i++) { FramePoint3D framePoint = footstep.getCustomPositionWaypoints().get(i); framePoint.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); message.getCustomPositionWaypoints().add().set(framePoint); } } return message; }
@Override public Footstep.FootstepType snapFootstep(Footstep footstep, HeightMapWithPoints heightMap) { // can only snap footsteps in world frame footstep.getFootstepPose().checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); FootstepDataMessage originalFootstep = HumanoidMessageTools.createFootstepDataMessage(footstep); //set to the sole pose FramePoint3D position = new FramePoint3D(); FrameQuaternion orientation = new FrameQuaternion(); footstep.getPose(position, orientation); originalFootstep.getLocation().set(position); originalFootstep.getOrientation().set(orientation); //get the footstep Footstep.FootstepType type = snapFootstep(originalFootstep, heightMap); footstep.setPredictedContactPoints(HumanoidMessageTools.unpackPredictedContactPoints(originalFootstep)); footstep.setFootstepType(type); FramePose3D solePoseInWorld = new FramePose3D(ReferenceFrame.getWorldFrame(), originalFootstep.getLocation(), originalFootstep.getOrientation()); footstep.setPose(solePoseInWorld); footstep.setSwingHeight(originalFootstep.getSwingHeight()); footstep.setTrajectoryType(TrajectoryType.fromByte(originalFootstep.getTrajectoryType())); return type; }
public Footstep.FootstepType snapFootstep(Footstep footstep, List<Point3D> pointList, double defaultHeight) { // can only snap footsteps in world frame footstep.getFootstepPose().checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); FootstepDataMessage originalFootstep = HumanoidMessageTools.createFootstepDataMessage(footstep); //set to the sole pose FramePoint3D position = new FramePoint3D(); FrameQuaternion orientation = new FrameQuaternion(); footstep.getPose(position, orientation); originalFootstep.getLocation().set(position); originalFootstep.getOrientation().set(orientation); //get the footstep Footstep.FootstepType type = snapFootstep(originalFootstep, pointList, defaultHeight); footstep.setFootstepType(type); footstep.setPredictedContactPoints(HumanoidMessageTools.unpackPredictedContactPoints(originalFootstep)); FramePose3D solePoseInWorld = new FramePose3D(ReferenceFrame.getWorldFrame(), originalFootstep.getLocation(), originalFootstep.getOrientation()); footstep.setPose(solePoseInWorld); footstep.setSwingHeight(originalFootstep.getSwingHeight()); footstep.setTrajectoryType(TrajectoryType.fromByte(originalFootstep.getTrajectoryType())); return type; }