public void initializeRobot(Robot robot, DRCRobotModel robotModel, YoGraphicsListRegistry yoGraphicsListRegistry) { robot.setGravity(gravity); LinearGroundContactModel groundContactModel = new LinearGroundContactModel(robot, robot.getRobotsYoVariableRegistry()); robotModel.getContactPointParameters().setupGroundContactModelParameters(groundContactModel); // if ((commonTerrain.getSteppingStones() != null) && (yoGraphicsListRegistry != null)) // commonTerrain.registerSteppingStonesArtifact(yoGraphicsListRegistry); // groundContactModel.setGroundProfile(commonTerrain.getGroundProfile()); if (groundProfile3D != null) groundContactModel.setGroundProfile3D(groundProfile3D); // TODO: change this to scs.setGroundContactModel(groundContactModel); robot.setGroundContactModel(groundContactModel); }
String[] imuSensorsToUseInStateEstimator = robotModel.getSensorInformation().getIMUSensorsToUseInStateEstimator(); double gravitationalAcceleration = -9.80; ContactableBodiesFactory contactableBodiesFactory = robotModel.getContactPointParameters().getContactableBodiesFactory(); CommonHumanoidReferenceFrames referenceFrames = new HumanoidReferenceFrames(estimatorFullRobotModel); SideDependentList<? extends ContactablePlaneBody> bipedFeet = contactableBodiesFactory.createFootContactableBodies(estimatorFullRobotModel, referenceFrames);
robotModel.getContactPointParameters(), robotModel, robotModel.getStateEstimatorParameters(), sensorReaderFactory, threadDataSynchronizer, realtimeRos2Node, null, null, robotVisualizer, gravity);
public FootstepPlanningToolboxModule(DRCRobotModel drcRobotModel, LogModelProvider modelProvider, boolean startYoVariableServer, DomainFactory.PubSubImplementation pubSubImplementation) throws IOException { super(drcRobotModel.getSimpleRobotName(), drcRobotModel.createFullRobotModel(), modelProvider, startYoVariableServer, pubSubImplementation); setTimeWithoutInputsBeforeGoingToSleep(Double.POSITIVE_INFINITY); footstepPlanningToolboxController = new FootstepPlanningToolboxController(drcRobotModel.getContactPointParameters(), drcRobotModel.getFootstepPlannerParameters(), drcRobotModel.getVisibilityGraphsParameters(), statusOutputManager, registry, yoGraphicsListRegistry, Conversions.millisecondsToSeconds(DEFAULT_UPDATE_PERIOD_MILLISECONDS)); footstepPlanningToolboxController.setTextToSpeechPublisher(textToSpeechPublisher); startYoVariableServer(); }
public void initializeRobot(Robot robot, DRCRobotModel robotModel, YoGraphicsListRegistry yoGraphicsListRegistry) { robot.setGravity(gravity); LinearGroundContactModel groundContactModel = new LinearGroundContactModel(robot, robot.getRobotsYoVariableRegistry()); robotModel.getContactPointParameters().setupGroundContactModelParameters(groundContactModel, simulateDT); if (enableGroundSlipping) groundContactModel.enableSlipping(); if (Double.isFinite(groundAlphaStick) && Double.isFinite(groundAlphaSlip)) groundContactModel.setAlphaStickSlip(groundAlphaStick, groundAlphaSlip); if (groundProfile3D != null) groundContactModel.setGroundProfile3D(groundProfile3D); robot.setGroundContactModel(groundContactModel); }
public FootstepPlanningToolboxController(DRCRobotModel drcRobotModel, FullHumanoidRobotModel fullHumanoidRobotModel, StatusMessageOutputManager statusOutputManager, PacketCommunicator packetCommunicator, YoVariableRegistry parentRegistry, double dt) { super(statusOutputManager, parentRegistry); this.packetCommunicator = packetCommunicator; this.contactPointParameters = drcRobotModel.getContactPointParameters(); this.walkingControllerParameters = drcRobotModel.getWalkingControllerParameters(); this.dt = dt; packetCommunicator.attachListener(PlanarRegionsListMessage.class, createPlanarRegionsConsumer()); // SideDependentList<ConvexPolygon2d> footPolygons = new SideDependentList<>(); // for (RobotSide side : RobotSide.values) // footPolygons.set(side, new ConvexPolygon2d(contactPointParameters.getFootContactPoints().get(side))); SideDependentList<ConvexPolygon2d> planningPolygonsInSoleFrame = FootstepPlannerForBehaviorsHelper.createDefaultFootPolygonsForAnytimePlannerAndPlannerToolbox(contactPointParameters); SideDependentList<ConvexPolygon2d> controllerPolygonsInSoleFrame = FootstepPlannerForBehaviorsHelper.createDefaultFootPolygons(contactPointParameters, 1.0, 1.0); humanoidReferenceFrames = createHumanoidReferenceFrames(fullHumanoidRobotModel); footstepDataListWithSwingOverTrajectoriesAssembler = new FootstepDataListWithSwingOverTrajectoriesAssembler(humanoidReferenceFrames, walkingControllerParameters, parentRegistry, new YoGraphicsListRegistry()); plannerMap.put(Planners.PLANAR_REGION_BIPEDAL, createPlanarRegionBipedalPlanner(planningPolygonsInSoleFrame, controllerPolygonsInSoleFrame)); plannerMap.put(Planners.PLAN_THEN_SNAP, new PlanThenSnapPlanner(new TurnWalkTurnPlanner(), planningPolygonsInSoleFrame)); activePlanner.set(Planners.PLANAR_REGION_BIPEDAL); usePlanarRegions.set(true); isDone.set(true); }
private DRCSimulationStarter(DRCRobotModel robotModel, CommonAvatarEnvironmentInterface environment, GroundProfile3D groundProfile3D) { this.robotModel = robotModel; this.environment = environment; this.guiInitialSetup = new DRCGuiInitialSetup(false, false); this.robotInitialSetup = robotModel.getDefaultRobotInitialSetup(0, 0); this.createSCSSimulatedSensors = true; this.scsInitialSetup = new DRCSCSInitialSetup(groundProfile3D, robotModel.getSimulateDT()); this.scsInitialSetup.setDrawGroundProfile(environment == null); this.scsInitialSetup.setInitializeEstimatorToActual(false); this.scsInitialSetup.setTimePerRecordTick(robotModel.getControllerDT()); this.scsInitialSetup.setRunMultiThreaded(true); this.highLevelControllerParameters = robotModel.getHighLevelControllerParameters(); this.walkingControllerParameters = robotModel.getWalkingControllerParameters(); this.capturePointPlannerParameters = robotModel.getCapturePointPlannerParameters(); this.contactPointParameters = robotModel.getContactPointParameters(); }
public DRCSimulationStarter(DRCRobotModel robotModel, CommonAvatarEnvironmentInterface environment) { this.robotModel = robotModel; this.environment = environment; this.guiInitialSetup = new DRCGuiInitialSetup(false, false); this.robotInitialSetup = robotModel.getDefaultRobotInitialSetup(0, 0); this.createSCSSimulatedSensors = true; scsInitialSetup = new DRCSCSInitialSetup(environment, robotModel.getSimulateDT()); scsInitialSetup.setInitializeEstimatorToActual(false); scsInitialSetup.setTimePerRecordTick(robotModel.getControllerDT()); scsInitialSetup.setRunMultiThreaded(true); this.walkingControllerParameters = robotModel.getWalkingControllerParameters(); this.armControllerParameters = robotModel.getArmControllerParameters(); this.capturePointPlannerParameters = robotModel.getCapturePointPlannerParameters(); this.icpOptimizationParameters = robotModel.getICPOptimizationParameters(); this.contactPointParameters = robotModel.getContactPointParameters(); }
private HighLevelHumanoidControllerFactory createHighLevelHumanoidControllerFactory(DRCRobotModel robotModel, PacketCommunicator packetCommunicator) RobotContactPointParameters<RobotSide> contactPointParameters = robotModel.getContactPointParameters(); ArrayList<String> additionalContactRigidBodyNames = contactPointParameters.getAdditionalContactRigidBodyNames(); ArrayList<String> additionalContactNames = contactPointParameters.getAdditionalContactNames();
RobotContactPointParameters<RobotSide> contactPointParameters = robotModel.getContactPointParameters(); DRCRobotSensorInformation sensorInformation = robotModel.getSensorInformation(); StateEstimatorParameters stateEstimatorParameters = robotModel.getStateEstimatorParameters();
RobotContactPointParameters contactPointParameters = robotModel.getContactPointParameters(); ContactableBodiesFactory contactableBodiesFactory = contactPointParameters.getContactableBodiesFactory(); SideDependentList<? extends ContactablePlaneBody> bipedFeet = contactableBodiesFactory.createFootContactableBodies(fullRobotModel, humanoidReferenceFrames);
private void setupStateEstimationThread() { stateEstimationThread = new DRCEstimatorThread(robotModel.get().getSensorInformation(), robotModel.get().getContactPointParameters(), robotModel.get().getStateEstimatorParameters(), sensorReaderFactory, threadDataSynchronizer, new PeriodicNonRealtimeThreadScheduler("DRCSimGazeboYoVariableServer"), humanoidGlobalDataProducer.get(), yoVariableServer, gravity.get()); if (humanoidGlobalDataProducer.get() != null) { PelvisPoseCorrectionCommunicatorInterface pelvisPoseCorrectionCommunicator = new PelvisPoseCorrectionCommunicator(humanoidGlobalDataProducer.get()); humanoidGlobalDataProducer.get().attachListener(StampedPosePacket.class, pelvisPoseCorrectionCommunicator); stateEstimationThread.setExternalPelvisCorrectorSubscriber(pelvisPoseCorrectionCommunicator); } else { stateEstimationThread.setExternalPelvisCorrectorSubscriber(null); } }
SideDependentList<String> wristForceSensorNames = sensorInformation.getWristForceSensorNames(); RobotContactPointParameters<RobotSide> contactPointParameters = model.getContactPointParameters(); ArrayList<String> additionalContactRigidBodyNames = contactPointParameters.getAdditionalContactRigidBodyNames(); ArrayList<String> additionalContactNames = contactPointParameters.getAdditionalContactNames();
RobotContactPointParameters<RobotSide> contactPointParameters = robotModel.getContactPointParameters(); ArrayList<String> additionalContactRigidBodyNames = contactPointParameters.getAdditionalContactRigidBodyNames(); ArrayList<String> additionalContactNames = contactPointParameters.getAdditionalContactNames();
this.walkingControllerParameters = model.getWalkingControllerParameters(); ContactableBodiesFactory contactableBodiesFactory = model.getContactPointParameters().getContactableBodiesFactory(); contactableFeet = contactableBodiesFactory.createFootContactableBodies(fullRobotModel, referenceFrames);
footstepPlanningController = new MultiStageFootstepPlanningController(drcRobotModel.getContactPointParameters(), drcRobotModel.getFootstepPlannerParameters(), drcRobotModel.getVisibilityGraphsParameters(), commandInputManager,
private void setupStateEstimationThread() { String robotName = robotModel.get().getSimpleRobotName(); MessageTopicNameGenerator publisherTopicNameGenerator = ControllerAPIDefinition.getPublisherTopicNameGenerator(robotName); MessageTopicNameGenerator subscriberTopicNameGenerator = ControllerAPIDefinition.getSubscriberTopicNameGenerator(robotName); PelvisPoseCorrectionCommunicatorInterface pelvisPoseCorrectionCommunicator; if (externalPelvisCorrectorSubscriber.hasValue()) { pelvisPoseCorrectionCommunicator = externalPelvisCorrectorSubscriber.get(); } else { pelvisPoseCorrectionCommunicator = new PelvisPoseCorrectionCommunicator(realtimeRos2Node.get(), publisherTopicNameGenerator); ROS2Tools.createCallbackSubscription(realtimeRos2Node.get(), StampedPosePacket.class, subscriberTopicNameGenerator, s -> pelvisPoseCorrectionCommunicator.receivedPacket(s.takeNextData())); } stateEstimationThread = new DRCEstimatorThread(robotName, robotModel.get().getSensorInformation(), robotModel.get().getContactPointParameters(), robotModel.get(), robotModel.get().getStateEstimatorParameters(), sensorReaderFactory, threadDataSynchronizer, realtimeRos2Node.get(), pelvisPoseCorrectionCommunicator, simulationOutputWriter, yoVariableServer, gravity.get()); }
RobotContactPointParameters contactPointParameters = model.getContactPointParameters(); CapturePointPlannerParameters capturePointPlannerParameters = model.getCapturePointPlannerParameters(); ICPOptimizationParameters icpOptimizationParameters = model.getICPOptimizationParameters();
private HighLevelHumanoidControllerFactory createDRCControllerFactory(DRCRobotModel robotModel, PacketCommunicator packetCommunicator, DRCRobotSensorInformation sensorInformation) ContactableBodiesFactory contactableBodiesFactory = robotModel.getContactPointParameters().getContactableBodiesFactory();
private HighLevelHumanoidControllerFactory createDRCControllerFactory(DRCRobotModel robotModel, PacketCommunicator packetCommunicator, DRCRobotSensorInformation sensorInformation) ContactableBodiesFactory contactableBodiesFactory = robotModel.getContactPointParameters().getContactableBodiesFactory();