/** * @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()); }
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); }