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

[eisoku9618]

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

手順としては

自前のiob

• 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

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