Merge pull request ihmcrobotics#261 in ROB/ihmc-open-robotics-software from feature/new_toe_cmp_calculation to develop

* commit '11481f033e416372c03585fbf42cc6ec9ae701bc':
  modified the action taken when the put exit CMP on toes boolean is set true

Author: rjgriffin42

CommonWalkingControlModules/src/us/ihmc/commonWalkingControlModules/instantaneousCapturePoint/ReferenceCentroidalMomentumPivotLocationsCalculator.java

@@ -667,7 +667,9 @@ else if (putExitCMPOnToes.getBooleanValue())
       }
       else if (putCMPOnToesWalking)
       {
-         putExitCMPOnToes(cmp2d, footSupportPolygon, exitCMPUserOffsets.get(robotSide).getY());
+         constrainCMPAccordingToSupportPolygonAndUserOffsets(cmp2d, footSupportPolygon, centroidOfFootstepToConsider, exitCMPUserOffsets.get(robotSide),
+                                                             minForwardExitCMPOffset.getDoubleValue(), footSupportPolygon.getMaxX(),
+                                                             exitCMPForwardSafetyMarginOnToes.getDoubleValue());
       }
       else if (putCMPOnToesSteppingDown)
       {
@@ -699,7 +701,16 @@ private void putExitCMPOnToes(FramePoint2D exitCMPToPack, FrameConvexPolygon2d f
    }
 
    private void constrainCMPAccordingToSupportPolygonAndUserOffsets(FramePoint2D cmpToPack, FrameConvexPolygon2d footSupportPolygon,
-         FramePoint2D centroidOfFootstepToConsider, YoFrameVector2d cmpOffset, double minForwardCMPOffset, double maxForwardCMPOffset)
+                                                                    FramePoint2D centroidOfFootstepToConsider, YoFrameVector2d cmpOffset,
+                                                                    double minForwardCMPOffset, double maxForwardCMPOffset)
+   {
+      constrainCMPAccordingToSupportPolygonAndUserOffsets(cmpToPack, footSupportPolygon, centroidOfFootstepToConsider, cmpOffset, minForwardCMPOffset,
+                                                          maxForwardCMPOffset, safeDistanceFromCMPToSupportEdges.getDoubleValue());
+   }
+
+   private void constrainCMPAccordingToSupportPolygonAndUserOffsets(FramePoint2D cmpToPack, FrameConvexPolygon2d footSupportPolygon,
+                                                                    FramePoint2D centroidOfFootstepToConsider, YoFrameVector2d cmpOffset,
+                                                                    double minForwardCMPOffset, double maxForwardCMPOffset, double safeDistanceFromCMPToSupportEdges)
    {
       // First constrain the computed CMP to the given min/max along the x-axis.
       FramePoint2D footSupportCentroid = footSupportPolygon.getCentroid();
@@ -711,7 +722,7 @@ private void constrainCMPAccordingToSupportPolygonAndUserOffsets(FramePoint2D cm
 
       // Then constrain the computed CMP to be inside a safe support region
       tempSupportPolygonForShrinking.setIncludingFrameAndUpdate(footSupportPolygon);
-      convexPolygonShrinker.scaleConvexPolygon(tempSupportPolygonForShrinking, safeDistanceFromCMPToSupportEdges.getDoubleValue(), footSupportPolygon);
+      convexPolygonShrinker.scaleConvexPolygon(tempSupportPolygonForShrinking, safeDistanceFromCMPToSupportEdges, footSupportPolygon);
 
       footSupportPolygon.orthogonalProjection(cmpToPack);
    }