public void update() { if (orientation == null) { throw new NullPointerException("FiniteDifferenceAngularVelocityYoFrameVector must be constructed with a non null " + "orientation variable to call update(), otherwise use update(FrameOrientation)"); } orientation.get(currentOrientationMatrix); update(currentOrientationMatrix); }
public void update(FrameOrientation currentOrientation) { checkReferenceFrameMatch(currentOrientation); currentOrientation.getMatrix3d(currentOrientationMatrix); update(currentOrientationMatrix); }
rateLimitedQuaternion.update(new Quaternion()); FiniteDifferenceAngularVelocityYoFrameVector angularVelocity = new FiniteDifferenceAngularVelocityYoFrameVector("rate", rateLimitedQuaternion, dt, registry); int numberOfIterations = (int) (timeToConverge / dt); double previousDistance = distanceToGoal; angularVelocity.update(); double distance = Math.abs(AngleTools.trimAngleMinusPiToPi(rateLimitedQuaternion.distance(goalQuaternion))); assertTrue(distance < previousDistance); angularVelocity.update(); double rate = angularVelocity.length(); assertEquals("difference: " + Math.abs(rate - maxRate.doubleValue()), rate, maxRate.doubleValue(), EPSILON); previousDistance = distance;
@Override public void update() { localVelocityFromFD.update(); localVelocityFromFD.getFrameTupleIncludingFrame(tempAngularVelocity); tempAngularVelocity.changeFrame(referenceFrameUsedForComparison); localVelocityFiltered.update(tempAngularVelocity); angularVelocityToCheck.getFrameTupleIncludingFrame(tempAngularVelocity); tempAngularVelocity.changeFrame(referenceFrameUsedForComparison); filteredVelocityToCheck.update(tempAngularVelocity); if (!localVelocityFiltered.getHasBufferWindowFilled()) return; for (Direction direction : Direction.values) delayEstimators.get(direction).update(); }
public void update(Matrix3d rotationMatrix) { if (!hasBeenCalled.getBooleanValue()) { orientationPreviousValue.set(rotationMatrix); hasBeenCalled.set(true); } if (rotationMatrix != currentOrientationMatrix) currentOrientationMatrix.set(rotationMatrix); orientationPreviousValue.get(previousOrientationMatrix); deltaOrientationMatrix.mulTransposeRight(currentOrientationMatrix, previousOrientationMatrix); deltaAxisAngle.set(deltaOrientationMatrix); set(deltaAxisAngle.getX(), deltaAxisAngle.getY(), deltaAxisAngle.getZ()); scale(deltaAxisAngle.getAngle() / dt); orientationPreviousValue.set(currentOrientationMatrix); } }
public OrientationAngularVelocityConsistencyChecker(String namePrefix, YoFrameQuaternion orientation, YoFrameVector angularVelocityToCheck, ReferenceFrame referenceFrameUsedForComparison, double updateDT, YoVariableRegistry parentRegistry) { registry = new YoVariableRegistry(namePrefix + "OrientationVelocityCheck"); this.referenceFrameUsedForComparison = referenceFrameUsedForComparison; localVelocityFromFD = new FiniteDifferenceAngularVelocityYoFrameVector(namePrefix + "referenceFD", orientation, updateDT, registry); this.angularVelocityToCheck = angularVelocityToCheck; int windowSize = 10; localVelocityFiltered = createSimpleMovingAverageFilteredYoFrameVector(namePrefix, "_referenceFiltered", windowSize, referenceFrameUsedForComparison, registry); filteredVelocityToCheck = createSimpleMovingAverageFilteredYoFrameVector(namePrefix, "_filtered", windowSize, referenceFrameUsedForComparison, registry); DelayEstimatorBetweenTwoSignals xVelocityDelayEstimator = new DelayEstimatorBetweenTwoSignals(namePrefix + "WX", localVelocityFiltered.getYoX(), filteredVelocityToCheck.getYoX(), updateDT, registry); DelayEstimatorBetweenTwoSignals yVelocityDelayEstimator = new DelayEstimatorBetweenTwoSignals(namePrefix + "WY", localVelocityFiltered.getYoY(), filteredVelocityToCheck.getYoY(), updateDT, registry); DelayEstimatorBetweenTwoSignals zVelocityDelayEstimator = new DelayEstimatorBetweenTwoSignals(namePrefix + "WZ", localVelocityFiltered.getYoZ(), filteredVelocityToCheck.getYoZ(), updateDT, registry); delayEstimators.put(Direction.X, xVelocityDelayEstimator); delayEstimators.put(Direction.Y, yVelocityDelayEstimator); delayEstimators.put(Direction.Z, zVelocityDelayEstimator); parentRegistry.addChild(registry); }
rateLimitedOrientation.update(new Quaternion()); FiniteDifferenceAngularVelocityYoFrameVector angularVelocity = new FiniteDifferenceAngularVelocityYoFrameVector("rate", ReferenceFrame.getWorldFrame(), dt, registry); int numberOfIterations = (int) (timeToConverge / dt); double previousDistance = distanceToGoal; angularVelocity.update(rateLimitedOrientation.getFrameOrientation()); double distance = Math.abs(AngleTools.trimAngleMinusPiToPi(rateLimitedOrientation.getFrameOrientation().distance(goalQuaternion))); assertTrue(distance < previousDistance); angularVelocity.update(rateLimitedOrientation.getFrameOrientation()); double rate = angularVelocity.length(); assertEquals("difference: " + Math.abs(rate - maxRate.doubleValue()), rate, maxRate.doubleValue(), EPSILON); previousDistance = distance;
public void update(RotationMatrixReadOnly rotationMatrix) { if (!hasBeenCalled.getBooleanValue()) { orientationPreviousValue.set(rotationMatrix); hasBeenCalled.set(true); } if (rotationMatrix != currentOrientationMatrix) currentOrientationMatrix.set(rotationMatrix); previousOrientationMatrix.set(orientationPreviousValue); deltaOrientationMatrix.set(currentOrientationMatrix); deltaOrientationMatrix.multiplyTransposeOther(previousOrientationMatrix); deltaAxisAngle.set(deltaOrientationMatrix); set(deltaAxisAngle.getX(), deltaAxisAngle.getY(), deltaAxisAngle.getZ()); scale(deltaAxisAngle.getAngle() / dt); orientationPreviousValue.set(currentOrientationMatrix); } }
public IMUBasedPelvisRotationalStateUpdater(FullInverseDynamicsStructure inverseDynamicsStructure, List<? extends IMUSensorReadOnly> imuProcessedOutputs, IMUBiasProvider imuBiasProvider, IMUYawDriftEstimator imuYawDriftEstimator, double dt, YoVariableRegistry parentRegistry) { this.imuBiasProvider = imuBiasProvider; this.imuYawDriftEstimator = imuYawDriftEstimator; checkNumberOfSensors(imuProcessedOutputs); imuProcessedOutput = imuProcessedOutputs.get(0); rootJoint = inverseDynamicsStructure.getRootJoint(); rootJointFrame = rootJoint.getFrameAfterJoint(); twistCalculator = inverseDynamicsStructure.getTwistCalculator(); measurementFrame = imuProcessedOutput.getMeasurementFrame(); measurementLink = imuProcessedOutput.getMeasurementLink(); yoRootJointFrameOrientation = new YoFrameOrientation("estimatedRootJoint", worldFrame, registry); yoRootJointFrameQuaternion = new YoFrameQuaternion("estimatedRootJoint", worldFrame, registry); rootJointYawOffsetFromFrozenState = new DoubleYoVariable("rootJointYawOffsetFromFrozenState", registry); yoRootJointAngularVelocity = new YoFrameVector("estimatedRootJointAngularVelocity", rootJointFrame, registry); yoRootJointAngularVelocityInWorld = new YoFrameVector("estimatedRootJointAngularVelocityWorld", worldFrame, registry); yoRootJointAngularVelocityMeasFrame = new YoFrameVector("estimatedRootJointAngularVelocityMeasFrame", measurementFrame, registry); yoRootJointAngularVelocityFromFD = new FiniteDifferenceAngularVelocityYoFrameVector("estimatedRootJointAngularVelocityFromFD", yoRootJointFrameQuaternion, dt, registry); parentRegistry.addChild(registry); angularVelocityRootJointFrameRelativeToWorld = new FrameVector(rootJointFrame); }
public void update(Quat4d currentOrientation) { currentOrientationMatrix.set(currentOrientation); update(currentOrientationMatrix); }
public void update(FrameQuaternionReadOnly currentOrientation) { checkReferenceFrameMatch(currentOrientation); currentOrientationMatrix.set(currentOrientation); update(currentOrientationMatrix); }
public void update(AxisAngle4d currentOrientation) { currentOrientationMatrix.set(currentOrientation); update(currentOrientationMatrix); }
public void update() { if (orientation == null) { throw new NullPointerException("FiniteDifferenceAngularVelocityYoFrameVector must be constructed with a non null " + "orientation variable to call update(), otherwise use update(FrameOrientation)"); } currentOrientationMatrix.set(orientation); update(currentOrientationMatrix); }
public void update(QuaternionReadOnly currentOrientation) { currentOrientationMatrix.set(currentOrientation); update(currentOrientationMatrix); }
public void update(AxisAngleReadOnly currentOrientation) { currentOrientationMatrix.set(currentOrientation); update(currentOrientationMatrix); }
private void updateViz() { yoRootJointFrameQuaternion.checkReferenceFrameMatch(worldFrame); yoRootJointFrameQuaternion.set(rotationFromRootJointFrameToWorld); yoRootJointAngularVelocityFromFD.update(); yoRootJointFrameOrientation.checkReferenceFrameMatch(worldFrame); yoRootJointFrameOrientation.set(rotationFromRootJointFrameToWorld); }