How to use

us.ihmc.humanoidRobotics.footstep.Footstep

constructor

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 FootstepTask(FullHumanoidRobotModel fullRobotModel, RobotSide robotSide, FootstepListBehavior footstepListBehavior, FramePose3D footPose)
{
 super(footstepListBehavior);
 footsteps.add(new Footstep(robotSide, footPose));
 this.footstepListBehavior = footstepListBehavior;
}

public static Footstep generateStandingFootstep(RobotSide side, RigidBodyBasics foot, ReferenceFrame soleFrame)
{
 FramePose3D footFramePose = new FramePose3D(soleFrame);
 footFramePose.changeFrame(worldFrame);
 boolean trustHeight = false;
 Footstep footstep = new Footstep(side, footFramePose, trustHeight);
 return footstep;
}

private ArrayList<Footstep> createRandomFootsteps(int number)
{
 Random random = new Random(77);
 ArrayList<Footstep> footsteps = new ArrayList<Footstep>();
 for (int footstepNumber = 0; footstepNumber < number; footstepNumber++)
 {
   FramePose3D pose = new FramePose3D(ReferenceFrame.getWorldFrame(), new Point3D(footstepNumber, 0.0, 0.0),
      new Quaternion(random.nextDouble(), random.nextDouble(), random.nextDouble(), random.nextDouble()));
   boolean trustHeight = true;
   Footstep footstep = new Footstep(robotSide, pose, trustHeight);
   footsteps.add(footstep);
 }
 return footsteps;
}

public FootstepTask(E stateEnum,FullHumanoidRobotModel fullRobotModel, RobotSide robotSide, FootstepListBehavior footstepListBehavior, FramePose footPose)
{
 super(stateEnum, footstepListBehavior);
 ReferenceFrame soleFrame;
 RigidBody endEffector;
 
 soleFrame = fullRobotModel.getSoleFrame(robotSide);
 endEffector = fullRobotModel.getEndEffector(robotSide, LimbName.LEG);
 
 PoseReferenceFrame poseReferenceFrame = new PoseReferenceFrame("poseReferenceFrame", ReferenceFrame.getWorldFrame());
 poseReferenceFrame.setPoseAndUpdate(footPose);
 footsteps.add(new Footstep(endEffector, robotSide, soleFrame, poseReferenceFrame));
 this.footstepListBehavior = footstepListBehavior;
}

private Footstep createFootsteps(double length, double width, int numberOfSteps)
{
 ArrayList<Footstep> upcomingFootsteps = new ArrayList<>();
 RobotSide robotSide = RobotSide.LEFT;
 for (int i = 0; i < numberOfSteps; i++)
 {
   FramePose3D footstepPose = new FramePose3D(ReferenceFrame.getWorldFrame());
   footstepPose.setPosition(length * (i + 1), robotSide.negateIfRightSide(0.5 * width), 0.0);
   upcomingFootsteps.add(new Footstep(robotSide, footstepPose, false));
   FramePoint2D referenceFootstepPosition = new FramePoint2D(footstepPose.getPosition());
   robotSide = robotSide.getOppositeSide();
 }
 return upcomingFootsteps.get(0);
}

public static Footstep generateStandingFootstep(RobotSide side, SideDependentList<? extends ContactablePlaneBody> bipedFeet)
{
 ContactablePlaneBody endEffector = bipedFeet.get(side);
 FramePose3D footFramePose = new FramePose3D(endEffector.getSoleFrame());
 footFramePose.changeFrame(worldFrame);
 boolean trustHeight = false;
 Footstep footstep = new Footstep(side, footFramePose, trustHeight);
 return footstep;
}

@Override
public void receivedPacket(FootstepDataMessage packet)
{
  FramePose3D pose = new FramePose3D(ReferenceFrame.getWorldFrame(), packet.getLocation(), packet.getOrientation());
  boolean trustHeight = true;
  Footstep footstep = new Footstep(RobotSide.fromByte(packet.getRobotSide()), pose, trustHeight);
  reconstructedFootsteps.add(footstep);
}

