public LineTrajectory(double controlDT, Tuple3DReadOnly initialPosition, YoVariableRegistry parentRegistry, YoGraphicsListRegistry graphicsListRegistry)
{
this.controlDT = controlDT;
Vector3D initialPositionA = new Vector3D(initialPosition);
Vector3D initialPositionB = new Vector3D(initialPosition);
initialPositionA.addX(0.025);
initialPositionB.addX(-0.025);
pointA = new ParameterVector3D("PointA", initialPositionA, registry);
pointB = new ParameterVector3D("PointB", initialPositionB, registry);
frequency = new DoubleParameter("Frequency", registry, 0.25);
maxVelocity = new DoubleParameter("MaxVelocity", registry, 0.1);
limitedPointA = new RateLimitedYoFramePoint("PointALim", "", registry, maxVelocity, controlDT, createFrameTuple(ReferenceFrame.getWorldFrame(), pointA));
limitedPointB = new RateLimitedYoFramePoint("PointBLim", "", registry, maxVelocity, controlDT, createFrameTuple(ReferenceFrame.getWorldFrame(), pointB));
pointAViz = new YoGraphicPosition("PointAViz", limitedPointA, 0.025, YoAppearance.Blue());
pointBViz = new YoGraphicPosition("PointBViz", limitedPointB, 0.025, YoAppearance.Blue());
graphicsListRegistry.registerYoGraphic(getClass().getSimpleName(), pointAViz);
graphicsListRegistry.registerYoGraphic(getClass().getSimpleName(), pointBViz);
parentRegistry.addChild(registry);
}