public RateLimitedYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, ReferenceFrame referenceFrame) { this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, null, referenceFrame); }
@Override public ReferenceFrame getReferenceFrame() { return position.getReferenceFrame(); }
public void update(double xUnfiltered, double yUnfiltered, double zUnfiltered) { if (!hasBeenCalled.getBooleanValue() || containsNaN()) { hasBeenCalled.set(true); set(xUnfiltered, yUnfiltered, zUnfiltered); } if (maxRateVariable.getValue() < 0) throw new RuntimeException("The maxRate parameter in the " + getClass().getSimpleName() + " cannot be negative."); differenceVector.setToZero(getReferenceFrame()); differenceVector.set(xUnfiltered, yUnfiltered, zUnfiltered); differenceVector.sub(getX(), getY(), getZ()); limited.set(differenceVector.clipToMaxLength(maxRateVariable.getValue() * dt)); add(differenceVector); } }
private RateLimitedYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, FrameTuple3DReadOnly rawPosition, ReferenceFrame referenceFrame) { super(namePrefix, nameSuffix, referenceFrame, registry); this.hasBeenCalled = new YoBoolean(namePrefix + "HasBeenCalled" + nameSuffix, registry); this.limited = new YoBoolean(namePrefix + "Limited" + nameSuffix, registry); if (maxRate == null) maxRate = createMaxRateYoDouble(namePrefix, nameSuffix, Double.POSITIVE_INFINITY, registry); maxRateVariable = maxRate; this.rawPosition = rawPosition; this.dt = dt; reset(); }
public void update(FrameTuple3DReadOnly frameVectorUnfiltered) { checkReferenceFrameMatch(frameVectorUnfiltered); update(frameVectorUnfiltered.getX(), frameVectorUnfiltered.getY(), frameVectorUnfiltered.getZ()); }
private RateLimitedYoFramePose(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, FramePose3DReadOnly rawPose, ReferenceFrame referenceFrame) { if (rawPose != null) { this.position = new RateLimitedYoFramePoint(namePrefix, "Position" + nameSuffix, registry, maxRate, dt, rawPose.getPosition()); this.orientation = new RateLimitedYoFrameQuaternion(namePrefix, "Orientation" + nameSuffix, registry, maxRate, dt, rawPose.getOrientation()); } else { this.position = new RateLimitedYoFramePoint(namePrefix, "Position" + nameSuffix, registry, maxRate, dt, referenceFrame); this.orientation = new RateLimitedYoFrameQuaternion(namePrefix, "Orientation" + nameSuffix, registry, maxRate, dt, referenceFrame); } reset(); }
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); }
public RateLimitedYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, FrameTuple3DReadOnly rawPosition) { this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, rawPosition, rawPosition.getReferenceFrame()); }
public void reset() { position.reset(); orientation.reset(); }