public VisualizeFramesController(
ArrayList<ReferenceFrame> referenceFrames,
YoGraphicsListRegistry yoGraphicsListRegistry,
double coordinateSystemLength) {
for (ReferenceFrame frame : referenceFrames) {
YoGraphicReferenceFrame dynamicGraphicReferenceFrame =
new YoGraphicReferenceFrame(frame, registry, coordinateSystemLength);
dynamicGraphicReferenceFrames.add(dynamicGraphicReferenceFrame);
yoGraphicsList.add(dynamicGraphicReferenceFrame);
}
yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList);
}
public CirclePoseTrajectoryGenerator(
String namePrefix,
ReferenceFrame trajectoryFrame,
DoubleProvider trajectoryTimeProvider,
YoVariableRegistry parentRegistry,
YoGraphicsListRegistry yoGraphicsListRegistry) {
this.registry = new YoVariableRegistry(namePrefix + getClass().getSimpleName());
this.desiredTrajectoryTime = new DoubleYoVariable(namePrefix + "TrajectoryTime", registry);
this.currentTime = new DoubleYoVariable(namePrefix + "Time", registry);
// this.anglePolynomial = new YoPolynomial(namePrefix + "ParameterPolynomial", 2,
// registry);
this.anglePolynomial = new YoPolynomial(namePrefix + "ParameterPolynomial", 4, registry);
// this.anglePolynomial = new YoPolynomial(namePrefix + "ParameterPolynomial", 6,
// registry);
this.desiredTrajectoryTimeProvider = trajectoryTimeProvider;
this.trajectoryFrame = trajectoryFrame;
initialRadius = new DoubleYoVariable(namePrefix + "Radius", registry);
initialZ = new DoubleYoVariable(namePrefix + "ZPosition", registry);
initialAngle = new DoubleYoVariable(namePrefix + "InitialAngle", registry);
finalAngle = new DoubleYoVariable(namePrefix + "FinalAngle", registry);
isCurrentAngleBeingAdjusted =
new BooleanYoVariable(namePrefix + "IsCurrentAngleBeingAdjusted", registry);
maximumAngleTrackingErrorTolerated =
new DoubleYoVariable(namePrefix + "MaxAngleTrackingErrorTolerated", registry);
maximumAngleTrackingErrorTolerated.set(Math.toRadians(30.0));
currentControlledFrameRelativeAngle =
new DoubleYoVariable(namePrefix + "CurrentControlledFrameAngle", registry);
currentAngleTrackingError =
new DoubleYoVariable(namePrefix + "CurrentAngleTrackingError", registry);
currentAdjustedRelativeAngle =
new DoubleYoVariable(namePrefix + "CurrentAdjustedRelativeAngle", registry);
desiredRotationAngle = new DoubleYoVariable(namePrefix + "DesiredRotationAngle", registry);
currentRelativeAngle = new DoubleYoVariable(namePrefix + "CurrentRelativeAngle", registry);
yoInitialPosition = new YoFramePoint(namePrefix + "InitialPosition", trajectoryFrame, registry);
yoFinalPosition = new YoFramePoint(namePrefix + "FinalPosition", trajectoryFrame, registry);
yoCurrentPosition = new YoFramePoint(namePrefix + "CurrentPosition", trajectoryFrame, registry);
yoCurrentVelocity =
new YoFrameVector(namePrefix + "CurrentVelocity", trajectoryFrame, registry);
yoCurrentAcceleration =
new YoFrameVector(namePrefix + "CurrentAcceleration", trajectoryFrame, registry);
yoInitialOrientation =
new YoFrameQuaternion(namePrefix + "InitialOrientation", trajectoryFrame, registry);
yoFinalOrientation =
new YoFrameQuaternion(namePrefix + "FinalOrientation", trajectoryFrame, registry);
yoCurrentOrientation =
new YoFrameQuaternion(namePrefix + "CurrentOrientation", trajectoryFrame, registry);
yoCurrentAngularVelocity =
new YoFrameVector(namePrefix + "CurrentAngularVelocity", trajectoryFrame, registry);
yoCurrentAngularAcceleration =
new YoFrameVector(namePrefix + "CurrentAngularAcceleration", trajectoryFrame, registry);
yoInitialPositionWorld =
new YoFramePoint(namePrefix + "InitialPositionWorld", worldFrame, registry);
yoFinalPositionWorld =
new YoFramePoint(namePrefix + "FinalPositionWorld", worldFrame, registry);
yoCurrentPositionWorld =
new YoFramePoint(namePrefix + "CurrentPositionWorld", worldFrame, registry);
yoCurrentAdjustedPositionWorld =
new YoFramePoint(namePrefix + "CurrentAdjustedPositionWorld", worldFrame, registry);
circleOrigin = new YoFramePoint(namePrefix + "CircleOrigin", trajectoryFrame, registry);
rotationAxis = new YoFrameVector(namePrefix + "RotationAxis", trajectoryFrame, registry);
rotationAxis.set(0.0, 0.0, 1.0);
circleFrame =
new ReferenceFrame("CircleFrame", trajectoryFrame) {
private static final long serialVersionUID = 9102217353690768074L;
private final Vector3d localTranslation = new Vector3d();
private final Vector3d localRotationAxis = new Vector3d();
private final AxisAngle4d localAxisAngle = new AxisAngle4d();
@Override
protected void updateTransformToParent(RigidBodyTransform transformToParent) {
circleOrigin.get(localTranslation);
rotationAxis.get(localRotationAxis);
GeometryTools.getRotationBasedOnNormal(localAxisAngle, localRotationAxis);
transformToParent.set(localAxisAngle, localTranslation);
}
};
tangentialCircleFrame = new PoseReferenceFrame("TangentialCircleFrame", circleFrame);
yoTangentialCircleFramePose =
new YoFramePose(namePrefix + "TangentialCircleFramePose", worldFrame, registry);
if (this.visualize && yoGraphicsListRegistry != null) {
final YoGraphicPosition currentPositionViz =
new YoGraphicPosition(
namePrefix + "CurrentPosition", yoCurrentPositionWorld, 0.025, YoAppearance.Blue());
final YoGraphicPosition currentAdjustedPositionViz =
new YoGraphicPosition(
namePrefix + "CurrentAdjustedPosition",
yoCurrentAdjustedPositionWorld,
0.023,
YoAppearance.Gold());
final YoGraphicPosition initialPositionViz =
new YoGraphicPosition(
namePrefix + "InitialPosition",
yoInitialPositionWorld,
0.02,
YoAppearance.BlueViolet());
final YoGraphicPosition finalPositionViz =
new YoGraphicPosition(
namePrefix + "FinalPosition", yoFinalPositionWorld, 0.02, YoAppearance.Red());
final YoGraphicCoordinateSystem tangentialFrameViz =
new YoGraphicCoordinateSystem(
namePrefix + "TangentialFrame",
yoTangentialCircleFramePose,
0.2,
YoAppearance.Pink());
YoGraphicsList yoGraphicsList = new YoGraphicsList(namePrefix + "CircleTraj");
yoGraphicsList.add(currentPositionViz);
yoGraphicsList.add(currentAdjustedPositionViz);
yoGraphicsList.add(initialPositionViz);
yoGraphicsList.add(finalPositionViz);
yoGraphicsList.add(tangentialFrameViz);
yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList);
bagOfBalls =
new BagOfBalls(
numberOfBalls, 0.01, yoGraphicsList.getLabel(), registry, yoGraphicsListRegistry);
showViz = new BooleanYoVariable(namePrefix + "ShowViz", registry);
showViz.addVariableChangedListener(
new VariableChangedListener() {
public void variableChanged(YoVariable<?> v) {
boolean visible = showViz.getBooleanValue();
currentPositionViz.setVisible(visible);
currentAdjustedPositionViz.setVisible(visible);
initialPositionViz.setVisible(visible);
finalPositionViz.setVisible(visible);
tangentialFrameViz.setVisible(visible);
bagOfBalls.setVisible(visible);
}
});
showViz.notifyVariableChangedListeners();
} else {
visualize = false;
bagOfBalls = null;
showViz = null;
}
parentRegistry.addChild(registry);
}