How to use

update

method

in

us.ihmc.robotics.math.filters.AlphaFilteredYoFrameVector

public void update()
{
 alphaFilteredAngularPart.update();
 alphaFilteredLinearPart.update();
}

  public void update(double rawAngularX, double rawAngularY, double rawAngularZ, double rawLinearX, double rawLinearY, double rawLinearZ)
  {
   alphaFilteredAngularPart.update(rawAngularX, rawAngularY, rawAngularZ);
   alphaFilteredLinearPart.update(rawLinearX, rawLinearY, rawLinearZ);
  }
}

public void update(Vector3DReadOnly rawAngularPart, Vector3DReadOnly rawLinearPart)
{
 alphaFilteredAngularPart.update(rawAngularPart);
 alphaFilteredLinearPart.update(rawLinearPart);
}

public void update(FrameVector3DReadOnly rawAngularPart, FrameVector3DReadOnly rawLinearPart)
{
 alphaFilteredAngularPart.update(rawAngularPart);
 alphaFilteredLinearPart.update(rawLinearPart);
}

@Override
public void update()
{
 if (position == null)
 {
   throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null "
      + "position variable to call update(), otherwise use update(double)");
 }
 update(position);
}

public void update(FrameTuple3DReadOnly unfilteredFrameTuple3D)
{
 checkReferenceFrameMatch(unfilteredFrameTuple3D);
 update((Tuple3DReadOnly) unfilteredFrameTuple3D);
}

public void update(Tuple3DReadOnly unfilteredTuple3D)
{
 update(unfilteredTuple3D.getX(), unfilteredTuple3D.getY(), unfilteredTuple3D.getZ());
}

private void computeAngularMomentum()
{
 robotMomentum.setToZero(centerOfMassFrame);
 momentumCalculator.computeAndPack(robotMomentum);
 robotMomentum.getAngularPartIncludingFrame(angularMomentum);
 yoAngularMomentum.set(angularMomentum);
 filteredYoAngularMomentum.update();
}

/**
* Updates the different kinematics related stuff that is used to estimate the pelvis state
*/
public void updateKinematics()
{
 reset();
 updateKinematicsNewTwist();
 twistCalculator.compute();
 for (int i = 0; i < feetRigidBodies.size(); i++)
 {
   RigidBody foot = feetRigidBodies.get(i);
   tempFramePoint.setToZero(rootJointFrame);
   tempFramePoint.changeFrame(soleFrames.get(foot));
   tempFrameVector.setIncludingFrame(tempFramePoint);
   tempFrameVector.changeFrame(worldFrame);
   footToRootJointPositions.get(foot).update(tempFrameVector);
 }
 kinematicsIsUpToDate.set(true);
}

private void computeDesiredSpatialVelocityToSolveFor(DenseMatrix64F spatialDesiredVelocityToPack, DenseMatrix64F spatialVelocityFromError,
   Twist desiredControlFrameTwist)
{
 DenseMatrix64F selectionMatrix = inverseJacobianSolver.getSelectionMatrix();
 // Clip to maximum velocity
 clipSpatialVector(spatialVelocityFromError, selectionMatrix, maximumTaskspaceAngularVelocityMagnitude.getDoubleValue(),
    maximumTaskspaceLinearVelocityMagnitude.getDoubleValue());
 // Update YoVariables for the velocity
 getAngularAndLinearPartsFromSpatialVector(yoAngularVelocityFromError, yoLinearVelocityFromError, spatialVelocityFromError);
 filteredAngularVelocityFromError.update();
 filteredLinearVelocityFromError.update();
 setSpatialVectorFromAngularAndLinearParts(spatialVelocityFromError, filteredAngularVelocityFromError, filteredLinearVelocityFromError);
 desiredControlFrameTwist.getMatrix(spatialDesiredVelocityToPack, 0);
 CommonOps.add(spatialVelocityFromError, spatialDesiredVelocityToPack, spatialDesiredVelocityToPack);
}

angularVelocityBias.update(0.0, 0.0, 0.0);
angularVelocityBiases.add(angularVelocityBias);
linearAccelerationBias.update(0.0, 0.0, 0.0);
linearAccelerationBiases.add(linearAccelerationBias);

angularVelocityBiases.get(imuIndex).update(measurement);
angularVelocityBiases.get(imuIndex).get(measurementBias);
orientationMeasurement.transform(measurementBias);
  linearAccelerationBiases.get(imuIndex).update(measurementMinusGravity);
  linearAccelerationBiases.get(imuIndex).update(measurement);

private void updateWristMeasuredWrench(FrameVector measuredForceToPack, FrameVector measuredTorqueToPack)
{
 momentumBasedController.getWristMeasuredWrenchHandWeightCancelled(measuredWrench, robotSide);
 measuredWrench.getLinearPartIncludingFrame(tempForceVector);
 measuredWrench.getAngularPartIncludingFrame(tempTorqueVector);
 deadzoneMeasuredForce.update(tempForceVector);
 deadzoneMeasuredTorque.update(tempTorqueVector);
 filteredMeasuredForce.update();
 filteredMeasuredTorque.update();
 filteredMeasuredForce.getFrameTupleIncludingFrame(tempForceVector);
 filteredMeasuredTorque.getFrameTupleIncludingFrame(tempTorqueVector);
 measuredWrench.setLinearPart(tempForceVector);
 measuredWrench.setAngularPart(tempTorqueVector);
 measuredWrench.changeFrame(controlFrame);
 measuredWrench.getLinearPartIncludingFrame(measuredForceToPack);
 measuredWrench.getAngularPartIncludingFrame(measuredTorqueToPack);
}

private void updateFootSensorRawMeasurement()
{
 for (RobotSide robotSide : RobotSide.values)
 {
   FootSwitchInterface footSwitch = footSwitches.get(robotSide);
   tempWrench.setToZero(footSwitch.getMeasurementFrame(), footSwitch.getMeasurementFrame());
   tempFrameVector.setToZero(footSwitch.getMeasurementFrame());
   footSwitch.computeAndPackFootWrench(tempWrench);
   tempWrench.getLinearPart(tempFrameVector);
   footForcesRaw.get(robotSide).set(tempFrameVector);
   tempWrench.getAngularPart(tempFrameVector);
   footTorquesRaw.get(robotSide).set(tempFrameVector);
   footForcesRawFiltered.get(robotSide).update();
   footTorquesRawFiltered.get(robotSide).update();
 }
}

private void updateFootSensorRawMeasurement()
{
 for (RobotSide robotSide : RobotSide.values)
 {
   FootSwitchInterface footSwitch = footSwitches.get(robotSide);
   tempWrench.setToZero(footSwitch.getMeasurementFrame(), footSwitch.getMeasurementFrame());
   tempFrameVector.setToZero(footSwitch.getMeasurementFrame());
   footSwitch.computeAndPackFootWrench(tempWrench);
   tempFrameVector.set(tempWrench.getLinearPart());
   footForcesRaw.get(robotSide).set(tempFrameVector);
   tempFrameVector.set(tempWrench.getAngularPart());
   footTorquesRaw.get(robotSide).set(tempFrameVector);
   footForcesRawFiltered.get(robotSide).update();
   footTorquesRawFiltered.get(robotSide).update();
 }
}

filteredVector.update(unfilteredVector);
xFiltered.update(unfilteredVector.getX());
yFiltered.update(unfilteredVector.getY());