started passing in a use imu flag

Author: rjgriffin42

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

@@ -107,6 +107,7 @@ public PelvisKinematicsBasedLinearStateCalculator(FullInverseDynamicsStructure i
       copFilterBreakFrequency = new DoubleParameter("CopFilterBreakFrequency", registry, stateEstimatorParameters.getCoPFilterFreqInHertz());
       correctTrustedFeetPositions = new BooleanParameter("correctTrustedFeetPositions", registry, stateEstimatorParameters.correctTrustedFeetPositions());
 
+      YoBoolean useFootIMUData = new YoBoolean("useFootIMUData", registry);
       YoDouble footAlphaLeakIMUOnly = new YoDouble("footIMUOnlyAlphaLeak", registry);
       YoDouble imuAgainstKinematicsForVelocityBreakFrequency = new YoDouble("footIMUAgainstKinematicsForVelocityBreakFrequency", registry);
       YoDouble imuAgainstKinematicsForPositionBreakFrequency = new YoDouble("footIMUAgainstKinematicsForPositionBreakFrequency", registry);
@@ -133,6 +134,7 @@ public PelvisKinematicsBasedLinearStateCalculator(FullInverseDynamicsStructure i
                                                      centerOfPressureDataHolderFromController,
                                                      cancelGravityFromAccelerationMeasurement,
                                                      gravityVector,
+                                                     useFootIMUData,
                                                      imuAgainstKinematicsForPositionBreakFrequency,
                                                      imuAgainstKinematicsForVelocityBreakFrequency,
                                                      footAlphaLeakIMUOnly,

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

@@ -44,14 +44,13 @@
 
 class SingleFootEstimator implements SCS2YoGraphicHolder
 {
-   private static boolean USE_IMU_DATA = true;
-
    private final RigidBodyBasics foot;
 
    private final ReferenceFrame rootJointFrame;
    private final ReferenceFrame soleFrame;
    private final ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();
 
+   private final BooleanProvider useIMUData;
    private final YoFrameVector3D footIKLinearVelocityInWorld;
    private final YoFrameVector3D footIKAngularVelocityInWorld;
    private final YoFrameVector3D footIMULinearVelocityInWorld;
@@ -99,6 +98,7 @@ public SingleFootEstimator(FloatingJointBasics rootJoint,
                               CenterOfPressureDataHolder centerOfPressureDataHolderFromController,
                               BooleanProvider cancelGravityFromAccelerationMeasurement,
                               FrameVector3DReadOnly gravityVector,
+                              BooleanProvider useIMUData,
                               DoubleProvider imuAgainstKinematicsForPositionBreakFrequency,
                               DoubleProvider imuAgainstKinematicsForVelocityBreakFrequency,
                               DoubleProvider footAlphaLeakIMUOnly,
@@ -112,6 +112,7 @@ public SingleFootEstimator(FloatingJointBasics rootJoint,
       this.centerOfPressureDataHolderFromController = centerOfPressureDataHolderFromController;
       this.cancelGravityFromAccelerationMeasurement = cancelGravityFromAccelerationMeasurement;
       this.gravityVector = gravityVector;
+      this.useIMUData = useIMUData;
       this.imuAgainstKinematicsForPositionBreakFrequency = imuAgainstKinematicsForPositionBreakFrequency;
       this.imuAgainstKinematicsForVelocityBreakFrequency = imuAgainstKinematicsForVelocityBreakFrequency;
       this.footAlphaLeakIMUOnly = footAlphaLeakIMUOnly;
@@ -204,7 +205,7 @@ public void updateKinematics(TwistReadOnly rootBodyTwist)
       footToRootJointPosition.update(tempFrameVector);
 
       // FIXME remove magic parameters
-      if (USE_IMU_DATA)
+      if (useIMUData.getValue() && footIMU != null)
          footIsMoving.set(getFootLinearVelocityInWorld().norm() > 0.5 || getFootAngularVelocityInWorld().norm() > 1.0);
       else
          footIsMoving.set(false);
@@ -380,7 +381,7 @@ private void updateCoPPosition(boolean trustCoPAsNonSlippingContactPoint, boolea
          copIKPositionInWorld.setFromReferenceFrame(soleFrame);
       }
 
-      if (USE_IMU_DATA)
+      if (useIMUData.getValue() && footIMU != null)
       {
          // Integrate the fused position using the velocity, which is from the kinematics and the IMU.
          computeLinearVelocityAtPointInWorld(copFusedPositionInWorld, tempFrameVector);
@@ -418,7 +419,7 @@ private void computeFootTwistInWorld(TwistReadOnly rootBodyTwist)
       footTwistInWorld.setBodyFrame(soleFrame);
       footTwistInWorld.changeFrame(worldFrame);
 
-      if (USE_IMU_DATA)
+      if (useIMUData.getValue() && footIMU != null)
       {
          // Record the kinematic data
          footIKLinearVelocityInWorld.set(footTwistInWorld.getLinearPart());