public void setDerivativeGain(double derivativeGain) { pdController.setDerivativeGain(derivativeGain); }
public void setDerivativeGain(double derivativeGain) { pdController.setDerivativeGain(derivativeGain); }
public void setCoMHeightGains(double kp, double kd) { centerOfMassHeightController.setProportionalGain(kp); centerOfMassHeightController.setDerivativeGain(kd); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testAngleBoundaries() { PDController pdController = new PDController("suffix", new YoVariableRegistry("testRegistry")); pdController.setDerivativeGain(1.0); pdController.setProportionalGain(1.0); Random random = new Random(); for(int i = 0; i < 1000; i++) { double current = Math.PI - random.nextDouble()*Math.toRadians(89); double desired = -(Math.PI - random.nextDouble()*Math.toRadians(89)); double value = pdController.computeForAngles(current, desired, 0, 0); double error = (Math.PI - current) + (Math.PI - Math.abs(desired)); assertEquals((Math.PI - current) + (Math.PI - Math.abs(desired)), value, 1e-6); } } }
Double kd = jointGains.get("kd"); if (kd != null) jointPDController.setDerivativeGain(kd); jointPDController.setDerivativeGain(5.0); jointPDController.setDerivativeGain(5.0);
pdControllers.get(fullRobotModel.getArmJoint(robotSide, ArmJointName.SHOULDER_YAW)).setDerivativeGain(3.0); pdControllers.get(fullRobotModel.getArmJoint(robotSide, ArmJointName.SHOULDER_PITCH)).setDerivativeGain(5.0); pdControllers.get(fullRobotModel.getArmJoint(robotSide, ArmJointName.SHOULDER_ROLL)).setDerivativeGain(5.0); pdControllers.get(fullRobotModel.getArmJoint(robotSide, ArmJointName.ELBOW_PITCH)).setDerivativeGain(4.0); pdControllers.get(fullRobotModel.getSpineJoint(SpineJointName.SPINE_YAW)).setDerivativeGain(2.0); pdControllers.get(fullRobotModel.getSpineJoint(SpineJointName.SPINE_PITCH)).setDerivativeGain(8.0); pdControllers.get(fullRobotModel.getSpineJoint(SpineJointName.SPINE_ROLL)).setDerivativeGain(8.0); pdControllers.get(fullRobotModel.getLegJoint(robotSide, LegJointName.HIP_YAW)).setDerivativeGain(2.0); pdControllers.get(fullRobotModel.getLegJoint(robotSide, LegJointName.HIP_PITCH)).setDerivativeGain(7.5); pdControllers.get(fullRobotModel.getLegJoint(robotSide, LegJointName.HIP_ROLL)).setDerivativeGain(6.0); pdControllers.get(fullRobotModel.getLegJoint(robotSide, LegJointName.KNEE_PITCH)).setDerivativeGain(3.0); pdControllers.get(fullRobotModel.getLegJoint(robotSide, LegJointName.ANKLE_PITCH)).setDerivativeGain(2.0); pdControllers.get(fullRobotModel.getLegJoint(robotSide, LegJointName.ANKLE_ROLL)).setDerivativeGain(1.0);
pdControllers.get(fullRobotModel.getArmJoint(robotSide, ArmJointName.SHOULDER_YAW)).setDerivativeGain(3.0); pdControllers.get(fullRobotModel.getArmJoint(robotSide, ArmJointName.SHOULDER_PITCH)).setDerivativeGain(5.0); pdControllers.get(fullRobotModel.getArmJoint(robotSide, ArmJointName.SHOULDER_ROLL)).setDerivativeGain(5.0); pdControllers.get(fullRobotModel.getArmJoint(robotSide, ArmJointName.ELBOW_PITCH)).setDerivativeGain(4.0); pdControllers.get(fullRobotModel.getSpineJoint(SpineJointName.SPINE_YAW)).setDerivativeGain(2.0); pdControllers.get(fullRobotModel.getSpineJoint(SpineJointName.SPINE_PITCH)).setDerivativeGain(8.0); pdControllers.get(fullRobotModel.getSpineJoint(SpineJointName.SPINE_ROLL)).setDerivativeGain(8.0); pdControllers.get(fullRobotModel.getLegJoint(robotSide, LegJointName.HIP_YAW)).setDerivativeGain(2.0); pdControllers.get(fullRobotModel.getLegJoint(robotSide, LegJointName.HIP_PITCH)).setDerivativeGain(7.5); pdControllers.get(fullRobotModel.getLegJoint(robotSide, LegJointName.HIP_ROLL)).setDerivativeGain(6.0); pdControllers.get(fullRobotModel.getLegJoint(robotSide, LegJointName.KNEE_PITCH)).setDerivativeGain(3.0); pdControllers.get(fullRobotModel.getLegJoint(robotSide, LegJointName.ANKLE_PITCH)).setDerivativeGain(2.0); pdControllers.get(fullRobotModel.getLegJoint(robotSide, LegJointName.ANKLE_ROLL)).setDerivativeGain(1.0);
pdControllers.get(fullRobotModel.getArmJoint(robotSide, ArmJointName.SHOULDER_YAW)).setDerivativeGain(3.0); pdControllers.get(fullRobotModel.getArmJoint(robotSide, ArmJointName.SHOULDER_PITCH)).setDerivativeGain(5.0); pdControllers.get(fullRobotModel.getArmJoint(robotSide, ArmJointName.SHOULDER_ROLL)).setDerivativeGain(5.0); pdControllers.get(fullRobotModel.getArmJoint(robotSide, ArmJointName.ELBOW_PITCH)).setDerivativeGain(4.0); pdControllers.get(fullRobotModel.getSpineJoint(SpineJointName.SPINE_YAW)).setDerivativeGain(2.0); pdControllers.get(fullRobotModel.getSpineJoint(SpineJointName.SPINE_PITCH)).setDerivativeGain(8.0); pdControllers.get(fullRobotModel.getSpineJoint(SpineJointName.SPINE_ROLL)).setDerivativeGain(8.0); pdControllers.get(fullRobotModel.getLegJoint(robotSide, LegJointName.HIP_YAW)).setDerivativeGain(2.0); pdControllers.get(fullRobotModel.getLegJoint(robotSide, LegJointName.HIP_PITCH)).setDerivativeGain(7.5); pdControllers.get(fullRobotModel.getLegJoint(robotSide, LegJointName.HIP_ROLL)).setDerivativeGain(6.0); pdControllers.get(fullRobotModel.getLegJoint(robotSide, LegJointName.KNEE_PITCH)).setDerivativeGain(3.0); pdControllers.get(fullRobotModel.getLegJoint(robotSide, LegJointName.ANKLE_PITCH)).setDerivativeGain(2.0); pdControllers.get(fullRobotModel.getLegJoint(robotSide, LegJointName.ANKLE_ROLL)).setDerivativeGain(1.0);
pdControllers.get(fullRobotModel.getArmJoint(robotSide, ArmJointName.SHOULDER_YAW)).setDerivativeGain(3.0); pdControllers.get(fullRobotModel.getArmJoint(robotSide, ArmJointName.SHOULDER_PITCH)).setDerivativeGain(5.0); pdControllers.get(fullRobotModel.getArmJoint(robotSide, ArmJointName.SHOULDER_ROLL)).setDerivativeGain(5.0); pdControllers.get(fullRobotModel.getArmJoint(robotSide, ArmJointName.ELBOW_PITCH)).setDerivativeGain(4.0); pdControllers.get(fullRobotModel.getSpineJoint(SpineJointName.SPINE_YAW)).setDerivativeGain(2.0); pdControllers.get(fullRobotModel.getSpineJoint(SpineJointName.SPINE_PITCH)).setDerivativeGain(8.0); pdControllers.get(fullRobotModel.getSpineJoint(SpineJointName.SPINE_ROLL)).setDerivativeGain(8.0); pdControllers.get(fullRobotModel.getLegJoint(robotSide, LegJointName.HIP_YAW)).setDerivativeGain(2.0); pdControllers.get(fullRobotModel.getLegJoint(robotSide, LegJointName.HIP_PITCH)).setDerivativeGain(7.5); pdControllers.get(fullRobotModel.getLegJoint(robotSide, LegJointName.HIP_ROLL)).setDerivativeGain(6.0); pdControllers.get(fullRobotModel.getLegJoint(robotSide, LegJointName.KNEE_PITCH)).setDerivativeGain(3.0); pdControllers.get(fullRobotModel.getLegJoint(robotSide, LegJointName.ANKLE_PITCH)).setDerivativeGain(2.0); pdControllers.get(fullRobotModel.getLegJoint(robotSide, LegJointName.ANKLE_ROLL)).setDerivativeGain(1.0);
Double kd = jointGains.get("kd"); if (kd != null) jointPDController.setDerivativeGain(kd); jointPDController.setDerivativeGain(5.0); jointPDController.setDerivativeGain(5.0);
@Override public void initialize(long timestamp) { //Initialize controller gains in super-class from StandPrep for(StepprStandPrepSetpoints jointPair: StepprStandPrepSetpoints.values) { for(StepprJoint stepprJoint:jointPair.getJoints()) { for(int i=0;i<joints.size();i++) { if(joints.get(i).getName().equals(stepprJoint.getSdfName())) { PDController controller = controllers.get(i); controller.setDerivativeGain(jointPair.getKd()); controller.setProportionalGain(jointPair.getKp()); damping.get(i).set(jointPair.getDamping()); break; } } } } }
public void setGains(PDGainsReadOnly gains) { setProportionalGain(gains.getKp()); setDerivativeGain(gains.getKd()); setPositionDeadband(gains.getPositionDeadband()); }
@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 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 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 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 ValkyrieSliderBoardJointHolder(ValkyrieRosControlSliderBoard valkyrieRosControlSliderBoard, OneDoFJointBasics joint, YoVariableRegistry parentRegistry, double dt) { this.valkyrieRosControlSliderBoard = valkyrieRosControlSliderBoard; this.joint = joint; this.dt = dt; String jointName = joint.getName(); this.registry = new YoVariableRegistry(jointName); this.pdController = new PDController(jointName, registry); pdController.setProportionalGain(ValkyrieRosControlSliderBoard.KP_DEFAULT); pdController.setDerivativeGain(ValkyrieRosControlSliderBoard.KD_DEFAULT); q = new YoDouble(jointName + "_q", registry); qd = new YoDouble(jointName + "_qd", registry); bl_qd = new RevisedBacklashCompensatingVelocityYoVariable("bl_qd_" + jointName, "", valkyrieRosControlSliderBoard.jointVelocityAlphaFilter, q, dt, valkyrieRosControlSliderBoard.jointVelocitySlopTime, registry); tau = new YoDouble(jointName + "_tau", registry); q_d = new YoDouble(jointName + "_q_d", registry); qd_d = new YoDouble(jointName + "_qd_d", registry); if (valkyrieRosControlSliderBoard.setPointMap != null && valkyrieRosControlSliderBoard.setPointMap.containsKey(jointName)) q_d.set(valkyrieRosControlSliderBoard.setPointMap.get(jointName)); tau_offset = new YoDouble(joint.getName() + "_tau_offset", parentRegistry); tau_d = new YoDouble(joint.getName() + "_tau_d", registry); jointCommand_pd = new YoDouble(joint.getName() + "_tau_pd", registry); jointCommand_function = new YoDouble(joint.getName() + "_tau_function", registry); if (valkyrieRosControlSliderBoard.torqueOffsetMap != null && valkyrieRosControlSliderBoard.torqueOffsetMap.containsKey(joint.getName())) tau_offset.set(-valkyrieRosControlSliderBoard.torqueOffsetMap.get(joint.getName())); }
controller.setDerivativeGain(kd); double tau = controller.compute(oneDoFJoint.getQ(), qDesired, oneDoFJoint.getQd(), qdDesired); oneDoFJoint.setTau(tau);
controller.setDerivativeGain(kd); double tau = controller.compute(oneDoFJoint.getQ(), qDesired, oneDoFJoint.getQd(), qdDesired); oneDoFJoint.setTau(tau);
selected.qd_d.set(qdDesiredSelected.getDoubleValue()); selected.pdController.setProportionalGain(kpSelected.getDoubleValue()); selected.pdController.setDerivativeGain(kdSelected.getDoubleValue()); selected.jointCommand_function.set(tauFunctionSelected.getDoubleValue()); selected.tau_offset.set(tauOffsetSelected.getDoubleValue());