diff --git a/IHMCQuadrupedRobotics/src/us/ihmc/quadrupedRobotics/estimator/stateEstimator/QuadrupedFootContactableBodiesFactory.java b/IHMCQuadrupedRobotics/src/us/ihmc/quadrupedRobotics/estimator/stateEstimator/QuadrupedFootContactableBodiesFactory.java
new file mode 100644
index 00000000000..09228f444ce
--- /dev/null
+++ b/IHMCQuadrupedRobotics/src/us/ihmc/quadrupedRobotics/estimator/stateEstimator/QuadrupedFootContactableBodiesFactory.java
@@ -0,0 +1,52 @@
+package us.ihmc.quadrupedRobotics.estimator.stateEstimator;
+
+import us.ihmc.SdfLoader.SDFFullQuadrupedRobotModel;
+import us.ihmc.commonWalkingControlModules.bipedSupportPolygons.ListOfPointsContactablePlaneBody;
+import us.ihmc.humanoidRobotics.bipedSupportPolygons.ContactablePlaneBody;
+import us.ihmc.quadrupedRobotics.estimator.referenceFrames.CommonQuadrupedReferenceFrames;
+import us.ihmc.quadrupedRobotics.model.QuadrupedPhysicalProperties;
+import us.ihmc.robotics.robotSide.QuadrantDependentList;
+import us.ihmc.robotics.robotSide.RobotQuadrant;
+import us.ihmc.robotics.screwTheory.RigidBody;
+import us.ihmc.tools.factories.FactoryTools;
+import us.ihmc.tools.factories.RequiredFactoryField;
+
+public class QuadrupedFootContactableBodiesFactory
+{
+   private final RequiredFactoryField<SDFFullQuadrupedRobotModel> fullRobotModel = new RequiredFactoryField<>("fullRobotModel");
+   private final RequiredFactoryField<CommonQuadrupedReferenceFrames> referenceFrames = new RequiredFactoryField<>("referenceFrames");
+   private final RequiredFactoryField<QuadrupedPhysicalProperties> physicalProperties = new RequiredFactoryField<>("physicalProperties");
+   
+   public QuadrantDependentList<ContactablePlaneBody> createFootContactableBodies()
+   {
+      FactoryTools.checkAllRequiredFactoryFieldsAreSet(this);
+      
+      QuadrantDependentList<ContactablePlaneBody> footContactableBodies = new QuadrantDependentList<ContactablePlaneBody>();
+
+      for (RobotQuadrant robotQuadrant : RobotQuadrant.values)
+      {
+         RigidBody foot = fullRobotModel.get().getFoot(robotQuadrant);
+         ListOfPointsContactablePlaneBody footContactableBody = new ListOfPointsContactablePlaneBody(foot, referenceFrames.get().getFootFrame(robotQuadrant),
+                                                                                                     physicalProperties.get()
+                                                                                                                       .getFootGroundContactPoints(robotQuadrant));
+         footContactableBodies.set(robotQuadrant, footContactableBody);
+      }
+
+      return footContactableBodies;
+   }
+   
+   public void setFullRobotModel(SDFFullQuadrupedRobotModel fullRobotModel)
+   {
+      this.fullRobotModel.set(fullRobotModel);
+   }
+   
+   public void setReferenceFrames(CommonQuadrupedReferenceFrames referenceFrames)
+   {
+      this.referenceFrames.set(referenceFrames);
+   }
+   
+   public void setPhysicalProperties(QuadrupedPhysicalProperties physicalProperties)
+   {
+      this.physicalProperties.set(physicalProperties);
+   }
+}
diff --git a/IHMCQuadrupedRobotics/src/us/ihmc/quadrupedRobotics/estimator/stateEstimator/QuadrupedFootSwitchFactory.java b/IHMCQuadrupedRobotics/src/us/ihmc/quadrupedRobotics/estimator/stateEstimator/QuadrupedFootSwitchFactory.java
new file mode 100644
index 00000000000..9e66a394c1e
--- /dev/null
+++ b/IHMCQuadrupedRobotics/src/us/ihmc/quadrupedRobotics/estimator/stateEstimator/QuadrupedFootSwitchFactory.java
@@ -0,0 +1,58 @@
+package us.ihmc.quadrupedRobotics.estimator.stateEstimator;
+
+import us.ihmc.SdfLoader.SDFFullRobotModel;
+import us.ihmc.commonWalkingControlModules.sensors.footSwitch.FootSwitchInterface;
+import us.ihmc.commonWalkingControlModules.sensors.footSwitch.SettableFootSwitch;
+import us.ihmc.humanoidRobotics.bipedSupportPolygons.ContactablePlaneBody;
+import us.ihmc.robotics.dataStructures.registry.YoVariableRegistry;
+import us.ihmc.robotics.robotSide.QuadrantDependentList;
+import us.ihmc.robotics.robotSide.RobotQuadrant;
+import us.ihmc.robotics.screwTheory.TotalMassCalculator;
+import us.ihmc.tools.factories.FactoryTools;
+import us.ihmc.tools.factories.RequiredFactoryField;
+
+public class QuadrupedFootSwitchFactory
+{
+   private final RequiredFactoryField<Double> gravity = new RequiredFactoryField<>("gravity");
+   private final RequiredFactoryField<YoVariableRegistry> yoVariableRegistry = new RequiredFactoryField<>("yoVariableRegistry");
+   private final RequiredFactoryField<QuadrantDependentList<ContactablePlaneBody>> footContactableBodies = new RequiredFactoryField<>("footContactableBodies");
+   private final RequiredFactoryField<SDFFullRobotModel> fullRobotModel = new RequiredFactoryField<>("fullRobotModel");
+   
+   public QuadrantDependentList<FootSwitchInterface> createFootSwitches()
+   {
+      FactoryTools.checkAllRequiredFactoryFieldsAreSet(this);
+      
+      QuadrantDependentList<FootSwitchInterface> footSwitches = new QuadrantDependentList<FootSwitchInterface>();
+      double gravityMagnitude = Math.abs(gravity.get());
+      double totalRobotWeight = TotalMassCalculator.computeSubTreeMass(fullRobotModel.get().getElevator()) * gravityMagnitude;
+
+      for (RobotQuadrant robotQuadrant : RobotQuadrant.values)
+      {
+         SettableFootSwitch footSwitch = new SettableFootSwitch(footContactableBodies.get().get(robotQuadrant), robotQuadrant, totalRobotWeight, yoVariableRegistry.get());
+         // KinematicsBasedFootSwitch footSwitch = new KinematicsBasedFootSwitch(namePrefix, footContactableBodies.get(), switchZThreshold, totalRobotWeight, robotQuadrant, yoVariableRegistry.get());
+         footSwitches.set(robotQuadrant, footSwitch);
+      }
+      
+      return footSwitches;
+   }
+   
+   public void setGravity(double gravity)
+   {
+      this.gravity.set(gravity);
+   }
+   
+   public void setYoVariableRegistry(YoVariableRegistry yoVariableRegistry)
+   {
+      this.yoVariableRegistry.set(yoVariableRegistry);
+   }
+   
+   public void setFootContactableBodies(QuadrantDependentList<ContactablePlaneBody> footContactableBodies)
+   {
+      this.footContactableBodies.set(footContactableBodies);
+   }
+   
+   public void setFullRobotModel(SDFFullRobotModel fullRobotModel)
+   {
+      this.fullRobotModel.set(fullRobotModel);
+   }
+}
diff --git a/IHMCQuadrupedRobotics/src/us/ihmc/quadrupedRobotics/estimator/stateEstimator/QuadrupedStateEstimatorFactory.java b/IHMCQuadrupedRobotics/src/us/ihmc/quadrupedRobotics/estimator/stateEstimator/QuadrupedStateEstimatorFactory.java
index 245af057376..821b1596494 100644
--- a/IHMCQuadrupedRobotics/src/us/ihmc/quadrupedRobotics/estimator/stateEstimator/QuadrupedStateEstimatorFactory.java
+++ b/IHMCQuadrupedRobotics/src/us/ihmc/quadrupedRobotics/estimator/stateEstimator/QuadrupedStateEstimatorFactory.java
@@ -3,20 +3,15 @@
 import java.util.HashMap;
 import java.util.Map;
 
