public void setGainsByPolePlacement(double w0, double zeta) { x.setGainsByPolePlacement(w0, zeta); y.setGainsByPolePlacement(w0, zeta); }
YoDouble maxAccelerationYo = new YoDouble("max_Acceleration", registry); maxAccelerationYo.set(maxAcceleration, notifyListeners); AccelerationLimitedYoVariable processed = new AccelerationLimitedYoVariable(nameYo, registry, maxRateYo, maxAccelerationYo, dt); rawRate.update(raw.getDoubleValue()); rawAcceleration.update(rawRate.getDoubleValue()); processed.update(raw.getDoubleValue()); assertTrue(maxRate >= processed.getSmoothedRate().getDoubleValue()); assertTrue(maxAcceleration >= processed.getSmoothedAcceleration().getDoubleValue()); assertTrue(Math.abs(processed.getDoubleValue()) <= amplitude); assertTrue(Math.abs(processed.getSmoothedRate().getDoubleValue()) <= maxRate); assertTrue(Math.abs(processed.getSmoothedAcceleration().getDoubleValue()) <= maxAcceleration);
YoDouble maxAccelerationYo = new YoDouble("max_Acceleration", registry); maxAccelerationYo.set(maxAcceleration, notifyListeners); AccelerationLimitedYoVariable processed = new AccelerationLimitedYoVariable("processed", registry, maxRateYo, maxAccelerationYo, dt); assertEquals(nameYo, processed.getName()); rawRate.update(raw.getDoubleValue()); rawAcceleration.update(rawRate.getDoubleValue()); processed.update(raw.getDoubleValue()); assertTrue(maxRate >= processed.getSmoothedRate().getDoubleValue()); assertTrue(maxAcceleration >= processed.getSmoothedAcceleration().getDoubleValue()); assertEquals(2 * constant * time, processed.getSmoothedRate().getDoubleValue(), EPSILON); assertEquals(2 * constant, processed.getSmoothedAcceleration().getDoubleValue(), EPSILON); processed.initialize(3.0); assertEquals(3.0, processed.getDoubleValue(), EPSILON); assertTrue(processed.hasBeenInitialized()); assertEquals(0.0, processed.getSmoothedRate().getDoubleValue(), EPSILON); assertEquals(0.0, processed.getSmoothedAcceleration().getDoubleValue(), EPSILON);
YoDouble maxAccelerationYo = new YoDouble("max_Acceleration", registry); maxAccelerationYo.set(maxAcceleration, notifyListeners); AccelerationLimitedYoVariable processed = new AccelerationLimitedYoVariable(nameYo, registry, maxRateYo, maxAccelerationYo, dt); rawRate.update(raw.getDoubleValue()); rawAcceleration.update(rawRate.getDoubleValue()); processed.update(raw.getDoubleValue()); if (VISUALIZE) scs.tickAndUpdate(); assertTrue(maxRate >= processed.getSmoothedRate().getDoubleValue()); assertTrue(maxAcceleration >= processed.getSmoothedAcceleration().getDoubleValue()); assertTrue(Math.abs(processed.getDoubleValue()) <= amplitude); assertTrue(Math.abs(processed.getSmoothedRate().getDoubleValue()) <= maxRate); assertTrue(Math.abs(processed.getSmoothedAcceleration().getDoubleValue()) <= maxAcceleration); processed.reset(); assertFalse(processed.hasBeenInitialized()); assertEquals(0.0, processed.getSmoothedRate().getDoubleValue(), EPSILON); assertEquals(0.0, processed.getSmoothedAcceleration().getDoubleValue(), EPSILON);
AccelerationLimitedYoVariable smoothedYoVariable = new AccelerationLimitedYoVariable("smoothedVariable", registry, maxRateYo, maxAccelerationYo, dt); smoothedYoVariable.setGainsByPolePlacement(2.0 * Math.PI * breakFrequencyHertz, 1.0); smoothedYoVariable.setMaximumAcceleration(400.0); smoothedYoVariable.setMaximumRate(12.0); System.out.println("maximumOmega0 = " + maximumOmega0 + " = " + maximumOmega0 / (2.0 * Math.PI) + " hertz."); smoothedYoVariable.update(0.0); smoothedYoVariable.update(variable.getDoubleValue()); scs.tickAndUpdate(); smoothedYoVariable.update(variable.getDoubleValue()); scs.tickAndUpdate(); smoothedYoVariable.update(variable.getDoubleValue()); scs.tickAndUpdate();
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testUpdate() { setupSCSStuff(); maxRate = 10.0; maxAcceleration = 7.0; double dT = 1; YoDouble maxRateYo = new YoDouble("max_Rate", registry); maxRateYo.set(maxRate, notifyListeners); YoDouble maxAccelerationYo = new YoDouble("max_Acceleration", registry); maxAccelerationYo.set(maxAcceleration, notifyListeners); AccelerationLimitedYoVariable processed = new AccelerationLimitedYoVariable(nameYo, registry, maxRateYo, maxAccelerationYo, dT); processed.setMaximumRate(maxRate); processed.setMaximumAcceleration(maxAcceleration); for(double displacement = 1.0; displacement < 10000.0; displacement += 1000.0) { processed.update(displacement); assertTrue(processed.getSmoothedRate().getDoubleValue() <= maxRate); } }
public void update(double input) { if (!hasBeenInitialized.getBooleanValue()) initialize(input); double positionError = input - this.getDoubleValue(); double acceleration = -velocityGain.getDoubleValue() * smoothedRate.getDoubleValue() + positionGain.getDoubleValue() * positionError; acceleration = MathTools.clamp(acceleration, -maximumAcceleration.getDoubleValue(), maximumAcceleration.getDoubleValue()); smoothedAcceleration.set(acceleration); smoothedRate.add(smoothedAcceleration.getDoubleValue() * dt); smoothedRate.set(MathTools.clamp(smoothedRate.getDoubleValue(), maximumRate.getDoubleValue())); this.add(smoothedRate.getDoubleValue() * dt); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testSetAndGetGainsByPolePlacement() { setupSCSStuff(); YoDouble maxRateYo = new YoDouble("max_Rate", registry); maxRateYo.set(maxRate, notifyListeners); YoDouble maxAccelerationYo = new YoDouble("max_Acceleration", registry); maxAccelerationYo.set(maxAcceleration, notifyListeners); AccelerationLimitedYoVariable processed = new AccelerationLimitedYoVariable(nameYo, registry, maxRateYo, maxAccelerationYo, dt); double positionGain = random.nextDouble(); double velocityGain = random.nextDouble(); processed.setGainsByPolePlacement(positionGain, velocityGain); double positionGainResult = positionGain * positionGain; double velocityGainResult = 2 * positionGain * velocityGain; assertEquals(positionGainResult, processed.getPositionGain().getDoubleValue(), EPSILON); assertEquals(velocityGainResult, processed.getVelocityGain().getDoubleValue(), EPSILON); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testGetAndSetMaximumRateaAndAcceleration() { setupSCSStuff(); maxRate = 1.0; maxAcceleration = 1.0; YoDouble maxRateYo = new YoDouble("max_Rate", registry); maxRateYo.set(maxRate, notifyListeners); YoDouble maxAccelerationYo = new YoDouble("max_Acceleration", registry); maxAccelerationYo.set(maxAcceleration, notifyListeners); AccelerationLimitedYoVariable processed = new AccelerationLimitedYoVariable(nameYo, registry, maxRateYo, maxAccelerationYo, dt); processed.setMaximumRate(maxRate); processed.setMaximumAcceleration(maxAcceleration); assertEquals(maxRate, processed.getMaximumRate(), EPSILON); assertEquals(maxAcceleration, processed.getMaximumAcceleration(), EPSILON); maxRate = 8.5; maxAcceleration = 4.5; processed.setMaximumRate(maxRate); processed.setMaximumAcceleration(maxAcceleration); assertEquals(maxRate, processed.getMaximumRate(), EPSILON); assertEquals(maxAcceleration, processed.getMaximumAcceleration(), EPSILON); }
public static AccelerationLimitedYoFrameVector2d createAccelerationLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleYoVariable maxRate, DoubleYoVariable maxAcceleration, double dt, ReferenceFrame referenceFrame) { AccelerationLimitedYoVariable x = new AccelerationLimitedYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, maxAcceleration, dt); AccelerationLimitedYoVariable y = new AccelerationLimitedYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, maxAcceleration, dt); AccelerationLimitedYoFrameVector2d ret = new AccelerationLimitedYoFrameVector2d(x, y, referenceFrame); return ret; }
public void initialize(double input) { this.set(input); smoothedRate.set(0.0); smoothedAcceleration.set(0.0); this.hasBeenInitialized.set(true); }
maxAccelerationYo.set(maxAcceleration, notifyListeners); YoDouble inputVariable = new YoDouble("inputVariable", registry); AccelerationLimitedYoVariable processed = new AccelerationLimitedYoVariable(nameYo, registry, maxRateYo, maxAccelerationYo, inputVariable, dt); double beforeValue = processed.getDoubleValue(); processed.update(); double afterValue = processed.getDoubleValue(); if (processed.getSmoothedRate().getDoubleValue() > maxRate) fail(); scs.tickAndUpdate(); assertTrue(maxRate >= processed.getSmoothedRate().getDoubleValue()); assertTrue(maxAcceleration >= processed.getSmoothedAcceleration().getDoubleValue()); assertTrue(Math.abs(processed.getDoubleValue()) <= amplitude || isValueWithinMarginOfError(amplitude, processed.getDoubleValue(), permissibleErrorRatio)); assertTrue(Math.abs(processed.getSmoothedRate().getDoubleValue()) <= maxRate); assertTrue(Math.abs(processed.getSmoothedAcceleration().getDoubleValue()) <= maxAcceleration);
public void update(double input) { if (!hasBeenInitialized.getBooleanValue()) initialize(input); double positionError = input - this.getDoubleValue(); double acceleration = -velocityGain.getDoubleValue() * smoothedRate.getDoubleValue() + positionGain.getDoubleValue() * positionError; acceleration = MathTools.clipToMinMax(acceleration, -maximumAcceleration.getDoubleValue(), maximumAcceleration.getDoubleValue()); smoothedAcceleration.set(acceleration); smoothedRate.add(smoothedAcceleration.getDoubleValue() * dt); smoothedRate.set(MathTools.clipToMinMax(smoothedRate.getDoubleValue(), maximumRate.getDoubleValue())); this.add(smoothedRate.getDoubleValue() * dt); }
public static AccelerationLimitedYoFrameVector2d createAccelerationLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble maxRate, YoDouble maxAcceleration, double dt, ReferenceFrame referenceFrame) { AccelerationLimitedYoVariable x = new AccelerationLimitedYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, maxAcceleration, dt); AccelerationLimitedYoVariable y = new AccelerationLimitedYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, maxAcceleration, dt); AccelerationLimitedYoFrameVector2d ret = new AccelerationLimitedYoFrameVector2d(x, y, referenceFrame); return ret; }
public void initialize(double input) { this.set(input); smoothedRate.set(0.0); smoothedAcceleration.set(0.0); this.hasBeenInitialized.set(true); }
YoDouble maxAccelerationYo = new YoDouble("max_Acceleration", registry); maxAccelerationYo.set(maxAcceleration, notifyListeners); AccelerationLimitedYoVariable processed = new AccelerationLimitedYoVariable("processed", registry, maxRateYo, maxAccelerationYo, dt); rawRate.update(raw.getDoubleValue()); rawAcceleration.update(rawRate.getDoubleValue()); processed.update(raw.getDoubleValue()); assertTrue(maxRate >= processed.getSmoothedRate().getDoubleValue()); assertTrue(maxAcceleration >= processed.getSmoothedAcceleration().getDoubleValue()); assertEquals(value, processed.getSmoothedRate().getDoubleValue(), EPSILON); assertEquals(0.0, processed.getSmoothedAcceleration().getDoubleValue(), EPSILON);
public static AccelerationLimitedYoFrameVector2d createAccelerationLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleYoVariable maxRate, DoubleYoVariable maxAcceleration, double dt, YoFrameVector2d unfilteredVector) { AccelerationLimitedYoVariable x = new AccelerationLimitedYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, maxAcceleration, unfilteredVector.getYoX(), dt); AccelerationLimitedYoVariable y = new AccelerationLimitedYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, maxAcceleration, unfilteredVector.getYoY(), dt); AccelerationLimitedYoFrameVector2d ret = new AccelerationLimitedYoFrameVector2d(x, y, unfilteredVector.getReferenceFrame()); return ret; }