public PassiveJointController(OneDegreeOfFreedomJoint simulatedJoint, YoPDGains gains) { registry = new YoVariableRegistry(simulatedJoint.getName() + name); jointController = new PDController(gains.getYoKp(), gains.getYoKd(), simulatedJoint.getName() + "PassiveController", registry); this.simulatedJoint = simulatedJoint; jointController.setProportionalGain(36000.0); jointController.setDerivativeGain(1000.0); }
public double compute(double currentPosition, double desiredPosition, double currentRate, double desiredRate, double deltaTime) { pdController.compute(currentPosition, desiredPosition, currentRate, desiredRate); return computeIntegralEffortAndAddPDEffort(deltaTime); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testComputerForAngles() { PDController pdController = new PDController("suffix", new YoVariableRegistry("testRegistry")); pdController.setDerivativeGain(0.0); pdController.setProportionalGain(0.0); double value = pdController.computeForAngles(0, Math.toRadians(90), 0, 0); assertEquals(0.0, value, 1e-6); pdController.setDerivativeGain(1.0); pdController.setProportionalGain(1.0); Random random = new Random(); for (int i = 0; i < 100; i++) { double current = 0; double desired = random.nextDouble() * Math.PI; value = pdController.computeForAngles(current, desired, 0, 0); assertEquals(desired, value, 1e-6); } for (int i = 0; i < 100; i++) { double current = Math.PI; double desired = random.nextDouble() * Math.PI + Math.PI; value = pdController.computeForAngles(current, desired, 0, 0); assertEquals(desired - Math.PI, value, 1e-6); } }
public void setCoMHeightGains(double kp, double kd) { centerOfMassHeightController.setProportionalGain(kp); centerOfMassHeightController.setDerivativeGain(kd); }
public void setGains(PDGainsReadOnly gains) { setProportionalGain(gains.getKp()); setDerivativeGain(gains.getKd()); setPositionDeadband(gains.getPositionDeadband()); }
controller.setProportionalGain(kp); controller.setDerivativeGain(kd); double tau = controller.compute(oneDoFJoint.getQ(), qDesired, oneDoFJoint.getQd(), qdDesired); oneDoFJoint.setTau(tau); oneDoFJoint.setKd(damping);
@Override public void setFullRobotModel(FullRobotModel fullRobotModel) { for(OneDoFJoint joint : fullRobotModel.getOneDoFJoints()) { joints.add(joint); controllers.add(new PDController(joint.getName(), registry)); desiredPositions.add(new YoDouble(joint.getName() + "_q_d", registry)); desiredVelocities.add(new YoDouble(joint.getName() + "_qd_d", registry)); tauFFs.add(new YoDouble(joint.getName() + "_tau_ff", registry)); damping.add(new YoDouble(joint.getName() + "_damping", registry)); } }
@Override public void setFullRobotModel(FullRobotModel fullRobotModel) { for (StepprJoint joint : StepprJoint.values) { joints.put(joint, fullRobotModel.getOneDoFJointByName(joint.getSdfName())); controllers.put(joint, new PDController(joint.getSdfName(), registry)); } for (StepprStandPrepSetpoints setpoint : StepprStandPrepSetpoints.values) { for (int i = 0; i < setpoint.getJoints().length; i++) { StepprJoint joint = setpoint.getJoints()[i]; YoDouble desiredPosition = new YoDouble(setpoint.getName() + "_q_d", registry); desiredPosition.set(setpoint.getQ()); if (i == 1) { desiredPosition.mul(setpoint.getReflectRight()); } desiredOffsets.put(joint, desiredPosition); controllers.get(joint).setProportionalGain(setpoint.getKp()); controllers.get(joint).setProportionalGain(setpoint.getKd()); joints.get(joint).setKd(setpoint.getDamping()); } } }
public void setDerivativeGain(double derivativeGain) { pdController.setDerivativeGain(derivativeGain); }
public void setProportionalGain(double proportionalGain) { pdController.setProportionalGain(proportionalGain); }
private double computeIntegralEffortAndAddPDEffort(double deltaTime) { double outputSignal = (pdController.getProportionalGain() * pdController.getPositionError()) + (pdController.getDerivativeGain() * pdController.getRateError()); if (integralGain.getDoubleValue() < 1.0e-5) { cumulativeError.set(0.0); } else { // LIMIT THE MAX INTEGRAL ERROR SO WON'T WIND UP double maxError = maxIntegralError.getDoubleValue(); double errorAfterLeak = pdController.getPositionError() * deltaTime + integralLeakRatio.getDoubleValue() * cumulativeError.getDoubleValue(); cumulativeError.set(MathTools.clipToMinMax(errorAfterLeak, maxError)); actionI.set(integralGain.getDoubleValue() * cumulativeError.getDoubleValue()); outputSignal += actionI.getDoubleValue(); } double maximumOutput = Math.abs( maxOutput.getDoubleValue() ); outputSignal = MathTools.clipToMinMax(outputSignal, maximumOutput); return outputSignal; } }
qdDesiredSelected.set(selected.qd_d.getDoubleValue()); kpSelected.set(selected.pdController.getProportionalGain()); kdSelected.set(selected.pdController.getDerivativeGain());
public double getDerivativeGain() { return pdController.getDerivativeGain(); }
public double computeForAngles(double currentPosition, double desiredPosition, double currentRate, double desiredRate, double deltaTime) { pdController.computeForAngles(currentPosition, desiredPosition, currentRate, desiredRate); return computeIntegralEffortAndAddPDEffort(deltaTime); }
pdControllers.get(fullRobotModel.getArmJoint(robotSide, ArmJointName.SHOULDER_YAW)).setProportionalGain(30.0); pdControllers.get(fullRobotModel.getArmJoint(robotSide, ArmJointName.SHOULDER_YAW)).setDerivativeGain(3.0); pdControllers.get(fullRobotModel.getArmJoint(robotSide, ArmJointName.SHOULDER_PITCH)).setProportionalGain(50.0); pdControllers.get(fullRobotModel.getArmJoint(robotSide, ArmJointName.SHOULDER_PITCH)).setDerivativeGain(5.0); pdControllers.get(fullRobotModel.getArmJoint(robotSide, ArmJointName.SHOULDER_ROLL)).setProportionalGain(50.0); pdControllers.get(fullRobotModel.getArmJoint(robotSide, ArmJointName.SHOULDER_ROLL)).setDerivativeGain(5.0); pdControllers.get(fullRobotModel.getArmJoint(robotSide, ArmJointName.ELBOW_PITCH)).setProportionalGain(40.0); pdControllers.get(fullRobotModel.getArmJoint(robotSide, ArmJointName.ELBOW_PITCH)).setDerivativeGain(4.0); pdControllers.get(fullRobotModel.getSpineJoint(SpineJointName.SPINE_YAW)).setProportionalGain(30.0); pdControllers.get(fullRobotModel.getSpineJoint(SpineJointName.SPINE_YAW)).setDerivativeGain(2.0); pdControllers.get(fullRobotModel.getSpineJoint(SpineJointName.SPINE_PITCH)).setProportionalGain(150.0); pdControllers.get(fullRobotModel.getSpineJoint(SpineJointName.SPINE_PITCH)).setDerivativeGain(8.0); pdControllers.get(fullRobotModel.getSpineJoint(SpineJointName.SPINE_ROLL)).setProportionalGain(150.0); pdControllers.get(fullRobotModel.getSpineJoint(SpineJointName.SPINE_ROLL)).setDerivativeGain(8.0); pdControllers.get(fullRobotModel.getLegJoint(robotSide, LegJointName.HIP_YAW)).setProportionalGain(30.0); pdControllers.get(fullRobotModel.getLegJoint(robotSide, LegJointName.HIP_YAW)).setDerivativeGain(2.0); pdControllers.get(fullRobotModel.getLegJoint(robotSide, LegJointName.HIP_PITCH)).setProportionalGain(150.0); pdControllers.get(fullRobotModel.getLegJoint(robotSide, LegJointName.HIP_PITCH)).setDerivativeGain(7.5); pdControllers.get(fullRobotModel.getLegJoint(robotSide, LegJointName.HIP_ROLL)).setProportionalGain(165); pdControllers.get(fullRobotModel.getLegJoint(robotSide, LegJointName.HIP_ROLL)).setDerivativeGain(6.0);
controller.setProportionalGain(kp); controller.setDerivativeGain(kd); double tau = controller.compute(oneDoFJoint.getQ(), qDesired, oneDoFJoint.getQd(), qdDesired); oneDoFJoint.setTau(tau); oneDoFJoint.setKd(damping);
@Override public void setFullRobotModel(FullRobotModel fullRobotModel) { for(OneDoFJoint joint : fullRobotModel.getOneDoFJoints()) { joints.add(joint); controllers.add(new PDController(joint.getName(), registry)); desiredPositions.add(new YoDouble(joint.getName() + "_q_d", registry)); desiredVelocities.add(new YoDouble(joint.getName() + "_qd_d", registry)); tauFFs.add(new YoDouble(joint.getName() + "_tau_ff", registry)); damping.add(new YoDouble(joint.getName() + "_damping", registry)); } }
public void setDerivativeGain(double derivativeGain) { pdController.setDerivativeGain(derivativeGain); }
public void setProportionalGain(double proportionalGain) { pdController.setProportionalGain(proportionalGain); }
public PassiveJointController(OneDegreeOfFreedomJoint simulatedJoint, YoPDGains gains) { registry = new YoVariableRegistry(simulatedJoint.getName() + name); jointController = new PDController(gains.getYoKp(), gains.getYoKd(), simulatedJoint.getName() + "PassiveController", registry); this.simulatedJoint = simulatedJoint; jointController.setProportionalGain(36000.0); jointController.setDerivativeGain(1000.0); }