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