Feature/extract quickster touchdown calculator

ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/AvatarStepGeneratorThread.java

@@ -98,12 +98,13 @@ public AvatarStepGeneratorThread(HumanoidSteppingPluginFactory pluginFactory,
          pluginFactory.createStepGeneratorNetworkSubscriber(drcRobotModel.getSimpleRobotName(), ros2Node);
 
       humanoidReferenceFrames = new HumanoidReferenceFrames(fullRobotModel);
-      continuousStepGeneratorPlugin = pluginFactory.buildPlugin(humanoidReferenceFrames,
+      continuousStepGeneratorPlugin = pluginFactory.buildPlugin(fullRobotModel,
+                                                                humanoidReferenceFrames,
                                                                 drcRobotModel.getStepGeneratorDT(),
                                                                 drcRobotModel.getWalkingControllerParameters(),
                                                                 walkingOutputManager,
                                                                 walkingCommandInputManager,
-                                                                null,
+                                                                csgGraphics,
                                                                 null,
                                                                 csgTime);
       csgRegistry.addChild(continuousStepGeneratorPlugin.getRegistry());

ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/kinematicsSimulation/HumanoidKinematicsSimulation.java

@@ -312,6 +312,7 @@ private HumanoidKinematicsSimulation(DRCRobotModel robotModel, HumanoidKinematic
                                                                             controllerToolbox.getContactableFeet(),
                                                                             fullRobotModel.getElevator(),
                                                                             walkingControllerParameters,
+                                                                            controllerToolbox.getWalkingMessageHandler(),
                                                                             controllerToolbox.getTotalMassProvider(),
                                                                             GRAVITY_Z,
                                                                             controllerToolbox.getControlDT(),

ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/walkingPreview/WalkingControllerPreviewToolboxController.java

@@ -183,6 +183,7 @@ public WalkingControllerPreviewToolboxController(DRCRobotModel robotModel,
                                                                             contactableFeet,
                                                                             elevator,
                                                                             walkingControllerParameters,
+                                                                            controllerToolbox.getWalkingMessageHandler(),
                                                                             controllerToolbox.getTotalMassProvider(),
                                                                             gravityZ,
                                                                             controlDT,

ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/scs2/SCS2AvatarSimulationFactory.java

@@ -460,7 +460,7 @@ private void setupStepGeneratorThread()
       else
       {
          JoystickBasedSteppingPluginFactory joystickPluginFactory = new JoystickBasedSteppingPluginFactory();
-         if (heightMapForFootstepZ.hasValue())
+         if (heightMapForFootstepZ.hasValue() && heightMapForFootstepZ.get() != null)
             joystickPluginFactory.setFootStepAdjustment(new HeightMapBasedFootstepAdjustment(heightMapForFootstepZ.get()));
          else
          {

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

@@ -19,6 +19,7 @@
 import us.ihmc.commonWalkingControlModules.dynamicPlanning.bipedPlanning.*;
 import us.ihmc.commonWalkingControlModules.dynamicPlanning.comPlanning.CoMContinuousContinuityCalculator;
 import us.ihmc.commonWalkingControlModules.dynamicPlanning.comPlanning.CoMTrajectoryPlanner;
+import us.ihmc.commonWalkingControlModules.dynamicPlanning.comPlanning.CornerPointViewer;
 import us.ihmc.commonWalkingControlModules.dynamicPlanning.comPlanning.SettableContactStateProvider;
 import us.ihmc.commonWalkingControlModules.messageHandlers.CenterOfMassTrajectoryHandler;
 import us.ihmc.commonWalkingControlModules.messageHandlers.MomentumTrajectoryHandler;
@@ -299,6 +300,7 @@ public BalanceManager(HighLevelHumanoidControllerToolbox controllerToolbox,
       maintainInitialCoMVelocityContinuitySingleSupport = new BooleanParameter("maintainInitialCoMVelocityContinuitySingleSupport", registry, true);
       maintainInitialCoMVelocityContinuityTransfer = new BooleanParameter("maintainInitialCoMVelocityContinuityTransfer", registry, true);
       comTrajectoryPlanner = new CoMTrajectoryPlanner(controllerToolbox.getGravityZ(), controllerToolbox.getOmega0Provider(), registry);
+      comTrajectoryPlanner.setCornerPointViewer(new CornerPointViewer(registry, yoGraphicsListRegistry));
       comTrajectoryPlanner.setComContinuityCalculator(new CoMContinuousContinuityCalculator(controllerToolbox.getGravityZ(),
                                                                                             controllerToolbox.getOmega0Provider(),
                                                                                             registry));

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

@@ -13,13 +13,15 @@
 import us.ihmc.commonWalkingControlModules.controllerCore.command.inverseDynamics.CenterOfPressureCommand;
 import us.ihmc.commonWalkingControlModules.controllerCore.command.inverseDynamics.MomentumRateCommand;
 import us.ihmc.commonWalkingControlModules.controllerCore.command.inverseDynamics.PlaneContactStateCommand;
+import us.ihmc.commonWalkingControlModules.messageHandlers.WalkingMessageHandler;
 import us.ihmc.commonWalkingControlModules.momentumBasedController.CapturePointCalculator;
 import us.ihmc.commonWalkingControlModules.momentumBasedController.HighLevelHumanoidControllerToolbox;
 import us.ihmc.commonWalkingControlModules.momentumBasedController.optimization.MomentumOptimizationSettings;
 import us.ihmc.commonWalkingControlModules.momentumControlCore.CoMHeightController;
 import us.ihmc.commonWalkingControlModules.momentumControlCore.HeightController;
 import us.ihmc.commonWalkingControlModules.momentumControlCore.PelvisHeightController;
 import us.ihmc.commonWalkingControlModules.wrenchDistribution.WrenchDistributorTools;
+import us.ihmc.commons.MathTools;
 import us.ihmc.euclid.referenceFrame.FramePoint2D;
 import us.ihmc.euclid.referenceFrame.FramePoint3D;
 import us.ihmc.euclid.referenceFrame.FrameVector2D;
@@ -59,6 +61,7 @@
 import us.ihmc.yoVariables.providers.DoubleProvider;
 import us.ihmc.yoVariables.registry.YoRegistry;
 import us.ihmc.yoVariables.variable.YoBoolean;
+import us.ihmc.yoVariables.variable.YoDouble;
 
 import static us.ihmc.graphicsDescription.appearance.YoAppearance.*;
 import static us.ihmc.scs2.definition.yoGraphic.YoGraphicDefinitionFactory.newYoGraphicPoint2D;
@@ -110,6 +113,7 @@ public class LinearMomentumRateControlModule implements SCS2YoGraphicHolder
    private final FixedFramePoint2DBasics perfectCoP = new FramePoint2D();
    private final FixedFramePoint2DBasics desiredCMP = new FramePoint2D();
    private final FixedFramePoint2DBasics desiredCoP = new FramePoint2D();
+   private final FramePoint3D desiredPendulumBase = new FramePoint3D();
    private final FixedFramePoint2DBasics achievedCMP = new FramePoint2D();
 
    private final FrameVector3D achievedLinearMomentumRate = new FrameVector3D();
@@ -155,6 +159,15 @@ public class LinearMomentumRateControlModule implements SCS2YoGraphicHolder
 
    private final LinearMomentumRateControlModuleOutput output = new LinearMomentumRateControlModuleOutput();
 
+   ////
+   private final SideDependentList<ContactableFoot> contactableFeet;
+   private final WalkingControllerParameters walkingControllerParameters;
+   private final double controlDt;
+   private final FramePoint3D leadingPendulumBase = new FramePoint3D();
+   private final FramePoint3D trailingPendulumBase = new FramePoint3D();
+   private final FramePoint3D alternateCoP = new FramePoint3D();
+   private final WalkingMessageHandler walkingMessageHandler;
+
    public LinearMomentumRateControlModule(HighLevelHumanoidControllerToolbox controllerToolbox,
                                           WalkingControllerParameters walkingControllerParameters,
                                           YoRegistry parentRegistry)
@@ -164,6 +177,7 @@ public LinearMomentumRateControlModule(HighLevelHumanoidControllerToolbox contro
            controllerToolbox.getContactableFeet(),
            controllerToolbox.getFullRobotModel().getElevator(),
            walkingControllerParameters,
+           controllerToolbox.getWalkingMessageHandler(),
            controllerToolbox.getTotalMassProvider(),
            controllerToolbox.getGravityZ(),
            controllerToolbox.getControlDT(),
@@ -176,6 +190,7 @@ public LinearMomentumRateControlModule(CenterOfMassStateProvider centerOfMassSta
                                           SideDependentList<ContactableFoot> contactableFeet,
                                           RigidBodyBasics elevator,
                                           WalkingControllerParameters walkingControllerParameters,
+                                          WalkingMessageHandler walkingMessageHandler,
                                           DoubleProvider totalMassProvider,
                                           double gravityZ,
                                           double controlDT,
@@ -184,6 +199,10 @@ public LinearMomentumRateControlModule(CenterOfMassStateProvider centerOfMassSta
    {
       this.totalMassProvider = totalMassProvider;
       this.gravityZ = gravityZ;
+      this.contactableFeet = contactableFeet;
+      this.walkingControllerParameters = walkingControllerParameters;
+      this.walkingMessageHandler = walkingMessageHandler;
+      this.controlDt = controlDT;
 
       MomentumOptimizationSettings momentumOptimizationSettings = walkingControllerParameters.getMomentumOptimizationSettings();
       linearMomentumRateWeight = new ParameterVector3D("LinearMomentumRateWeight", momentumOptimizationSettings.getLinearMomentumWeight(), registry);
@@ -256,9 +275,74 @@ public LinearMomentumRateControlModule(CenterOfMassStateProvider centerOfMassSta
          icpController = new ICPController(walkingControllerParameters, icpControlPolygons, contactableFeet, controlDT, registry, yoGraphicsListRegistry);
       }
 
+      useAlternateCoP = new YoBoolean("useAlternateCoP", registry);
+
       parentRegistry.addChild(registry);
    }
 
+   private double leftTimeInContact = 0.0;
+   private double rightTimeInContact = 0.0;
+   private double leadingTimeInContact = 0.0;
+   private final YoBoolean useAlternateCoP;
+
+   private void computeAlternateCoP()
+   {
+      boolean leftInContact = contactStateCommands.get(RobotSide.LEFT).getNumberOfContactPoints() > 0;
+      boolean rightInContact = contactStateCommands.get(RobotSide.RIGHT).getNumberOfContactPoints() > 0;
+
+      RobotSide leadingSide = RobotSide.RIGHT;
+      RobotSide trailingSide = RobotSide.LEFT;
+
+      if (leftInContact && !rightInContact)
+      {
+         leftTimeInContact += controlDt;
+         rightTimeInContact = 0.0;
+         leadingTimeInContact = leftTimeInContact;
+         leadingSide = RobotSide.LEFT;
+         trailingSide = RobotSide.LEFT;
+      }
+      else if (rightInContact && !leftInContact)
+      {
+         rightTimeInContact += controlDt;
+         leftTimeInContact = 0.0;
+         leadingTimeInContact = rightTimeInContact;
+         leadingSide = RobotSide.RIGHT;
+         trailingSide = RobotSide.RIGHT;
+      }
+      else if (leftInContact && rightInContact)
+      {
+         leftTimeInContact += controlDt;
+         rightTimeInContact += controlDt;
+
+         if (leftTimeInContact > rightTimeInContact)
+         {
+            leadingSide = RobotSide.RIGHT;
+            trailingSide = RobotSide.LEFT;
+            leadingTimeInContact = rightTimeInContact;
+         }
+         else
+         {
+            leadingSide = RobotSide.LEFT;
+            trailingSide = RobotSide.RIGHT;
+            leadingTimeInContact = leftTimeInContact;
+         }
+      }
+
+      leadingPendulumBase.setToZero(contactableFeet.get(leadingSide).getSoleFrame());
+      trailingPendulumBase.setToZero(contactableFeet.get(trailingSide).getSoleFrame());
+
+
+      double transferDuration = walkingMessageHandler.getNextTransferTime();
+      double alpha = 1.0;
+      if (transferDuration > 0.0)
+         alpha = MathTools.clamp(leadingTimeInContact / transferDuration, 0.0, 1.0);
+
+      leadingPendulumBase.changeFrame(worldFrame);
+      trailingPendulumBase.changeFrame(worldFrame);
+      alternateCoP.changeFrame(worldFrame);
+      alternateCoP.interpolate(trailingPendulumBase, leadingPendulumBase, alpha);
+   }
+
    public void reset()
    {
       desiredLinearMomentumRateWeight.set(linearMomentumRateWeight);
@@ -379,13 +463,22 @@ public boolean computeControllerCoreCommands()
       else
          desiredLinearMomentumRateWeight.update(linearMomentumRateWeight);
 
+      computeAlternateCoP();
+      FixedFramePoint2DBasics desiredCoPToUse = desiredCoP;
+
+      if (useAlternateCoP.getBooleanValue())
+         desiredCoPToUse.set(alternateCoP);
+
       yoDesiredCMP.set(desiredCMP);
-      yoDesiredCoP.set(desiredCoP);
+      yoDesiredCoP.set(desiredCoPToUse);
       yoCenterOfMass.setFromReferenceFrame(centerOfMassFrame);
       yoCenterOfMassVelocity.set(capturePointCalculator.getCenterOfMassVelocity());
       yoCapturePoint.set(capturePoint);
 
-      success = success && computeDesiredLinearMomentumRateOfChange();
+      if (useAlternateCoP.getBooleanValue())
+         success = success && computeDesiredLinearMomentumRateOfChangeNew();
+      else
+         success = success && computeDesiredLinearMomentumRateOfChange();
 
       selectionMatrix.setToLinearSelectionOnly();
       selectionMatrix.selectLinearX(!useCenterOfPressureCommandOnly.getValue());
@@ -396,7 +489,7 @@ public boolean computeControllerCoreCommands()
       momentumRateCommand.setSelectionMatrix(selectionMatrix);
       momentumRateCommand.setWeights(angularMomentumRateWeight, desiredLinearMomentumRateWeight);
 
-      centerOfPressureCommandCalculator.computeCenterOfPressureCommand(desiredCoP, contactStateCommands, bipedSupportPolygons.getFootPolygonsInSoleFrame());
+      centerOfPressureCommandCalculator.computeCenterOfPressureCommand(desiredCoPToUse, contactStateCommands, bipedSupportPolygons.getFootPolygonsInSoleFrame());
 
       return success;
    }
@@ -563,6 +656,25 @@ private boolean computeDesiredLinearMomentumRateOfChange()
       return success;
    }
 
+   private boolean computeDesiredLinearMomentumRateOfChangeNew()
+   {
+      double totalMass = totalMassProvider.getValue();
+      double desiredVerticalRateOfChange = totalMass * desiredCoMHeightAcceleration;
+
+      desiredPendulumBase.setIncludingFrame(alternateCoP);
+      desiredPendulumBase.changeFrame(centerOfMassFrame);
+
+      linearMomentumRateOfChange.setToZero();
+      linearMomentumRateOfChange.setMatchingFrame(desiredPendulumBase);
+      double weight = totalMass * Math.abs(gravityZ);
+      double expectedVerticalForce = desiredVerticalRateOfChange + weight;
+      linearMomentumRateOfChange.scale(expectedVerticalForce / linearMomentumRateOfChange.getZ());
+      linearMomentumRateOfChange.subZ(weight);
+//      linearMomentumRateOfChange.changeFrame(worldFrame);
+
+      return true;
+   }
+
    private static boolean checkInputs(FramePoint2DReadOnly capturePoint,
                                       FixedFramePoint2DBasics desiredCapturePoint,
                                       FixedFrameVector2DBasics desiredCapturePointVelocity,

ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/foot/SwingState.java

@@ -375,18 +375,18 @@ public void doSpecificAction(double timeInState)
 
       computeAndPackTrajectory(timeInState);
 
-      if (workspaceLimiterControlModule != null)
-      {
-         desiredPose.setIncludingFrame(desiredPosition, desiredOrientation);
-         changeDesiredPoseBodyFrame(controlFrame, ankleFrame, desiredPose);
-         desiredAnklePosition.setIncludingFrame(desiredPose.getPosition());
-
-         workspaceLimiterControlModule.correctSwingFootTrajectory(desiredAnklePosition, desiredLinearVelocity, desiredLinearAcceleration);
-
-         desiredPose.getPosition().set(desiredAnklePosition);
-         changeDesiredPoseBodyFrame(ankleFrame, controlFrame, desiredPose);
-         desiredPosition.setIncludingFrame(desiredPose.getPosition());
-      }
+//      if (workspaceLimiterControlModule != null)
+//      {
+//         desiredPose.setIncludingFrame(desiredPosition, desiredOrientation);
+//         changeDesiredPoseBodyFrame(controlFrame, ankleFrame, desiredPose);
+//         desiredAnklePosition.setIncludingFrame(desiredPose.getPosition());
+//
+//         workspaceLimiterControlModule.correctSwingFootTrajectory(desiredAnklePosition, desiredLinearVelocity, desiredLinearAcceleration);
+//
+//         desiredPose.getPosition().set(desiredAnklePosition);
+//         changeDesiredPoseBodyFrame(ankleFrame, controlFrame, desiredPose);
+//         desiredPosition.setIncludingFrame(desiredPose.getPosition());
+//      }
 
       if (yoSetDesiredVelocityToZero.getBooleanValue())
       {
@@ -479,11 +479,11 @@ else if (replanTrajectory.getBooleanValue()) // need to update the swing traject
       yoReferenceSolePosition.setMatchingFrame(desiredPosition);
       yoReferenceSoleLinearVelocity.setMatchingFrame(desiredLinearVelocity);
 
-      if (!isInTouchdown)
-      { // we're still in swing, so update the desired setpoints using the smoother
-         swingTrajectorySmoother.compute(time);
-         swingTrajectorySmoother.getLinearData(desiredPosition, desiredLinearVelocity, desiredLinearAcceleration);
-      }
+//      if (!isInTouchdown)
+//      { // we're still in swing, so update the desired setpoints using the smoother
+//         swingTrajectorySmoother.compute(time);
+//         swingTrajectorySmoother.getLinearData(desiredPosition, desiredLinearVelocity, desiredLinearAcceleration);
+//      }
 
       if (isSwingSpeedUpEnabled.getBooleanValue() && !currentTimeWithSwingSpeedUp.isNaN())
       {