private ArrayList<Footstep> concatenateFootstepPaths(ArrayList<Footstep> firstSetOfSteps, ArrayList<Footstep> secondSetOfSteps) { int indexOfLastStepOfFirstSegment = firstSetOfSteps.size() - 1; RobotSide sideOfLastFootstepOfFirstSegment = firstSetOfSteps.get(indexOfLastStepOfFirstSegment).getRobotSide(); RobotSide sideOfFirstFootstepOfSecondSegment = secondSetOfSteps.get(0).getRobotSide(); if (sideOfLastFootstepOfFirstSegment == sideOfFirstFootstepOfSecondSegment) firstSetOfSteps.remove(indexOfLastStepOfFirstSegment); for (Footstep footstep : secondSetOfSteps) { firstSetOfSteps.add(footstep); } return firstSetOfSteps; }
private ArrayList<Footstep> concatenateFootstepPaths(ArrayList<Footstep> firstSetOfSteps, ArrayList<Footstep> secondSetOfSteps) { int indexOfLastStepOfFirstSegment = firstSetOfSteps.size() - 1; RobotSide sideOfLastFootstepOfFirstSegment = firstSetOfSteps.get(indexOfLastStepOfFirstSegment).getRobotSide(); RobotSide sideOfFirstFootstepOfSecondSegment = secondSetOfSteps.get(0).getRobotSide(); if (sideOfLastFootstepOfFirstSegment == sideOfFirstFootstepOfSecondSegment) firstSetOfSteps.remove(indexOfLastStepOfFirstSegment); for (Footstep footstep : secondSetOfSteps) { firstSetOfSteps.add(footstep); } return firstSetOfSteps; }
private void checkStartStanceAndEndAlternateSides(String message, Footstep swingStart, Footstep stance, Footstep swingEnd, boolean assertValidity) { RobotSide swingStartSide = swingStart.getRobotSide(); RobotSide stanceSide = stance.getRobotSide(); RobotSide swingEndSide = swingEnd.getRobotSide(); valid &= swingStartSide == swingEndSide; assertValidIfTrue(message + " swing step start and end must be same side", assertValidity); valid &= stanceSide == swingEndSide.getOppositeSide(); assertValidIfTrue(message + " start and stance must be different", assertValidity); }
private void assertStepSide(String message, Footstep footstep, RobotSide expectedSide) { if (expectedSide != null) assertTrue(message + " first step should be " + expectedSide, expectedSide == footstep.getRobotSide()); }
private void setMidstep(Footstep footstep) { midStanceFeet.set(footstep.getRobotSide(), footstep); }
private void setTransitionStep(Footstep footstep, SideDependentList<Footstep> transitionStanceFeet) { transitionStanceFeet.set(footstep.getRobotSide(), footstep); }
private void setTransitionStep(Footstep footstep, SideDependentList<Footstep> transitionStanceFeet) { transitionStanceFeet.set(footstep.getRobotSide(), footstep); }
private void setMidstep(Footstep footstep) { midStanceFeet.set(footstep.getRobotSide(), footstep); }
private ArrayList<Footstep> prependStanceToFootstepQueue(ArrayList<Footstep> footSteps, SideDependentList<Footstep> currentFootLocations) { RobotSide firstStepSide = footSteps.get(0).getRobotSide(); ArrayList<Footstep> footstepQueue = new ArrayList<Footstep>(); footstepQueue.add(currentFootLocations.get(firstStepSide)); footstepQueue.add(currentFootLocations.get(firstStepSide.getOppositeSide())); footstepQueue.addAll(footSteps); return footstepQueue; }
private void updateVisualization() { for (int i = 0; i < upcomingFootsteps.size(); i++) { if (i < numberOfFootstepsToVisualize) upcomingFoostepSide[i].set(upcomingFootsteps.get(i).getRobotSide()); } for (int i = upcomingFootsteps.size(); i < numberOfFootstepsToVisualize; i++) { upcomingFoostepSide[i].set(null); } footstepListVisualizer.update(upcomingFootsteps); }
public void visualizeFootsteps(SideDependentList<? extends ContactablePlaneBody> bipedFeet, List<Footstep> footsteps) { for (Footstep footstep : footsteps) { RobotSide robotSide = footstep.getRobotSide(); visualizeFootstep(bipedFeet.get(robotSide), footstep); } }
public boolean isNextFootstepFor(RobotSide swingSide) { if (!hasUpcomingFootsteps()) return false; else return peek(0).getRobotSide() == swingSide; }
public void visualizeFootsteps(SideDependentList<? extends ContactablePlaneBody> bipedFeet, List<Footstep> footsteps) { for (Footstep footstep : footsteps) { RobotSide robotSide = footstep.getRobotSide(); visualizeFootstep(bipedFeet.get(robotSide), footstep); } }
public void updateContactPointsForUpcomingFootstep(Footstep nextFootstep) { RobotSide robotSide = nextFootstep.getRobotSide(); List<Point2d> predictedContactPoints = nextFootstep.getPredictedContactPoints(); if ((predictedContactPoints != null) && (!predictedContactPoints.isEmpty())) { setFootPlaneContactPoints(robotSide, predictedContactPoints); } else { resetFootPlaneContactPoint(robotSide); } }
public void updateFirstFootstep(Footstep firstFootstep) { RobotSide robotSide = firstFootstep.getRobotSide(); if (counters.get(robotSide).intValue() < 1) return; footstepVisualizers.get(robotSide).get(0).update(firstFootstep); }
@Override public boolean checkCondition() { boolean transferringToThisRobotSide; if (walkingMessageHandler.hasUpcomingFootsteps()) transferringToThisRobotSide = transferToSide == walkingMessageHandler.peek(0).getRobotSide().getOppositeSide(); else transferringToThisRobotSide = false; return transferringToThisRobotSide; } }
private FrameVector3D getTranslationVector(Footstep stance, double circleCenterOffset) { RobotSide stanceSide = stance.getRobotSide(); double yCenterDisplacementTowardsExpectedOtherFoot = (stanceSide == RobotSide.LEFT) ? -circleCenterOffset : circleCenterOffset; FrameVector3D translationFromFootCenterToCircleCenter = new FrameVector3D(stance.getSoleReferenceFrame(), 0.0, yCenterDisplacementTowardsExpectedOtherFoot, 0.0); translationFromFootCenterToCircleCenter.changeFrame(ReferenceFrame.getWorldFrame()); return translationFromFootCenterToCircleCenter; }
private void computeEntryCMPForFootstep(FramePoint entryCMPToPack, Footstep footstep, FramePoint2d centroidInSoleFrameOfPreviousSupportFoot, YoFramePoint previousExitCMP) { ReferenceFrame soleFrame = footstep.getSoleReferenceFrame(); List<Point2d> predictedContactPoints = footstep.getPredictedContactPoints(); RobotSide robotSide = footstep.getRobotSide(); if (predictedContactPoints != null) tempSupportPolygon.setIncludingFrameAndUpdate(soleFrame, predictedContactPoints); else tempSupportPolygon.setIncludingFrameAndUpdate(soleFrame, defaultFootPolygons.get(robotSide)); computeEntryCMP(entryCMPToPack, robotSide, soleFrame, tempSupportPolygon, centroidInSoleFrameOfPreviousSupportFoot, previousExitCMP); }
public void setNextFootstep(Footstep nextFootstep) { isUsingNextFootstep.set(nextFootstep != null); if (isUsingNextFootstep.getBooleanValue()) { ReferenceFrame footstepSoleFrame = nextFootstep.getSoleReferenceFrame(); ConvexPolygon2d footPolygon = footPolygons.get(nextFootstep.getRobotSide()).getConvexPolygon2d(); nextFootstepPolygon.setIncludingFrameAndUpdate(footstepSoleFrame, footPolygon); nextFootstepPolygon.changeFrameAndProjectToXYPlane(worldFrame); } }
private void printFootstepConstructor(Footstep footstep) { RobotSide robotSide = footstep.getRobotSide(); Point3d position = new Point3d(); footstep.getPositionInWorldFrame(position); Quat4d orientation = new Quat4d(); footstep.getOrientationInWorldFrame(orientation); System.out.println("footsteps.add(footstepProviderTestHelper.createFootstep(RobotSide." + robotSide + ", new Point3d(" + position.getX() + ", " + position.getY() + ", " + position.getZ() + "), new Quat4d(" + orientation.getW() + ", " + orientation.getX() + ", " + orientation.getY() + ", " + orientation.getZ() + ")));"); } }