- Add the Codota plugin to your IDE and get smart completions
private void myMethod () {ScheduledThreadPoolExecutor s =
new ScheduledThreadPoolExecutor(corePoolSize)
- Smart code suggestions by Tabnine
}
public StandStillDoNothingPelvisPoseHistoryCorrectorController() { requestedHighLevelState = (YoEnum<HighLevelControllerName>) simulationConstructionSet.getVariable( HumanoidHighLevelControllerManager.class.getSimpleName(), "requestedHighLevelState"); requestedHighLevelState.set(HighLevelControllerName.DO_NOTHING_BEHAVIOR); jointMap = getRobotModel().getJointMap(); Vector3D robotLocation = new Vector3D(); qDesireds = new LinkedHashMap<>(); oneDegreeOfFreedomJoints = new ArrayList<>(); robot.getAllOneDegreeOfFreedomJoints(oneDegreeOfFreedomJoints); for (OneDegreeOfFreedomJoint joint : oneDegreeOfFreedomJoints) { qDesireds.put(joint, joint.getQYoVariable().getDoubleValue()); } }
@Override public void variableChanged(YoVariable<?> v) { alphaFilteredHeadYawPercentage.update(headYawPercentage.getDoubleValue()); NeckJointName headYaw = NeckJointName.DISTAL_NECK_YAW; double neckYawJointRange = sliderBoardControlledNeckJointsWithLimits.get(headYaw).getRight() - sliderBoardControlledNeckJointsWithLimits.get(headYaw).getLeft(); DoubleYoVariable desiredAngle = (DoubleYoVariable) registry.getVariable(drcRobotModel.getJointMap().getNeckJointName(headYaw) + "_unconstrained" + CommonNames.q_d); desiredAngle.set(alphaFilteredHeadYawPercentage.getDoubleValue() * neckYawJointRange + sliderBoardControlledNeckJointsWithLimits.get(headYaw).getLeft()); } });
@Override public void notifyOfVariableChange(YoVariable<?> v) { alphaFilteredHeadYawPercentage.update(headYawPercentage.getDoubleValue()); NeckJointName headYaw = NeckJointName.DISTAL_NECK_YAW; double neckYawJointRange = sliderBoardControlledNeckJointsWithLimits.get(headYaw).getRight() - sliderBoardControlledNeckJointsWithLimits.get(headYaw).getLeft(); YoDouble desiredAngle = (YoDouble) registry.getVariable(drcRobotModel.getJointMap().getNeckJointName(headYaw) + "_unconstrained" + CommonNames.q_d); desiredAngle.set(alphaFilteredHeadYawPercentage.getDoubleValue() * neckYawJointRange + sliderBoardControlledNeckJointsWithLimits.get(headYaw).getLeft()); } });
@Override public void notifyOfVariableChange(YoVariable<?> v) { alphaFilteredUpperHeadPitchPercentage.update(upperHeadPitchPercentage.getDoubleValue()); NeckJointName upperHeadPitch = NeckJointName.DISTAL_NECK_PITCH; double jointRange = sliderBoardControlledNeckJointsWithLimits.get(upperHeadPitch).getRight() - sliderBoardControlledNeckJointsWithLimits.get(upperHeadPitch).getLeft(); YoDouble desiredAngle = (YoDouble) registry.getVariable(drcRobotModel.getJointMap().getNeckJointName(upperHeadPitch) + "_unconstrained" + CommonNames.q_d); desiredAngle.set(alphaFilteredUpperHeadPitchPercentage.getDoubleValue() * jointRange + sliderBoardControlledNeckJointsWithLimits.get(upperHeadPitch).getLeft()); } });
@Override public void notifyOfVariableChange(YoVariable<?> v) { alphaFilteredLowerHeadPitchYawPercentage.update(lowerHeadPitchPercentage.getDoubleValue()); NeckJointName lowerHeadPitch = NeckJointName.PROXIMAL_NECK_PITCH; double jointRange = sliderBoardControlledNeckJointsWithLimits.get(lowerHeadPitch).getRight() - sliderBoardControlledNeckJointsWithLimits.get(lowerHeadPitch).getLeft(); YoDouble desiredAngle = (YoDouble) registry.getVariable(drcRobotModel.getJointMap().getNeckJointName(lowerHeadPitch) + "_unconstrained" + CommonNames.q_d); desiredAngle.set(alphaFilteredLowerHeadPitchYawPercentage.getDoubleValue() * jointRange + sliderBoardControlledNeckJointsWithLimits.get(lowerHeadPitch).getLeft()); } });
@Override public void variableChanged(YoVariable<?> v) { alphaFilteredUpperHeadPitchPercentage.update(upperHeadPitchPercentage.getDoubleValue()); NeckJointName upperHeadPitch = NeckJointName.DISTAL_NECK_PITCH; double jointRange = sliderBoardControlledNeckJointsWithLimits.get(upperHeadPitch).getRight() - sliderBoardControlledNeckJointsWithLimits.get(upperHeadPitch).getLeft(); DoubleYoVariable desiredAngle = (DoubleYoVariable) registry.getVariable(drcRobotModel.getJointMap().getNeckJointName(upperHeadPitch) + "_unconstrained" + CommonNames.q_d); desiredAngle.set(alphaFilteredUpperHeadPitchPercentage.getDoubleValue() * jointRange + sliderBoardControlledNeckJointsWithLimits.get(upperHeadPitch).getLeft()); } });
@Override public void variableChanged(YoVariable<?> v) { alphaFilteredLowerHeadPitchYawPercentage.update(lowerHeadPitchPercentage.getDoubleValue()); NeckJointName lowerHeadPitch = NeckJointName.PROXIMAL_NECK_PITCH; double jointRange = sliderBoardControlledNeckJointsWithLimits.get(lowerHeadPitch).getRight() - sliderBoardControlledNeckJointsWithLimits.get(lowerHeadPitch).getLeft(); DoubleYoVariable desiredAngle = (DoubleYoVariable) registry.getVariable(drcRobotModel.getJointMap().getNeckJointName(lowerHeadPitch) + "_unconstrained" + CommonNames.q_d); desiredAngle.set(alphaFilteredLowerHeadPitchYawPercentage.getDoubleValue() * jointRange + sliderBoardControlledNeckJointsWithLimits.get(lowerHeadPitch).getLeft()); } });
public ForwardDynamicComparisonScript(Robot robot, DRCRobotModel robotModel) { this.robot = robot; FullHumanoidRobotModel fullRobotModel = robotModel.createFullRobotModel(); List<JointBasics> ignoredJoints = robotModel.getJointMap().getLastSimulatedJoints().stream() .flatMap(name -> SubtreeStreams.fromChildren(fullRobotModel.getOneDoFJointByName(name).getSuccessor())) .collect(Collectors.toList()); multiBodySystem = MultiBodySystemBasics.toMultiBodySystemBasics(fullRobotModel.getElevator(), ignoredJoints); calculator = new ForwardDynamicsCalculator(multiBodySystem); floatingJoint = fullRobotModel.getRootJoint(); allOneDoFJoints = MultiBodySystemTools.filterJoints(multiBodySystem.getJointsToConsider(), OneDoFJointBasics.class); multiBodySystem.getAllJoints().forEach(joint -> nameToJointMap.put(joint.getName(), joint)); }
private void setupPassiveJoints() { YoVariableRegistry robotsYoVariableRegistry = humanoidFloatingRootJointRobot.getRobotsYoVariableRegistry(); List<ImmutablePair<String, YoPDGains>> passiveJointNameWithGains = robotModel.get().getJointMap().getPassiveJointNameWithGains(robotsYoVariableRegistry); if (passiveJointNameWithGains != null) { for (int i = 0; i < passiveJointNameWithGains.size(); i++) { String jointName = passiveJointNameWithGains.get(i).getLeft(); OneDegreeOfFreedomJoint simulatedJoint = humanoidFloatingRootJointRobot.getOneDegreeOfFreedomJoint(jointName); YoPDGains gains = passiveJointNameWithGains.get(i).getRight(); PassiveJointController passiveJointController = new PassiveJointController(simulatedJoint, gains); humanoidFloatingRootJointRobot.setController(passiveJointController); } } }
private void setupPassiveJoints() { YoVariableRegistry robotsYoVariableRegistry = humanoidFloatingRootJointRobot.getRobotsYoVariableRegistry(); List<ImmutablePair<String, YoPDGains>> passiveJointNameWithGains = robotModel.get().getJointMap().getPassiveJointNameWithGains(robotsYoVariableRegistry); if (passiveJointNameWithGains != null) { for (int i = 0; i < passiveJointNameWithGains.size(); i++) { String jointName = passiveJointNameWithGains.get(i).getLeft(); OneDegreeOfFreedomJoint simulatedJoint = humanoidFloatingRootJointRobot.getOneDegreeOfFreedomJoint(jointName); YoPDGains gains = passiveJointNameWithGains.get(i).getRight(); PassiveJointController passiveJointController = new PassiveJointController(simulatedJoint, gains); humanoidFloatingRootJointRobot.setController(passiveJointController); } } }
public static void main(String[] args) throws IOException { DRCRobotModel robotModel = new ValkyrieRobotModel(RobotTarget.SCS, false); DRCRobotJointMap jointMap = robotModel.getJointMap(); HumanoidFloatingRootJointRobot sdfRobot = robotModel.createHumanoidFloatingRootJointRobot(false); FullHumanoidRobotModel fullRobotModel = robotModel.createFullRobotModel(); FullHumanoidRobotModel fullRobotModelForSlider = robotModel.createFullRobotModel(); new PosePlaybackSCSBridge(sdfRobot, fullRobotModel, fullRobotModelForSlider, robotModel.getControllerDT()); } }
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 FullHumanoidRobotModel createFullRobotModelAtInitialConfiguration() { DRCRobotModel robotModel = getRobotModel(); FullHumanoidRobotModel initialFullRobotModel = robotModel.createFullRobotModel(); HumanoidFloatingRootJointRobot robot = robotModel.createHumanoidFloatingRootJointRobot(false); robotModel.getDefaultRobotInitialSetup(0.0, 0.0).initializeRobot(robot, robotModel.getJointMap()); DRCPerfectSensorReaderFactory drcPerfectSensorReaderFactory = new DRCPerfectSensorReaderFactory(robot, null, 0); drcPerfectSensorReaderFactory.build(initialFullRobotModel.getRootJoint(), null, null, null, null, null, null); drcPerfectSensorReaderFactory.getSensorReader().read(); return initialFullRobotModel; }
protected FullHumanoidRobotModel createFullRobotModelAtInitialConfiguration() { DRCRobotModel robotModel = getRobotModel(); FullHumanoidRobotModel initialFullRobotModel = robotModel.createFullRobotModel(); HumanoidFloatingRootJointRobot robot = robotModel.createHumanoidFloatingRootJointRobot(false); robotModel.getDefaultRobotInitialSetup(0.0, 0.0).initializeRobot(robot, robotModel.getJointMap()); DRCPerfectSensorReaderFactory drcPerfectSensorReaderFactory = new DRCPerfectSensorReaderFactory(robot, null, 0); drcPerfectSensorReaderFactory.build(initialFullRobotModel.getRootJoint(), null, null, null, null, null, null); drcPerfectSensorReaderFactory.getSensorReader().read(); return initialFullRobotModel; }
private FullHumanoidRobotModel createFullRobotModelAtInitialConfiguration() { DRCRobotModel robotModel = getRobotModel(); FullHumanoidRobotModel initialFullRobotModel = robotModel.createFullRobotModel(); HumanoidFloatingRootJointRobot robot = robotModel.createHumanoidFloatingRootJointRobot(false); robotModel.getDefaultRobotInitialSetup(0.0, 0.0).initializeRobot(robot, robotModel.getJointMap()); DRCPerfectSensorReaderFactory drcPerfectSensorReaderFactory = new DRCPerfectSensorReaderFactory(robot, null, 0); drcPerfectSensorReaderFactory.build(initialFullRobotModel.getRootJoint(), null, null, null, null, null, null); drcPerfectSensorReaderFactory.getSensorReader().read(); return initialFullRobotModel; }
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(); OneDoFJointBasics controllerJoint = controllerFullRobotModel.getOneDoFJointByName(positionControlledJointName); if (simulatedJoint == null || controllerJoint == null) continue; JointDesiredOutputList controllerLowLevelDataList = threadDataSynchronizer.getControllerDesiredJointDataHolder(); JointDesiredOutputBasics controllerDesiredOutput = controllerLowLevelDataList.getJointDesiredOutput(controllerJoint); JointRole jointRole = robotModel.get().getJointMap().getJointRole(positionControlledJointName); boolean isUpperBodyJoint = ((jointRole != JointRole.LEG) && (jointRole != JointRole.SPINE)); boolean isBackJoint = jointRole == JointRole.SPINE; JointLowLevelJointControlSimulator positionControlSimulator = new JointLowLevelJointControlSimulator(simulatedJoint, controllerJoint, controllerDesiredOutput, isUpperBodyJoint, isBackJoint, false, controllerFullRobotModel.getTotalMass(), robotModel.get().getSimulateDT()); humanoidFloatingRootJointRobot.setController(positionControlSimulator); } } }
public VisualizePoseWorkspace(DRCRobotModel robotModel) throws IOException { this.controlDT = robotModel.getControllerDT(); DRCRobotJointMap jointMap = robotModel.getJointMap(); HumanoidFloatingRootJointRobot sdfRobot = robotModel.createHumanoidFloatingRootJointRobot(false); interpolator = new PlaybackPoseInterpolator(registry); SimulationConstructionSet scs = new SimulationConstructionSet(sdfRobot); scs.addYoVariableRegistry(registry); scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); fullRobotModelForSlider = robotModel.createFullRobotModel(); DRCRobotMidiSliderBoardPositionManipulation sliderBoard = new DRCRobotMidiSliderBoardPositionManipulation(scs, sdfRobot, fullRobotModelForSlider, yoGraphicsListRegistry); posePlaybackRobotPoseSequence = new PlaybackPoseSequence(fullRobotModelForSlider); CaptureSnapshotListener captureSnapshotListener = new CaptureSnapshotListener(sdfRobot, scs); sliderBoard.addCaptureSnapshotListener(captureSnapshotListener); SaveSequenceListener saveSequenceListener = new SaveSequenceListener(); sliderBoard.addSaveSequenceRequestedListener(saveSequenceListener); LoadSequenceListener loadSequenceListener = new LoadSequenceListener(fullRobotModelForSlider, sdfRobot, scs); sliderBoard.addLoadSequenceRequestedListener(loadSequenceListener); scs.startOnAThread(); }
public VisualizePoseWorkspace(DRCRobotModel robotModel) throws IOException { this.controlDT = robotModel.getControllerDT(); DRCRobotJointMap jointMap = robotModel.getJointMap(); HumanoidFloatingRootJointRobot sdfRobot = robotModel.createHumanoidFloatingRootJointRobot(false); interpolator = new PlaybackPoseInterpolator(registry); SimulationConstructionSet scs = new SimulationConstructionSet(sdfRobot); scs.addYoVariableRegistry(registry); scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); fullRobotModelForSlider = robotModel.createFullRobotModel(); DRCRobotMidiSliderBoardPositionManipulation sliderBoard = new DRCRobotMidiSliderBoardPositionManipulation(scs, sdfRobot, fullRobotModelForSlider, yoGraphicsListRegistry); posePlaybackRobotPoseSequence = new PlaybackPoseSequence(fullRobotModelForSlider); CaptureSnapshotListener captureSnapshotListener = new CaptureSnapshotListener(sdfRobot, scs); sliderBoard.addCaptureSnapshotListener(captureSnapshotListener); SaveSequenceListener saveSequenceListener = new SaveSequenceListener(); sliderBoard.addSaveSequenceRequestedListener(saveSequenceListener); LoadSequenceListener loadSequenceListener = new LoadSequenceListener(fullRobotModelForSlider, sdfRobot, scs); sliderBoard.addLoadSequenceRequestedListener(loadSequenceListener); scs.startOnAThread(); }
private void initializeSimulationConstructionSet() { humanoidFloatingRootJointRobot.setDynamicIntegrationMethod(scsInitialSetup.get().getDynamicIntegrationMethod()); scsInitialSetup.get().initializeSimulation(simulationConstructionSet); if (guiInitialSetup.get().isGuiShown()) { SimulationOverheadPlotterFactory simulationOverheadPlotterFactory = simulationConstructionSet.createSimulationOverheadPlotterFactory(); simulationOverheadPlotterFactory.setShowOnStart(guiInitialSetup.get().isShowOverheadView()); simulationOverheadPlotterFactory.setVariableNameToTrack("centerOfMass"); simulationOverheadPlotterFactory.addYoGraphicsListRegistries(controllerThread.getDynamicGraphicObjectsListRegistry()); simulationOverheadPlotterFactory.addYoGraphicsListRegistries(stateEstimationThread.getDynamicGraphicObjectsListRegistry()); simulationOverheadPlotterFactory.addYoGraphicsListRegistries(actualCMPComputer.getYoGraphicsListRegistry()); simulationOverheadPlotterFactory.createOverheadPlotter(); guiInitialSetup.get().initializeGUI(simulationConstructionSet, humanoidFloatingRootJointRobot, robotModel.get()); } if (commonAvatarEnvironment.get() != null && commonAvatarEnvironment.get().getTerrainObject3D() != null) { simulationConstructionSet.addStaticLinkGraphics(commonAvatarEnvironment.get().getTerrainObject3D().getLinkGraphics()); } scsInitialSetup.get().initializeRobot(humanoidFloatingRootJointRobot, robotModel.get(), null); robotInitialSetup.get().initializeRobot(humanoidFloatingRootJointRobot, robotModel.get().getJointMap()); humanoidFloatingRootJointRobot.update(); }
private void initializeSimulationConstructionSet() { simulationConstructionSet.setParameterRootPath(threadedRobotController.getYoVariableRegistry()); humanoidFloatingRootJointRobot.setDynamicIntegrationMethod(scsInitialSetup.get().getDynamicIntegrationMethod()); scsInitialSetup.get().initializeSimulation(simulationConstructionSet); if (guiInitialSetup.get().isGuiShown()) { SimulationOverheadPlotterFactory simulationOverheadPlotterFactory = simulationConstructionSet.createSimulationOverheadPlotterFactory(); simulationOverheadPlotterFactory.setShowOnStart(guiInitialSetup.get().isShowOverheadView()); simulationOverheadPlotterFactory.setVariableNameToTrack("centerOfMass"); simulationOverheadPlotterFactory.addYoGraphicsListRegistries(controllerThread.getYoGraphicsListRegistry()); simulationOverheadPlotterFactory.addYoGraphicsListRegistries(stateEstimationThread.getYoGraphicsListRegistry()); simulationOverheadPlotterFactory.addYoGraphicsListRegistries(actualCMPComputer.getYoGraphicsListRegistry()); simulationOverheadPlotterFactory.createOverheadPlotter(); guiInitialSetup.get().initializeGUI(simulationConstructionSet, humanoidFloatingRootJointRobot, robotModel.get()); } if (commonAvatarEnvironment.get() != null && commonAvatarEnvironment.get().getTerrainObject3D() != null) { simulationConstructionSet.addStaticLinkGraphics(commonAvatarEnvironment.get().getTerrainObject3D().getLinkGraphics()); } scsInitialSetup.get().initializeRobot(humanoidFloatingRootJointRobot, robotModel.get(), null); robotInitialSetup.get().initializeRobot(humanoidFloatingRootJointRobot, robotModel.get().getJointMap()); humanoidFloatingRootJointRobot.update(); }