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();
  }
  public void initialize() {
    currentTime.set(0.0);
    desiredTrajectoryTime.set(desiredTrajectoryTimeProvider.getValue());

    yoInitialPosition.getFrameTupleIncludingFrame(initialPosition);
    initialPosition.changeFrame(circleFrame);

    if (rotateHandAngleAboutAxis)
      rotateInitialOrientation(finalOrientation, desiredRotationAngle.getDoubleValue());
    else finalOrientation.setIncludingFrame(initialOrientation);
    finalOrientation.changeFrame(trajectoryFrame);
    yoFinalOrientation.set(finalOrientation);

    double x = initialPosition.getX();
    double y = initialPosition.getY();
    initialZ.set(initialPosition.getZ());

    initialRadius.set(Math.sqrt(x * x + y * y));
    initialAngle.set(Math.atan2(y, x));
    finalAngle.set(initialAngle.getDoubleValue() + desiredRotationAngle.getDoubleValue());

    //      anglePolynomial.setLinear(0.0, desiredTrajectoryTime.getDoubleValue(),
    // initialAngle.getDoubleValue(), finalAngle.getDoubleValue());
    anglePolynomial.setCubic(
        0.0,
        desiredTrajectoryTime.getDoubleValue(),
        initialAngle.getDoubleValue(),
        0.0,
        finalAngle.getDoubleValue(),
        0.0);
    //      anglePolynomial.setQuintic(0.0, desiredTrajectoryTime.getDoubleValue(),
    // initialAngle.getDoubleValue(), 0.0, 0.0, finalAngle.getDoubleValue(), 0.0, 0.0);

    double xFinal = initialRadius.getDoubleValue() * Math.cos(finalAngle.getDoubleValue());
    double yFinal = initialRadius.getDoubleValue() * Math.sin(finalAngle.getDoubleValue());
    double zFinal = initialZ.getDoubleValue();
    finalPosition.setIncludingFrame(circleFrame, xFinal, yFinal, zFinal);
    yoFinalPosition.setAndMatchFrame(finalPosition);

    rotateInitialOrientation(finalOrientation, finalAngle.getDoubleValue());

    currentAngleTrackingError.set(0.0);
    currentControlledFrameRelativeAngle.set(initialAngle.getDoubleValue());

    updateTangentialCircleFrame();

    if (visualize) visualizeTrajectory();
  }
  public void compute(double time, boolean adjustAngle) {
    this.currentTime.set(time);
    time = MathTools.clipToMinMax(time, 0.0, desiredTrajectoryTime.getDoubleValue());
    anglePolynomial.compute(time);

    double angle = anglePolynomial.getPosition();
    double angleDot = anglePolynomial.getVelocity();
    double angleDDot = anglePolynomial.getAcceleration();

    double cos = Math.cos(angle);
    double sin = Math.sin(angle);
    double r = initialRadius.getDoubleValue();

    double x = r * cos;
    double y = r * sin;
    double z = initialZ.getDoubleValue();

    currentPosition.setIncludingFrame(circleFrame, x, y, z);
    yoCurrentPositionWorld.setAndMatchFrame(currentPosition);

    currentRelativeAngle.set(
        computeAngleDifferenceMinusPiToPi(angle, initialAngle.getDoubleValue()));
    if (adjustAngle)
      currentAdjustedRelativeAngle.set(
          adjustCurrentDesiredRelativeAngle(currentRelativeAngle.getDoubleValue()));
    else currentAdjustedRelativeAngle.set(currentRelativeAngle.getDoubleValue());

    angle =
        trimAngleMinusPiToPi(
            currentAdjustedRelativeAngle.getDoubleValue() + initialAngle.getDoubleValue());

    if (isDone()) {
      angle = finalAngle.getDoubleValue();
      angleDot = 0.0;
      angleDDot = 0.0;
    }

    cos = Math.cos(angle);
    sin = Math.sin(angle);

    x = r * cos;
    y = r * sin;

    double xDot = -r * sin * angleDot;
    double yDot = x * angleDot;
    double zDot = 0.0;

    double xDDot = -r * cos * angleDot * angleDot - y * angleDDot;
    double yDDot = xDot * angleDot + x * angleDDot;
    double zDDot = 0.0;

    currentPosition.setIncludingFrame(circleFrame, x, y, z);
    currentVelocity.setIncludingFrame(circleFrame, xDot, yDot, zDot);
    currentAcceleration.setIncludingFrame(circleFrame, xDDot, yDDot, zDDot);

    if (rotateHandAngleAboutAxis) {
      rotateInitialOrientation(currentOrientation, angle - initialAngle.getDoubleValue());
      currentAngularVelocity.setIncludingFrame(circleFrame, 0.0, 0.0, angleDot);
      currentAngularAcceleration.setIncludingFrame(circleFrame, 0.0, 0.0, angleDDot);
    } else {
      currentOrientation.setIncludingFrame(initialOrientation);
      currentAngularVelocity.setIncludingFrame(circleFrame, 0.0, 0.0, 0.0);
      currentAngularAcceleration.setIncludingFrame(circleFrame, 0.0, 0.0, 0.0);
    }

    yoCurrentPosition.setAndMatchFrame(currentPosition);
    yoCurrentAdjustedPositionWorld.setAndMatchFrame(currentPosition);
    yoCurrentVelocity.setAndMatchFrame(currentVelocity);
    yoCurrentAcceleration.setAndMatchFrame(currentAcceleration);

    currentOrientation.changeFrame(trajectoryFrame);
    yoCurrentOrientation.set(currentOrientation);
    yoCurrentAngularVelocity.setAndMatchFrame(currentAngularVelocity);
    yoCurrentAngularAcceleration.setAndMatchFrame(currentAngularAcceleration);

    updateTangentialCircleFrame();
  }
 public void updateCircleFrame(FramePoint circleCenter, FrameVector circleNormal) {
   circleOrigin.setAndMatchFrame(circleCenter);
   rotationAxis.setAndMatchFrame(circleNormal);
   rotationAxis.normalize();
   circleFrame.update();
 }