private void rotateInitialOrientation(
FrameOrientation orientationToPack, double angleFromInitialOrientation) {
initialOrientation.changeFrame(circleFrame);
orientationToPack.setIncludingFrame(initialOrientation);
axisRotationTransform.rotZ(angleFromInitialOrientation);
orientationToPack.applyTransform(axisRotationTransform);
}
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();
}
