/** * Will create a new set of gains will the specified coupling and integration setting for * position and orientation. Will use the provided gains as default values for the parameters. * * @param suffix the name of the gains will be attached to all parameters. * @param gainCouplingPosition the gain coupling for the position gains. * @param gainCouplingOrientation the gain coupling for the orientation gains. * @param useIntegratorPosition whether the position gains will use an integrator. * @param useIntegratorOrientation whether the orientation gains will use an integrator. * @param defaultPositionGains the default values for the position gains. * @param defaultOrientationGains the default values for the orientation gains. * @param registry the registry to which the tuning variables are attached. */ public ParameterizedPIDSE3Gains(String suffix, GainCoupling gainCouplingPosition, GainCoupling gainCouplingOrientation, boolean useIntegratorPosition, boolean useIntegratorOrientation, PID3DGainsReadOnly defaultPositionGains, PID3DGainsReadOnly defaultOrientationGains, YoVariableRegistry registry) { positionGains = new ParameterizedPID3DGains(suffix + "Position", gainCouplingPosition, useIntegratorPosition, defaultPositionGains, registry); orientationGains = new ParameterizedPID3DGains(suffix + "Orientation", gainCouplingOrientation, useIntegratorOrientation, defaultOrientationGains, registry); }
@Override public double[] getProportionalGains() { fillFromMap(kpMap, tempProportionalGains); return tempProportionalGains; }
populateMap(kpMap, "kp", suffix, gainCoupling, registry); DefaultYoPID3DGains.populateMap(kdMap, "kd", suffix, gainCoupling, registry); populateMap(zetaMap, "zeta", suffix, gainCoupling, registry); populateMap(kiMap, "ki", suffix, gainCoupling, registry); maxIntegralError = new DoubleParameter("maxIntegralError" + suffix, registry, Double.POSITIVE_INFINITY); createDampingUpdaters(kpMap, kdMap, zetaMap, gainCoupling); populateMap(kpMap, "kp", suffix, gainCoupling, defaultProportionalGains, registry); DefaultYoPID3DGains.populateMap(kdMap, "kd", suffix, gainCoupling, registry); populateMap(zetaMap, "zeta", suffix, gainCoupling, defaultZetas, registry); populateMap(kiMap, "ki", suffix, gainCoupling, defaultIntegralGains, registry); maxIntegralError = new DoubleParameter("maxIntegralError" + suffix, registry, defaults.getMaximumIntegralError()); createDampingUpdaters(kpMap, kdMap, zetaMap, gainCoupling);
private static void createDampingUpdaters(Map<Axis, DoubleParameter> kpMap, Map<Axis, YoDouble> kdMap, Map<Axis, DoubleParameter> zetaMap, GainCoupling gainCoupling) { switch (gainCoupling) { case NONE: addDampingUpdater(Axis.X, kpMap, kdMap, zetaMap); addDampingUpdater(Axis.Y, kpMap, kdMap, zetaMap); addDampingUpdater(Axis.Z, kpMap, kdMap, zetaMap); break; case XY: addDampingUpdater(Axis.X, kpMap, kdMap, zetaMap); addDampingUpdater(Axis.Z, kpMap, kdMap, zetaMap); break; case YZ: addDampingUpdater(Axis.X, kpMap, kdMap, zetaMap); addDampingUpdater(Axis.Y, kpMap, kdMap, zetaMap); break; case XZ: addDampingUpdater(Axis.X, kpMap, kdMap, zetaMap); addDampingUpdater(Axis.Y, kpMap, kdMap, zetaMap); break; case XYZ: addDampingUpdater(Axis.X, kpMap, kdMap, zetaMap); break; } }
@Override public double[] getIntegralGains() { if (!usingIntegrator) { for (int i = 0; i < 3; i++) { tempIntegralGains[i] = 0.0; } return tempIntegralGains; } fillFromMap(kiMap, tempIntegralGains); return tempIntegralGains; }