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 1 commit
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
7 changes: 5 additions & 2 deletions pr2eus/robot-interface.l
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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))
Expand All @@ -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))
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)))
Expand All @@ -478,17 +480,18 @@
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
(push av interpolated-avs)
(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