public RateLimitedYoSpatialVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maximumAngularRate, DoubleProvider maximumLinearRate, double dt, FrameVector3DReadOnly rawAngularPart, FrameVector3DReadOnly rawLinearPart) { super(new RateLimitedYoFrameVector(namePrefix, nameSuffix, registry, maximumAngularRate, dt, rawAngularPart), new RateLimitedYoFrameVector(namePrefix, nameSuffix, registry, maximumLinearRate, dt, rawLinearPart)); this.rateLimitedAngularPart = (RateLimitedYoFrameVector) getAngularPart(); this.rateLimitedLinearPart = (RateLimitedYoFrameVector) getLinearPart(); }
public RateLimitedYoSpatialVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maximumAngularRate, DoubleProvider maximumLinearRate, double dt, YoFixedFrameSpatialVector rawSpatialVector) { super(new RateLimitedYoFrameVector(namePrefix, nameSuffix, registry, maximumAngularRate, dt, rawSpatialVector.getAngularPart()), new RateLimitedYoFrameVector(namePrefix, nameSuffix, registry, maximumLinearRate, dt, rawSpatialVector.getLinearPart())); this.rateLimitedAngularPart = (RateLimitedYoFrameVector) getAngularPart(); this.rateLimitedLinearPart = (RateLimitedYoFrameVector) getLinearPart(); }
public static RateLimitedYoFrameVector createRateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, ReferenceFrame referenceFrame) { // alpha is a double RateLimitedYoVariable x = new RateLimitedYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, dt); RateLimitedYoVariable y = new RateLimitedYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, dt); RateLimitedYoVariable z = new RateLimitedYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, maxRate, dt); RateLimitedYoFrameVector ret = new RateLimitedYoFrameVector(x, y, z, referenceFrame); return ret; }
public static RateLimitedYoFrameVector createRateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleYoVariable maxRate, double dt, ReferenceFrame referenceFrame) { // alpha is a double RateLimitedYoVariable x = new RateLimitedYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, dt); RateLimitedYoVariable y = new RateLimitedYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, dt); RateLimitedYoVariable z = new RateLimitedYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, maxRate, dt); RateLimitedYoFrameVector ret = new RateLimitedYoFrameVector(x, y, z, referenceFrame); return ret; }
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); }
public static RateLimitedYoFrameVector createRateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, YoFrameVector unfilteredVector) { // alpha is a double RateLimitedYoVariable x = new RateLimitedYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoX(), dt); RateLimitedYoVariable y = new RateLimitedYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoY(), dt); RateLimitedYoVariable z = new RateLimitedYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoZ(), dt); RateLimitedYoFrameVector ret = new RateLimitedYoFrameVector(x, y, z, unfilteredVector.getReferenceFrame()); return ret; }
public static RateLimitedYoFrameVector createRateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleYoVariable maxRate, double dt, YoFrameVector unfilteredVector) { // alpha is a YoVariable RateLimitedYoVariable x = new RateLimitedYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoX(), dt); RateLimitedYoVariable y = new RateLimitedYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoY(), dt); RateLimitedYoVariable z = new RateLimitedYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoZ(), dt); RateLimitedYoFrameVector ret = new RateLimitedYoFrameVector(x, y, z, unfilteredVector.getReferenceFrame()); return ret; }