Merge pull request #325 from Affonso-Gui/end-coords-interpolation-branch

Fix :end-coords-interpolation problems
jsk-ros-pkg · Dec 24, 2017 · 8816ece · 8816ece
2 parents ba4beb0 + c9e342d
commit 8816ece
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 15 deletions.
diff --git a/pr2eus/pr2-interface.l b/pr2eus/pr2-interface.l
@@ -372,7 +372,7 @@ Example: (send self :gripper :rarm :position) => 0.00"
      ))
   (:angle-vector-sequence
    (avs &optional (tms (list 3000)) &rest args)
-   (unless (send self :simulation-modep)
+   (unless (or (send self :simulation-modep) (cadr (memq :end-coords-interpolation args)))
      (let* ((prev-av (send robot :angle-vector (send self :state :reference-vector)))
             (len-av (length prev-av))
             (max-av (fill (instantiate float-vector len-av)  180))

diff --git 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-steps 10))
    "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-steps : number of divisions when 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-steps end-coords-interpolation-steps)))
    (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-steps 10))
    "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-steps : number of divisions when interpolating end-coords
    "
    (setq ctype (or ctype controller-type))  ;; use default controller-type if ctype is nil
    (unless (gethash ctype controller-table)
@@ -439,12 +441,13 @@
        (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
+	     (diff-prev (instantiate float-vector (length av-prev))) diff ;; for over 360 deg turns
+             (limbs '(:larm :rarm :lleg :rleg)) ;; do not move :head and :torso by IK
              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
+             tm pass-tm i p (ret t)) ;; 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))
@@ -453,38 +456,43 @@
          (dolist (av avs)
            (send robot :angle-vector av)
            (setq end-coords-current (mapcar #'(lambda (limb) (send robot limb :end-coords :copy-worldcoords)) limbs))
+	   (setq diff (v- (v- av (setq av (v+ av-prev (send self :sub-angle-vector av av-prev)))) diff-prev))
            (setq target-limbs nil)
+	   (setq tm (elt tms i))
+	   (setq pass-tm (/ tm (float end-coords-interpolation-steps)))
            (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 minjerk :reset :position-list (list #f(0) #f(1)) :time-list (list tm))
            (send robot :angle-vector av-prev)
            (if target-limbs
                (progn
                  (send minjerk :start-interpolation)
-                 (while (send minjerk :interpolatingp)
-                   (send minjerk :pass-time 100)
+		 (send minjerk :pass-time pass-tm)
+                 (while (progn (send minjerk :pass-time pass-tm) (send minjerk :interpolatingp))
                    (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)
-                   ))
+                   (push (v++ diff-prev (scale p diff) (send robot :angle-vector)) interpolated-avs)
+                   (push pass-tm interpolated-tms)
+                   )
+               (push (v++ diff-prev diff av) interpolated-avs)
+               (push pass-tm interpolated-tms)
+	       )
              (progn
                (push av interpolated-avs)
-               (push 50 interpolated-tms)))
+               (push tm interpolated-tms)))
            (setq end-coords-prev end-coords-current)
            (setq av-prev av)
+	   (setq diff-prev diff)
            (incf i)) ;; dolist (av avs)
          ;; restore states
          (setq avs (nreverse interpolated-avs) tms (nreverse interpolated-tms))