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); }
@Before public void setUp() { registry = new YoVariableRegistry(getClass().getSimpleName()); this.yoTime = new YoDouble("yoTime", registry); this.ros2Node = ROS2Tools.createRos2Node(PubSubImplementation.INTRAPROCESS, "ihmc_humanoid_behavior_dispatcher_test"); drcSimulationTestHelper = new DRCSimulationTestHelper(simulationTestingParameters, getRobotModel()); drcSimulationTestHelper.createSimulation(getSimpleRobotName()); MemoryTools.printCurrentMemoryUsageAndReturnUsedMemoryInMB(getClass().getSimpleName() + " before test."); robot = drcSimulationTestHelper.getRobot(); fullRobotModel = getRobotModel().createFullRobotModel(); walkingControllerParameters = getRobotModel().getWalkingControllerParameters(); yoGraphicsListRegistry = new YoGraphicsListRegistry(); behaviorDispatcher = setupBehaviorDispatcher(getRobotModel().getSimpleRobotName(), fullRobotModel, ros2Node, yoGraphicsListRegistry, registry); CapturePointUpdatable capturePointUpdatable = createCapturePointUpdateable(drcSimulationTestHelper, registry, yoGraphicsListRegistry); behaviorDispatcher.addUpdatable(capturePointUpdatable); DRCRobotSensorInformation sensorInfo = getRobotModel().getSensorInformation(); for (RobotSide robotSide : RobotSide.values) { WristForceSensorFilteredUpdatable wristSensorUpdatable = new WristForceSensorFilteredUpdatable(drcSimulationTestHelper.getRobotName(), robotSide, fullRobotModel, sensorInfo, robotDataReceiver.getForceSensorDataHolder(), IHMCHumanoidBehaviorManager.BEHAVIOR_YO_VARIABLE_SERVER_DT, drcSimulationTestHelper.getRos2Node(), registry); behaviorDispatcher.addUpdatable(wristSensorUpdatable); } referenceFrames = robotDataReceiver.getReferenceFrames(); }
HumanoidRobotDataReceiver robotDataReceiver = new HumanoidRobotDataReceiver(fullRobotModel, forceSensorDataHolder); HumanoidReferenceFrames referenceFrames = robotDataReceiver.getReferenceFrames(); behaviorPacketCommunicator.attachListener(RobotConfigurationData.class, robotDataReceiver);
referenceFrames = robotDataReceiver.getReferenceFrames(); humanoidBehabiorTypePublisher = ROS2Tools.createPublisher(ros2Node, HumanoidBehaviorTypePacket.class, IHMCHumanoidBehaviorManager.getSubscriberTopicNameGenerator(robotName));
HumanoidRobotDataReceiver robotDataReceiver = new HumanoidRobotDataReceiver(fullRobotModel, forceSensorDataHolder); HumanoidReferenceFrames referenceFrames = robotDataReceiver.getReferenceFrames();
ReferenceFrame soleFrame = humanoidRobotDataReceiver.getReferenceFrames().getSoleFrame(RobotSide.LEFT); FramePose3D initialStancePose = new FramePose3D(soleFrame, new Point3D(0.0, 0.0, 0.001), new AxisAngle()); initialStancePose.changeFrame(ReferenceFrame.getWorldFrame());