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); }
private void computeDerivativeTerm(FrameVector desiredVelocity, FrameVector currentVelocity) { desiredVelocity.changeFrame(bodyFrame); currentVelocity.changeFrame(bodyFrame); derivativeTerm.sub(desiredVelocity, currentVelocity); // Limit the maximum velocity error considered for control action double maximumVelocityError = gains.getMaximumDerivativeError(); double velocityErrorMagnitude = derivativeTerm.length(); if (velocityErrorMagnitude > maximumVelocityError) { derivativeTerm.scale(maximumVelocityError / velocityErrorMagnitude); } velocityError.set(derivativeTerm); tempMatrix.set(derivativeGainMatrix); tangentialDampingCalculator.compute(positionError, tempMatrix); tempMatrix.transform(derivativeTerm.getVector()); }
/** * Computes the new derivative gains to use that reduces the amount of damping in the direction with the most position error. * This prevents a high desired correcting velocity from being penalized when it deviates from the objective motion. * @param positionError current position error being used by the feedback control * @param derivativeGainsToPack derivative gain matrix to use with less damping in the main direction of motion */ public void compute(YoFrameVector3D positionError, Matrix3D derivativeGainsToPack) { this.positionError.setIncludingFrame(positionError); if (positionError.length() > tangentialDampingGains.getParallelDampingDeadband()) { bodyFrameTangentToControl.update(); double alpha = computeDampingReductionRatioParallelToMotion(positionError.length()); transformedGains.setToZero(bodyFrame); transformedGains.set(derivativeGainsToPack); transformedGains.changeFrame(bodyFrameTangentToControl); transformedGains.setElement(0, 0, alpha * transformedGains.getElement(0, 0)); transformedGains.setElement(1, 0, alpha * transformedGains.getElement(1, 0)); transformedGains.setElement(2, 0, alpha * transformedGains.getElement(2, 0)); transformedGains.changeFrame(bodyFrame); transformedGains.getMatrix(derivativeGainsToPack); } }
/** * Computes the new derivative gains to use that reduces the amount of damping in the direction with the most position error. * This prevents a high desired correcting velocity from being penalized when it deviates from the objective motion. * @param positionError current position error being used by the feedback control * @param derivativeGainsToPack derivative gain matrix to use with less damping in the main direction of motion */ public void compute(YoFrameVector positionError, Matrix3d derivativeGainsToPack) { this.positionError.setIncludingFrame(positionError.getFrameTuple()); if (positionError.length() > tangentialDampingGains.getParallelDampingDeadband()) { bodyFrameTangentToControl.update(); double alpha = computeDampingReductionRatioParallelToMotion(positionError.length()); transformedGains.setToZero(bodyFrame); transformedGains.set(derivativeGainsToPack); transformedGains.changeFrame(bodyFrameTangentToControl); transformedGains.setElement(0, 0, alpha * transformedGains.getElement(0, 0)); transformedGains.setElement(1, 0, alpha * transformedGains.getElement(1, 0)); transformedGains.setElement(2, 0, alpha * transformedGains.getElement(2, 0)); transformedGains.changeFrame(bodyFrame); transformedGains.getMatrix(derivativeGainsToPack); } }