public void doControl() {
   jointStateUpdater.updateJointState();
   fullRobotModel.updateFrames();
 }
 public void initialize() {
   jointStateUpdater.initialize();
   fullRobotModel.updateFrames();
 }