diff --git a/pr2eus/robot-interface.l b/pr2eus/robot-interface.l index 1f0844b7b..c8dd07c4c 100644 --- a/pr2eus/robot-interface.l +++ b/pr2eus/robot-interface.l @@ -350,7 +350,7 @@ (let* ((prev-av (send robot :angle-vector))) (send-all (gethash ctype controller-table) :push-angle-vector-simulation av tm prev-av))) (:angle-vector - (av &optional (tm nil) (ctype controller-type) (start-time 0) &key (scale 1) (min-time 1.0) (end-coords-interpolation nil)) + (av &optional (tm nil) (ctype controller-type) (start-time 0) &key (scale 1) (min-time 1.0) (end-coords-interpolation nil) (end-coords-interpolation-pass-time 100)) "Send joind angle to robot, this method retuns immediately, so use :wait-interpolation to block until the motion stops. - av : joint angle vector [deg] - tm : (time to goal in [msec]) @@ -362,9 +362,10 @@ - scale : if tm is not specified, it will use 1/scale of the fastest speed - min-time : minimum time for time to goal - end-coords-interpolation : set t if you want to move robot in cartesian space interpolation +- end-coords-interpolation-pass-time : time interval for interpolating end-coords " (if end-coords-interpolation - (return-from :angle-vector (send self :angle-vector-sequence (list av) (list tm) ctype start-time :scale scale :min-time min-time :end-coords-interpolation t))) + (return-from :angle-vector (send self :angle-vector-sequence (list av) (list tm) ctype start-time :scale scale :min-time min-time :end-coords-interpolation t :end-coords-interpolation-pass-time end-coords-interpolation-pass-time))) (setq ctype (or ctype controller-type)) ;; use default controller-type if ctype is nil (unless (gethash ctype controller-table) (warn ";; controller-type: ~A not found" ctype) @@ -405,7 +406,7 @@ cacts (send self ctype))) av) (:angle-vector-sequence - (avs &optional (tms (list 3000)) (ctype controller-type) (start-time 0.1) &key (scale 1) (min-time 0.0) (end-coords-interpolation nil)) + (avs &optional (tms (list 3000)) (ctype controller-type) (start-time 0.1) &key (scale 1) (min-time 0.0) (end-coords-interpolation nil) (end-coords-interpolation-pass-time 100)) "Send sequence of joind angle to robot, this method retuns immediately, so use :wait-interpolation to block until the motion stops. - avs: sequence of joint angles(float-vector) [deg], (list av0 av1 ... avn) - tms: sequence of duration(float) from previous angle-vector to next goal [msec], (list tm0 tm1 ... tmn) @@ -418,6 +419,7 @@ - scale : if tms is not specified, it will use 1/scale of the fastest speed - min-time : minimum time for time to goal - end-coords-interpolation : set t if you want to move robot in cartesian space interpolation +- end-coords-interpolation-pass-time : time interval for interpolating end-coords " (setq ctype (or ctype controller-type)) ;; use default controller-type if ctype is nil (unless (gethash ctype controller-table) @@ -439,62 +441,93 @@ (let ((av-orig (send robot :angle-vector)) ;; initial av, restore at end (c-orig (send robot :copy-worldcoords)) ;; inital coords, restore at end (av-prev-orig av-prev) ;; prev-av - (limbs '(:larm :rarm :lleg :rleg :torso :head)) ;; defined in https://github.com/euslisp/jskeus/blob/master/irteus/irtrobot.l#L79 + (limbs '(:larm :rarm :lleg :rleg)) ;; defined in https://github.com/euslisp/jskeus/blob/master/irteus/irtrobot.l#L79, head and torso is ignored becuase ik usually fails when the head moves. target-limbs (minjerk (instance minjerk-interpolator :init)) end-coords-prev end-coords-current ec-prev ec-current interpolated-avs interpolated-tms - tm i p (ret t)) ;; if nil failed to interpolate + min-interpolated-avs min-interpolated-tms + tm i p (ret t) min-diff-ik) ;; if nil failed to interpolate ;; set prev-av - (send robot :angle-vector av-prev) - (setq end-coords-prev (mapcar #'(lambda (limb) (send robot limb :end-coords :copy-worldcoords)) limbs)) - ;; choose moved end-coords - (setq i 0) - (dolist (av avs) - (send robot :angle-vector av) - (setq end-coords-current (mapcar #'(lambda (limb) (send robot limb :end-coords :copy-worldcoords)) limbs)) - (setq target-limbs nil) - (dotimes (l (length limbs)) - (setq ec-prev (elt end-coords-prev l) ec-current (elt end-coords-current l)) - (when (and ec-prev ec-current - (or (> (norm (send ec-prev :difference-position ec-current)) 1) - (> (norm (send ec-prev :difference-rotation ec-current)) (deg2rad 1)))) - (push (elt limbs l) target-limbs))) - (send minjerk :reset :position-list (list #f(0) #f(1)) :time-list (list (elt tms i))) - (send robot :angle-vector av-prev) - (if target-limbs + ;; choose best initial pose of IK + (dolist (ik-method '(nil :midpoint :av-prev :car-avs)) + ;; reset status + (send robot :move-to c-orig :world) + (send robot :angle-vector av-orig) + (setq av-prev av-prev-orig) + (setq interpolated-avs nil interpolated-tms nil) + ;; choose moved end-coords + (setq end-coords-prev (mapcar #'(lambda (limb) (send robot limb :end-coords :copy-worldcoords)) limbs)) + (setq i 0) + (dolist (av avs) + (send robot :angle-vector av) + (setq end-coords-current (mapcar #'(lambda (limb) (send robot limb :end-coords :copy-worldcoords)) limbs)) + (setq target-limbs nil) + (print (list 'method ik-method 'limbs limbs)) + (dotimes (l (length limbs)) + (setq ec-prev (elt end-coords-prev l) ec-current (elt end-coords-current l)) + (print ec-prev) + (when (and ec-prev ec-current + (or (> (norm (send ec-prev :difference-position ec-current)) 1) + (> (norm (send ec-prev :difference-rotation ec-current)) (deg2rad 1)))) + (push (elt limbs l) target-limbs))) + (send minjerk :reset :position-list (list #f(0) #f(1)) :time-list (list (elt tms i))) + (send robot :angle-vector av-prev) + (if target-limbs + (progn + (send minjerk :start-interpolation) + (while (send minjerk :interpolatingp) + (send minjerk :pass-time end-coords-interpolation-pass-time) + (setq p (elt (send minjerk :position) 0)) + ;; set midpoint of av as initial pose of IK + (cond ((null ik-method)) + ((eq ik-method :midpoint) + (send robot :angle-vector (midpoint p av-prev av))) + ((eq ik-method :av-prev) + (send robot :angle-vector av-prev)) + ((eq ik-method :car-avs) + (if (car interpolated-avs) + (send robot :angle-vector (car interpolated-avs)))) + ) + (dolist (limb target-limbs) + ;; don't move arm by IK when interpolation is already ended + (if (>= p 1) (return)) + (setq ec-prev (elt end-coords-prev (position limb limbs)) + ec-current (elt end-coords-current (position limb limbs))) + (setq ret (and ret + (send robot limb :inverse-kinematics (midcoords p ec-prev ec-current))))) + (push (send robot :angle-vector) interpolated-avs) + (push end-coords-interpolation-pass-time interpolated-tms) + )) (progn - (send minjerk :start-interpolation) - (while (send minjerk :interpolatingp) - (send minjerk :pass-time 100) - (setq p (elt (send minjerk :position) 0)) - ;; set midpoint of av as initial pose of IK - (send robot :angle-vector (midpoint p av-prev av)) - (dolist (limb target-limbs) - ;; don't move arm by IK when interpolation is already ended - (if (>= p 1) (return)) - (setq ec-prev (elt end-coords-prev (position limb limbs)) - ec-current (elt end-coords-current (position limb limbs))) - (setq ret (and ret - (send robot limb :inverse-kinematics (midcoords p ec-prev ec-current))))) - (push (send robot :angle-vector) interpolated-avs) - (push 100 interpolated-tms) - )) - (progn - (push av interpolated-avs) - (push 50 interpolated-tms))) - (setq end-coords-prev end-coords-current) - (setq av-prev av) - (incf i)) ;; dolist (av avs) + (push av interpolated-avs) + (push end-coords-interpolation-pass-time interpolated-tms))) + (setq end-coords-prev end-coords-current) + (setq av-prev av) + (incf i)) ;; dolist (av avs) + ;; evaluate avs + (let* ((prev-av (car interpolated-avs)) + (diff-av (instantiate float-vector (length prev-av))) + diff-ik) + (dolist (av (cdr interpolated-avs)) + (setq diff-av (v+ diff-av (v-abs av prev-av)))) + (setq diff-ik (geo::find-extream (coerce diff-av cons) #'identity #'>=)) + (when (or (null min-diff-ik) + (< diff-ik min-diff-ik)) + (setq min-diff-ik diff-ik) + (setq min-interpolated-avs interpolated-avs) + (setq min-interpolated-tms interpolated-tms)) + )) ;; choose bet initial pose of IK + (ros::ros-warn "choose ~A diff interpolation sequence" min-diff-ik) ;; restore states - (setq avs (nreverse interpolated-avs) tms (nreverse interpolated-tms)) + (setq avs (nreverse min-interpolated-avs) tms (nreverse min-interpolated-tms)) (send robot :move-to c-orig :world) (send robot :angle-vector av-orig) (setq av-prev av-prev-orig) (unless ret (warning-message 1 ":anlge-vector-sequnce fail to generate end-coords-interpolation motion~%") (return-from :angle-vector-sequence nil)) - )) + )) ;; when end-coords-interpolation (prog1 ;; angle-vector-sequence returns avs avs (while avs diff --git a/pr2eus/test/pr2-ri-test-simple.l b/pr2eus/test/pr2-ri-test-simple.l index 6e156f20c..d2a9743a1 100644 --- a/pr2eus/test/pr2-ri-test-simple.l +++ b/pr2eus/test/pr2-ri-test-simple.l @@ -155,6 +155,36 @@ ) )) +;; test to check unintentional 180 rotation during end-coords interpolation +;; c.f. https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/325 +(deftest test-angle-vector-sequence-end-coords-interpolation + (let ((vec #f(50.0 47.7222 12.2897 117.176 -106.014 -77.3995 -76.8709 -105.904 -47.7222 12.2897 -117.176 -106.014 77.3995 -76.8709 105.904 0.0 0.0)) + (v0 #f(50.0 53.1894 14.6871 123.249 -121.524 -74.0435 -76.2839 266.776 -53.1894 14.6871 -123.249 -121.524 74.0435 -76.2839 93.2238 0.0 0.0)) + (i 0) avs av-pre) + (setq avs (send *ri* :angle-vector-sequence (list vec (send *pr2* :reset-pose)) (list 1000 1000) :default-controller 0 :end-coords-interpolation t)) + (setq av-pre (car avs)) + (dolist (av (cdr avs)) + (format t "~A/~A diff ~A, max ~A~%" (incf i) (length avs) (v- av av-pre) (abs (geo::find-extream (coerce (v- av av-pre) cons) #'abs #'>=))) + (assert (< (abs (geo::find-extream (coerce (v- av av-pre) cons) #'abs #'>=)) 180) + (format nil "found unintentional +180 joint rotation ~A" (v- av av-pre))) + (setq av-pre av)) + ;; + )) + +;; test to check unintentional 180 rotation during end-coords interpolation +;; c.f. https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/325 +(deftest test-angle-vector-sequence-end-coords-interpolation-head + (let ((v1 #f(50.0 -16.4077 18.0876 93.6936 -54.8829 77.4245 -30.5008 123.25 -50.4711 19.1116 -59.6967 -13.4894 275.94 -109.525 -35.7734 -47.3098 49.3376)) + (v2 #f(191.303 50.0682 17.9701 102.195 -10.7369 43.6391 -103.396 221.198 32.3493 45.9097 -15.2106 -55.9981 227.517 -63.9359 38.9722 0.0 0.0)) + avs) + (send *ri* :angle-vector (send *pr2* :reset-pose)) + (send *ri* :angle-vector v1) + (send *ri* :wait-interpolation) + (setq avs (send *ri* :angle-vector v2 2000 :default-controller 0 :end-coords-interpolation t)) + (format t "abs = ~A~%" avs) + (assert avs) + )) + (run-all-tests) (exit)