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; }
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 double convertStandardDeviationToDiscreteTime(double continuousStdDev) { double continuousVariance = MathTools.square(continuousStdDev); double discreteVariance = continuousVariance * controlDT; double discreteStdDev = Math.sqrt(discreteVariance); return discreteStdDev; }
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 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 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 void getAcceleration(FrameVector accelerationToPack, ReferenceFrame frame, double angle, double angleDot, double angleDDot, double radius, double radiusDot, double radiusDDot, double zDDot) { double cos = Math.cos(angle); double sin = Math.sin(angle); accelerationToPack.setToZero(frame); double xDDot = -cos * radius * MathTools.square(angleDot) - 2.0 * sin * angleDot * radiusDot - radius * sin * angleDDot + cos * radiusDDot; double yDDot = -sin * radius * MathTools.square(angleDot) + 2.0 * cos * angleDot * radiusDot + radius * cos * angleDDot + sin * radiusDDot; accelerationToPack.setX(xDDot); accelerationToPack.setY(yDDot); accelerationToPack.setZ(zDDot); }
public void setProcessNoiseStandardDeviation(double standardDeviation) { double variance = MathTools.square(standardDeviation); CommonOps.setIdentity(processNoiseCovarianceBlock); CommonOps.scale(variance, processNoiseCovarianceBlock); }
public void update(double input) { int nUpdatesOld = nUpdates.getIntegerValue(); nUpdates.increment(); int nUpdatesNew = nUpdates.getIntegerValue(); double ratio = ((double) nUpdatesOld) / ((double) nUpdatesNew); rms.set(Math.sqrt(square(rms.getDoubleValue()) * ratio + square(input) / nUpdatesNew)); }
public static double getAngularVelocity(FramePoint position, FrameVector velocity) { position.checkReferenceFrameMatch(velocity); double x = position.getX(); double y = position.getY(); double xd = velocity.getX(); double yd = velocity.getY(); double radiusSquared = MathTools.square(x) + MathTools.square(y); return (x * yd - y * xd) / radiusSquared; } }
private static DenseMatrix64F createDiagonalCovarianceMatrix(double standardDeviation, int size) { DenseMatrix64F orientationCovarianceMatrix = new DenseMatrix64F(size, size); CommonOps.setIdentity(orientationCovarianceMatrix); CommonOps.scale(MathTools.square(standardDeviation), orientationCovarianceMatrix); return orientationCovarianceMatrix; }
private static DenseMatrix64F createDiagonalCovarianceMatrix(double standardDeviation, int size) { DenseMatrix64F orientationCovarianceMatrix = new DenseMatrix64F(size, size); CommonOps.setIdentity(orientationCovarianceMatrix); CommonOps.scale(MathTools.square(standardDeviation), orientationCovarianceMatrix); return orientationCovarianceMatrix; }
private static DenseMatrix64F createDiagonalCovarianceMatrix(double standardDeviation) { DenseMatrix64F orientationCovarianceMatrix = new DenseMatrix64F(3, 3); CommonOps.setIdentity(orientationCovarianceMatrix); CommonOps.scale(MathTools.square(standardDeviation), orientationCovarianceMatrix); return orientationCovarianceMatrix; } }
public boolean isTransitionToSingleSupportSafe(RobotSide transferToSide) { getICPError(icpError2d); ReferenceFrame leadingAnkleZUpFrame = bipedSupportPolygons.getAnkleZUpFrames().get(transferToSide); icpError2d.changeFrame(leadingAnkleZUpFrame); double ellipticErrorSquared = MathTools.square(icpError2d.getX() / maxICPErrorBeforeSingleSupportX.getDoubleValue()) + MathTools.square(icpError2d.getY() / maxICPErrorBeforeSingleSupportY.getDoubleValue()); boolean closeEnough = ellipticErrorSquared < 1.0; return closeEnough; }
public void computeQDDotByFiniteDifferenceCentral(Quat4d qPrevious, Quat4d q, Quat4d qNext, double dt, Quat4d qDDotToPack) { qDDotToPack.set(qNext); qDDotToPack.sub(q); qDDotToPack.sub(q); qDDotToPack.add(qPrevious); qDDotToPack.scale(1.0 / MathTools.square(dt)); }
public FrameVector2d computeDesiredMomentumXY(FramePoint2d desiredCoMXY, MutableDouble errorMagnitude) { FrameVector2d ret = new FrameVector2d(); FramePoint2d errorCoMXY = new FramePoint2d(desiredCoMXY); errorCoMXY.changeFrame(referenceFrames.getCenterOfMassFrame()); errorMagnitude.setValue(MathTools.square(errorCoMXY.getX()) + MathTools.square(errorCoMXY.getY())); errorMagnitude.setValue(Math.sqrt(errorMagnitude.doubleValue())); errorCoMXY.scale(1.0 / updateDT); ret.setIncludingFrame(errorCoMXY); ret.scale(toolbox.getTotalRobotMass()); return ret; }
public FrameVector getCurrentDesiredAcceleration(double timeInMove) { FramePoint Xfh = getCurrentDesiredPoint(timeInMove + stepSizeforNumericalCalculation); FramePoint X = getCurrentDesiredPoint(timeInMove); FramePoint Xrh = getCurrentDesiredPoint(timeInMove - stepSizeforNumericalCalculation); FrameVector ret = new FrameVector(X.getReferenceFrame()); ret.setX((Xfh.getX() - 2.0 * X.getX() + Xrh.getX()) / (MathTools.square(stepSizeforNumericalCalculation))); ret.setY((Xfh.getY() - 2.0 * X.getY() + Xrh.getY()) / (MathTools.square(stepSizeforNumericalCalculation))); ret.setZ((Xfh.getZ() - 2.0 * X.getZ() + Xrh.getZ()) / (MathTools.square(stepSizeforNumericalCalculation))); return ret; }
/** * Division of ComplexNumbers (doesn't change this ComplexNumber). * <br>(x+i*y)/(s+i*t) = ((x*s+y*t) + i*(y*s-y*t)) / (s^2+t^2) * @param w is the number to divide by * @return new ComplexNumber z/w where z is this ComplexNumber */ public ComplexNumber dividedBy(ComplexNumber w) { double den = MathTools.square(w.magnitude()); return new ComplexNumber((real * w.real() + imag * w.imag()) / den, (imag * w.real() - real * w.imag()) / den); }
public static void computePseudoCMP3d(FramePoint pseudoCMP3dToPack, FramePoint centerOfMass, FramePoint2d cmp, double fZ, double totalMass, double omega0) { double zCMP = centerOfMass.getZ() - fZ / (totalMass * MathTools.square(omega0)); pseudoCMP3dToPack.setIncludingFrame(cmp.getReferenceFrame(), cmp.getX(), cmp.getY(), 0.0); pseudoCMP3dToPack.changeFrame(centerOfMass.getReferenceFrame()); pseudoCMP3dToPack.setZ(zCMP); }