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.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); }
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.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.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); }
pidController.setProportionalGain(gains.getKp()); pidController.setIntegralGain(gains.getKi()); YoEffortJointHandleHolder handle = jointHandles.get(fingerMotorNameEnum);
jointPositionController.setProportionalGain(15.0 * subtreeMass); jointPositionController.setIntegralGain(35.0 * subtreeMass); jointPositionController.setMaxIntegralError(0.3); jointVelocityController.setProportionalGain(2.0 * subtreeMass); jointVelocityController.setIntegralGain(35.0 * subtreeMass); jointVelocityController.setMaxIntegralError(0.3); jointPositionController.setProportionalGain(145.0 * subtreeMass); jointPositionController.setIntegralGain(9.0 * 50.0 * subtreeMass); jointPositionController.setMaxIntegralError(0.2); jointVelocityController.setProportionalGain(4.5 * subtreeMass); jointVelocityController.setIntegralGain(9.0 * 50.0 * subtreeMass); jointVelocityController.setMaxIntegralError(0.2); jointPositionController.setProportionalGain(8000.0); jointPositionController.setIntegralGain(1000.0 * 50.0); jointPositionController.setMaxIntegralError(0.2); jointVelocityController.setProportionalGain(200.0); jointVelocityController.setIntegralGain(1000.0 * 50.0); jointVelocityController.setMaxIntegralError(0.2); jointPositionController.setProportionalGain(63.0 * totalMass); jointPositionController.setIntegralGain(6.0 * 50.0 * totalMass); jointPositionController.setMaxIntegralError(0.2); jointVelocityController.setProportionalGain(3.2 * totalMass);
jointController.setProportionalGain(15.0 * subtreeMass); jointController.setIntegralGain(35.0 * subtreeMass); jointController.setMaxIntegralError(0.3); jointController.setProportionalGain(16000.0); jointController.setIntegralGain(1000.0 * 50.0); jointController.setMaxIntegralError(0.2); jointController.setProportionalGain(8000.0); jointController.setIntegralGain(1000.0 * 50.0); jointController.setMaxIntegralError(0.2); jointController.setProportionalGain(12000.0); jointController.setIntegralGain(1000.0 * 50.0); jointController.setMaxIntegralError(0.2);
jointController.setProportionalGain(15.0 * subtreeMass); jointController.setIntegralGain(35.0 * subtreeMass); jointController.setMaxIntegralError(0.3); jointController.setProportionalGain(16000.0); jointController.setIntegralGain(1000.0 * 50.0); jointController.setMaxIntegralError(0.2); jointController.setProportionalGain(8000.0); jointController.setIntegralGain(1000.0 * 50.0); jointController.setMaxIntegralError(0.2); jointController.setProportionalGain(12000.0); jointController.setIntegralGain(1000.0 * 50.0); jointController.setMaxIntegralError(0.2);
@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); }
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);