private double computeDampingReductionRatioParallelToMotion(double parallelError) { double reductionRatio = MathTools.clamp(tangentialDampingGains.getKdReductionRatio(), 0.0, 1.0); double deadband = tangentialDampingGains.getParallelDampingDeadband(); double maxError = tangentialDampingGains.getPositionErrorForMinimumKd(); if (Double.isInfinite(deadband) || deadband > maxError || (Math.abs(parallelError) < deadband)) { return 1.0; } else { double slope = (1.0 - reductionRatio) / (maxError - deadband); return 1.0 - slope * (Math.abs(parallelError) - deadband); } } }
private double computeDampingReductionRatioParallelToMotion(double parallelError) { double reductionRatio = MathTools.clipToMinMax(tangentialDampingGains.getKdReductionRatio(), 0.0, 1.0); double deadband = tangentialDampingGains.getParallelDampingDeadband(); double maxError = tangentialDampingGains.getPositionErrorForMinimumKd(); if (Double.isInfinite(deadband) || deadband > maxError || (Math.abs(parallelError) < deadband)) { return 1.0; } else { double slope = (1.0 - reductionRatio) / (maxError - deadband); return 1.0 - slope * (Math.abs(parallelError) - deadband); } } }
public void set(TangentialDampingGains tangentialDampingGains) { if (tangentialDampingGains != null) { set(tangentialDampingGains.getKdReductionRatio(), tangentialDampingGains.getParallelDampingDeadband(), tangentialDampingGains.getPositionErrorForMinimumKd()); } }
public void set(TangentialDampingGains tangentialDampingGains) { if (tangentialDampingGains != null) { set(tangentialDampingGains.getKdReductionRatio(), tangentialDampingGains.getParallelDampingDeadband(), tangentialDampingGains.getPositionErrorForMinimumKd()); } }
/** * 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); } }