public static Footstep generateStandingFootstep(RobotSide side, RigidBody foot, ReferenceFrame soleFrame)
{
 FramePose footFramePose = new FramePose(foot.getParentJoint().getFrameAfterJoint());
 footFramePose.changeFrame(worldFrame);
 boolean trustHeight = false;
 PoseReferenceFrame footstepPoseFrame = new PoseReferenceFrame("footstepPoseFrame", footFramePose);
 Footstep footstep = new Footstep(foot, side, soleFrame, footstepPoseFrame, trustHeight);
 return footstep;
}

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

private void addFootstepDataList(FootstepDataListMessage footstepDataList)
{
 ArrayList<FootstepDataMessage> footstepList = footstepDataList.getDataList();
 ArrayList<Footstep> footsteps = new ArrayList<Footstep>();
 for (FootstepDataMessage footstepData : footstepList)
 {
   RobotSide robotSide = footstepData.getRobotSide();
   ContactablePlaneBody contactableBody = bipedFeet.get(robotSide);
   FramePose footstepPose = new FramePose(ReferenceFrame.getWorldFrame(), footstepData.getLocation(), footstepData.getOrientation());
   PoseReferenceFrame footstepPoseFrame = new PoseReferenceFrame("footstepPoseFrame", footstepPose);
   String id = "scriptedFootstep_" + footstepCounter;
   Footstep footstep = new Footstep(id, contactableBody.getRigidBody(), robotSide, contactableBody.getSoleFrame(), footstepPoseFrame, true);
   footsteps.add(footstep);
   footstepCounter++;
 }
 footstepQueue.addAll(footsteps);
}

public static Footstep generateStandingFootstep(RobotSide side, SideDependentList<? extends ContactablePlaneBody> bipedFeet)
{
 ContactablePlaneBody endEffector = bipedFeet.get(side);
 FramePose footFramePose = new FramePose(endEffector.getFrameAfterParentJoint());
 footFramePose.changeFrame(worldFrame);
 boolean trustHeight = false;
 PoseReferenceFrame footstepPoseFrame = new PoseReferenceFrame("footstepPoseFrame", footFramePose);
 Footstep footstep = new Footstep(endEffector.getRigidBody(), side, endEffector.getSoleFrame(), footstepPoseFrame, trustHeight);
 return footstep;
}

private void addFootstepDataList(FootstepDataListMessage footstepDataList)
{
 ArrayList<FootstepDataMessage> footstepList = footstepDataList.getDataList();
 ArrayList<Footstep> footsteps = new ArrayList<Footstep>();
 for (FootstepDataMessage footstepData : footstepList)
 {
   RobotSide robotSide = footstepData.getRobotSide();
   ContactablePlaneBody contactableBody = bipedFeet.get(robotSide);
   FramePose footstepPose = new FramePose(ReferenceFrame.getWorldFrame(), footstepData.getLocation(), footstepData.getOrientation());
   PoseReferenceFrame footstepPoseFrame = new PoseReferenceFrame("footstepPoseFrame", footstepPose);
   String id = "scriptedFootstep_" + footstepCounter;
   Footstep footstep = new Footstep(id, contactableBody.getRigidBody(), robotSide, contactableBody.getSoleFrame(), footstepPoseFrame, true);
   footsteps.add(footstep);
   footstepCounter++;
 }
 footstepQueue.addAll(footsteps);
}

  public Footstep createFootstep(RobotSide robotSide, FramePose footstepPose)
  {
   RigidBody foot = contactableFeet.get(robotSide).getRigidBody();
   ReferenceFrame soleFrame = contactableFeet.get(robotSide).getSoleFrame();
   Footstep ret = new Footstep(foot, robotSide, soleFrame);
   ret.setPose(footstepPose);
   ret.setPredictedContactPointsFromFramePoint2ds(contactableFeet.get(robotSide).getContactPoints2d());

   return ret;
  }
}

