public void update(FrameQuaternionReadOnly frameOrientationUnfiltered) { checkReferenceFrameMatch(frameOrientationUnfiltered); update((QuaternionReadOnly) frameOrientationUnfiltered); }
public RateLimitedYoFrameQuaternion(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, ReferenceFrame referenceFrame) { this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, referenceFrame, null); }
public void reset() { position.reset(); orientation.reset(); }
MutableDouble maxRate = new MutableDouble(); YoVariableRegistry registry = new YoVariableRegistry("dummy"); RateLimitedYoFrameQuaternion rateLimitedQuaternion = new RateLimitedYoFrameQuaternion("blop", "", registry, (DoubleProvider) () -> maxRate.doubleValue(), dt, ReferenceFrame.getWorldFrame()); rateLimitedQuaternion.update(new Quaternion()); Quaternion goalQuaternion = EuclidCoreRandomTools.nextQuaternion(random); double distanceToGoal = Math.abs(AngleTools.trimAngleMinusPiToPi(rateLimitedQuaternion.distance(goalQuaternion))); if (distanceToGoal / dt < maxRate.doubleValue()) rateLimitedQuaternion.update(goalQuaternion); EuclidCoreTestTools.assertQuaternionGeometricallyEquals(goalQuaternion, rateLimitedQuaternion, EPSILON); rateLimitedQuaternion.update(goalQuaternion); double distance = Math.abs(AngleTools.trimAngleMinusPiToPi(rateLimitedQuaternion.distance(goalQuaternion))); assertTrue(distance < previousDistance); angularVelocity.update(); assertEquals("difference: " + Math.abs(rate - maxRate.doubleValue()), rate, maxRate.doubleValue(), EPSILON); previousDistance = distance; assertFalse(rateLimitedQuaternion.geometricallyEquals(goalQuaternion, EPSILON)); rateLimitedQuaternion.update(goalQuaternion); EuclidCoreTestTools.assertQuaternionGeometricallyEquals(goalQuaternion, rateLimitedQuaternion, EPSILON);
public void update(QuaternionReadOnly quaternionUnfiltered) if (!hasBeenCalled.getBooleanValue() || containsNaN()) set(quaternionUnfiltered); return; if (dot(quaternionUnfiltered) > 0.0) multiply(difference); set(quaternionUnfiltered);
public void update() { if (rawQuaternion == null) { throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " + "position variable to call update(), otherwise use update(double)"); } update(rawQuaternion); }
public RateLimitedYoFrameQuaternion(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, ReferenceFrame referenceFrame, QuaternionReadOnly rawQuaternion) { super(namePrefix, nameSuffix, referenceFrame, registry); hasBeenCalled = new YoBoolean(namePrefix + "HasBeenCalled" + nameSuffix, registry); limited = new YoBoolean(namePrefix + "Limited" + nameSuffix, registry); if (maxRate == null) maxRate = createMaxRateYoDouble(namePrefix, nameSuffix, Double.POSITIVE_INFINITY, registry); maxRateVariable = maxRate; this.rawQuaternion = rawQuaternion; this.dt = dt; reset(); }
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 void update() { position.update(); orientation.update(); set(position, orientation); }
public void update(FramePose3DReadOnly framePoseUnfiltered) { checkReferenceFrameMatch(framePoseUnfiltered); position.update(framePoseUnfiltered.getPosition()); orientation.update(framePoseUnfiltered.getOrientation()); set(position, orientation); }
public RateLimitedYoFrameQuaternion(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, ReferenceFrame referenceFrame, QuaternionReadOnly rawQuaternion) { this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, referenceFrame, rawQuaternion); }
public RateLimitedYoFrameQuaternion(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, FrameQuaternionReadOnly rawQuaternion) { this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, rawQuaternion.getReferenceFrame(), rawQuaternion); }