diff --git a/scs2-simulation/src/main/java/us/ihmc/scs2/simulation/physicsEngine/YoMatrix.java b/scs2-simulation/src/main/java/us/ihmc/scs2/simulation/physicsEngine/YoMatrix.java
index 6545d44f..c7d376f0 100644
--- a/scs2-simulation/src/main/java/us/ihmc/scs2/simulation/physicsEngine/YoMatrix.java
+++ b/scs2-simulation/src/main/java/us/ihmc/scs2/simulation/physicsEngine/YoMatrix.java
@@ -10,6 +10,10 @@
 import us.ihmc.yoVariables.variable.YoDouble;
 import us.ihmc.yoVariables.variable.YoInteger;
 
+/**
+ * @deprecated please use {@link us.ihmc.yoVariables.math.YoMatrix}
+ */
+@Deprecated
 public class YoMatrix implements DMatrix, ReshapeMatrix
 {
    // TODO: eventually consolidate YoMatrix implementations
diff --git a/scs2-simulation/src/main/java/us/ihmc/scs2/simulation/physicsEngine/contactPointBased/ContactPointBasedRobotPhysics.java b/scs2-simulation/src/main/java/us/ihmc/scs2/simulation/physicsEngine/contactPointBased/ContactPointBasedRobotPhysics.java
index 479d5460..af7d460e 100644
--- a/scs2-simulation/src/main/java/us/ihmc/scs2/simulation/physicsEngine/contactPointBased/ContactPointBasedRobotPhysics.java
+++ b/scs2-simulation/src/main/java/us/ihmc/scs2/simulation/physicsEngine/contactPointBased/ContactPointBasedRobotPhysics.java
@@ -12,7 +12,7 @@
 import us.ihmc.scs2.simulation.RobotJointWrenchCalculator;
 import us.ihmc.scs2.simulation.collision.Collidable;
 import us.ihmc.scs2.simulation.collision.FrameShapePosePredictor;
-import us.ihmc.scs2.simulation.physicsEngine.YoMatrix;
+import us.ihmc.yoVariables.math.YoMatrix;
 import us.ihmc.scs2.simulation.robot.RobotInterface;
 import us.ihmc.scs2.simulation.robot.RobotPhysicsOutput;
 import us.ihmc.scs2.simulation.robot.controller.RobotOneDoFJointDampingCalculator;
@@ -192,6 +192,6 @@ public RobotPhysicsOutput getPhysicsOutput()
    private void sumJointTauContributions()
    {
       MultiBodySystemTools.extractJointsState(owner.getJointsToConsider(), JointStateType.EFFORT, jointsTau);
-      jointsTau.add(jointsTauLowLevelController);
+      jointsTau.addEquals(jointsTauLowLevelController);
    }
 }
diff --git a/scs2-simulation/src/main/java/us/ihmc/scs2/simulation/physicsEngine/impulseBased/ImpulseBasedRobotPhysics.java b/scs2-simulation/src/main/java/us/ihmc/scs2/simulation/physicsEngine/impulseBased/ImpulseBasedRobotPhysics.java
index 440d6910..05fc9c8d 100644
--- a/scs2-simulation/src/main/java/us/ihmc/scs2/simulation/physicsEngine/impulseBased/ImpulseBasedRobotPhysics.java
+++ b/scs2-simulation/src/main/java/us/ihmc/scs2/simulation/physicsEngine/impulseBased/ImpulseBasedRobotPhysics.java
@@ -16,7 +16,7 @@
 import us.ihmc.scs2.simulation.RobotJointWrenchCalculator;
 import us.ihmc.scs2.simulation.collision.Collidable;
 import us.ihmc.scs2.simulation.collision.FrameShapePosePredictor;
-import us.ihmc.scs2.simulation.physicsEngine.YoMatrix;
+import us.ihmc.yoVariables.math.YoMatrix;
 import us.ihmc.scs2.simulation.robot.RobotInterface;
 import us.ihmc.scs2.simulation.robot.RobotPhysicsOutput;
 import us.ihmc.scs2.simulation.robot.controller.RobotOneDoFJointDampingCalculator;
@@ -310,6 +310,6 @@ public RigidBodyTwistProvider getRigidBodyTwistChangeProvider()
    private void sumJointTauContributions()
    {
       MultiBodySystemTools.extractJointsState(owner.getJointsToConsider(), JointStateType.EFFORT, jointsTau);
-      jointsTau.add(jointsTauLowLevelController);
+      jointsTau.addEquals(jointsTauLowLevelController);
    }
 }
diff --git a/scs2-simulation/src/main/java/us/ihmc/scs2/simulation/physicsEngine/impulseBased/YoSingleContactImpulseCalculator.java b/scs2-simulation/src/main/java/us/ihmc/scs2/simulation/physicsEngine/impulseBased/YoSingleContactImpulseCalculator.java
index b5f3b187..e75ec8a8 100644
--- a/scs2-simulation/src/main/java/us/ihmc/scs2/simulation/physicsEngine/impulseBased/YoSingleContactImpulseCalculator.java
+++ b/scs2-simulation/src/main/java/us/ihmc/scs2/simulation/physicsEngine/impulseBased/YoSingleContactImpulseCalculator.java
@@ -18,7 +18,7 @@
 import us.ihmc.mecano.yoVariables.spatial.YoFixedFrameSpatialVector;
 import us.ihmc.mecano.yoVariables.spatial.YoFixedFrameTwist;
 import us.ihmc.scs2.simulation.collision.CollisionResult;
-import us.ihmc.scs2.simulation.physicsEngine.YoMatrix;
+import us.ihmc.yoVariables.math.YoMatrix;
 import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint3D;
 import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D;
 import us.ihmc.yoVariables.registry.YoRegistry;
diff --git a/scs2-simulation/src/main/java/us/ihmc/scs2/simulation/screwTools/SimMultiBodySystemTools.java b/scs2-simulation/src/main/java/us/ihmc/scs2/simulation/screwTools/SimMultiBodySystemTools.java
index 8fee4cd6..5b897244 100644
--- a/scs2-simulation/src/main/java/us/ihmc/scs2/simulation/screwTools/SimMultiBodySystemTools.java
+++ b/scs2-simulation/src/main/java/us/ihmc/scs2/simulation/screwTools/SimMultiBodySystemTools.java
@@ -5,7 +5,7 @@
 import us.ihmc.mecano.multiBodySystem.interfaces.*;
 import us.ihmc.mecano.multiBodySystem.iterators.SubtreeStreams;
 import us.ihmc.mecano.tools.MultiBodySystemTools;
-import us.ihmc.scs2.simulation.physicsEngine.YoMatrix;
+import us.ihmc.yoVariables.math.YoMatrix;
 import us.ihmc.scs2.simulation.robot.multiBodySystem.interfaces.*;
 import us.ihmc.yoVariables.registry.YoRegistry;