-import us.ihmc.SdfLoader.SDFFullQuadrupedRobotModel;
 import us.ihmc.SdfLoader.SDFFullRobotModel;
-import us.ihmc.quadrupedRobotics.model.QuadrupedPhysicalProperties;
-import us.ihmc.commonWalkingControlModules.bipedSupportPolygons.ListOfPointsContactablePlaneBody;
 import us.ihmc.commonWalkingControlModules.sensors.footSwitch.FootSwitchInterface;
-import us.ihmc.commonWalkingControlModules.sensors.footSwitch.SettableFootSwitch;
 import us.ihmc.humanoidRobotics.bipedSupportPolygons.ContactablePlaneBody;
-import us.ihmc.quadrupedRobotics.estimator.referenceFrames.CommonQuadrupedReferenceFrames;
+import us.ihmc.humanoidRobotics.model.CenterOfPressureDataHolder;
 import us.ihmc.robotics.dataStructures.registry.YoVariableRegistry;
 import us.ihmc.robotics.robotSide.QuadrantDependentList;
 import us.ihmc.robotics.robotSide.RobotQuadrant;
 import us.ihmc.robotics.screwTheory.RigidBody;
 import us.ihmc.robotics.screwTheory.SixDoFJoint;
-import us.ihmc.robotics.screwTheory.TotalMassCalculator;
 import us.ihmc.robotics.sensors.ForceSensorDataHolder;
 import us.ihmc.sensorProcessing.model.RobotMotionStatus;
 import us.ihmc.sensorProcessing.model.RobotMotionStatusHolder;
@@ -25,75 +20,108 @@
 import us.ihmc.sensorProcessing.stateEstimation.evaluation.FullInverseDynamicsStructure;
 import us.ihmc.simulationconstructionset.yoUtilities.graphics.YoGraphicsListRegistry;
 import us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.DRCKinematicsBasedStateEstimator;
+import us.ihmc.tools.factories.FactoryTools;
+import us.ihmc.tools.factories.RequiredFactoryField;
 
 public class QuadrupedStateEstimatorFactory
 {
-
-   private static double switchZThreshold = 0.005;
+   private final RequiredFactoryField<QuadrupedSensorInformation> sensorInformation = new RequiredFactoryField<>("sensorInformation");
+   private final RequiredFactoryField<StateEstimatorParameters> stateEstimatorParameters = new RequiredFactoryField<>("stateEstimatorParameters");
+   private final RequiredFactoryField<SDFFullRobotModel> fullRobotModel = new RequiredFactoryField<>("fullRobotModel");
+   private final RequiredFactoryField<SensorOutputMapReadOnly> sensorOutputMapReadOnly = new RequiredFactoryField<>("sensorOutputMapReadOnly");
+   private final RequiredFactoryField<QuadrantDependentList<ContactablePlaneBody>> footContactableBodies = new RequiredFactoryField<>("footContactableBodies");
+   private final RequiredFactoryField<QuadrantDependentList<FootSwitchInterface>> footSwitches = new RequiredFactoryField<>("footSwitches");
+   private final RequiredFactoryField<Double> gravity = new RequiredFactoryField<>("gravity");
+   private final RequiredFactoryField<Double> estimatorDT = new RequiredFactoryField<>("estimatorDT");
+   private final RequiredFactoryField<YoVariableRegistry> yoVariableRegistry = new RequiredFactoryField<>("yoVariableRegistry");
+   private final RequiredFactoryField<YoGraphicsListRegistry> yoGraphicsListRegistry = new RequiredFactoryField<>("yoGraphicsListRegistry");
    
-   public static DRCKinematicsBasedStateEstimator createStateEstimator(QuadrupedSensorInformation sensorInformation, StateEstimatorParameters stateEstimatorParameters, SDFFullRobotModel fullRobotModel, SensorOutputMapReadOnly sensorOutputMapReadOnly,
-         QuadrantDependentList<ContactablePlaneBody> feet, QuadrantDependentList<FootSwitchInterface> footSwitches, double gravity, double estimatorDT, YoVariableRegistry registry, YoGraphicsListRegistry yoGraphicsListRegistry)
+   public DRCKinematicsBasedStateEstimator createStateEstimator()
    {
-      RigidBody elevator = fullRobotModel.getElevator();
-      SixDoFJoint rootInverseDynamicsJoint = fullRobotModel.getRootJoint();
-      RigidBody estimationLink = fullRobotModel.getPelvis();
+      FactoryTools.checkAllRequiredFactoryFieldsAreSet(this);
+      
+      RigidBody elevator = fullRobotModel.get().getElevator();
+      SixDoFJoint rootInverseDynamicsJoint = fullRobotModel.get().getRootJoint();
+      RigidBody estimationLink = fullRobotModel.get().getPelvis();
       FullInverseDynamicsStructure inverseDynamicsStructure = new FullInverseDynamicsStructure(elevator, estimationLink, rootInverseDynamicsJoint);
 
       RobotMotionStatusHolder robotMotionStatusFromController = new RobotMotionStatusHolder();
       robotMotionStatusFromController.setCurrentRobotMotionStatus(RobotMotionStatus.IN_MOTION);
       ForceSensorDataHolder forceSensorDataHolderToUpdate = null;
+      CenterOfPressureDataHolder centerOfPressureDataHolder = null;
 
       Map<RigidBody, ContactablePlaneBody> feetMap = new HashMap<RigidBody, ContactablePlaneBody>();
       Map<RigidBody, FootSwitchInterface> footSwitchMap = new HashMap<RigidBody, FootSwitchInterface>();
-      for(RobotQuadrant quadrant : RobotQuadrant.values)
+      for (RobotQuadrant quadrant : RobotQuadrant.values)
       {
-         ContactablePlaneBody contactablePlaneBody = feet.get(quadrant);
+         ContactablePlaneBody contactablePlaneBody = footContactableBodies.get().get(quadrant);
          RigidBody rigidBody = contactablePlaneBody.getRigidBody();
          feetMap.put(rigidBody, contactablePlaneBody);
-         FootSwitchInterface footSwitch = footSwitches.get(quadrant);
+         FootSwitchInterface footSwitch = footSwitches.get().get(quadrant);
          footSwitchMap.put(rigidBody, footSwitch);
       }
-      
-      String[] imuSensorsToUseInStateEstimator = sensorInformation.getImuNames();
-      double gravityMagnitude = Math.abs(gravity);
-
-      // Create the sensor readers and state estimator here:
-      DRCKinematicsBasedStateEstimator stateEstimator = new DRCKinematicsBasedStateEstimator(inverseDynamicsStructure, stateEstimatorParameters, sensorOutputMapReadOnly, forceSensorDataHolderToUpdate, imuSensorsToUseInStateEstimator,
-            gravityMagnitude, footSwitchMap, null, robotMotionStatusFromController, feetMap, yoGraphicsListRegistry);
-
-      registry.addChild(stateEstimator.getYoVariableRegistry());
 
+      String[] imuSensorsToUseInStateEstimator = sensorInformation.get().getImuNames();
+      double gravityMagnitude = Math.abs(gravity.get());
+      
+      DRCKinematicsBasedStateEstimator stateEstimator = new DRCKinematicsBasedStateEstimator(inverseDynamicsStructure, stateEstimatorParameters.get(),
+                                                                                             sensorOutputMapReadOnly.get(), forceSensorDataHolderToUpdate,
+                                                                                             imuSensorsToUseInStateEstimator, gravityMagnitude, footSwitchMap,
+                                                                                             centerOfPressureDataHolder , robotMotionStatusFromController, feetMap,
+                                                                                             yoGraphicsListRegistry.get());
+      
+      yoVariableRegistry.get().addChild(stateEstimator.getYoVariableRegistry());
+      
       return stateEstimator;
    }
-
-   public static QuadrantDependentList<FootSwitchInterface> createFootSwitches(QuadrantDependentList<ContactablePlaneBody> quadrupedFeet, double gravity, SDFFullRobotModel fullRobotModel, YoVariableRegistry registry)
+   
+   public void setSensorInformation(QuadrupedSensorInformation sensorInformation)
    {
-      QuadrantDependentList<FootSwitchInterface> footSwitches = new QuadrantDependentList<FootSwitchInterface>();
-      double gravityMagnitude = Math.abs(gravity);
-      double totalRobotWeight = TotalMassCalculator.computeSubTreeMass(fullRobotModel.getElevator()) * gravityMagnitude;
-
-      for (RobotQuadrant robotQuadrant : RobotQuadrant.values)
-      {
-         ContactablePlaneBody contactablePlaneBody = quadrupedFeet.get(robotQuadrant);
-         String namePrefix = contactablePlaneBody.getName() + "StateEstimator";
-         SettableFootSwitch footSwitch = new SettableFootSwitch(quadrupedFeet.get(robotQuadrant), robotQuadrant, totalRobotWeight, registry);
-//         KinematicsBasedFootSwitch footSwitch = new KinematicsBasedFootSwitch(namePrefix, quadrupedFeet, switchZThreshold, totalRobotWeight, robotQuadrant, registry);
-         footSwitches.set(robotQuadrant, footSwitch);
-      }
-      return footSwitches;
+      this.sensorInformation.set(sensorInformation);
    }
-
-   public static QuadrantDependentList<ContactablePlaneBody> createFootContactableBodies(SDFFullQuadrupedRobotModel fullRobotModel, CommonQuadrupedReferenceFrames referenceFrames,
-         QuadrupedPhysicalProperties quadrupedPhysicalProperties)
+   
+   public void setStateEstimatorParameters(StateEstimatorParameters stateEstimatorParameters)
    {
-      QuadrantDependentList<ContactablePlaneBody> footContactableBodies = new QuadrantDependentList<ContactablePlaneBody>();
-
-      for (RobotQuadrant robotQuadrant : RobotQuadrant.values)
-      {
-         RigidBody foot = fullRobotModel.getFoot(robotQuadrant);
-         ListOfPointsContactablePlaneBody footContactableBody = new ListOfPointsContactablePlaneBody(foot, referenceFrames.getFootFrame(robotQuadrant), quadrupedPhysicalProperties.getFootGroundContactPoints(robotQuadrant));
-         footContactableBodies.set(robotQuadrant, footContactableBody);
-      }
-      return footContactableBodies;
+      this.stateEstimatorParameters.set(stateEstimatorParameters);
+   }
+   
+   public void setFullRobotModel(SDFFullRobotModel fullRobotModel)
+   {
+      this.fullRobotModel.set(fullRobotModel);
+   }
+   
+   public void setSensorOutputMapReadOnly(SensorOutputMapReadOnly sensorOutputMapReadOnly)
+   {
+      this.sensorOutputMapReadOnly.set(sensorOutputMapReadOnly);
+   }
+   
+   public void setFootContactableBodies(QuadrantDependentList<ContactablePlaneBody> footContactableBodies)
+   {
+      this.footContactableBodies.set(footContactableBodies);
+   }
+   
+   public void setFootSwitches(QuadrantDependentList<FootSwitchInterface> footSwitches)
+   {
+      this.footSwitches.set(footSwitches);
+   }
+   
+   public void setGravity(double gravity)
+   {
+      this.gravity.set(gravity);
+   }
+   
+   public void setEstimatorDT(double estimatorDT)
+   {
+      this.estimatorDT.set(estimatorDT);
+   }
+   
+   public void setYoVariableRegistry(YoVariableRegistry yoVariableRegistry)
+   {
+      this.yoVariableRegistry.set(yoVariableRegistry);
+   }
+   
+   public void setYoGraphicsListRegistry(YoGraphicsListRegistry yoGraphicsListRegistry)
+   {
+      this.yoGraphicsListRegistry.set(yoGraphicsListRegistry);
    }
 }
