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