- MPCControllerOutput is saved into mpcControllerOutputDataHolder in …

…HumanoidRobotMPCContextData. - In HumanoidWholeBodyControllerCoreManager, the desiredMPCControllerOutput is called and then saved into `desiredJointOutput`which is populated into actuator.
ihmcrobotics · Dec 14, 2024 · c4b24fa · c4b24fa
1 parent e0645c5
commit c4b24fa
Show file tree

Hide file tree

Showing 7 changed files with 52 additions and 40 deletions.
diff --git a/atlas/src/main/java/us/ihmc/atlas/AtlasRobotModel.java b/atlas/src/main/java/us/ihmc/atlas/AtlasRobotModel.java
@@ -3,19 +3,7 @@
 import com.jme3.math.Transform;
 import us.ihmc.atlas.diagnostic.AtlasDiagnosticParameters;
 import us.ihmc.atlas.initialSetup.AtlasSimInitialSetup;
-import us.ihmc.atlas.parameters.AtlasCoPTrajectoryParameters;
-import us.ihmc.atlas.parameters.AtlasContactPointParameters;
-import us.ihmc.atlas.parameters.AtlasFootstepPlannerParameters;
-import us.ihmc.atlas.parameters.AtlasHighLevelControllerParameters;
-import us.ihmc.atlas.parameters.AtlasICPSplitFractionCalculatorParameters;
-import us.ihmc.atlas.parameters.AtlasKinematicsCollisionModel;
-import us.ihmc.atlas.parameters.AtlasPhysicalProperties;
-import us.ihmc.atlas.parameters.AtlasSensorInformation;
-import us.ihmc.atlas.parameters.AtlasSimulationCollisionModel;
-import us.ihmc.atlas.parameters.AtlasStateEstimatorParameters;
-import us.ihmc.atlas.parameters.AtlasSwingPlannerParameters;
-import us.ihmc.atlas.parameters.AtlasVisibilityGraphParameters;
-import us.ihmc.atlas.parameters.AtlasWalkingControllerParameters;
+import us.ihmc.atlas.parameters.*;
 import us.ihmc.atlas.ros.AtlasPPSTimestampOffsetProvider;
 import us.ihmc.atlas.sensors.AtlasCollisionBoxProvider;
 import us.ihmc.atlas.sensors.AtlasSensorSuiteManager;
@@ -243,10 +231,10 @@ public RobotDefinition createRobotDefinition(MaterialDefinition materialDefiniti
          RobotDefinitionTools.setDefaultMaterial(robotDefinition, new MaterialDefinition(ColorDefinitions.Black()));
 
       getRobotDefinitionMutator().accept(robotDefinition);
-      
+
       if (isUseHandMutatorCollisions())
          getRobotDefinitionHandMutator().accept(robotDefinition);
-      
+
       return robotDefinition;
    }
 
@@ -283,15 +271,14 @@ public Consumer<RobotDefinition> getRobotDefinitionMutator()
          robotDefinitionMutator = new AtlasRobotDefinitionMutator(getJointMap(), getSensorInformation());
       return robotDefinitionMutator;
    }
-   
+
    public Consumer<RobotDefinition> getRobotDefinitionHandMutator()
    {
       if (robotDefinitionHandMutator == null)
          robotDefinitionHandMutator = new AtlasRobotDefinitionHandMutator();
       return robotDefinitionHandMutator;
    }
 
-
    @Override
    public HighLevelControllerParameters getHighLevelControllerParameters()
    {
@@ -428,8 +415,9 @@ private FullHumanoidRobotModel doArmJointRestriction(FullHumanoidRobotModel full
             }
             else
             {
-               System.out.println(this.getClass().getName() + ", createFullRobotModel(): range not large enough to reduce for side="
-                     + robotSide.getLowerCaseName() + " joint=" + armJointName.getCamelCaseNameForStartOfExpression());
+               System.out.println(
+                     this.getClass().getName() + ", createFullRobotModel(): range not large enough to reduce for side=" + robotSide.getLowerCaseName()
+                     + " joint=" + armJointName.getCamelCaseNameForStartOfExpression());
             }
          }
       }
