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(75);
+
       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

@@ -163,6 +163,7 @@ public class ContinuousStepGenerator implements Updatable, SCS2YoGraphicHolder
    private final YoEnum<ContinuousStepGeneratorMode> currentCSGMode = new YoEnum<>("currentCSGMode", registry, ContinuousStepGeneratorMode.class);
    private final YoEnum<ContinuousStepGeneratorMode> requestedCSGMode = new YoEnum<>("requestedCSGMode", registry, ContinuousStepGeneratorMode.class);
    private final OptionalFactoryField<QuicksterFootstepProvider> quicksterFootstepProvider = new OptionalFactoryField<>("QuicksterFootstepProviderField");
+   private boolean useQFPParameters = false;
 
    /**
     * Creates a new step generator, its {@code YoVariable}s will not be attached to any registry.
@@ -186,6 +187,12 @@ public ContinuousStepGenerator(YoRegistry parentRegistry)
       numberOfTicksBeforeSubmittingFootsteps.set(0);
       currentFootstepDataListCommandID.set(new Random().nextLong(0, Long.MAX_VALUE / 2)); // To make this command ID unique
 
+      requestedCSGMode.addListener(change ->
+                                   {
+//                                      if (requestedCSGMode.getEnumValue().equals(ContinuousStepGeneratorMode.STANDARD))
+//                                         useQFPParameters = false;
+                                   });
+
       currentCSGMode.addListener(change ->
                           {
                              if (currentCSGMode.getEnumValue() == ContinuousStepGeneratorMode.QFP)
@@ -287,12 +294,33 @@ else if (statusToProcess == FootstepStatus.COMPLETED)
          previousFootstepPose.set(previousFootstep.getLocation(), previousFootstep.getOrientation());
       }
 
+      if (quicksterFootstepProvider.hasValue())
+      {
+         if (currentCSGMode.getEnumValue().equals(ContinuousStepGeneratorMode.QFP))
+            swingSide = quicksterFootstepProvider.get().getCurrentSwingSide();
+
+         if (useQFPParameters)
+            quicksterFootstepProvider.get().setUseAlternateTransferDuration(false);
+         else
+         {
+            quicksterFootstepProvider.get().setUseAlternateTransferDuration(true);
+            quicksterFootstepProvider.get().setAlternateTransferDuration(parameters.getTransferDuration());
+         }
+
+         // If in standard mode, keep initializing QFP so its control frame matches pelvis yaw
+         if (currentCSGMode.getEnumValue() == ContinuousStepGeneratorMode.STANDARD)
+            quicksterFootstepProvider.get().initialize();
+
+         // Continuously update QFP for data visualization purposes
+         quicksterFootstepProvider.get().update(parameters.getNumberOfFootstepsToPlan());
+      }
+
       // Set footstep parameters
       if (currentCSGMode.getEnumValue() == ContinuousStepGeneratorMode.QFP && quicksterFootstepProvider.hasValue())
       {
-         footstepDataListMessage.setDefaultSwingDuration(quicksterFootstepProvider.get().getSwingDuration(swingSide));
          footstepDataListMessage.setDefaultTransferDuration(quicksterFootstepProvider.get().getTransferDuration(swingSide));
          footstepDataListMessage.setFinalTransferDuration(quicksterFootstepProvider.get().getTransferDuration(swingSide));
+         footstepDataListMessage.setDefaultSwingDuration(quicksterFootstepProvider.get().getSwingDuration(swingSide));
          footstepDataListMessage.setAreFootstepsAdjustable(false);
          footstepDataListMessage.setOffsetFootstepsWithExecutionError(false);
       }
@@ -335,42 +363,16 @@ else if (statusToProcess == FootstepStatus.COMPLETED)
       this.desiredTurningVelocity.set(turningVelocity);
 
       int startingIndexToAdjust = footsteps.size();
-
-      // Continuously update QFP for data visualization purposes
-      if (quicksterFootstepProvider.hasValue())
-      {
-         // If in standard mode, keep initializing QFP so its control frame matches pelvis yaw
-         if (currentCSGMode.getEnumValue() == ContinuousStepGeneratorMode.STANDARD)
-            quicksterFootstepProvider.get().initialize();
-
-         quicksterFootstepProvider.get().update(time);
-      }
+      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
          {
@@ -407,7 +409,7 @@ else if (statusToProcess == FootstepStatus.COMPLETED)
 
          if (swingHeightInputProvider == null && currentCSGMode.getEnumValue() == ContinuousStepGeneratorMode.STANDARD)
             footstep.setSwingHeight(parameters.getSwingHeight());
-         else if (swingHeightInputProvider == null && currentCSGMode.getEnumValue() == ContinuousStepGeneratorMode.QFP)
+         else if (swingHeightInputProvider == null && currentCSGMode.getEnumValue() == ContinuousStepGeneratorMode.QFP && quicksterFootstepProvider.hasValue())
             footstep.setSwingHeight(quicksterFootstepProvider.get().getSwingHeight(swingSide));
          else
             footstep.setSwingHeight(swingHeightInputProvider.getValue());
@@ -438,30 +440,30 @@ else if (swingHeightInputProvider == null && currentCSGMode.getEnumValue() == Co
       }
 
       // run through and make sure these adjusted steps are valid.
-      for (int i = startingIndexToAdjust; i < footstepDataListMessage.getFootstepDataList().size(); i++)
-      {
-         FootstepDataMessage footstepData = footstepDataListMessage.getFootstepDataList().get(i);
-         nextFootstepPose2D.getPosition().set(footstepData.getLocation());
-         nextFootstepPose2D.getOrientation().set(footstepData.getOrientation());
-         nextFootstepPose3D.getPosition().set(footstepData.getLocation());
-         nextFootstepPose3D.getOrientation().set(footstepData.getOrientation());
-         swingSide = RobotSide.fromByte(footstepData.getRobotSide());
-
-         if (!isStepValid(nextFootstepPose3D, previousFootstepPose, swingSide))
-         {
-            alternateStepChooser.computeStep(footstepPose2D, nextFootstepPose2D, swingSide, footstepData);
-
-            // remove all the other steps after the invalid one.
-            while (footstepDataListMessage.getFootstepDataList().size() > i + 1)
-            {
-               footstepDataListMessage.getFootstepDataList().remove(i + 1);
-            }
-         }
-
-         previousFootstepPose.getPosition().set(footstepData.getLocation());
-         previousFootstepPose.getOrientation().set(footstepData.getOrientation());
-         footstepPose2D.set(previousFootstepPose);
-      }
+//      for (int i = startingIndexToAdjust; i < footstepDataListMessage.getFootstepDataList().size(); i++)
+//      {
+//         FootstepDataMessage footstepData = footstepDataListMessage.getFootstepDataList().get(i);
+//         nextFootstepPose2D.getPosition().set(footstepData.getLocation());
+//         nextFootstepPose2D.getOrientation().set(footstepData.getOrientation());
+//         nextFootstepPose3D.getPosition().set(footstepData.getLocation());
+//         nextFootstepPose3D.getOrientation().set(footstepData.getOrientation());
+//         swingSide = RobotSide.fromByte(footstepData.getRobotSide());
+//
+//         if (!isStepValid(nextFootstepPose3D, previousFootstepPose, swingSide))
+//         {
+//            alternateStepChooser.computeStep(footstepPose2D, nextFootstepPose2D, swingSide, footstepData);
+//
+//            // remove all the other steps after the invalid one.
+//            while (footstepDataListMessage.getFootstepDataList().size() > i + 1)
+//            {
+//               footstepDataListMessage.getFootstepDataList().remove(i + 1);
+//            }
+//         }
+//
+//         previousFootstepPose.getPosition().set(footstepData.getLocation());
+//         previousFootstepPose.getOrientation().set(footstepData.getOrientation());
+//         footstepPose2D.set(previousFootstepPose);
+//      }
 
       // Update the visualizers
       for (int i = startingIndexToAdjust; i < footstepDataListMessage.getFootstepDataList().size(); i++)
@@ -689,6 +691,8 @@ public void notifyFootstepStarted(RobotSide robotSide)
    {
       latestStatusReceived.setValue(FootstepStatus.STARTED);
       currentSupportSide.set(robotSide.getOppositeSide());
+
+      useQFPParameters = currentCSGMode.getEnumValue().equals(ContinuousStepGeneratorMode.QFP);
    }
 
    /**