private void visualizeTrajectory() {
yoInitialPosition.getFrameTupleIncludingFrame(initialPosition);
yoInitialPositionWorld.setAndMatchFrame(initialPosition);
yoFinalPosition.getFrameTupleIncludingFrame(finalPosition);
yoFinalPositionWorld.setAndMatchFrame(finalPosition);
for (int i = 0; i < numberOfBalls; i++) {
double t = (double) i / ((double) numberOfBalls - 1) * desiredTrajectoryTime.getDoubleValue();
compute(t, false);
yoCurrentPosition.getFrameTupleIncludingFrame(ballPosition);
ballPosition.changeFrame(worldFrame);
bagOfBalls.setBallLoop(ballPosition);
}
reset();
}
private void visualize() {
if (waypointViz == null) return;
for (int i = 0; i < numberWaypoints; i++) waypointViz.setBall(waypointPositions.get(i), i);
}