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(Quat4d 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); }
public void update(FrameOrientation currentOrientation) { checkReferenceFrameMatch(currentOrientation); currentOrientation.getMatrix3d(currentOrientationMatrix); update(currentOrientationMatrix); }
public void update(FrameQuaternionReadOnly currentOrientation) { checkReferenceFrameMatch(currentOrientation); currentOrientationMatrix.set(currentOrientation); update(currentOrientationMatrix); }
private void updateViz() { yoRootJointFrameQuaternion.checkReferenceFrameMatch(worldFrame); yoRootJointFrameQuaternion.set(rotationFromRootJointFrameToWorld); yoRootJointAngularVelocityFromFD.update(); yoRootJointFrameOrientation.checkReferenceFrameMatch(worldFrame); yoRootJointFrameOrientation.set(rotationFromRootJointFrameToWorld); }
@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(); }
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);
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);