@Override
public void onEntry() {
// initialize body orientation
bodyOrientation.setToZero(bodyZUpFrame);
bodyOrientation.changeFrame(worldFrame);
bodyYaw = bodyOrientation.getYaw();
// initialize step queue
updateXGaitSettings();
supportCentroid.setToZero(supportFrame);
double initialTime = timestamp.getDoubleValue() + initialStepDelayParameter.get();
Vector3d initialVelocity = planarVelocityProvider.get();
RobotQuadrant initialQuadrant =
(xGaitSettings.getEndPhaseShift() < 90)
? RobotQuadrant.HIND_LEFT
: RobotQuadrant.FRONT_LEFT;
xGaitStepPlanner.computeInitialPlan(
xGaitPreviewSteps,
initialVelocity,
initialQuadrant,
supportCentroid,
initialTime,
bodyYaw,
xGaitSettings);
for (int i = 0; i < 2; i++) {
RobotEnd robotEnd = xGaitPreviewSteps.get(i).getRobotQuadrant().getEnd();
xGaitCurrentSteps.get(robotEnd).set(xGaitPreviewSteps.get(i));
}
this.process();
}
private void updateXGaitSettings() {
xGaitSettingsProvider.getSettings(xGaitSettings);
// increase stance dimensions as a function of velocity to prevent self collisions
double strideRotation = planarVelocityProvider.get().getZ() * xGaitSettings.getStepDuration();
double strideLength =
Math.abs(2 * planarVelocityProvider.get().getX() * xGaitSettings.getStepDuration());
double strideWidth =
Math.abs(2 * planarVelocityProvider.get().getY() * xGaitSettings.getStepDuration());
strideLength += Math.abs(xGaitSettings.getStanceWidth() / 2 * Math.sin(2 * strideRotation));
strideWidth += Math.abs(xGaitSettings.getStanceLength() / 2 * Math.sin(2 * strideRotation));
xGaitSettings.setStanceLength(
Math.max(
xGaitSettings.getStanceLength(),
strideLength / 2 + minimumStepClearanceParameter.get()));
xGaitSettings.setStanceWidth(
Math.max(
xGaitSettings.getStanceWidth(), strideWidth / 2 + minimumStepClearanceParameter.get()));
}
@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));
}
}
}