public void updateOutput(double actual, double desired) { this.desired.set(desired); this.actual.set(actual); updateOutput(); }
public void updateOutput(double actual, double desired) { this.desired.set(desired); this.actual.set(actual); updateOutput(); }
public void computeAndUpdateJointPosition() { double currentJointAngle = yoPositionJointHandleHolder.getQ(); double desiredPosition = yoPositionJointHandleHolder.getControllerPositionDesired(); positionStepSizeLimiter.updateOutput(currentJointAngle, desiredPosition); yoPositionJointHandleHolder.setDesiredPosition(positionStepSizeLimiter.getDoubleValue()); } }
@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.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); } }
@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); } }
variable.updateOutput(reference, input); variable.updateOutput(reference, input); double newRequestedDelta = Math.abs(input - reference); Assert.assertFalse(newRequestedDelta > delta);
@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.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); } }
@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.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); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testReferenceAndInputBothPositiveNoOvershootInputGreaterThanReference() { 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); 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); } }
@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()); } }