Feature/alip calculator modifications

ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/LinearMomentumRateControlModule.java

@@ -350,7 +350,7 @@ public MomentumRateCommand getMomentumRateCommand()
     */
    public CenterOfPressureCommand getCenterOfPressureCommand()
    {
-      return centerOfPressureCommand;
+      return centerOfPressureCommandCalculator.getCenterOfPressureCommand();
    }
 
    /**

ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPController.java

@@ -15,6 +15,8 @@
 import us.ihmc.commonWalkingControlModules.capturePoint.ICPControlPolygons;
 import us.ihmc.commonWalkingControlModules.capturePoint.ParameterizedICPControlGains;
 import us.ihmc.commonWalkingControlModules.configurations.WalkingControllerParameters;
+import us.ihmc.commons.InterpolationTools;
+import us.ihmc.commons.MathTools;
 import us.ihmc.euclid.referenceFrame.FramePoint2D;
 import us.ihmc.euclid.referenceFrame.FrameVector2D;
 import us.ihmc.euclid.referenceFrame.ReferenceFrame;
@@ -108,6 +110,7 @@ public class ICPController implements ICPControllerInterface
    private final YoBoolean hasNotConvergedInPast = new YoBoolean(yoNamePrefix + "HasNotConvergedInPast", registry);
    private final YoBoolean previousTickFailed = new YoBoolean(yoNamePrefix + "PreviousTickFailed", registry);
    private final YoInteger hasNotConvergedCounts = new YoInteger(yoNamePrefix + "HasNotConvergedCounts", registry);
+   private final YoInteger ticksToInterpolateAlpha = new YoInteger("ticksToInterpolateAlpha", registry);
 
    private final IntegerProvider maxNumberOfIterations = new IntegerParameter(yoNamePrefix + "MaxNumberOfIterations", registry, 100);
 
@@ -212,6 +215,8 @@ public ICPController(WalkingControllerParameters walkingControllerParameters,
       parameters.createFeedbackAlphaCalculator(registry, null);
       parameters.createFeedbackProjectionOperator(registry, null);
 
+      ticksToInterpolateAlpha.set(150);
+
       if (yoGraphicsListRegistry != null)
          setupVisualizers(yoGraphicsListRegistry);
 
@@ -452,14 +457,31 @@ private void extractSolutionsFromSolver(boolean converged)
       integrator.update(desiredICPVelocity, currentCoMVelocity, icpError);
    }
 
+   private int alphaInterpolationTicks = 0;
    private void computeFeedForwardAndFeedBackAlphas()
    {
       if (disableCoPFeedback.getValue() || !useCoPFeedbackByDefault.getValue())
-         feedbackAlpha.set(1.0);
+      {
+         double maxNumberOfAlphaInterpolationTicks = MathTools.clamp(ticksToInterpolateAlpha.getIntegerValue(), 1, Integer.MAX_VALUE);
+         double interpolationFactor = (double) alphaInterpolationTicks / maxNumberOfAlphaInterpolationTicks;
+         interpolationFactor = MathTools.clamp(interpolationFactor, 0.0, 1.0);
+         double startValue = 0.0; //parameters.getFeedbackAlphaCalculator() != null ? parameters.getFeedbackAlphaCalculator().computeAlpha(currentICP, copConstraintHandler.getCoPConstraint()) : 0.0;
+         double endValue = 1.0;
+         double valueToUse = InterpolationTools.linearInterpolate(startValue, endValue, interpolationFactor);
+         feedbackAlpha.set(valueToUse);
+         if (alphaInterpolationTicks < ticksToInterpolateAlpha.getValue())
+            alphaInterpolationTicks++;
+      }
       else if (parameters.getFeedbackAlphaCalculator() != null)
+      {
          feedbackAlpha.set(parameters.getFeedbackAlphaCalculator().computeAlpha(currentICP, copConstraintHandler.getCoPConstraint()));
+         alphaInterpolationTicks = 0;
+      }
       else
+      {
          feedbackAlpha.set(0.0);
+         alphaInterpolationTicks = 0;
+      }
 
       if (parameters.getFeedForwardAlphaCalculator() != null)
          feedForwardAlpha.set(parameters.getFeedForwardAlphaCalculator()

ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/ContinuousStepGenerator.java

@@ -343,34 +343,19 @@ else if (statusToProcess == FootstepStatus.COMPLETED)
          if (currentCSGMode.getEnumValue() == ContinuousStepGeneratorMode.STANDARD)
             quicksterFootstepProvider.get().initialize();
 
-         quicksterFootstepProvider.get().update(time);
+         quicksterFootstepProvider.get().update(parameters.getNumberOfFootstepsToPlan());
       }
 
+      int perFootIndex = 0;
+
       for (int i = startingIndexToAdjust; i < parameters.getNumberOfFootstepsToPlan(); i++)
       {
          if (currentCSGMode.getEnumValue() == ContinuousStepGeneratorMode.QFP && quicksterFootstepProvider.hasValue())
          {
-            // FIXME we want all steps in plan to be QFP eventually
-            if (i == startingIndexToAdjust)
-            {
-               quicksterFootstepProvider.get().getDesiredTouchdownPose(swingSide, nextFootstepPose2D);
-            }
-            else
-            {
-               calculateNextFootstepPose2D(stepTime.getValue(),
-                                           desiredVelocityX,
-                                           desiredVelocityY,
-                                           desiredTurningVelocity.getDoubleValue(),
-                                           swingSide,
-                                           maxStepLength,
-                                           maxStepWidth,
-                                           defaultStepWidth,
-                                           minStepWidth,
-                                           turnMaxAngleInward,
-                                           turnMaxAngleOutward,
-                                           footstepPose2D,
-                                           nextFootstepPose2D);
-            }
+            if (i != 0 && i % 2 == 0)
+               perFootIndex++;
+
+            quicksterFootstepProvider.get().getDesiredTouchdownPose(perFootIndex, swingSide, nextFootstepPose2D);
          }
          else
          {