@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testReferenceAndInputBothPositiveNoOvershootReferenceGreaterThanInput() { Random random = new Random(1976L); registry = new YoVariableRegistry("registry"); variable = new DeltaLimitedYoVariable("testVar", registry, 0.0); for(int i = 0; i < 60000; i++) { double reference = RandomNumbers.nextInt(random, RANDOM_LOWER_BOUND, RANDOM_UPPER_BOUND); double input = reference / 2.0; double delta = Math.abs(input - reference); variable.setMaxDelta(delta); variable.updateOutput(reference, input); Assert.assertTrue(input > 0.0); Assert.assertTrue(reference > 0.0); Assert.assertTrue(input < reference); Assert.assertFalse(variable.isLimitingActive()); Assert.assertEquals(input, variable.getDoubleValue(), 1e-8); } }
public void computeAndUpdateJointPosition() { double currentJointAngle = yoPositionJointHandleHolder.getQ(); double desiredPosition = yoPositionJointHandleHolder.getControllerPositionDesired(); positionStepSizeLimiter.updateOutput(currentJointAngle, desiredPosition); yoPositionJointHandleHolder.setDesiredPosition(positionStepSizeLimiter.getDoubleValue()); } }
public void updateOutput(double actual, double desired) { this.desired.set(desired); this.actual.set(actual); updateOutput(); }
public PositionJointHolder(ValkyrieRosControlSliderBoard valkyrieRosControlSliderBoard, OneDoFJointBasics joint, PositionJointHandle handle, YoVariableRegistry parentRegistry, double dt) { super(valkyrieRosControlSliderBoard, joint, parentRegistry, dt); this.handle = handle; this.positionStepSizeLimiter = new DeltaLimitedYoVariable(handle.getName() + "PositionStepSizeLimiter", registry, 0.15); parentRegistry.addChild(registry); }
private void updateOutput() { double actualDoubleValue = actual.getDoubleValue(); double desiredDoubleValue = desired.getDoubleValue(); double maxDeltaDoubleValue = Math.abs(maxDelta.getDoubleValue()); double rawDelta = actualDoubleValue - desiredDoubleValue; double sign = Math.signum(rawDelta); double requestedDelta = Math.abs(rawDelta); double overshoot = maxDeltaDoubleValue - requestedDelta; if(overshoot < 0) { desiredDoubleValue = actualDoubleValue - maxDeltaDoubleValue * sign; isLimitingActive.set(true); } else { isLimitingActive.set(false); } this.set(desiredDoubleValue); } }
@Override public void update() { joint.setQ(handle.getPosition()); joint.setQd(handle.getVelocity()); bl_qd.update(); joint.setTau(handle.getEffort()); q.set(joint.getQ()); qd.set(joint.getQd()); tau.set(joint.getTau()); positionStepSizeLimiter.updateOutput(q.getDoubleValue(), q_d.getDoubleValue()); handle.setDesiredPosition(positionStepSizeLimiter.getDoubleValue()); } }
public void updateOutput(double actual, double desired) { this.desired.set(desired); this.actual.set(actual); updateOutput(); }
public ValkyrieRosControlPositionJointControlCommandCalculator(YoPositionJointHandleHolder yoPositionJointHandleHolder, YoVariableRegistry parentRegistry) { this.yoPositionJointHandleHolder = yoPositionJointHandleHolder; String positionJumpLimiterBaseName = yoPositionJointHandleHolder.getName(); YoVariableRegistry registry = new YoVariableRegistry(positionJumpLimiterBaseName + "Command"); this.positionStepSizeLimiter = new DeltaLimitedYoVariable(positionJumpLimiterBaseName + "PositionStepSizeLimiter", registry, 0.15); parentRegistry.addChild(registry); }
private void updateOutput() { double actualDoubleValue = actual.getDoubleValue(); double desiredDoubleValue = desired.getDoubleValue(); double maxDeltaDoubleValue = Math.abs(maxDelta.getDoubleValue()); double rawDelta = actualDoubleValue - desiredDoubleValue; double sign = Math.signum(rawDelta); double requestedDelta = Math.abs(rawDelta); double overshoot = maxDeltaDoubleValue - requestedDelta; if(overshoot < 0) { desiredDoubleValue = actualDoubleValue - maxDeltaDoubleValue * sign; isLimitingActive.set(true); } else { isLimitingActive.set(false); } this.set(desiredDoubleValue); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testPositiveReferenceNegativeInputNoOvershoot() { Random random = new Random(1976L); registry = new YoVariableRegistry("registry"); variable = new DeltaLimitedYoVariable("testVar", registry, 0.0); for(int i = 0; i < 60000; i++) { double reference = RandomNumbers.nextInt(random, RANDOM_LOWER_BOUND, RANDOM_UPPER_BOUND); double input = reference * -0.5; double delta = Math.abs(input - reference); variable.setMaxDelta(delta); variable.updateOutput(reference, input); Assert.assertTrue(input < 0.0); Assert.assertTrue(reference > 0.0); Assert.assertTrue(input < reference); Assert.assertFalse(variable.isLimitingActive()); Assert.assertEquals(input, variable.getDoubleValue(), 1e-8); } }
@ContinuousIntegrationTest(estimatedDuration = 0.1) @Test(timeout=300000) public void testReferenceAndInputBothNegativeWithOvershootInputGreaterThanReference() { Random random = new Random(1976L); registry = new YoVariableRegistry("registry"); variable = new DeltaLimitedYoVariable("testVar", registry, 0.0); for(int i = 0; i < 60000; i++) { double reference = RandomNumbers.nextInt(random, RANDOM_LOWER_BOUND, RANDOM_UPPER_BOUND) * -1.0; double input = reference / 2.0; double delta = Math.abs(input - reference) / 2.0; variable.setMaxDelta(delta); variable.updateOutput(reference, input); double overshoot = Math.abs(input - reference) - delta; double expectedClip = input - overshoot; Assert.assertTrue(input < 0.0); Assert.assertTrue(reference < 0.0); Assert.assertTrue(input > reference); Assert.assertTrue(variable.isLimitingActive()); Assert.assertEquals("Variable not clipped correctly\nReference: " + reference + "\nInput: " + input + "\nMagnitude of requested delta: " + Math.abs(input - reference) + "\nMax Allowed Delta: " + delta + "\nOvershoot: " + overshoot, expectedClip, variable.getDoubleValue(), 1e-8); } }
@ContinuousIntegrationTest(estimatedDuration = 0.1) @Test(timeout=300000) public void testReferenceAndInputBothPositiveWithOvershootReferenceGreaterThanInput() { Random random = new Random(1976L); registry = new YoVariableRegistry("registry"); variable = new DeltaLimitedYoVariable("testVar", registry, 0.0); for(int i = 0; i < 60000; i++) { double reference = RandomNumbers.nextInt(random, RANDOM_LOWER_BOUND, RANDOM_UPPER_BOUND); double input = reference / 2.0; double delta = Math.abs(input - reference) / 2.0; variable.setMaxDelta(delta); variable.updateOutput(reference, input); double overshoot = Math.abs(input - reference) - delta; double expectedClip = input + overshoot; Assert.assertTrue(input > 0.0); Assert.assertTrue(reference > 0.0); Assert.assertTrue(input < reference); Assert.assertTrue(variable.isLimitingActive()); Assert.assertEquals("Variable not clipped correctly\nReference: " + reference + "\nInput: " + input + "\nMagnitude of requested delta: " + Math.abs(input - reference) + "\nMax Allowed Delta: " + delta + "\nOvershoot: " + overshoot, expectedClip, variable.getDoubleValue(), 1e-8); } }
variable = new DeltaLimitedYoVariable("testVar", registry, 0.0); double delta = Math.abs(input - reference) / 2.0; variable.setMaxDelta(delta); variable.updateOutput(reference, input); Assert.assertTrue(reference < 0.0); Assert.assertTrue(input > reference); Assert.assertTrue(variable.isLimitingActive()); Assert.assertEquals("Variable not clipped correctly\nReference: " + reference + "\nInput: " + input + "\nMagnitude of requested delta: " + Math .abs(input - reference) + "\nMax Allowed Delta: " + delta + "\nOvershoot: " + overshoot, expectedClip, variable.getDoubleValue(), 1e-8); input = variable.getDoubleValue(); variable.updateOutput(reference, input); double newRequestedDelta = Math.abs(input - reference); Assert.assertFalse(newRequestedDelta > delta); Assert.assertFalse(variable.isLimitingActive()); Assert.assertEquals("Variable not clipped correctly\nReference: " + reference + "\nInput: " + input + "\nMagnitude of requested delta: " + Math .abs(input - reference) + "\nMax Allowed Delta: " + delta + "\nOvershoot: " + overshoot, expectedClip, variable.getDoubleValue(), 1e-8);
@ContinuousIntegrationTest(estimatedDuration = 0.1) @Test(timeout=300000) public void testPositiveReferenceNegativeInputWithOvershoot() { Random random = new Random(1976L); registry = new YoVariableRegistry("registry"); variable = new DeltaLimitedYoVariable("testVar", registry, 0.0); for(int i = 0; i < 60000; i++) { double reference = RandomNumbers.nextInt(random, RANDOM_LOWER_BOUND, RANDOM_UPPER_BOUND); double input = reference * -0.5; double delta = Math.abs(input - reference) / 2.0; variable.setMaxDelta(delta); variable.updateOutput(reference, input); double overshoot = Math.abs(input - reference) - delta; double expectedClip = input + overshoot; Assert.assertTrue(input < 0.0); Assert.assertTrue(reference > 0.0); Assert.assertTrue(input < reference); Assert.assertTrue(variable.isLimitingActive()); Assert.assertEquals("Variable not clipped correctly\nReference: " + reference + "\nInput: " + input + "\nMagnitude of requested delta: " + Math.abs(input - reference) + "\nMax Allowed Delta: " + delta + "\nOvershoot: " + overshoot, expectedClip, variable.getDoubleValue(), 1e-8); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testNegativeReferencePositiveInputNoOvershoot() { Random random = new Random(1976L); registry = new YoVariableRegistry("registry"); variable = new DeltaLimitedYoVariable("testVar", registry, 0.0); for(int i = 0; i < 60000; i++) { double reference = RandomNumbers.nextInt(random, RANDOM_LOWER_BOUND, RANDOM_UPPER_BOUND) * -1.0; double input = reference * -0.5; double delta = Math.abs(input - reference); variable.setMaxDelta(delta); variable.updateOutput(reference, input); Assert.assertTrue(input > 0.0); Assert.assertTrue(reference < 0.0); Assert.assertTrue(input > reference); Assert.assertFalse(variable.isLimitingActive()); Assert.assertEquals(input, variable.getDoubleValue(), 1e-8); } }
@ContinuousIntegrationTest(estimatedDuration = 0.1) @Test(timeout=300000) public void testReferenceAndInputBothNegativeWithOvershootReferenceGreaterThanInput() { Random random = new Random(1976L); registry = new YoVariableRegistry("registry"); variable = new DeltaLimitedYoVariable("testVar", registry, 0.0); for(int i = 0; i < 60000; i++) { double reference = RandomNumbers.nextInt(random, RANDOM_LOWER_BOUND, RANDOM_UPPER_BOUND) * -1.0; double input = reference * 2; double delta = Math.abs(input - reference) / 2.0; variable.setMaxDelta(delta); variable.updateOutput(reference, input); double overshoot = Math.abs(input - reference) - delta; double expectedClip = input + overshoot; Assert.assertTrue(input < 0.0); Assert.assertTrue(reference < 0.0); Assert.assertTrue(input < reference); Assert.assertTrue(variable.isLimitingActive()); Assert.assertEquals("Variable not clipped correctly\nReference: " + reference + "\nInput: " + input + "\nMagnitude of requested delta: " + Math.abs(input - reference) + "\nMax Allowed Delta: " + delta + "\nOvershoot: " + overshoot, expectedClip, variable.getDoubleValue(), 1e-8); } }
@ContinuousIntegrationTest(estimatedDuration = 0.1) @Test(timeout=300000) public void testReferenceAndInputBothPositiveWithOvershootInputGreaterThanReference() { Random random = new Random(1976L); registry = new YoVariableRegistry("registry"); variable = new DeltaLimitedYoVariable("testVar", registry, 0.0); for(int i = 0; i < 60000; i++) { double reference = RandomNumbers.nextInt(random, RANDOM_LOWER_BOUND, RANDOM_UPPER_BOUND); double input = reference * 2; double delta = Math.abs(input - reference) / 2.0; variable.setMaxDelta(delta); variable.updateOutput(reference, input); double overshoot = Math.abs(input - reference) - delta; double expectedClip = input - overshoot; Assert.assertTrue(input > 0.0); Assert.assertTrue(reference > 0.0); Assert.assertTrue(input > reference); Assert.assertTrue(variable.isLimitingActive()); Assert.assertEquals("Variable not clipped correctly\nReference: " + reference + "\nInput: " + input + "\nMagnitude of requested delta: " + Math.abs(input - reference) + "\nMax Allowed Delta: " + delta + "\nOvershoot: " + overshoot, expectedClip, variable.getDoubleValue(), 1e-8); } }
@ContinuousIntegrationTest(estimatedDuration = 0.1) @Test(timeout=300000) public void testNegativeReferencePositiveInputWithOvershoot() { Random random = new Random(1976L); registry = new YoVariableRegistry("registry"); variable = new DeltaLimitedYoVariable("testVar", registry, 0.0); for(int i = 0; i < 60000; i++) { double reference = RandomNumbers.nextInt(random, RANDOM_LOWER_BOUND, RANDOM_UPPER_BOUND) * -1.0; double input = reference * -0.5; double delta = Math.abs(input - reference) / 2.0; variable.setMaxDelta(delta); variable.updateOutput(reference, input); double overshoot = Math.abs(input - reference) - delta; double expectedClip = input - overshoot; Assert.assertTrue(input > 0.0); Assert.assertTrue(reference < 0.0); Assert.assertTrue(input > reference); Assert.assertTrue(variable.isLimitingActive()); Assert.assertEquals("Variable not clipped correctly\nReference: " + reference + "\nInput: " + input + "\nMagnitude of requested delta: " + Math.abs(input - reference) + "\nMax Allowed Delta: " + delta + "\nOvershoot: " + overshoot, expectedClip, variable.getDoubleValue(), 1e-8); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testReferenceAndInputBothNegativeNoOvershootInputGreaterThanReference() { Random random = new Random(1976L); registry = new YoVariableRegistry("registry"); variable = new DeltaLimitedYoVariable("testVar", registry, 0.0); for(int i = 0; i < 60000; i++) { double reference = RandomNumbers.nextInt(random, RANDOM_LOWER_BOUND, RANDOM_UPPER_BOUND) * -1.0; double input = reference / 2.0; double delta = Math.abs(input - reference); variable.setMaxDelta(delta); variable.updateOutput(reference, input); Assert.assertTrue(input < 0.0); Assert.assertTrue(reference < 0.0); Assert.assertTrue(input > reference); Assert.assertFalse(variable.isLimitingActive()); Assert.assertEquals(input, variable.getDoubleValue(), 1e-8); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testReferenceAndInputBothNegativeNoOvershootReferenceGreaterThanInput() { Random random = new Random(1976L); registry = new YoVariableRegistry("registry"); variable = new DeltaLimitedYoVariable("testVar", registry, 0.0); for(int i = 0; i < 60000; i++) { double reference = RandomNumbers.nextInt(random, RANDOM_LOWER_BOUND, RANDOM_UPPER_BOUND) * -1.0; double input = reference * 2; double delta = Math.abs(input - reference); variable.setMaxDelta(delta); variable.updateOutput(reference, input); Assert.assertTrue(input < 0.0); Assert.assertTrue(reference < 0.0); Assert.assertTrue(input < reference); Assert.assertFalse(variable.isLimitingActive()); Assert.assertEquals(input, variable.getDoubleValue(), 1e-8); } }