diff --git a/IHMCQuadrupedRobotics/src/us/ihmc/quadrupedRobotics/factories/QuadrupedSimulationFactory.java b/IHMCQuadrupedRobotics/src/us/ihmc/quadrupedRobotics/factories/QuadrupedSimulationFactory.java
index adfdfa307a9..4b4c2f72b4c 100644
--- a/IHMCQuadrupedRobotics/src/us/ihmc/quadrupedRobotics/factories/QuadrupedSimulationFactory.java
+++ b/IHMCQuadrupedRobotics/src/us/ihmc/quadrupedRobotics/factories/QuadrupedSimulationFactory.java
@@ -28,6 +28,8 @@
 import us.ihmc.quadrupedRobotics.controller.positionDevelopment.QuadrupedPositionDevelopmentControllerManager;
 import us.ihmc.quadrupedRobotics.estimator.referenceFrames.QuadrupedReferenceFrames;
 import us.ihmc.quadrupedRobotics.estimator.sensorProcessing.simulatedSensors.SDFQuadrupedPerfectSimulatedSensor;
+import us.ihmc.quadrupedRobotics.estimator.stateEstimator.QuadrupedFootContactableBodiesFactory;
+import us.ihmc.quadrupedRobotics.estimator.stateEstimator.QuadrupedFootSwitchFactory;
 import us.ihmc.quadrupedRobotics.estimator.stateEstimator.QuadrupedSensorInformation;
 import us.ihmc.quadrupedRobotics.estimator.stateEstimator.QuadrupedStateEstimatorFactory;
 import us.ihmc.quadrupedRobotics.mechanics.inverseKinematics.QuadrupedInverseKinematicsCalculators;
