us.ihmc.robotics.math.filters.AlphaFilteredYoFrameVector java code examples

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

/**
* @deprecated Use
*             {@link #AlphaFilteredYoFrameVector(String, String, YoVariableRegistry, double, ReferenceFrame)}
*             instead.
*/
@Deprecated
public static AlphaFilteredYoFrameVector createAlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha,
                                     ReferenceFrame referenceFrame)
{
 return new AlphaFilteredYoFrameVector(namePrefix, nameSuffix, registry, alpha, referenceFrame);
}

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

private void printFootSensorsOffset()
{
 java.text.NumberFormat doubleFormat = new java.text.DecimalFormat(" 0.00;-0.00");
 String offsetString = "";
 DateFormat dateFormat = new SimpleDateFormat("yyyyMMdd_HHmmss");
 Calendar calendar = Calendar.getInstance();
 String timestamp = dateFormat.format(calendar.getTime());
 offsetString += "Copy the following in ValkyrieSensorInformation:\n";
 for (RobotSide robotSide : RobotSide.values)
 {
   String side = robotSide.getCamelCaseNameForStartOfExpression();
   offsetString += "      SpatialForceVector " + side + "FootForceSensorTareOffset_" + timestamp + " = new SpatialForceVector(null, new double[] {";
   offsetString += doubleFormat.format(footTorquesRawFiltered.get(robotSide).getX()) + ", ";
   offsetString += doubleFormat.format(footTorquesRawFiltered.get(robotSide).getY()) + ", ";
   offsetString += doubleFormat.format(footTorquesRawFiltered.get(robotSide).getZ()) + ", ";
   offsetString += doubleFormat.format(footForcesRawFiltered.get(robotSide).getX()) + ", ";
   offsetString += doubleFormat.format(footForcesRawFiltered.get(robotSide).getY()) + ", ";
   offsetString += doubleFormat.format(footForcesRawFiltered.get(robotSide).getZ()) + "});\n";
 }
 offsetString += "\n      footForceSensorTareOffsets = new SideDependentList<SpatialForceVector>(leftFootForceSensorTareOffset_" + timestamp
    + ", rightFootForceSensorTareOffset_" + timestamp + ");";
 System.out.println(offsetString);
}

private Map<String, Integer> addIMUVectorTypeDataAlphaFilter(DoubleYoVariable alphaFilter, boolean forVizOnly, SensorType sensorType, List<String> sensorsToIgnore)
{
 Map<String, Integer> processorsIDs = new HashMap<>();
 LinkedHashMap<IMUDefinition, YoFrameVector> intermediateIMUVectorTypeSignals = getIntermediateIMUVectorTypeSignals(sensorType);
 LinkedHashMap<IMUDefinition, List<ProcessingYoVariable>> processedIMUVectorTypeSignals = getProcessedIMUVectorTypeSignals(sensorType);
 for (int i = 0; i < imuSensorDefinitions.size(); i++)
 {
   IMUDefinition imuDefinition = imuSensorDefinitions.get(i);
   String imuName = imuDefinition.getName();
   if (sensorsToIgnore.contains(imuName))
    continue;
   YoFrameVector intermediateSignal = intermediateIMUVectorTypeSignals.get(imuDefinition);
   List<ProcessingYoVariable> processors = processedIMUVectorTypeSignals.get(imuDefinition);
   String prefix = sensorType.getProcessorNamePrefix(ALPHA_FILTER);
   int newProcessorID = processors.size();
   processorsIDs.put(imuName, newProcessorID);
   String suffix = sensorType.getProcessorNameSuffix(imuName, newProcessorID);
   AlphaFilteredYoFrameVector filter = AlphaFilteredYoFrameVector.createAlphaFilteredYoFrameVector(prefix, suffix, registry, alphaFilter, intermediateSignal);
   processors.add(filter);
      if (!forVizOnly)
    intermediateIMUVectorTypeSignals.put(imuDefinition, filter);
 }
 return processorsIDs;
}

AlphaFilteredYoFrameVector angularVelocityBias = createAlphaFilteredYoFrameVector("estimated" + sensorName + "AngularVelocityBias", "", registry, biasAlphaFilter, measurementFrame);
angularVelocityBias.update(0.0, 0.0, 0.0);
angularVelocityBiases.add(angularVelocityBias);
AlphaFilteredYoFrameVector linearAccelerationBias = createAlphaFilteredYoFrameVector("estimated" + sensorName + "LinearAccelerationBias", "", registry, biasAlphaFilter, measurementFrame);
linearAccelerationBias.update(0.0, 0.0, 0.0);
linearAccelerationBiases.add(linearAccelerationBias);

