/** * 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); }
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); }
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); }