public HumanoidReferenceFrames createHumanoidReferenceFrames(FullHumanoidRobotModel fullHumanoidRobotModel) { ForceSensorDataHolder forceSensorDataHolder = new ForceSensorDataHolder(Arrays.asList(fullHumanoidRobotModel.getForceSensorDefinitions())); HumanoidRobotDataReceiver robotDataReceiver = new HumanoidRobotDataReceiver(fullHumanoidRobotModel, forceSensorDataHolder); packetCommunicator.attachListener(RobotConfigurationData.class, robotDataReceiver); return robotDataReceiver.getReferenceFrames(); }
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); }
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); }
public MocapToHeadFrameConverter(DRCRobotModel robotModel, PacketCommunicator mocapModulePacketCommunicator) { FullHumanoidRobotModel fullRobotModel = robotModel.createFullRobotModel(); robotDataReceiver = new HumanoidRobotDataReceiver(fullRobotModel, null); HumanoidReferenceFrames referenceFrames = robotDataReceiver.getReferenceFrames(); robotHeadFrame = referenceFrames.getNeckFrame(NeckJointName.PROXIMAL_NECK_PITCH); mocapHeadFrame = new ReferenceFrame("headInMocapFrame", mocapOrigin) { @Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { transformToParent.set(mocapHeadPoseInZUp); } }; mocapOffsetFrame = new ReferenceFrame("mocapOffsetFrame", mocapOrigin) { @Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { transformToParent.set(transformFromMocapHeadToRobotHead); } }; mocapModulePacketCommunicator.attachListener(RobotConfigurationData.class, robotDataReceiver); }
public MocapToStateEstimatorFrameConverter(DRCRobotModel robotModel, PacketCommunicator mocapModulePacketCommunicator) { FullHumanoidRobotModel fullRobotModel = robotModel.createFullRobotModel(); robotDataReceiver = new HumanoidRobotDataReceiver(fullRobotModel, null); HumanoidReferenceFrames referenceFrames = robotDataReceiver.getReferenceFrames(); robotHeadFrame = referenceFrames.getNeckFrame(NeckJointName.PROXIMAL_NECK_PITCH); mocapHeadFrame = new ReferenceFrame("headInMocapFrame", mocapOrigin) { @Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { transformToParent.set(mocapHeadPoseInZUp); } }; mocapOffsetFrame = new ReferenceFrame("mocapOffsetFrame", mocapOrigin) { @Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { transformToParent.set(transformFromMocapHeadToRobotHead); } }; mocapModulePacketCommunicator.attachListener(RobotConfigurationData.class, robotDataReceiver); }
public MocapToStateEstimatorFrameConverter(DRCRobotModel robotModel, PacketCommunicator mocapModulePacketCommunicator) { FullHumanoidRobotModel fullRobotModel = robotModel.createFullRobotModel(); robotDataReceiver = new HumanoidRobotDataReceiver(fullRobotModel, null); HumanoidReferenceFrames referenceFrames = robotDataReceiver.getReferenceFrames(); robotHeadFrame = referenceFrames.getNeckFrame(NeckJointName.PROXIMAL_NECK_PITCH); mocapHeadFrame = new ReferenceFrame("headInMocapFrame", mocapOrigin) { @Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { transformToParent.set(mocapHeadPoseInZUp); } }; mocapOffsetFrame = new ReferenceFrame("mocapOffsetFrame", mocapOrigin) { @Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { transformToParent.set(transformFromMocapHeadToRobotHead); } }; mocapModulePacketCommunicator.attachListener(RobotConfigurationData.class, robotDataReceiver); }
this.messageFactory = nodeConfiguration.getTopicMessageFactory(); this.fullRobotModel = robotModel.createFullRobotModel(); HumanoidRobotDataReceiver robotDataReceiver = new HumanoidRobotDataReceiver(fullRobotModel, null); rosAPI_communicator.attachListener(RobotConfigurationData.class, robotDataReceiver); rosAPI_communicator.attachListener(RobotConfigurationData.class, ppsOffsetProvider);
this.messageFactory = nodeConfiguration.getTopicMessageFactory(); this.fullRobotModel = robotModel.createFullRobotModel(); HumanoidRobotDataReceiver robotDataReceiver = new HumanoidRobotDataReceiver(fullRobotModel, null); rosAPI_communicator.attachListener(RobotConfigurationData.class, robotDataReceiver); rosAPI_communicator.attachListener(RobotConfigurationData.class, ppsOffsetProvider);
this.messageFactory = nodeConfiguration.getTopicMessageFactory(); this.fullRobotModel = robotModel.createFullRobotModel(); HumanoidRobotDataReceiver robotDataReceiver = new HumanoidRobotDataReceiver(fullRobotModel, null); rosAPI_communicator.attachListener(RobotConfigurationData.class, robotDataReceiver); rosAPI_communicator.attachListener(RobotConfigurationData.class, ppsOffsetProvider);
yoGraphicsListRegistry.setYoGraphicsUpdatedRemotely(false); ForceSensorDataHolder forceSensorDataHolder = new ForceSensorDataHolder(Arrays.asList(fullRobotModel.getForceSensorDefinitions())); HumanoidRobotDataReceiver robotDataReceiver = new HumanoidRobotDataReceiver(fullRobotModel, forceSensorDataHolder);
private BehaviorDispatcher<HumanoidBehaviorType> setupBehaviorDispatcher(String robotName, FullHumanoidRobotModel fullRobotModel, Ros2Node ros2Node, YoGraphicsListRegistry yoGraphicsListRegistry, YoVariableRegistry registry) { ForceSensorDataHolder forceSensorDataHolder = new ForceSensorDataHolder(Arrays.asList(fullRobotModel.getForceSensorDefinitions())); robotDataReceiver = new HumanoidRobotDataReceiver(fullRobotModel, forceSensorDataHolder); ROS2Tools.createCallbackSubscription(ros2Node, RobotConfigurationData.class, ControllerAPIDefinition.getPublisherTopicNameGenerator(robotName), s -> robotDataReceiver.receivedPacket(s.takeNextData())); BehaviorControlModeSubscriber desiredBehaviorControlSubscriber = new BehaviorControlModeSubscriber(); ROS2Tools.createCallbackSubscription(ros2Node, BehaviorControlModePacket.class, IHMCHumanoidBehaviorManager.getSubscriberTopicNameGenerator(robotName), s -> desiredBehaviorControlSubscriber.receivedPacket(s.takeNextData())); HumanoidBehaviorTypeSubscriber desiredBehaviorSubscriber = new HumanoidBehaviorTypeSubscriber(); ROS2Tools.createCallbackSubscription(ros2Node, HumanoidBehaviorTypePacket.class, IHMCHumanoidBehaviorManager.getSubscriberTopicNameGenerator(robotName), s -> desiredBehaviorSubscriber.receivedPacket(s.takeNextData())); YoVariableServer yoVariableServer = null; yoGraphicsListRegistry.setYoGraphicsUpdatedRemotely(false); BehaviorDispatcher<HumanoidBehaviorType> ret = new BehaviorDispatcher<>(robotName, yoTime, robotDataReceiver, desiredBehaviorControlSubscriber, desiredBehaviorSubscriber, ros2Node, yoVariableServer, HumanoidBehaviorType.class, HumanoidBehaviorType.STOP, registry, yoGraphicsListRegistry); return ret; }
robotDataReceiver = new HumanoidRobotDataReceiver(fullRobotModel, forceSensorDataHolder); ROS2Tools.createCallbackSubscription(ros2Node, RobotConfigurationData.class, ControllerAPIDefinition.getPublisherTopicNameGenerator(robotName), s -> robotDataReceiver.receivedPacket(s.takeNextData()));
yoGraphicsListRegistry.setYoGraphicsUpdatedRemotely(false); ForceSensorDataHolder forceSensorDataHolder = new ForceSensorDataHolder(Arrays.asList(fullRobotModel.getForceSensorDefinitions())); HumanoidRobotDataReceiver robotDataReceiver = new HumanoidRobotDataReceiver(fullRobotModel, forceSensorDataHolder);
humanoidRobotDataReceiver = new HumanoidRobotDataReceiver(fullHumanoidRobotModel, forceSensorDataHolder); planCompleted = false;