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();
}
@Override
public void get(FramePose framePoseToPack) {
yoCurrentAdjustedPositionWorld.getFrameTupleIncludingFrame(currentPosition);
yoCurrentOrientation.getFrameOrientationIncludingFrame(currentOrientation);
framePoseToPack.setPoseIncludingFrame(currentPosition, currentOrientation);
}
public void get(FramePoint positionToPack) {
yoCurrentAdjustedPositionWorld.getFrameTupleIncludingFrame(positionToPack);
}