public static double computeAlphaGivenBreakFrequency(double breakFrequencyInHetrz, double dt) { double alpha = 1.0 - 4.0 * dt * breakFrequencyInHetrz; alpha = MathTools.clamp(alpha, 0.0, 1.0); return alpha; }
public static double computeAlphaGivenBreakFrequencyProperly(double breakFrequencyInHertz, double dt) { if (Double.isInfinite(breakFrequencyInHertz)) return 0.0; double omega = 2.0 * Math.PI * breakFrequencyInHertz; double alpha = (1.0 - omega * dt / 2.0) / (1.0 + omega * dt / 2.0); alpha = MathTools.clamp(alpha, 0.0, 1.0); return alpha; }
@Override public FramePose2D getPoseAtS(double pathVariableS) { pathVariableS = MathTools.clamp(pathVariableS, 0.0, 1.0); return getExtrapolatedPoseAtS(pathVariableS); }
@Override public FramePose2D getPoseAtS(double pathVariableS) { pathVariableS = MathTools.clamp(pathVariableS, 0.0, 1.0); return getExtrapolatedPoseAtS(pathVariableS); }
public void getPosition(FramePoint3D positionToPack) { double parameter = minimumJerkTrajectory.getPosition(); parameter = MathTools.clamp(parameter, 0.0, 1.0); parabolicTrajectoryGenerator.getPosition(positionToPack, parameter); }
public void getPosition(FramePoint3D positionToPack) { double parameter = minimumJerkTrajectory.getPosition(); parameter = MathTools.clamp(parameter, 0.0, 1.0); parabolicTrajectoryGenerator.getPosition(positionToPack, parameter); }
@Override public void notifyOfVariableChange(YoVariable<?> v) { integralLeakRatio.set(MathTools.clamp(integralLeakRatio.getDoubleValue(), 0.0, 1.0), false); } };
private Color getHeatColor(int heat) { double maxHeat = 30.0; int heatIndex = (int) MathTools.roundToPrecision(MathTools.clamp((heat / maxHeat) * 500.0, 0.0, 499.0), 1.0); return ColorConversions.awtToJfx(rainbow[heatIndex]); }
private void updateTurningVelocity(double alpha) { double minMaxVelocity = (maxAngleTurnOutwards - maxAngleTurnInwards) / stepTime.getValue(); if (forwardVelocityProperty.get() < -1.0e-10) alpha = -alpha; turningVelocityProperty.set(minMaxVelocity * MathTools.clamp(alpha, 1.0)); }
@Override public void processEvent(Event event) { if (event.getComponent() == component) { double value = getDPadValue(event); double newValue = (sign * value * increment) + variable.getDoubleValue(); newValue = MathTools.clamp(newValue, min, max); variable.set(newValue); } }
public static HandCollisionDetectedPacket createHandCollisionDetectedPacket(RobotSide robotSide, int collisionSeverityLevelZeroToThree) { HandCollisionDetectedPacket message = new HandCollisionDetectedPacket(); message.setRobotSide(robotSide.toByte()); message.setCollisionSeverityLevelOneToThree(MathTools.clamp(collisionSeverityLevelZeroToThree, 1, 3)); return message; }
public void getVelocity(FrameVector3D velocityToPack) { double parameter = minimumJerkTrajectory.getPosition(); parameter = MathTools.clamp(parameter, 0.0, 1.0); parabolicTrajectoryGenerator.getVelocity(tempVector, parameter); velocityToPack.setIncludingFrame(tempVector); velocityToPack.scale(minimumJerkTrajectory.getVelocity()); }
private double getBias() { if (useBias.getBooleanValue()) { double biasWalk = biasDelta.getDoubleValue() * ((2.0 * rand.nextDouble()) - 1.0); bias.set(MathTools.clamp(bias.getDoubleValue() + biasWalk, biasMin.getDoubleValue(), biasMax.getDoubleValue())); return bias.getDoubleValue(); } else return 0.0; }
private static double getCosineAngleWithCosineLaw(double l_neighbour1, double l_neighbour2, double l_opposite) { checkIntervalContains(l_neighbour1, 0.0, Double.POSITIVE_INFINITY); checkIntervalContains(l_neighbour2, 0.0, Double.POSITIVE_INFINITY); checkIntervalContains(l_opposite, 0.0, Double.POSITIVE_INFINITY); double cosAngle = MathTools.clamp((square(l_neighbour1) + square(l_neighbour2) - square(l_opposite)) / (2.0 * l_neighbour1 * l_neighbour2), -1.0, 1.0); return cosAngle; }
public static double getCosineAngleWithCosineLaw(double l_neighbour1, double l_neighbour2, double l_opposite) { checkIntervalContains(l_neighbour1, 0.0, Double.POSITIVE_INFINITY); checkIntervalContains(l_neighbour2, 0.0, Double.POSITIVE_INFINITY); checkIntervalContains(l_opposite, 0.0, Double.POSITIVE_INFINITY); double cosAngle = MathTools .clamp((square(l_neighbour1) + square(l_neighbour2) - square(l_opposite)) / (2.0 * l_neighbour1 * l_neighbour2), -1.0, 1.0); return cosAngle; }
private void updateJointAngles() { for (int i = 0; i < oneDoFJoints.length; i++) { double newQ = oneDoFJoints[i].getQ() + correction.get(i, 0); newQ = MathTools.clamp(newQ, oneDoFJoints[i].getJointLimitLower(), oneDoFJoints[i].getJointLimitUpper()); oneDoFJoints[i].setQ(newQ); oneDoFJoints[i].getFrameAfterJoint().update(); } }
public void update(double input) { if (!hasBeenInitialized.getBooleanValue()) initialize(input); double positionError = input - this.getDoubleValue(); double acceleration = -velocityGain.getDoubleValue() * smoothedRate.getDoubleValue() + positionGain.getDoubleValue() * positionError; acceleration = MathTools.clamp(acceleration, -maximumAcceleration.getDoubleValue(), maximumAcceleration.getDoubleValue()); smoothedAcceleration.set(acceleration); smoothedRate.add(smoothedAcceleration.getDoubleValue() * dt); smoothedRate.set(MathTools.clamp(smoothedRate.getDoubleValue(), maximumRate.getDoubleValue())); this.add(smoothedRate.getDoubleValue() * dt); }
public void getAcceleration(FrameVector3D accelerationToPack) { double parameter = minimumJerkTrajectory.getPosition(); parameter = MathTools.clamp(parameter, 0.0, 1.0); parabolicTrajectoryGenerator.getAcceleration(accelerationToPack); accelerationToPack.scale(minimumJerkTrajectory.getVelocity() * minimumJerkTrajectory.getVelocity()); parabolicTrajectoryGenerator.getVelocity(tempVector, parameter); tempVector.scale(minimumJerkTrajectory.getAcceleration()); accelerationToPack.add(tempVector); }
public void getAcceleration(FrameVector3D accelerationToPack) { double parameter = minimumJerkTrajectory.getPosition(); parameter = MathTools.clamp(parameter, 0.0, 1.0); parabolicTrajectoryGenerator.getAcceleration(accelerationToPack); accelerationToPack.scale(minimumJerkTrajectory.getVelocity() * minimumJerkTrajectory.getVelocity()); parabolicTrajectoryGenerator.getVelocity(tempVector, parameter); tempVector.scale(minimumJerkTrajectory.getAcceleration()); accelerationToPack.add(tempVector); }
@Override public void getRMatrix(DenseMatrix64F matrixToPack) { matrixToPack.reshape(getMeasurementSize(), getMeasurementSize()); CommonOps.setIdentity(matrixToPack); double percent = (loadPercentage.getValue() - weightFractionForNoTrust.getValue()) / (weightFractionForFullTrust.getValue() - weightFractionForNoTrust.getValue()); percent = MathTools.clamp(percent, 0.0, 1.0); variance.set(maxVariance.getValue() - percent * (maxVariance.getValue() - minVariance.getValue())); CommonOps.scale(variance.getValue() * sqrtHz, matrixToPack); }