How to use

setPose

method

in

us.ihmc.humanoidRobotics.footstep.Footstep

public void setCurrentDesiredFootstep(Footstep currentDesiredFootstep)
{
 this.currentDesiredFootstep.setPose(currentDesiredFootstep);
}

public Footstep getFootstepAtCurrentLocation(RobotSide robotSide)
{
 tempPose.setToZero(contactableFeet.get(robotSide).getFrameAfterParentJoint());
 tempPose.changeFrame(worldFrame);
 Footstep footstep = footstepsAtCurrentLocation.get(robotSide);
 footstep.setPose(tempPose);
 return footstep;
}

public void sendFootStepMessages(int numberOfFootstepsToPlan)
{
 RobotSide robotSide = RobotSide.LEFT;
 FramePoint3D footstepLocation = new FramePoint3D();
 FrameQuaternion footstepOrientation = new FrameQuaternion();
 upcomingFootstepsData.clear();
 for (int i = 1; i < numberOfFootstepsToPlan + 1; i++)
 {
   Footstep footstep = new Footstep(robotSide);
   footstepLocation.set(i * stepLength, robotSide.negateIfRightSide(stepWidth), 0.0);
   footstep.setPose(footstepLocation, footstepOrientation);
   FootstepTiming timing = new FootstepTiming(swingTime, transferTime);
   upcomingFootstepsData.add(new FootstepData(footstep, timing));
   robotSide = robotSide.getOppositeSide();
 }
 numberOfUpcomingFootsteps.set(upcomingFootstepsData.size());
}

@Override public boolean getUpcomingFootstepSolution(Footstep footstepToPack)
{
 if (icpOptimizationController.getNumberOfFootstepsToConsider() > 0)
 {
   footstepToPack.getPose(footstepPose);
   icpOptimizationController.getFootstepSolution(0, footstepPositionSolution);
   footstepPose.setXYFromPosition2d(footstepPositionSolution);
   footstepToPack.setPose(footstepPose);
 }
 return icpOptimizationController.wasFootstepAdjusted();
}

  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 void setupInputs()
{
 clear();
 RobotSide side = RobotSide.LEFT;
 for (int i = 0; i < testParameters.getNumberOfFootstepsToConsider(); i++)
 {
   // Update the current location based on walking direction
   tempFrameVector.setIncludingFrame(walkingDirectionUnitVector);
   tempFrameVector.scale(stepLength);
   currentLocation.add(tempFrameVector);
   // Create new footstep based on updated CoM location
   Footstep newFootstep = new Footstep();
   newFootstep.setRobotSide(side);
   getFootLocationFromCoMLocation(tempFramePoint1, side, currentLocation, walkingDirectionUnitVector, stepLength, stepWidth);
   // Set calculated pose to footstep
   newFootstep.setPose(tempFramePoint1, robotOrientation);
   upcomingFootstepsData.add(new FootstepData(newFootstep, new FootstepTiming(swingTime, transferTime)));
   side = side.getOppositeSide();
 }
 numberOfUpcomingFootsteps.set(upcomingFootstepsData.size());
}

  footstepToAdjust.setPose(adjustedPosition, adjustedOrientation);
  break;
case AT_SOLE_FRAME:

private int checkForEndingOfAdjustment(double omega0)
{
 int numberOfFootstepsToConsider = clipNumberOfFootstepsToConsiderToProblem(this.numberOfFootstepsToConsider.getIntegerValue());
 if (timeRemainingInState.getDoubleValue() < remainingTimeToStopAdjusting.getDoubleValue() && localUseStepAdjustment && !isStanding.getBooleanValue() && !isInTransfer.getBooleanValue())
 {
   //record current locations
   for (int i = 0; i < numberOfFootstepsToConsider; i++)
   {
    upcomingFootstepLocations.get(i).set(footstepSolutions.get(i));
    Footstep footstep = upcomingFootsteps.get(i);
    footstep.getPose(footstepPose);
    footstepPose.setXYFromPosition2d(footstepSolutions.get(i).getFrameTuple2d());
    footstep.setPose(footstepPose);
    inputHandler.addFootstepToPlan(footstep);
   }
   localUseStepAdjustment = false;
   numberOfFootstepsToConsider = clipNumberOfFootstepsToConsiderToProblem(this.numberOfFootstepsToConsider.getIntegerValue());
   footstepRecursionMultiplierCalculator.resetTimes();
   footstepRecursionMultiplierCalculator.submitTimes(0, 0.0, singleSupportDuration.getDoubleValue());
   for (int i = 1; i < numberOfFootstepsToConsider + 1; i++)
    footstepRecursionMultiplierCalculator.submitTimes(i, doubleSupportDuration.getDoubleValue(), singleSupportDuration.getDoubleValue());
   footstepRecursionMultiplierCalculator.submitTimes(numberOfFootstepsToConsider + 1, doubleSupportDuration.getDoubleValue(), singleSupportDuration.getDoubleValue());
   footstepRecursionMultiplierCalculator.computeRecursionMultipliers(numberOfFootstepsToConsider, isInTransfer.getBooleanValue(), localUseTwoCMPs, omega0);
 }
 return numberOfFootstepsToConsider;
}

@Override
public void adjustFootstepWithoutHeightmap(Footstep footstep, double height, Vector3D planeNormal)
{
 // can only snap footsteps in world frame
 footstep.getFootstepPose().checkReferenceFrameMatch(ReferenceFrame.getWorldFrame());
 FramePose3D footstepPose = new FramePose3D();
 footstep.getPose(footstepPose);
 Point3D position = new Point3D(footstepPose.getPosition());
 double yaw = footstep.getFootstepPose().getYaw();
 Quaternion orientation = new Quaternion();
 RotationTools.computeQuaternionFromYawAndZNormal(yaw, planeNormal, orientation);
 position.setZ(height);
 footstepPose.set(new Point3D(position), orientation);
 footstep.setPose(footstepPose);
}

@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);
 if (type == Footstep.FootstepType.FULL_FOOTSTEP && originalFootstep.getPredictedContactPoints2d().size() > 0 )
 {
   throw new RuntimeException(this.getClass().getSimpleName() + "Full Footstep should have null contact points");
 }
 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;
}

@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);
 if (type == Footstep.FootstepType.FULL_FOOTSTEP && originalFootstep.getPredictedContactPoints2d().size() > 0){
   throw new RuntimeException(this.getClass().getSimpleName() + "Full Footstep should have null contact points");
 }
 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;
}

@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 Footstep.FootstepType snapFootstep(Footstep footstep, List<Point3D> pointList, double defaultHeight)
{
 // 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, pointList, defaultHeight);
 footstep.setFootstepType(type);
 footstep.setPredictedContactPoints(HumanoidMessageTools.unpackPredictedContactPoints(originalFootstep));
 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;
}