started improving the update to the foot state for the kinematic pelvis linear velocity

Author: rjgriffin42

ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/PelvisKinematicsBasedLinearStateCalculator.java

@@ -45,6 +45,7 @@ public class PelvisKinematicsBasedLinearStateCalculator
 
    private final YoFramePoint3D rootJointPosition = new YoFramePoint3D("estimatedRootJointPositionKinematics", worldFrame, registry);
    private final YoFrameVector3D rootJointLinearVelocity = new YoFrameVector3D("estimatedRootJointLinearVelocityKinematics", worldFrame, registry);
+   private final YoFrameVector3D averagedTrustedFootVelocity = new YoFrameVector3D("averagedTrustedFootVelocity", worldFrame, registry);
    private final DoubleProvider alphaRootJointLinearVelocity;
 
    /** Debug variable */
@@ -180,17 +181,23 @@ public void estimatePelvisLinearState(List<RigidBodyBasics> trustedFeet, FramePo
       }
       else
       {
+         averagedTrustedFootVelocity.setToZero();
          for (int i = 0; i < trustedFeet.size(); i++)
          {
             SingleFootEstimator footEstimator = footEstimatorMap.get(trustedFeet.get(i));
 
             double scaleFactor = 1.0 / trustedFeet.size();
 
+            // The root joint position is zeroed when we call #updateKinematics
             rootJointPosition.scaleAdd(scaleFactor, footEstimator.getRootJointPositionFromKinematics(), rootJointPosition);
-            rootJointLinearVelocity.scaleAdd(-scaleFactor * alphaRootJointLinearVelocity.getValue(),
-                                             footEstimator.getCopVelocityInWorld(),
-                                             rootJointLinearVelocity);
+            averagedTrustedFootVelocity.scaleAdd(scaleFactor, footEstimator.getCopVelocityInWorld(), averagedTrustedFootVelocity);
+
          }
+
+         // We want the average velocity of the feet to trend to zero, since the trusted feet are assumed to not be moving.
+         rootJointLinearVelocity.scaleAdd(-alphaRootJointLinearVelocity.getValue(),
+                                          averagedTrustedFootVelocity,
+                                          rootJointLinearVelocity);
       }
 
       rootJointLinearVelocityFDDebug.update();

ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/SingleFootEstimator.java

@@ -63,9 +63,11 @@ public class SingleFootEstimator implements SCS2YoGraphicHolder
 
    private final YoFrameVector3D copVelocityInWorld;
    private final AlphaFilteredYoFrameVector3D footToRootJointPosition;
+   private final AlphaFilteredYoFrameVector3D footToRootJointVelocity;
    private final YoFramePoint3D footPositionInWorld;
    /** Debug variable */
    private final YoFramePoint3D rootJointPositionPerFoot;
+   private final YoFrameVector3D rootJointVelocityPerFoot;
 
    private final YoFramePoint3D copIKPositionInWorld;
    private final YoFramePoint3D copIMUPositionInWorld;
@@ -131,7 +133,9 @@ public SingleFootEstimator(FloatingJointBasics rootJoint,
 
       DoubleProvider alphaFoot = () -> AlphaFilterTools.computeAlphaGivenBreakFrequencyProperly(footToRootJointPositionBreakFrequency.getValue(), estimatorDT);
       footToRootJointPosition = new AlphaFilteredYoFrameVector3D(namePrefix + "FootToRootJointPosition", "", registry, alphaFoot, worldFrame);
+      footToRootJointVelocity = new AlphaFilteredYoFrameVector3D(namePrefix + "FootToRootJointVelocity", "", registry, alphaFoot, worldFrame);
       rootJointPositionPerFoot = new YoFramePoint3D(namePrefix + "BasedRootJointPosition", worldFrame, registry);
+      rootJointVelocityPerFoot = new YoFrameVector3D(namePrefix + "BasedRootJointVelocity", worldFrame, registry);
       footPositionInWorld = new YoFramePoint3D(namePrefix + "FootPositionInWorld", worldFrame, registry);
       footPolygon = new FrameConvexPolygon2D(FrameVertex2DSupplier.asFrameVertex2DSupplier(contactableFoot.getContactPoints2D()));
       footCenterCoPLineSegment = new FrameLineSegment2D(soleFrame);
@@ -182,6 +186,7 @@ public YoGraphicDefinition getSCS2YoGraphics()
    public void initialize()
    {
       footToRootJointPosition.reset();
+      footToRootJointVelocity.reset();
       copFilteredInFootFrame.reset();
       copFilteredInFootFrame.update(0.0, 0.0);
       copFusedPositionInWorld.setMatchingFrame(copFilteredInFootFrame, 0.0);
@@ -236,6 +241,7 @@ public void computeStateSettingCoPZ(boolean trustCoPAsNonSlippingContactPoint, b
          correctFootPositionUsingCoP();
 
       rootJointPositionPerFoot.add(footPositionInWorld, footToRootJointPosition);
+      rootJointVelocityPerFoot.sub(footFusedLinearVelocityInWorld, footToRootJointVelocity);
    }
 
    public void computeState(boolean trustCoPAsNonSlippingContactPoint, boolean useControllerDesiredCoP)
@@ -245,6 +251,7 @@ public void computeState(boolean trustCoPAsNonSlippingContactPoint, boolean useC
          correctFootPositionUsingCoP();
 
       rootJointPositionPerFoot.add(footPositionInWorld, footToRootJointPosition);
+      rootJointVelocityPerFoot.sub(footFusedLinearVelocityInWorld, footToRootJointVelocity);
    }
 
    public void computeStateSettingFootZ(double zPosition)
@@ -255,6 +262,7 @@ public void computeStateSettingFootZ(double zPosition)
       copFusedPositionInWorld.set(footPositionInWorld);
 
       rootJointPositionPerFoot.add(footPositionInWorld, footToRootJointPosition);
+      rootJointVelocityPerFoot.sub(footFusedLinearVelocityInWorld, footToRootJointVelocity);
    }
 
    /**
@@ -298,6 +306,11 @@ public FramePoint3DReadOnly getRootJointPositionFromKinematics()
       return rootJointPositionPerFoot;
    }
 
+   public FrameVector3DReadOnly getRootJointVelocityFromKinematics()
+   {
+      return rootJointVelocityPerFoot;
+   }
+
    public FrameVector3DReadOnly getCopVelocityInWorld()
    {
       return copVelocityInWorld;
@@ -426,6 +439,11 @@ private void computeFootTwistInWorldFusingBaseStateAndFootIMU(TwistReadOnly root
       tempRootBodyTwist.changeFrame(foot.getBodyFixedFrame());
 
       foot.getBodyFixedFrame().getTwistRelativeToOther(rootJointFrame, footTwistInWorld);
+
+      tempFrameVector.setIncludingFrame(footTwistInWorld.getLinearPart());
+      tempFrameVector.changeFrame(worldFrame);
+      footToRootJointVelocity.update(tempFrameVector);
+
       footTwistInWorld.add(tempRootBodyTwist);
       footTwistInWorld.setBodyFrame(soleFrame);
       footTwistInWorld.changeFrame(worldFrame);