got the IMU based locomotion working better

rjgriffin42 · rjgriffin42 · commit a76a67fba4c3 · 2025-09-04T01:36:26.000-05:00
diff --git a/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/PelvisKinematicsBasedLinearStateCalculator.java b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/PelvisKinematicsBasedLinearStateCalculator.java
@@ -1,5 +1,6 @@
 package us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation;
 
+import java.util.ArrayList;
 import java.util.HashMap;
 import java.util.List;
 import java.util.Map;
@@ -109,7 +110,7 @@ public PelvisKinematicsBasedLinearStateCalculator(FullInverseDynamicsStructure i
       YoDouble footAlphaLeakIMUOnly = new YoDouble("footIMUOnlyAlphaLeak", registry);
       YoDouble imuAgainstKinematicsForVelocityBreakFrequency = new YoDouble("footIMUAgainstKinematicsForVelocityBreakFrequency", registry);
       YoDouble imuAgainstKinematicsForPositionBreakFrequency = new YoDouble("footIMUAgainstKinematicsForPositionBreakFrequency", registry);
-      imuAgainstKinematicsForPositionBreakFrequency.set(200.0);
+      imuAgainstKinematicsForPositionBreakFrequency.set(0.0);
       imuAgainstKinematicsForVelocityBreakFrequency.set(2.0);
       footAlphaLeakIMUOnly.set(0.999);
 
@@ -234,14 +235,28 @@ public void updateNoTrustedFeet(FramePoint3DReadOnly pelvisPosition, FrameVector
          rootJointLinearVelocity.setToZero();
    }
 
-   public void estimatePelvisLinearState(List<RigidBodyBasics> trustedFeet, List<RigidBodyBasics> unTrustedFeet, FramePoint3DReadOnly pelvisPosition)
+   private final List<RigidBodyBasics> trustedFeetLocal = new ArrayList<>();
+   private final List<RigidBodyBasics> unTrustedFeetLocal = new ArrayList<>();
+
+   public void estimatePelvisLinearState(List<RigidBodyBasics> trustedFeet,
+                                         List<RigidBodyBasics> unTrustedFeet,
+                                         FramePoint3DReadOnly pelvisPosition,
+                                         FrameVector3DReadOnly pelvisLinearVelocity)
    {
       if (!kinematicsIsUpToDate.getBooleanValue())
          throw new RuntimeException("Leg kinematics needs to be updated before trying to estimate the pelvis position/linear velocity.");
 
+      trustedFeetLocal.clear();
+      unTrustedFeetLocal.clear();
+
       for (int i = 0; i < trustedFeet.size(); i++)
+         trustedFeetLocal.add(trustedFeet.get(i));
+      for (int i = 0; i < unTrustedFeet.size(); i++)
+         unTrustedFeetLocal.add(unTrustedFeet.get(i));
+
+      for (int i = 0; i < trustedFeetLocal.size(); i++)
       {
-         SingleFootEstimator footEstimator = footEstimatorMap.get(trustedFeet.get(i));
+         SingleFootEstimator footEstimator = footEstimatorMap.get(trustedFeetLocal.get(i));
          if (assumeTrustedFootAtZeroHeight.getValue())
          {
             if (trustCoPAsNonSlippingContactPoint.getValue())
@@ -257,23 +272,52 @@ public void estimatePelvisLinearState(List<RigidBodyBasics> trustedFeet, List<Ri
          {
             footEstimator.updateCoPAndFootPosition(trustCoPAsNonSlippingContactPoint.getValue(), useControllerDesiredCoP.getValue());
          }
-         footEstimator.correctPelvisFromKinematics(trustedFeet.size(), alphaRootJointLinearVelocity.getValue(), rootJointPosition, rootJointLinearVelocity);
+      }
+
+      int trustIdx = 0;
+      while (trustIdx < trustedFeetLocal.size())
+      {
+         if (footEstimatorMap.get(trustedFeetLocal.get(trustIdx)).isFootMoving())
+            unTrustedFeetLocal.add(trustedFeetLocal.remove(trustIdx));
+         else
+            trustIdx++;
+      }
+
+      if (trustedFeetLocal.isEmpty())
+      {
+         rootJointPosition.set(pelvisPosition);
+         if (pelvisLinearVelocity != null)
+            rootJointLinearVelocity.set(pelvisLinearVelocity);
+         else
+            rootJointLinearVelocity.setToZero();
+      }
+      else
+      {
+         for (int i = 0; i < trustedFeetLocal.size(); i++)
+         {
+            SingleFootEstimator footEstimator = footEstimatorMap.get(trustedFeetLocal.get(i));
+            footEstimator.correctPelvisFromKinematics(trustedFeetLocal.size(),
+                                                      alphaRootJointLinearVelocity.getValue(),
+                                                      rootJointPosition,
+                                                      rootJointLinearVelocity);
+         }
       }
 
       rootJointLinearVelocityFDDebug.update();
 
+      // TODO should this use the local copies?
       if (correctTrustedFeetPositions.getValue())
       {
-         for (int i = 0; i < trustedFeet.size(); i++)
+         for (int i = 0; i < trustedFeetLocal.size(); i++)
          {
-            SingleFootEstimator footEstimator = footEstimatorMap.get(trustedFeet.get(i));
+            SingleFootEstimator footEstimator = footEstimatorMap.get(trustedFeetLocal.get(i));
             footEstimator.correctFootPositionFromRootJoint(true, trustCoPAsNonSlippingContactPoint.getValue(), rootJointPosition);
          }
       }
 
-      for (int i = 0; i < unTrustedFeet.size(); i++)
+      for (int i = 0; i < unTrustedFeetLocal.size(); i++)
       {
-         SingleFootEstimator footEstimator = footEstimatorMap.get(unTrustedFeet.get(i));
+         SingleFootEstimator footEstimator = footEstimatorMap.get(unTrustedFeetLocal.get(i));
          footEstimator.correctFootPositionFromRootJoint(false, false, pelvisPosition);
       }
 
diff --git a/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/PelvisLinearStateUpdater.java b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/PelvisLinearStateUpdater.java
@@ -364,11 +364,11 @@ public void updateForFrozenState()
 
    public void updateRootJointPositionAndLinearVelocity()
    {
+      kinematicsBasedLinearStateCalculator.updateKinematics();
+
       if (requestStopEstimationOfPelvisLinearState.getBooleanValue())
          return;
 
-      kinematicsBasedLinearStateCalculator.updateKinematics();
-
       numberOfEndEffectorsTrusted.set(setTrustedFeetUsingFootSwitches());
       numberOfEndEffectorsFilteredByLoad.set(0);
 
@@ -413,7 +413,7 @@ public void updateRootJointPositionAndLinearVelocity()
       else if (numberOfEndEffectorsTrusted.getIntegerValue() > 0)
       {
          updateTrustedFeetLists();
-         kinematicsBasedLinearStateCalculator.estimatePelvisLinearState(listOfTrustedFeet, listOfUnTrustedFeet, rootJointPosition);
+         kinematicsBasedLinearStateCalculator.estimatePelvisLinearState(listOfTrustedFeet, listOfUnTrustedFeet, rootJointPosition, rootJointVelocity);
 
          if (imuBasedLinearStateCalculator.isEstimationEnabled())
          {
diff --git a/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/SingleFootEstimator.java b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/SingleFootEstimator.java
@@ -9,6 +9,7 @@
 import us.ihmc.euclid.referenceFrame.interfaces.FixedFramePoint3DBasics;
 import us.ihmc.euclid.referenceFrame.interfaces.FixedFrameVector3DBasics;
 import us.ihmc.euclid.referenceFrame.interfaces.FramePoint2DBasics;
+import us.ihmc.euclid.referenceFrame.interfaces.FramePoint2DReadOnly;
 import us.ihmc.euclid.referenceFrame.interfaces.FramePoint3DReadOnly;
 import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly;
 import us.ihmc.euclid.referenceFrame.interfaces.FrameVertex2DSupplier;
@@ -39,6 +40,7 @@
 import us.ihmc.yoVariables.providers.BooleanProvider;
 import us.ihmc.yoVariables.providers.DoubleProvider;
 import us.ihmc.yoVariables.registry.YoRegistry;
+import us.ihmc.yoVariables.variable.YoBoolean;
 
 class SingleFootEstimator implements SCS2YoGraphicHolder
 {
@@ -57,13 +59,17 @@ class SingleFootEstimator implements SCS2YoGraphicHolder
    private final YoFrameVector3D footFusedLinearVelocityInWorld;
    private final YoFrameVector3D footFusedAngularVelocityInWorld;
 
+   private final YoBoolean footIsMoving;
+
    private final YoFrameVector3D copVelocityInWorld;
    private final AlphaFilteredYoFrameVector3D footToRootJointPosition;
    private final YoFramePoint3D footPositionInWorld;
    /** Debug variable */
    private final YoFramePoint3D rootJointPositionPerFoot;
 
-   private final YoFramePoint3D copPositionInWorld;
+   private final YoFramePoint3D copIKPositionInWorld;
+   private final YoFramePoint3D copIMUPositionInWorld;
+   private final YoFramePoint3D copFusedPositionInWorld;
    private final YoFramePoint2D previousCoPInFootFrame;
 
    private final AlphaFilteredYoFramePoint2D copFilteredInFootFrame;
@@ -124,6 +130,8 @@ public SingleFootEstimator(FloatingJointBasics rootJoint,
       copRawInFootFrame = new YoFramePoint2D(namePrefix + "CoPRawInFootFrame", soleFrame, registry);
       previousCoPInFootFrame = new YoFramePoint2D(namePrefix + "PreviousCoPInFootFrame", soleFrame, registry);
 
+      footIsMoving = new YoBoolean(namePrefix + "IsMoving", registry);
+
       footIKLinearVelocityInWorld = new YoFrameVector3D(namePrefix + "IKLinearVelocityInWorld", worldFrame, registry);
       footIKAngularVelocityInWorld = new YoFrameVector3D(namePrefix + "IKAngularVelocityInWorld", worldFrame, registry);
       footIMULinearVelocityInWorld = new YoFrameVector3D(namePrefix + "IMULinearVelocityInWorld", worldFrame, registry);
@@ -134,7 +142,9 @@ public SingleFootEstimator(FloatingJointBasics rootJoint,
       DoubleProvider alphaCop = () -> AlphaFilterTools.computeAlphaGivenBreakFrequencyProperly(copFilterBreakFrequency.getValue(), estimatorDT);
       copFilteredInFootFrame = new AlphaFilteredYoFramePoint2D(namePrefix + "CoPFilteredInFootFrame", "", registry, alphaCop, copRawInFootFrame);
       copFilteredInFootFrame.update(0.0, 0.0);
-      copPositionInWorld = new YoFramePoint3D(namePrefix + "CoPPositionInWorld", worldFrame, registry);
+      copIKPositionInWorld = new YoFramePoint3D(namePrefix + "CoPIKPositionInWorld", worldFrame, registry);
+      copIMUPositionInWorld = new YoFramePoint3D(namePrefix + "CoPIMUPositionInWorld", worldFrame, registry);
+      copFusedPositionInWorld = new YoFramePoint3D(namePrefix + "CoPFusedPositionInWorld", worldFrame, registry);
       copVelocityInWorld = new YoFrameVector3D(namePrefix + "CoPVelocityInWorld", worldFrame, registry);
    }
 
@@ -144,7 +154,7 @@ public void createVisualization(YoGraphicsListRegistry yoGraphicsListRegistry)
          return;
 
       String sidePrefix = foot.getName();
-      YoGraphicPosition copInWorld = new YoGraphicPosition(sidePrefix + "StateEstimatorCoP", copPositionInWorld, 0.005, YoAppearance.DeepPink());
+      YoGraphicPosition copInWorld = new YoGraphicPosition(sidePrefix + "StateEstimatorCoP", copFusedPositionInWorld, 0.005, YoAppearance.DeepPink());
       YoArtifactPosition artifact = copInWorld.createArtifact();
       artifact.setVisible(false);
       yoGraphicsListRegistry.registerArtifact("StateEstimator", artifact);
@@ -153,7 +163,7 @@ public void createVisualization(YoGraphicsListRegistry yoGraphicsListRegistry)
    @Override
    public YoGraphicDefinition getSCS2YoGraphics()
    {
-      return YoGraphicDefinitionFactory.newYoGraphicPoint2D(foot.getName() + "StateEstimatorCoP", copPositionInWorld,
+      return YoGraphicDefinitionFactory.newYoGraphicPoint2D(foot.getName() + "StateEstimatorCoP", copFusedPositionInWorld,
                                                             0.01,
                                                             ColorDefinitions.DeepPink(),
                                                             DefaultPoint2DGraphic.CIRCLE);
@@ -192,6 +202,12 @@ public void updateKinematics(TwistReadOnly rootBodyTwist)
       tempFrameVector.changeFrame(worldFrame);
 
       footToRootJointPosition.update(tempFrameVector);
+
+      // FIXME remove magic parameters
+      if (USE_IMU_DATA)
+         footIsMoving.set(getFootLinearVelocityInWorld().norm() > 0.5 || getFootAngularVelocityInWorld().norm() > 1.0);
+      else
+         footIsMoving.set(false);
    }
 
    /**
@@ -202,7 +218,7 @@ public void updateKinematics(TwistReadOnly rootBodyTwist)
    public void updateCoPAndFootSettingZ(boolean trustCoPAsNonSlippingContactPoint, boolean useControllerDesiredCoP, double zPosition)
    {
       updateCoPPosition(trustCoPAsNonSlippingContactPoint, useControllerDesiredCoP);
-      copPositionInWorld.setZ(zPosition);
+      copFusedPositionInWorld.setZ(zPosition);
       correctFootPositionsUsingCoP(trustCoPAsNonSlippingContactPoint);
    }
 
@@ -257,14 +273,14 @@ public void correctFootPositionFromRootJoint(boolean isTrusted, boolean trustCoP
 
       if (!isTrusted)
       {
-         copPositionInWorld.set(footPositionInWorld);
+         copFusedPositionInWorld.set(footPositionInWorld);
          copRawInFootFrame.setToZero();
          copFilteredInFootFrame.setToZero();
       }
       else if (trustCoPAsNonSlippingContactPoint)
       {
          tempFrameVector.sub(footPositionInWorld, tempPoint); // Delta from previous to new foot position
-         copPositionInWorld.add(tempFrameVector); // New CoP position
+         copFusedPositionInWorld.add(tempFrameVector); // New CoP position
       }
    }
 
@@ -288,6 +304,11 @@ public FrameVector3DReadOnly getFootAngularVelocityInWorld()
       return footFusedAngularVelocityInWorld;
    }
 
+   public boolean isFootMoving()
+   {
+      return footIsMoving.getBooleanValue();
+   }
+
    private final FramePoint2D tempCoP2d = new FramePoint2D();
    private final FrameVector3D tempCoPOffset = new FrameVector3D();
 
@@ -340,23 +361,35 @@ private void updateCoPPosition(boolean trustCoPAsNonSlippingContactPoint, boolea
          copRawInFootFrame.set(tempCoP2d);
          copFilteredInFootFrame.update();
 
+         // Update the CoP value from kinematics. This treats it as non-stationary.
+         copIKPositionInWorld.setMatchingFrame(soleFrame, copFilteredInFootFrame.getX(), copFilteredInFootFrame.getY(), 0.0);
+
          // Update the change in the CoP from the previous tick.
          tempCoPOffset.setIncludingFrame(soleFrame,
                                          copFilteredInFootFrame.getX() - previousCoPInFootFrame.getX(),
                                          copFilteredInFootFrame.getY() - previousCoPInFootFrame.getY(),
                                          0.0);
          tempCoPOffset.changeFrame(worldFrame);
          // Apply this same change to the CoP in world.
-         copPositionInWorld.add(tempCoPOffset);
+         copFusedPositionInWorld.add(tempCoPOffset);
       }
       else
       {
          tempCoP2d.setToZero(soleFrame);
-         copRawInFootFrame.setToZero();
-         copFilteredInFootFrame.setToZero();
-         copPositionInWorld.setFromReferenceFrame(soleFrame);
+         // Update the CoP value to match the center of the foot.
+         copIKPositionInWorld.setFromReferenceFrame(soleFrame);
       }
 
+      if (USE_IMU_DATA)
+      {
+         // Integrate the fused position using the velocity, which is from the kinematics and the IMU.
+         computeLinearVelocityAtPointInWorld(copFusedPositionInWorld, tempFrameVector);
+         copIMUPositionInWorld.scaleAdd(footAlphaLeakIMUOnly.getValue() * estimatorDT, tempFrameVector, copFusedPositionInWorld);
+
+         // Fuse the kinematic data with the IMU data
+         double alpha = AlphaFilterTools.computeAlphaGivenBreakFrequencyProperly(imuAgainstKinematicsForPositionBreakFrequency.getValue(), estimatorDT);
+         copFusedPositionInWorld.interpolate(copIKPositionInWorld, copIMUPositionInWorld, alpha);
+      }
    }
 
    private void correctFootPositionsUsingCoP(boolean trustCoPAsNonSlippingContactPoint)
@@ -368,7 +401,7 @@ private void correctFootPositionsUsingCoP(boolean trustCoPAsNonSlippingContactPo
       tempCoPOffset.setIncludingFrame(copFilteredInFootFrame, 0.0);
       tempCoPOffset.changeFrame(worldFrame);
       // Update where the foot must be, according to this CoP location
-      footPositionInWorld.sub(copPositionInWorld, tempCoPOffset);
+      footPositionInWorld.sub(copFusedPositionInWorld, tempCoPOffset);
    }
 
    private final FrameVector3D linearAcceleration = new FrameVector3D();
@@ -445,10 +478,15 @@ private void computeFootTwistInWorld(TwistReadOnly rootBodyTwist)
    }
 
    private void computeCoPLinearVelocityInWorld(FixedFrameVector3DBasics footLinearVelocityToPack)
+   {
+      computeLinearVelocityAtPointInWorld(copFusedPositionInWorld, footLinearVelocityToPack);
+   }
+
+   private void computeLinearVelocityAtPointInWorld(FramePoint3DReadOnly point, FixedFrameVector3DBasics footLinearVelocityToPack)
    {
       footTwistInWorld.getLinearPart().set(footFusedLinearVelocityInWorld);
       footTwistInWorld.getAngularPart().set(footFusedAngularVelocityInWorld);
 
-      footTwistInWorld.getLinearVelocityAt(copPositionInWorld, footLinearVelocityToPack);
+      footTwistInWorld.getLinearVelocityAt(point, footLinearVelocityToPack);
    }
 }

Original file line number	Diff line number	Diff line change
`@@ -364,11 +364,11 @@ public void updateForFrozenState()`
`364`	`364`
`365`	`365`	`public void updateRootJointPositionAndLinearVelocity()`
`366`	`366`	`{`
	`367`	`+ kinematicsBasedLinearStateCalculator.updateKinematics();`
	`368`	`+`
`367`	`369`	`if (requestStopEstimationOfPelvisLinearState.getBooleanValue())`
`368`	`370`	`return;`
`369`	`371`
`370`		`- kinematicsBasedLinearStateCalculator.updateKinematics();`
`371`		`-`
`372`	`372`	`numberOfEndEffectorsTrusted.set(setTrustedFeetUsingFootSwitches());`
`373`	`373`	`numberOfEndEffectorsFilteredByLoad.set(0);`
`374`	`374`
`@@ -413,7 +413,7 @@ public void updateRootJointPositionAndLinearVelocity()`
`413`	`413`	`else if (numberOfEndEffectorsTrusted.getIntegerValue() > 0)`
`414`	`414`	`{`
`415`	`415`	`updateTrustedFeetLists();`
`416`		`- kinematicsBasedLinearStateCalculator.estimatePelvisLinearState(listOfTrustedFeet, listOfUnTrustedFeet, rootJointPosition);`
	`416`	`+ kinematicsBasedLinearStateCalculator.estimatePelvisLinearState(listOfTrustedFeet, listOfUnTrustedFeet, rootJointPosition, rootJointVelocity);`
`417`	`417`
`418`	`418`	`if (imuBasedLinearStateCalculator.isEstimationEnabled())`
`419`	`419`	`{`