@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testDelayedYoVariableZeroTickDelay() { YoVariableRegistry registry = new YoVariableRegistry("registry"); YoDouble variableToDelay = new YoDouble("variableToDelay", registry); int ticksToDelay = 0; variableToDelay.set(0.0); DelayedYoDouble delayedYoVariable = new DelayedYoDouble("delayedVariable" + ticksToDelay, "", variableToDelay, ticksToDelay, registry); assertEquals(delayedYoVariable.getDoubleValue(), 0.0, 1e-7); variableToDelay.set(1.0); delayedYoVariable.update(); assertEquals(delayedYoVariable.getDoubleValue(), 1.0, 1e-7); variableToDelay.set(2.0); delayedYoVariable.update(); assertEquals(delayedYoVariable.getDoubleValue(), 2.0, 1e-7); variableToDelay.set(3.0); delayedYoVariable.update(); assertEquals(delayedYoVariable.getDoubleValue(), 3.0, 1e-7); delayedYoVariable.update(); assertEquals(delayedYoVariable.getDoubleValue(), 3.0, 1e-7); delayedYoVariable.update(); assertEquals(delayedYoVariable.getDoubleValue(), 3.0, 1e-7); delayedYoVariable.update(); assertEquals(delayedYoVariable.getDoubleValue(), 3.0, 1e-7); }
public void reset() { for (YoDouble var : previousYoVariables) { var.set(variableToDelay.getDoubleValue()); } this.set(variableToDelay.getDoubleValue()); } }
delayedJointTorque.update(); tau = delayedJointTorque.getDoubleValue();
DelayedYoDouble delayedSignal = new DelayedYoDouble("delayedSignal", "", referenceSignal, expectedDelayInTicks, registry); delayedSignal.update(); delayEstimatorBetweenTwoSignals.update();
@Override public void setLowLevelControllerCoreOutput(FullHumanoidRobotModel controllerRobotModel, JointDesiredOutputList lowLevelControllerOutput, RawJointSensorDataHolderMap rawJointSensorDataHolderMap) { for (int i = 0; i < lowLevelControllerOutput.getNumberOfJointsWithDesiredOutput(); i++) { String jointName = lowLevelControllerOutput.getJointName(i); OutputDataSet data = new OutputDataSet(); data.rawJointTorque = new YoDouble("tau_desired_" + jointName, registry); data.delayedJointTorque = new DelayedYoDouble("tau_delayed_" + jointName, "", data.rawJointTorque, TICKS_TO_DELAY, registry); data.simulatedJoint = robot.getOneDegreeOfFreedomJoint(jointName); data.jointData = lowLevelControllerOutput.getJointDesiredOutput(i); revoluteJoints.add(data); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testDelayedYoVariableOneTickDelay() { YoVariableRegistry registry = new YoVariableRegistry("registry"); YoDouble variableToDelay = new YoDouble("variableToDelay", registry); int ticksToDelay = 1; variableToDelay.set(0.0); DelayedYoDouble delayedYoVariable = new DelayedYoDouble("delayedVariable" + ticksToDelay, "", variableToDelay, ticksToDelay, registry); assertEquals(delayedYoVariable.getDoubleValue(), 0.0, 1e-7); variableToDelay.set(1.0); delayedYoVariable.update(); assertEquals(delayedYoVariable.getDoubleValue(), 0.0, 1e-7); variableToDelay.set(2.0); delayedYoVariable.update(); assertEquals(delayedYoVariable.getDoubleValue(), 1.0, 1e-7); variableToDelay.set(3.0); delayedYoVariable.update(); assertEquals(delayedYoVariable.getDoubleValue(), 2.0, 1e-7); delayedYoVariable.update(); assertEquals(delayedYoVariable.getDoubleValue(), 3.0, 1e-7); delayedYoVariable.update(); assertEquals(delayedYoVariable.getDoubleValue(), 3.0, 1e-7); delayedYoVariable.update(); assertEquals(delayedYoVariable.getDoubleValue(), 3.0, 1e-7); }
public void update() { if (previousYoVariables.length == 0) { this.set(variableToDelay.getDoubleValue()); return; } this.set(previousYoVariables[0].getDoubleValue()); for (int i = 0; i < previousYoVariables.length - 1; i++) { previousYoVariables[i].set(previousYoVariables[i + 1].getDoubleValue()); } previousYoVariables[previousYoVariables.length - 1].set(variableToDelay.getDoubleValue()); }
variableToDelay.set(firstValue); DelayedYoDouble delayedYoVariable = new DelayedYoDouble("delayedVariable" + ticksToDelay, "", variableToDelay, ticksToDelay, registry); assertEquals(delayedYoVariable.getDoubleValue(), firstValue, 1e-7); delayedYoVariable.update(); assertEquals(delayedYoVariable.getDoubleValue(), firstValue, 1e-7); assertEquals(delayedYoVariable.getDoubleValue(), valuesToSet[i - ticksToDelay], 1e-7);
public DelayedYoDouble(String name, String description, YoDouble variableToDelay, int ticksToDelay, YoVariableRegistry registry) { super(name, description, registry); this.variableToDelay = variableToDelay; previousYoVariables = new YoDouble[ticksToDelay]; for (int i = 0; i < ticksToDelay; i++) { previousYoVariables[i] = new YoDouble(name + "_previous" + i, registry); previousYoVariables[i].set(variableToDelay.getDoubleValue()); } this.set(variableToDelay.getDoubleValue()); }
DelayedYoDouble delayedSignal = new DelayedYoDouble("delayedSignal", "", referenceSignal, expectedDelayInTicks, registry); YoDouble noisyDelayedSignal = new YoDouble("noisyDelayedSignal", registry); delayedSignal.update(); noisyDelayedSignal.set(delayedSignal.getDoubleValue()); noisyDelayedSignal.add(0.5 * (random.nextDouble() - 0.5) + 1.5); delayEstimatorBetweenTwoSignals.update();
DelayedYoDouble delayedSignal = new DelayedYoDouble("delayedSignal", "", referenceSignal, expectedDelayInTicks, registry); YoDouble shiftedDelayedSignal = new YoDouble("shiftedDelayedSignal", registry); delayedSignal.update(); shiftedDelayedSignal.set(delayedSignal.getDoubleValue() + 2.5); delayEstimatorBetweenTwoSignals.update();