diff --git a/pr2eus/pr2-interface.l b/pr2eus/pr2-interface.l index 9a8cd2b92..a2b5494dc 100644 --- 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 index e4ffff6d3..2e8723f97 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-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))