Integrate Invariant State Estimator into Framework

ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/AvatarEstimatorThread.java

@@ -186,6 +186,17 @@ public FullHumanoidRobotModel getFullRobotModel()
       return estimatorFullRobotModel;
    }
 
+   /**
+    * @return the main state estimator controller driving the root joint (e.g. the DRC estimator, or
+    *         {@code InvariantMainStateEstimator} when the invariant InEKF has been promoted to main).
+    *         Exposed so external comparators (e.g. against simulation ground truth) can reach the
+    *         estimator regardless of which implementation owns the root. May be {@code null}.
+    */
+   public StateEstimatorController getMainStateEstimator()
+   {
+      return mainStateEstimator;
+   }
+
    @Override
    public YoRegistry getYoRegistry()
    {

ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/scs2/SCS2AvatarSimulationFactory.java

@@ -102,6 +102,7 @@
 import us.ihmc.simulationToolkit.RobotDefinitionTools;
 import us.ihmc.simulationconstructionset.dataBuffer.MirroredYoVariableRegistry;
 import us.ihmc.stateEstimation.humanoid.StateEstimatorControllerFactory;
+import us.ihmc.stateEstimation.invariant_estimator.InvariantContactSource;
 import us.ihmc.tools.factories.FactoryFieldNotSetException;
 import us.ihmc.tools.factories.FactoryTools;
 import us.ihmc.tools.factories.OptionalFactoryField;
@@ -201,6 +202,11 @@ public class SCS2AvatarSimulationFactory
          "headingAndVelocityEvaluationScriptParameters");
    private final OptionalFactoryField<StateEstimatorControllerFactory> secondaryStateEstimatorFactory = new OptionalFactoryField<>(
          "SecondaryStateEstimatorFactory");
+   /** When true, the invariant InEKF replaces the DRC estimator as the main floating-base estimator. */
+   private boolean useInvariantMainStateEstimator = false;
+   private boolean invariantMainEstimatorYawSeeding = true;
+   /** Contact source for the invariant main estimator; forwarded to the estimator thread factory (shared by sim and hardware). */
+   private InvariantContactSource invariantMainEstimatorContactSource = InvariantContactSource.FOOT_SWITCHES;
    private final OptionalFactoryField<Boolean> createIKStreamingRealTimeController = new OptionalFactoryField<>(
          "createIKStreamingRealTimeController",
          false);
@@ -546,6 +552,12 @@ private void setupStateEstimationThread()
       avatarEstimatorThreadFactory.setGravity(gravity.get());
       if (secondaryStateEstimatorFactory.hasBeenSet())
          avatarEstimatorThreadFactory.addSecondaryStateEstimatorFactory(secondaryStateEstimatorFactory.get());
+      if (useInvariantMainStateEstimator)
+      {
+         avatarEstimatorThreadFactory.setUseInvariantStateEstimator(true);
+         avatarEstimatorThreadFactory.setInvariantEstimatorYawSeeding(invariantMainEstimatorYawSeeding);
+         avatarEstimatorThreadFactory.setInvariantContactSource(invariantMainEstimatorContactSource);
+      }
       estimatorThread = avatarEstimatorThreadFactory.createAvatarEstimatorThread();
    }
 
@@ -1220,6 +1232,33 @@ public void setSecondaryStateEstimatorFactory(StateEstimatorControllerFactory se
       this.secondaryStateEstimatorFactory.set(secondaryStateEstimatorFactory);
    }
 
+   /**
+    * Replaces the DRC main state estimator with the contact-aided right-invariant InEKF. Joints are still
+    * read from the processed sensor output (good-enough FK); the InEKF drives the floating base.
+    */
+   public void setUseInvariantMainStateEstimator(boolean useInvariantMainStateEstimator)
+   {
+      this.useInvariantMainStateEstimator = useInvariantMainStateEstimator;
+   }
+
+   /** Enables/disables foot-referenced yaw seeding when the invariant InEKF is the main estimator (default true). */
+   public void setInvariantMainEstimatorYawSeeding(boolean invariantMainEstimatorYawSeeding)
+   {
+      this.invariantMainEstimatorYawSeeding = invariantMainEstimatorYawSeeding;
+   }
+
+   /**
+    * Selects the invariant main estimator's contact-probability source: production foot switches
+    * ({@link InvariantContactSource#FOOT_SWITCHES}, default; joint-torque based on Alex, independent of the
+    * filter's own base estimate) or the built-in kinematic height detector
+    * ({@link InvariantContactSource#KINEMATIC_DETECTOR}, circular when the filter is main; A/B only). This is
+    * forwarded to the shared estimator thread factory, so the same source runs in sim and on hardware.
+    */
+   public void setInvariantMainEstimatorContactSource(InvariantContactSource invariantMainEstimatorContactSource)
+   {
+      this.invariantMainEstimatorContactSource = invariantMainEstimatorContactSource;
+   }
+
    public void setComponentFootstepGeneratorParameters(boolean useHeadingAndVelocityScript,
                                                        HeightMap heightMapForFootstepZ,
                                                        HeadingAndVelocityEvaluationScriptParameters headingAndVelocityEvaluationScriptParameters)

ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/wholeBodyHardwareControl/AvatarMultiThreadingFactory.java

