@Override
public Footstep predictFootstepAfterDesiredFootstep(
RobotSide supportLegSide, Footstep desiredFootstep) {
RobotSide futureSwingLegSide = supportLegSide;
ReferenceFrame futureSupportAnkleFrame = desiredFootstep.getPoseReferenceFrame();
ReferenceFrame futureSupportAnkleZUpFrame =
new ZUpFrame(ReferenceFrame.getWorldFrame(), futureSupportAnkleFrame, "ankleZUp");
futureSupportAnkleZUpFrame.update();
computeDistanceAndAngleToDestination(
futureSupportAnkleZUpFrame, futureSwingLegSide, desiredDestination.getFramePoint2dCopy());
Matrix3d footToWorldRotation =
computeDesiredFootRotation(
angleToDestination.getDoubleValue(), futureSwingLegSide, futureSupportAnkleFrame);
FramePoint footstepPosition =
getDesiredFootstepPositionCopy(
futureSupportAnkleZUpFrame,
futureSupportAnkleFrame,
futureSwingLegSide,
desiredDestination.getFramePoint2dCopy(),
footToWorldRotation);
FrameOrientation footstepOrientation = new FrameOrientation(ReferenceFrame.getWorldFrame());
double[] yawPitchRoll = new double[3];
RotationTools.convertMatrixToYawPitchRoll(footToWorldRotation, yawPitchRoll);
footstepOrientation.setYawPitchRoll(yawPitchRoll);
FramePose footstepPose = new FramePose(footstepPosition, footstepOrientation);
footstepPose.changeFrame(ReferenceFrame.getWorldFrame());
PoseReferenceFrame poseReferenceFrame =
new PoseReferenceFrame("poseReferenceFrame", footstepPose);
ContactablePlaneBody foot = contactableBodies.get(futureSwingLegSide);
boolean trustHeight = true;
return new Footstep(
foot.getRigidBody(),
futureSwingLegSide,
foot.getSoleFrame(),
poseReferenceFrame,
trustHeight);
}
@Override
public void process() {
double currentTime = timestamp.getDoubleValue();
// update body orientation
bodyYaw += planarVelocityProvider.get().getZ() * controlDT;
bodyOrientation.changeFrame(worldFrame);
bodyOrientation.setYawPitchRoll(bodyYaw, 0.0, 0.0);
// update xgait current steps
for (int i = 0; i < xGaitPreviewSteps.size(); i++) {
QuadrupedTimedStep xGaitPreviewStep = xGaitPreviewSteps.get(i);
if (xGaitPreviewStep.getTimeInterval().getStartTime() <= currentTime) {
xGaitCurrentSteps.get(xGaitPreviewStep.getRobotEnd()).set(xGaitPreviewStep);
}
}
// update xgait preview steps
updateXGaitSettings();
Vector3d inputVelocity = planarVelocityProvider.get();
xGaitStepPlanner.computeOnlinePlan(
xGaitPreviewSteps, xGaitCurrentSteps, inputVelocity, currentTime, bodyYaw, xGaitSettings);
// update step sequence
stepSequence.clear();
for (RobotEnd robotEnd : RobotEnd.values) {
if (xGaitCurrentSteps.get(robotEnd).getTimeInterval().getEndTime() >= currentTime) {
stepSequence.add();
stepSequence.get(stepSequence.size() - 1).set(xGaitCurrentSteps.get(robotEnd));
}
}
for (int i = 0; i < xGaitPreviewSteps.size(); i++) {
if (xGaitPreviewSteps.get(i).getTimeInterval().getEndTime() >= currentTime) {
stepSequence.add();
stepSequence.get(stepSequence.size() - 1).set(xGaitPreviewSteps.get(i));
}
}
}
