@Override public double compute(double currentPosition, double desiredPosition, double currentRate, double desiredRate, double deltaTime) { checkGainReductionUpToDate(); setGainsReducedIfBacklash(); return super.compute(currentPosition, desiredPosition, currentRate, desiredRate, deltaTime); }
desiredRate = actuatorDesireds.getVelocity(); outputEffort = jointController.compute(currentRate, desiredRate, 0.0, 0.0, controlDT); break; case EFFORT: desiredRate = actuatorDesireds.isVelocityValid() ? actuatorDesireds.getVelocity() : 0.0; double feedbackEffort = jointController.compute(currentPosition, desiredPosition, currentRate, desiredRate, controlDT);
@Override public double compute(double currentPosition, double desiredPosition, double currentRate, double desiredRate, double deltaTime) { checkGainReductionUpToDate(); setGainsReducedIfBacklash(); return super.compute(currentPosition, desiredPosition, currentRate, desiredRate, deltaTime); }
currentAngularZ.set(currentOrientation.getZ()); double linearForceX = pidControllerLinearX.compute(currentPosition.getX(), desiredPosition.getX(), 0.0, 0.0, 0.0); double linearForceY = pidControllerLinearY.compute(currentPosition.getY(), desiredPosition.getY(), 0.0, 0.0, 0.0); double linearForceZ = pidControllerLinearZ.compute(currentPosition.getZ(), desiredPosition.getZ(), 0.0, 0.0, 0.0);
@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); }
@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.3) @Test(timeout=300000) public void testCompute_all_PID_withDeadband() { PIDController pid = new PIDController("", null); pid.setProportionalGain(2.0); pid.setIntegralGain(3.0); pid.setDerivativeGain(4.0); pid.setMaxIntegralError(10.0); pid.setPositionDeadband(1.5); double currentPosition = 0.0; double desiredPosition = 5.0; double currentRate = 0.0; double desiredRate = 2.0; assertEquals((7.0 + 1.05 + 8.0), pid.compute(currentPosition, desiredPosition, currentRate, desiredRate, 0.1), 0.001); assertEquals((7.0 + 2.1 + 8.0), pid.compute(currentPosition, desiredPosition, currentRate, desiredRate, 0.1), 0.001); assertEquals((7.0 + 12.6 + 8.0), pid.compute(currentPosition, desiredPosition, currentRate, desiredRate, 1.0), 0.001); // tests max integration error assertEquals((7.0 + 30.0 + 8.0), pid.compute(currentPosition, desiredPosition, currentRate, desiredRate, 3.0), 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); }
@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); }
public void doControl() { if (highLevelControllerOutputJoint.isUnderPositionControl()) { double currentPosition = simulatedJoint.getQYoVariable().getDoubleValue(); double desiredPosition = highLevelControllerOutputJoint.getqDesired(); double currentRate = simulatedJoint.getQDYoVariable().getDoubleValue(); double desiredRate = highLevelControllerOutputJoint.getQdDesired(); double desiredTau = jointController.compute(currentPosition, desiredPosition, currentRate, desiredRate, controlDT); simulatedJoint.setTau(desiredTau); } }
@Override public void doControl() { if (highLevelControllerOutputJoint.isUnderPositionControl()) { double currentPosition = simulatedJoint.getQYoVariable().getDoubleValue(); double desiredPosition = highLevelControllerOutputJoint.getqDesired(); double currentRate = simulatedJoint.getQDYoVariable().getDoubleValue(); double desiredRate = highLevelControllerOutputJoint.getQdDesired(); double desiredTau = jointController.compute(currentPosition, desiredPosition, currentRate, desiredRate, controlDT); simulatedJoint.setTau(desiredTau); } }
double qd_d = desiredVelocities.get(fingerMotorNameEnum).getDoubleValue(); double tau = pidController.compute(q, q_d, qd, qd_d, controlDT);
@Override public void doControl() { desiredLidarAngle.add(desiredLidarVelocity.getDoubleValue() * controlDT); double lidarJointTau = lidarJointController.compute(lidarJoint.getQYoVariable().getDoubleValue(), desiredLidarAngle.getDoubleValue(), lidarJoint.getQDYoVariable() .getDoubleValue(), desiredLidarVelocity.getDoubleValue(), controlDT) + lidarJoint.getDamping() * desiredLidarVelocity.getDoubleValue(); lidarJoint.setTau(lidarJointTau); }
@Override public void doControl() { desiredLidarAngle.add(desiredLidarVelocity.getDoubleValue() * controlDT); double lidarJointTau = lidarJointController.compute(lidarJoint.getQYoVariable().getDoubleValue(), desiredLidarAngle.getDoubleValue(), lidarJoint.getQDYoVariable() .getDoubleValue(), desiredLidarVelocity.getDoubleValue(), controlDT) + lidarJoint.getDamping() * desiredLidarVelocity.getDoubleValue(); lidarJoint.setTau(lidarJointTau); }
public void doControl() { desiredLidarAngle.add(desiredLidarVelocity.getDoubleValue() * controlDT); double lidarJointTau = lidarJointController.compute(lidarJoint.getQYoVariable().getDoubleValue(), desiredLidarAngle.getDoubleValue(), lidarJoint.getQDYoVariable() .getDoubleValue(), desiredLidarVelocity.getDoubleValue(), controlDT) + lidarJoint.getDamping() * desiredLidarVelocity.getDoubleValue(); lidarJoint.setTau(lidarJointTau); }
@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.3) @Test(timeout=300000) public void testCompute() { YoVariableRegistry registry = new YoVariableRegistry("mike"); YoDouble proportional = new YoDouble("proportional", registry); proportional.set(3.0); YoDouble integral = new YoDouble("integral", registry); integral.set(2.0); YoDouble derivative = new YoDouble("derivative", registry); derivative.set(1.0); YoDouble maxError = new YoDouble("maxError", registry); maxError.set(10.0); PIDController pid = new PIDController(proportional, integral, derivative, maxError, "", 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 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); }
double desiredTau = jointPositionController.compute(currentPosition, desiredPosition, currentRate, desiredRate, controlDT); simulatedJoint.setTau(desiredTau);