private void mapJointsAndVariables(YoPIDGains gains) { for (ValkyrieFingerMotorName fingerMotorNameEnum : ValkyrieFingerMotorName.values) { String jointName = fingerMotorNameEnum.getJointName(robotSide); YoDouble desiredAngle = new YoDouble("q_d_" + jointName, registry); desiredAngle.set(fingerStateEstimator.getFingerMotorPosition(robotSide, fingerMotorNameEnum)); desiredAngles.put(fingerMotorNameEnum, desiredAngle); YoDouble desiredVelocity = new YoDouble("qd_d_" + jointName, registry); desiredVelocities.put(fingerMotorNameEnum, desiredVelocity); PIDController pidController = new PIDController(fingerMotorNameEnum.getPascalCaseJointName(robotSide), registry); pidControllers.put(fingerMotorNameEnum, pidController); } }
@ContinuousIntegrationTest(estimatedDuration = 0.2) @Test(timeout=300000) public void testSetIntegralLeakRatio() { double leakRatio = random.nextDouble(); YoVariableRegistry registry = new YoVariableRegistry("robert"); PIDController pid = new PIDController("", registry); pid.setIntegralLeakRatio(leakRatio); assertEquals(leakRatio, pid.getIntegralLeakRatio(), 0.001); }
@ContinuousIntegrationTest(estimatedDuration = 0.2) @Test(timeout=300000) public void testSetDeadband() { double deadband = random.nextDouble() * 10.0; YoVariableRegistry registry = new YoVariableRegistry("robert"); PIDController pid = new PIDController("", registry); pid.setPositionDeadband(deadband); assertEquals(deadband, pid.getPositionDeadband(), 0.001); }
@ContinuousIntegrationTest(estimatedDuration = 0.3) @Test(timeout=300000) public void testGetDeadband() { YoVariableRegistry registry = new YoVariableRegistry("robert"); PIDController pid = new PIDController("", registry); assertEquals(0.0, pid.getPositionDeadband(), 0.001); }
@ContinuousIntegrationTest(estimatedDuration = 0.2) @Test(timeout=300000) public void testGetIntegralGain() { YoVariableRegistry registry = new YoVariableRegistry("mike"); PIDController pid = new PIDController("", registry); assertEquals(0.0, pid.getIntegralGain(), 0.001); }
@ContinuousIntegrationTest(estimatedDuration = 0.3) @Test(timeout=300000) public void testGetDerivativeGain() { YoVariableRegistry registry = new YoVariableRegistry("mike"); PIDController pid = new PIDController("", registry); assertEquals(0.0, pid.getDerivativeGain(), 0.001); }
@ContinuousIntegrationTest(estimatedDuration = 0.2) @Test(timeout=300000) public void testGetLeakRate() { YoVariableRegistry registry = new YoVariableRegistry("robert"); PIDController pid = new PIDController("", registry); assertEquals(1.0, pid.getIntegralLeakRatio(), 0.001); }
@ContinuousIntegrationTest(estimatedDuration = 0.3) @Test(timeout=300000) public void testGetCumulativeError() { YoVariableRegistry registry = new YoVariableRegistry("mike"); PIDController pid = new PIDController("", registry); assertEquals(0.0, pid.getCumulativeError(), 0.001); }
@ContinuousIntegrationTest(estimatedDuration = 0.2) @Test(timeout=300000) public void testGetProportionalGain() { YoVariableRegistry registry = new YoVariableRegistry("mike"); PIDController pid = new PIDController("", registry); assertEquals(0.0, pid.getProportionalGain(), 0.001); }
@ContinuousIntegrationTest(estimatedDuration = 0.3) @Test(timeout=300000) public void testGetMaxIntegralError() { YoVariableRegistry registry = new YoVariableRegistry("mike"); PIDController pid = new PIDController("", registry); assertEquals(Double.POSITIVE_INFINITY, pid.getMaxIntegralError(), 0.001); }
@ContinuousIntegrationTest(estimatedDuration = 0.3) @Test(timeout=300000) public void testSetIntegralLeakRatio2() { YoVariableRegistry registry = new YoVariableRegistry("robert"); PIDController pid = new PIDController("", registry); double leakRatio = random.nextDouble() * 100.0; pid.setIntegralLeakRatio(leakRatio); assertTrue(pid.getIntegralLeakRatio() <= 1.0); leakRatio = random.nextDouble() * -100.0; pid.setIntegralLeakRatio(leakRatio); assertTrue(pid.getIntegralLeakRatio() >= 0.0); }
@ContinuousIntegrationTest(estimatedDuration = 0.2) @Test(timeout=300000) public void testSetIntegralGain() { YoVariableRegistry registry = new YoVariableRegistry("mike"); PIDController pid = new PIDController("", registry); pid.setIntegralGain(5.0); assertEquals(5.0, pid.getIntegralGain(), 0.001); }
@ContinuousIntegrationTest(estimatedDuration = 0.2) @Test(timeout=300000) public void testSetDerivativeGain() { YoVariableRegistry registry = new YoVariableRegistry("mike"); PIDController pid = new PIDController("", registry); pid.setDerivativeGain(5.0); assertEquals(5.0, pid.getDerivativeGain(), 0.001); }
@ContinuousIntegrationTest(estimatedDuration = 0.2) @Test(timeout=300000) public void testSetCumulativeError() { YoVariableRegistry registry = new YoVariableRegistry("mike"); PIDController pid = new PIDController("", registry); pid.setCumulativeError(5.0); assertEquals(5.0, pid.getCumulativeError(), 0.001); }
@ContinuousIntegrationTest(estimatedDuration = 0.3) @Test(timeout=300000) public void testSetMaxIntegralError() { YoVariableRegistry registry = new YoVariableRegistry("mike"); PIDController pid = new PIDController("", registry); pid.setMaxIntegralError(5.0); assertEquals(5.0, pid.getMaxIntegralError(), 0.001); }
@ContinuousIntegrationTest(estimatedDuration = 0.2) @Test(timeout=300000) public void testSetProportionalGain() { YoVariableRegistry registry = new YoVariableRegistry("mike"); PIDController pid = new PIDController("", registry); pid.setProportionalGain(5.0); assertEquals(5.0, pid.getProportionalGain(), 0.001); }
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; }
@ContinuousIntegrationTest(estimatedDuration = 0.3) @Test(timeout=300000) public void testCompute_integral() { PIDController pid = new PIDController("", null); pid.setIntegralGain(4.0); double currentPosition = 0.0; double desiredPosition = 5.0; double currentRate = 0.0; double desiredRate = 3.0; assertEquals(2.0, pid.compute(currentPosition, desiredPosition, currentRate, desiredRate, 0.1), 0.001); pid.setIntegralGain(8.0); assertEquals(8.0, pid.compute(currentPosition, desiredPosition, currentRate, desiredRate, 0.1), 0.001); }
@ContinuousIntegrationTest(estimatedDuration = 0.3) @Test(timeout=300000) public void testCompute_proportional() { PIDController pid = new PIDController("", null); pid.setProportionalGain(3.0); double currentPosition = 0.0; double desiredPosition = 5.0; double currentRate = 0.0; double desiredRate = 1.0; assertEquals(15.0, pid.compute(currentPosition, desiredPosition, currentRate, desiredRate, 0.1), 0.001); pid.setProportionalGain(6.0); assertEquals(30.0, pid.compute(currentPosition, desiredPosition, currentRate, desiredRate, 0.1), 0.001); }
@ContinuousIntegrationTest(estimatedDuration = 0.2) @Test(timeout=300000) public void testCompute_derivative() { PIDController pid = new PIDController("", null); pid.setDerivativeGain(6.0); double currentPosition = 0.0; double desiredPosition = 5.0; double currentRate = 0.0; double desiredRate = 3.0; assertEquals(18.0, pid.compute(currentPosition, desiredPosition, currentRate, desiredRate, 0.1), 0.001); pid.setDerivativeGain(12.0); assertEquals(36.0, pid.compute(currentPosition, desiredPosition, currentRate, desiredRate, 0.1), 0.001); }