VRMLにAttitudeSensorを定義して使いたい

姿勢角を出力するセンサを入手したので， https://github.com/fkanehiro/hrpsys-base/blob/master/lib/io/iob.h#L562 にある read_attitude_sensorを使って，姿勢角をhrpsysのレイヤーまで持ってきたいです．

手順としては

read_attitude_sensorなどをオーバーライドする
VRML
VRMLにPROTO AttitudeSensorを定義する
openhrp3
server/ModelLoader/BodyInfo_impl.cpp のreadSensorNode に AttitudeSensor を追加する
hrplib/hrpModel/Sensor.h と hrplib/hrpModel/Sensor.cppに AttitudeSensorクラスを追加する
hrplib/hrpModel/ModelLoaderUtil.cpp のcreateSensorsに AttitudeSensor用の条件分岐を追加する
hrplib/hrpModel/ModelNodeSet.cppのModelNodeSetImplにAttitudeSensor用の行を追加する
hrpsys-base
iobのattitude_sensorに関わる部分を実装する
https://github.com/fkanehiro/hrpsys-base/tree/master/rtc/RobotHardware にあるファイルたちに，AttitudeSensorに関わる部分を追加する
lib/util/PortHandler.cpp と lib/uitl/PortHandler.h にAttitudeSensorPortHandlerクラスを追加する
lib/util/BodyRTC.cppのcreateOutPortにAttitudeSensorに関わる部分を追加する
idl/RobotHardwareService.idlのRobotStateにAttitudeSensorに関わる部分を追加する

以外に何をする必要がありますでしょうか？

上記の変更だけですと，以下のように https://github.com/fkanehiro/hrpsys-base/blob/master/rtc/ImpedanceController/ImpedanceController.cpp#L583 のif文のチェックに引っかかってしまいます．

ImpedanceController is not working...
         m_qRef 0
     m_qCurrent 0

ご確認よろしくお願いします．

fkanehiro / openhrp3