search

start-jsk / rtmros_choreonoid

using chreonoid for simulator with hrpsys and other ros system
9 stars 43 forks source link

[hrpsys_choreonoid/iob/iob.cpp] fix bug on pdgains_sim_file_name #326

Closed Naoki-Hiraoka closed 3 years ago

Naoki-Hiraoka commented 4 years ago

PDゲインに関するバグを直すPull Requestです． RobotHardwareServiceのsetServoGainPercentageを呼ぶと，もともと数万だったゲインの値が突然1になるというバグがありました．

現在choreonoidの世界で実際に関節のトルクに適用されるPDゲインは， 始めは.confのpdgains_sim_file_nameに書かれた値 ですが， 一度でもRobotHardwareServiceのsetServoGainPercentageやpowerが呼ばれると， 以後は .conf(または.RobotHardware.conf)のpdgains.file_nameに書かれた値 になってしまい， pdgains_sim_file_nameの値が参照されなくなります．

https://github.com/start-jsk/rtmros_gazebo/blob/master/hrpsys_gazebo_general/iob/iob.cpp と同様に， 始めは.confのpdgains_sim_file_nameに書かれた値 ですが， 一度でもRobotHardwareServiceのsetServoGainPercentageやpowerが呼ばれると， 以後は.confのpdgains_sim_file_nameに書かれた値に .conf(または.RobotHardware.conf)のpdgains.file_nameに書かれた値をかけたものになるようにしました．

.confのpdgains_sim_file_name = hrpsysが関知しないハードウェア側が持つゲイン .conf(またはRobotHardware.conf)のpdgains.file_name = hrpsysが関知するゲイン

Naoki-Hiraoka commented 3 years ago

https://github.com/start-jsk/rtmros_choreonoid/blob/master/hrpsys_choreonoid_tutorials/models/JAXON_JVRC.PDgains.sav と https://github.com/start-jsk/rtmros_choreonoid/blob/master/hrpsys_choreonoid_tutorials/models/JAXON_JVRC.PDgains_sim.dat が同じ値になっているのを見ると， 上に書いたpdgains_sim_file_nameとpdgains.file_nameの関係は，現在の運用とは異なっているようです． 現在，pdgains_sim_file_nameとpdgains.file_nameの関係はどうなっているのでしょうか．

YutaKojio commented 3 years ago

@kindsenior

上に書いたpdgains_sim_file_nameとpdgains.file_nameの関係は，現在の運用とは異なっているようです． 現在，pdgains_sim_file_nameとpdgains.file_nameの関係はどうなっているのでしょうか．

このPRをマージするとトルク制御モードで起動後関節が暴れるので，運用が異なっているということだと思いますが，現状どうなっているか教えてくれると助かります．

Naoki-Hiraoka commented 3 years ago

現状を整理すると，

となっていて， hrpsysは，RobotHardwareServiceのsetServoGainPercentageやpowerがよばれると，pdgains.file_nameの値を引数としてiob.hのwrite_pgain等を呼びます．

Naoki-Hiraoka commented 3 years ago

問題をややこしくしているのは，以下の3点です．

YoheiKakiuchi commented 3 years ago

URATA系のロボットは、もともと関節のゲイン(Nm/rad)が分かっていないので、 pdgains.file_name(実機)は全関節のゲインに1.0が入っている。

pdgains_sim_file_nameはsimulationに使う関節の実ゲイン(Nm/rad)が入っている。

1.0 x sim_gain をゲインとして使っていて、RobotHardwareを使うことで、 setJointGainPercentage等を使えるようになっている。 (シミュレーションだとpowerを呼ばない前提)

と思っていたのだけど、そうなっていなかったのを平岡くんが今回正しく直してくれたんだと思っています。

YoheiKakiuchi commented 3 years ago

あと、JAXON_JVRCは古い実装で、RobotHardwareを使わないので比較に入れないほうがいいです。 JAXON_JVRCもRobotHardwareで動くはずなので、全部そちらに移行するのはありだと思います。

YutaKojio commented 3 years ago

USE_ROBOTHARDWARE:=trueしてるときでも呼ばれるのは https://github.com/start-jsk/rtmros_choreonoid/blob/master/hrpsys_choreonoid_tutorials/models/JAXON_JVRC.PDgains.sav だったんですね． 上のgainを全部１にするとこのPRありで正常に起動しました．

ということで，このPR方針が正しくて https://github.com/start-jsk/rtmros_choreonoid/blob/master/hrpsys_choreonoid_tutorials/models/JAXON_JVRC.PDgains.sav も併せて直すということでいいでしょうか？

YoheiKakiuchi commented 3 years ago

@YutaKojio そうですね。その方針で、JAXONも常にUSE_ROBOTHARDWARE:=trueにしましょうか。

Naoki-Hiraoka commented 3 years ago

皆様ありがとうございます。 このPRの方針で進めます。

Naoki-Hiraoka commented 3 years ago

https://github.com/start-jsk/rtmros_choreonoid/blob/master/hrpsys_choreonoid_tutorials/models/JAXON_JVRC.PDgains.sav をすべて1にし、JAXONも常にUSE_ROBOTHARDWARE:=trueにしました

kindsenior commented 3 years ago

すみません．出遅れました

僕も全ては理解できていませんでしたが， @YoheiKakiuchi の通りだと思うので，このPRの方針で問題ないと思います

以下補足です

HRP-2はトルク制御ができないので関係ないかもしれませんが， トルク制御モードのとき（isPosTq[i]がtrue）のPDゲインの扱いとして現状の https://github.com/start-jsk/rtmros_choreonoid/blob/e1b66948ce8ac12c20e44c3f8016e1106cb69f15/hrpsys_choreonoid/iob/iob.cpp#L777-L781 は不都合だと思っていて ee548e1ca49afdb961e65fabc8f4f532603cd666 の https://github.com/kindsenior/rtmros_choreonoid/blob/84cdfaddaa3e82c2bf128f34e07098e026a1e463/hrpsys_choreonoid/iob/iob.cpp#L780-L783 のように単純にトルクとPD制御を足すべきだと思っています

このようにするとトルクのゲインに関しては，initial_tqPgainもtqPgainも0-1の値になるようになります （トルクのmaxゲインは常に0 or 1で，真のゲインも割合を示すゲインも0-1）

なのでPRを出せていなくて申し訳ないのですが， もしトルク制御を使う予定があるならトルクのPDゲインに関しては 7a2f376ba078bb439644587b952ded6b2743520c のようにするといいと思っています

Naoki-Hiraoka commented 3 years ago

mergeをお願いできますでしょうか．