public List<Footstep> createFootsteps(double stepWidth, double stepLength, int numberOfSteps) { final ArrayList<Footstep> footsteps = new ArrayList<>(); RobotSide currentSide = RobotSide.LEFT; FramePoint lastFootstepPosition = new FramePoint(ankleFrames.get(currentSide)); lastFootstepPosition.changeFrame(worldFrame); for (int i = 0; i < numberOfSteps; i++) { currentSide = currentSide.getOppositeSide(); double x = lastFootstepPosition.getX(); if (i < numberOfSteps - 1) x += stepLength; double y = lastFootstepPosition.getY() + currentSide.negateIfRightSide(stepWidth); Footstep footstep = createFootstep(currentSide, x, y); footstep.getPositionIncludingFrame(lastFootstepPosition); lastFootstepPosition.changeFrame(worldFrame); footsteps.add(footstep); } return footsteps; }
public static RobotSide getFrontFootRobotSideFromFootsteps(SideDependentList<Footstep> footSteps, FramePoint destination) { FramePoint destinationInWorld = new FramePoint(destination); destinationInWorld.changeFrame(worldFrame); SideDependentList<Double> distances = new SideDependentList<Double>(); for (RobotSide side : RobotSide.values) { FramePoint footstepPosition = new FramePoint(); footSteps.get(side).getPositionIncludingFrame(footstepPosition); footstepPosition.changeFrame(worldFrame); distances.set(side, new Double(footstepPosition.distanceSquared(destinationInWorld))); } RobotSide frontSide = (distances.get(RobotSide.LEFT) <= distances.get(RobotSide.RIGHT)) ? RobotSide.LEFT : RobotSide.RIGHT; return frontSide; }
/** * This function takes a footstep and calculates the touch-down polygon in the * desired reference frame */ private void calculateTouchdownFootPolygon(Footstep footstep, ReferenceFrame desiredFrame, FrameConvexPolygon2d polygonToPack) { footstep.getPositionIncludingFrame(centroid3d); centroid3d.getFramePoint2d(centroid2d); centroid2d.changeFrame(desiredFrame); polygonToPack.setIncludingFrameAndUpdate(footstep.getSoleReferenceFrame(), defaultSupportPolygons.get(footstep.getRobotSide())); polygonToPack.changeFrameAndProjectToXYPlane(desiredFrame); // shrink the polygon for safety by pulling all the corner points towards the center polygonToPack.scale(centroid2d, SHRINK_TOUCHDOWN_POLYGON_FACTOR); }
public void setWithUpcomingFootstep(Footstep upcomingFootstep) { RobotSide upcomingFootstepSide = upcomingFootstep.getRobotSide(); desiredPelvisOrientation.getFrameOrientationIncludingFrame(tempOrientation); initialPelvisOrientation.set(tempOrientation); upcomingFootstep.getOrientationIncludingFrame(upcomingFootstepOrientation); upcomingFootstepOrientation.changeFrame(worldFrame); tempOrientation.setToZero(ankleZUpFrames.get(upcomingFootstepSide.getOppositeSide())); tempOrientation.changeFrame(worldFrame); double finalDesiredPelvisYawAngle = AngleTools.computeAngleAverage(upcomingFootstepOrientation.getYaw(), tempOrientation.getYaw()); upcomingFootstep.getPositionIncludingFrame(upcomingFootstepLocation); upcomingFootstepLocation.changeFrame(ankleZUpFrames.get(upcomingFootstepSide.getOppositeSide())); double desiredSwingPelvisYawAngle = 0.0; if (Math.abs(upcomingFootstepLocation.getX()) > 0.1) { desiredSwingPelvisYawAngle = Math.atan2(upcomingFootstepLocation.getY(), upcomingFootstepLocation.getX()); desiredSwingPelvisYawAngle -= upcomingFootstepSide.negateIfRightSide(Math.PI / 2.0); } swingPelvisYaw.set(desiredSwingPelvisYawAngle); finalPelvisOrientation.set(finalDesiredPelvisYawAngle + swingPelvisYawScale.getDoubleValue() * desiredSwingPelvisYawAngle, 0.0, 0.0); initialize(worldFrame); }