How to use

getSolePose

method

in

us.ihmc.humanoidRobotics.footstep.Footstep

public double getSwingHeight(Footstep initialFootstep, Footstep stanceFootstep, Footstep endFootstep, HeightMapWithPoints heightMap)
{
 FramePose startPose = new FramePose();
 initialFootstep.getSolePose(startPose);
 FramePose endPose = new FramePose();
 endFootstep.getSolePose(endPose);
 Point3d position = new Point3d();
 stanceFootstep.getPosition(position);
 return getSwingHeight(startPose, endPose, position.getZ(), heightMap);
}

public void update(Footstep footstep)
{
 footstep.getSolePose(footstepPose);
 yoFootstepPose.setAndMatchFrame(footstepPose);
 List<Point2d> predictedContactPoints = footstep.getPredictedContactPoints();
 List<Point2d> contactPointsToVisualize;
 if (predictedContactPoints == null || predictedContactPoints.isEmpty())
   contactPointsToVisualize = defaultContactPointsInSoleFrame;
 else
   contactPointsToVisualize = predictedContactPoints;
 foothold.setAndUpdate(contactPointsToVisualize, contactPointsToVisualize.size());
 yoFoothold.setConvexPolygon2d(foothold);
 poseViz.update();
 footholdViz.update();
}

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

@Override
public void adjustFootstepWithoutHeightmap(Footstep footstep, double height, Vector3d planeNormal)
{
 Vector3d position = new Vector3d();
 Quat4d orientation = new Quat4d();
 RigidBodyTransform solePose = new RigidBodyTransform();
 footstep.getSolePose(solePose);
 solePose.get(orientation, position);
 double yaw = RotationTools.computeYaw(orientation);
 RotationTools.computeQuaternionFromYawAndZNormal(yaw, planeNormal, orientation);
 position.setZ(height);
 footstep.setSolePose(new FramePose(ReferenceFrame.getWorldFrame(), new Point3d(position), orientation));
}

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

private void calculateInitialFootPoseAndOffsets()
{
 SideDependentList<Footstep> currentFootsteps;
 if (priorStanceFeetSpecified)
   currentFootsteps = priorStanceFeet;
 else
   currentFootsteps = createFootstepsFromBipedFeet();
 Footstep left = currentFootsteps.get(RobotSide.LEFT);
 Footstep right = currentFootsteps.get(RobotSide.RIGHT);
 FramePose leftPose = new FramePose();
 FramePose rightPose = new FramePose();
 left.getSolePose(leftPose);
 right.getSolePose(rightPose);
 FramePose2d leftPose2d = new FramePose2d();
 FramePose2d rightPose2d = new FramePose2d();
 leftPose.getPose2dIncludingFrame(leftPose2d);
 rightPose.getPose2dIncludingFrame(rightPose2d);
 startPose.interpolate(leftPose2d, rightPose2d, 0.5);
 Pose2dReferenceFrame startFramePose = new Pose2dReferenceFrame("StartPoseFrame", startPose);
 leftPose.changeFrame(startFramePose);
 rightPose.changeFrame(startFramePose);
 FrameOrientation2d leftOrientation = new FrameOrientation2d();
 FrameOrientation2d rightOrientation = new FrameOrientation2d();
 leftPose.getOrientation2dIncludingFrame(leftOrientation);
 rightPose.getOrientation2dIncludingFrame(rightOrientation);
 initialDeltaFeetYaw = leftOrientation.sub(rightOrientation);
 initialDeltaFeetY = leftPose.getY() - rightPose.getY();
 initialDeltaFeetX = leftPose.getX() - rightPose.getX();
}

public void setFootstep(Footstep footstep, double swingTime)
 footstep.getSolePose(footstepSolePose);

footstep.getSolePose(swingEndPose);

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

public Footstep.FootstepType snapFootstep(Footstep footstep, List<Point3d> pointList, double defaultHeight)
{
 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, pointList, defaultHeight);
 footstep.setFootstepType(type);
 footstep.setPredictedContactPointsFromPoint2ds(originalFootstep.getPredictedContactPoints());
 FramePose solePoseInWorld = new FramePose(ReferenceFrame.getWorldFrame(), originalFootstep.getLocation(), originalFootstep.getOrientation());
 footstep.setSolePose(solePoseInWorld);
 footstep.setSwingHeight(originalFootstep.getSwingHeight());
 footstep.setTrajectoryType(originalFootstep.getTrajectoryType());
 return type;
}