diff --git a/atlas/src/main/java/us/ihmc/atlas/AtlasRobotModel.java b/atlas/src/main/java/us/ihmc/atlas/AtlasRobotModel.java index 32a5e777068..45c3ea6005e 100644 --- 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,7 +271,7 @@ public Consumer getRobotDefinitionMutator() robotDefinitionMutator = new AtlasRobotDefinitionMutator(getJointMap(), getSensorInformation()); return robotDefinitionMutator; } - + public Consumer getRobotDefinitionHandMutator() { if (robotDefinitionHandMutator == null) @@ -291,7 +279,6 @@ public Consumer getRobotDefinitionHandMutator() 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 index 7a1f983e370..1aa6fdcd22a 100644 --- 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/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/barrierScheduler/context/HumanoidRobotMPCContextData.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/barrierScheduler/context/HumanoidRobotMPCContextData.java index 1ad7f7c977d..92c7eb870c0 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/barrierScheduler/context/HumanoidRobotMPCContextData.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/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-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controllerCore/command/MPCCrossRobotCommandResolver.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controllerCore/command/MPCCrossRobotCommandResolver.java index b44f80e84fa..e9b9c4afe8c 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controllerCore/command/MPCCrossRobotCommandResolver.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/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/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/HumanoidWholeBodyControllerCoreManager.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/HumanoidWholeBodyControllerCoreManager.java index 1c68c45cbb3..ae0afb90b3b 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/HumanoidWholeBodyControllerCoreManager.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/HumanoidWholeBodyControllerCoreManager.java @@ -125,7 +125,7 @@ public void doControl() // controllerToolbox.reportControllerFailureToListeners(null); } -// copyJointDesiredsToJoints(); + copyJointDesiredsToJoints(); // reportControllerCoreOutputDataHolderForController(); } diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/MPCHumanoidHighLevelControllerManager.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/MPCHumanoidHighLevelControllerManager.java index b2cb79235a3..2da3e564bc2 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/MPCHumanoidHighLevelControllerManager.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/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/ihmc-whole-body-controller/src/main/java/us/ihmc/wholeBodyController/WholeBodyControllerParameters.java b/ihmc-whole-body-controller/src/main/java/us/ihmc/wholeBodyController/WholeBodyControllerParameters.java index 37b57198d3a..8daafd0ca7c 100644 --- a/ihmc-whole-body-controller/src/main/java/us/ihmc/wholeBodyController/WholeBodyControllerParameters.java +++ b/ihmc-whole-body-controller/src/main/java/us/ihmc/wholeBodyController/WholeBodyControllerParameters.java @@ -1,51 +1,50 @@ 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 & RobotSegment> { - 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 getContactPointParameters(); + RobotContactPointParameters getContactPointParameters(); - public HumanoidRobotSensorInformation getSensorInformation(); + HumanoidRobotSensorInformation getSensorInformation(); /** * Get the parameter XML file for the controller. - * + *

* 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()} @@ -53,12 +52,12 @@ default SplitFractionCalculatorParametersReadOnly getSplitFractionCalculatorPara * * @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"; }