@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();
}
@Override
public void compute(double time) {
double trajectoryTime = stepTime.getDoubleValue();
isDone.set(time >= trajectoryTime);
time = MathTools.clipToMinMax(time, 0.0, trajectoryTime);
timeIntoStep.set(time);
double percent = time / trajectoryTime;
trajectory.compute(percent);
}
@Override
public void getAcceleration(FrameVector accelerationToPack) {
trajectory.getAcceleration(accelerationToPack);
}
@Override
public void getVelocity(FrameVector velocityToPack) {
trajectory.getVelocity(velocityToPack);
}
@Override
public void getPosition(FramePoint positionToPack) {
trajectory.getPosition(positionToPack);
}
public void informDone() {
trajectory.informDone();
}