public ZeroPoseMockRobotConfigurationDataPublisherModule(final DRCRobotModel robotModel) { fullRobotModel = robotModel.createFullRobotModel(); forceSensorDefinitions = fullRobotModel.getForceSensorDefinitions(); try { packetCommunicator.connect(); Thread t = new Thread(this); t.start(); } catch (IOException e) { e.printStackTrace(); } }
public ZeroPoseMockRobotConfigurationDataPublisherModule(final DRCRobotModel robotModel) { fullRobotModel = robotModel.createFullRobotModel(); forceSensorDefinitions = fullRobotModel.getForceSensorDefinitions(); publisher = ROS2Tools.createPublisher(ros2Node, RobotConfigurationData.class, ControllerAPIDefinition.getPublisherTopicNameGenerator(robotModel.getSimpleRobotName())); Thread t = new Thread(this); t.start(); }
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 setupHeightQuadTreeToolboxModule(DRCRobotModel robotModel, DRCNetworkModuleParameters params) throws IOException { if (params.isHeightQuadTreeToolboxEnabled()) new HeightQuadTreeToolboxModule(robotModel.getSimpleRobotName(), robotModel.createFullRobotModel(), robotModel.getLogModelProvider()); }
public DRCInverseDynamicsCalculatorTestHelper createInverseDynamicsCalculatorTestHelper(boolean visualize, double gravityZ) { DRCRobotModel drcRobotModel = getRobotModel(); FullHumanoidRobotModel fullRobotModel = drcRobotModel.createFullRobotModel(); boolean createCollisionMeshes = false; boolean enableJointDamping = false; HumanoidFloatingRootJointRobot robot = drcRobotModel.createHumanoidFloatingRootJointRobot(createCollisionMeshes, enableJointDamping); robot.setGravity(gravityZ); return new DRCInverseDynamicsCalculatorTestHelper(fullRobotModel, robot, visualize, gravityZ); } }
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)); }
public KinematicsToolboxModule(DRCRobotModel robotModel, boolean startYoVariableServer) throws IOException { super(robotModel.createFullRobotModel(), robotModel.getLogModelProvider(), startYoVariableServer, PACKET_DESTINATION, NETWORK_PORT); kinematicsToolBoxController = new KinematicsToolboxController(commandInputManager, statusOutputManager, fullRobotModel, robotModel, yoGraphicsListRegistry, registry); packetCommunicator.attachListener(RobotConfigurationData.class, kinematicsToolBoxController.createRobotConfigurationDataConsumer()); startYoVariableServer(); }
public MultisensePointCloudReceiver(PacketCommunicator packetCommunicator, MultisenseTest testInfo, DRCRobotModel robotModel) { this.packetCommunicator = packetCommunicator; this.testInfo = testInfo; this.frame = testInfo.getFrame(); FullHumanoidRobotModel fullRobotModel = robotModel.createFullRobotModel(); robotDataReceiver = new HumanoidRobotDataReceiver(fullRobotModel, null); HumanoidReferenceFrames referenceFrames = robotDataReceiver.getReferenceFrames(); this.headRootReferenceFrame = referenceFrames.getNeckFrame(NeckJointName.PROXIMAL_NECK_PITCH); setupReferenceFrames(referenceFrames); }
@ContinuousIntegrationTest(estimatedDuration = 1.0) @Test(timeout = 30000, expected = IllegalArgumentException.class) public void testAddingTwoFramesWithTheSameNameThrowsException() { DRCRobotModel robotModelA = getRobotModel(); FullHumanoidRobotModel fullRobotModel = robotModelA.createFullRobotModel(); TestReferenceFrames referenceFrames = new TestReferenceFrames(); //should throw an IllegalArgumentException ReferenceFrameHashCodeResolver referenceFrameHashCodeResolverA = new ReferenceFrameHashCodeResolver(fullRobotModel, referenceFrames); }
public KinematicsToolboxModule(DRCRobotModel robotModel, boolean startYoVariableServer, PubSubImplementation pubSubImplementation) throws IOException { super(robotModel.getSimpleRobotName(), robotModel.createFullRobotModel(), robotModel.getLogModelProvider(), startYoVariableServer, DEFAULT_UPDATE_PERIOD_MILLISECONDS, pubSubImplementation); kinematicsToolBoxController = new HumanoidKinematicsToolboxController(commandInputManager, statusOutputManager, fullRobotModel, yoGraphicsListRegistry, registry); commandInputManager.registerConversionHelper(new KinematicsToolboxCommandConverter(fullRobotModel)); startYoVariableServer(); }
public KinematicsPlanningToolboxModule(DRCRobotModel robotModel, boolean startYoVariableServer, PubSubImplementation pubSubImplementation) throws IOException { super(robotModel.getSimpleRobotName(), robotModel.createFullRobotModel(), robotModel.getLogModelProvider(), startYoVariableServer, DEFAULT_UPDATE_PERIOD_MILLISECONDS, pubSubImplementation); kinematicsPlanningToolboxController = new KinematicsPlanningToolboxController(robotModel, fullRobotModel, commandInputManager, statusOutputManager, yoGraphicsListRegistry, registry); commandInputManager.registerConversionHelper(new KinematicsPlanningToolboxCommandConverter(fullRobotModel)); startYoVariableServer(); }
@ContinuousIntegrationTest(estimatedDuration = 1.0) @Test(timeout = 30000) public void testGetReferenceFrameFromHashCodeReturnsSameNamedFrames() { DRCRobotModel robotModelA = getRobotModel(); FullHumanoidRobotModel fullRobotModelA = robotModelA.createFullRobotModel(); HumanoidReferenceFrames referenceFramesA = new HumanoidReferenceFrames(fullRobotModelA); DRCRobotModel robotModelB = getRobotModel(); FullHumanoidRobotModel fullRobotModelB = robotModelB.createFullRobotModel(); HumanoidReferenceFrames referenceFramesB = new HumanoidReferenceFrames(fullRobotModelB); ReferenceFrameHashCodeResolver referenceFrameHashCodeResolverB = new ReferenceFrameHashCodeResolver(fullRobotModelB, referenceFramesB); ReferenceFrame midFeetZUpFrameA = referenceFramesA.getMidFeetZUpFrame(); long hashCode = midFeetZUpFrameA.hashCode(); ReferenceFrame midZUpFrameB = referenceFrameHashCodeResolverB.getReferenceFrameFromHashCode(hashCode); checkReferenceFramesMatch(midFeetZUpFrameA, midZUpFrameB); }
private void setupHeightQuadTreeToolboxModule(DRCRobotModel robotModel, DRCNetworkModuleParameters params) throws IOException { if (params.isHeightQuadTreeToolboxEnabled()) { new HeightQuadTreeToolboxModule(robotModel.createFullRobotModel(), robotModel.getLogModelProvider()); PacketCommunicator heightQuadTreeToolboxCommunicator = PacketCommunicator .createIntraprocessPacketCommunicator(NetworkPorts.HEIGHT_QUADTREE_TOOLBOX_MODULE_PORT, NET_CLASS_LIST); packetRouter.attachPacketCommunicator(PacketDestination.HEIGHT_QUADTREE_TOOLBOX_MODULE, heightQuadTreeToolboxCommunicator); heightQuadTreeToolboxCommunicator.connect(); String methodName = "setupHeightQuadTreeToolboxModule"; printModuleConnectedDebugStatement(PacketDestination.HEIGHT_QUADTREE_TOOLBOX_MODULE, methodName); } }
private void setupFootstepPlanningToolboxModule(DRCRobotModel robotModel, DRCNetworkModuleParameters params) throws IOException { if (!params.isFootstepPlanningToolboxEnabled()) return; FullHumanoidRobotModel fullRobotModel = robotModel.createFullRobotModel(); new FootstepPlanningToolboxModule(robotModel, fullRobotModel, null, params.isFootstepPlanningToolboxVisualizerEnabled()); PacketCommunicator footstepPlanningToolboxCommunicator = PacketCommunicator .createIntraprocessPacketCommunicator(NetworkPorts.FOOTSTEP_PLANNING_TOOLBOX_MODULE_PORT, NET_CLASS_LIST); packetRouter.attachPacketCommunicator(PacketDestination.FOOTSTEP_PLANNING_TOOLBOX_MODULE, footstepPlanningToolboxCommunicator); footstepPlanningToolboxCommunicator.connect(); String methodName = "setupFootstepPlanningModule"; printModuleConnectedDebugStatement(PacketDestination.FOOTSTEP_PLANNING_TOOLBOX_MODULE, methodName); }
public WholeBodyTrajectoryToolboxModule(DRCRobotModel drcRobotModel, boolean startYoVariableServer, PubSubImplementation pubSubImplementation) throws IOException { super(drcRobotModel.getSimpleRobotName(), drcRobotModel.createFullRobotModel(), drcRobotModel.getLogModelProvider(), startYoVariableServer, DEFAULT_UPDATE_PERIOD_MILLISECONDS, pubSubImplementation); setTimeWithoutInputsBeforeGoingToSleep(Double.POSITIVE_INFINITY); wholeBodyTrajectoryToolboxController = new WholeBodyTrajectoryToolboxController(drcRobotModel, fullRobotModel, commandInputManager, statusOutputManager, registry, yoGraphicsListRegistry, startYoVariableServer); controllerNetworkSubscriber.registerSubcriberWithMessageUnpacker(WholeBodyTrajectoryToolboxMessage.class, 10, MessageUnpackingTools.createWholeBodyTrajectoryToolboxMessageUnpacker()); commandInputManager.registerConversionHelper(new WholeBodyTrajectoryToolboxCommandConverter(fullRobotModel)); }
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(); }
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 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 void setupTest(DRCRobotModel robotModel) throws SimulationExceededMaximumTimeException, InterruptedException { boolean runMultiThreaded = false; setupTrack(runMultiThreaded, robotModel); FullHumanoidRobotModel fullRobotModel = robotModel.createFullRobotModel(); pushRobotController = new PushRobotController(drcFlatGroundWalkingTrack.getAvatarSimulation().getHumanoidFloatingRootJointRobot(), fullRobotModel); if (VISUALIZE_FORCE) { drcFlatGroundWalkingTrack.getSimulationConstructionSet().addYoGraphic(pushRobotController.getForceVisualizer()); } SimulationConstructionSet scs = drcFlatGroundWalkingTrack.getSimulationConstructionSet(); YoBoolean enable = (YoBoolean) scs.getVariable("PushRecoveryControlModule", "enablePushRecovery"); // enable push recovery enable.set(true); }