us.ihmc.atlas.AtlasRoughTerrainSimulation Maven / Gradle / Ivy

Go to download
Show more of this group Show more artifacts with this name
Show all versions of atlas
Atlas
There is a newer version: 0.14.0-240126
package us.ihmc.atlas;

import java.util.List;

import us.ihmc.avatar.RoughTerrainEnvironmentStartingLocation;
import us.ihmc.avatar.initialSetup.DRCSCSInitialSetup;
import us.ihmc.avatar.simulationStarter.DRCSimulationStarter;
import us.ihmc.avatar.simulationStarter.DRCSimulationTools;
import us.ihmc.robotics.robotSide.RobotSide;
import us.ihmc.simulationConstructionSetTools.util.environments.RoughTerrainEnvironment;
import us.ihmc.simulationconstructionset.SimulationConstructionSet;
import us.ihmc.wholeBodyController.AdditionalSimulationContactPoints;
import us.ihmc.wholeBodyController.FootContactPoints;
import us.ihmc.yoVariables.registry.YoRegistry;
import us.ihmc.yoVariables.variable.YoVariable;

public class AtlasRoughTerrainSimulation
{
   // Increase to 10 when you want the sims to run a little faster and don't need the data.
   private final int recordFrequencySpeedup = 1;
   
   // Set to true if you are walking over steps or things where your foot might overhang a little.
   private boolean addExtraContactPoints = false;
   
   public AtlasRoughTerrainSimulation()
   {
      FootContactPoints simulationContactPoints = null;
      if (addExtraContactPoints)
      {
         int nContactPointsX = 5;
         int nContactPointsY = 4;
         boolean edgePointsOnly = true;
         simulationContactPoints = new AdditionalSimulationContactPoints<>(RobotSide.values, nContactPointsX, nContactPointsY, edgePointsOnly, false);
      }

      AtlasRobotVersion version = AtlasRobotVersion.ATLAS_UNPLUGGED_V5_DUAL_ROBOTIQ;
      AtlasRobotModel robotModel = AtlasRobotModelFactory.createDefaultRobotModel(version, simulationContactPoints);

      DRCSimulationStarter simulationStarter = new DRCSimulationStarter(robotModel, new RoughTerrainEnvironment());
      DRCSCSInitialSetup scsInitialSetup = simulationStarter.getSCSInitialSetup();
      int recordFrequency = recordFrequencySpeedup * scsInitialSetup.getRecordFrequency();
      scsInitialSetup.setRecordFrequency(recordFrequency);
      simulationStarter.setRunMultiThreaded(true);

      simulationStarter.setInitializeEstimatorToActual(true);

      DRCSimulationTools.startSimulationWithGraphicSelector(simulationStarter, null, null, RoughTerrainEnvironmentStartingLocation.values());
      boolean printOutAllYoVariables = false;
      if (printOutAllYoVariables)
         printOutAllYoVariables(simulationStarter);
   }

   private void printOutAllYoVariables(DRCSimulationStarter simulationStarter)
   {
      SimulationConstructionSet simulationConstructionSet = simulationStarter.getSimulationConstructionSet();
      YoRegistry rootRegistry = simulationConstructionSet.getRootRegistry();

      List allVariablesIncludingDescendants = rootRegistry.collectSubtreeVariables();
      System.out.println("Size = " + allVariablesIncludingDescendants.size());
      for (YoVariable yoVariable : allVariablesIncludingDescendants)
      {
         System.out.println(yoVariable.getName());
      }
      System.out.println("Size = " + allVariablesIncludingDescendants.size());
   }

   public static void main(String[] args)
   {
      new AtlasRoughTerrainSimulation();
   }

}