@@ -153,21 +155,39 @@ private void createSensorReader()
    
    private void createContactibleFeet()
    {
-      contactableFeet = QuadrupedStateEstimatorFactory.createFootContactableBodies(fullRobotModel.get(), referenceFrames.get(), physicalProperties.get());
+      QuadrupedFootContactableBodiesFactory footContactableBodiesFactory = new QuadrupedFootContactableBodiesFactory();
+      footContactableBodiesFactory.setFullRobotModel(fullRobotModel.get());
+      footContactableBodiesFactory.setPhysicalProperties(physicalProperties.get());
+      footContactableBodiesFactory.setReferenceFrames(referenceFrames.get());
+      contactableFeet = footContactableBodiesFactory.createFootContactableBodies();
    }
    
    private void createFootSwitches()
    {
-      footSwitches = QuadrupedStateEstimatorFactory.createFootSwitches(contactableFeet, gravity.get(), fullRobotModel.get(), sdfRobot.get().getRobotsYoVariableRegistry());
+      QuadrupedFootSwitchFactory footSwitchFactory = new QuadrupedFootSwitchFactory();
+      footSwitchFactory.setFootContactableBodies(contactableFeet);
+      footSwitchFactory.setFullRobotModel(fullRobotModel.get());
+      footSwitchFactory.setGravity(gravity.get());
+      footSwitchFactory.setYoVariableRegistry(sdfRobot.get().getRobotsYoVariableRegistry());
+      footSwitches = footSwitchFactory.createFootSwitches();
    }
    
    private void createStateEstimator()
    {
       if (useStateEstimator.get())
       {
-         stateEstimator = QuadrupedStateEstimatorFactory
-               .createStateEstimator(sensorInformation.get(), stateEstimatorParameters.get(), fullRobotModel.get(), sensorReader.getSensorOutputMapReadOnly(), contactableFeet,
-                                     footSwitches, gravity.get(), controlDT.get(), sdfRobot.get().getRobotsYoVariableRegistry(), yoGraphicsListRegistry);
+         QuadrupedStateEstimatorFactory stateEstimatorFactory = new QuadrupedStateEstimatorFactory();
+         stateEstimatorFactory.setEstimatorDT(controlDT.get());
+         stateEstimatorFactory.setFootContactableBodies(contactableFeet);
+         stateEstimatorFactory.setFootSwitches(footSwitches);
+         stateEstimatorFactory.setFullRobotModel(fullRobotModel.get());
+         stateEstimatorFactory.setGravity(gravity.get());
+         stateEstimatorFactory.setSensorInformation(sensorInformation.get());
+         stateEstimatorFactory.setSensorOutputMapReadOnly(sensorReader.getSensorOutputMapReadOnly());
+         stateEstimatorFactory.setStateEstimatorParameters(stateEstimatorParameters.get());
+         stateEstimatorFactory.setYoGraphicsListRegistry(yoGraphicsListRegistry);
+         stateEstimatorFactory.setYoVariableRegistry(sdfRobot.get().getRobotsYoVariableRegistry());
+         stateEstimator = stateEstimatorFactory.createStateEstimator();
       }
       else
       {
diff --git a/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/AbstractInverseDynamicsJoint.java b/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/AbstractInverseDynamicsJoint.java
index bb4403626a9..a7bed77100d 100644
--- a/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/AbstractInverseDynamicsJoint.java
+++ b/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/AbstractInverseDynamicsJoint.java
@@ -24,31 +24,37 @@ public AbstractInverseDynamicsJoint(String name, RigidBody predecessor, Referenc
       predecessor.addChildJoint(this);
    }
 
+   @Override
    public final ReferenceFrame getFrameBeforeJoint()
    {
       return beforeJointFrame;
    }
 
+   @Override
    public final GeometricJacobian getMotionSubspace()
    {
       return motionSubspace;
    }
 
+   @Override
    public final RigidBody getPredecessor()
    {
       return predecessor;
    }
 
+   @Override
    public final RigidBody getSuccessor()
    {
       return successor;
    }
 
+   @Override
    public final String getName()
    {
       return name;
    }
 
+   @Override
    public final void updateFramesRecursively()
    {
       getFrameAfterJoint().update();
@@ -59,6 +65,7 @@ public final void updateFramesRecursively()
       }
    }
 
+   @Override
    public void getSuccessorTwist(Twist twistToPack)
    {
       getJointTwist(twistToPack);
@@ -71,6 +78,7 @@ public void getSuccessorTwist(Twist twistToPack)
       twistToPack.changeFrame(successorFrame);
    }
 
+   @Override
    public void getPredecessorTwist(Twist twistToPack)
    {
       getJointTwist(twistToPack);
@@ -84,6 +92,7 @@ public void getPredecessorTwist(Twist twistToPack)
       twistToPack.changeFrame(predecessorFrame);
    }
 
+   @Override
    public void getSuccessorAcceleration(SpatialAccelerationVector accelerationToPack)
    {
       getJointAcceleration(accelerationToPack);
@@ -96,6 +105,7 @@ public void getSuccessorAcceleration(SpatialAccelerationVector accelerationToPac
       accelerationToPack.changeFrameNoRelativeMotion(successorFrame);
    }
 
+   @Override
    public void getDesiredSuccessorAcceleration(SpatialAccelerationVector accelerationToPack)
    {
       getDesiredJointAcceleration(accelerationToPack);
@@ -108,6 +118,7 @@ public void getDesiredSuccessorAcceleration(SpatialAccelerationVector accelerati
       accelerationToPack.changeFrameNoRelativeMotion(successorFrame);
    }
 
+   @Override
    public void getDesiredPredecessorAcceleration(SpatialAccelerationVector accelerationToPack)
    {
       getDesiredJointAcceleration(accelerationToPack);
@@ -122,11 +133,13 @@ public void getDesiredPredecessorAcceleration(SpatialAccelerationVector accelera
    }
 
    
+   @Override
    public RigidBodyTransform getOffsetTransform3D()
    {
       return getFrameBeforeJoint().getTransformToParent();
    }
    
+   @Override
    public RigidBodyTransform getJointTransform3D()
    {
       return getFrameAfterJoint().getTransformToParent();
diff --git a/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/CentroidalMomentumRateADotVTerm.java b/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/CentroidalMomentumRateADotVTerm.java
index 6f20a7ff8ee..2647b43b52c 100644
--- a/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/CentroidalMomentumRateADotVTerm.java
+++ b/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/CentroidalMomentumRateADotVTerm.java
@@ -18,8 +18,6 @@ public class CentroidalMomentumRateADotVTerm
    private final CentroidalMomentumMatrix centroidalMomentumMatrix;
    private final RigidBody rootBody;
    private final DenseMatrix64F v;
-   private final DenseMatrix64F tmpOmegaSkew;
-   private final DenseMatrix64F tmpVelSkew;
    private final double robotMass;
    private final DenseMatrix64F aDotV;
 
@@ -53,8 +51,6 @@ public CentroidalMomentumRateADotVTerm(RigidBody rootBody, ReferenceFrame center
       comTwist.setToZero();
 
       this.comVelocityVector = new Vector3d();
-      this.tmpVelSkew = new DenseMatrix64F(3, 3);
-      this.tmpOmegaSkew = new DenseMatrix64F(3, 3);
       this.aDotV = new DenseMatrix64F(6, 1);
       this.tempSpatialAcceleration = new SpatialAccelerationVector();
       this.tempMomentum = new Momentum();
@@ -126,22 +122,6 @@ public void compute()
       }
    }
 
-   /**
-    * @param twist
-    * Create a 6x6 adjoint from a twist. See Vincent Duindam's thesis, Lemma 2.8(f)
-    * Method is depracated as the computation is now done without the creation a 6x6 matrix for efficiency. 
-    */
-   @Deprecated
-   private void createAdjointFromTwist(Twist twist, DenseMatrix64F matrixToPack)
-   {
-      MatrixTools.vectorToSkewSymmetricMatrix(tmpOmegaSkew, twist.getAngularPart());
-      MatrixTools.vectorToSkewSymmetricMatrix(tmpVelSkew, twist.getLinearPart());
-
-      MatrixTools.setMatrixBlock(matrixToPack, 0, 0, tmpOmegaSkew, 0, 0, 3, 3, 1.0); // top left block
-      MatrixTools.setMatrixBlock(matrixToPack, 3, 3, tmpOmegaSkew, 0, 0, 3, 3, 1.0); // bottom right block
-      MatrixTools.setMatrixBlock(matrixToPack, 3, 0, tmpVelSkew, 0, 0, 3, 3, 1.0); // bottom left block
-   }
-
    /**
     * Compute the center of mass velocity given a centroidal momentum matrix.
     * Just takes linear portion of centroidal momentum matrix(bottom three rows),
diff --git a/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/CompositeRigidBodyMassMatrixCalculator.java b/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/CompositeRigidBodyMassMatrixCalculator.java
index ed1518c3434..10d1c31d2c7 100644
--- a/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/CompositeRigidBodyMassMatrixCalculator.java
+++ b/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/CompositeRigidBodyMassMatrixCalculator.java
@@ -45,6 +45,7 @@ public CompositeRigidBodyMassMatrixCalculator(RigidBody rootBody)
       this(rootBody, new ArrayList<InverseDynamicsJoint>());
    }
    
+   @Override
    public void compute()
    {
       MatrixTools.setToZero(massMatrix);
@@ -111,11 +112,13 @@ private void buildCrbInertia(int i)
       }
    }
 
+   @Override
    public DenseMatrix64F getMassMatrix()
    {
       return massMatrix;
    }
 
+   @Override
    public InverseDynamicsJoint[] getJointsInOrder()
    {
       return jointsInOrder;
diff --git a/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java b/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java
index c32b47e14d1..593ce9151f9 100644
--- a/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java
+++ b/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java
@@ -50,6 +50,7 @@ public DifferentialIDMassMatrixCalculator(ReferenceFrame inertialFrame, RigidBod
       }
    }
 
+   @Override
    public void compute()
    {
       storeJointState();
@@ -109,11 +110,13 @@ private void restoreJointState()
       }
    }
 
+   @Override
    public DenseMatrix64F getMassMatrix()
    {
       return massMatrix;
    }
 
+   @Override
    public InverseDynamicsJoint[] getJointsInOrder()
    {
       return jointsInOrder;
diff --git a/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/GeometricJacobian.java b/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/GeometricJacobian.java
index 1317c6e05db..03003b812a8 100644
--- a/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/GeometricJacobian.java
+++ b/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/GeometricJacobian.java
@@ -271,6 +271,7 @@ private static DenseMatrix64F createJacobianMatrix(LinkedHashMap<InverseDynamics
       return new DenseMatrix64F(SpatialMotionVector.SIZE, ScrewTools.computeDegreesOfFreedom(unitTwists.keySet()));
    }
 
+   @Override
    public String toString()
    {
       StringBuilder builder = new StringBuilder();
diff --git a/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/InverseDynamicsMechanismExplorer.java b/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/InverseDynamicsMechanismExplorer.java
index 9f89d9479ec..b39f79e2031 100644
--- a/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/InverseDynamicsMechanismExplorer.java
+++ b/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/InverseDynamicsMechanismExplorer.java
@@ -129,11 +129,11 @@ private void printLinkInformation(RigidBody link, StringBuffer buffer)
 
    }
 
+   @Override
    public String toString()
    {
       StringBuffer buffer = new StringBuffer();
       getRobotInformationAsStringBuffer(buffer);
       return buffer.toString();
    }
-
 }
diff --git a/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/InverseJacobianSolver.java b/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/InverseJacobianSolver.java
index 18c47a7941f..2125fcce2b4 100644
--- a/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/InverseJacobianSolver.java
+++ b/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/InverseJacobianSolver.java
@@ -16,6 +16,7 @@ public InverseJacobianSolver(int matrixSize)
       jacobianMatrix = new DenseMatrix64F(matrixSize, matrixSize);
    }
 
+   @Override
    public void solve(DenseMatrix64F solutionToPack, DenseMatrix64F vector)
    {
       if (!solver.setA(jacobianMatrix))
@@ -23,6 +24,7 @@ public void solve(DenseMatrix64F solutionToPack, DenseMatrix64F vector)
       solver.solve(vector, solutionToPack);
    }
 
+   @Override
    public void setJacobian(DenseMatrix64F jacobianMatrix)
    {
       this.jacobianMatrix.set(jacobianMatrix);      
diff --git a/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/Momentum.java b/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/Momentum.java
index 0d00db855cc..ed377cbdb8c 100644
--- a/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/Momentum.java
+++ b/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/Momentum.java
@@ -97,6 +97,7 @@ public double computeKineticCoEnergy(Twist twist)
       return ret;
    }
    
+   @Override
    public String toString()
    {
       String ret = new String("Momentum expressed in frame " + expressedInFrame + "\n" + "Angular part: " + angularPart + "\n"
diff --git a/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/OneDoFJoint.java b/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/OneDoFJoint.java
index 9f00a4fc0a5..6d8278bc4ea 100644
--- a/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/OneDoFJoint.java
+++ b/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/OneDoFJoint.java
@@ -75,11 +75,13 @@ public OneDoFJoint(String name, RigidBody predecessor, ReferenceFrame beforeJoin
       this.afterJointFrame = afterJointFrame;
    }
 
+   @Override
    public ReferenceFrame getFrameAfterJoint()
    {
       return afterJointFrame;
    }
 
+   @Override
    public void getJointTwist(Twist twistToPack)
    {
       twistToPack.set(unitJointTwist);
@@ -100,6 +102,7 @@ public void getPredecessorTwist(Twist twistToPack)
       twistToPack.scale(qd);
    }
 
+   @Override
    public void getJointAcceleration(SpatialAccelerationVector accelerationToPack)
    {
       accelerationToPack.set(unitJointAcceleration);
@@ -113,6 +116,7 @@ public void getSuccessorAcceleration(SpatialAccelerationVector accelerationToPac
       accelerationToPack.scale(qdd);
    }
 
+   @Override
    public void getDesiredJointAcceleration(SpatialAccelerationVector accelerationToPack)
    {
       accelerationToPack.set(unitJointAcceleration);
@@ -133,6 +137,7 @@ public void getDesiredPredecessorAcceleration(SpatialAccelerationVector accelera
       accelerationToPack.scale(qddDesired);
    }
 
+   @Override
    public void getTauMatrix(DenseMatrix64F matrix)
    {
       MathTools.checkIfInRange(matrix.getNumRows(), 1, Integer.MAX_VALUE);
@@ -140,6 +145,7 @@ public void getTauMatrix(DenseMatrix64F matrix)
       matrix.set(0, 0, tau);
    }
 
+   @Override
    public void getVelocityMatrix(DenseMatrix64F matrix, int rowStart)
    {
       MathTools.checkIfInRange(matrix.getNumRows(), 1, Integer.MAX_VALUE);
@@ -147,6 +153,7 @@ public void getVelocityMatrix(DenseMatrix64F matrix, int rowStart)
       matrix.set(rowStart, 0, qd);
    }
 
+   @Override
    public void getDesiredAccelerationMatrix(DenseMatrix64F matrix, int rowStart)
    {
       MathTools.checkIfInRange(matrix.getNumRows(), 1, Integer.MAX_VALUE);
@@ -154,16 +161,19 @@ public void getDesiredAccelerationMatrix(DenseMatrix64F matrix, int rowStart)
       matrix.set(rowStart, 0, qddDesired);
    }
 
+   @Override
    public void setDesiredAccelerationToZero()
    {
       qddDesired = 0.0;
    }
 
+   @Override
    public void updateMotionSubspace()
    {
       // empty
    }
 
+   @Override
    public void setTorqueFromWrench(Wrench jointWrench)
    {
       unitSuccessorTwist.getBodyFrame().checkReferenceFrameMatch(jointWrench.getBodyFrame());
@@ -175,11 +185,13 @@ public void setTorqueFromWrench(Wrench jointWrench)
       // we disregard dimensions and just use .dot(.) for efficiency
    }
 
+   @Override
    public int getDegreesOfFreedom()
    {
       return 1;
    }
 
+   @Override
    public void setDesiredAcceleration(DenseMatrix64F matrix, int rowStart)
    {
       setQddDesired(matrix.get(rowStart + 0, 0));
@@ -257,21 +269,25 @@ protected void setMotionSubspace(Twist unitTwist)
       this.motionSubspace.compute();
    }
 
+   @Override
    public void getConfigurationMatrix(DenseMatrix64F matrix, int rowStart)
    {
       matrix.set(rowStart, q);
    }
 
+   @Override
    public void setConfiguration(DenseMatrix64F matrix, int rowStart)
    {
       setQ(matrix.get(rowStart, 0));
    }
 
+   @Override
    public void setVelocity(DenseMatrix64F matrix, int rowStart)
    {
       setQd(matrix.get(rowStart, 0));
    }
 
+   @Override
    public int getConfigurationMatrixSize()
    {
       return getDegreesOfFreedom();
@@ -289,6 +305,7 @@ public OneDoFJoint checkAndGetAsOneDoFJoint(InverseDynamicsJoint originalJoint)
       }
    }
 
+   @Override
    public void setJointPositionVelocityAndAcceleration(InverseDynamicsJoint originalJoint)
    {
       OneDoFJoint oneDoFOriginalJoint = checkAndGetAsOneDoFJoint(originalJoint);
@@ -299,6 +316,7 @@ public void setJointPositionVelocityAndAcceleration(InverseDynamicsJoint origina
       setEnabled(oneDoFOriginalJoint.isEnabled());
    }
 
+   @Override
    public void setQddDesired(InverseDynamicsJoint originalJoint)
    {
       OneDoFJoint oneDoFOriginalJoint = checkAndGetAsOneDoFJoint(originalJoint);
@@ -415,6 +433,7 @@ public void setKd(double kd)
       this.kd = kd;
    }
 
+   @Override
    public void calculateJointStateChecksum(GenericCRC32 checksum)
    {
       checksum.update(q);
@@ -422,6 +441,7 @@ public void calculateJointStateChecksum(GenericCRC32 checksum)
       checksum.update(qdd);
    }
 
+   @Override
    public void calculateJointDesiredChecksum(GenericCRC32 checksum)
    {
       checksum.update(qddDesired);
diff --git a/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/OriginalDynamicallyConsistentNullspaceCalculator.java b/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/OriginalDynamicallyConsistentNullspaceCalculator.java
index 0505e540401..21640c7778b 100644
--- a/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/OriginalDynamicallyConsistentNullspaceCalculator.java
+++ b/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/OriginalDynamicallyConsistentNullspaceCalculator.java
@@ -68,6 +68,7 @@ public OriginalDynamicallyConsistentNullspaceCalculator(SixDoFJoint rootJoint,
       this.computeSNsBar = computeSNsBar;
    }
 
+   @Override
    public void reset()
    {
       nConstraints = 0;
@@ -75,6 +76,7 @@ public void reset()
       actuatedJoints.clear();
    }
 
+   @Override
    public void addConstraint(RigidBody body, DenseMatrix64F selectionMatrix)
    {
       constrainedBodiesAndSelectionMatrices.put(body, selectionMatrix);
@@ -83,6 +85,7 @@ public void addConstraint(RigidBody body, DenseMatrix64F selectionMatrix)
       this.supportingBodyToJointPathMap.put(body, Arrays.asList(jointPath));
    }
 
+   @Override
    public void addActuatedJoint(InverseDynamicsJoint joint)
    {
       actuatedJoints.add(joint);
@@ -109,6 +112,7 @@ private static void computeS(DenseMatrix64F S, InverseDynamicsJoint[] jointsInOr
       }
    }
 
+   @Override
    public void compute()
    {
       resizeMatrices();
@@ -168,11 +172,13 @@ private void resizeMatrices()
       SNsBar.reshape(nDegreesOfFreedom, nActuatedDegreesOfFreedom);
    }
 
+   @Override
    public DenseMatrix64F getDynamicallyConsistentNullspace()
    {
       return Ns;
    }
 
+   @Override
    public DenseMatrix64F getSNsBar()
    {
       if (!computeSNsBar)
diff --git a/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/PlanarJoint.java b/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/PlanarJoint.java
index 8756071246a..9433235e388 100644
--- a/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/PlanarJoint.java
+++ b/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/PlanarJoint.java
@@ -36,11 +36,13 @@ public PlanarJoint(String name, RigidBody predecessor, ReferenceFrame beforeJoin
       this.tauTrans = new FrameVector2d(afterJointFrame);
    }
 
+   @Override
    public ReferenceFrame getFrameAfterJoint()
    {
       return afterJointFrame;
    }
 
+   @Override
    public void getJointTwist(Twist twistToPack)
    {
       twistToPack.setToZero(afterJointFrame, beforeJointFrame, afterJointFrame);
@@ -49,6 +51,7 @@ public void getJointTwist(Twist twistToPack)
       twistToPack.setLinearPartY(qdTrans.getY());
    }
 
+   @Override
    public void getJointAcceleration(SpatialAccelerationVector accelerationToPack)
    {
       accelerationToPack.setToZero(afterJointFrame, beforeJointFrame, afterJointFrame);
@@ -57,6 +60,7 @@ public void getJointAcceleration(SpatialAccelerationVector accelerationToPack)
       accelerationToPack.setLinearPartY(qddTrans.getY());
    }
 
+   @Override
    public void getDesiredJointAcceleration(SpatialAccelerationVector jointAcceleration)
    {
       jointAcceleration.setToZero(afterJointFrame, beforeJointFrame, afterJointFrame);
@@ -65,6 +69,7 @@ public void getDesiredJointAcceleration(SpatialAccelerationVector jointAccelerat
       jointAcceleration.setLinearPartY(qddTransDesired.getY());
    }
 
+   @Override
    public void getTauMatrix(DenseMatrix64F matrix)
    {
       matrix.set(0, 0, tauRot);
@@ -72,6 +77,7 @@ public void getTauMatrix(DenseMatrix64F matrix)
       matrix.set(2, 0, tauTrans.getY());
    }
 
+   @Override
    public void getVelocityMatrix(DenseMatrix64F matrix, int rowStart)
    {
       matrix.set(rowStart + 0, 0, qdRot);
@@ -79,6 +85,7 @@ public void getVelocityMatrix(DenseMatrix64F matrix, int rowStart)
       matrix.set(rowStart + 2, 0, qdTrans.getY());
    }
 
+   @Override
    public void getDesiredAccelerationMatrix(DenseMatrix64F matrix, int rowStart)
    {
       matrix.set(rowStart + 0, 0, qddRotDesired);
@@ -86,23 +93,27 @@ public void getDesiredAccelerationMatrix(DenseMatrix64F matrix, int rowStart)
       matrix.set(rowStart + 2, 0, qddTransDesired.getY());
    }
 
+   @Override
    public void setDesiredAccelerationToZero()
    {
       qddRot = 0.0;
       qddTrans.set(0.0, 0.0);
    }
 
+   @Override
    public void setSuccessor(RigidBody successor)
    {
       this.successor = successor;
       setMotionSubspace();
    }
 
+   @Override
    public void updateMotionSubspace()
    {
       // empty
    }
 
+   @Override
    public void setTorqueFromWrench(Wrench jointWrench)
    {
       jointWrench.getBodyFrame().checkReferenceFrameMatch(successor.getBodyFixedFrame());
@@ -113,11 +124,13 @@ public void setTorqueFromWrench(Wrench jointWrench)
       jointWrench.changeFrame(successor.getBodyFixedFrame());
    }
 
+   @Override
    public int getDegreesOfFreedom()
    {
       return 3;
    }
 
+   @Override
    public void setDesiredAcceleration(DenseMatrix64F matrix, int rowStart)
    {
       qddRotDesired = matrix.get(rowStart + 0, 0);
@@ -292,6 +305,7 @@ private void setMotionSubspace()
       this.motionSubspace.compute();
    }
 
+   @Override
    public void getConfigurationMatrix(DenseMatrix64F matrix, int rowStart)
    {
       int index = rowStart;
@@ -300,6 +314,7 @@ public void getConfigurationMatrix(DenseMatrix64F matrix, int rowStart)
       matrix.set(index++, 0, qTrans.getY());
    }
 
+   @Override
    public void setConfiguration(DenseMatrix64F matrix, int rowStart)
    {
       int index = rowStart;
@@ -311,6 +326,7 @@ public void setConfiguration(DenseMatrix64F matrix, int rowStart)
       afterJointFrame.setTranslationAndUpdate(qTrans);
    }
 
+   @Override
    public void setVelocity(DenseMatrix64F matrix, int rowStart)
    {
       int index = rowStart;
@@ -321,6 +337,7 @@ public void setVelocity(DenseMatrix64F matrix, int rowStart)
    }
 
 
+   @Override
    public int getConfigurationMatrixSize()
    {
       return getDegreesOfFreedom();
@@ -338,6 +355,7 @@ private PlanarJoint checkAndGetAsPlanarJoint(InverseDynamicsJoint originalJoint)
       }
    }
 
+   @Override
    public void setJointPositionVelocityAndAcceleration(InverseDynamicsJoint originalJoint)
    {
       PlanarJoint originalPlanarJoint = checkAndGetAsPlanarJoint(originalJoint);
@@ -351,6 +369,7 @@ public void setJointPositionVelocityAndAcceleration(InverseDynamicsJoint origina
       setTranslationalAcceleration(originalPlanarJoint.qddTrans.getVector());
    }
 
+   @Override
    public void setQddDesired(InverseDynamicsJoint originalJoint)
    {
       PlanarJoint originalPlanarJoint = checkAndGetAsPlanarJoint(originalJoint);
@@ -360,6 +379,7 @@ public void setQddDesired(InverseDynamicsJoint originalJoint)
       
    }
 
+   @Override
    public void calculateJointStateChecksum(GenericCRC32 checksum)
    {
       checksum.update(qRot);
@@ -371,6 +391,7 @@ public void calculateJointStateChecksum(GenericCRC32 checksum)
       checksum.update(qddTrans.getVector());
    }
 
+   @Override
    public void calculateJointDesiredChecksum(GenericCRC32 checksum)
    {
       checksum.update(qddRotDesired);
diff --git a/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/PrismaticJoint.java b/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/PrismaticJoint.java
index 12844d267ac..85ea5cbb31b 100644
--- a/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/PrismaticJoint.java
+++ b/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/PrismaticJoint.java
@@ -16,6 +16,7 @@ public PrismaticJoint(String name, RigidBody predecessor, ReferenceFrame beforeJ
       this.unitJointTwist = new Twist(afterJointFrame, beforeJointFrame, afterJointFrame, jointAxis.getVector(), new Vector3d());
    }
 
+   @Override
    public void setSuccessor(RigidBody successor)
    {
       this.successor = successor;
diff --git a/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/RevoluteJointReferenceFrame.java b/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/RevoluteJointReferenceFrame.java
index 4eec237ea21..7c9a0d7a6fc 100644
--- a/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/RevoluteJointReferenceFrame.java
+++ b/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/RevoluteJointReferenceFrame.java
@@ -21,6 +21,7 @@ public RevoluteJointReferenceFrame(String frameName, ReferenceFrame parentFrame,
       this.axis = axis.getVectorCopy();
    }
 
+   @Override
    public void setAndUpdate(double jointPosition)
    {
       this.angle = jointPosition;
diff --git a/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/SixDoFJoint.java b/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/SixDoFJoint.java
index dbff421d365..2da48095b65 100644
--- a/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/SixDoFJoint.java
+++ b/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/SixDoFJoint.java
@@ -36,31 +36,37 @@ public SixDoFJoint(String name, RigidBody predecessor, ReferenceFrame beforeJoin
       this.jointAccelerationDesired = new SpatialAccelerationVector(afterJointFrame, beforeJointFrame, afterJointFrame);
    }
    
+   @Override
    public SixDoFJointReferenceFrame getFrameAfterJoint()
    {
       return afterJointFrame;
    }
    
+   @Override
    public void getJointTwist(Twist twistToPack)
    {
       twistToPack.set(jointTwist);
    }
    
+   @Override
    public void getJointAcceleration(SpatialAccelerationVector accelerationToPack)
    {
       accelerationToPack.set(jointAcceleration);
    }
    
+   @Override
    public void getDesiredJointAcceleration(SpatialAccelerationVector accelerationToPack)
    {
       accelerationToPack.set(jointAccelerationDesired);
    }
    
+   @Override
    public void getTauMatrix(DenseMatrix64F matrix)
    {
       successorWrench.getMatrix(matrix);
    }
    
+   @Override
    public void getVelocityMatrix(DenseMatrix64F matrix, int rowStart)
    {
       jointTwist.getMatrix(matrix, rowStart);
@@ -86,16 +92,19 @@ public Vector3d getAngularVelocityForReading()
       return jointTwist.getAngularPart();
    }
    
+   @Override
    public void getDesiredAccelerationMatrix(DenseMatrix64F matrix, int rowStart)
    {
       jointAccelerationDesired.getMatrix(matrix, rowStart);
    }
    
+   @Override
    public void setDesiredAccelerationToZero()
    {
       jointAccelerationDesired.setToZero();
    }
    
+   @Override
    public void setSuccessor(RigidBody successor)
    {
       this.successor = successor;
@@ -106,16 +115,19 @@ public void setSuccessor(RigidBody successor)
       this.successorWrench = new Wrench(successorFrame, successorFrame);
    }
    
+   @Override
    public void setTorqueFromWrench(Wrench jointWrench)
    {
       this.successorWrench.checkAndSet(jointWrench);
    }
    
+   @Override
    public int getDegreesOfFreedom()
    {
       return 6;
    }
    
+   @Override
    public void setDesiredAcceleration(DenseMatrix64F matrix, int rowStart)
    {
       jointAccelerationDesired.set(jointAccelerationDesired.getBodyFrame(), jointAccelerationDesired.getBaseFrame(),
@@ -283,11 +295,13 @@ private void setMotionSubspace()
       this.motionSubspace.compute();
    }
    
+   @Override
    public void updateMotionSubspace()
    {
       // empty
    }
    
+   @Override
    public void getConfigurationMatrix(DenseMatrix64F matrix, int rowStart)
    {
       int index = rowStart;
@@ -299,6 +313,7 @@ public void getConfigurationMatrix(DenseMatrix64F matrix, int rowStart)
       matrix.set(index++, 0, jointTranslation.getZ());
    }
    
+   @Override
    public void setConfiguration(DenseMatrix64F matrix, int rowStart)
    {
       int index = rowStart;
@@ -312,6 +327,7 @@ public void setConfiguration(DenseMatrix64F matrix, int rowStart)
       this.afterJointFrame.setTranslation(jointTranslation);
    }
    
+   @Override
    public void setVelocity(DenseMatrix64F matrix, int rowStart)
    {
       jointTwist.set(jointTwist.getBodyFrame(), jointTwist.getBaseFrame(), jointTwist.getExpressedInFrame(), matrix, rowStart);
@@ -342,6 +358,7 @@ public void setLinearVelocityInWorld(Vector3d linearVelocityInWorld)
       jointTwist.set(newTwist);
    }
    
+   @Override
    public int getConfigurationMatrixSize()
    {
       int positionSize = 3;
@@ -361,6 +378,7 @@ private SixDoFJoint checkAndGetAsSiXDoFJoint(InverseDynamicsJoint originalJoint)
       }
    }
    
+   @Override
    public void setJointPositionVelocityAndAcceleration(InverseDynamicsJoint originalJoint)
    {
       SixDoFJoint sixDoFOriginalJoint = checkAndGetAsSiXDoFJoint(originalJoint);
@@ -374,6 +392,7 @@ public void setJointPositionVelocityAndAcceleration(InverseDynamicsJoint origina
       jointAcceleration.setLinearPart(sixDoFOriginalJoint.jointAcceleration.getLinearPart());
    }
    
+   @Override
    public void setQddDesired(InverseDynamicsJoint originalJoint)
    {
       SixDoFJoint sixDoFOriginalJoint = checkAndGetAsSiXDoFJoint(originalJoint);
@@ -381,6 +400,7 @@ public void setQddDesired(InverseDynamicsJoint originalJoint)
       jointAccelerationDesired.setLinearPart(sixDoFOriginalJoint.jointAccelerationDesired.getLinearPart());
    }
    
+   @Override
    public void calculateJointStateChecksum(GenericCRC32 checksum)
    {
       checksum.update(jointTranslation);
@@ -391,6 +411,7 @@ public void calculateJointStateChecksum(GenericCRC32 checksum)
       checksum.update(jointAcceleration.getLinearPart());
    }
    
+   @Override
    public void calculateJointDesiredChecksum(GenericCRC32 checksum)
    {
       checksum.update(jointAccelerationDesired.getAngularPart());
diff --git a/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/SpatialAccelerationVector.java b/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/SpatialAccelerationVector.java
index 0072c73759b..798ae73607a 100644
--- a/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/SpatialAccelerationVector.java
+++ b/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/SpatialAccelerationVector.java
@@ -367,6 +367,7 @@ public void setScrew(ReferenceFrame bodyFrame, ReferenceFrame baseFrame, Referen
    }
 
    ///CLOVER:OFF
+   @Override
    public String toString()
    {
       String ret = new String("Spatial acceleration of " + bodyFrame + ", with respect to " + baseFrame + ", expressed in " + expressedInFrame + "\n"
@@ -375,6 +376,4 @@ public String toString()
       return ret;
    }
    ///CLOVER:ON
-
-
 }
diff --git a/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/SpatialForceVector.java b/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/SpatialForceVector.java
index 2604a3add01..45851db1176 100644
--- a/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/SpatialForceVector.java
+++ b/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/SpatialForceVector.java
@@ -524,6 +524,7 @@ public void negate()
       this.angularPart.negate();
    }
    
+   @Override
    public String toString()
    {
       String ret = new String("SpatialForceVector expressed in frame " + expressedInFrame + "\n" + "Angular part: " + angularPart + "\n"
diff --git a/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/SpatialMotionVector.java b/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/SpatialMotionVector.java
index 0034a9cee82..7c49c0ecf4c 100644
--- a/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/SpatialMotionVector.java
+++ b/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/SpatialMotionVector.java
@@ -539,6 +539,7 @@ public void checkReferenceFramesMatch(ReferenceFrame bodyFrame, ReferenceFrame b
    }
 
    ///CLOVER:OFF
+   @Override
    public String toString()
    {
       String ret = new String("Spatial motion of " + bodyFrame + ", with respect to " + baseFrame + ", expressed in frame " + expressedInFrame + "\n"
@@ -547,5 +548,4 @@ public String toString()
       return ret;
    }
    ///CLOVER:ON
-
 }
diff --git a/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/SphericalJoint.java b/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/SphericalJoint.java
index 015a09226d6..2ecd60e35f9 100644
--- a/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/SphericalJoint.java
+++ b/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/SphericalJoint.java
@@ -31,29 +31,34 @@ public SphericalJoint(String name, RigidBody predecessor, ReferenceFrame beforeJ
       this.jointAngularAccelerationDesired = new FrameVector(afterJointFrame);
    }
 
+   @Override
    public ReferenceFrame getFrameAfterJoint()
    {
       return afterJointFrame;
    }
 
+   @Override
    public void getJointTwist(Twist twistToPack)
    {
       twistToPack.setToZero(afterJointFrame, beforeJointFrame, afterJointFrame);
       twistToPack.setAngularPart(jointAngularVelocity.getVector());
    }
 
+   @Override
    public void getJointAcceleration(SpatialAccelerationVector accelerationToPack)
    {
       accelerationToPack.setToZero(afterJointFrame, beforeJointFrame, afterJointFrame);
       accelerationToPack.setAngularPart(jointAngularAcceleration.getVector());
    }
 
+   @Override
    public void getDesiredJointAcceleration(SpatialAccelerationVector jointAcceleration)
    {
       jointAcceleration.setToZero(afterJointFrame, beforeJointFrame, afterJointFrame);
       jointAcceleration.setAngularPart(jointAngularAccelerationDesired.getVector());
    }
 
+   @Override
    public void getTauMatrix(DenseMatrix64F matrix)
    {
       matrix.set(0, 0, jointTorque.getX());
@@ -61,6 +66,7 @@ public void getTauMatrix(DenseMatrix64F matrix)
       matrix.set(2, 0, jointTorque.getZ());
    }
 
+   @Override
    public void getVelocityMatrix(DenseMatrix64F matrix, int rowStart)
    {
       matrix.set(rowStart + 0, 0, jointAngularVelocity.getX());
@@ -68,6 +74,7 @@ public void getVelocityMatrix(DenseMatrix64F matrix, int rowStart)
       matrix.set(rowStart + 2, 0, jointAngularVelocity.getZ());
    }
 
+   @Override
    public void getDesiredAccelerationMatrix(DenseMatrix64F matrix, int rowStart)
    {
       matrix.set(rowStart + 0, 0, jointAngularAccelerationDesired.getX());
@@ -75,11 +82,13 @@ public void getDesiredAccelerationMatrix(DenseMatrix64F matrix, int rowStart)
       matrix.set(rowStart + 2, 0, jointAngularAccelerationDesired.getZ());
    }
 
+   @Override
    public void setDesiredAccelerationToZero()
    {
       jointAngularAccelerationDesired.setToZero(jointAngularAccelerationDesired.getReferenceFrame());
    }
 
+   @Override
    public void setSuccessor(RigidBody successor)
    {
       this.successor = successor;
@@ -89,11 +98,13 @@ public void setSuccessor(RigidBody successor)
       this.jointTorque = new FrameVector(successorFrame);
    }
 
+   @Override
    public void updateMotionSubspace()
    {
       // empty
    }
 
+   @Override
    public void setTorqueFromWrench(Wrench jointWrench)
    {
       jointWrench.getBodyFrame().checkReferenceFrameMatch(successor.getBodyFixedFrame());
@@ -101,11 +112,13 @@ public void setTorqueFromWrench(Wrench jointWrench)
       jointTorque.set(jointWrench.getAngularPart());
    }
 
+   @Override
    public int getDegreesOfFreedom()
    {
       return 3;
    }
 
+   @Override
    public void setDesiredAcceleration(DenseMatrix64F matrix, int rowStart)
    {
       this.jointAngularAccelerationDesired.setX(matrix.get(rowStart + 0, 0));
@@ -218,17 +231,20 @@ private void setMotionSubspace()
       this.motionSubspace.compute();
    }
 
+   @Override
    public void getConfigurationMatrix(DenseMatrix64F matrix, int rowStart)
    {
       MatrixTools.insertQuat4dIntoEJMLVector(matrix, jointRotation, rowStart);
    }
 
+   @Override
    public void setConfiguration(DenseMatrix64F matrix, int rowStart)
    {
       MatrixTools.extractQuat4dFromEJMLVector(jointRotation, matrix, rowStart);
       this.afterJointFrame.setRotation(jointRotation);
    }
 
+   @Override
    public void setVelocity(DenseMatrix64F matrix, int rowStart)
    {
       jointAngularVelocity.setX(matrix.get(rowStart + 0, 0));
@@ -237,6 +253,7 @@ public void setVelocity(DenseMatrix64F matrix, int rowStart)
    }
 
 
+   @Override
    public int getConfigurationMatrixSize()
    {
       return RotationTools.QUATERNION_SIZE;
@@ -254,6 +271,7 @@ private SphericalJoint checkAndGetAsSphericalJoint(InverseDynamicsJoint original
       }
    }
    
+   @Override
    public void setJointPositionVelocityAndAcceleration(InverseDynamicsJoint originalJoint)
    {
          SphericalJoint originalSphericalJoint = checkAndGetAsSphericalJoint(originalJoint);
@@ -262,12 +280,14 @@ public void setJointPositionVelocityAndAcceleration(InverseDynamicsJoint origina
          jointAngularAcceleration.set(originalSphericalJoint.jointAngularAcceleration.getVector());
    }
 
+   @Override
    public void setQddDesired(InverseDynamicsJoint originalJoint)
    {
       SphericalJoint originalSphericalJoint = checkAndGetAsSphericalJoint(originalJoint);
       jointAngularAccelerationDesired.set(originalSphericalJoint.jointAngularAccelerationDesired.getVector()); 
    }
 
+   @Override
    public void calculateJointStateChecksum(GenericCRC32 checksum)
    {
       checksum.update(jointRotation);
@@ -275,6 +295,7 @@ public void calculateJointStateChecksum(GenericCRC32 checksum)
       checksum.update(jointAngularAcceleration.getVector());
    }
 
+   @Override
    public void calculateJointDesiredChecksum(GenericCRC32 checksum)
    {
       checksum.update(jointAngularAccelerationDesired.getVector());
diff --git a/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/Twist.java b/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/Twist.java
index 07327f68dc8..1b1e279a982 100644
--- a/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/Twist.java
+++ b/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/Twist.java
@@ -340,6 +340,7 @@ public void setScrew(ReferenceFrame bodyFrame, ReferenceFrame baseFrame, Referen
    }
 
    ///CLOVER:OFF
+   @Override
    public String toString()
    {
       String ret = new String("Twist of " + bodyFrame + ", with respect to " + baseFrame + ", expressed in " + expressedInFrame + "\n" + "Linear part: "
diff --git a/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/Wrench.java b/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/Wrench.java
index 60e165bc578..7b4bb692766 100644
--- a/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/Wrench.java
+++ b/IHMCRoboticsToolkit/src/us/ihmc/robotics/screwTheory/Wrench.java
@@ -150,6 +150,7 @@ public double dot(Twist twist)
       return twist.dot(this);
    }
 
+   @Override
    public String toString()
    {
       String ret = new String("Wrench exerted on " + bodyFrame + ", expressed in frame " + expressedInFrame + "\n" + "Torque part: " + angularPart + "\n"