us.ihmc.atlas.AtlasFlatGroundWalkingTrackSCS2Bullet Maven / Gradle / Ivy
package us.ihmc.atlas;
import static us.ihmc.humanoidRobotics.communication.packets.dataobjects.HighLevelControllerName.DO_NOTHING_BEHAVIOR;
import static us.ihmc.humanoidRobotics.communication.packets.dataobjects.HighLevelControllerName.WALKING;
import java.util.ArrayList;
import us.ihmc.avatar.drcRobot.DRCRobotModel;
import us.ihmc.avatar.initialSetup.RobotInitialSetup;
import us.ihmc.avatar.scs2.SCS2AvatarSimulation;
import us.ihmc.avatar.scs2.SCS2AvatarSimulationFactory;
import us.ihmc.commonWalkingControlModules.configurations.HighLevelControllerParameters;
import us.ihmc.commonWalkingControlModules.configurations.WalkingControllerParameters;
import us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator.HeadingAndVelocityEvaluationScriptParameters;
import us.ihmc.commonWalkingControlModules.dynamicPlanning.bipedPlanning.CoPTrajectoryParameters;
import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.factories.ContactableBodiesFactory;
import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.factories.HighLevelHumanoidControllerFactory;
import us.ihmc.communication.ROS2Tools;
import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.humanoidRobotics.communication.packets.dataobjects.HighLevelControllerName;
import us.ihmc.pubsub.DomainFactory.PubSubImplementation;
import us.ihmc.robotics.robotSide.RobotSide;
import us.ihmc.robotics.robotSide.SideDependentList;
import us.ihmc.ros2.RealtimeROS2Node;
import us.ihmc.sensorProcessing.parameters.HumanoidRobotSensorInformation;
import us.ihmc.simulationConstructionSetTools.util.HumanoidFloatingRootJointRobot;
import us.ihmc.simulationConstructionSetTools.util.environments.FlatGroundEnvironment;
import us.ihmc.wholeBodyController.RobotContactPointParameters;
public class AtlasFlatGroundWalkingTrackSCS2Bullet
{
private static final boolean USE_STAND_PREP = false;
private static boolean createYoVariableServer = System.getProperty("create.yovariable.server") != null
&& Boolean.parseBoolean(System.getProperty("create.yovariable.server"));
private final RealtimeROS2Node realtimeROS2Node = ROS2Tools.createRealtimeROS2Node(PubSubImplementation.INTRAPROCESS,
"flat_ground_walking_track_simulation");
//private static final double SIMULATION_DT = 0.001;
public AtlasFlatGroundWalkingTrackSCS2Bullet()
{
AtlasRobotModel robotModel = new AtlasRobotModel(AtlasRobotVersion.ATLAS_UNPLUGGED_V5_DUAL_ROBOTIQ);
robotModel.setUseHandMutatorCollisions(true);
robotModel.setUseSDFCollisions(true);
FlatGroundEnvironment environment = new FlatGroundEnvironment();
boolean useVelocityAndHeadingScript = true;
HeadingAndVelocityEvaluationScriptParameters walkingScriptParameters = new HeadingAndVelocityEvaluationScriptParameters();
double initialYaw = 0.3;
RobotInitialSetup robotInitialSetup = robotModel.getDefaultRobotInitialSetup(0.0, initialYaw);
SCS2AvatarSimulationFactory avatarSimulationFactory = new SCS2AvatarSimulationFactory();
//avatarSimulationFactory.setSimulationDT(SIMULATION_DT);
avatarSimulationFactory.setRobotModel(robotModel);
avatarSimulationFactory.setRealtimeROS2Node(realtimeROS2Node);
if (USE_STAND_PREP)
setupStandPrepController(robotModel, realtimeROS2Node, avatarSimulationFactory);
else
avatarSimulationFactory.setDefaultHighLevelHumanoidControllerFactory(useVelocityAndHeadingScript, walkingScriptParameters);
avatarSimulationFactory.setCommonAvatarEnvrionmentInterface(environment);
avatarSimulationFactory.setRobotInitialSetup(robotInitialSetup);
//avatarSimulationFactory.setSimulationDataRecordTimePeriod(robotModel.getControllerDT());
avatarSimulationFactory.setCreateYoVariableServer(createYoVariableServer);
avatarSimulationFactory.setUseBulletPhysicsEngine(true);
avatarSimulationFactory.setBulletCollisionMutator(AtlasSCS2BulletSimulationTools::fixHumanoidCollisionGroupsMasksToPreventSelfCollision);
avatarSimulationFactory.setUseRobotDefinitionCollisions(true);
SCS2AvatarSimulation avatarSimulation = avatarSimulationFactory.createAvatarSimulation();
avatarSimulation.start();
avatarSimulation.getSimulationConstructionSet().setCameraFocusPosition(0.3, 0.0, 1.0);
avatarSimulation.getSimulationConstructionSet().setCameraPosition(7.0, 4.0, 3.0);
}
public static HighLevelHumanoidControllerFactory setupStandPrepController(DRCRobotModel robotModel, RealtimeROS2Node realtimeROS2Node, SCS2AvatarSimulationFactory simulationFactory)
{
HighLevelControllerParameters highLevelControllerParameters = robotModel.getHighLevelControllerParameters();
WalkingControllerParameters walkingControllerParameters = robotModel.getWalkingControllerParameters();
CoPTrajectoryParameters copTrajectoryParameters = robotModel.getCoPTrajectoryParameters();
HumanoidRobotSensorInformation sensorInformation = robotModel.getSensorInformation();
SideDependentList feetForceSensorNames = sensorInformation.getFeetForceSensorNames();
SideDependentList wristForceSensorNames = sensorInformation.getWristForceSensorNames();
RobotContactPointParameters contactPointParameters = robotModel.getContactPointParameters();
ArrayList additionalContactRigidBodyNames = contactPointParameters.getAdditionalContactRigidBodyNames();
ArrayList additionalContactNames = contactPointParameters.getAdditionalContactNames();
ArrayList additionalContactTransforms = contactPointParameters.getAdditionalContactTransforms();
ContactableBodiesFactory contactableBodiesFactory = new ContactableBodiesFactory<>();
contactableBodiesFactory.setFootContactPoints(contactPointParameters.getFootContactPoints());
contactableBodiesFactory.setToeContactParameters(contactPointParameters.getControllerToeContactPoints(),
contactPointParameters.getControllerToeContactLines());
for (int i = 0; i < contactPointParameters.getAdditionalContactNames().size(); i++)
contactableBodiesFactory.addAdditionalContactPoint(additionalContactRigidBodyNames.get(i),
additionalContactNames.get(i),
additionalContactTransforms.get(i));
HighLevelHumanoidControllerFactory controllerFactory = new HighLevelHumanoidControllerFactory(contactableBodiesFactory,
feetForceSensorNames,
wristForceSensorNames,
highLevelControllerParameters,
walkingControllerParameters,
copTrajectoryParameters);
HighLevelControllerName fallbackControllerState = highLevelControllerParameters.getFallbackControllerState();
controllerFactory.useDefaultDoNothingControlState();
controllerFactory.useDefaultWalkingControlState();
controllerFactory.useDefaultStandPrepControlState();
controllerFactory.addRequestableTransition(DO_NOTHING_BEHAVIOR, WALKING);
controllerFactory.addRequestableTransition(WALKING, DO_NOTHING_BEHAVIOR);
controllerFactory.addControllerFailureTransition(DO_NOTHING_BEHAVIOR, fallbackControllerState);
controllerFactory.addControllerFailureTransition(WALKING, fallbackControllerState);
controllerFactory.setInitialState(HighLevelControllerName.STAND_PREP_STATE);
controllerFactory.createControllerNetworkSubscriber(robotModel.getSimpleRobotName(), realtimeROS2Node);
simulationFactory.setHighLevelHumanoidControllerFactory(controllerFactory);
return controllerFactory;
}
public static void main(String[] args)
{
new AtlasFlatGroundWalkingTrackSCS2Bullet();
}
}