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 < maxNumberOfAlphaInterpolationTicks)
+            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/EllipticalStepPositionLimiter.java

@@ -0,0 +1,258 @@
+package us.ihmc.commonWalkingControlModules.desiredFootStep;
+
+import us.ihmc.commons.MathTools;
+import us.ihmc.euclid.referenceFrame.FramePoint2D;
+import us.ihmc.euclid.referenceFrame.FramePoint3D;
+import us.ihmc.euclid.referenceFrame.ReferenceFrame;
+import us.ihmc.euclid.referenceFrame.interfaces.FixedFramePoint3DBasics;
+import us.ihmc.euclid.referenceFrame.interfaces.FramePoint3DBasics;
+import us.ihmc.euclid.referenceFrame.interfaces.FramePoint3DReadOnly;
+import us.ihmc.euclid.tools.EuclidCoreTools;
+import us.ihmc.robotics.geometry.GroundPlaneEstimator;
+import us.ihmc.robotics.robotSide.RobotSide;
+
+public class EllipticalStepPositionLimiter
+{
+   public final static double NOMINAL_STANCE_WIDTH_DEFAULT = 0.25;
+   public final static double MAX_STEP_FORWARD_DEFAULT = 0.85;
+   public final static double MIN_STANCE_WIDTH_DEFAULT = 0.1;
+   public final static double MAX_STANCE_WIDTH_DEFAULT = 0.75;
+   public final static double MIN_DISTANCE_FROM_STANCE_FOOT_DEFAULT = 0.1;
+
+   private final FramePoint2D stanceFootPosition = new FramePoint2D();
+   private final FramePoint2D constrainedTouchdownPosition2D = new FramePoint2D();
+   private final FramePoint3DBasics constrainedSoleTouchdownPosition = new FramePoint3D();
+
+   public void enforceFootPositionConstraint(FramePoint3DReadOnly desiredSoleTouchdownPosition,
+                                             FixedFramePoint3DBasics constrainedSoleTouchdownPositionToPack,
+                                             ReferenceFrame desiredConstraintFrame,
+                                             ReferenceFrame stanceFootZUpFrame,
+                                             RobotSide swingSide)
+   {
+      enforceFootPositionConstraint(desiredSoleTouchdownPosition,
+                                    constrainedSoleTouchdownPositionToPack,
+                                    desiredConstraintFrame,
+                                    stanceFootZUpFrame,
+                                    NOMINAL_STANCE_WIDTH_DEFAULT,
+                                    MAX_STEP_FORWARD_DEFAULT,
+                                    MIN_STANCE_WIDTH_DEFAULT,
+                                    MAX_STANCE_WIDTH_DEFAULT,
+                                    MIN_DISTANCE_FROM_STANCE_FOOT_DEFAULT,
+                                    swingSide);
+   }
+
+   /**
+    * @param desiredSoleTouchdownPosition              The unconstrained position. Not Modified.
+    * @param constrainedSoleTouchdownPositionToPack    The step position that is constrained.
+    * @param desiredConstraintFrame                    The frame in which to apply the constraint.
+    */
+   public boolean enforceFootPositionConstraint(FramePoint3DReadOnly desiredSoleTouchdownPosition,
+                                                FixedFramePoint3DBasics constrainedSoleTouchdownPositionToPack,
+                                                ReferenceFrame desiredConstraintFrame,
+                                                ReferenceFrame stanceFootZUpFrame,
+                                                double nominalStanceWidth,
+                                                double maxStepForward,
+                                                double minStanceWidth,
+                                                double maxStanceWidth,
+                                                double minDistanceFromStanceFoot,
+                                                RobotSide swingSide)
+   {
+      return enforceFootPositionConstraint(desiredSoleTouchdownPosition,
+                                           constrainedSoleTouchdownPositionToPack,
+                                           desiredConstraintFrame,
+                                           stanceFootZUpFrame,
+                                           null,
+                                           nominalStanceWidth,
+                                           maxStepForward,
+                                           minStanceWidth,
+                                           maxStanceWidth,
+                                           minDistanceFromStanceFoot,
+                                           swingSide);
+   }
+
+   /**
+    * @param desiredSoleTouchdownPosition              The unconstrained position. Not Modified.
+    * @param constrainedSoleTouchdownPositionToPack    The step position that is constrained.
+    * @param desiredConstraintFrame                    The frame in which to apply the constraint.
+    */
+   public boolean enforceFootPositionConstraint(FramePoint3DReadOnly desiredSoleTouchdownPosition,
+                                                FixedFramePoint3DBasics constrainedSoleTouchdownPositionToPack,
+                                                ReferenceFrame desiredConstraintFrame,
+                                                ReferenceFrame stanceFootZUpFrame,
+                                                GroundPlaneEstimator groundPlaneEstimator,
+                                                double nominalStanceWidth,
+                                                double maxStepForward,
+                                                double minStanceWidth,
+                                                double maxStanceWidth,
+                                                double minDistanceFromStanceFoot,
+                                                RobotSide swingSide)
+   {
+      constrainedSoleTouchdownPosition.setIncludingFrame(desiredSoleTouchdownPosition);
+
+      boolean wasLimited = false;
+      wasLimited |= enforceMinimumStepWidthInConstraintFrame(constrainedSoleTouchdownPosition, desiredConstraintFrame, minStanceWidth, swingSide);
+
+      wasLimited |= enforceOuterEllipticalBoundInConstraintFrame(constrainedSoleTouchdownPosition,
+                                                                 desiredConstraintFrame,
+                                                                 nominalStanceWidth,
+                                                                 maxStepForward,
+                                                                 maxStanceWidth,
+                                                                 swingSide);
+
+      wasLimited |= enforceStepDistanceFromStance(constrainedSoleTouchdownPosition, stanceFootZUpFrame, minDistanceFromStanceFoot);
+
+      wasLimited |= enforceStepWidthFromStance(constrainedSoleTouchdownPosition, desiredConstraintFrame, stanceFootZUpFrame, minStanceWidth, swingSide);
+
+      //TODO do i need to do this on line 90?
+      constrainedSoleTouchdownPosition.changeFrame(desiredConstraintFrame);
+
+      //TODO limit z position as well
+      if (groundPlaneEstimator != null)
+      {
+         constrainedTouchdownPosition2D.setIncludingFrame(constrainedSoleTouchdownPosition);
+         //         constrainedSoleTouchdownPosition.setMatchingFrame(groundPlaneEstimator.getGroundPosition(constrainedTouchdownPosition2D));
+      }
+
+      constrainedSoleTouchdownPositionToPack.setMatchingFrame(constrainedSoleTouchdownPosition);
+      return wasLimited;
+   }
+
+   static boolean enforceMinimumStepWidthInConstraintFrame(FramePoint3DBasics desiredStepPositionToConstrain,
+                                                           ReferenceFrame desiredConstraintFrame,
+                                                           double minStanceWidth,
+                                                           RobotSide swingSide)
+   {
+      desiredStepPositionToConstrain.changeFrame(desiredConstraintFrame);
+
+      // make sure you don't step too narrowly. This is done by cropping the lateral position of the footstep.
+      // If the width is negative, we assume that we're allowing a cross over distance relative to the constraint frame. If it's positive, then that's the
+      // nominal stance width, as defined by one foot relative to the other. This means we then divide it by half when computing this value.
+      double desiredWidth = desiredStepPositionToConstrain.getY();
+      double widthBound = minStanceWidth < 0.0 ? minStanceWidth : 0.5 * minStanceWidth;
+      if (swingSide == RobotSide.LEFT)
+      {
+         if (desiredWidth < widthBound)
+         {
+            desiredStepPositionToConstrain.setY(widthBound);
+            return true;
+         }
+      }
+      else
+      {
+         if (desiredWidth > -widthBound)
+         {
+            desiredStepPositionToConstrain.setY(-widthBound);
+            return true;
+         }
+      }
+
+      return false;
+   }
+
+   /**
+    * The constraint frame is assumed to be the center of the pelvis, or the center of mass frame. So the ellipse is centered half the nominal stance width from
+    * the  constraint frame.
+    * @param desiredStepPositionToConstrain
+    * @param constraintFrame
+    * @param nominalStanceWidth
+    * @param maxStepForward
+    * @param maxStanceWidth
+    * @param stepSide
+    * @return
+    */
+   static boolean enforceOuterEllipticalBoundInConstraintFrame(FramePoint3DBasics desiredStepPositionToConstrain,
+                                                               ReferenceFrame constraintFrame,
+                                                               double nominalStanceWidth,
+                                                               double maxStepForward,
+                                                               double maxStanceWidth,
+                                                               RobotSide stepSide)
+   {
+      ReferenceFrame originalFrame = desiredStepPositionToConstrain.getReferenceFrame();
+      desiredStepPositionToConstrain.changeFrame(constraintFrame);
+
+      double ellipseYOrigin = stepSide.negateIfRightSide(0.5 * nominalStanceWidth);
+      double ellipseYMax = maxStanceWidth - nominalStanceWidth;
+
+      double ellipseX = desiredStepPositionToConstrain.getX();
+      double ellipseY = desiredStepPositionToConstrain.getY() - ellipseYOrigin;
+
+      // check if the point is inside the ellipse
+      double r = MathTools.square(ellipseX / maxStepForward) + MathTools.square(ellipseY / ellipseYMax);
+
+      if (r <= 1.0)
+      {
+         desiredStepPositionToConstrain.changeFrame(originalFrame);
+         return false;
+      }
+
+      // project the constraint onto the ellipse bound, since it was outside
+      double theta = Math.atan2(desiredStepPositionToConstrain.getX(), ellipseYMax);
+      double sinTheta = Math.sin(theta);
+      double cosTheta = Math.cos(theta);
+      double k = maxStepForward * ellipseYMax / (Math.sqrt(MathTools.square(ellipseYMax * sinTheta) + MathTools.square(maxStepForward * cosTheta)));
+      double x = Math.abs(k * sinTheta) * Math.signum(ellipseX);
+      double y = Math.abs(k * cosTheta) * Math.signum(ellipseY);
+
+      y += ellipseYOrigin;
+
+      desiredStepPositionToConstrain.set(x, y, desiredStepPositionToConstrain.getZ());
+      desiredStepPositionToConstrain.changeFrame(originalFrame);
+
+      return true;
+   }
+
+   private static boolean enforceStepDistanceFromStance(FramePoint3DBasics desiredTouchdownInStanceFootToPack,
+                                                        ReferenceFrame stanceFootZUpFrame,
+                                                        double minDistanceFromStanceFoot)
+   {
+      // make sure you don't step too close to the stance foot. This is done by making sure the desired touchdown position stays outside a circle
+      // around the stance foot.
+      if (minDistanceFromStanceFoot > 0.0)
+      {
+         desiredTouchdownInStanceFootToPack.changeFrame(stanceFootZUpFrame);
+         double distanceFromStance = desiredTouchdownInStanceFootToPack.distanceXY(EuclidCoreTools.origin2D);
+         if (distanceFromStance < minDistanceFromStanceFoot)
+         {
+            double scaleFactor = minDistanceFromStanceFoot / distanceFromStance;
+            desiredTouchdownInStanceFootToPack.scale(scaleFactor, scaleFactor, 1.0);
+
+            return true;
+         }
+      }
+
+      return false;
+   }
+
+   private boolean enforceStepWidthFromStance(FramePoint3DBasics desiredStepPositionToConstrain,
+                                              ReferenceFrame desiredConstraintFrame,
+                                              ReferenceFrame stanceFootZUpFrame,
+                                              double minStanceWidth,
+                                              RobotSide swingSide)
+   {
+      // This makes sure step is no less than a defined distance in y away from the stance foot.
+      stanceFootPosition.setToZero(stanceFootZUpFrame);
+      stanceFootPosition.changeFrameAndProjectToXYPlane(desiredConstraintFrame);
+      desiredStepPositionToConstrain.changeFrame(desiredConstraintFrame);
+
+      if (swingSide == RobotSide.LEFT)
+      {
+         double innerYBound = stanceFootPosition.getY() + minStanceWidth;
+         if (innerYBound > desiredStepPositionToConstrain.getY())
+         {
+            desiredStepPositionToConstrain.setY(innerYBound);
+            return true;
+         }
+      }
+      else
+      {
+         double innerYBound = stanceFootPosition.getY() - minStanceWidth;
+         if (innerYBound < desiredStepPositionToConstrain.getY())
+         {
+            desiredStepPositionToConstrain.setY(innerYBound);
+            return true;
+         }
+      }
+
+      return false;
+   }
+}