add an interface for multi legs footsteps

[eisoku9618]

set-foot-steps と set-foot-steps-with-param の引数にリストのリストを与えられるようにして，従来の使い方を保ちつつ， #792 (comment) のように

(send *ri* :set-foot-steps
      (list (list (make-coords :coords (send *robot* :lleg :end-coords :copy-worldcoords) :name :lleg)
                  (make-coords :coords (send *robot* :lleg :end-coords :copy-worldcoords) :name :rarm))
            (list (make-coords :coords (send (send *robot* :rleg :end-coords :copy-worldcoords) :translate (float-vector 200 0 0)) :name :rleg)
                  (make-coords :coords (send (send *robot* :rleg :end-coords :copy-worldcoords) :translate (float-vector 200 0 0)) :name :larm))
            (list (make-coords :coords (send (send *robot* :lleg :end-coords :copy-worldcoords) :translate (float-vector 200 0 0)) :name :lleg)
                  (make-coords :coords (send (send *robot* :lleg :end-coords :copy-worldcoords) :translate (float-vector 200 0 0)) :name :rarm))))

のような使い方が出来るようにしました．

以下サンプル

(progn
  (setq *robot* (samplerobot-init))
  (send *robot* :reset-manip-pose)
  (dolist (l '(:rarm :larm))
    (send *robot* l :inverse-kinematics (make-coords :pos (send (send *robot* l :end-coords :copy-worldcoords) :worldpos)) :debug-view :no-message))
  (send *ri* :angle-vector (send *robot* :angle-vector))
  (send *ri* :wait-interpolation)
  (send *ri* :set-auto-balancer-param :leg-names (list :rleg :lleg :rarm :larm))
  (send *ri* :set-gait-generator-param :default-step-height 0)
  (send *ri* :start-auto-balancer :limbs '(:rleg :lleg :rarm :larm))
  (send *ri* :set-foot-steps
        (list (list (make-coords :coords (send *robot* :lleg :end-coords :copy-worldcoords) :name :lleg)
                    (make-coords :coords (send *robot* :rarm :end-coords :copy-worldcoords) :name :rarm))
              (list (make-coords :coords (send *robot* :rleg :end-coords :copy-worldcoords) :name :rleg)
                    (make-coords :coords (send *robot* :larm :end-coords :copy-worldcoords) :name :larm))
              (list (make-coords :coords (send *robot* :lleg :end-coords :copy-worldcoords) :name :lleg)
                    (make-coords :coords (send *robot* :rarm :end-coords :copy-worldcoords) :name :rarm)))))

[eisoku9618]

fkanehiro/hrpsys-base#767 がマージされてから，こちらをまーじしていただけると幸いです．

[k-okada]

Please use jsk-ros-pkg/jsk_demos#1036 (comment) to restart travis

[eisoku9618]

hrpsysの方がマージされましたので、こちらもマージしていただけると幸いです。

hrpsysのcommit順的には
#794
をマージしていただいてからの方が良さそうです。

[k-okada]

https://travis-ci.org/start-jsk/rtmros_common/jobs/77248804
が通らないんだけど大丈夫かな．PRの時のtravisと違ってrtmros_hrp2を入れてコンパイルされるんだけど．．．次も通らなかったらログを見てみて下さい．

[eisoku9618]

https://github.com/start-jsk/rtmros_tutorials/blob/master/hrpsys_ros_bridge_tutorials/test/test-robot-model.l でfailしているみたいです
VRMLの可動域が変わった or 変換に失敗している？

[eisoku9618]

可動域が異様に小さくなっているみたいです．

 #<rotational-joint #X6e4e408 CHEST_JOINT0> :joint-angle(-2.0) violate max-angle(-2.0) �[0m�[0m
�]2;[build] 7/9��[0m�[33m;; #<rotational-joint #X6e4e408 CHEST_JOINT0> :joint-angle(-2.0) violate min-angle(2.0) �[0m�[0m
�]2;[build] 7/9��[0m�[33m;; #<rotational-joint #X6e543c0 CHEST_JOINT1> :joint-angle(-2.0) violate max-angle(-2.0) �[0m�[0m
�]2;[build] 7/9��[0m�[33m;; #<rotational-joint #X6e543c0 CHEST_JOINT1> :joint-angle(-2.0) violate min-angle(2.0) �[0m�[0m
�]2;[build] 7/9��[0m�[33m;; #<rotational-joint #X6e27248 RARM_JOINT6> :joint-angle(-1.0) violate max-angle(-1.0) �[0m�[0m
�]2;[build] 7/9��[0m�[33m;; #<rotational-joint #X6e27248 RARM_JOINT6> :joint-angle(-1.0) violate min-angle(1.0) �[0m�[0m
�]2;[build] 7/9��[0m�[33m;; #<rotational-joint #X6e274b8 RARM_JOINT7> :joint-angle(-1.0) violate max-angle(-1.0) �[0m�[0m
�]2;[build] 7/9��[0m�[33m;; #<rotational-joint #X6e274b8 RARM_JOINT7> :joint-angle(-1.0) violate min-angle(1.0) �[0m�[0m
�]2;[build] 7/9��[0m�[33m;; #<rotational-joint #X8522778 LARM_JOINT6> :joint-angle(-1.0) violate max-angle(-1.0) �[0m�[0m
�]2;[build] 7/9��[0m�[33m;; #<rotational-joint #X8522778 LARM_JOINT6> :joint-angle(-1.0) violate min-angle(1.0) �[0m�[0m
�]2;[build] 7/9��[0m�[33m;; #<rotational-joint #X85226b8 LARM_JOINT7> :joint-angle(-1.0) violate max-angle(-1.0) �[0m�[0m
�]2;[build] 7/9��[0m�[33m;; #<rotational-joint #X85226b8 LARM_JOINT7> :joint-angle(-1.0) violate min-angle(1.0) �[0m�[0m
�]2;[build] 7/9��[0m�[33m;; #<rotational-joint #X6e4e408 CHEST_JOINT0> :joint-angle(-2.0) violate max-angle(-2.0) �[0m�[0m
�]2;[build] 7/9��[0m�[33m;; #<rotational-joint #X6e4e408 CHEST_JOINT0> :joint-angle(-2.0) violate min-angle(2.0) �[0m�[0m
�]2;[build] 7/9��[0m�[33m;; #<rotational-joint #X6e543c0 CHEST_JOINT1> :joint-angle(-2.0) violate max-angle(-2.0) �[0m�[0m
�]2;[build] 7/9��[0m�[33m;; #<rotational-joint #X6e543c0 CHEST_JOINT1> :joint-angle(-2.0) violate min-angle(2.0) �[0m�[0m
�]2;[build] 7/9��[0m�[33m;; #<rotational-joint #X6e27248 RARM_JOINT6> :joint-angle(-1.0) violate max-angle(-1.0) �[0m�[0m
�]2;[build] 7/9��[0m�[33m;; #<rotational-joint #X6e27248 RARM_JOINT6> :joint-angle(-1.0) violate min-angle(1.0) �[0m�[0m
�]2;[build] 7/9��[0m�[33m;; #<rotational-joint #X6e274b8 RARM_JOINT7> :joint-angle(-1.0) violate max-angle(-1.0) �[0m�[0m
�]2;[build] 7/9��[0m�[33m;; #<rotational-joint #X6e274b8 RARM_JOINT7> :joint-angle(-1.0) violate min-angle(1.0) �[0m�[0m
�]2;[build] 7/9��[0m�[33m;; #<rotational-joint #X8522778 LARM_JOINT6> :joint-angle(-1.0) violate max-angle(-1.0) �[0m�[0m
�]2;[build] 7/9��[0m�[33m;; #<rotational-joint #X8522778 LARM_JOINT6> :joint-angle(-1.0) violate min-angle(1.0) �[0m�[0m
�]2;[build] 7/9��[0m�[33m;; #<rotational-joint #X85226b8 LARM_JOINT7> :joint-angle(-1.0) violate max-angle(-1.0) �[0m�[0m

[eisoku9618]

上の可動域が狭くなっている関節は以下のmin-max-tableで指定している関節のようです

https://github.com/start-jsk/rtmros_tutorials/blob/master/hrpsys_ros_bridge_tutorials/euslisp/make-joint-min-max-table.l#L89-L114

[garaemon]

手元では再現する?

[eisoku9618]

手元は古かったのでbuild中なのですが，travisはまた失敗して，今度は一歩前段階のcolladaの生成で失敗しているようです．

[eisoku9618]

手元では通りました