@ContinuousIntegrationTest(estimatedDuration = 0.2) @Test(timeout=300000) public void testCompute_all_PID() { PIDController pid = new PIDController("", null); pid.setProportionalGain(2.0); pid.setIntegralGain(3.0); pid.setDerivativeGain(4.0); pid.setMaxIntegralError(10.0); double currentPosition = 0.0; double desiredPosition = 5.0; double currentRate = 0.0; double desiredRate = 2.0; assertEquals((10.0 + 1.5 + 8.0), pid.compute(currentPosition, desiredPosition, currentRate, desiredRate, 0.1), 0.001); assertEquals((10.0 + 3.0 + 8.0), pid.compute(currentPosition, desiredPosition, currentRate, desiredRate, 0.1), 0.001); assertEquals((10.0 + 18.0 + 8.0), pid.compute(currentPosition, desiredPosition, currentRate, desiredRate, 1.0), 0.001); // tests max integration error assertEquals((10.0 + 30.0 + 8.0), pid.compute(currentPosition, desiredPosition, currentRate, desiredRate, 1.01), 0.001); }
public void resetIntegrator() { jointController.resetIntegrator(); }
jointController = new PIDController(simulatedJoint.getName() + "PositionControllerSimulator", registry); jointController.setProportionalGain(15.0 * subtreeMass); jointController.setIntegralGain(35.0 * subtreeMass); jointController.setMaxIntegralError(0.3); jointController.setIntegralLeakRatio(integralLeakRatio); jointController.setDerivativeGain(2.0 * subtreeMass); jointController.setMaximumOutputLimit(40.0 * subtreeMass); jointController.setProportionalGain(16000.0); jointController.setIntegralGain(1000.0 * 50.0); jointController.setMaxIntegralError(0.2); jointController.setIntegralLeakRatio(integralLeakRatio); jointController.setDerivativeGain(500.0); jointController.setMaximumOutputLimit(400.0); jointController.setProportionalGain(8000.0); jointController.setIntegralGain(1000.0 * 50.0); jointController.setMaxIntegralError(0.2); jointController.setIntegralLeakRatio(integralLeakRatio); jointController.setDerivativeGain(200.0); jointController.setMaximumOutputLimit(400.0); jointController.setProportionalGain(12000.0); jointController.setIntegralGain(1000.0 * 50.0); jointController.setMaxIntegralError(0.2); jointController.setIntegralLeakRatio(integralLeakRatio); jointController.setDerivativeGain(500.0); jointController.setMaximumOutputLimit(400.0);
@ContinuousIntegrationTest(estimatedDuration = 0.3) @Test(timeout=300000) public void testCompute_proportional_withDeadband() { PIDController pid = new PIDController("", null); pid.setProportionalGain(3.0); pid.setPositionDeadband(1.0); double currentPosition = 0.0; double desiredPosition = 5.0; double currentRate = 0.0; double desiredRate = 1.0; assertEquals(12.0, pid.compute(currentPosition, desiredPosition, currentRate, desiredRate, 0.1), 0.001); pid.setProportionalGain(6.0); pid.setPositionDeadband(4.0); assertEquals(6.0, pid.compute(currentPosition, desiredPosition, currentRate, desiredRate, 0.1), 0.001); }
@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); }
PIDController pid1 = new PIDController(pidGains, "1", registry); PIDController pid2 = new PIDController("2", registry); pid2.setProportionalGain(proportional); pid2.setIntegralGain(integral); pid2.setDerivativeGain(derivative); pid2.setMaxIntegralError(maxIntegral); assertEquals(proportional, pid1.getProportionalGain(), 0.001); assertEquals(integral, pid1.getIntegralGain(), 0.001); assertEquals(derivative, pid1.getDerivativeGain(), 0.001); assertEquals(maxIntegral, pid1.getMaxIntegralError(), 0.001); assertEquals(pid2.getProportionalGain(), pid1.getProportionalGain(), 0.001); assertEquals(pid2.getIntegralGain(), pid1.getIntegralGain(), 0.001); assertEquals(pid2.getDerivativeGain(), pid1.getDerivativeGain(), 0.001); assertEquals(pid2.getMaxIntegralError(), pid1.getMaxIntegralError(), 0.001); assertEquals((10.0 + 1.5 + 8.0), pid1.compute(currentPosition, desiredPosition, currentRate, desiredRate, 0.1), 0.001); assertEquals((10.0 + 3.0 + 8.0), pid1.compute(currentPosition, desiredPosition, currentRate, desiredRate, 0.1), 0.001); assertEquals((10.0 + 18.0 + 8.0), pid1.compute(currentPosition, desiredPosition, currentRate, desiredRate, 1.0), 0.001); assertEquals((10.0 + 1.5 + 8.0), pid2.compute(currentPosition, desiredPosition, currentRate, desiredRate, 0.1), 0.001); assertEquals((10.0 + 3.0 + 8.0), pid2.compute(currentPosition, desiredPosition, currentRate, desiredRate, 0.1), 0.001); assertEquals((10.0 + 18.0 + 8.0), pid2.compute(currentPosition, desiredPosition, currentRate, desiredRate, 1.0), 0.001); assertEquals(pid2.compute(currentPosition, desiredPosition, currentRate, desiredRate, 0.1), pid1.compute(currentPosition, desiredPosition, currentRate, desiredRate, 0.1), 0.001); assertEquals(pid2.compute(currentPosition, desiredPosition, currentRate, desiredRate, 0.1), pid1.compute(currentPosition, desiredPosition, currentRate, desiredRate, 0.1), 0.001); assertEquals(pid2.compute(currentPosition, desiredPosition, currentRate, desiredRate, 1.0), pid1.compute(currentPosition, desiredPosition, currentRate, desiredRate, 1.0), 0.001);
jointPositionController.setProportionalGain(jointDesiredOutput.getStiffness()); if (jointDesiredOutput.hasDamping()) jointPositionController.setDerivativeGain(jointDesiredOutput.getDamping()); jointPositionController.setIntegralGain(0.0); jointPositionController.setMaximumOutputLimit(Double.POSITIVE_INFINITY); double desiredTau = jointPositionController.compute(currentPosition, desiredPosition, currentRate, desiredRate, controlDT); simulatedJoint.setTau(desiredTau);
pidController.setGains(gains); pidController.setProportionalGain(gains.getKp()); pidController.setIntegralGain(gains.getKi()); YoEffortJointHandleHolder handle = jointHandles.get(fingerMotorNameEnum); double qd_d = desiredVelocities.get(fingerMotorNameEnum).getDoubleValue(); double tau = pidController.compute(q, q_d, qd, qd_d, controlDT); pidController.resetIntegrator(); pidController.resetIntegrator();
public PIDLidarTorqueController(FloatingRootJointRobot robot, String jointName, double desiredSpindleSpeed, double controlDT) { this.controlDT = controlDT; this.lidarJoint = robot.getOneDegreeOfFreedomJoint(jointName); desiredLidarVelocity.set(desiredSpindleSpeed); lidarJointController.setProportionalGain(10.0); lidarJointController.setDerivativeGain(1.0); }
@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); }
public void setGains(PIDGainsReadOnly gains) { pdController.setGains(gains); setMaximumOutputLimit(gains.getMaximumFeedback()); setIntegralLeakRatio(gains.getIntegralLeakRatio()); setIntegralGain(gains.getKi()); setMaxIntegralError(gains.getMaxIntegralError()); }
@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 computeAndUpdateJointTorque() { JointDesiredOutputReadOnly desiredOutput = yoEffortJointHandleHolder.getDesiredJointData(); pidController.setProportionalGain(desiredOutput.hasStiffness() ? desiredOutput.getStiffness() : 0.0); pidController.setDerivativeGain(desiredOutput.hasDamping() ? desiredOutput.getDamping() : 0.0); OneDoFJointBasics oneDoFJoint = yoEffortJointHandleHolder.getOneDoFJoint(); double q, qd; if (oneDoFJoint != null) { q = oneDoFJoint.getQ(); qd = oneDoFJoint.getQd(); } else { q = yoEffortJointHandleHolder.getQ(); qd = yoEffortJointHandleHolder.getQd(); } double qDesired = desiredOutput.hasDesiredPosition() ? desiredOutput.getDesiredPosition() : q; double qdDesired = desiredOutput.hasDesiredVelocity() ? desiredOutput.getDesiredVelocity() : qd; double fb_tau = pidController.compute(q, qDesired, qd, qdDesired, controlDT); double ff_tau = desiredOutput.hasDesiredTorque() ? desiredOutput.getDesiredTorque() : 0.0; double desiredEffort = fb_tau + ff_tau + tauOffset.getDoubleValue(); yoEffortJointHandleHolder.setDesiredEffort(desiredEffort); }
@Override public double compute(double currentPosition, double desiredPosition, double currentRate, double desiredRate, double deltaTime) { checkGainReductionUpToDate(); setGainsReducedIfBacklash(); return super.compute(currentPosition, desiredPosition, currentRate, desiredRate, deltaTime); }
@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); }
@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 testSetProportionalGain() { YoVariableRegistry registry = new YoVariableRegistry("mike"); PIDController pid = new PIDController("", registry); pid.setProportionalGain(5.0); assertEquals(5.0, pid.getProportionalGain(), 0.001); }
public ValkyrieRosControlEffortJointControlCommandCalculator(YoEffortJointHandleHolder yoEffortJointHandleHolder, double torqueOffset, double controlDT, YoVariableRegistry parentRegistry) { this.yoEffortJointHandleHolder = yoEffortJointHandleHolder; this.controlDT = controlDT; String pdControllerBaseName = yoEffortJointHandleHolder.getName(); YoVariableRegistry registry = new YoVariableRegistry(pdControllerBaseName + "Command"); pidController = new PIDController(pdControllerBaseName + "LowLevelControl", registry); tauOffset = new YoDouble("tau_offset_" + pdControllerBaseName, registry); pidController.setMaxIntegralError(50.0); pidController.setCumulativeError(0.0); tauOffset.set(torqueOffset); parentRegistry.addChild(registry); }
@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); }