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; }
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 CVXMomentumOptimizerWithGRFPenalizedSmootherNative(int nDoF, int rhoSize) { MathTools.checkIfInRange(nDoF, 0, CVXMomentumOptimizerWithGRFPenalizedSmootherNative.nDoF); MathTools.checkIfInRange(rhoSize, 0, CVXMomentumOptimizerWithGRFPenalizedSmootherNative.rhoSize); cvxMomentumOptimizerWithGRFPenalizedSmootherNativeOutput = new CVXMomentumOptimizerWithGRFPenalizedSmootherNativeOutput(nDoF, rhoSize); }
public static double getAngleDotWithCosineLaw(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 cosAngle = getCosineAngleWithCosineLaw(l_neighbour1, l_neighbour2, l_opposite); double cosAngleDot = getCosineAngleDotWithCosineLaw(l_neighbour1, l_neighbour2, lDot_neighbour2, l_opposite, lDot_opposite); double angleDot = -cosAngleDot / sqrt(1 - cosAngle * cosAngle); return angleDot; }
public void setTrajectoryTime(double duration) { MathTools.checkIfInRange(duration, 0.0, Double.POSITIVE_INFINITY); trajectoryTime.set(duration); }
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 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; }
@Override public void getTauMatrix(DenseMatrix64F matrix) { MathTools.checkIfInRange(matrix.getNumRows(), 1, Integer.MAX_VALUE); MathTools.checkIfInRange(matrix.getNumCols(), 1, Integer.MAX_VALUE); matrix.set(0, 0, tau); }
/** * Torque on a passive joint is always zero */ @Override public void getTauMatrix(DenseMatrix64F matrix) { MathTools.checkIfInRange(matrix.getNumRows(), 1, Integer.MAX_VALUE); MathTools.checkIfInRange(matrix.getNumCols(), 1, Integer.MAX_VALUE); matrix.set(0, 0, 0); }
@Override public void getVelocityMatrix(DenseMatrix64F matrix, int rowStart) { MathTools.checkIfInRange(matrix.getNumRows(), 1, Integer.MAX_VALUE); MathTools.checkIfInRange(matrix.getNumCols(), 1, Integer.MAX_VALUE); matrix.set(rowStart, 0, qd); }
public void getPose(FrameVector translationFromVoxelOrigin, FrameOrientation orientation, int rayIndex, int rotationAroundRayIndex) { MathTools.checkIfInRange(rayIndex, 0, numberOfRays - 1); MathTools.checkIfInRange(rotationAroundRayIndex, 0, numberOfRotationsAroundRay - 1); if (type == SphereVoxelType.graspAroundSphere) translationFromVoxelOrigin.setIncludingFrame(parentFrame, pointsOnSphere[rayIndex]); else translationFromVoxelOrigin.setToZero(parentFrame); orientation.setIncludingFrame(parentFrame, rotations[rayIndex][rotationAroundRayIndex]); }
public ProviderBasedConstantOrientationTrajectoryGenerator(String namePrefix, ReferenceFrame referenceFrame, OrientationProvider orientationProvider, double finalTime, YoVariableRegistry parentRegistry) { MathTools.checkIfInRange(finalTime, 0.0, Double.POSITIVE_INFINITY); this.registry = new YoVariableRegistry(namePrefix + getClass().getSimpleName()); this.referenceFrame = referenceFrame; this.orientationProvider = orientationProvider; this.finalTime = new DoubleYoVariable("finalTime", registry); this.time = new DoubleYoVariable("time", registry); this.finalTime.set(finalTime); parentRegistry.addChild(registry); }
public ConstantOrientationTrajectoryGenerator(String namePrefix, ReferenceFrame referenceFrame, OrientationProvider orientationProvider, double finalTime, YoVariableRegistry parentRegistry) { MathTools.checkIfInRange(finalTime, 0.0, Double.POSITIVE_INFINITY); this.registry = new YoVariableRegistry(namePrefix + getClass().getSimpleName()); this.orientationProvider = orientationProvider; this.orientation = new YoFrameQuaternion("orientation", referenceFrame, registry); this.finalTime = new DoubleYoVariable("finalTime", registry); this.time = new DoubleYoVariable("time", registry); this.finalTime.set(finalTime); parentRegistry.addChild(registry); }
public ConstantForceTrajectoryGenerator(String namePrefix, double force, double finalTime, YoVariableRegistry parentRegistry) { MathTools.checkIfInRange(finalTime, 0.0, Double.POSITIVE_INFINITY); this.registry = new YoVariableRegistry(namePrefix + getClass().getSimpleName()); this.force = new DoubleYoVariable("force", registry); this.finalTime = new DoubleYoVariable("finalTime", registry); this.time = new DoubleYoVariable("time", registry); this.force.set(force); this.finalTime.set(finalTime); parentRegistry.addChild(registry); }
public void initialize() { MathTools.checkIfInRange(trajectoryTime.getDoubleValue(), 0.0, Double.POSITIVE_INFINITY); quinticParameterPolynomial.setQuintic(0.0, trajectoryTime.getDoubleValue(), 0.0, 0.0, 0.0, 1.0, 0.0, 0.0); reset(); if (visualize) visualizeTrajectory(); }
public void getVelocity(FrameVector velocityToPack, double parameter) { double q = parameter; MathTools.checkIfInRange(q, 0.0, 1.0); velocityToPack.setToZero(referenceFrame); // 2 * c2 * q c2.getFrameTuple(velocityToPack); velocityToPack.scale(2.0 * q); // c1 c1.getFrameTuple(tempPackVelocity); velocityToPack.add(tempPackVelocity); }
public void setQuadraticUsingIntermediatePoint(double t0, double tIntermediate, double tFinal, double z0, double zIntermediate, double zFinal) { reshape(3); MathTools.checkIfInRange(tIntermediate, t0, tFinal); setPositionRow(0, t0, z0); setPositionRow(1, tIntermediate, zIntermediate); setPositionRow(2, tFinal, zFinal); solveForCoefficients(); setYoVariables(); }
@Override public void initialize() { currentTime.set(0.0); MathTools.checkIfInRange(trajectoryTime.getDoubleValue(), 0.0, Double.POSITIVE_INFINITY); parameterPolynomial.setQuintic(0.0, trajectoryTime.getDoubleValue(), 0.0, 0.0, 0.0, 1.0, 0.0, 0.0); currentOrientation.set(initialOrientation); currentAngularVelocity.setToZero(); currentAngularAcceleration.setToZero(); }
public void initialize(FramePoint initialPosition, FramePoint finalPosition, double heightAtParameter, double parameter) { double q = parameter; MathTools.checkIfInRange(q, 0.0, 1.0); initialPosition.changeFrame(referenceFrame); finalPosition.changeFrame(referenceFrame); FramePoint intermediatePosition = new FramePoint(referenceFrame); intermediatePosition.setX(initialPosition.getX() + q * (finalPosition.getX() - initialPosition.getX())); intermediatePosition.setY(initialPosition.getY() + q * (finalPosition.getY() - initialPosition.getY())); intermediatePosition.setZ(heightAtParameter); initialize(initialPosition, intermediatePosition, finalPosition, q); }