Skip to content

Commit

Permalink
- Adding HumanoidRobotMPCContextData and its factory
Browse files Browse the repository at this point in the history
  • Loading branch information
Beomyeong committed Dec 13, 2024
1 parent 51ef8b1 commit c092e3d
Show file tree
Hide file tree
Showing 6 changed files with 130 additions and 43 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -3,10 +3,7 @@
import controller_msgs.msg.dds.ControllerCrashNotificationPacket;
import us.ihmc.avatar.drcRobot.DRCRobotModel;
import us.ihmc.avatar.initialSetup.RobotInitialSetup;
import us.ihmc.commonWalkingControlModules.barrierScheduler.context.HumanoidRobotContextData;
import us.ihmc.commonWalkingControlModules.barrierScheduler.context.HumanoidRobotContextDataFactory;
import us.ihmc.commonWalkingControlModules.barrierScheduler.context.HumanoidRobotContextJointData;
import us.ihmc.commonWalkingControlModules.barrierScheduler.context.HumanoidRobotContextTools;
import us.ihmc.commonWalkingControlModules.barrierScheduler.context.*;
import us.ihmc.commonWalkingControlModules.controllerCore.command.lowLevel.LowLevelOneDoFJointDesiredDataHolder;
import us.ihmc.commonWalkingControlModules.corruptors.FullRobotModelCorruptor;
import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.MPCHumanoidHighLevelControllerManager;
Expand Down Expand Up @@ -84,7 +81,7 @@ public class AvatarMPCControllerThread implements AvatarControllerThreadInterfac

private final ROS2Publisher<ControllerCrashNotificationPacket> crashNotificationPublisher;

private final HumanoidRobotContextData humanoidRobotContextData;
private final HumanoidRobotMPCContextData humanoidRobotMPCContextData;

private final ExecutionTimer controllerThreadTimer;

