public PDController(YoPDGains pdGains, String suffix, YoVariableRegistry registry) { super(suffix, registry); this.proportionalGain = pdGains.getYoKp(); this.derivativeGain = pdGains.getYoKd(); this.positionDeadband = pdGains.getYoPositionDeadband(); }
public void set(PDGainsReadOnly pdGains) { setKp(pdGains.getKp()); setKd(pdGains.getKd()); setZeta(GainCalculator.computeDampingRatio(pdGains.getKp(), pdGains.getKd())); setMaximumFeedback(pdGains.getMaximumFeedback()); setMaximumFeedbackRate(pdGains.getMaximumFeedbackRate()); setPositionDeadband(pdGains.getPositionDeadband()); }
public void testParameters() YoPDGains gains = new YoPDGains("pdGains", new YoVariableRegistry("testRegistry")); gains.setKd(kd); gains.setKp(kp); gains.setMaximumFeedback(maxAcc); gains.setMaximumFeedbackRate(maxJerk); gains.setZeta(zeta); gains.setPositionDeadband(deadband); assertEquals(kp, gains.getKp(), 1e-6); assertEquals(kp, gains.getYoKp().getDoubleValue(), 1e-6); assertEquals(kd, gains.getKd(), 1e-6); assertEquals(kd, gains.getYoKd().getDoubleValue(), 1e-6); assertEquals(maxAcc, gains.getMaximumFeedback(), 1e-6); assertEquals(maxAcc, gains.getYoMaximumFeedback().getDoubleValue(), 1e-6); assertEquals(maxJerk, gains.getMaximumFeedbackRate(), 1e-6); assertEquals(maxJerk, gains.getYoMaximumFeedbackRate().getDoubleValue(), 1e-6); assertEquals(zeta, gains.getZeta(), 1e-6); assertEquals(zeta, gains.getYoZeta().getDoubleValue(), 1e-6); assertEquals(deadband, gains.getPositionDeadband(), 1e-6); assertEquals(deadband, gains.getYoPositionDeadband().getDoubleValue(), 1e-6);
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testParameters_2() { YoPDGains gains = new YoPDGains("pdGains", new YoVariableRegistry("testRegistry")); Random rand = new Random(); for (int i = 0; i < 1000; i++) { double kp = rand.nextDouble() * 100; double maxAcc = rand.nextDouble() * 100; double maxJerk = rand.nextDouble() * 100; double zeta = rand.nextDouble()*100; gains.setPDGains(kp, zeta); gains.setMaximumFeedbackAndMaximumFeedbackRate(maxAcc, maxJerk); assertEquals(kp, gains.getKp(), 1e-6); assertEquals(0.0, gains.getKd(), 1e-6); assertEquals(maxAcc, gains.getMaximumFeedback(), 1e-6); assertEquals(maxJerk, gains.getMaximumFeedbackRate(), 1e-6); assertEquals(zeta, gains.getZeta(), 1e-6); assertEquals(0.0, gains.getPositionDeadband(), 1e-6); assertEquals(0.0, gains.getYoPositionDeadband().getDoubleValue(), 1e-6); } }
public YoLimitedPDGains(String suffix, double controlDT, YoVariableRegistry registry) { super(suffix, registry); maxKpRate = new YoDouble("maxKpRate" + suffix, registry); maxKdRate = new YoDouble("maxKdRate" + suffix, registry); limitedKp = new RateLimitedYoVariable("limitedKp" + suffix, registry, maxKpRate, super.getYoKp(), controlDT); limitedKd = new RateLimitedYoVariable("limitedKd" + suffix, registry, maxKdRate, super.getYoKd(), controlDT); maxKpRate.set(Double.POSITIVE_INFINITY); maxKdRate.set(Double.POSITIVE_INFINITY); }
public PassiveJointController(OneDegreeOfFreedomJoint simulatedJoint, YoPDGains gains) { registry = new YoVariableRegistry(simulatedJoint.getName() + name); jointController = new PDController(gains.getYoKp(), gains.getYoKd(), simulatedJoint.getName() + "PassiveController", registry); this.simulatedJoint = simulatedJoint; jointController.setProportionalGain(36000.0); jointController.setDerivativeGain(1000.0); }