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); }
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); }
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.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); }
private void setGainsReducedIfBacklash() { double proportionalGain = gainReduction.getDoubleValue() * maxProportionalGain.getDoubleValue(); double derivativeGain = gainReduction.getDoubleValue() * maxDerivativeGain.getDoubleValue(); super.setProportionalGain(proportionalGain); super.setDerivativeGain(derivativeGain); }
private void setGainsReducedIfBacklash() { double proportionalGain = gainReduction.getDoubleValue() * maxProportionalGain.getDoubleValue(); double derivativeGain = gainReduction.getDoubleValue() * maxDerivativeGain.getDoubleValue(); super.setProportionalGain(proportionalGain); super.setDerivativeGain(derivativeGain); }
@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); }
jointController.setMaxIntegralError(0.3); jointController.setIntegralLeakRatio(integralLeakRatio); jointController.setDerivativeGain(2.0 * subtreeMass); jointController.setMaximumOutputLimit(40.0 * subtreeMass); jointController.setMaxIntegralError(0.2); jointController.setIntegralLeakRatio(integralLeakRatio); jointController.setDerivativeGain(500.0); jointController.setMaximumOutputLimit(400.0); jointController.setMaxIntegralError(0.2); jointController.setIntegralLeakRatio(integralLeakRatio); jointController.setDerivativeGain(200.0); jointController.setMaximumOutputLimit(400.0); jointController.setMaxIntegralError(0.2); jointController.setIntegralLeakRatio(integralLeakRatio); jointController.setDerivativeGain(500.0); jointController.setMaximumOutputLimit(400.0);
jointController.setMaxIntegralError(0.3); jointController.setIntegralLeakRatio(integralLeakRatio); jointController.setDerivativeGain(2.0 * subtreeMass); jointController.setMaximumOutputLimit(40.0 * subtreeMass); jointController.setMaxIntegralError(0.2); jointController.setIntegralLeakRatio(integralLeakRatio); jointController.setDerivativeGain(500.0); jointController.setMaximumOutputLimit(400.0); jointController.setMaxIntegralError(0.2); jointController.setIntegralLeakRatio(integralLeakRatio); jointController.setDerivativeGain(200.0); jointController.setMaximumOutputLimit(400.0); jointController.setMaxIntegralError(0.2); jointController.setIntegralLeakRatio(integralLeakRatio); jointController.setDerivativeGain(500.0); jointController.setMaximumOutputLimit(400.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_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); }
jointPositionController.setMaxIntegralError(0.3); jointPositionController.setIntegralLeakRatio(integralLeakRatio); jointPositionController.setDerivativeGain(2.0 * subtreeMass); jointPositionController.setMaximumOutputLimit(40.0 * subtreeMass); jointPositionController.setMaxIntegralError(0.2); jointPositionController.setIntegralLeakRatio(integralLeakRatio); jointPositionController.setDerivativeGain(4.5 * subtreeMass); jointPositionController.setMaximumOutputLimit(3.5 * subtreeMass); jointPositionController.setMaxIntegralError(0.2); jointPositionController.setIntegralLeakRatio(integralLeakRatio); jointPositionController.setDerivativeGain(200.0); jointPositionController.setMaximumOutputLimit(400.0); jointPositionController.setIntegralLeakRatio(integralLeakRatio); jointPositionController.setDerivativeGain(0.1 * totalMass); jointPositionController.setMaximumOutputLimit(2.5 * totalMass);
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); }
jointPositionController.setProportionalGain(jointDesiredOutput.getStiffness()); if (jointDesiredOutput.hasDamping()) jointPositionController.setDerivativeGain(jointDesiredOutput.getDamping());
pid2.setProportionalGain(proportional); pid2.setIntegralGain(integral); pid2.setDerivativeGain(derivative); pid2.setMaxIntegralError(maxIntegral);