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(); }
estimatorForceSensorDataHolder = new ForceSensorDataHolder(Arrays.asList(estimatorFullRobotModel.getForceSensorDefinitions())); estimatorCenterOfMassDataHolder = new CenterOfMassDataHolder(); estimatorRawJointSensorDataHolderMap = new RawJointSensorDataHolderMap(estimatorFullRobotModel);
inputForceSensors = new ForceSensorDataHolder(forceSensorDefinitions); outputForceSensors = new ForceSensorDataHolder(forceSensorDefinitions);
estimatorForceSensorDataHolder = new ForceSensorDataHolder(Arrays.asList(estimatorFullRobotModel.getForceSensorDefinitions())); estimatorCenterOfMassDataHolder = new CenterOfMassDataHolder(); estimatorRawJointSensorDataHolderMap = new RawJointSensorDataHolderMap(estimatorFullRobotModel);
public IntermediateEstimatorStateHolder(WholeBodyControllerParameters wholeBodyControlParameters, RigidBody estimatorRootBody, RigidBody controllerRootBody, ForceSensorDataHolder estimatorForceSensorDataHolder, ForceSensorDataHolder controllerForceSensorDataHolder, CenterOfMassDataHolder estimatorCenterOfMassDataHolder, CenterOfMassDataHolder controllerCenterOfMassDataHolder, ContactSensorHolder estimatorContactSensorHolder, ContactSensorHolder controllerContactSensorHolder, RawJointSensorDataHolderMap estimatorRawJointSensorDataHolderMap, RawJointSensorDataHolderMap controllerRawJointSensorDataHolderMap) { FullHumanoidRobotModel intermediateModel = wholeBodyControlParameters.createFullRobotModel(); RigidBody intermediateRootBody = intermediateModel.getElevator(); estimatorChecksum = new InverseDynamicsJointStateChecksum(estimatorRootBody, estimatorChecksumCalculator); controllerChecksum = new InverseDynamicsJointStateChecksum(controllerRootBody, controllerChecksumCalculator); estimatorToIntermediateCopier = new InverseDynamicsJointStateCopier(estimatorRootBody, intermediateRootBody); intermediateToControllerCopier = new InverseDynamicsJointStateCopier(intermediateRootBody, controllerRootBody); this.estimatorForceSensorDataHolder = estimatorForceSensorDataHolder; this.intermediateForceSensorDataHolder = new ForceSensorDataHolder(Arrays.asList(intermediateModel.getForceSensorDefinitions())); this.controllerForceSensorDataHolder = controllerForceSensorDataHolder; this.estimatorCenterOfMassDataHolder = estimatorCenterOfMassDataHolder; this.intermediateCenterOfMassDataHolder = new CenterOfMassDataHolder(); this.controllerCenterOfMassDataHolder = controllerCenterOfMassDataHolder; this.estimatorContactSensorHolder = estimatorContactSensorHolder; this.intermediateContactSensorHolder = new ContactSensorHolder(Arrays.asList(intermediateModel.getContactSensorDefinitions())); this.controllerContactSensorHolder = controllerContactSensorHolder; RawJointSensorDataHolderMap intermediateRawJointSensorDataHolderMap = new RawJointSensorDataHolderMap(intermediateModel); rawDataEstimatorToIntermadiateCopier = new RawJointSensorDataHolderMapCopier(estimatorRawJointSensorDataHolderMap, intermediateRawJointSensorDataHolderMap); rawDataIntermediateToControllerCopier = new RawJointSensorDataHolderMapCopier(intermediateRawJointSensorDataHolderMap, controllerRawJointSensorDataHolderMap); }
public ThreadDataSynchronizer(FullHumanoidRobotModelFactory robotModelFactory) estimatorForceSensorDataHolder = new ForceSensorDataHolder(Arrays.asList(estimatorFullRobotModel.getForceSensorDefinitions())); estimatorCenterOfMassDataHolder = new CenterOfMassDataHolder(); estimatorRawJointSensorDataHolderMap = new RawJointSensorDataHolderMap(estimatorFullRobotModel); controllerForceSensorDataHolder = new ForceSensorDataHolder(Arrays.asList(controllerFullRobotModel.getForceSensorDefinitions())); controllerCenterOfMassDataHolder = new CenterOfMassDataHolder(); controllerContactSensorHolder = new ContactSensorHolder(Arrays.asList(controllerFullRobotModel.getContactSensorDefinitions()));
public ThreadDataSynchronizer(WholeBodyControllerParameters wholeBodyControlParameters) estimatorForceSensorDataHolder = new ForceSensorDataHolder(Arrays.asList(estimatorFullRobotModel.getForceSensorDefinitions())); estimatorCenterOfMassDataHolder = new CenterOfMassDataHolder(); estimatorRawJointSensorDataHolderMap = new RawJointSensorDataHolderMap(estimatorFullRobotModel); controllerForceSensorDataHolder = new ForceSensorDataHolder(Arrays.asList(controllerFullRobotModel.getForceSensorDefinitions())); controllerCenterOfMassDataHolder = new CenterOfMassDataHolder(); controllerContactSensorHolder = new ContactSensorHolder(Arrays.asList(controllerFullRobotModel.getContactSensorDefinitions()));
public IntermediateEstimatorStateHolder(FullHumanoidRobotModelFactory fullRobotModelFactory, RigidBodyBasics estimatorRootBody, RigidBodyBasics controllerRootBody, ForceSensorDataHolder estimatorForceSensorDataHolder, ForceSensorDataHolder controllerForceSensorDataHolder, CenterOfMassDataHolder estimatorCenterOfMassDataHolder, CenterOfMassDataHolder controllerCenterOfMassDataHolder, ContactSensorHolder estimatorContactSensorHolder, ContactSensorHolder controllerContactSensorHolder, RawJointSensorDataHolderMap estimatorRawJointSensorDataHolderMap, RawJointSensorDataHolderMap controllerRawJointSensorDataHolderMap) { FullHumanoidRobotModel intermediateModel = fullRobotModelFactory.createFullRobotModel(); RigidBodyBasics intermediateRootBody = intermediateModel.getElevator(); estimatorChecksum = new InverseDynamicsJointStateChecksum(estimatorRootBody, estimatorChecksumCalculator); controllerChecksum = new InverseDynamicsJointStateChecksum(controllerRootBody, controllerChecksumCalculator); estimatorToIntermediateCopier = new InverseDynamicsJointStateCopier(estimatorRootBody, intermediateRootBody); intermediateToControllerCopier = new InverseDynamicsJointStateCopier(intermediateRootBody, controllerRootBody); this.estimatorForceSensorDataHolder = estimatorForceSensorDataHolder; this.intermediateForceSensorDataHolder = new ForceSensorDataHolder(Arrays.asList(intermediateModel.getForceSensorDefinitions())); this.controllerForceSensorDataHolder = controllerForceSensorDataHolder; this.estimatorCenterOfMassDataHolder = estimatorCenterOfMassDataHolder; this.intermediateCenterOfMassDataHolder = new CenterOfMassDataHolder(); this.controllerCenterOfMassDataHolder = controllerCenterOfMassDataHolder; this.estimatorContactSensorHolder = estimatorContactSensorHolder; this.intermediateContactSensorHolder = new ContactSensorHolder(Arrays.asList(intermediateModel.getContactSensorDefinitions())); this.controllerContactSensorHolder = controllerContactSensorHolder; RawJointSensorDataHolderMap intermediateRawJointSensorDataHolderMap = new RawJointSensorDataHolderMap(intermediateModel); rawDataEstimatorToIntermadiateCopier = new RawJointSensorDataHolderMapCopier(estimatorRawJointSensorDataHolderMap, intermediateRawJointSensorDataHolderMap); rawDataIntermediateToControllerCopier = new RawJointSensorDataHolderMapCopier(intermediateRawJointSensorDataHolderMap, controllerRawJointSensorDataHolderMap); }
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; }
ForceSensorDataHolder forceSensorDataHolderToUpdate = new ForceSensorDataHolder(Arrays.asList(forceSensorDefinitions)); CenterOfMassDataHolder centerOfMassDataHolderToUpdate = new CenterOfMassDataHolder();
DesiredJointDataHolder estimatorDesiredJointDataHolder = null; ForceSensorDataHolder forceSensorDataHolderToUpdate = new ForceSensorDataHolder(Arrays.asList(forceSensorDefinitions)); CenterOfMassDataHolder centerOfMassDataHolderToUpdate = new CenterOfMassDataHolder();
yoTimeLastFullRobotModelUpdate = new YoDouble("yoTimeRobotModelUpdate", registry); ForceSensorDataHolder forceSensorDataHolder = new ForceSensorDataHolder(Arrays.asList(fullRobotModel.getForceSensorDefinitions())); robotDataReceiver = new HumanoidRobotDataReceiver(fullRobotModel, forceSensorDataHolder); ROS2Tools.createCallbackSubscription(ros2Node, RobotConfigurationData.class, ControllerAPIDefinition.getPublisherTopicNameGenerator(robotName),
DesiredJointDataHolder estimatorDesiredJointDataHolder = null; ForceSensorDataHolder forceSensorDataHolderToUpdate = new ForceSensorDataHolder(Arrays.asList(forceSensorDefinitions)); CenterOfMassDataHolder centerOfMassDataHolderToUpdate = new CenterOfMassDataHolder();
ForceSensorDataHolder forceSensorDataHolder = new ForceSensorDataHolder(Arrays.asList(fullRobotModel.getForceSensorDefinitions())); HumanoidRobotDataReceiver robotDataReceiver = new HumanoidRobotDataReceiver(fullRobotModel, forceSensorDataHolder);
JointDesiredOutputList estimatorDesiredJointDataHolder = null; ForceSensorDataHolder forceSensorDataHolderToUpdate = new ForceSensorDataHolder(Arrays.asList(forceSensorDefinitions)); CenterOfMassDataHolder centerOfMassDataHolderToUpdate = new CenterOfMassDataHolder();
ForceSensorDataHolder forceSensorDataHolder = new ForceSensorDataHolder(forceSensorDefinitionList); JointConfigurationGatherer jointConfigurationGatherer = new JointConfigurationGatherer(humanoidFullRobotModel, forceSensorDataHolder);
ForceSensorDataHolder forceSensorDataHolder = new ForceSensorDataHolder(Arrays.asList(fullHumanoidRobotModel.getForceSensorDefinitions())); humanoidRobotDataReceiver = new HumanoidRobotDataReceiver(fullHumanoidRobotModel, forceSensorDataHolder); planCompleted = false;
this.forceSensorDataHolderToUpdate = forceSensorDataHolderToUpdate; outputForceSensorDataHolder = new ForceSensorDataHolder(inputForceSensorDataHolder.getForceSensorDefinitions()); outputForceSensorDataHolderWithGravityCancelled = new ForceSensorDataHolder(inputForceSensorDataHolder.getForceSensorDefinitions());
ForceSensorDataHolder forceSensorDataHolder = new ForceSensorDataHolder(forceSensorDefinitionList); JointConfigurationGatherer jointConfigurationGatherer = new JointConfigurationGatherer(humanoidFullRobotModel, forceSensorDataHolder);