Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix :end-coords-interpolation problems #325

Merged
merged 5 commits into from
Dec 24, 2017
Merged

Fix :end-coords-interpolation problems #325

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
8091a73
Changes to :end-coords-interpolation
Affonso-Gui Nov 17, 2017
0187d76
Add :steps to :end-coords-interpolation
Affonso-Gui Nov 17, 2017
1937dfa
Adapt end-coords-interpolation for over-360 deg turns
Affonso-Gui Nov 21, 2017
a0905ec
Merge branch 'master' into end-coords-interpolation-branch
Affonso-Gui Nov 21, 2017
c9e342d
Merge branch 'master' into end-coords-interpolation-branch
Affonso-Gui Dec 13, 2017
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion pr2eus/pr2-interface.l
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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))
Expand Down
36 changes: 22 additions & 14 deletions pr2eus/robot-interface.l
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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])
Expand All @@ -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)
Expand Down Expand Up @@ -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)
Expand All @@ -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)
Expand All @@ -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))
Expand All @@ -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))
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@Affonso-Gui I have added your code to #326, but not sure about the logic of this code.

I thought...
setq av.. update av from avs to av-prev + sub-angle-vector, so if there is no 180 rotation this new av is av. Let set this new av as new-av.
Then, we calculate av - new-av - diff-prev, If new-av is equal to av, then diff is - diff-prev, Where did I go wrong?

Copy link
Member Author

@Affonso-Gui Affonso-Gui Dec 21, 2017 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@k-okada It's exactly as you said, when there is no 180 turns diff is set to - diff-prev, meaning to undo last rotation.

For example, in an av sequence from

0 → 1000 → 0

the new-av sequence becomes:

0 → - 80 → 0

Which is then used to set midpoint for IK.
In this case we have an over 180 deg rotation at first interpolation (0 → 1000) and none at the second (- 80 → 0), making the diff become:

  +1080 , -1080

Which is then slowly added to prev-av and to the IK result (Line 484) in order to generate something like:

0 → 200 → 500 → 800 → 1000 → 800 → 500 → 200 → 0

(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))
Expand Down