@@ -466,6 +454,12 @@ public double getControllerDT()
       return CONTROL_DT;
    }
 
+   @Override
+   public double getWBCCDT()
+   {
+      return 0.0;
+   }
+
    @Override
    public RobotROSClockCalculator getROSClockCalculator()
    {

diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/MPCWholeBodyControllerCoreTask.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/MPCWholeBodyControllerCoreTask.java
@@ -62,12 +62,14 @@ protected void updateMasterContext(HumanoidRobotMPCContextData masterContext)
    {
       runAll(schedulerThreadRunnables);
       //      masterResolver.resolveHumanoidRobotContextDataController(wholeBodyControllerCoreThread.getHumanoidRobotContextData(), masterContext);
+      wholeBodyControllerCoreResolver.resolveLowLevelOneDoFJointDesiredDataHolder(wholeBodyControllerCoreThread.getHumanoidRobotContextData().getJointDesiredOutputList(), masterContext.getJointDesiredOutputList());
    }
 
    protected void updateLocalContext(HumanoidRobotMPCContextData masterContext)
    {
       wholeBodyControllerCoreResolver.resolveHumanoidRobotContextDataScheduler(masterContext, (HumanoidRobotMPCContextData) wholeBodyControllerCoreThread.getHumanoidRobotContextData());
       wholeBodyControllerCoreResolver.resolveHumanoidRobotContextDataEstimator(masterContext, (HumanoidRobotMPCContextData) wholeBodyControllerCoreThread.getHumanoidRobotContextData());
+      wholeBodyControllerCoreResolver.resolveHumanoidRobotContextDataControllerCore(masterContext, (HumanoidRobotMPCContextData) wholeBodyControllerCoreThread.getHumanoidRobotContextData());
    }
 
    @Override

diff --git a/...hmc/commonWalkingControlModules/barrierScheduler/context/HumanoidRobotMPCContextData.java b/...hmc/commonWalkingControlModules/barrierScheduler/context/HumanoidRobotMPCContextData.java
@@ -66,6 +66,10 @@ public boolean getWholeBodyControllerCoreRan()
    {
       return wholeBodyControllerCoreRan;
    }
+   public LowLevelOneDoFJointDesiredDataHolder getMpcControllerDesiredOutputList()
+   {
+      return mpcControllerDesiredOutputList;
+   }
 
    public void set(HumanoidRobotMPCContextData other)
    {

diff --git a/...ihmc/commonWalkingControlModules/controllerCore/command/MPCCrossRobotCommandResolver.java b/...ihmc/commonWalkingControlModules/controllerCore/command/MPCCrossRobotCommandResolver.java
@@ -1,6 +1,5 @@
 package us.ihmc.commonWalkingControlModules.controllerCore.command;
 
-import us.ihmc.commonWalkingControlModules.barrierScheduler.context.HumanoidRobotContextData;
 import us.ihmc.commonWalkingControlModules.barrierScheduler.context.HumanoidRobotMPCContextData;
 import us.ihmc.robotModels.FullHumanoidRobotModel;
 import us.ihmc.robotModels.JointHashCodeResolver;
@@ -13,6 +12,7 @@ public MPCCrossRobotCommandResolver(FullHumanoidRobotModel fullHumanoidRobotMode
    {
       super(fullHumanoidRobotModel);
    }
+
    public MPCCrossRobotCommandResolver(ReferenceFrameHashCodeResolver referenceFrameHashCodeResolver,
                                        RigidBodyHashCodeResolver rigidBodyHashCodeResolver,
                                        JointHashCodeResolver jointHashCodeResolver)
@@ -26,13 +26,16 @@ public void resolveHumanoidRobotContextDataScheduler(HumanoidRobotMPCContextData
       out.setTimestamp(in.getTimestamp());
       out.setSchedulerTick(in.getSchedulerTick());
    }
+
    public void resolveHumanoidRobotContextDataController(HumanoidRobotMPCContextData in, HumanoidRobotMPCContextData out)
    {
       resolveCenterOfPressureDataHolder(in.getCenterOfPressureDataHolder(), out.getCenterOfPressureDataHolder());
       resolveRobotMotionStatusHolder(in.getRobotMotionStatusHolder(), out.getRobotMotionStatusHolder());
-      resolveLowLevelOneDoFJointDesiredDataHolder(in.getJointDesiredOutputList(), out.getJointDesiredOutputList());
+//      resolveLowLevelOneDoFJointDesiredDataHolder(in.getJointDesiredOutputList(), out.getJointDesiredOutputList());
+      resolveLowLevelOneDoFJointDesiredDataHolder(in.getMpcControllerDesiredOutputList(), out.getMpcControllerDesiredOutputList());
       out.setControllerRan(in.getControllerRan());
    }
+
    public void resolveHumanoidRobotContextDataEstimator(HumanoidRobotMPCContextData in, HumanoidRobotMPCContextData out)
    {
       resolveHumanoidRobotContextJointData(in.getProcessedJointData(), out.getProcessedJointData());
@@ -41,4 +44,8 @@ public void resolveHumanoidRobotContextDataEstimator(HumanoidRobotMPCContextData
       out.setEstimatorRan(in.getEstimatorRan());
    }
 
+   public void resolveHumanoidRobotContextDataControllerCore(HumanoidRobotMPCContextData in, HumanoidRobotMPCContextData out)
+   {
+      resolveLowLevelOneDoFJointDesiredDataHolder(in.getMpcControllerDesiredOutputList(), out.getMpcControllerDesiredOutputList());
+   }
 }
diff --git a/...alkingControlModules/highLevelHumanoidControl/HumanoidWholeBodyControllerCoreManager.java b/...alkingControlModules/highLevelHumanoidControl/HumanoidWholeBodyControllerCoreManager.java
@@ -125,7 +125,7 @@ public void doControl()
          //         controllerToolbox.reportControllerFailureToListeners(null);
       }
 
-//      copyJointDesiredsToJoints();
+      copyJointDesiredsToJoints();
       //      reportControllerCoreOutputDataHolderForController();
    }
 

diff --git a/...WalkingControlModules/highLevelHumanoidControl/MPCHumanoidHighLevelControllerManager.java b/...WalkingControlModules/highLevelHumanoidControl/MPCHumanoidHighLevelControllerManager.java
@@ -11,7 +11,6 @@
 import us.ihmc.commonWalkingControlModules.controllerCore.command.lowLevel.YoLowLevelOneDoFJointDesiredDataHolder;
 import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.factories.*;
 import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates.HighLevelControllerState;
-import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates.MPCHighLevelControllerState;
 import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.plugin.HighLevelHumanoidControllerPlugin;
 import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.plugin.HighLevelHumanoidControllerPluginFactory;
 import us.ihmc.commonWalkingControlModules.momentumBasedController.HighLevelHumanoidControllerToolbox;
@@ -345,7 +344,14 @@ private void copyJointDesiredsToJoints()
       }
 
       yoLowLevelOneDoFJointDesiredDataHolder.overwriteWith(lowLevelOneDoFJointDesiredDataHolder);