YoVariableRegistry registry = new YoVariableRegistry("blop");
AlphaFilteredYoFrameVector filteredVector = new AlphaFilteredYoFrameVector("tested", "", registry, alpha, ReferenceFrame.getWorldFrame());
AlphaFilteredYoVariable xFiltered = new AlphaFilteredYoVariable("xRef", registry, alpha);
AlphaFilteredYoVariable yFiltered = new AlphaFilteredYoVariable("yRef", registry, alpha);
 filteredVector.update(unfilteredVector);
 xFiltered.update(unfilteredVector.getX());
 yFiltered.update(unfilteredVector.getY());

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);
}

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

  public void getFootToPelvisPosition(FramePoint positionToPack, RigidBody foot)
  {
   footToRootJointPositions.get(foot).getFrameTupleIncludingFrame(positionToPack);
  }
}

/**
* Estimates the pelvis position and linear velocity using the leg kinematics
* @param trustedFoot is the foot used to estimates the pelvis state
* @param numberOfTrustedSides is only one or both legs used to estimate the pelvis state
*/
private void updatePelvisWithKinematics(RigidBody trustedFoot, int numberOfTrustedFeet)
{
 double scaleFactor = 1.0 / numberOfTrustedFeet;
 footToRootJointPositions.get(trustedFoot).getFrameTuple(tempPosition);
 tempPosition.scale(scaleFactor);
 rootJointPosition.add(tempPosition);
 footPositionsInWorld.get(trustedFoot).getFrameTuple(tempPosition);
 tempPosition.scale(scaleFactor);
 rootJointPosition.add(tempPosition);
 YoFramePoint rootJointPositionPerFoot = rootJointPositionsPerFoot.get(trustedFoot);
 rootJointPositionPerFoot.set(footPositionsInWorld.get(trustedFoot));
 rootJointPositionPerFoot.add(footToRootJointPositions.get(trustedFoot));
 footVelocitiesInWorld.get(trustedFoot).getFrameTupleIncludingFrame(tempFrameVector);
 tempFrameVector.scale(scaleFactor * alphaRootJointLinearVelocityNewTwist.getDoubleValue());
 rootJointLinearVelocityNewTwist.sub(tempFrameVector);
}

public void update(FrameVector vectorUnfiltered)
{
 checkReferenceFrameMatch(vectorUnfiltered);
 x.update(vectorUnfiltered.getX());
 y.update(vectorUnfiltered.getY());
 z.update(vectorUnfiltered.getZ());
}

private Map<String, Integer> addForceSensorAlphaFilterWithSensorsToIgnore(DoubleYoVariable alphaFilter, boolean forVizOnly, SensorType sensorType, List<String> sensorsToIgnore)
{
 Map<String, Integer> processorsIDs = new HashMap<>();
 LinkedHashMap<ForceSensorDefinition, YoFrameVector> intermediateForceSensorSignals = getIntermediateForceSensorSignals(sensorType);
 LinkedHashMap<ForceSensorDefinition, List<ProcessingYoVariable>> processedForceSensorSignals = getProcessedForceSensorSignals(sensorType);
 for (int i = 0; i < forceSensorDefinitions.size(); i++)
 {
   ForceSensorDefinition forceSensorDefinition = forceSensorDefinitions.get(i);
   String sensorName = forceSensorDefinition.getSensorName();
   if (sensorsToIgnore.contains(sensorName))
    continue;
   YoFrameVector intermediateSignal = intermediateForceSensorSignals.get(forceSensorDefinition);
   List<ProcessingYoVariable> processors = processedForceSensorSignals.get(forceSensorDefinition);
   String prefix = sensorType.getProcessorNamePrefix(ALPHA_FILTER);
   int newProcessorID = processors.size();
   processorsIDs.put(sensorName, newProcessorID);
   String suffix = sensorType.getProcessorNameSuffix(sensorName, newProcessorID);
   AlphaFilteredYoFrameVector filter = AlphaFilteredYoFrameVector.createAlphaFilteredYoFrameVector(prefix, suffix, registry, alphaFilter, intermediateSignal);
   processors.add(filter);
   if (!forVizOnly)
    intermediateForceSensorSignals.put(forceSensorDefinition, filter);
 }
 return processorsIDs;
}

