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

[Naoki-Hiraoka]

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]

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]

@kindsenior

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

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

[Naoki-Hiraoka]

現状を整理すると，

• 現在のmasterでは，関節ゲインは起動直後はpdgains_sim_file_nameの値．iob.hのwrite_pgain等を呼ぶと引数の値で上書きされる．

• このPullRequestでは，関節ゲインは起動直後はpdgains_sim_file_nameの値．iob.hのwrite_pgain等を呼ぶと引数の値とpdgains_sim_file_nameの値を掛けた値になる．

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

[Naoki-Hiraoka]

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

• iob.hのwrite_pgain等は，https://github.com/fkanehiro/hrpsys-base/blob/e98cf26c12eeeda761cef5b836e9640d68bf7387/lib/io/iob.h#L336 によれば単位は[Nm/rad]となっているが，実機の某ロボットはそうなっていない．
• 実機は必ず最初にRobotHardwareServiceのpowerを呼ぶので，write_pgainが呼ばれる．しかし，hrpsys_choreonoidはシミュレーション開始時にRobotHardwareServiceのpowerを呼ばないので，write_pgainが呼ばれない．が，hrpsys_choreonoidでもRobotHardwareServiceのsetServoGainPercentageやpowerを呼ぶと，write_pgainが呼ばれる．
• masterのhrpsys_choreonoidが，「pdgains_sim_file_nameはシミュレーション時のゲイン」「pdgains.file_nameは実機のゲイン」というような区別をできていない．

[YoheiKakiuchi]

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

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

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

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

[YoheiKakiuchi]

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

[YutaKojio]

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]

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

[Naoki-Hiraoka]

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

[Naoki-Hiraoka]

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

[kindsenior]

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

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

以下補足です

HRP-2はトルク制御ができないので関係ないかもしれませんが，
トルク制御モードのとき（isPosTq[i]がtrue）のPDゲインの扱いとして現状の

rtmros_choreonoid/hrpsys_choreonoid/iob/iob.cpp

Lines 777 to 781 in e1b6694

	if(isPosTq[i]){
	// position & torque control
	//ctq = -(q - q_ref) * Pgain[i] - (dq - dq_ref) * Dgain[i] - (tq - tq_ref) * tqPgain[i] - (dtq - dtq_ref) * tqDgain[i];
	ctq = -(q - q_ref) * Pgain[i] / (tqPgain[i] + 1) - (dq - dq_ref) * Dgain[i] / (tqPgain[i] + 1)
	+ tq_ref * tqPgain[i] / (tqPgain[i] + 1) - (dtq - dtq_ref) * tqDgain[i] / (tqPgain[i] + 1);

は不都合だと思っていて
ee548e1 の
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ゲインに関しては
7a2f376
のようにするといいと思っています

[Naoki-Hiraoka]

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