angle-vector-sequenceでのHiroの実機の動作補間について #626
Description
経由点付きの連続した動作を以下のコードのようにangle-vector-sequenceでHiroの実機に送りました。
(load "package://hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l")
(setq *robot* (hironxjsk-init))
(objects (list *robot*))
; open hand
(send *ri* :stop-grasp :rarm :time 1000)
(send *ri* :stop-grasp :larm :time 1000)
(ros::duration-sleep 1.5)
; define route
(setq route (list (make-coords :pos #f(600.0 -100.0 115.0) :rpy #f(0 0 0))
(make-coords :pos #f(500.0 -100.0 115.0) :rpy #f(0 0 0))
(make-coords :pos #f(300.0 0.0 115.0) :rpy #f(0 0 0))
(make-coords :pos #f(300.0 200.0 115.0) :rpy #f(0 0 0))
(make-coords :pos #f(300.0 400.0 115.0) :rpy #f(0 0 0))))
; move hand to the start position safely
(send *robot* :inverse-kinematics (make-coords :pos #f(600.0 -100.0 115.0) :rpy #f(0 0 0)) :move-target (send *robot* :larm :end-coords) :rotation-axis t :debug-view :no-message)
(send *ri* :angle-vector (send *robot* :angle-vector) 2000 nil 0 :min-time 0)
(send *ri* :wait-interpolation)
; move hand by the route (main problem)
(let ((avs nil))
(dolist (coords route)
(send *robot* :inverse-kinematics coords :move-target (send *robot* :larm :end-coords) :rotation-axis t :debug-view :no-message)
(push (send *robot* :angle-vector) avs))
(send *ri* :angle-vector-sequence (reverse avs) 1000 nil 0 :min-time 0)
(send *ri* :wait-interpolation))
その結果、下の動画のような上下運動が、補間された動きに入ってきました。
hiro_video.mp4
指定した経由点の高さ(ｚ座標)は全て同じなので、本来ならばこのような動きは出るはずではありません。
この上下運動を抑える方策を教えていただきたいです。