private FramePose2d getCurrentMidFeetPose2d(HumanoidReferenceFrames referenceFrames) {
robotDataReceiver.updateRobotModel();
referenceFrames.updateFrames();
ReferenceFrame midFeetFrame = referenceFrames.getMidFeetZUpFrame();
FramePose midFeetPose = new FramePose();
midFeetPose.setToZero(midFeetFrame);
midFeetPose.changeFrame(ReferenceFrame.getWorldFrame());
FramePose2d ret = new FramePose2d();
ret.setPoseIncludingFrame(
midFeetPose.getReferenceFrame(),
midFeetPose.getX(),
midFeetPose.getY(),
midFeetPose.getYaw());
return ret;
}
public RosConnectedZeroPoseRobotConfigurationDataProducer(
URI rosMasterURI, PacketCommunicator objectCommunicator, final DRCRobotModel robotModel) {
this.robotModel = robotModel;
this.packetCommunicator = objectCommunicator;
fullRobotModel = robotModel.createFullRobotModel();
referenceFrames = new HumanoidReferenceFrames(fullRobotModel);
pelvisFrame = referenceFrames.getPelvisFrame();
headFrame = referenceFrames.getNeckFrame(NeckJointName.PROXIMAL_NECK_PITCH);
forceSensorDefinitions = fullRobotModel.getForceSensorDefinitions();
updateRobotLocationBasedOnMultisensePose(new RigidBodyTransform());
if (rosMasterURI != null) {
NodeConfiguration nodeConfiguration = RosTools.createNodeConfiguration(rosMasterURI);
NodeMainExecutor nodeMainExecutor = DefaultNodeMainExecutor.newDefault();
nodeMainExecutor.execute(this, nodeConfiguration);
}
}