From 8091a73e3433c4b47b9cbf4b1f10f4aca6cb451c Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 17 Nov 2017 18:30:08 +0900 Subject: [PATCH 1/3] Changes to :end-coords-interpolation --- pr2eus/robot-interface.l | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/pr2eus/robot-interface.l b/pr2eus/robot-interface.l index 6906c7e00..a50163a79 100644 --- a/pr2eus/robot-interface.l +++ b/pr2eus/robot-interface.l @@ -438,7 +438,7 @@ (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)) ;; 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 @@ -464,24 +464,25 @@ (if target-limbs (progn (send minjerk :start-interpolation) - (while (send minjerk :interpolatingp) - (send minjerk :pass-time 100) + (send minjerk :pass-time 100) + (while (progn (send minjerk :pass-time 100) (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 av interpolated-avs) + (push 100 interpolated-tms) + ) (progn (push av interpolated-avs) - (push 50 interpolated-tms))) + (push (elt tms i) interpolated-tms))) (setq end-coords-prev end-coords-current) (setq av-prev av) (incf i)) ;; dolist (av avs) From 0187d76304a1b846acfe4826c95b9bc4ac9126c8 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 17 Nov 2017 19:11:36 +0900 Subject: [PATCH 2/3] Add :steps to :end-coords-interpolation --- pr2eus/robot-interface.l | 24 ++++++++++++++---------- 1 file changed, 14 insertions(+), 10 deletions(-) diff --git a/pr2eus/robot-interface.l b/pr2eus/robot-interface.l index a50163a79..e27809f38 100644 --- a/pr2eus/robot-interface.l +++ b/pr2eus/robot-interface.l @@ -349,7 +349,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]) @@ -361,9 +361,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) @@ -404,7 +405,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) @@ -417,6 +418,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) @@ -443,7 +445,7 @@ (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,19 +455,21 @@ (send robot :angle-vector av) (setq end-coords-current (mapcar #'(lambda (limb) (send robot limb :end-coords :copy-worldcoords)) limbs)) (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) - (send minjerk :pass-time 100) - (while (progn (send minjerk :pass-time 100) (send minjerk :interpolatingp)) + (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)) @@ -475,14 +479,14 @@ (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 pass-tm interpolated-tms) ) (push av interpolated-avs) - (push 100 interpolated-tms) + (push pass-tm interpolated-tms) ) (progn (push av interpolated-avs) - (push (elt tms i) interpolated-tms))) + (push tm interpolated-tms))) (setq end-coords-prev end-coords-current) (setq av-prev av) (incf i)) ;; dolist (av avs) From 1937dfa6391ccb0bab42f7ebad4e2d3bc0cbd102 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 21 Nov 2017 12:23:37 +0900 Subject: [PATCH 3/3] Adapt end-coords-interpolation for over-360 deg turns --- pr2eus/pr2-interface.l | 2 +- pr2eus/robot-interface.l | 7 +++++-- 2 files changed, 6 insertions(+), 3 deletions(-) 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 e27809f38..4870884aa 100644 --- a/pr2eus/robot-interface.l +++ b/pr2eus/robot-interface.l @@ -440,6 +440,7 @@ (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 + (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)) @@ -454,6 +455,7 @@ (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))) @@ -478,10 +480,10 @@ 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 (v++ diff-prev (scale p diff) (send robot :angle-vector)) interpolated-avs) (push pass-tm interpolated-tms) ) - (push av interpolated-avs) + (push (v++ diff-prev diff av) interpolated-avs) (push pass-tm interpolated-tms) ) (progn @@ -489,6 +491,7 @@ (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))