@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testClippingLeakRate() { YoPIDGains pidGains = new YoPIDGains("test", new YoVariableRegistry("PIDGainsRegistry")); Random random = new Random(); for (int i = 0; i < 1000; i++) { double integratorLeakRatio = random.nextDouble() * 100; pidGains.setIntegralLeakRatio(integratorLeakRatio); assertTrue(pidGains.getYoIntegralLeakRatio().getDoubleValue() <= 1.0); integratorLeakRatio = -random.nextDouble() * 100; pidGains.setIntegralLeakRatio(integratorLeakRatio); assertTrue(pidGains.getYoIntegralLeakRatio().getDoubleValue() >= 0.0); } }
public LowLevelActuatorSimulator(OneDegreeOfFreedomJoint simulatedJoint, LowLevelStateReadOnly jointDesiredOutput, double controlDT, double kp, double kd, LowLevelActuatorMode actuatorMode) { this.controlDT = controlDT; this.actuatorMode = actuatorMode; registry = new YoVariableRegistry(simulatedJoint.getName() + name); gains = new YoPIDGains(simulatedJoint.getName() + "Actuator", registry); jointController = new PIDController(gains, simulatedJoint.getName() + "LowLevelActuatorSimulator", registry); gains.setKp(kp); gains.setKd(kd); this.simulatedJoint = simulatedJoint; this.actuatorDesireds = jointDesiredOutput; }
YoPIDGains gains = new YoPIDGains("Hand", registry); gains.setKp(7.0); gains.setKi(3.0);
@ContinuousIntegrationTest(estimatedDuration = 0.3) @Test(timeout=300000) public void testPIDControllerConstructorFromGains() { YoVariableRegistry registry = new YoVariableRegistry("robert"); double proportional = random.nextDouble(); double integral = random.nextDouble(); double derivative = random.nextDouble(); double maxError = random.nextDouble(); double deadband = random.nextDouble(); double leakRate = random.nextDouble(); double maxOutput = 100 * random.nextDouble(); YoPIDGains pidGains = new YoPIDGains("", registry); pidGains.setKp(proportional); pidGains.setKi(integral); pidGains.setKd(derivative); pidGains.setMaximumIntegralError(maxError); pidGains.setPositionDeadband(deadband); pidGains.setIntegralLeakRatio(leakRate); pidGains.setMaximumFeedback(maxOutput); PIDController pid = new PIDController(pidGains, "", registry); assertEquals(proportional, pid.getProportionalGain(), 0.001); assertEquals(integral, pid.getIntegralGain(), 0.001); assertEquals(derivative, pid.getDerivativeGain(), 0.001); assertEquals(maxError, pid.getMaxIntegralError(), 0.001); assertEquals(deadband, pid.getPositionDeadband(), 0.001); assertEquals(leakRate, pid.getIntegralLeakRatio(), 0.001); assertEquals(maxOutput, pid.getMaximumFeedback(), 1e-5); }
@ContinuousIntegrationTest(estimatedDuration = 0.2) @Test(timeout=300000) public void testPIDControllerConstructorFromGains2() { YoVariableRegistry registry = new YoVariableRegistry("robert"); double proportional = random.nextDouble(); double integral = random.nextDouble(); double derivative = random.nextDouble(); double maxError = random.nextDouble(); double deadband = random.nextDouble(); double maxOutput = random.nextDouble() * 100; YoPIDGains pidGains = new YoPIDGains("", registry); pidGains.setKp(proportional); pidGains.setKi(integral); pidGains.setKd(derivative); pidGains.setMaximumIntegralError(maxError); pidGains.setPositionDeadband(deadband); pidGains.setMaximumFeedback(maxOutput); PIDController pid = new PIDController(pidGains, "", registry); assertEquals(proportional, pid.getProportionalGain(), 0.001); assertEquals(integral, pid.getIntegralGain(), 0.001); assertEquals(derivative, pid.getDerivativeGain(), 0.001); assertEquals(maxError, pid.getMaxIntegralError(), 0.001); assertEquals(deadband, pid.getPositionDeadband(), 0.001); assertEquals(maxOutput, pid.getMaximumFeedback(), 0.001); assertEquals(1.0, pid.getIntegralLeakRatio(), 0.001); }
@ContinuousIntegrationTest(estimatedDuration = 0.2) @Test(timeout=300000) public void testPIDControllerConstructorFromGains3() { YoVariableRegistry registry = new YoVariableRegistry("robert"); double proportional = random.nextDouble(); double integral = random.nextDouble(); double derivative = random.nextDouble(); double maxIntegralError = random.nextDouble(); double maxOutput = random.nextDouble() * 100; YoPIDGains pidGains = new YoPIDGains("", registry); pidGains.setKp(proportional); pidGains.setKi(integral); pidGains.setKd(derivative); pidGains.setMaximumIntegralError(maxIntegralError); pidGains.setMaximumFeedback(maxOutput); PIDController pid = new PIDController(pidGains, "", registry); assertEquals(proportional, pid.getProportionalGain(), 0.001); assertEquals(integral, pid.getIntegralGain(), 0.001); assertEquals(derivative, pid.getDerivativeGain(), 0.001); assertEquals(maxIntegralError, pid.getMaxIntegralError(), 0.001); assertEquals(maxOutput, pid.getMaximumFeedback(), 1e-5); assertEquals(0.0, pid.getPositionDeadband(), 0.001); assertEquals(1.0, pid.getIntegralLeakRatio(), 0.001); }
@ContinuousIntegrationTest(estimatedDuration = 0.2) @Test(timeout=300000) public void testPIDControllerConstructorFromGains4() { YoVariableRegistry registry = new YoVariableRegistry("robert"); double proportional = random.nextDouble(); double integral = random.nextDouble(); double derivative = random.nextDouble(); double maxIntegralError = random.nextDouble(); YoPIDGains pidGains = new YoPIDGains("", registry); pidGains.setKp(proportional); pidGains.setKi(integral); pidGains.setKd(derivative); pidGains.setMaximumIntegralError(maxIntegralError); PIDController pid = new PIDController(pidGains, "", registry); assertEquals(proportional, pid.getProportionalGain(), 0.001); assertEquals(integral, pid.getIntegralGain(), 0.001); assertEquals(derivative, pid.getDerivativeGain(), 0.001); assertEquals(maxIntegralError, pid.getMaxIntegralError(), 0.001); assertEquals(Double.POSITIVE_INFINITY, pid.getMaximumFeedback(), 0.001); assertEquals(0.0, pid.getPositionDeadband(), 0.001); assertEquals(1.0, pid.getIntegralLeakRatio(), 0.001); }
public void testParameters_2() YoPIDGains pidGains = new YoPIDGains("test", new YoVariableRegistry("PIDGainsRegistry"));
jointspaceGains.put(joint1.getName(), new YoPIDGains("Joint1Gains", testRegistry)); jointspaceGains.put(joint2.getName(), new YoPIDGains("Joint2Gains", testRegistry)); YoPID3DGains taskspaceOrientationGains = new SymmetricYoPIDSE3Gains("OrientationGains", testRegistry); YoPID3DGains taskspacePositionGains = new SymmetricYoPIDSE3Gains("PositionGains", testRegistry);
public void testParameters_3() YoPIDGains pidGains = new YoPIDGains("test", new YoVariableRegistry("PIDGainsRegistry"));
@ContinuousIntegrationTest(estimatedDuration = 0.3) @Test(timeout=300000) public void testComputeFromYoPIDGains() { YoVariableRegistry registry = new YoVariableRegistry("robert"); double proportional = 3.0; double integral = 2.0; double derivative = 1.0; double maxError = 10.0; YoPIDGains pidGains = new YoPIDGains("", registry); pidGains.setKp(proportional); pidGains.setKi(integral); pidGains.setKd(derivative); pidGains.setMaximumIntegralError(maxError); PIDController pid = new PIDController(pidGains, "", registry); double currentPosition = 0.0; double desiredPosition = 5.0; double currentRate = 0.0; double desiredRate = 1.0; assertEquals(17.0, pid.compute(currentPosition, desiredPosition, currentRate, desiredRate, 0.1), 0.001); }
public void test() YoPIDGains pidGains = new YoPIDGains("test", new YoVariableRegistry("PIDGainsRegistry"));
linearPidGains = new YoPIDGains(forcePoint.getName() + "_linear" + suffix, registry); linearPidGains.setKp(linearKp); linearPidGains.setKi(linearKi); linearPidGains.setPositionDeadband(linearDeadband); angularPidGains = new YoPIDGains(forcePoint.getName() + "_angular" + suffix, registry); angularPidGains.setKp(angularKp); angularPidGains.setKi(angularKi);
YoPIDGains pidGains = new YoPIDGains("", registry); double proportional = 2.0; double integral = 3.0;