start-jsk/rtmros_tutorials — pull request #71 by mmurooka (2014-07-21 13:10:21+00:00)
Title: Not use toe joint in fullbody ik

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.
```lisp
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.

```lisp
(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)
```