search

start-jsk / rtmros_gazebo

gazebo simulation for rtmros robots
8 stars 24 forks source link

[hrosys_gazebo_general/iob.cpp]check number_of_joints #235

Closed Naoki-Hiraoka closed 4 years ago

Naoki-Hiraoka commented 6 years ago

hrpsys_gazebo_general/iob/iob.cppの https://github.com/start-jsk/rtmros_gazebo/blob/7389a49529be6d373581779f0aa88563c6e6d2bf/hrpsys_gazebo_general/iob/iob.cpp#L370-L382 の，command[JOINT_ID_REAL2MODEL(i)],prev_command[JOINT_ID_REAL2MODEL(i)]が，配列の範囲外へアクセスしています．

アクセス前に配列のサイズを確認するようにしました．

以下，現状の説明です． 発生条件 hrpsysがロードするモデルファイルとgazeboがロードするモデルファイルが異なり，gazeboのIOBPluginが扱う関節の中にhrpsysが扱わない関節がある場合に， https://github.com/start-jsk/rtmros_gazebo/blob/7389a49529be6d373581779f0aa88563c6e6d2bf/hrpsys_gazebo_general/iob/iob.cpp#L742 で読み込むjoint_id_listに書かれた数字の中に，hrpsysの関節数以上の数字が書かれている場合． (例えばhrp2jskntsは，HPR3Handが付いているため，hrpsysで扱う関節は34個ですが，IOBPluginが扱う関節は46個です)

問題点 配列command,prev_commandは https://github.com/start-jsk/rtmros_gazebo/blob/7389a49529be6d373581779f0aa88563c6e6d2bf/hrpsys_gazebo_general/iob/iob.cpp#L158-L159 https://github.com/fkanehiro/hrpsys-base/blob/fd3bf75a1dc9d0e14898ff11a7c2f35b525f5b59/rtc/RobotHardware/robot.cpp#L78 でサイズがhrp::Body::numJoints()，即ちhrpsysがロードするモデルファイルの関節数に等しくなるように定められています．

一方，'''JOINT_ID_REAL2MODEL(i)'''は，gazeboのIOBPluginに合わせて人が.yamlファイルのjoint_id_listに書いた数字がそのまま入っているため，command,prev_commandのサイズ以上の数字が書かれている可能性があります．

症状 プログラムが止まることはありませんが，配列の範囲外にアクセスしてしまう関節はgazeboに送られるjoint_commandトピックのposition,velocityの指令値に4.00135071e-314，2.660006257743614e+250などの値が入ってしまいます．

Naoki-Hiraoka commented 4 years ago

gazeboのIOBPluginが扱う関節の中にhrpsysが扱わない関節があるのが悪いので， IOBPluginが読み込むyamlファイルの

hrpsys_gazebo_configuration: 
    joints:

を，hrpsysが扱う関節のみを指定する形に変更するのが正しい直し方でした．