Bugfix/fix pelvis rotation on delays (#1106)

* started working on a fix for pelvis rotationd elays

* rolled back on how the time in the phase is found, and started adding a decay factor so the feedforward values dont go to the moon

* fixed a bound inclusiveness problem

Author: rjgriffin42

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

@@ -114,7 +114,6 @@ public void initialize()
 
    public void initializeForStanding()
    {
-
       inSingleSupport.set(false);
       inStanding.set(true);
 
@@ -136,6 +135,7 @@ public void initializeForStanding()
    public void initializeForTransfer(double transferDuration, double swingDuration)
    {
       timeAtStartOfSupportSequence.set(time.getValue());
+
       timeInSupportSequence.set(0.0);
       currentStateDuration.set(transferDuration);
       totalStateDuration.set(transferDuration + swingDuration);
@@ -159,14 +159,14 @@ public void initializeForSingleSupport(double transferDuration, double swingDura
 
       double stepDuration = transferDuration + swingDuration;
       timeAtStartOfSupportSequence.set(time.getValue() - transferDuration);
+
       timeInSupportSequence.set(transferDuration);
       currentStateDuration.set(stepDuration);
       totalStateDuration.set(stepDuration);
       remainingTimeInContactSequence.set(swingDuration);
       adjustedRemainingTimeUnderDisturbance.set(swingDuration);
       offsetTimeInState.set(transferDuration);
 
-
       adjustedTimeInSupportSequence.set(transferDuration);
       totalTimeAdjustment.set(0.0);
 
@@ -176,6 +176,7 @@ public void initializeForSingleSupport(double transferDuration, double swingDura
    public void initializeForTransferToStanding(double finalTransferDuration )
    {
       timeAtStartOfSupportSequence.set(time.getValue());
+
       timeInSupportSequence.set(0.0);
       currentStateDuration.set(finalTransferDuration);
       totalStateDuration.set(finalTransferDuration);

ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/WalkingHighLevelHumanoidController.java

@@ -46,6 +46,7 @@
 import us.ihmc.commonWalkingControlModules.momentumBasedController.optimization.ControllerCoreOptimizationSettings;
 import us.ihmc.commonWalkingControlModules.momentumBasedController.optimization.JointLimitEnforcement;
 import us.ihmc.commonWalkingControlModules.momentumBasedController.optimization.JointLimitParameters;
+import us.ihmc.commonWalkingControlModules.referenceFrames.WalkingTrajectoryPath;
 import us.ihmc.commonWalkingControlModules.staticEquilibrium.StabilityMarginRegionCalculator;
 import us.ihmc.commonWalkingControlModules.staticEquilibrium.WholeBodyContactState;
 import us.ihmc.commons.lists.RecyclingArrayList;
@@ -589,10 +590,12 @@ public void initialize()
 
    private void initializeWalkingTrajectoryPath()
    {
-      controllerToolbox.getWalkingTrajectoryPath().clearFootsteps();
-      controllerToolbox.getWalkingTrajectoryPath().reset();
-      controllerToolbox.getWalkingTrajectoryPath().initializeDoubleSupport();
-      controllerToolbox.getWalkingTrajectoryPath().updateTrajectory(FootControlModule.ConstraintType.FULL, FootControlModule.ConstraintType.FULL);
+      WalkingTrajectoryPath walkingTrajectoryPath = controllerToolbox.getWalkingTrajectoryPath();
+      walkingTrajectoryPath.clearFootsteps();
+      walkingTrajectoryPath.reset();
+      walkingTrajectoryPath.initializeDoubleSupport();
+      walkingTrajectoryPath.updateTrajectory(FootControlModule.ConstraintType.FULL,
+                                             FootControlModule.ConstraintType.FULL);
    }
 
    private void initializeManagers()

ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/states/TransferToStandingState.java

@@ -59,7 +59,8 @@ public void doAction(double timeInState)
    {
       balanceManager.computeICPPlan();
       controllerToolbox.getWalkingTrajectoryPath()
-                       .updateTrajectory(feetManager.getCurrentConstraintType(RobotSide.LEFT), feetManager.getCurrentConstraintType(RobotSide.RIGHT));
+                       .updateTrajectory(feetManager.getCurrentConstraintType(RobotSide.LEFT),
+                                         feetManager.getCurrentConstraintType(RobotSide.RIGHT));
 
       switchToPointToeOffIfAlreadyInLine();
 

ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/states/WalkingSingleSupportState.java

@@ -256,7 +256,8 @@ private void updateWalkingTrajectoryPath()
       walkingTrajectoryPath.clearFootsteps();
       walkingTrajectoryPath.addFootstep(nextFootstep, footstepTiming);
       walkingTrajectoryPath.addFootsteps(walkingMessageHandler);
-      walkingTrajectoryPath.updateTrajectory(feetManager.getCurrentConstraintType(RobotSide.LEFT), feetManager.getCurrentConstraintType(RobotSide.RIGHT));
+      walkingTrajectoryPath.updateTrajectory(feetManager.getCurrentConstraintType(RobotSide.LEFT),
+                                             feetManager.getCurrentConstraintType(RobotSide.RIGHT));
    }
 
    @Override

ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/referenceFrames/WalkingTrajectoryPath.java

@@ -45,7 +45,6 @@
 import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint3D;
 import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D;
 import us.ihmc.yoVariables.filters.AlphaFilterTools;
-import us.ihmc.yoVariables.filters.AlphaFilteredYoVariable;
 import us.ihmc.yoVariables.math.YoMatrix;
 import us.ihmc.yoVariables.parameters.DoubleParameter;
 import us.ihmc.yoVariables.providers.DoubleProvider;
@@ -77,6 +76,7 @@ public class WalkingTrajectoryPath implements SCS2YoGraphicHolder
    private static final ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();
    private static final String WALKING_TRAJECTORY_PATH_FRAME_NAME = "walkingTrajectoryPathFrame";
    public static final int WALKING_TRAJECTORY_FRAME_ID = WALKING_TRAJECTORY_PATH_FRAME_NAME.hashCode();
+   private static final double DECAY_RATE = 10.0;
 
    private static final int MAX_NUMBER_OF_FOOTSTEPS = 2;
 
@@ -105,6 +105,9 @@ public class WalkingTrajectoryPath implements SCS2YoGraphicHolder
    private final YoDouble currentYawAcceleration = new YoDouble(namePrefix + "CurrentYawAcceleration", registry);
    private final YoDouble finalTransferDuration = new YoDouble(namePrefix + "FinalTransferDuration", registry);
 
+   private final YoDouble decayRate = new YoDouble(namePrefix + "DecayRate", registry);
+   private final YoDouble decayFactor = new YoDouble(namePrefix + "DecayFactor", registry);
+
    private final YoFrameVector3D initialLinearVelocity = new YoFrameVector3D(namePrefix + "InitialLinearVelocity", worldFrame, registry);
    private final YoDouble initialYawRate = new YoDouble(namePrefix + "InitialYawRate", registry);
 
@@ -154,6 +157,7 @@ public WalkingTrajectoryPath(DoubleProvider time,
       this.soleFrames = soleFrames;
 
       trajectoryManager = new TrajectoryManager(registry);
+      decayRate.set(DECAY_RATE);
       YoGraphicsList yoGraphicList;
 
       if (yoGraphicsListRegistry != null)
@@ -349,7 +353,16 @@ public void updateTrajectory(ConstraintType leftFootConstraintType, ConstraintTy
       if (updateWaypoints)
          updateWaypoints();
 
-      double currentTime = MathTools.clamp(time.getValue() - startTime.getValue(), 0.0, totalDuration.getValue());
+      double maxTime = totalDuration.getDoubleValue();
+      if (!footsteps.isEmpty())
+      {
+         if (isInDoubleSupport.getBooleanValue())
+            maxTime = Math.min(footstepTimings.getFirst().getTransferTime(), maxTime);
+         else
+            maxTime = Math.min(footstepTimings.getFirst().getSwingTime(), maxTime);
+      }
+      double timeInState = time.getValue() - startTime.getDoubleValue();
+      double currentTime = MathTools.clamp(timeInState, 0.0, maxTime);
 
       trajectoryManager.initialize(initialLinearVelocity, initialYawRate.getValue(), waypoints, isLastWaypointOpen.getValue());
       trajectoryManager.computePosition(currentTime, currentPosition);
@@ -359,6 +372,12 @@ public void updateTrajectory(ConstraintType leftFootConstraintType, ConstraintTy
       currentYawRate.set(trajectoryManager.computeYawRate(currentTime));
       currentYawAcceleration.set(trajectoryManager.computeYawAcceleration(currentTime));
 
+      // If we've overshot when this was supposed to end, decay the rate and acceleration
+      double overageTime = timeInState - currentTime;
+      decayFactor.set(Math.exp(-decayRate.getDoubleValue() * overageTime));
+      currentYawRate.mul(decayFactor.getDoubleValue());
+      currentYawAcceleration.mul(decayFactor.getDoubleValue() * decayFactor.getDoubleValue());
+
       walkingTrajectoryPathFrame.update();
       walkingTrajectoryAcceleration.getLinearPart().setMatchingFrame(currentLinearAcceleration);
       walkingTrajectoryAcceleration.getAngularPart().set(0.0, 0.0, currentYawAcceleration.getValue());

ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/generators/MultiCubicSpline1DSolver.java

@@ -465,9 +465,10 @@ public double computeVelocity(double time, DMatrixRMaj solution)
 
    public double computeAcceleration(double time, DMatrixRMaj solution)
    {
-      if (time <= 0.0)
+      // We don't want these bounds to be inclusive. The position and velocity bounds are not inclusive, as they return this trajectory at those bounds.
+      if (time < 0.0)
          return 0.0;
-      if (time >= 1.0)
+      if (time > 1.0)
          return 0.0;
 
       int index = findSolutionOffsetIndex(time, solution);