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.

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

[irteus/irtutil.l] Enable to pass integer time-list to interpolator #625

Merged
merged 3 commits into from
Oct 18, 2022

Conversation

pazeshun
Copy link
Contributor

@pazeshun pazeshun commented Oct 14, 2022

Currently, interpolator does not work with integer time-list:

EusLisp 9.29(7b154531 1.2.5) for Linux64 created on pazeshun-ThinkPad-T460s-1804(Fri Oct 14 20:33:55 JST 2022)
1.irteusgl$ (progn (setq l (instance minjerk-interpolator :init)) (send l :reset :position-list (list #f(1 2 3) #f(3 4 5) #f(5 6 7)) :time-list (list 1000 1800)) (send l :start-interpolation) (while (send l :interpolatingp) (send l :pass-time 200) (print (send l :position))))
#f(5.0 6.0 7.0)
nil
2.irteusgl$ (progn (setq l (instance minjerk-interpolator :init)) (send l :reset :position-list (list #f(1 2 3) #f(3 4 5) #f(5 6 7)) :time-list (list 1000.0 1800)) (send l :start-interpolation) (while (send l :interpolatingp) (send l :pass-time 200) (print (send l :position))))
#f(1.0 2.0 3.0)
#f(1.11584 2.11584 3.11584)
#f(1.63488 2.63488 3.63488)
#f(2.36512 3.36512 4.36512)
#f(2.88416 3.88416 4.88416)
#f(5.0 6.0 7.0)
nil

This PR fixes this issue by forcibly converting time-list to float.
Result:

EusLisp 9.29(7b154531 1.2.5) for Linux64 created on pazeshun-ThinkPad-T460s-1804(Fri Oct 14 20:33:55 JST 2022)
1.irteusgl$ (progn (setq l (instance minjerk-interpolator :init)) (send l :reset :position-list (list #f(1 2 3) #f(3 4 5) #f(5 6 7)) :time-list (list 1000 1800)) (send l :start-interpolation) (while (send l :interpolatingp) (send l :pass-time 200) (print (send l :position))))
#f(1.0 2.0 3.0)
#f(1.11584 2.11584 3.11584)
#f(1.63488 2.63488 3.63488)
#f(2.36512 3.36512 4.36512)
#f(2.88416 3.88416 4.88416)
#f(3.0 4.0 5.0)
#f(3.20703 4.20703 5.20703)
#f(4.0 5.0 6.0)
#f(4.79297 5.79297 6.79297)
#f(5.0 6.0 7.0)
nil

Details

When irteus/irtutil.l and lisp/geo/geopack.l are loaded explicitly, that issue disappears:

3.irteusgl$ (load "irteus/irtutil.l")
t
4.irteusgl$ (progn (setq l (instance minjerk-interpolator :init)) (send l :reset :position-list (list #f(1 2 3) #f(3 4 5) #f(5 6 7)) :time-list (list 1000 1800)) (send l :start-interpolation) (while (send l :interpolatingp) (send l :pass-time 200) (print (send l :position))))
#f(5.0 6.0 7.0)
nil
5.irteusgl$ (load "lisp/geo/geopack.l")
t
6.irteusgl$ (progn (setq l (instance minjerk-interpolator :init)) (send l :reset :position-list (list #f(1 2 3) #f(3 4 5) #f(5 6 7)) :time-list (list 1000 1800)) (send l :start-interpolation) (while (send l :interpolatingp) (send l :pass-time 200) (print (send l :position))))
#f(1.0 2.0 3.0)
#f(1.11584 2.11584 3.11584)
#f(1.63488 2.63488 3.63488)
#f(2.36512 3.36512 4.36512)
#f(2.88416 3.88416 4.88416)
#f(3.0 4.0 5.0)
#f(3.20703 4.20703 5.20703)
#f(4.0 5.0 6.0)
#f(4.79297 5.79297 6.79297)
#f(5.0 6.0 7.0)
nil

That issue was cause by #596 and euslisp/EusLisp#406.
#596 introduced eps> to interpolator:

(when (eps> time (nth segment time-list) (* 0.1 dt))

(while (and (< segment segment-num) (eps> time (nth segment time-list) (* 0.1 dt)))

But compiled eps> does not work when one item is float and the other is integer as euslisp/EusLisp#406.
So if (nth segment time-list) is integer, it cannot be compared with time (float).

This issue is fatal in jsk-ros-pkg/jsk_pr2eus#486.
We usually pass integer time to :angle-vector, but this cannot be correctly managed in current interpolator:
https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/4700cf84db91d3b8d6cb17a540c29add5fbfd656/pr2eus/robot-interface.l#L516

@pazeshun pazeshun closed this Oct 17, 2022
@pazeshun pazeshun reopened this Oct 17, 2022
@pazeshun
Copy link
Contributor Author

CI successfully failed on #626 and passed on this PR.
@k-okada Could you review this?

@k-okada k-okada merged commit caa563b into euslisp:master Oct 18, 2022
@pazeshun
Copy link
Contributor Author

Thank you!

@pazeshun pazeshun deleted the interpolator-integer branch October 18, 2022 05:02
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

3 participants