private void setupSimulatedRobotTimeProvider() { simulatedRobotTimeProvider = new SimulatedDRCRobotTimeProvider(robotModel.get().getSimulateDT()); humanoidFloatingRootJointRobot.setController(simulatedRobotTimeProvider); }
private void setupCOMVisualization() { SimulatedRobotCenterOfMassVisualizer simulatedRobotCenterOfMassVisualizer = new SimulatedRobotCenterOfMassVisualizer(humanoidFloatingRootJointRobot, robotModel.get().getSimulateDT()); humanoidFloatingRootJointRobot.setController(simulatedRobotCenterOfMassVisualizer); }
private void setupCOMVisualization() { SimulatedRobotCenterOfMassVisualizer simulatedRobotCenterOfMassVisualizer = new SimulatedRobotCenterOfMassVisualizer(humanoidFloatingRootJointRobot, robotModel.get().getSimulateDT()); humanoidFloatingRootJointRobot.setController(simulatedRobotCenterOfMassVisualizer); }
private void setupSimulatedRobotTimeProvider() { simulatedRobotTimeProvider = new SimulatedDRCRobotTimeProvider(robotModel.get().getSimulateDT()); humanoidFloatingRootJointRobot.setController(simulatedRobotTimeProvider); }
public void startSimulation() { SimulationConstructionSetParameters simulationParameters = new SimulationConstructionSetParameters(true, 16000); SimulationConstructionSet scs = new SimulationConstructionSet(simulatedRobot, simulationParameters); scs.setDT(robotModel.getSimulateDT(), 10); scs.setCameraPosition(scsCameraPosition.x, scsCameraPosition.y, scsCameraPosition.z); scs.setCameraFix(scsCameraFix.x, scsCameraFix.y, scsCameraFix.z); scs.startOnAThread(); }
private DRCFlatGroundWalkingTrack setupSimulationTrack(WalkingControllerParameters drcControlParameters, GroundProfile3D groundProfile, GroundProfile3D groundProfile3D, boolean drawGroundProfile, boolean useVelocityAndHeadingScript, boolean cheatWithGroundHeightAtForFootstep, DRCRobotInitialSetup<HumanoidFloatingRootJointRobot> robotInitialSetup) { DRCGuiInitialSetup guiInitialSetup = createGUIInitialSetup(); DRCSCSInitialSetup scsInitialSetup; if (groundProfile != null) { scsInitialSetup = new DRCSCSInitialSetup(groundProfile, robotModel.getSimulateDT()); } else { scsInitialSetup = new DRCSCSInitialSetup(groundProfile3D, robotModel.getSimulateDT()); } scsInitialSetup.setDrawGroundProfile(drawGroundProfile); if (cheatWithGroundHeightAtForFootstep) scsInitialSetup.setInitializeEstimatorToActual(true); DRCFlatGroundWalkingTrack drcFlatGroundWalkingTrack = new DRCFlatGroundWalkingTrack(robotInitialSetup, guiInitialSetup, scsInitialSetup, useVelocityAndHeadingScript, cheatWithGroundHeightAtForFootstep, robotModel); SimulationConstructionSet scs = drcFlatGroundWalkingTrack.getSimulationConstructionSet(); scs.setGroundVisible(false); setupCameraForUnitTest(scs); return drcFlatGroundWalkingTrack; }
private void setupMultiThreadedRobotController() { if (scsInitialSetup.get().getRunMultiThreaded()) { threadedRobotController = new MultiThreadedRobotController("DRCSimulation", humanoidFloatingRootJointRobot, simulationConstructionSet); } else { PrintTools.warn(this, "Running simulation in single threaded mode", true); threadedRobotController = new SingleThreadedRobotController("DRCSimulation", humanoidFloatingRootJointRobot, simulationConstructionSet); } int estimatorTicksPerSimulationTick = (int) Math.round(robotModel.get().getEstimatorDT() / robotModel.get().getSimulateDT()); int controllerTicksPerSimulationTick = (int) Math.round(robotModel.get().getControllerDT() / robotModel.get().getSimulateDT()); int slowPublisherTicksPerSimulationTick = (int) Math.round(10 * robotModel.get().getEstimatorDT() / robotModel.get().getSimulateDT()); threadedRobotController.addController(stateEstimationThread, estimatorTicksPerSimulationTick, false); threadedRobotController.addController(controllerThread, controllerTicksPerSimulationTick, true); MultiThreadedRobotControlElement simulatedHandController = robotModel.get().createSimulatedHandController(humanoidFloatingRootJointRobot, threadDataSynchronizer, realtimeRos2Node.get(), closeableAndDisposableRegistry); if (simulatedHandController != null) { threadedRobotController.addController(simulatedHandController, controllerTicksPerSimulationTick, false); } }
private void setupTrack(boolean runMultiThreaded, DRCRobotModel robotModel) { DRCGuiInitialSetup guiInitialSetup = new DRCGuiInitialSetup(true, false, simulationTestingParameters); GroundProfile3D groundProfile = new FlatGroundProfile(); DRCSCSInitialSetup scsInitialSetup = new DRCSCSInitialSetup(groundProfile, robotModel.getSimulateDT()); // scsInitialSetup.setInitializeEstimatorToActual(true); scsInitialSetup.setDrawGroundProfile(true); scsInitialSetup.setRunMultiThreaded(runMultiThreaded); DRCRobotInitialSetup<HumanoidFloatingRootJointRobot> robotInitialSetup = robotModel.getDefaultRobotInitialSetup(0.0, 0.0); drcFlatGroundWalkingTrack = new DRCFlatGroundWalkingTrack(robotInitialSetup, guiInitialSetup, scsInitialSetup, true, false, robotModel); drcFlatGroundWalkingTrack.getAvatarSimulation(); }
private void setupTrack(boolean runMultiThreaded, DRCRobotModel robotModel) { DRCGuiInitialSetup guiInitialSetup = new DRCGuiInitialSetup(true, false, simulationTestingParameters); GroundProfile3D groundProfile = new FlatGroundProfile(); DRCSCSInitialSetup scsInitialSetup = new DRCSCSInitialSetup(groundProfile, robotModel.getSimulateDT()); scsInitialSetup.setInitializeEstimatorToActual(true); scsInitialSetup.setDrawGroundProfile(true); scsInitialSetup.setRunMultiThreaded(runMultiThreaded); DRCRobotInitialSetup<HumanoidFloatingRootJointRobot> robotInitialSetup = robotModel.getDefaultRobotInitialSetup(0.0, 0.0); drcFlatGroundWalkingTrack = new DRCFlatGroundWalkingTrack(robotInitialSetup, guiInitialSetup, scsInitialSetup, true, false, robotModel); }
private void setupSimulationConstructionSet() { SimulationConstructionSetParameters simulationConstructionSetParameters = guiInitialSetup.get().getSimulationConstructionSetParameters(); simulationConstructionSetParameters.setDataBufferSize(scsInitialSetup.get().getSimulationDataBufferSize()); List<Robot> allSimulatedRobotList = new ArrayList<Robot>(); allSimulatedRobotList.add(humanoidFloatingRootJointRobot); if (commonAvatarEnvironment.get() != null && commonAvatarEnvironment.get().getEnvironmentRobots() != null) { allSimulatedRobotList.addAll(commonAvatarEnvironment.get().getEnvironmentRobots()); commonAvatarEnvironment.get().addContactPoints(humanoidFloatingRootJointRobot.getAllGroundContactPoints()); commonAvatarEnvironment.get().createAndSetContactControllerToARobot(); } simulationConstructionSet = new SimulationConstructionSet(allSimulatedRobotList.toArray(new Robot[0]), guiInitialSetup.get().getGraphics3DAdapter(), simulationConstructionSetParameters); simulationConstructionSet.setDT(robotModel.get().getSimulateDT(), 1); }
private void setupSimulationConstructionSet() { SimulationConstructionSetParameters simulationConstructionSetParameters = guiInitialSetup.get().getSimulationConstructionSetParameters(); simulationConstructionSetParameters.setDataBufferSize(scsInitialSetup.get().getSimulationDataBufferSize()); List<Robot> allSimulatedRobotList = new ArrayList<Robot>(); allSimulatedRobotList.add(humanoidFloatingRootJointRobot); if (commonAvatarEnvironment.get() != null && commonAvatarEnvironment.get().getEnvironmentRobots() != null) { allSimulatedRobotList.addAll(commonAvatarEnvironment.get().getEnvironmentRobots()); commonAvatarEnvironment.get().addContactPoints(humanoidFloatingRootJointRobot.getAllGroundContactPoints()); commonAvatarEnvironment.get().createAndSetContactControllerToARobot(); } simulationConstructionSet = new SimulationConstructionSet(allSimulatedRobotList.toArray(new Robot[0]), guiInitialSetup.get().getGraphics3DAdapter(), simulationConstructionSetParameters); simulationConstructionSet.setDT(robotModel.get().getSimulateDT(), 1); }
private void setupLidarController() { DRCRobotLidarParameters lidarParameters = robotModel.get().getSensorInformation().getLidarParameters(0); if (lidarParameters != null && lidarParameters.getLidarSpindleJointName() != null) { PIDLidarTorqueController pidLidarTorqueController = new PIDLidarTorqueController(humanoidFloatingRootJointRobot, lidarParameters.getLidarSpindleJointName(), lidarParameters.getLidarSpindleVelocity(), robotModel.get().getSimulateDT()); humanoidFloatingRootJointRobot.setController(pidLidarTorqueController); } }
private void setupLidarController() { DRCRobotLidarParameters lidarParameters = robotModel.get().getSensorInformation().getLidarParameters(0); if (lidarParameters != null && lidarParameters.getLidarSpindleJointName() != null) { PIDLidarTorqueController pidLidarTorqueController = new PIDLidarTorqueController(humanoidFloatingRootJointRobot, lidarParameters.getLidarSpindleJointName(), lidarParameters.getLidarSpindleVelocity(), robotModel.get().getSimulateDT()); humanoidFloatingRootJointRobot.setController(pidLidarTorqueController); } }
private DRCFlatGroundWalkingTrack setupFlatGroundSimulationTrackForSameWayTwiceVerifier(DRCRobotModel robotModel) { DRCGuiInitialSetup guiInitialSetup = createGUIInitialSetup(); GroundProfile3D groundProfile = new FlatGroundProfile(); DRCSCSInitialSetup scsInitialSetup = new DRCSCSInitialSetup(groundProfile, robotModel.getSimulateDT()); scsInitialSetup.setDrawGroundProfile(drawGroundProfile); if (cheatWithGroundHeightAtForFootstep) scsInitialSetup.setInitializeEstimatorToActual(true); DRCRobotInitialSetup<HumanoidFloatingRootJointRobot> robotInitialSetup = robotModel.getDefaultRobotInitialSetup(0.0, 0.0); DRCFlatGroundWalkingTrack drcFlatGroundWalkingTrack = new DRCFlatGroundWalkingTrack(robotInitialSetup, guiInitialSetup, scsInitialSetup, useVelocityAndHeadingScript, cheatWithGroundHeightAtForFootstep, robotModel); SimulationConstructionSet scs = drcFlatGroundWalkingTrack.getSimulationConstructionSet(); setupCameraForUnitTest(scs); return drcFlatGroundWalkingTrack; }
private SimulationConstructionSet setupScs() { boolean useVelocityAndHeadingScript = true; boolean cheatWithGroundHeightAtForFootstep = false; GroundProfile3D groundProfile = new FlatGroundProfile(); DRCRobotModel robotModel = getRobotModel(); DRCGuiInitialSetup guiInitialSetup = createGUIInitialSetup(); DRCRobotInitialSetup<HumanoidFloatingRootJointRobot> robotInitialSetup = robotModel.getDefaultRobotInitialSetup(0, 0); DRCSCSInitialSetup scsInitialSetup = new DRCSCSInitialSetup(groundProfile, robotModel.getSimulateDT()); scsInitialSetup.setRunMultiThreaded(false); DRCFlatGroundWalkingTrack drcFlatGroundWalkingTrack = new DRCFlatGroundWalkingTrack(robotInitialSetup, guiInitialSetup, scsInitialSetup, useVelocityAndHeadingScript, cheatWithGroundHeightAtForFootstep, robotModel); SimulationConstructionSet scs = drcFlatGroundWalkingTrack.getSimulationConstructionSet(); setupCameraForUnitTest(scs); return scs; }
public void startSimulation() { SimulationConstructionSetParameters simulationParameters = new SimulationConstructionSetParameters(true, 16000); SimulationConstructionSet scs = new SimulationConstructionSet(simulatedRobot, simulationParameters); scs.setDT(robotModel.getSimulateDT(), 10); scs.setCameraPosition(scsCameraPosition.getX(), scsCameraPosition.getY(), scsCameraPosition.getZ()); scs.setCameraFix(scsCameraFix.getX(), scsCameraFix.getY(), scsCameraFix.getZ()); scs.startOnAThread(); }
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 void setupPositionControlledJointsForSimulation() { String[] positionControlledJointNames = robotModel.get().getJointMap().getPositionControlledJointsForSimulation(); if (positionControlledJointNames != null) { for (String positionControlledJointName : positionControlledJointNames) { OneDegreeOfFreedomJoint simulatedJoint = humanoidFloatingRootJointRobot.getOneDegreeOfFreedomJoint(positionControlledJointName); FullRobotModel controllerFullRobotModel = threadDataSynchronizer.getControllerFullRobotModel(); OneDoFJoint controllerJoint = controllerFullRobotModel.getOneDoFJointByName(positionControlledJointName); JointRole jointRole = robotModel.get().getJointMap().getJointRole(positionControlledJointName); boolean isUpperBodyJoint = ((jointRole != JointRole.LEG) && (jointRole != JointRole.SPINE)); boolean isBackJoint = jointRole == JointRole.SPINE; JointLowLevelPositionControlSimulator positionControlSimulator = new JointLowLevelPositionControlSimulator(simulatedJoint, controllerJoint, isUpperBodyJoint, isBackJoint, false, robotModel.get().getSimulateDT()); humanoidFloatingRootJointRobot.setController(positionControlSimulator); } } }
private DRCFlatGroundWalkingTrack setupWalkingSim() { DRCRobotModel robotModel = getRobotModel(); DRCGuiInitialSetup guiInitialSetup = new DRCGuiInitialSetup(true, false, simulationTestingParameters); GroundProfile3D groundProfile = new FlatGroundProfile(); DRCSCSInitialSetup scsInitialSetup = new DRCSCSInitialSetup(groundProfile, robotModel.getSimulateDT()); scsInitialSetup.setDrawGroundProfile(false); externalPelvisPosePublisher = new ExternalPelvisPoseCreator(); DRCFlatGroundWalkingTrack drcFlatGroundWalkingTrack = new DRCFlatGroundWalkingTrack(robotModel.getDefaultRobotInitialSetup(0.0, 0.0), guiInitialSetup, scsInitialSetup, true, false, getRobotModel(), externalPelvisPosePublisher); simulationConstructionSet = drcFlatGroundWalkingTrack.getSimulationConstructionSet(); robot = drcFlatGroundWalkingTrack.getAvatarSimulation().getHumanoidFloatingRootJointRobot(); registry = robot.getRobotsYoVariableRegistry(); YoBoolean walk = (YoBoolean) simulationConstructionSet.getVariable("walkCSG"); walk.set(true); return drcFlatGroundWalkingTrack; }