public HandCollisionDetectedPacket(RobotSide robotSide, int collisionSeverityLevelZeroToThree) { this.robotSide = robotSide; this.collisionSeverityLevelOneToThree = MathTools.clipToMinMax(collisionSeverityLevelZeroToThree, 1, 3); }
public void setTrajectoryTime(double trajectoryTime) { // this.trajectoryTime = trajectoryTime; // limiting motor speed for safe joint speed. if the arms exceed (700 rad / sec) / (100 gear ratio) = 7 rad/sec we are in trouble this.trajectoryTime = MathTools.clipToMinMax(trajectoryTime, 2.0, Double.MAX_VALUE); }
@Override public FramePose2d getPoseAtS(double pathVariableS) { pathVariableS = MathTools.clipToMinMax(pathVariableS, 0.0, 1.0); return getExtrapolatedPoseAtS(pathVariableS); }
public void interpolate(Quat4d quaternion1, Quat4d quaternion2, double alpha) { alpha = MathTools.clipToMinMax(alpha, 0.0, 1.0); quaternion.interpolate(quaternion1, quaternion2, alpha); }
public void setLinearAlphaTaskPriority(double linearX, double linearY, double linearZ) { alphaTaskPriorityVector.set(3, 0, MathTools.clipToMinMax(linearX, 0.0, 1.0)); alphaTaskPriorityVector.set(4, 0, MathTools.clipToMinMax(linearY, 0.0, 1.0)); alphaTaskPriorityVector.set(5, 0, MathTools.clipToMinMax(linearZ, 0.0, 1.0)); }
public void setAngularAlphasTaskPriority(double angularX, double angularY, double angularZ) { alphaTaskPriorityVector.set(0, 0, MathTools.clipToMinMax(angularX, 0.0, 1.0)); alphaTaskPriorityVector.set(1, 0, MathTools.clipToMinMax(angularY, 0.0, 1.0)); alphaTaskPriorityVector.set(2, 0, MathTools.clipToMinMax(angularZ, 0.0, 1.0)); }
@Override public FramePose2d getPoseAtS(double pathVariableS) { pathVariableS = MathTools.clipToMinMax(pathVariableS, 0.0, 1.0); return getExtrapolatedPoseAtS(pathVariableS); }
@Override public void variableChanged(YoVariable<?> v) { integralLeakRatio.set(MathTools.clipToMinMax(integralLeakRatio.getDoubleValue(), 0.0, 1.0), false); } };
public void compute(double time) { this.currentTime.set(time); time = MathTools.clipToMinMax(time, 0.0, trajectoryTime.getDoubleValue()); polynomial.compute(time); }
public void interpolate(FrameOrientation orientationOne, FrameOrientation orientationTwo, double alpha) { orientationOne.checkReferenceFrameMatch(orientationTwo); alpha = MathTools.clipToMinMax(alpha, 0.0, 1.0); quaternion.interpolate(orientationOne.quaternion, orientationTwo.quaternion, alpha); referenceFrame = orientationOne.getReferenceFrame(); }
@Override public void compute(double time) { double trajectoryTime = stepTime.getDoubleValue(); isDone.set(time >= trajectoryTime); time = MathTools.clipToMinMax(time, 0.0, trajectoryTime); timeIntoStep.set(time); double percent = time / trajectoryTime; trajectoryMap.get(trajectoryType.getEnumValue()).compute(percent); }
private double predictDesiredHeading(double timeFromNow) { double error = desiredHeadingFinal.getDoubleValue() - desiredHeading.getDoubleValue(); double maximumChange = maxHeadingDot.getDoubleValue() * timeFromNow; double deltaHeading = MathTools.clipToMinMax(error, -maximumChange, maximumChange); return desiredHeading.getDoubleValue() + deltaHeading; } }
private double getBias() { if (useBias.getBooleanValue()) { double biasWalk = biasDelta.getDoubleValue() * ((2.0 * rand.nextDouble()) - 1.0); bias.set(MathTools.clipToMinMax(bias.getDoubleValue() + biasWalk, biasMin.getDoubleValue(), biasMax.getDoubleValue())); return bias.getDoubleValue(); } else return 0.0; }
public void getVelocity(FrameVector velocityToPack) { double parameter = minimumJerkTrajectory.getPosition(); parameter = MathTools.clipToMinMax(parameter, 0.0, 1.0); parabolicTrajectoryGenerator.getVelocity(tempVector, parameter); velocityToPack.setIncludingFrame(tempVector); velocityToPack.scale(minimumJerkTrajectory.getVelocity()); }
public void getVelocity(FrameVector velocityToPack) { double parameter = minimumJerkTrajectory.getPosition(); parameter = MathTools.clipToMinMax(parameter, 0.0, 1.0); parabolicTrajectoryGenerator.getVelocity(tempVector, parameter); velocityToPack.setIncludingFrame(tempVector); velocityToPack.scale(minimumJerkTrajectory.getVelocity()); }
private static double getCosineAngleWithCosineLaw(double l_neighbour1, double l_neighbour2, double l_opposite) { checkIfInRange(l_neighbour1, 0.0, Double.POSITIVE_INFINITY); checkIfInRange(l_neighbour2, 0.0, Double.POSITIVE_INFINITY); checkIfInRange(l_opposite, 0.0, Double.POSITIVE_INFINITY); double cosAngle = MathTools.clipToMinMax((square(l_neighbour1) + square(l_neighbour2) - square(l_opposite)) / (2.0 * l_neighbour1 * l_neighbour2), -1.0, 1.0); return cosAngle; }
private void zoomInternal(double direction) { double newOffset = zoom.get() + direction * zoom.get() * zoomSpeedFactor.get(); newOffset = MathTools.clipToMinMax(newOffset, minZoom.get(), maxZoom.get()); zoom.set(newOffset); }
private void updatePrivilegedConfiguration() { double timeInTrajectory = MathTools.clipToMinMax(getTimeInCurrentState(), 0.0, durationForStanceLegStraightening.getDoubleValue()); kneePrivilegedConfigurationTrajectory.compute(timeInTrajectory); straightLegsPrivilegedConfigurationCommand.clear(); straightLegsPrivilegedConfigurationCommand.addJoint(kneePitch, kneePrivilegedConfigurationTrajectory.getPosition()); straightLegsPrivilegedConfigurationCommand.applyPrivilegedConfigurationToSubChain(pelvis, contactableFoot.getRigidBody()); }
public void getAcceleration(FrameVector accelerationToPack) { double parameter = minimumJerkTrajectory.getPosition(); parameter = MathTools.clipToMinMax(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(FrameVector accelerationToPack) { double parameter = minimumJerkTrajectory.getPosition(); parameter = MathTools.clipToMinMax(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); }