How to use

computeForAngles

method

in

us.ihmc.robotics.controllers.PIDController

@Override
public double computeForAngles(double currentPosition, double desiredPosition, double currentRate, double desiredRate, double deltaTime)
{
 checkGainReductionUpToDate();
 setGainsReducedIfBacklash();
 return super.computeForAngles(currentPosition, desiredPosition, currentRate, desiredRate, deltaTime);
}

@Override
public double computeForAngles(double currentPosition, double desiredPosition, double currentRate, double desiredRate, double deltaTime)
{
 checkGainReductionUpToDate();
 setGainsReducedIfBacklash();
 return super.computeForAngles(currentPosition, desiredPosition, currentRate, desiredRate, deltaTime);
}

double currentRate = simulatedJoint.getQDYoVariable().getDoubleValue();
outputEffort = jointController.computeForAngles(currentPosition, desiredPosition, currentRate, desiredRate, controlDT);

contactForce.setZ(linearForceZ);
double torqueX = pidControllerAngularX.computeForAngles(currentOrientation.getX(), desiredOrientation.getX(), 0.0, 0.0, 0.0);
double torqueY = pidControllerAngularY.computeForAngles(currentOrientation.getY(), desiredOrientation.getY(), 0.0, 0.0, 0.0);
double torqueZ = pidControllerAngularZ.computeForAngles(currentOrientation.getZ(), desiredOrientation.getZ(), 0.0, 0.0, 0.0);