deleted many other classes

ihmcrobotics · Aug 7, 2024 · 6648620 · 6648620
1 parent 60ee2e0
commit 6648620
Show file tree

Hide file tree

Showing 53 changed files with 197 additions and 1,429 deletions.
diff --git a/atlas/src/main/java/us/ihmc/atlas/parameters/AtlasPhysicalProperties.java b/atlas/src/main/java/us/ihmc/atlas/parameters/AtlasPhysicalProperties.java
@@ -2,6 +2,8 @@
 
 import us.ihmc.avatar.drcRobot.RobotPhysicalProperties;
 import us.ihmc.euclid.transform.RigidBodyTransform;
+import us.ihmc.euclid.tuple3D.Vector3D;
+import us.ihmc.euclid.tuple4D.Quaternion;
 import us.ihmc.robotics.geometry.TransformTools;
 import us.ihmc.robotics.robotSide.RobotSide;
 import us.ihmc.robotics.robotSide.SideDependentList;
@@ -69,7 +71,7 @@ public AtlasPhysicalProperties(double modelScale)
       // TODO Auto-generated constructor stub
       for (RobotSide robotSide : RobotSide.values)
       {
-         RigidBodyTransform soleToAnkleFrame = TransformTools.createTranslationTransform(footLengthForControl / 2.0 - footBackForControl, 0.0, -ankleHeight);
+         RigidBodyTransform soleToAnkleFrame = new RigidBodyTransform(new Quaternion(), new Vector3D(footLengthForControl / 2.0 - footBackForControl, 0.0, -ankleHeight));
          soleToAnkleFrameTransforms.put(robotSide, soleToAnkleFrame);
 
          double y = robotSide.negateIfRightSide(0.1);

diff --git a/atlas/src/main/java/us/ihmc/atlas/physics/AtlasPhysicsEngineConfiguration.java b/atlas/src/main/java/us/ihmc/atlas/physics/AtlasPhysicsEngineConfiguration.java
@@ -3,6 +3,7 @@
 import us.ihmc.atlas.AtlasJointMap;
 import us.ihmc.euclid.transform.RigidBodyTransform;
 import us.ihmc.euclid.tuple3D.Vector3D;
+import us.ihmc.euclid.tuple4D.Quaternion;
 import us.ihmc.robotics.geometry.TransformTools;
 import us.ihmc.robotics.robotSide.RobotSide;
 import us.ihmc.simulationconstructionset.FloatingJoint;
@@ -52,7 +53,7 @@ public void setup(Robot robot, ScsCollisionDetector collisionDetector, Collision
       CollisionShapeDescription collisionFoot = factoryShape.createBox(jointMap.getPhysicalProperties().getActualFootLength() / 2, jointMap.getPhysicalProperties().getActualFootWidth() / 2, 0.05);
 
       //      public static final double ankleHeight = 0.084;
-      RigidBodyTransform ankleToSole = TransformTools.createTranslationTransform(new Vector3D(0.0, 0.0, 0.084));//jointMap.getPhysicalProperties().getAnkle_to_sole_frame_tranform();
+      RigidBodyTransform ankleToSole = new RigidBodyTransform(new Quaternion(), new Vector3D(0.0, 0.0, 0.084)); //jointMap.getPhysicalProperties().getAnkle_to_sole_frame_tranform();
       RigidBodyTransform soleToAnkle = new RigidBodyTransform();
       ankleToSole.setAndInvert(soleToAnkle);
 

diff --git a/...ons/src/main/java/us/ihmc/exampleSimulations/sphereICPControl/model/SphereRobotModel.java b/...ons/src/main/java/us/ihmc/exampleSimulations/sphereICPControl/model/SphereRobotModel.java
@@ -14,10 +14,10 @@
 import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
 import us.ihmc.mecano.tools.MultiBodySystemTools;
 import us.ihmc.robotModels.FullRobotModel;
+import us.ihmc.robotics.MultiBodySystemMissingTools;
 import us.ihmc.robotics.partNames.NeckJointName;
 import us.ihmc.robotics.partNames.RobotSpecificJointNames;
 import us.ihmc.robotics.partNames.SpineJointName;
-import us.ihmc.robotics.screwTheory.TotalMassCalculator;
 import us.ihmc.robotics.sensors.ForceSensorDefinition;
 import us.ihmc.robotics.sensors.IMUDefinition;
 
@@ -50,7 +50,7 @@ public SphereRobotModel()
       centerOfMassFrame = new CenterOfMassReferenceFrame("centerOfMass", worldFrame, elevator);
 
       oneDoFJoints = MultiBodySystemTools.createOneDoFJointPath(elevator, body);
-      totalMass = TotalMassCalculator.computeSubTreeMass(body);
+      totalMass = MultiBodySystemMissingTools.computeSubTreeMass(body);
    }
 
    @Override

diff --git a/...rfaces/src/main/java/us/ihmc/avatar/diagnostics/AutomatedDiagnosticSimulationFactory.java b/...rfaces/src/main/java/us/ihmc/avatar/diagnostics/AutomatedDiagnosticSimulationFactory.java
@@ -16,11 +16,11 @@
 import us.ihmc.mecano.multiBodySystem.interfaces.FloatingJointBasics;
 import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
 import us.ihmc.robotModels.FullHumanoidRobotModel;
+import us.ihmc.robotics.MultiBodySystemMissingTools;
 import us.ihmc.robotics.contactable.ContactablePlaneBody;
 import us.ihmc.robotics.math.filters.AlphaFilteredYoVariable;
 import us.ihmc.robotics.robotSide.RobotSide;
 import us.ihmc.robotics.robotSide.SideDependentList;
-import us.ihmc.robotics.screwTheory.TotalMassCalculator;
 import us.ihmc.robotics.sensors.CenterOfMassDataHolder;
 import us.ihmc.robotics.sensors.FootSwitchFactory;
 import us.ihmc.robotics.sensors.FootSwitchInterface;
@@ -152,7 +152,7 @@ private SensorOutputMapReadOnly createStateEstimator(FullHumanoidRobotModel full
       HumanoidRobotSensorInformation sensorInformation = robotModel.getSensorInformation();
       String[] imuSensorsToUseInStateEstimator = sensorInformation.getIMUSensorsToUseInStateEstimator();
       double gravitationalAcceleration = 9.81;
-      double totalRobotWeight = TotalMassCalculator.computeSubTreeMass(fullRobotModel.getElevator()) * gravitationalAcceleration;
+      double totalRobotWeight = MultiBodySystemMissingTools.computeSubTreeMass(fullRobotModel.getElevator()) * gravitationalAcceleration;
 
       humanoidReferenceFrames = new HumanoidReferenceFrames(fullRobotModel);
       RobotContactPointParameters<RobotSide> contactPointParameters = robotModel.getContactPointParameters();

diff --git a/...faces/src/main/java/us/ihmc/avatar/kinematicsSimulation/HumanoidKinematicsSimulation.java b/...faces/src/main/java/us/ihmc/avatar/kinematicsSimulation/HumanoidKinematicsSimulation.java
@@ -63,7 +63,6 @@
 import us.ihmc.robotics.contactable.ContactablePlaneBody;
 import us.ihmc.robotics.robotSide.RobotSide;
 import us.ihmc.robotics.robotSide.SideDependentList;
-import us.ihmc.robotics.screwTheory.TotalMassCalculator;
 import us.ihmc.robotics.sensors.ForceSensorDataHolder;
 import us.ihmc.robotics.sensors.ForceSensorDataHolderReadOnly;
 import us.ihmc.robotics.sensors.IMUDefinition;
@@ -192,7 +191,7 @@ private HumanoidKinematicsSimulation(DRCRobotModel robotModel, HumanoidKinematic
       allContactableBodies.addAll(feet.values());
       contactableBodiesFactory.disposeFactory();
 
-      double totalRobotWeight = TotalMassCalculator.computeSubTreeMass(fullRobotModel.getElevator());
+      double totalRobotWeight = MultiBodySystemMissingTools.computeSubTreeMass(fullRobotModel.getElevator());
       for (RobotSide robotSide : RobotSide.values)
       {
          SettableFootSwitch footSwitch = new SettableFootSwitch(feet.get(robotSide), totalRobotWeight, 2, registry);

diff --git a/.../us/ihmc/avatar/networkProcessor/kinematicsToolboxModule/KinematicsToolboxController.java b/.../us/ihmc/avatar/networkProcessor/kinematicsToolboxModule/KinematicsToolboxController.java
@@ -83,6 +83,7 @@
 import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
 import us.ihmc.mecano.multiBodySystem.iterators.SubtreeStreams;
 import us.ihmc.mecano.tools.MultiBodySystemTools;
+import us.ihmc.robotics.MultiBodySystemMissingTools;
 import us.ihmc.robotics.controllers.pidGains.GainCoupling;
 import us.ihmc.robotics.controllers.pidGains.YoPIDSE3Gains;
 import us.ihmc.robotics.controllers.pidGains.implementations.DefaultYoPIDSE3Gains;
@@ -91,7 +92,6 @@
 import us.ihmc.robotics.physics.Collidable;
 import us.ihmc.robotics.physics.CollisionResult;
 import us.ihmc.robotics.screwTheory.SelectionMatrix6D;
-import us.ihmc.robotics.screwTheory.TotalMassCalculator;
 import us.ihmc.robotics.time.ThreadTimer;
 import us.ihmc.sensorProcessing.outputData.JointDesiredOutputList;
 import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint3D;
@@ -456,7 +456,7 @@ public KinematicsToolboxController(CommandInputManager commandInputManager,
 
       // This will find the root body without using rootJoint so it can be null.
       rootBody = MultiBodySystemTools.getRootBody(desiredOneDoFJoints[0].getPredecessor());
-      totalRobotMass = TotalMassCalculator.computeSubTreeMass(rootBody);
+      totalRobotMass = MultiBodySystemMissingTools.computeSubTreeMass(rootBody);
 
       centerOfMassFrame = new CenterOfMassReferenceFrame("centerOfMass", worldFrame, rootBody);
 
@@ -467,7 +467,7 @@ public KinematicsToolboxController(CommandInputManager commandInputManager,
       inverseKinematicsSolution = MessageTools.createKinematicsToolboxOutputStatus(desiredOneDoFJoints);
       inverseKinematicsSolution.setDestination(-1);
 
-      robotMass = TotalMassCalculator.computeSubTreeMass(rootBody);
+      robotMass = MultiBodySystemMissingTools.computeSubTreeMass(rootBody);
       centerOfMassSafeMargin.set(0.04); // Same as the walking controller.
 
       spatialGains.setPositionProportionalGains(GLOBAL_PROPORTIONAL_GAIN); // Gains used for everything. It is as high as possible to reduce the convergence time.

diff --git a/...hmc/avatar/networkProcessor/walkingPreview/WalkingControllerPreviewToolboxController.java b/...hmc/avatar/networkProcessor/walkingPreview/WalkingControllerPreviewToolboxController.java
@@ -53,10 +53,10 @@
 import us.ihmc.mecano.tools.MultiBodySystemStateIntegrator;
 import us.ihmc.robotModels.FullHumanoidRobotModel;
 import us.ihmc.robotModels.FullRobotModelUtils;
+import us.ihmc.robotics.MultiBodySystemMissingTools;
 import us.ihmc.robotics.contactable.ContactablePlaneBody;
 import us.ihmc.robotics.robotSide.RobotSide;
 import us.ihmc.robotics.robotSide.SideDependentList;
-import us.ihmc.robotics.screwTheory.TotalMassCalculator;
 import us.ihmc.robotics.taskExecutor.StateExecutor;
 import us.ihmc.sensorProcessing.frames.CommonHumanoidReferenceFrames;
 import us.ihmc.sensorProcessing.frames.ReferenceFrameHashCodeResolver;
@@ -213,7 +213,7 @@ private HighLevelHumanoidControllerToolbox createHighLevelControllerToolbox(DRCR
       List<ContactablePlaneBody> allContactableBodies = new ArrayList<>(additionalContacts);
       allContactableBodies.addAll(feet.values());
 
-      double robotMass = TotalMassCalculator.computeSubTreeMass(fullRobotModel.getElevator());
+      double robotMass = MultiBodySystemMissingTools.computeSubTreeMass(fullRobotModel.getElevator());
 
       for (RobotSide robotSide : RobotSide.values)
       {

diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/warmup/HumanoidControllerWarmup.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/warmup/HumanoidControllerWarmup.java
@@ -39,11 +39,11 @@
 import us.ihmc.mecano.multiBodySystem.interfaces.OneDoFJointBasics;
 import us.ihmc.mecano.spatial.Twist;
 import us.ihmc.robotModels.FullHumanoidRobotModel;
+import us.ihmc.robotics.MultiBodySystemMissingTools;
 import us.ihmc.robotics.contactable.ContactablePlaneBody;
 import us.ihmc.robotics.geometry.RotationTools;
 import us.ihmc.robotics.robotSide.RobotSide;
 import us.ihmc.robotics.robotSide.SideDependentList;
-import us.ihmc.robotics.screwTheory.TotalMassCalculator;
 import us.ihmc.robotics.sensors.FootSwitchInterface;
 import us.ihmc.sensorProcessing.frames.ReferenceFrameHashCodeResolver;
 import us.ihmc.sensorProcessing.outputData.JointDesiredOutputReadOnly;
@@ -265,7 +265,7 @@ private void createWalkingControllerAndSetUpManagerFactory(YoRegistry managerFac
          contactableBodies.add(feet.get(robotSide));
       contactableBodies.addAll(additionalContacts);
 
-      double totalRobotWeight = TotalMassCalculator.computeSubTreeMass(fullRobotModel.getElevator()) * gravityZ;
+      double totalRobotWeight = MultiBodySystemMissingTools.computeSubTreeMass(fullRobotModel.getElevator()) * gravityZ;
       SideDependentList<FootSwitchInterface> footSwitches = createFootSwitches(feet, totalRobotWeight, referenceFrames.getSoleZUpFrames());
       JointBasics[] jointsToIgnore = AvatarControllerThread.createListOfJointsToIgnore(fullRobotModel, robotModel, robotModel.getSensorInformation());
 

diff --git a/...odules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/BalanceManager.java b/...odules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/BalanceManager.java
@@ -41,12 +41,12 @@
 import us.ihmc.humanoidRobotics.footstep.FootstepTiming;
 import us.ihmc.humanoidRobotics.footstep.SimpleFootstep;
 import us.ihmc.robotModels.FullHumanoidRobotModel;
+import us.ihmc.robotics.MultiBodySystemMissingTools;
 import us.ihmc.robotics.SCS2YoGraphicHolder;
 import us.ihmc.robotics.geometry.ConvexPolygonScaler;
 import us.ihmc.robotics.math.trajectories.generators.MultipleWaypointsPoseTrajectoryGenerator;
 import us.ihmc.robotics.robotSide.RobotSide;
 import us.ihmc.robotics.robotSide.SideDependentList;
-import us.ihmc.robotics.screwTheory.TotalMassCalculator;
 import us.ihmc.robotics.time.ExecutionTimer;
 import us.ihmc.scs2.definition.visual.ColorDefinitions;
 import us.ihmc.scs2.definition.yoGraphic.YoGraphicDefinition;
@@ -226,7 +226,7 @@ public BalanceManager(HighLevelHumanoidControllerToolbox controllerToolbox,
       YoGraphicsListRegistry yoGraphicsListRegistry = controllerToolbox.getYoGraphicsListRegistry();
 
       double gravityZ = controllerToolbox.getGravityZ();
-      double totalMass = TotalMassCalculator.computeSubTreeMass(fullRobotModel.getElevator());
+      double totalMass = MultiBodySystemMissingTools.computeSubTreeMass(fullRobotModel.getElevator());
 
       this.controllerToolbox = controllerToolbox;
       yoTime = controllerToolbox.getYoTime();

diff --git a/.../ihmc/commonWalkingControlModules/capturePoint/SimpleLinearMomentumRateControlModule.java b/.../ihmc/commonWalkingControlModules/capturePoint/SimpleLinearMomentumRateControlModule.java
@@ -41,16 +41,15 @@
 import us.ihmc.humanoidRobotics.footstep.SimpleFootstep;
 import us.ihmc.log.LogTools;
 import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
+import us.ihmc.robotics.MultiBodySystemMissingTools;
 import us.ihmc.robotics.dataStructures.parameters.ParameterVector3D;
 import us.ihmc.robotics.math.filters.AlphaFilteredYoVariable;
 import us.ihmc.robotics.math.filters.FilteredVelocityYoFrameVector2d;
 import us.ihmc.robotics.math.filters.RateLimitedYoFrameVector;
-import us.ihmc.robotics.math.trajectories.core.Polynomial3D;
 import us.ihmc.robotics.math.trajectories.interfaces.Polynomial3DReadOnly;
 import us.ihmc.robotics.robotSide.RobotSide;
 import us.ihmc.robotics.robotSide.SideDependentList;
 import us.ihmc.robotics.screwTheory.SelectionMatrix6D;
-import us.ihmc.robotics.screwTheory.TotalMassCalculator;
 import us.ihmc.sensorProcessing.frames.CommonHumanoidReferenceFrames;
 import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint2D;
 import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint3D;
@@ -174,7 +173,7 @@ public SimpleLinearMomentumRateControlModule(CommonHumanoidReferenceFrames refer
                                           YoGraphicsListRegistry yoGraphicsListRegistry,
                                           double omega0)
    {
-      this.totalMass = TotalMassCalculator.computeSubTreeMass(elevator);
+      this.totalMass = MultiBodySystemMissingTools.computeSubTreeMass(elevator);
       this.gravityZ = gravityZ;
       this.yoTime = yoTime;
 

diff --git a/.../java/us/ihmc/commonWalkingControlModules/controllerCore/WholeBodyControlCoreToolbox.java b/.../java/us/ihmc/commonWalkingControlModules/controllerCore/WholeBodyControlCoreToolbox.java
@@ -19,11 +19,11 @@
 import us.ihmc.mecano.algorithms.interfaces.RigidBodyAccelerationProvider;
 import us.ihmc.mecano.frames.CenterOfMassReferenceFrame;
 import us.ihmc.mecano.multiBodySystem.interfaces.*;
+import us.ihmc.robotics.MultiBodySystemMissingTools;
 import us.ihmc.robotics.SCS2YoGraphicHolder;
 import us.ihmc.robotics.contactable.ContactablePlaneBody;
 import us.ihmc.robotics.screwTheory.GravityCoriolisExternalWrenchMatrixCalculator;
 import us.ihmc.robotics.screwTheory.RigidBodyTwistCalculator;
-import us.ihmc.robotics.screwTheory.TotalMassCalculator;
 import us.ihmc.scs2.definition.yoGraphic.YoGraphicDefinition;
 import us.ihmc.scs2.definition.yoGraphic.YoGraphicGroupDefinition;
 import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D;
@@ -184,7 +184,7 @@ public WholeBodyControlCoreToolbox(double controlDT,
       rigidBodyAccelerationProvider = inverseDynamicsCalculator.getAccelerationProvider();
       rigidBodyTwistCalculator = new RigidBodyTwistCalculator(multiBodySystemInput);
 
-      totalMassProvider = () -> TotalMassCalculator.computeSubTreeMass(multiBodySystemInput.getRootBody());
+      totalMassProvider = () -> MultiBodySystemMissingTools.computeSubTreeMass(multiBodySystemInput.getRootBody());
 
       parentRegistry.addChild(registry);
    }

diff --git a/...ControlModules/highLevelHumanoidControl/factories/HighLevelHumanoidControllerFactory.java b/...ControlModules/highLevelHumanoidControl/factories/HighLevelHumanoidControllerFactory.java
@@ -51,12 +51,12 @@
 import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics;
 import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
 import us.ihmc.robotModels.FullHumanoidRobotModel;
+import us.ihmc.robotics.MultiBodySystemMissingTools;
 import us.ihmc.robotics.contactable.ContactablePlaneBody;
 import us.ihmc.robotics.controllers.ControllerFailureListener;
 import us.ihmc.robotics.controllers.ControllerStateChangedListener;
 import us.ihmc.robotics.robotSide.RobotSide;
 import us.ihmc.robotics.robotSide.SideDependentList;
-import us.ihmc.robotics.screwTheory.TotalMassCalculator;
 import us.ihmc.robotics.sensors.CenterOfMassDataHolderReadOnly;
 import us.ihmc.robotics.sensors.FootSwitchFactory;
 import us.ihmc.robotics.sensors.FootSwitchInterface;
@@ -507,7 +507,7 @@ public FrameVector3DReadOnly getCenterOfMassVelocity()
       contactablePlaneBodies.addAll(additionalContacts);
 
       double gravityZ = Math.abs(gravity);
-      double totalMass = TotalMassCalculator.computeSubTreeMass(fullRobotModel.getElevator());
+      double totalMass = MultiBodySystemMissingTools.computeSubTreeMass(fullRobotModel.getElevator());
       double totalRobotWeight = totalMass * gravityZ;
 
       SideDependentList<FootSwitchInterface> footSwitches = createFootSwitches(feet,

diff --git a/...mmonWalkingControlModules/momentumBasedController/HighLevelHumanoidControllerToolbox.java b/...mmonWalkingControlModules/momentumBasedController/HighLevelHumanoidControllerToolbox.java
@@ -40,6 +40,7 @@
 import us.ihmc.mecano.spatial.Wrench;
 import us.ihmc.mecano.tools.MultiBodySystemTools;
 import us.ihmc.robotModels.FullHumanoidRobotModel;
+import us.ihmc.robotics.MultiBodySystemMissingTools;
 import us.ihmc.robotics.SCS2YoGraphicHolder;
 import us.ihmc.robotics.contactable.ContactablePlaneBody;
 import us.ihmc.robotics.controllers.ControllerFailureListener;
@@ -50,7 +51,6 @@
 import us.ihmc.robotics.robotSide.RobotSide;
 import us.ihmc.robotics.robotSide.SideDependentList;
 import us.ihmc.robotics.screwTheory.AngularExcursionCalculator;
-import us.ihmc.robotics.screwTheory.TotalMassCalculator;
 import us.ihmc.robotics.sensors.FootSwitchInterface;
 import us.ihmc.robotics.sensors.ForceSensorDataReadOnly;
 import us.ihmc.robotics.time.ExecutionTimer;
@@ -235,7 +235,7 @@ public HighLevelHumanoidControllerToolbox(FullHumanoidRobotModel fullRobotModel,
       this.contactableBodies = contactableBodies;
 
       RigidBodyBasics elevator = fullRobotModel.getElevator();
-      this.totalMass.set(TotalMassCalculator.computeSubTreeMass(elevator));
+      this.totalMass.set(MultiBodySystemMissingTools.computeSubTreeMass(elevator));
 
       for (RobotSide robotSide : RobotSide.values)
       {
@@ -344,7 +344,7 @@ public HighLevelHumanoidControllerToolbox(FullHumanoidRobotModel fullRobotModel,
             handCenterOfMassFrames.put(robotSide, handCoMFrame);
             YoDouble handMass = new YoDouble(sidePrefix + "HandTotalMass", registry);
             handsMass.put(robotSide, handMass);
-            handMass.set(TotalMassCalculator.computeSubTreeMass(measurementLink));
+            handMass.set(MultiBodySystemMissingTools.computeSubTreeMass(measurementLink));
          }
       }
 
@@ -421,7 +421,7 @@ public SideDependentList<YoPlaneContactState> getFootContactStates()
 
    public void update()
    {
-      totalMass.set(TotalMassCalculator.computeSubTreeMass(fullRobotModel.getElevator()));
+      totalMass.set(MultiBodySystemMissingTools.computeSubTreeMass(fullRobotModel.getElevator()));
 
       centerOfMassStateProvider.updateState(); // Needs to be updated before the frames, as it is need to update the CoM frame.
       referenceFrames.updateFrames();

diff --git a/...va/us/ihmc/commonWalkingControlModules/sensors/footSwitch/JointTorqueBasedFootSwitch.java b/...va/us/ihmc/commonWalkingControlModules/sensors/footSwitch/JointTorqueBasedFootSwitch.java
@@ -18,10 +18,10 @@
 import us.ihmc.mecano.spatial.interfaces.WrenchReadOnly;
 import us.ihmc.mecano.tools.MultiBodySystemTools;
 import us.ihmc.mecano.yoVariables.spatial.YoFixedFrameWrench;
+import us.ihmc.robotics.MultiBodySystemMissingTools;
 import us.ihmc.robotics.math.filters.AlphaFilteredYoVariable;
 import us.ihmc.robotics.math.filters.GlitchFilteredYoBoolean;
 import us.ihmc.robotics.screwTheory.GeometricJacobian;
-import us.ihmc.robotics.screwTheory.TotalMassCalculator;
 import us.ihmc.robotics.sensors.FootSwitchInterface;
 import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint2D;
 import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D;
@@ -94,7 +94,7 @@ public JointTorqueBasedFootSwitch(String namePrefix,
       wrenchDetector = new JacobianBasedBasedTouchdownDetector(foot,
                                                                rootBody,
                                                                soleFrame,
-                                                               TotalMassCalculator.computeSubTreeMass(MultiBodySystemTools.getRootBody(rootBody)),
+                                                               MultiBodySystemMissingTools.computeSubTreeMass(MultiBodySystemTools.getRootBody(rootBody)),
                                                                contactForceThreshold,
                                                                contactThresholdWindowSize,
                                                                compensateGravity,

diff --git a/...rc/test/java/us/ihmc/commonWalkingControlModules/capturePoint/lqrControl/SphereRobot.java b/...rc/test/java/us/ihmc/commonWalkingControlModules/capturePoint/lqrControl/SphereRobot.java
@@ -21,7 +21,7 @@
 import us.ihmc.mecano.multiBodySystem.RigidBody;
 import us.ihmc.mecano.multiBodySystem.SixDoFJoint;
 import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
-import us.ihmc.robotics.screwTheory.TotalMassCalculator;
+import us.ihmc.robotics.MultiBodySystemMissingTools;
 import us.ihmc.simulationConstructionSetTools.tools.RobotTools.SCSRobotFromInverseDynamicsRobotModel;
 import us.ihmc.simulationconstructionset.ExternalForcePoint;
 import us.ihmc.simulationconstructionset.GroundContactPoint;
@@ -126,7 +126,7 @@ public SphereRobot(String name, double gravity, double controlDt, double desired
       scsRobot.addYoGraphicsListRegistry(yoGraphicsListRegistry);
       scsRobot.update();
 
-      totalMass = TotalMassCalculator.computeSubTreeMass(body);
+      totalMass = MultiBodySystemMissingTools.computeSubTreeMass(body);
    }
 
    public SCSRobotFromInverseDynamicsRobotModel getScsRobot()