Expand All @@ -93,7 +90,7 @@ public AvatarMPCControllerThread(String robotName,
RobotInitialSetup<?> robotInitialSetup,
HumanoidRobotSensorInformation sensorInformation,
MPCHighLevelHumanoidControllerFactory controllerFactory,
HumanoidRobotContextDataFactory contextDataFactory,
HumanoidRobotMPCContextDataFactory contextDataFactory,
DRCOutputProcessor outputProcessor,
RealtimeROS2Node realtimeROS2Node,
double gravity,
Expand All @@ -118,7 +115,7 @@ public AvatarMPCControllerThread(String robotName,
contextDataFactory.setJointDesiredOutputList(desiredJointDataHolder);
contextDataFactory.setProcessedJointData(processedJointData);
contextDataFactory.setSensorDataContext(new SensorDataContext(controllerFullRobotModel));
humanoidRobotContextData = contextDataFactory.createHumanoidRobotContextData();
humanoidRobotMPCContextData = contextDataFactory.createHumanoidRobotMPCContextData();

if (realtimeROS2Node != null)
{
Expand Down Expand Up @@ -300,10 +297,10 @@ private ModularRobotController createHighLevelController(FullHumanoidRobotModel
public void initialize()
{
firstTick.set(true);
humanoidRobotContextData.setControllerRan(false);
humanoidRobotContextData.setEstimatorRan(false);
humanoidRobotMPCContextData.setControllerRan(false);
humanoidRobotMPCContextData.setEstimatorRan(false);

LowLevelOneDoFJointDesiredDataHolder jointDesiredOutputList = humanoidRobotContextData.getJointDesiredOutputList();
LowLevelOneDoFJointDesiredDataHolder jointDesiredOutputList = humanoidRobotMPCContextData.getJointDesiredOutputList();

for (int i = 0; i < jointDesiredOutputList.getNumberOfJointsWithDesiredOutput(); i++)
{
Expand All @@ -316,16 +313,16 @@ public void run()
{
controllerThreadTimer.startMeasurement();

runController.set(humanoidRobotContextData.getEstimatorRan());
runController.set(humanoidRobotMPCContextData.getEstimatorRan());
if (!runController.getValue())
{
return;
}

try
{
HumanoidRobotContextTools.updateRobot(controllerFullRobotModel, humanoidRobotContextData.getProcessedJointData());
timestamp.set(humanoidRobotContextData.getTimestamp());
HumanoidRobotContextTools.updateRobot(controllerFullRobotModel, humanoidRobotMPCContextData.getProcessedJointData());
timestamp.set(humanoidRobotMPCContextData.getTimestamp());
if (firstTick.getValue())
{
// Record this to have time start at 0.0 on the real robot for viewing pleasure.
Expand All @@ -340,7 +337,7 @@ public void run()
}

robotController.doControl();
humanoidRobotContextData.setControllerRan(true);
humanoidRobotMPCContextData.setControllerRan(true);
}
catch (Exception e)
{
Expand Down Expand Up @@ -393,14 +390,14 @@ public FullHumanoidRobotModel getFullRobotModel()
return controllerFullRobotModel;
}

@Override
public HumanoidRobotContextData getHumanoidRobotContextData()
{
return humanoidRobotContextData;
return humanoidRobotMPCContextData;
}

// This method comes from implemented AvatarControllerThreadInterface, it was used in AvatarSimulation, not the SCS2.
public JointDesiredOutputListBasics getDesiredJointDataHolder()
{
return humanoidRobotContextData.getJointDesiredOutputList();
return humanoidRobotMPCContextData.getJointDesiredOutputList();
}
}
Original file line number Diff line number Diff line change
@@ -1,10 +1,7 @@
package us.ihmc.avatar;

import us.ihmc.avatar.drcRobot.DRCRobotModel;
import us.ihmc.commonWalkingControlModules.barrierScheduler.context.HumanoidRobotContextData;
import us.ihmc.commonWalkingControlModules.barrierScheduler.context.HumanoidRobotContextDataFactory;
import us.ihmc.commonWalkingControlModules.barrierScheduler.context.HumanoidRobotContextJointData;
import us.ihmc.commonWalkingControlModules.barrierScheduler.context.HumanoidRobotContextTools;
import us.ihmc.commonWalkingControlModules.barrierScheduler.context.*;
import us.ihmc.commonWalkingControlModules.controllerCore.command.lowLevel.LowLevelOneDoFJointDesiredDataHolder;
import us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator.FootstepValidityIndicator;
import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.plugin.HumanoidSteppingPlugin;
Expand Down Expand Up @@ -42,7 +39,7 @@ public class AvatarMPCStepGeneratorThread implements AvatarControllerThreadInter
private final HumanoidSteppingPlugin continuousStepGeneratorPlugin;
private final FullHumanoidRobotModel fullRobotModel;

private final HumanoidRobotContextData humanoidRobotContextData;
private final HumanoidRobotMPCContextData humanoidRobotMPCContextData;
private final HumanoidReferenceFrames humanoidReferenceFrames;
private final YoBoolean firstTick = new YoBoolean("FirstTick", csgRegistry);
private final YoLong timestampOffset = new YoLong("TimestampOffsetCSG", csgRegistry);
Expand All @@ -53,7 +50,7 @@ public class AvatarMPCStepGeneratorThread implements AvatarControllerThreadInter
private final StepGeneratorCommandInputManager csgCommandInputManager;

public AvatarMPCStepGeneratorThread(HumanoidSteppingPluginFactory pluginFactory,
HumanoidRobotContextDataFactory contextDataFactory,
HumanoidRobotMPCContextDataFactory contextDataFactory,
StatusMessageOutputManager walkingOutputManager,
CommandInputManager walkingCommandInputManager,
DRCRobotModel drcRobotModel,
Expand All @@ -75,7 +72,7 @@ public AvatarMPCStepGeneratorThread(HumanoidSteppingPluginFactory pluginFactory,
contextDataFactory.setJointDesiredOutputList(desiredJointDataHolder);
contextDataFactory.setProcessedJointData(processedJointData);
contextDataFactory.setSensorDataContext(new SensorDataContext(fullRobotModel));
humanoidRobotContextData = contextDataFactory.createHumanoidRobotContextData();
humanoidRobotMPCContextData = contextDataFactory.createHumanoidRobotMPCContextData();

csgCommandInputManager = pluginFactory.getStepGeneratorCommandInputManager();

Expand Down Expand Up @@ -124,8 +121,8 @@ public AvatarMPCStepGeneratorThread(HumanoidSteppingPluginFactory pluginFactory,
public void initialize()
{
firstTick.set(true);
humanoidRobotContextData.setControllerRan(false);
humanoidRobotContextData.setEstimatorRan(false);
humanoidRobotMPCContextData.setControllerRan(false);
humanoidRobotMPCContextData.setEstimatorRan(false);
}

private void runOnFirstTick()
Expand All @@ -135,18 +132,18 @@ private void runOnFirstTick()
@Override
public void run()
{
runCSG.set(humanoidRobotContextData.getEstimatorRan());
runCSG.set(humanoidRobotMPCContextData.getEstimatorRan());
if (!runCSG.getValue())
{
return;
}

try
{
HumanoidRobotContextTools.updateRobot(fullRobotModel, humanoidRobotContextData.getProcessedJointData());
HumanoidRobotContextTools.updateRobot(fullRobotModel, humanoidRobotMPCContextData.getProcessedJointData());
humanoidReferenceFrames.updateFrames();

timestamp.set(humanoidRobotContextData.getTimestamp());
timestamp.set(humanoidRobotMPCContextData.getTimestamp());
if (firstTick.getValue())
{
// Record this to have time start at 0.0 on the real robot for viewing pleasure.
Expand All @@ -161,7 +158,7 @@ public void run()
}

continuousStepGeneratorPlugin.update(csgTime.getValue());
humanoidRobotContextData.setPerceptionRan(true);
humanoidRobotMPCContextData.setPerceptionRan(true);
}
catch (Exception e)
{
Expand Down Expand Up @@ -194,10 +191,9 @@ public FullHumanoidRobotModel getFullRobotModel()
return fullRobotModel;
}

@Override
public HumanoidRobotContextData getHumanoidRobotContextData()
{
return humanoidRobotContextData;
return humanoidRobotMPCContextData;
}

public StepGeneratorCommandInputManager getCsgCommandInputManager()
Expand Down
Original file line number Diff line number Diff line change
@@ -1,10 +1,7 @@
package us.ihmc.avatar;

import us.ihmc.avatar.drcRobot.DRCRobotModel;
import us.ihmc.commonWalkingControlModules.barrierScheduler.context.HumanoidRobotContextData;
import us.ihmc.commonWalkingControlModules.barrierScheduler.context.HumanoidRobotContextDataFactory;
import us.ihmc.commonWalkingControlModules.barrierScheduler.context.HumanoidRobotContextJointData;
import us.ihmc.commonWalkingControlModules.barrierScheduler.context.HumanoidRobotContextTools;
import us.ihmc.commonWalkingControlModules.barrierScheduler.context.*;
import us.ihmc.commonWalkingControlModules.controllerCore.command.lowLevel.LowLevelOneDoFJointDesiredDataHolder;
import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.HumanoidWholeBodyControllerCoreManager;
import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.factories.MPCHighLevelHumanoidControllerFactory;
Expand Down Expand Up @@ -51,15 +48,15 @@ public class AvatarMPCWholeBodyControllerCoreThread implements AvatarControllerT
private final YoBoolean firstTick = new YoBoolean("FirstTick", registry);
private final YoLong timeStamp = new YoLong("TimeStampWholeBodyControllerCore", registry);
private final YoLong timeStampOffset = new YoLong("TimestampOffsetWholeBodyControllerCore", registry);
private final HumanoidRobotContextData humanoidRobotContextData;
private final HumanoidRobotMPCContextData humanoidRobotContextData;
private final YoBoolean runWholeBodyControllerCore = new YoBoolean("RunWholeBodyControllerCore", registry);
private final YoGraphicsListRegistry yoGraphicsListRegistry = new YoGraphicsListRegistry();
private final List<Supplier<YoGraphicDefinition>> scs2YoGraphicHolders = new ArrayList<>();
private ModularRobotController wholeBodyControllerCoreCalculator = new ModularRobotController("WBCC");
private final ExecutionTimer wholeBodyControllerCoreThreadTimer;

public AvatarMPCWholeBodyControllerCoreThread(String robotName,
HumanoidRobotContextDataFactory contextDataFactory,
HumanoidRobotMPCContextDataFactory contextDataFactory,
StatusMessageOutputManager walkingOutputManager,
DRCRobotModel robotModel,
HumanoidRobotSensorInformation sensorInformation,
Expand Down Expand Up @@ -93,7 +90,7 @@ public AvatarMPCWholeBodyControllerCoreThread(String robotName,
// contextDataFactory.setControllerCoreOutputDataHolder(controllerCoreOutPutDataHolder);
// contextDataFactory.setControllerCoreCommandDataHolder(controllerCoreCommandDataHolder);
contextDataFactory.setSensorDataContext(new SensorDataContext(controllerCoreFullRobotModel));
humanoidRobotContextData = contextDataFactory.createHumanoidRobotContextData();
humanoidRobotContextData = contextDataFactory.createHumanoidRobotMPCContextData();

/**
* Something related to realtimeROS2Node should be here?
Expand Down Expand Up @@ -294,7 +291,6 @@ public FullHumanoidRobotModel getFullRobotModel()
return controllerCoreFullRobotModel;
}

@Override
public HumanoidRobotContextData getHumanoidRobotContextData()
{
return humanoidRobotContextData;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
import us.ihmc.avatar.networkProcessor.kinemtaticsStreamingToolboxModule.KinematicsStreamingToolboxParameters;
import us.ihmc.commonWalkingControlModules.barrierScheduler.context.HumanoidRobotContextData;
import us.ihmc.commonWalkingControlModules.barrierScheduler.context.HumanoidRobotContextDataFactory;
import us.ihmc.commonWalkingControlModules.barrierScheduler.context.HumanoidRobotMPCContextDataFactory;
import us.ihmc.commonWalkingControlModules.configurations.HighLevelControllerParameters;
import us.ihmc.commonWalkingControlModules.configurations.WalkingControllerParameters;
import us.ihmc.commonWalkingControlModules.controllerCore.command.lowLevel.RootJointDesiredConfigurationDataReadOnly;
Expand Down Expand Up @@ -411,7 +412,7 @@ private void setupStateEstimationThread()
private void setupControllerThread()
{
String robotName = robotModel.get().getSimpleRobotName();
HumanoidRobotContextDataFactory contextDataFactory = new HumanoidRobotContextDataFactory();
HumanoidRobotMPCContextDataFactory contextDataFactory = new HumanoidRobotMPCContextDataFactory();

RealtimeROS2Node ros2Node = null;
if (realtimeROS2Node.hasBeenSet())
Expand All @@ -437,7 +438,7 @@ private void setupControllerThread()

private void setupStepGeneratorThread()
{
HumanoidRobotContextDataFactory contextDataFactory = new HumanoidRobotContextDataFactory();
HumanoidRobotMPCContextDataFactory contextDataFactory = new HumanoidRobotMPCContextDataFactory();

HumanoidSteppingPluginFactory steppingFactory;
HumanoidSteppingPluginEnvironmentalConstraints stepSnapperUpdatable = null;
Expand Down Expand Up @@ -514,7 +515,7 @@ private void setupIKStreamingRTControllerThread()
private void setupWholeBodyControllerCoreThread()
{
String robotName = robotModel.get().getSimpleRobotName();
HumanoidRobotContextDataFactory contextDataFactory = new HumanoidRobotContextDataFactory();
HumanoidRobotMPCContextDataFactory contextDataFactory = new HumanoidRobotMPCContextDataFactory();

RealtimeROS2Node ros2Node = null;
if (realtimeROS2Node.hasBeenSet())
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,78 @@
package us.ihmc.commonWalkingControlModules.barrierScheduler.context;

import us.ihmc.commonWalkingControlModules.controllerCore.command.lowLevel.LowLevelOneDoFJointDesiredDataHolder;
import us.ihmc.humanoidRobotics.model.CenterOfPressureDataHolder;
import us.ihmc.mecano.multiBodySystem.interfaces.OneDoFJointBasics;
import us.ihmc.robotModels.FullHumanoidRobotModel;
import us.ihmc.robotics.sensors.CenterOfMassDataHolder;
import us.ihmc.robotics.sensors.ForceSensorDataHolder;
import us.ihmc.sensorProcessing.model.RobotMotionStatusHolder;
import us.ihmc.sensorProcessing.simulatedSensors.SensorDataContext;

import java.util.List;

/**
* @author Doug Stephen <a href="mailto:[email protected]">([email protected])</a>
*/
@SuppressWarnings("serial")
public class HumanoidRobotMPCContextData extends HumanoidRobotContextData
{
public HumanoidRobotMPCContextData()
{
super();
}

public HumanoidRobotMPCContextData(HumanoidRobotContextJointData processedJointData,
ForceSensorDataHolder forceSensorDataHolder,
CenterOfMassDataHolder centerOfMassDataHolder,
CenterOfPressureDataHolder centerOfPressureDataHolder,
RobotMotionStatusHolder robotMotionStatusHolder,
LowLevelOneDoFJointDesiredDataHolder jointDesiredOutputList,
SensorDataContext sensorDataContext)
{
super(processedJointData,
forceSensorDataHolder,
centerOfMassDataHolder,
centerOfPressureDataHolder,
robotMotionStatusHolder,
jointDesiredOutputList,
sensorDataContext);
}

public HumanoidRobotMPCContextData(FullHumanoidRobotModel fullRobotModel)
{
super(fullRobotModel);
}

public HumanoidRobotMPCContextData(List<OneDoFJointBasics> joints)
{
super(joints);
}


public void set(HumanoidRobotMPCContextData other)
{
copyFrom(other);
}

public void copyFrom(HumanoidRobotMPCContextData src)
{
super.copyFrom(src);
}
public boolean equals(Object obj)
{
if(obj == this)
{
return true;
}
else if(obj instanceof HumanoidRobotMPCContextData)
{
HumanoidRobotMPCContextData other = (HumanoidRobotMPCContextData) obj;
return super.equals(other);
}
else
{
return false;
}
}
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,19 @@
package us.ihmc.commonWalkingControlModules.barrierScheduler.context;

import us.ihmc.tools.factories.FactoryTools;

public class HumanoidRobotMPCContextDataFactory extends HumanoidRobotContextDataFactory
{
public HumanoidRobotMPCContextData createHumanoidRobotMPCContextData()
{
FactoryTools.checkAllFactoryFieldsAreSet(this);

return new HumanoidRobotMPCContextData(processedJointData.get(),
forceSensorDataHolder.get(),
centerOfMassDataHolder.get(),
centerOfPressureDataHolder.get(),
robotMotionStatusHolder.get(),
jointDesiredOutputList.get(),
sensorDataContext.get());
}
}

0 comments on commit c092e3d

Please sign in to comment.