private void printFootSensorsOffset()
{
 java.text.NumberFormat doubleFormat = new java.text.DecimalFormat(" 0.00;-0.00");
 String offsetString = "";
 DateFormat dateFormat = new SimpleDateFormat("yyyyMMdd_HHmmss");
 Calendar calendar = Calendar.getInstance();
 String timestamp = dateFormat.format(calendar.getTime());
 offsetString += "Copy the following in ValkyrieSensorInformation:\n";
 for (RobotSide robotSide : RobotSide.values)
 {
   String side = robotSide.getCamelCaseNameForStartOfExpression();
   offsetString += "      SpatialForceVector " + side + "FootForceSensorTareOffset_" + timestamp + " = new SpatialForceVector(null, new double[] {";
   offsetString += doubleFormat.format(footTorquesRawFiltered.get(robotSide).getX()) + ", ";
   offsetString += doubleFormat.format(footTorquesRawFiltered.get(robotSide).getY()) + ", ";
   offsetString += doubleFormat.format(footTorquesRawFiltered.get(robotSide).getZ()) + ", ";
   offsetString += doubleFormat.format(footForcesRawFiltered.get(robotSide).getX()) + ", ";
   offsetString += doubleFormat.format(footForcesRawFiltered.get(robotSide).getY()) + ", ";
   offsetString += doubleFormat.format(footForcesRawFiltered.get(robotSide).getZ()) + "});\n";
 }
 offsetString += "\n      footForceSensorTareOffsets = new SideDependentList<SpatialForceVector>(leftFootForceSensorTareOffset_" + timestamp
    + ", rightFootForceSensorTareOffset_" + timestamp + ");";
 System.out.println(offsetString);
}

@Override
public void getLinearAccelerationBiasInIMUFrame(IMUSensorReadOnly imu, FrameVector linearAccelerationBiasToPack)
{
 Integer imuIndex = imuToIndexMap.get(imu);
 if (!enableIMUBiasCompensation.getBooleanValue() || imuIndex == null)
   linearAccelerationBiasToPack.setToZero(imu.getMeasurementFrame());
 else
   linearAccelerationBiases.get(imuIndex.intValue()).getFrameTupleIncludingFrame(linearAccelerationBiasToPack);
}

public void getAdjustedDesiredCapturePoint(FramePoint2d desiredCapturePoint, FramePoint2d adjustedDesiredCapturePoint)
{
 filteredYoAngularMomentum.getFrameTuple(angularMomentum);
 ReferenceFrame comFrame = angularMomentum.getReferenceFrame();
 localDesiredCapturePoint.setIncludingFrame(desiredCapturePoint);
 localDesiredCapturePoint.changeFrameAndProjectToXYPlane(comFrame);
 double scaleFactor = momentumGain.getDoubleValue() * omega0.getDoubleValue() / (totalMass.getDoubleValue() * gravity);
 adjustedDesiredCapturePoint.setIncludingFrame(comFrame, -angularMomentum.getY(), angularMomentum.getX());
 adjustedDesiredCapturePoint.scale(scaleFactor);
 adjustedDesiredCapturePoint.add(localDesiredCapturePoint);
 adjustedDesiredCapturePoint.changeFrameAndProjectToXYPlane(desiredCapturePoint.getReferenceFrame());
}

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

AlphaFilteredYoFrameVector footToRootJointPosition = AlphaFilteredYoFrameVector.createAlphaFilteredYoFrameVector(namePrefix + "FootToRootJointPosition", "", registry, alphaFootToRootJointPosition, worldFrame);
footToRootJointPositions.put(foot, footToRootJointPosition);

/**
* @deprecated Use
*             {@link #AlphaFilteredYoFrameVector(String, String, YoVariableRegistry, DoubleProvider, ReferenceFrame)}
*             instead.
*/
@Deprecated
public static AlphaFilteredYoFrameVector createAlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry,
                                     DoubleProvider alpha, ReferenceFrame referenceFrame)
{
 return new AlphaFilteredYoFrameVector(namePrefix, nameSuffix, registry, alpha, referenceFrame);
}

@Override
public void getAngularVelocityBiasInIMUFrame(IMUSensorReadOnly imu, FrameVector angularVelocityBiasToPack)
{
 Integer imuIndex = imuToIndexMap.get(imu);
 if (!enableIMUBiasCompensation.getBooleanValue() || imuIndex == null)
   angularVelocityBiasToPack.setToZero(imu.getMeasurementFrame());
 else
   angularVelocityBiases.get(imuIndex.intValue()).getFrameTupleIncludingFrame(angularVelocityBiasToPack);
}

How to useAlphaFilteredYoFrameVector in us.ihmc.robotics.math.filters

Best Java code snippets using us.ihmc.robotics.math.filters.AlphaFilteredYoFrameVector (Showing top 20 results out of 315)

How to use
AlphaFilteredYoFrameVector
in
us.ihmc.robotics.math.filters