diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeExecutor.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeExecutor.java index 4f4ef59a05cc..f65893fd1b36 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeExecutor.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeExecutor.java @@ -3,6 +3,8 @@ import us.ihmc.avatar.drcRobot.DRCRobotModel; import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel; import us.ihmc.avatar.ros2.ROS2ControllerHelper; +import us.ihmc.avatar.scs2.SCS2AvatarSimulation; +import us.ihmc.avatar.scs2.SCS2AvatarSimulationFactory; import us.ihmc.behaviors.behaviorTree.topology.BehaviorTreeExtensionSubtreeRebuilder; import us.ihmc.behaviors.behaviorTree.ros2.ROS2BehaviorTreeState; import us.ihmc.communication.crdt.CRDTInfo; @@ -18,6 +20,7 @@ public class BehaviorTreeExecutor private final BehaviorTreeExtensionSubtreeRebuilder treeRebuilder; private final BehaviorTreeState behaviorTreeState; private BehaviorTreeNodeExecutor rootNode; + private final SCS2AvatarSimulation previewSimulation; public BehaviorTreeExecutor(DRCRobotModel robotModel, ROS2SyncedRobotModel syncedRobot, @@ -30,6 +33,23 @@ public BehaviorTreeExecutor(DRCRobotModel robotModel, treeRebuilder = new BehaviorTreeExtensionSubtreeRebuilder(this::getRootNode, crdtInfo); behaviorTreeState = new BehaviorTreeState(nodeBuilder, treeRebuilder, this::getRootNode, crdtInfo, null); + + // TODO: Need to build a way to parameterize the Controller API topics to have a set for previewing + // Need to parameterize all behavior topics with preview vs. real robot + SCS2AvatarSimulationFactory previewSimulationFactory = new SCS2AvatarSimulationFactory(); + previewSimulationFactory.setRobotModel(robotModel); + // TODO: Probably need to change this to accept ROS2NodeInterface + // previewSimulationFactory.setRealtimeROS2Node(); + previewSimulationFactory.setDefaultHighLevelHumanoidControllerFactory(); + // TODO: Need a way of setting up robot in current configuration before you preview + // avatarSimulationFactory.setRobotInitialSetup(robotInitialSetup); + previewSimulationFactory.setKinematicsSimulation(true); + previewSimulationFactory.setUsePerfectSensors(true); + previewSimulationFactory.setSimulationDT(1.0 / 1500.0); + previewSimulationFactory.setSimulationDataRecordTickPeriod(20); + previewSimulationFactory.setSimulationDataBufferDuration(5.0); + previewSimulation = previewSimulationFactory.createAvatarSimulation(); + previewSimulation.setSystemExitOnDestroy(false); } public void update()