-      lowLevelControllerOutput.overwriteWith(lowLevelOneDoFJointDesiredDataHolder);
+
+      /**
+       * This line tells the output of this Controller saved to lowLevelControllerOutput which the dataType to be delivered to the actuator.
+       * The output will be saved into desiredMPCControllerOutput and be delivered to the wholebodycontrollercore thread. Then, the data is saved into
+       * lowLevelControllerOutput in `copyJointDesiredsToJoints` in `HumanoidWholeBodyControllerCoreManager`
+       */
+      //      lowLevelControllerOutput.overwriteWith(lowLevelOneDoFJointDesiredDataHolder);
+      desiredMPCControllerOutput.overwriteWith(lowLevelOneDoFJointDesiredDataHolder);
 
       RootJointDesiredConfigurationDataReadOnly rootJointDesiredConfiguration = stateMachine.getCurrentState().getOutputForRootJoint();
       if (rootJointDesiredConfiguration != null)

diff --git a/...y-controller/src/main/java/us/ihmc/wholeBodyController/WholeBodyControllerParameters.java b/...y-controller/src/main/java/us/ihmc/wholeBodyController/WholeBodyControllerParameters.java
@@ -1,64 +1,63 @@
 package us.ihmc.wholeBodyController;
 
-import java.io.InputStream;
-
 import us.ihmc.commonWalkingControlModules.capturePoint.splitFractionCalculation.DefaultSplitFractionCalculatorParameters;
 import us.ihmc.commonWalkingControlModules.capturePoint.splitFractionCalculation.SplitFractionCalculatorParametersReadOnly;
