@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); } } }
@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(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 PIDController(YoPIDGains yoPIDGains, String suffix, YoVariableRegistry registry) { super(suffix, registry); pdController = new PDController(yoPIDGains, suffix, registry); this.integralGain = yoPIDGains.getYoKi(); this.maxIntegralError = yoPIDGains.getYoMaxIntegralError(); this.maxFeedback = yoPIDGains.getYoMaximumFeedback(); integralLeakRatio = yoPIDGains.getYoIntegralLeakRatio(); }
public PIDController(YoDouble proportionalGain, YoDouble integralGain, YoDouble derivativeGain, YoDouble maxIntegralError, String suffix, YoVariableRegistry registry) { super(suffix, registry); pdController = new PDController(proportionalGain, derivativeGain, suffix, registry); this.integralGain = integralGain; this.maxIntegralError = maxIntegralError; maxFeedback = new YoDouble("maxOutput_" + suffix, registry); maxFeedback.set(Double.POSITIVE_INFINITY); integralLeakRatio = new YoDouble("leak_" + suffix, registry); integralLeakRatio.set(1.0); addLeakRatioClipper(); }
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); }
@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 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 PIDController(YoPIDGains yoPIDGains, String suffix, YoVariableRegistry registry) { pdController = new PDController(yoPIDGains, suffix, registry); this.integralGain = yoPIDGains.getYoKi(); this.maxIntegralError = yoPIDGains.getYoMaxIntegralError(); this.maxOutput = yoPIDGains.getYoMaximumOutput(); cumulativeError = new DoubleYoVariable("cumulativeError_" + suffix, registry); cumulativeError.set(0.0); actionI = new DoubleYoVariable("integralAction_" + suffix, registry); actionI.set(0.0); integralLeakRatio = yoPIDGains.getYoIntegralLeakRatio(); }
@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); } }
@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) { YoDouble desiredPosition = new YoDouble(setpoint.getName() + "_q_d", registry); YoDouble kp = new YoDouble(setpoint.getName() + "_kp", registry); YoDouble kd = new YoDouble(setpoint.getName() + "_kd", registry); YoDouble damping = new YoDouble(setpoint.getName() + "_damping", registry); desiredPosition.set(setpoint.getQ()); kp.set(setpoint.getKp()); kd.set(setpoint.getKd()); damping.set(setpoint.getDamping()); desiredPositions.put(setpoint, desiredPosition); kps.put(setpoint, kp); kds.put(setpoint, kd); dampingValues.put(setpoint, damping); } }
@Override public void setFullRobotModel(FullRobotModel fullRobotModel) { for (WandererJoint joint : WandererJoint.values) { joints.put(joint, fullRobotModel.getOneDoFJointByName(joint.getSdfName())); controllers.put(joint, new PDController(joint.getSdfName(), registry)); } for (WandererStandPrepSetpoints setpoint : WandererStandPrepSetpoints.values) { YoDouble desiredPosition = new YoDouble(setpoint.getName() + "_q_d", registry); YoDouble kp = new YoDouble(setpoint.getName() + "_kp", registry); YoDouble kd = new YoDouble(setpoint.getName() + "_kd", registry); YoDouble damping = new YoDouble(setpoint.getName() + "_damping", registry); desiredPosition.set(setpoint.getQ()); kp.set(setpoint.getKp()); kd.set(setpoint.getKd()); damping.set(setpoint.getDamping()); desiredPositions.put(setpoint, desiredPosition); kps.put(setpoint, kp); kds.put(setpoint, kd); dampingValues.put(setpoint, damping); } }
public PIDController(DoubleYoVariable proportionalGain, DoubleYoVariable integralGain, DoubleYoVariable derivativeGain, DoubleYoVariable maxIntegralError, String suffix, YoVariableRegistry registry) { pdController = new PDController(proportionalGain, derivativeGain, suffix, registry); this.integralGain = integralGain; this.maxIntegralError = maxIntegralError; maxOutput = new DoubleYoVariable("maxOutput_" + suffix, registry); maxOutput.set(Double.POSITIVE_INFINITY); cumulativeError = new DoubleYoVariable("cumulativeError_" + suffix, registry); cumulativeError.set(0.0); actionI = new DoubleYoVariable("integralAction_" + suffix, registry); actionI.set(0.0); integralLeakRatio = new DoubleYoVariable("leak_" + suffix, registry); integralLeakRatio.set(1.0); }
public PIDController(String suffix, YoVariableRegistry registry) { super(suffix, registry); pdController = new PDController(suffix, registry); integralGain = new YoDouble("ki_" + suffix, registry); integralGain.set(0.0); maxIntegralError = new YoDouble("maxIntegralError_" + suffix, registry); maxIntegralError.set(Double.POSITIVE_INFINITY); maxFeedback = new YoDouble("maxOutput_" + suffix, registry); maxFeedback.set(Double.POSITIVE_INFINITY); integralLeakRatio = new YoDouble("leak_" + suffix, registry); integralLeakRatio.set(1.0); addLeakRatioClipper(); }
PDController controller = new PDController(jointName + "Calibration", registry); pdControllers.put(joint, controller);
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())); }
PDController controller = new PDController(jointName + "Calibration", registry); pdControllers.put(joint, controller);
public PIDController(String suffix, YoVariableRegistry registry) { pdController = new PDController(suffix, registry); integralGain = new DoubleYoVariable("ki_" + suffix, registry); integralGain.set(0.0); maxIntegralError = new DoubleYoVariable("maxIntegralError_" + suffix, registry); maxIntegralError.set(Double.POSITIVE_INFINITY); maxOutput = new DoubleYoVariable("maxOutput_" + suffix, registry); maxOutput.set(Double.POSITIVE_INFINITY); cumulativeError = new DoubleYoVariable("cumulativeError_" + suffix, registry); cumulativeError.set(0.0); actionI = new DoubleYoVariable("action_I_" + suffix, registry); actionI.set(0.0); integralLeakRatio = new DoubleYoVariable("leak_" + suffix, registry); integralLeakRatio.set(1.0); VariableChangedListener leakRatioClipper = new VariableChangedListener() { @Override public void variableChanged(YoVariable<?> v) { integralLeakRatio.set(MathTools.clipToMinMax(integralLeakRatio.getDoubleValue(), 0.0, 1.0), false); } }; integralLeakRatio.addVariableChangedListener(leakRatioClipper); }
public CenterOfMassHeightManager(HighLevelHumanoidControllerToolbox momentumBasedController, WalkingControllerParameters walkingControllerParameters, YoVariableRegistry parentRegistry) { CommonHumanoidReferenceFrames referenceFrames = momentumBasedController.getReferenceFrames(); centerOfMassFrame = referenceFrames.getCenterOfMassFrame(); centerOfMassJacobian = momentumBasedController.getCenterOfMassJacobian(); pelvisFrame = referenceFrames.getPelvisFrame(); gravity = momentumBasedController.getGravityZ(); pelvis = momentumBasedController.getFullRobotModel().getPelvis(); twistCalculator = momentumBasedController.getTwistCalculator(); centerOfMassTrajectoryGenerator = createTrajectoryGenerator(momentumBasedController, walkingControllerParameters, referenceFrames); // TODO: Fix low level stuff so that we are truly controlling pelvis height and not CoM height. controlPelvisHeightInsteadOfCoMHeight.set(true); YoPDGains comHeightControlGains = walkingControllerParameters.createCoMHeightControlGains(registry); DoubleYoVariable kpCoMHeight = comHeightControlGains.getYoKp(); DoubleYoVariable kdCoMHeight = comHeightControlGains.getYoKd(); DoubleYoVariable maxCoMHeightAcceleration = comHeightControlGains.getYoMaximumFeedback(); DoubleYoVariable maxCoMHeightJerk = comHeightControlGains.getYoMaximumFeedbackRate(); double controlDT = momentumBasedController.getControlDT(); // TODO Need to extract the maximum velocity parameter. coMHeightTimeDerivativesSmoother = new CoMHeightTimeDerivativesSmoother(null, maxCoMHeightAcceleration, maxCoMHeightJerk, controlDT, registry); this.centerOfMassHeightController = new PDController(kpCoMHeight, kdCoMHeight, "comHeight", registry); parentRegistry.addChild(registry); }