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; }
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; }
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);
AlphaFilteredYoFrameVector footToRootJointPosition = AlphaFilteredYoFrameVector.createAlphaFilteredYoFrameVector(namePrefix + "FootToRootJointPosition", "", registry, alphaFootToRootJointPosition, worldFrame); footToRootJointPositions.put(foot, footToRootJointPosition);
footTorquesRaw.put(robotSide, footTorqueRaw); AlphaFilteredYoFrameVector footForceRawFiltered = AlphaFilteredYoFrameVector.createAlphaFilteredYoFrameVector(sidePrefix + "DiagFootForceRawFilt", "", registry, alphaFootForce, footForceRaw); footForcesRawFiltered.put(robotSide, footForceRawFiltered); AlphaFilteredYoFrameVector footTorqueRawFiltered = AlphaFilteredYoFrameVector.createAlphaFilteredYoFrameVector(sidePrefix + "DiagFootTorqueRawFilt", "", registry, alphaFootForce, footTorqueRaw);
footTorquesRaw.put(robotSide, footTorqueRaw); AlphaFilteredYoFrameVector footForceRawFiltered = AlphaFilteredYoFrameVector.createAlphaFilteredYoFrameVector(sidePrefix + "DiagFootForceRawFilt", "", registry, alphaFootForce, footForceRaw); footForcesRawFiltered.put(robotSide, footForceRawFiltered); AlphaFilteredYoFrameVector footTorqueRawFiltered = AlphaFilteredYoFrameVector.createAlphaFilteredYoFrameVector(sidePrefix + "DiagFootTorqueRawFilt", "", registry, alphaFootForce, footTorqueRaw); footTorquesRawFiltered.put(robotSide, footTorqueRawFiltered);
forceSensorMeasurementFrame); measuredForceAlpha = new DoubleYoVariable(namePrefix + "MeasuredForceAlpha", registry); filteredMeasuredForce = AlphaFilteredYoFrameVector.createAlphaFilteredYoFrameVector(namePrefix + "FilteredMeasuredForce", "", registry, measuredForceAlpha, deadzoneMeasuredForce); filteredMeasuredTorque = AlphaFilteredYoFrameVector.createAlphaFilteredYoFrameVector(namePrefix + "FilteredMeasuredTorque", "", registry, measuredForceAlpha, deadzoneMeasuredTorque);
DoubleYoVariable alpha = new DoubleYoVariable("filteredAngularMomentumAlpha", registry); alpha.set(0.95); // switch to break frequency and move to walking parameters filteredYoAngularMomentum = AlphaFilteredYoFrameVector.createAlphaFilteredYoFrameVector("filteredAngularMomentum", "", registry, alpha, yoAngularMomentum); momentumGain.set(0.0);
filteredAngularVelocityFromError = AlphaFilteredYoFrameVector.createAlphaFilteredYoFrameVector(namePrefix + "FilteredAngularVelocityFromError", "", registry, alphaSpatialVelocityFromError, yoAngularVelocityFromError); filteredLinearVelocityFromError = AlphaFilteredYoFrameVector.createAlphaFilteredYoFrameVector(namePrefix + "FilteredLinearVelocityFromError", "", registry, alphaSpatialVelocityFromError, yoLinearVelocityFromError);