How to use

compute

method

in

us.ihmc.robotics.controllers.PIDController

@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);