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();
}
/**
* Set rotation axis in trajectoryFrame. Default axis is zUp in trajectoryFrame.
*
* @param circleCenterInTrajectoryFrame
* @param circleNormalInTrajectoryFrame
*/
public void updateCircleFrame(
Point3d circleCenterInTrajectoryFrame, Vector3d circleNormalInTrajectoryFrame) {
circleOrigin.set(circleCenterInTrajectoryFrame);
rotationAxis.set(circleNormalInTrajectoryFrame);
rotationAxis.normalize();
circleFrame.update();
}
public UserDesiredHandstepProvider(
FullHumanoidRobotModel fullRobotModel,
YoVariableRegistry parentRegistry,
YoGraphicsListRegistry yoGraphicsListRegistry) {
this.handstepHelper = new HandstepHelper(fullRobotModel);
userHandstepTakeIt.set(false);
userHandstepNormal.set(-1.0, 0.0, 0.0);
userHandstepRobotSide.set(RobotSide.LEFT);
userDesiredHandstepCoordinateSystem =
new YoGraphicCoordinateSystem("userHandstepViz", "", parentRegistry, 0.3);
VariableChangedListener listener =
new VariableChangedListener() {
public void variableChanged(YoVariable<?> v) {
Vector3d position = userHandstepPosition.getVector3dCopy();
Vector3d surfaceNormal = userHandstepNormal.getVector3dCopy();
double rotationAngleAboutNormal = userHandstepRotationAboutNormal.getDoubleValue();
userDesiredHandstepCoordinateSystem.setTransformToWorld(
HandstepHelper.computeHandstepTransform(
false, position, surfaceNormal, rotationAngleAboutNormal));
}
};
yoGraphicsListRegistry.registerYoGraphic(
"UserDesiredHandstep", userDesiredHandstepCoordinateSystem);
userHandstepNormal.attachVariableChangedListener(listener);
userHandstepRotationAboutNormal.addVariableChangedListener(listener);
userHandstepPosition.attachVariableChangedListener(listener);
parentRegistry.addChild(registry);
}
public void setInitialPoseToMatchReferenceFrame(ReferenceFrame referenceFrame) {
initialPosition.setToZero(referenceFrame);
initialOrientation.setToZero(referenceFrame);
initialPosition.changeFrame(trajectoryFrame);
initialOrientation.changeFrame(trajectoryFrame);
yoInitialPosition.set(initialPosition);
yoInitialOrientation.set(initialOrientation);
}
public static BacklashCompensatingVelocityYoFrameVector
createBacklashCompensatingVelocityYoFrameVector(
String namePrefix,
String nameSuffix,
DoubleYoVariable alpha,
double dt,
DoubleYoVariable slopTime,
YoVariableRegistry registry,
YoFramePoint yoFramePointToDifferentiate) {
BacklashCompensatingVelocityYoVariable xDot =
new BacklashCompensatingVelocityYoVariable(
YoFrameVariableNameTools.createXName(namePrefix, nameSuffix),
"",
alpha,
yoFramePointToDifferentiate.getYoX(),
dt,
slopTime,
registry);
BacklashCompensatingVelocityYoVariable yDot =
new BacklashCompensatingVelocityYoVariable(
YoFrameVariableNameTools.createYName(namePrefix, nameSuffix),
"",
alpha,
yoFramePointToDifferentiate.getYoY(),
dt,
slopTime,
registry);
BacklashCompensatingVelocityYoVariable zDot =
new BacklashCompensatingVelocityYoVariable(
YoFrameVariableNameTools.createZName(namePrefix, nameSuffix),
"",
alpha,
yoFramePointToDifferentiate.getYoZ(),
dt,
slopTime,
registry);
BacklashCompensatingVelocityYoFrameVector ret =
new BacklashCompensatingVelocityYoFrameVector(
xDot, yDot, zDot, registry, yoFramePointToDifferentiate);
return ret;
}
private BacklashCompensatingVelocityYoFrameVector(
BacklashCompensatingVelocityYoVariable xDot,
BacklashCompensatingVelocityYoVariable yDot,
BacklashCompensatingVelocityYoVariable zDot,
YoVariableRegistry registry,
YoFramePoint yoFramePointToDifferentiate) {
super(xDot, yDot, zDot, yoFramePointToDifferentiate.getReferenceFrame());
this.xDot = xDot;
this.yDot = yDot;
this.zDot = zDot;
}
public Handstep getDesiredHandstep(RobotSide robotSide) {
if (!userHandstepTakeIt.getBooleanValue()) return null;
if (userHandstepRobotSide.getEnumValue() != robotSide) return null;
Vector3d surfaceNormal = userHandstepNormal.getVector3dCopy();
double rotationAngleAboutNormal = userHandstepRotationAboutNormal.getDoubleValue();
Vector3d position = userHandstepPosition.getVector3dCopy();
Handstep handstep =
handstepHelper.getDesiredHandstep(
robotSide,
position,
surfaceNormal,
rotationAngleAboutNormal,
swingTrajectoryTime.getDoubleValue());
userHandstepTakeIt.set(false);
return handstep;
}
@Override
public void get(FramePose framePoseToPack) {
yoCurrentAdjustedPositionWorld.getFrameTupleIncludingFrame(currentPosition);
yoCurrentOrientation.getFrameOrientationIncludingFrame(currentOrientation);
framePoseToPack.setPoseIncludingFrame(currentPosition, currentOrientation);
}
public void get(FramePoint positionToPack) {
yoCurrentAdjustedPositionWorld.getFrameTupleIncludingFrame(positionToPack);
}
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 setInitialPose(FramePose initialPose) {
initialPose.changeFrame(trajectoryFrame);
initialPose.getPoseIncludingFrame(initialPosition, initialOrientation);
yoInitialPosition.set(initialPosition);
yoInitialOrientation.set(initialOrientation);
}
public void updateCircleFrame(FramePoint circleCenter, FrameVector circleNormal) {
circleOrigin.setAndMatchFrame(circleCenter);
rotationAxis.setAndMatchFrame(circleNormal);
rotationAxis.normalize();
circleFrame.update();
}
public void getTouchdownLocation(Point3d touchdownLocationToPack) {
touchdownLocation.get(touchdownLocationToPack);
}