How to use

getRobotSide

method

in

us.ihmc.humanoidRobotics.footstep.Footstep

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() + ")));");
  }
}