How to use

Footstep

in

us.ihmc.humanoidRobotics.footstep

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 static Footstep[] createFootsteps(int numberOfSteps)
{
 Footstep[] footsteps = new Footstep[numberOfSteps];
 for (int i = 0; i < numberOfSteps; i++)
 {
   footsteps[i] = new Footstep();
 }
 return footsteps;
}

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;
}

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 set(ArrayList<Footstep> footsteps, double swingTime, double transferTime)
{
 FootstepDataListMessage footstepDataList = new FootstepDataListMessage(swingTime,transferTime);
 for (int i = 0; i < footsteps.size(); i++)
 {
   Footstep footstep = footsteps.get(i);
   Point3d location = new Point3d(footstep.getX(), footstep.getY(), footstep.getZ());
   Quat4d orientation = new Quat4d();
   footstep.getOrientation(orientation);
   RobotSide footstepSide = footstep.getRobotSide();
   FootstepDataMessage footstepData = new FootstepDataMessage(footstepSide, location, orientation);
   footstepDataList.add(footstepData);
 }
 set(footstepDataList);
}

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;
}

@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 FootstepDataMessage(Footstep footstep, FootstepTiming timing)
 robotSide = footstep.getRobotSide();
 location = new Point3d();
 orientation = new Quat4d();
 footstep.getPositionInWorldFrame(location);
 footstep.getOrientationInWorldFrame(orientation);
 List<Point2d> footstepContactPoints = footstep.getPredictedContactPoints();
 if (footstepContactPoints != null)
 trajectoryType = footstep.getTrajectoryType();
 swingHeight = footstep.getSwingHeight();
 if (footstep.getSwingWaypoints().size() != 0)
   trajectoryWaypoints = new Point3d[footstep.getSwingWaypoints().size()];
   for (int i = 0; i < footstep.getSwingWaypoints().size(); i++)
    trajectoryWaypoints[i] = new Point3d(footstep.getSwingWaypoints().get(i));

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;
}

@Override
public Footstep.FootstepType snapFootstep(Footstep footstep, HeightMapWithPoints heightMap){
 FootstepDataMessage originalFootstep = new FootstepDataMessage(footstep);
 //set to the sole pose
 Vector3d position = new Vector3d();
 Quat4d orientation = new Quat4d();
 RigidBodyTransform solePose = new RigidBodyTransform();
 footstep.getSolePose(solePose);
 solePose.get(orientation, position);
 originalFootstep.setLocation(new Point3d(position));
 originalFootstep.setOrientation(orientation);
 //get the footstep
 Footstep.FootstepType type = snapFootstep(originalFootstep, heightMap);
 footstep.setPredictedContactPointsFromPoint2ds(originalFootstep.getPredictedContactPoints());
 footstep.setFootstepType(type);
 FramePose solePoseInWorld = new FramePose(ReferenceFrame.getWorldFrame(), originalFootstep.getLocation(), originalFootstep.getOrientation());
 footstep.setSolePose(solePoseInWorld);
 footstep.setSwingHeight(originalFootstep.getSwingHeight());
 footstep.setTrajectoryType(originalFootstep.getTrajectoryType());
 return type;
}

 swingStartPose.setToZero(humanoidReferenceFrames.getSoleFrame(footstepList.get(0).getRobotSide()));
 stanceFootPose.setToZero(humanoidReferenceFrames.getSoleFrame(footstepList.get(0).getRobotSide().getOppositeSide()));
footstep.getPose(swingEndPose);
FootstepDataMessage footstepDataMessage = HumanoidMessageTools.createFootstepDataMessage(footstep.getRobotSide(), new Point3D(swingEndPose.getPosition()), new Quaternion(swingEndPose.getOrientation()));
MessageTools.copyData(waypoints, footstepDataMessage.getCustomPositionWaypoints());
if (footstep.getFootstepType() == FootstepType.PARTIAL_FOOTSTEP)
 partialFootholdPolygon.addVertices(Vertex2DSupplier.asVertex2DSupplier(footstep.getPredictedContactPoints()));
 partialFootholdPolygon.update();

 transferToFootstep.setZ(transferToFootstep.getZ() + RandomNumbers.nextDouble(random, 0.0, maxZChange));
