@Override
public void initialize() {
timeIntoStep.set(0.0);
isDone.set(false);
initialPosition.changeFrame(worldFrame);
finalPosition.changeFrame(worldFrame);
double maxStepZ = Math.max(initialPosition.getZ(), finalPosition.getZ());
switch (trajectoryType.getEnumValue()) {
case OBSTACLE_CLEARANCE:
for (int i = 0; i < numberWaypoints; i++) {
waypointPositions
.get(i)
.interpolate(initialPosition, finalPosition, waypointProportions[i]);
waypointPositions.get(i).setZ(maxStepZ + swingHeight.getDoubleValue());
}
break;
case PUSH_RECOVERY:
case BASIC:
case DEFAULT:
for (int i = 0; i < numberWaypoints; i++) {
waypointPositions
.get(i)
.interpolate(initialPosition, finalPosition, waypointProportions[i]);
waypointPositions.get(i).add(0.0, 0.0, swingHeight.getDoubleValue());
}
break;
default:
throw new RuntimeException("Trajectory type not implemented");
}
stanceFootPosition.changeFrame(worldFrame);
double maxWaypointZ =
Math.max(
stanceFootPosition.getZ() + maxSwingHeight.getDoubleValue(),
maxStepZ + defaultSwingHeight);
for (int i = 0; i < numberWaypoints; i++) {
waypointPositions.get(i).setZ(Math.min(waypointPositions.get(i).getZ(), maxWaypointZ));
}
trajectory.setEndpointConditions(
initialPosition, initialVelocity, finalPosition, finalVelocity);
trajectory.setWaypoints(waypointPositions);
trajectory.initialize();
visualize();
}
