/** * Will create a new set of gains with the specified coupling and integration setting for * position and orientation. Will set the new gains to the provided initial values. * * @param suffix the name of the gains will be attached to all YoVariables. * @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 positionGains the initial values for the position gains. * @param orientationGains the initial values for the orientation gains. * @param registry the registry to which the tuning variables are attached. */ public DefaultYoPIDSE3Gains(String suffix, GainCoupling gainCouplingPosition, GainCoupling gainCouplingOrientation, boolean useIntegratorPosition, boolean useIntegratorOrientation, PID3DGainsReadOnly positionGains, PID3DGainsReadOnly orientationGains, YoVariableRegistry registry) { this.positionGains = new DefaultYoPID3DGains(suffix + "Position", gainCouplingPosition, useIntegratorPosition, positionGains, registry); this.orientationGains = new DefaultYoPID3DGains(suffix + "Orientation", gainCouplingOrientation, useIntegratorOrientation, orientationGains, registry); }
private static void createDampingUpdaters(Map<Axis, YoDouble> kpMap, Map<Axis, YoDouble> kdMap, Map<Axis, YoDouble> zetaMap, YoBoolean update, GainCoupling gainCoupling) { switch (gainCoupling) { case NONE: addDampingUpdater(Axis.X, kpMap, kdMap, zetaMap, update); addDampingUpdater(Axis.Y, kpMap, kdMap, zetaMap, update); addDampingUpdater(Axis.Z, kpMap, kdMap, zetaMap, update); break; case XY: addDampingUpdater(Axis.X, kpMap, kdMap, zetaMap, update); addDampingUpdater(Axis.Z, kpMap, kdMap, zetaMap, update); break; case YZ: addDampingUpdater(Axis.X, kpMap, kdMap, zetaMap, update); addDampingUpdater(Axis.Y, kpMap, kdMap, zetaMap, update); break; case XZ: addDampingUpdater(Axis.X, kpMap, kdMap, zetaMap, update); addDampingUpdater(Axis.Y, kpMap, kdMap, zetaMap, update); break; case XYZ: addDampingUpdater(Axis.X, kpMap, kdMap, zetaMap, update); break; } }
@Override public double[] getProportionalGains() { fillFromMap(kpMap, tempProportionalGains); return tempProportionalGains; }
populateMap(kpMap, "kp", suffix, gainCoupling, registry); populateMap(kdMap, "kd", suffix, gainCoupling, registry); populateMap(zetaMap, "zeta", suffix, gainCoupling, registry); populateMap(kiMap, "ki", suffix, gainCoupling, registry); maxIntegralError = new YoDouble("maxIntegralError" + suffix, registry); createDampingUpdaters(kpMap, kdMap, zetaMap, updateFromDampingRatio, gainCoupling); set(gains);
public void setDampingRatios(double dampingRatio) { setDampingRatios(dampingRatio, dampingRatio, dampingRatio); }
@Override public void setDerivativeGains(double derivativeGainX, double derivativeGainY, double derivativeGainZ) { updateFromDampingRatio.set(false); kdMap.get(Axis.X).set(derivativeGainX); kdMap.get(Axis.Y).set(derivativeGainY); kdMap.get(Axis.Z).set(derivativeGainZ); updateDampingRatios(); }
DefaultYoPID3DGains.populateMap(kdMap, "kd", suffix, gainCoupling, registry); populateMap(zetaMap, "zeta", suffix, gainCoupling, registry); DefaultYoPID3DGains.populateMap(kdMap, "kd", suffix, gainCoupling, registry); populateMap(zetaMap, "zeta", suffix, gainCoupling, defaultZetas, registry);
@Override public double[] getDerivativeGains() { DefaultYoPID3DGains.fillFromMap(kdMap, tempDerivativeGains); return tempDerivativeGains; }
public EuclideanPositionController(String prefix, ReferenceFrame bodyFrame, double dt, YoPID3DGains gains, YoVariableRegistry parentRegistry) { this.dt = dt; this.bodyFrame = bodyFrame; registry = new YoVariableRegistry(prefix + getClass().getSimpleName()); if (gains == null) gains = new DefaultYoPID3DGains(prefix, GainCoupling.NONE, true, registry); this.gains = gains; positionError = new YoFrameVector3D(prefix + "PositionError", bodyFrame, registry); positionErrorCumulated = new YoFrameVector3D(prefix + "PositionErrorCumulated", bodyFrame, registry); velocityError = new YoFrameVector3D(prefix + "LinearVelocityError", bodyFrame, registry); proportionalTerm = new FrameVector3D(bodyFrame); derivativeTerm = new FrameVector3D(bodyFrame); integralTerm = new FrameVector3D(bodyFrame); feedbackLinearAction = new YoFrameVector3D(prefix + "FeedbackLinearAction", bodyFrame, registry); rateLimitedFeedbackLinearAction = new RateLimitedYoFrameVector(prefix + "RateLimitedFeedbackLinearAction", "", registry, gains.getYoMaximumFeedbackRate(), dt, feedbackLinearAction); parentRegistry.addChild(registry); }
@Override public double[] getDerivativeGains() { fillFromMap(kdMap, tempDerivativeGains); return tempDerivativeGains; }
public AxisAngleOrientationController(String prefix, ReferenceFrame bodyFrame, double dt, YoPID3DGains gains, YoVariableRegistry parentRegistry) { this.dt = dt; this.bodyFrame = bodyFrame; registry = new YoVariableRegistry(prefix + getClass().getSimpleName()); if (gains == null) gains = new DefaultYoPID3DGains(prefix, GainCoupling.NONE, true, registry); this.gains = gains; rotationErrorInBody = new YoFrameVector3D(prefix + "RotationErrorInBody", bodyFrame, registry); rotationErrorCumulated = new YoFrameVector3D(prefix + "RotationErrorCumulated", bodyFrame, registry); velocityError = new YoFrameVector3D(prefix + "AngularVelocityError", bodyFrame, registry); proportionalTerm = new FrameVector3D(bodyFrame); derivativeTerm = new FrameVector3D(bodyFrame); integralTerm = new FrameVector3D(bodyFrame); feedbackAngularAction = new YoFrameVector3D(prefix + "FeedbackAngularAction", bodyFrame, registry); rateLimitedFeedbackAngularAction = new RateLimitedYoFrameVector(prefix + "RateLimitedFeedbackAngularAction", "", registry, gains.getYoMaximumFeedbackRate(), dt, feedbackAngularAction); parentRegistry.addChild(registry); }
@Override public double[] getIntegralGains() { if (!usingIntegrator) { for (int i = 0; i < 3; i++) { tempIntegralGains[i] = 0.0; } return tempIntegralGains; } fillFromMap(kiMap, tempIntegralGains); return tempIntegralGains; }