previousFootstep = transferToFootstep;
FootSpoof transferFromFootSpoof = contactableFeet.get(transferFromFootstep.getRobotSide());
FramePoint3D transferFromFootFramePoint = new FramePoint3D();
transferFromFootstep.getPosition(transferFromFootFramePoint);
FrameQuaternion transferFromFootOrientation = new FrameQuaternion();
transferFromFootstep.getOrientation(transferFromFootOrientation);
transferFromFootSpoof.setPose(transferFromFootFramePoint, transferFromFootOrientation);
FootSpoof transferToFootSpoof = contactableFeet.get(transferToFootstep.getRobotSide());
FramePoint3D transferToFootFramePoint = new FramePoint3D();
transferToFootstep.getPosition(transferToFootFramePoint);
FrameQuaternion transferToFootOrientation = new FrameQuaternion();
transferToFootstep.getOrientation(transferToFootOrientation);
transferToFootSpoof.setPose(transferToFootFramePoint, transferToFootOrientation);
transferToAndNextFootstepsData.setNextFootstep(upcomingFootstep);
transferToAndNextFootstepsData.setTransferToSide(transferToFootstep.getRobotSide());
RobotSide supportLeg = transferFromFootstep.getRobotSide();
supportLegFrameSide.set(supportLeg);
   supportLeg = transferToFootstep.getRobotSide();
 FramePoint2D transferFromFootPosition = new FramePoint2D(transferFromFootstep.getFootstepPose().getPosition());
 FramePoint2D transferToFootPosition = new FramePoint2D(transferToFootstep.getFootstepPose().getPosition());

  return false;
if (footstepToAdjust.getRobotSide() != requestedFootstepAdjustment.getRobotSide())
  PrintTools.warn(this, "RobotSide does not match: side of footstep to be adjusted: " + footstepToAdjust.getRobotSide() + ", side of adjusted footstep: " + requestedFootstepAdjustment.getRobotSide());
  hasNewFootstepAdjustment.set(false);
  requestedFootstepAdjustment.clear();
  footstepToAdjust.setPose(adjustedPosition, adjustedOrientation);
  break;
case AT_SOLE_FRAME:
  footstepToAdjust.setSolePose(adjustedPosition, adjustedOrientation);
  break;
default:
  for (int i = 0; i < footstepToAdjust.getPredictedContactPoints().size(); i++)
   contactPoints.add(footstepToAdjust.getPredictedContactPoints().get(i));
  footstepToAdjust.setPredictedContactPointsFromPoint2ds(contactPoints);

public AdjustFootstepMessage(Footstep footstep)
{
 uniqueId = VALID_MESSAGE_DEFAULT_ID;
 origin = FootstepOrigin.AT_ANKLE_FRAME;
 robotSide = footstep.getRobotSide();
 location = new Point3d();
 orientation = new Quat4d();
 footstep.getPositionInWorldFrame(location);
 footstep.getOrientationInWorldFrame(orientation);
 List<Point2d> footstepContactPoints = footstep.getPredictedContactPoints();
 if (footstepContactPoints != null)
 {
   if (predictedContactPoints == null)
   {
    predictedContactPoints = new ArrayList<>();
   }
   else
   {
    predictedContactPoints.clear();
   }
   for (Point2d contactPoint : footstepContactPoints)
   {
    predictedContactPoints.add((Point2d) contactPoint.clone());
   }
 }
 else
 {
   predictedContactPoints = null;
 }
}

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;
}

 swingStartPose.setToZero(humanoidReferenceFrames.getSoleFrame(footstepList.get(0).getRobotSide()));
 stanceFootPose.setToZero(humanoidReferenceFrames.getSoleFrame(footstepList.get(0).getRobotSide().getOppositeSide()));
footstep.getSolePose(swingEndPose);
FootstepDataMessage footstepDataMessage = new FootstepDataMessage(footstep.getRobotSide(), new Point3d(swingEndPose.getPositionUnsafe()),
                                 new Quat4d(swingEndPose.getOrientationUnsafe()));
footstepDataMessage.setOrigin(FootstepOrigin.AT_SOLE_FRAME);
footstepDataMessage.setTrajectoryWaypoints(waypoints);
if (footstep.getFootstepType() == FootstepType.PARTIAL_FOOTSTEP)
 partialFootholdPolygon.addVertices(footstep.getPredictedContactPoints(), footstep.getPredictedContactPoints().size());
 partialFootholdPolygon.update();

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;
}

/**
* 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);
}

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;
}

@Override
public void receivedPacket(FootstepDataListMessage packet)
{
  boolean adjustable = packet.getAreFootstepsAdjustable();
  List<FootstepDataMessage> footstepDataList = packet.getFootstepDataList();
  for (int i = 0; i < footstepDataList.size(); i++)
  {
   FootstepDataMessage footstepData = footstepDataList.get(i);
   FramePose3D footstepPose = new FramePose3D(ReferenceFrame.getWorldFrame(), footstepData.getLocation(), footstepData.getOrientation());
   Footstep footstep = new Footstep(robotSide, footstepPose, true, adjustable);
   footstep.setPredictedContactPoints(HumanoidMessageTools.unpackPredictedContactPoints(footstepData));
        footstep.setTrajectoryType(TrajectoryType.fromByte(footstepData.getTrajectoryType()));
   footstep.setSwingHeight(footstepData.getSwingHeight());
   reconstructedFootstepPath.add(footstep);
  }
}