[optmotiongen] fix gravity torque calculation

[mmurooka]

• test torque calculation result with irtdyna https://github.com/jsk-ros-pkg/jsk_control/commit/3ea8e32cce18a97b862cb77c9ba30c4acc666357
• update to support robot links with non-zero mass, which are not contained in (send *robot* :links) . (RHP4B, PR2, and other robots have such links.) https://github.com/jsk-ros-pkg/jsk_control/commit/c359a6b53945c5662400d3aa6439d35676b8feaf
• update manual document of gravity torque jacobian https://github.com/jsk-ros-pkg/jsk_control/commit/b814fe290975bff116a4c7a2ab7452d253e88708

[mmurooka]

Demo of torque and torque jacobian calculation:

roscd eus_qp/optmotiongen/euslisp/demo/
roseus demo-rhp4b-torque-gradient.l "(demo-rhp4b-torque-gradient)"

Before this PR's fix:

contact-torque-error: 6.851075e-15
gravity-torque-error: 7.19025 ;; <= not correct !!!                                                                                                                                                                                    
contact-torque-jacobian-error: 1.435224e-05
gravity-torque-jacobian-error: 2.768217e-05
torque-jacobian-error: 3.197068e-05

After this PR's fix:

contact-torque-error: 6.851075e-15
gravity-torque-error: 3.745714e-14 ;; <= correct !!!                                                                                                                                                                                   
contact-torque-jacobian-error: 1.435224e-05
gravity-torque-jacobian-error: 3.321519e-05
torque-jacobian-error: 3.682560e-05

[sshige]

遅くなってしまい申し訳ありません。コードを確認しました。

別の環境でもテストしてみて

(scale
               -1
               (get-gravity-torque
        :robot _robot-env
        :drive-joint-list _drive-joint-list
        :gravity-link-list (send _robot-env :robot :links)
        ))

と

(send (send _robot-env :robot) :calc-torque)

の値がiteration中も、変わらないことも確認したので、合っていると思います。

マニュアルも変更箇所を確認しました。こちらも合っていると思います。

[mmurooka]

Thank you for your review.