-import us.ihmc.commonWalkingControlModules.configurations.SteppingEnvironmentalConstraintParameters;
 import us.ihmc.commonWalkingControlModules.configurations.WalkingControllerParameters;
 import us.ihmc.commonWalkingControlModules.dynamicPlanning.bipedPlanning.CoPTrajectoryParameters;
 import us.ihmc.robotics.robotSide.RobotSegment;
 import us.ihmc.sensorProcessing.parameters.HumanoidRobotSensorInformation;
 import us.ihmc.sensorProcessing.stateEstimation.StateEstimatorParameters;
 
+import java.io.InputStream;
+
 public interface WholeBodyControllerParameters<E extends Enum<E> & RobotSegment<E>>
 {
-   public double getControllerDT();
+   double getControllerDT();
 
-   public default double getSimulatedHandControlDT()
+   default double getSimulatedHandControlDT()
    {
       return getControllerDT();
    }
-   public double getWBCCDT();
 
+   double getWBCCDT();
 
-   public StateEstimatorParameters getStateEstimatorParameters();
+   StateEstimatorParameters getStateEstimatorParameters();
 
-   public CoPTrajectoryParameters getCoPTrajectoryParameters();
+   CoPTrajectoryParameters getCoPTrajectoryParameters();
 
    default SplitFractionCalculatorParametersReadOnly getSplitFractionCalculatorParameters()
    {
       return new DefaultSplitFractionCalculatorParameters();
    }
 
-	public WalkingControllerParameters getWalkingControllerParameters();
+   WalkingControllerParameters getWalkingControllerParameters();
 
-	public RobotContactPointParameters<E> getContactPointParameters();
+   RobotContactPointParameters<E> getContactPointParameters();
 
-   public HumanoidRobotSensorInformation getSensorInformation();
+   HumanoidRobotSensorInformation getSensorInformation();
 
    /**
     * Get the parameter XML file for the controller.
-    *
+    * <p>
     * Each call to this method should return a new InputStream.
     * If null is returned the default values for the parameters are used.
     *
     * @return new InputStream with the controller parameters
     */
-   public InputStream getWholeBodyControllerParametersFile();
+   InputStream getWholeBodyControllerParametersFile();
 
    /**
     * Allows to overwrite parameters specified in the {@link #getWholeBodyControllerParametersFile()}
     * XML file. This is useful in unit tests when parameters need to be modified.
     *
     * @return InputStream with the parameters that need to be overwritten.
     */
-   public default InputStream getParameterOverwrites()
+   default InputStream getParameterOverwrites()
    {
       return null;
    }
-   
-   public default String getParameterFileName()
+
+   default String getParameterFileName()
    {
       return "not implemented";
    }