public void reset() { rateLimitedFeedbackLinearAcceleration.reset(); }
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(); }
private RateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, FrameTuple3DReadOnly rawPosition, ReferenceFrame referenceFrame) { super(namePrefix, nameSuffix, referenceFrame, registry); this.hasBeenCalled = new YoBoolean(namePrefix + "HasBeenCalled" + nameSuffix, registry); this.limited = new YoBoolean(namePrefix + "Limited" + nameSuffix, registry); if (maxRate == null) maxRate = createMaxRateYoDouble(namePrefix, nameSuffix, Double.POSITIVE_INFINITY, registry); maxRateVariable = maxRate; this.rawPosition = rawPosition; this.dt = dt; reset(); }
public void update(double xUnfiltered, double yUnfiltered, double zUnfiltered) { if (!hasBeenCalled.getBooleanValue() || containsNaN()) { hasBeenCalled.set(true); set(xUnfiltered, yUnfiltered, zUnfiltered); } if (maxRateVariable.getValue() < 0) throw new RuntimeException("The maxRate parameter in the " + getClass().getSimpleName() + " cannot be negative."); differenceVector.setToZero(getReferenceFrame()); differenceVector.set(xUnfiltered, yUnfiltered, zUnfiltered); differenceVector.sub(getX(), getY(), getZ()); limited.set(differenceVector.clipToMaxLength(maxRateVariable.getValue() * dt)); add(differenceVector); } }
@Override public void compute(FrameVector output, FramePoint desiredPosition, FrameVector desiredVelocity, FrameVector currentVelocity, FrameVector feedForward) { computeProportionalTerm(desiredPosition); if (currentVelocity != null) computeDerivativeTerm(desiredVelocity, currentVelocity); computeIntegralTerm(); output.setToNaN(bodyFrame); output.add(proportionalTerm, derivativeTerm); output.add(integralTerm); // Limit the max acceleration of the feedback, but not of the feedforward... // JEP changed 150430 based on Atlas hitting limit stops. double feedbackLinearActionMagnitude = output.length(); if (feedbackLinearActionMagnitude > gains.getMaximumFeedback()) { output.scale(gains.getMaximumFeedback() / feedbackLinearActionMagnitude); } feedbackLinearAction.set(output); rateLimitedFeedbackLinearAction.update(); rateLimitedFeedbackLinearAction.getFrameTuple(output); feedForward.changeFrame(bodyFrame); output.add(feedForward); }
public void update(FrameTuple3DReadOnly frameVectorUnfiltered) { checkReferenceFrameMatch(frameVectorUnfiltered); update(frameVectorUnfiltered.getX(), frameVectorUnfiltered.getY(), frameVectorUnfiltered.getZ()); }
public void compute(FrameVector acceleration, FramePoint desiredPosition, FrameVector desiredVelocity, FrameVector feedForward, RigidBody base) { ReferenceFrame baseFrame = base.getBodyFixedFrame(); computeProportionalTerm(desiredPosition, baseFrame); computeDerivativeTerm(desiredVelocity, base); computeIntegralTerm(baseFrame); acceleration.setIncludingFrame(proportionalTerm); acceleration.add(derivativeTerm); acceleration.add(integralTerm); // Limit the max acceleration of the feedback, but not of the feedforward... // JEP changed 150430 based on Atlas hitting limit stops. double feedbackLinearAccelerationMagnitude = acceleration.length(); if (feedbackLinearAccelerationMagnitude > gains.getMaximumFeedback()) { acceleration.scale(gains.getMaximumFeedback() / feedbackLinearAccelerationMagnitude); } feedbackLinearAcceleration.setAndMatchFrame(acceleration); rateLimitedFeedbackLinearAcceleration.update(); rateLimitedFeedbackLinearAcceleration.getFrameTupleIncludingFrame(acceleration); acceleration.changeFrame(baseFrame); feedForward.changeFrame(baseFrame); acceleration.add(feedForward); }
public BodyFixedPointLinearAccelerationControlModule(String namePrefix, TwistCalculator twistCalculator, RigidBody endEffector, double dt, YoPositionPIDGainsInterface gains, YoVariableRegistry parentRegistry) { registry = new YoVariableRegistry(namePrefix + getClass().getSimpleName()); this.dt = dt; this.twistCalculator = twistCalculator; this.endEffector = endEffector; if (gains == null) gains = new YoEuclideanPositionGains(namePrefix, registry); this.gains = gains; proportionalGainMatrix = gains.createProportionalGainMatrix(); derivativeGainMatrix = gains.createDerivativeGainMatrix(); integralGainMatrix = gains.createIntegralGainMatrix(); frameAtBodyFixedPoint = new YoTranslationFrame(namePrefix + "BodyFixedPointFrame", endEffector.getBodyFixedFrame(), registry); positionError = new YoFrameVector(namePrefix + "PositionError", worldFrame, registry); positionErrorCumulated = new YoFrameVector(namePrefix + "PositionErrorCumulated", worldFrame, registry); velocityError = new YoFrameVector(namePrefix + "LinearVelocityError", worldFrame, registry); feedbackLinearAcceleration = new YoFrameVector(namePrefix + "FeedbackLinearAcceleration", worldFrame, registry); rateLimitedFeedbackLinearAcceleration = RateLimitedYoFrameVector.createRateLimitedYoFrameVector(namePrefix + "RateLimitedFeedbackLinearAcceleration", "", registry, gains.getYoMaximumFeedbackRate(), dt, feedbackLinearAcceleration); parentRegistry.addChild(registry); }
public RateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, ReferenceFrame referenceFrame) { this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, null, referenceFrame); }
public void compute(FrameVector output, FrameOrientation desiredOrientation, FrameVector desiredAngularVelocity, FrameVector currentAngularVelocity, FrameVector feedForward) { computeProportionalTerm(desiredOrientation); if (currentAngularVelocity != null) computeDerivativeTerm(desiredAngularVelocity, currentAngularVelocity); computeIntegralTerm(); output.setToZero(proportionalTerm.getReferenceFrame()); output.add(proportionalTerm); output.add(derivativeTerm); output.add(integralTerm); // Limit the max acceleration of the feedback, but not of the feedforward... // JEP changed 150430 based on Atlas hitting limit stops. double feedbackAngularActionMagnitude = output.length(); double maximumAction = gains.getMaximumFeedback(); if (feedbackAngularActionMagnitude > maximumAction) { output.scale(maximumAction / feedbackAngularActionMagnitude); } feedbackAngularAction.set(output); rateLimitedFeedbackAngularAction.update(); rateLimitedFeedbackAngularAction.getFrameTuple(output); feedForward.changeFrame(bodyFrame); output.add(feedForward); }
public AxisAngleOrientationController(String prefix, ReferenceFrame bodyFrame, double dt, YoOrientationPIDGainsInterface gains, YoVariableRegistry parentRegistry) { this.dt = dt; this.bodyFrame = bodyFrame; registry = new YoVariableRegistry(prefix + getClass().getSimpleName()); if (gains == null) gains = new YoAxisAngleOrientationGains(prefix, registry); this.gains = gains; proportionalGainMatrix = gains.createProportionalGainMatrix(); derivativeGainMatrix = gains.createDerivativeGainMatrix(); integralGainMatrix = gains.createIntegralGainMatrix(); rotationErrorInBody = new YoFrameVector(prefix + "RotationErrorInBody", bodyFrame, registry); rotationErrorCumulated = new YoFrameVector(prefix + "RotationErrorCumulated", bodyFrame, registry); velocityError = new YoFrameVector(prefix + "AngularVelocityError", bodyFrame, registry); proportionalTerm = new FrameVector(bodyFrame); derivativeTerm = new FrameVector(bodyFrame); integralTerm = new FrameVector(bodyFrame); feedbackAngularAction = new YoFrameVector(prefix + "FeedbackAngularAction", bodyFrame, registry); rateLimitedFeedbackAngularAction = RateLimitedYoFrameVector.createRateLimitedYoFrameVector(prefix + "RateLimitedFeedbackAngularAction", "", registry, gains.getYoMaximumFeedbackRate(), dt, feedbackAngularAction); parentRegistry.addChild(registry); }
public RateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, FrameTuple3DReadOnly rawPosition) { this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, rawPosition, rawPosition.getReferenceFrame()); }
public void reset() { rateLimitedFeedbackLinearAction.reset(); }
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 EuclideanPositionController(String prefix, ReferenceFrame bodyFrame, double dt, YoPositionPIDGainsInterface gains, YoVariableRegistry parentRegistry) { this.dt = dt; this.bodyFrame = bodyFrame; registry = new YoVariableRegistry(prefix + getClass().getSimpleName()); if (gains == null) gains = new YoEuclideanPositionGains(prefix, registry); this.gains = gains; proportionalGainMatrix = gains.createProportionalGainMatrix(); derivativeGainMatrix = gains.createDerivativeGainMatrix(); integralGainMatrix = gains.createIntegralGainMatrix(); positionError = new YoFrameVector(prefix + "PositionError", bodyFrame, registry); positionErrorCumulated = new YoFrameVector(prefix + "PositionErrorCumulated", bodyFrame, registry); velocityError = new YoFrameVector(prefix + "LinearVelocityError", bodyFrame, registry); proportionalTerm = new FrameVector(bodyFrame); derivativeTerm = new FrameVector(bodyFrame); integralTerm = new FrameVector(bodyFrame); feedbackLinearAction = new YoFrameVector(prefix + "FeedbackLinearAction", bodyFrame, registry); rateLimitedFeedbackLinearAction = RateLimitedYoFrameVector.createRateLimitedYoFrameVector(prefix + "RateLimitedFeedbackLinearAction", "", registry, gains.getYoMaximumFeedbackRate(), dt, feedbackLinearAction); tangentialDampingCalculator = new EuclideanTangentialDampingCalculator(prefix, bodyFrame, gains.getTangentialDampingGains()); parentRegistry.addChild(registry); }
public void reset() { rateLimitedFeedbackLinearAction.reset(); }
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; }