@@ -50,6 +50,7 @@
 import us.ihmc.sensorProcessing.sensorProcessors.SensorProcessing;
 import us.ihmc.sensorProcessing.simulatedSensors.SensorReaderFactory;
 import us.ihmc.stateEstimation.humanoid.StateEstimatorController;
+import us.ihmc.stateEstimation.invariant_estimator.InvariantContactSource;
 import us.ihmc.tools.TimestampProvider;
 import us.ihmc.tools.factories.OptionalFactoryField;
 import us.ihmc.tools.factories.RequiredFactoryField;
@@ -109,6 +110,8 @@ public class AvatarMultiThreadingFactory
    private final OptionalFactoryField<Map<HighLevelControllerName, StateEstimatorMode>> estimatorModeMapReference = new OptionalFactoryField<>("estimatorModeMapReference");
    private final List<Runnable> preEstimatorRunnables = new ArrayList<>();
    private final List<Runnable> postEstimatorRunnables = new ArrayList<>();
+   private final boolean useInvariantMainEstimator;
+   private final InvariantContactSource invariantContactSource;
 
    // Controller
    private final HighLevelHumanoidControllerFactory avatarControllerFactory;
@@ -158,7 +161,9 @@ public AvatarMultiThreadingFactory(DRCRobotModel robotModel,
                                       double masterThreadDt,
                                       TimestampProvider monotonicTimeProvider,
                                       YoRegistry registry,
-                                      YoVariableServer yoVariableServer)
+                                      YoVariableServer yoVariableServer,
+                                      boolean useInvariantMainEstimator,
+                                      InvariantContactSource invariantContactSource)
    {
       this.masterRobotModel = robotModel;
       this.masterFullRobotModel = fullRobotModel;
@@ -172,6 +177,8 @@ public AvatarMultiThreadingFactory(DRCRobotModel robotModel,
       this.useMultiThreading = useMultiThreading;
       this.rootRegistry = registry;
       this.yoVariableServer = yoVariableServer;
+      this.useInvariantMainEstimator = useInvariantMainEstimator;
+      this.invariantContactSource = invariantContactSource;
 
       masterContext = new HumanoidRobotContextData(masterFullRobotModel);
 
@@ -258,6 +265,8 @@ private AvatarEstimatorThread createAndAddEstimatorThread(DRCRobotModel robotMod
       avatarEstimatorThreadFactory.setSensorReaderFactory(sensorReaderFactory);
       avatarEstimatorThreadFactory.setHumanoidRobotContextDataFactory(estimatorContextDataFactory);
       avatarEstimatorThreadFactory.setGravity(GRAVITY);
+      avatarEstimatorThreadFactory.setUseInvariantStateEstimator(useInvariantMainEstimator);
+      avatarEstimatorThreadFactory.setInvariantContactSource(invariantContactSource);
       //      if (secondaryEstimatorFactory != null)
       //         avatarEstimatorThreadFactory.addSecondaryStateEstimatorFactory(secondaryEstimatorFactory);
       StateEstimatorController stateEstimator = avatarEstimatorThreadFactory.getMainStateEstimator();

ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/stateEstimation/StateEstimatorParameters.java

@@ -26,6 +26,26 @@ public List<IMUBasedJointStateEstimatorParameters> getIMUBasedJointStateEstimato
       return Collections.emptyList();
    }
 
+   /**
+    * Which joint-level pre-filter implementation the estimator should construct.
+    * {@code ALPHA_COMPLEMENTARY} is the historical behavior (per-IMU-pair complementary joint fusion
+    * plus alpha-filtered IMU bias rejection); {@code JOINT_KF} selects the joint-level Kalman filter;
+    * {@code NONE} is an explicit pass-through (raw sensor values, zero biases) — the pre-seam
+    * behavior of the invariant pipeline, useful as a control condition. Note that on the DRC
+    * (kinematics-based) pipeline, {@code NONE} disables IMU-pair joint fusion AND IMU bias
+    * compensation, which is NOT that pipeline's historical behavior.
+    */
+   public enum JointLevelEstimatorType
+   {
+      NONE, ALPHA_COMPLEMENTARY, JOINT_KF
+   }
+
+   /** Selected once at construction time; the default preserves the historical behavior for all robots. */
+   public JointLevelEstimatorType getJointLevelEstimatorType()
+   {
+      return JointLevelEstimatorType.JOINT_KF;
+   }
+
    public boolean requestWristForceSensorCalibrationAtStart()
    {
       return false;
@@ -103,6 +123,51 @@ public double getIMUYawDriftRateFilterFreqInHertz()
       return 1.5e-3;
    }
 
+   /**
+    * Per-joint encoder position measurement-noise standard deviation (rad), by joint name, consumed by the
+    * {@code JOINT_KF} joint-level pre-filter for its direct encoder q update's measurement covariance.
+    * Return {@code Double.NaN} (the default) to use the filter's built-in scalar fallback for that joint.
+    */
+   public double getEncoderPositionMeasurementStandardDeviation(String jointName)
+   {
+      return Double.NaN;
+   }
+
+   /**
+    * Per-joint encoder velocity measurement-noise standard deviation (rad/s), by joint name. Consumed by the
+    * {@code JOINT_KF} pre-filter's stance anchor for base->foot chain joints that are NOT filter states (their
+    * measured velocity enters the anchor as a known input, so its noise is propagated into the anchor's
+    * measurement covariance). Return {@code Double.NaN} (the default) to use the filter's built-in fallback.
+    */
+   public double getEncoderVelocityMeasurementStandardDeviation(String jointName)
+   {
+      return Double.NaN;
+   }
+
+   /**
+    * Whether the {@code JOINT_KF} pre-filter should consume the sensed joint velocity as a direct measurement
+    * of its q̇ states (in addition to the pair-gyro and stance-anchor observations). Enable only when the
+    * velocity signal's low-pass corner is known — see
+    * {@link #getJointVelocityMeasurementBreakFrequency(String)} — or verified to sit far above the motion band.
+    */
+   public boolean useDirectJointVelocityMeasurementInJointKF()
+   {
+      return false;
+   }
+
+   /**
+    * Effective first-order break frequency (Hz) of the low-pass cascade the sensed joint velocity passed
+    * through before reaching the estimator (e.g. drive-firmware LPF then the sensor-processing alpha filter;
+    * corners compose as {@code 1/f_eff = sum of 1/f_stage}). The {@code JOINT_KF} direct-velocity channel uses
+    * it to inflate that joint's measurement covariance by the instantaneous lag error. Return {@code NaN} (the
+    * default) to treat the measurement as instantaneous — correct when there is no filtering (sim) and honest
+    * on hardware only when the corner sits far above the motion band.
+    */
+   public double getJointVelocityMeasurementBreakFrequency(String jointName)
+   {
+      return Double.NaN;
+   }
+
    public abstract boolean enableIMUBiasCompensation();
 
    public abstract double getIMUBiasFilterFreqInHertz();

ihmc-state-estimation/scs2/invariantFilterEstimator.scs2.chart

@@ -0,0 +1,86 @@
+<?xml version="1.0" encoding="UTF-8" standalone="yes"?>
+<!--
+  SCS2 chart-group layout for the InvariantEKFStateEstimator YoVariables.
+
+  Load it in the SCS2 visualizer: on a chart-panel tab, right-click (or use the chart
+  toolbar menu) -> "Load chart group..." and select this file. It also loads as part of a
+  full SCS2 GUI configuration.
+
+  Variable names are given as "<registrySimpleName>.<variableName>"; SCS2 resolves the
+  registry segment as a namespace ending, so the full path prefix is not required. Vector
+  and yaw-pitch-roll YoVariables are charted through their scalar components (suffixes
+  X/Y/Z and Yaw/Pitch/Roll). Position/velocity charts overlay the estimate with its
+  1-sigma covariance envelope (…UpperBound / …LowerBound).
+
+  Grid: 4 columns x 3 rows.
+-->
+<YoChartGroupConfiguration name="InvariantFilterEstimator">
+    <chartConfigurations>
+        <identifier column="0" row="0"/>
+        <yoVariables>InvariantEKFStateEstimator.invariantFilterPelvisBasePositionX</yoVariables>
+        <yoVariables>InvariantEKFStateEstimator.invariantFilterPelvisBasePositionUpperBoundX</yoVariables>
+        <yoVariables>InvariantEKFStateEstimator.invariantFilterPelvisBasePositionLowerBoundX</yoVariables>
+    </chartConfigurations>
+    <chartConfigurations>
+        <identifier column="1" row="0"/>
+        <yoVariables>InvariantEKFStateEstimator.invariantFilterPelvisBasePositionY</yoVariables>
+        <yoVariables>InvariantEKFStateEstimator.invariantFilterPelvisBasePositionUpperBoundY</yoVariables>
+        <yoVariables>InvariantEKFStateEstimator.invariantFilterPelvisBasePositionLowerBoundY</yoVariables>
+    </chartConfigurations>
+    <chartConfigurations>
+        <identifier column="2" row="0"/>
+        <yoVariables>InvariantEKFStateEstimator.invariantFilterPelvisBasePositionZ</yoVariables>
+        <yoVariables>InvariantEKFStateEstimator.invariantFilterPelvisBasePositionUpperBoundZ</yoVariables>
+        <yoVariables>InvariantEKFStateEstimator.invariantFilterPelvisBasePositionLowerBoundZ</yoVariables>
+    </chartConfigurations>
+    <chartConfigurations>
+        <identifier column="3" row="0"/>
+        <yoVariables>InvariantEKFStateEstimator.invariantFilterPelvisOrientationYaw</yoVariables>
+        <yoVariables>InvariantEKFStateEstimator.invariantFilterPelvisOrientationPitch</yoVariables>
+        <yoVariables>InvariantEKFStateEstimator.invariantFilterPelvisOrientationRoll</yoVariables>
+    </chartConfigurations>
+    <chartConfigurations>
+        <identifier column="0" row="1"/>
+        <yoVariables>InvariantEKFStateEstimator.invariantFilterPelvisBaseVelocityX</yoVariables>
+        <yoVariables>InvariantEKFStateEstimator.invariantFilterPelvisBaseVelocityUpperBoundX</yoVariables>
+        <yoVariables>InvariantEKFStateEstimator.invariantFilterPelvisBaseVelocityLowerBoundX</yoVariables>
+    </chartConfigurations>
+    <chartConfigurations>
+        <identifier column="1" row="1"/>
+        <yoVariables>InvariantEKFStateEstimator.invariantFilterPelvisBaseVelocityY</yoVariables>
+        <yoVariables>InvariantEKFStateEstimator.invariantFilterPelvisBaseVelocityUpperBoundY</yoVariables>
+        <yoVariables>InvariantEKFStateEstimator.invariantFilterPelvisBaseVelocityLowerBoundY</yoVariables>
+    </chartConfigurations>
+    <chartConfigurations>
+        <identifier column="2" row="1"/>
+        <yoVariables>InvariantEKFStateEstimator.invariantFilterPelvisBaseVelocityZ</yoVariables>
+        <yoVariables>InvariantEKFStateEstimator.invariantFilterPelvisBaseVelocityUpperBoundZ</yoVariables>
+        <yoVariables>InvariantEKFStateEstimator.invariantFilterPelvisBaseVelocityLowerBoundZ</yoVariables>
+    </chartConfigurations>
+    <chartConfigurations>
+        <identifier column="3" row="1"/>
+        <yoVariables>InvariantEKFStateEstimator.invariantFilterPelvisOrientationStdDevX</yoVariables>
+        <yoVariables>InvariantEKFStateEstimator.invariantFilterPelvisOrientationStdDevY</yoVariables>
+        <yoVariables>InvariantEKFStateEstimator.invariantFilterPelvisOrientationStdDevZ</yoVariables>
+    </chartConfigurations>
+    <chartConfigurations>
+        <identifier column="0" row="2"/>
+        <yoVariables>InvariantEKFStateEstimator.invariantFilterPelvisBaseVelocityBodyX</yoVariables>
+        <yoVariables>InvariantEKFStateEstimator.invariantFilterPelvisBaseVelocityBodyY</yoVariables>
+        <yoVariables>InvariantEKFStateEstimator.invariantFilterPelvisBaseVelocityBodyZ</yoVariables>
+    </chartConfigurations>
+    <chartConfigurations>
+        <identifier column="1" row="2"/>
+        <yoVariables>InvariantEKFStateEstimator.invariantMinusMainOrientationErrorAngle</yoVariables>
+    </chartConfigurations>
+    <chartConfigurations>
+        <identifier column="2" row="2"/>
+        <yoVariables>InvariantEKFStateEstimator.invariantMinusMainLinearVelocityErrorMagnitude</yoVariables>
+    </chartConfigurations>
+    <chartConfigurations>
+        <identifier column="3" row="2"/>
+        <yoVariables>InvariantEKFStateEstimator.invariantMinusMainAngularVelocityErrorMagnitude</yoVariables>
+    </chartConfigurations>
+    <numberOfColumns>4</numberOfColumns>
+    <numberOfRows>3</numberOfRows>
+</YoChartGroupConfiguration>