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