private Footstep generateFootstep(FramePose2D footPose2d, RigidBodyBasics foot, ReferenceFrame soleFrame, RobotSide robotSide, double height, Vector3D planeNormal)
{
 double yaw = footPose2d.getYaw();
 Point3D position = new Point3D(footPose2d.getX(), footPose2d.getY(), height);
 Quaternion orientation = new Quaternion();
 RotationTools.computeQuaternionFromYawAndZNormal(yaw, planeNormal, orientation);
 FramePose3D footstepPose = new FramePose3D(ReferenceFrame.getWorldFrame(), position, orientation);
 Footstep footstep = new Footstep(robotSide, footstepPose);
 return footstep;
}

@Override
public Footstep generateFootstepWithoutHeightMap(FramePose2D footPose2d, RigidBodyBasics foot, ReferenceFrame soleFrame, RobotSide robotSide, double height,
    Vector3D planeNormal)
{
 double yaw = footPose2d.getYaw();
 Point3D position = new Point3D(footPose2d.getX(), footPose2d.getY(), height);
 Quaternion orientation = new Quaternion();
 RotationTools.computeQuaternionFromYawAndZNormal(yaw, planeNormal, orientation);
 FramePose3D solePose = new FramePose3D(ReferenceFrame.getWorldFrame(), position, orientation);
 Footstep footstep = new Footstep(robotSide, solePose);
 return footstep;
}

private Footstep generateFootstepFromLocationAndOrientation(RobotSide robotSide, double[] positionArray, double[] orientationArray)
{
 Footstep footstep = new Footstep(robotSide);
 Point3D position = new Point3D(positionArray);
 Quaternion orientation = new Quaternion(orientationArray);
 RigidBodyTransform anklePose = new RigidBodyTransform();
 anklePose.setRotation(orientation);
 anklePose.setTranslation(position);
 FramePose3D pose = new FramePose3D(ReferenceFrame.getWorldFrame(), anklePose);
 footstep.setFromAnklePose(pose, transformsFromAnkleToSole.get(robotSide));
 return footstep;
}

@Override
public Footstep generateFootstepWithoutHeightMap(FramePose2d footPose2d, RigidBody foot, ReferenceFrame soleFrame, RobotSide robotSide, double height,
    Vector3d planeNormal)
{
 double yaw = footPose2d.getYaw();
 Point3d position = new Point3d(footPose2d.getX(), footPose2d.getY(), height);
 Quat4d orientation = new Quat4d();
 RotationTools.computeQuaternionFromYawAndZNormal(yaw, planeNormal, orientation);
 Footstep footstep = new Footstep(foot, robotSide, soleFrame);
 footstep.setSolePose(new FramePose(ReferenceFrame.getWorldFrame(), position, orientation));
 return footstep;
}

private Footstep createFootstepAtCurrentLocation(RobotSide robotSide)
{
 ContactablePlaneBody foot = feet.get(robotSide);
 ReferenceFrame footReferenceFrame = foot.getRigidBody().getParentJoint().getFrameAfterJoint();
 FramePose framePose = new FramePose(footReferenceFrame);
 framePose.changeFrame(worldFrame);
 PoseReferenceFrame poseReferenceFrame = new PoseReferenceFrame("poseReferenceFrame", framePose);
 boolean trustHeight = true;
 Footstep footstep = new Footstep(foot.getRigidBody(), robotSide, foot.getSoleFrame(), poseReferenceFrame, trustHeight);
 return footstep;
}

  @ContinuousIntegrationTest(estimatedDuration = 0.0)
  @Test(timeout = 30000)
  public void testCopy()
  {
   Footstep footstep = new Footstep(RobotSide.RIGHT);
   FootstepTiming timing = new FootstepTiming(0.1, 1.2);
   FootstepData obj1 = new FootstepData(footstep, timing);
   FootstepData obj2 = new FootstepData();
   assertTrue(obj2.getFootstep() == null);
   obj2.set(obj1);
   assertTrue(obj2.getFootstep() == footstep);
   assertTrue(obj2.getStepTime() == timing.getStepTime());
  }
}