public HandCollisionDetectedPacket(RobotSide robotSide, int collisionSeverityLevelZeroToThree) { this.robotSide = robotSide; this.collisionSeverityLevelOneToThree = MathTools.clipToMinMax(collisionSeverityLevelZeroToThree, 1, 3); }
@Override public boolean epsilonEquals(Sphere3d other, double epsilon) { return MathTools.epsilonEquals(radius, other.radius, epsilon); }
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; }
public static double getCosineAngleDotWithCosineLaw(double l_neighbour1, double l_neighbour2, double lDot_neighbour2, double l_opposite, double lDot_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 cosAngleDot = (square(l_neighbour2) * lDot_neighbour2 - 2.0 * l_neighbour2 * l_opposite * lDot_opposite - lDot_neighbour2 * square(l_neighbour1) + lDot_neighbour2 * square(l_opposite)) / (2.0 * square(l_neighbour2) * l_neighbour1); return cosAngleDot; }
public static double getCosineAngleDDotWithCosineLaw(double l_neighbour1, double l_neighbour2, double lDot_neighbour2, double lDDot_neighbour2, double l_opposite, double lDot_opposite, double lDDot_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 cosAngleDDot = (cube(l_neighbour2) * lDDot_neighbour2 - 2 * square(l_neighbour2 * lDot_opposite) - 2 * square(l_neighbour2) * l_opposite * lDDot_opposite + 4 * l_neighbour2 * lDot_neighbour2 * l_opposite * lDot_opposite + 2 * square(lDot_neighbour2 * l_neighbour1) - 2 * square( lDot_neighbour2 * l_opposite) - l_neighbour2 * lDDot_neighbour2 * square(l_neighbour1) + l_neighbour2 * lDDot_neighbour2 * square(l_opposite)) / (2.0 * cube(l_neighbour2) * l_neighbour1); return cosAngleDDot; }
private void computeOther(double value) { k++; sum = sum + value; double mKLast = mK; double kDouble = (double)k; mK = mKLast + (value - mKLast) / kDouble; qK = qK + (kDouble - 1.0) * MathTools.square(value - mKLast) / kDouble; }
public void enableWeightMethod(double maxWeight, double minWeight) { MathTools.checkIfInRange(minWeight, 1.0-3, 100.0); MathTools.checkIfInRange(maxWeight, 1.0-3, 100.0); this.useWeightMethod = true; double weightRatio = maxWeight / minWeight; this.minWeight = 1.0; this.maxWeight = weightRatio; }
public void addArmTrajectoryPoint(double trajectoryPointTime, double[] desiredJointPositions, double[] desiredJointVelocities) { MathTools.checkIfEqual(desiredJointPositions.length, desiredJointVelocities.length); for (int jointIndex = 0; jointIndex < desiredJointPositions.length; jointIndex++) { SimpleTrajectoryPoint1DList jointTrajectoryInput = jointTrajectoryInputs.getAndGrowIfNeeded(jointIndex); jointTrajectoryInput.addTrajectoryPoint(trajectoryPointTime, desiredJointPositions[jointIndex], desiredJointVelocities[jointIndex]); } }
public static void floorToGivenPrecision(Tuple3d tuple3d, double precision) { tuple3d.setX(floorToGivenPrecision(tuple3d.getX(), precision)); tuple3d.setY(floorToGivenPrecision(tuple3d.getY(), precision)); tuple3d.setZ(floorToGivenPrecision(tuple3d.getZ(), precision)); }
private double findDoubleSupportHeight(double distanceFromFoot, double s0, double sF, double s_d0, double foot0Height, double foot1Height) { double z_d0_A = foot0Height + Math.sqrt(MathTools.square(distanceFromFoot) - MathTools.square(s_d0 - s0)); double z_d0_B = foot1Height + Math.sqrt(MathTools.square(distanceFromFoot) - MathTools.square((sF - s_d0))); double z_d0 = Math.min(z_d0_A, z_d0_B); return z_d0; }
private static LinkedHashMap<ProcessedVelocitySensor, Double> createSensorsAndCoefficientsMap(ProcessedVelocitySensor sensor1, ProcessedVelocitySensor sensor2, double weight1, double weight2) { MathTools.checkIfInRange(weight1, 0.0, Double.POSITIVE_INFINITY); MathTools.checkIfInRange(weight2, 0.0, Double.POSITIVE_INFINITY); LinkedHashMap<ProcessedVelocitySensor, Double> ret = new LinkedHashMap<ProcessedVelocitySensor, Double>(); ret.put(sensor1, weight1); ret.put(sensor2, -weight2); return ret; }
public void addNeckTrajectoryPoint(double trajectoryPointTime, double[] desiredJointPositions, double[] desiredJointVelocities) { MathTools.checkIfEqual(desiredJointPositions.length, desiredJointVelocities.length); for (int jointIndex = 0; jointIndex < desiredJointPositions.length; jointIndex++) { SimpleTrajectoryPoint1DList jointTrajectoryInput = jointTrajectoryInputs.getAndGrowIfNeeded(jointIndex); jointTrajectoryInput.addTrajectoryPoint(trajectoryPointTime, desiredJointPositions[jointIndex], desiredJointVelocities[jointIndex]); } }
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 boolean epsilonEquals(LegCompliancePacket other, double epsilon) { if (maxVelocityDeltas.length != other.maxVelocityDeltas.length) return false; for (int i = 0; i < maxVelocityDeltas.length; i++) if (!MathTools.epsilonEquals(maxVelocityDeltas[i], other.maxVelocityDeltas[i], epsilon)) return false; return true; }
public 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 double convertStandardDeviationToDiscreteTime(double continuousStdDev) { double continuousVariance = MathTools.square(continuousStdDev); double discreteVariance = continuousVariance * controlDT; double discreteStdDev = Math.sqrt(discreteVariance); return discreteStdDev; }
public CVXMomentumOptimizerWithGRFPenalizedSmootherNative(int nDoF, int rhoSize) { MathTools.checkIfInRange(nDoF, 0, CVXMomentumOptimizerWithGRFPenalizedSmootherNative.nDoF); MathTools.checkIfInRange(rhoSize, 0, CVXMomentumOptimizerWithGRFPenalizedSmootherNative.rhoSize); cvxMomentumOptimizerWithGRFPenalizedSmootherNativeOutput = new CVXMomentumOptimizerWithGRFPenalizedSmootherNativeOutput(nDoF, rhoSize); }
public void set(ReferenceFrame expressedInFrame, double[] doubleArray) { MathTools.checkIfEqual(doubleArray.length, SIZE); this.expressedInFrame = expressedInFrame; this.angularPart.set(doubleArray[0], doubleArray[1], doubleArray[2]); this.linearPart.set(doubleArray[3], doubleArray[4], doubleArray[5]); }
public void interpolate(Quat4d quaternion1, Quat4d quaternion2, double alpha) { alpha = MathTools.clipToMinMax(alpha, 0.0, 1.0); quaternion.interpolate(quaternion1, quaternion2, alpha); }
@Override public boolean epsilonEquals(PelvisPoseErrorPacket other, double epsilon) { return (MathTools.epsilonEquals(residualError, other.residualError, epsilon) && MathTools.epsilonEquals(totalError, other.totalError, epsilon) && other.hasMapBeenReset == hasMapBeenReset); }