Not use toe joint in fullbody ik

[mmurooka]

hrp2jsknt and hrp2jsknts have toe joint, but we usually do not use toe joints in full-body motion.
By this PR, toe joints are not used in :fullbody-inverse-kinematics by default for these robots.
I copied from euslib/rbrain/hrpmodel.l but following two pars are modified.

1.
(send-message* self robot-frame :fullbody-inverse-kinematics ;; robot-frame is not used.
=>
(send-super* :fullbody-inverse-kinematics
2.
(if (find-method (send self l) :toe-p) ;; we can't access joints in this way for irtrobot.
=>
(if (send self l :toe-p)

I checked that this PR works by following test code.
By this PR, toe joint angle keeps zero, although toe joint moves before this PR is applied.

(setq *hrp2* (instance hrp2jsknt-robot :init))
(send *hrp2* :reset-pose)
(send *hrp2* :fix-leg-to-coords (make-coords))
(let* ((target-limb (list :larm :rarm :lleg :rleg))
       (move-pos (list (float-vector 0 0 0) (float-vector 0 0 0) (float-vector 0 0 100) (float-vector 0 0 100)))
       (target-coords
        (mapcar #'(lambda (l p) (send (send (send *hrp2* l :end-coords) :copy-worldcoords) :translate p :world))
                target-limb move-pos))
       (move-target
        (mapcar
         #'(lambda (k)
             (send *hrp2* k :end-coords))
         target-limb))
       (link-list
        (mapcar
         #'(lambda (mt)
             (send *hrp2* :link-list
                   (send mt :parent)))
         move-target)))
  (send *hrp2* :fullbody-inverse-kinematics target-coords  :move-target move-target :link-list link-list))
(send *hrp2* :legs :toe-p :joint-angle)

[mmurooka]

I implemented range function in https://github.com/start-jsk/rtmros_tutorials/pull/71/files#diff-593ce387ed2190d39cabefd5de3453b8R87.

I think this function should be merge to euslisp repository and made Issue, euslisp/EusLisp#39.

[k-okada]

it seems better to use (mapcan #'(lambda (x) (list (list -20 0 0))) (list :lleg :rleg)), consider when we want to use different offset value for each leg (-20 for left and +20 for right), it's better to pass original fix-leges instead of just to pass number value.

Another comment is did you consider changing min/max value and use weight=0 for toe joint? the proposed code seems too complex just to "not to use toe joint"

[snozawa]

the proposed code seems too complex just to "not to use toe joint"

@mmurooka, I agree with this comment.
Currently, this PR is just copying from rbrain code and includes unnecessary codes.
For example, the codes including ;; set root-link 6dof-joint's weight based on legs' joint limit is not necessary for this PR.

1. Codes which uses toe information
   Some codes may be committed to jskeus itself.
   In terms of toe joints, we need to consider these things:

   • We need to select whether using toe joints or not.
     User can select this through weighting:

   (send robot :inverse-kinematics xxxx :additional-weight-list (mapcar #'(lambda (l) (list (send robot l :toe-p :child-link) 0)) '(:rleg :lleg))) ;; do not use toe joints
   

   (send robot :inverse-kinematics xxxx) ;; use toe joints
   

   • We need to set adequate target cog pos for fullbody IK.
     User can set these parameter:

   (send robot :inverse-kinematics xxxx
   :target-centroid-pos (apply #'midpoint 0.5 (mapcar #'(lambda (l) (send robot l :end-coords :transform-vector (float-vector -20 0 0))) (list :lleg :rleg)))
   )
   

   • We need to change contact polygon (currently not implemented in irteus).
   • etc

2. Default value
   For hrp2 robots, default value is not use toe joint.
   So, if you add the following codes, you can neglect toe joint by default.
   (defmethod hrp2jsknt-robot (:inverse-kinematics (target-coords &rest args &key (additional-weight-list (mapcar #'(lambda (l) (list (send self l :toe-p :child-link) 0)) '(:rleg :lleg)) &allow-other-keys) (send-super* :inverse-kinematics target-coords :additional-weight-list additional-weight-list args) ))

3. Usability
   Although users can select to select whether use toe or not through additional-weight-list and target-centroid-pos, using the new argument use-toe is convenient.
   Do you want to add this?

[mmurooka]

なるほど．．
ご指摘のdefmethodをしたらつま先を使わなくなりました．
これで十分そうなのでcloseします．

デフォルトでつま先を使わないようにする（hrp2jsknt-util.lにdefmethodを加える）のは，やりすぎでしょうか．

[snozawa]

デフォルトでつま先を使わないようにする（hrp2jsknt-util.lにdefmethodを加える）のは，やりすぎでしょうか．

はい、hrp2jsknts-utils.l, hrp2jsknt-utils.lにデフォルトでつま先weight0のメソッド上書きを追加しておいてください。
そうすれば、デフォルトでつま先使わない挙動ができます。

それで、さらにつま先を使うときはどうすべきかは、議論の余地がありそうで、
実際つま先使うコードをかいてみて、再度考えるのでもよさそうです。
まずは、デフォルトつま先weight 0のPRを出して見てください

悩みどころとしては、

• use-toe引数をいれるかどうか。
  　入れたら便利。
  　入れないなら、weightは:additional-weight-listで設定可能、target-centroid-posもこの引数で設定できるはできるが、それをかくひつようがある。
  　実は実際つま先動作をやってみると、ベタ足からつま先立ちまでうまくせんいする必要があり、
  　例えばつま先角度かIKのweightを使ってる状態から０までスケジューリングっぽいものがいりそう
  　などなど

[mmurooka]

はい、hrp2jsknts-utils.l, hrp2jsknt-utils.lにデフォルトでつま先weight0のメソッド上書きを追加しておいてください。

してみました．

[snozawa]

Mergeしました．