add linear/minjerk-interpolator #391

k-okada · 2016-07-20T06:28:16Z

#389 の内容です

virtual_interpolator.l にも同じものがあって，そのまま持ってくればいいんだけど，あれやこれや直してしまった．本来はスべきではないのはわかっているが，angle-vector-listなどはやっぱ違うかなぁ，と．いまはposition/velocity/acceleratoinにしてある．．．．大丈夫か？
virtual_interpolator.l にはpos-list-interpolationなど便利関数っぽいものがあるけど，それも持って来たほうがいいか．
いいテストコード無いかな．

snozawa · 2016-07-20T08:16:56Z

virtual_interpolator.l にも同じものがあって，そのまま持ってくればいいんだけど，あれやこれや直してしまった．本来はスべきではないのはわかっているが，angle-vector-listなどはやっぱ違うかなぁ，と．いまはposition/velocity/acceleratoinにしてある．．．．大丈夫か？

これは、angle-vector-listというスロット変数名をposition/velocity/accelerationにしたというかんじでしょうか？
そのほうがわかりやすいので、さんせいです。

minjerkの補間方式は、:interpolation-sequential, :interpolation-absoluteの後者の方法になってるのでしょうか？
また後者の方法の場合、式がちがいそうなのですが、まったく同じ軌道になりますか？

virtual_interpolator.l にはpos-list-interpolationなど便利関数っぽいものがあるけど，それも持って来たほうがいいか．

これがない使い方ですと、exampleにコメントアウトしてかいてある４行を毎回かかなければいけないので、
ミスがあったり、こーどがふえたり、面倒だったりします。
面倒は我慢できるとして、ミスがあることが個人的には致命的だとおもうので、
何かしらの便利関数があると良いかなと思っています。

いいテストコード無いかな．

#389はオーバーシュートしてないくらいのテストになってますが、
virtual_interpolation.lの補間機は経由点を通る（近くを通るという制御点でなく、厳密に通る）
タイプの補間機になると思うので、引数であたえた経由点を
経由時刻に通るかどうかのテストがあると良いと思いました。

k-okada · 2016-07-20T10:45:13Z

2016-07-20 17:16 GMT+09:00 Shunichi Nozawa notifications@github.com:

minjerkの補間方式は、:interpolation-sequential,
:interpolation-absoluteの後者の方法になってるのでしょうか？
また後者の方法の場合、式がちがいそうなのですが、まったく同じ軌道になりますか？

これは微妙に違っていて，
fkanehiro/hrpsys-base@8c63984#diff-0138cf85be82802a0880f65c6ed7ca42
みると両方試した結果，結局後者が良いという判断をしたみたいんだんだけど，どうだろうか．

◉ Kei Okada

snozawa · 2016-07-20T10:55:05Z

はい、seuentialのほうは積分誤差があるとおもうので、
直接積分した式で実装したabsoluteのほうがいいんだと思います。

absoluteはabsoluteでも、もとのeuslib/rbrain/以下のコードとこのPRとでちがうのは、
hrpsys-baseに実装をそのまま合わせるといったかんじになっているからでしょうか。

snozawa · 2016-07-21T00:42:37Z

参考ですが、
https://github.com/jsk-ros-pkg/euslib/blob/master/demo/nozawa/motion/calc-minjerk.mac
に躍度最小補間の式を直接導出してtexでだしてくれるプログラムがあります。

k-okada · 2016-07-21T02:26:19Z

ちゃんとみていないですが，
euslibのものはa0-a5から直接x,y,zをだしている．
hrpsysのものはa0-a5からABCを計算してx,y,zを出しているのでおなじであｈないかというきがします．
いくつかの超簡単な例で試した所，一応同じ結果にはなっているようでした．

◉ Kei Okada

On Thu, Jul 21, 2016 at 9:42 AM, Shunichi Nozawa notifications@github.com
wrote:

参考ですが、

https://github.com/jsk-ros-pkg/euslib/blob/master/demo/nozawa/motion/calc-minjerk.mac
に躍度最小補間の式を直接導出してtexでだしてくれるプログラムがあります。

—
You are receiving this because you authored the thread.
Reply to this email directly, view it on GitHub
#391 (comment), or mute
the thread
https://github.com/notifications/unsubscribe-auth/AAeG3C4h71G_-cHAB4UH_S8BAX91if89ks5qXsB9gaJpZM4JQdsJ
.

snozawa · 2016-07-21T03:46:07Z

なるほど、ありがとうございます。
hrpsys-baseに合わせるのでしたら変数名も同じになってると対応がとりやすいと思いました。
（例えば、t1+t2がtarget-t）

ちなみに、これがirteusにコミットされてeuslibの(rbrain)をよんでいて
virtual_interpolator.lのものとバッティングした場合、どうなるんでしたっけ。
例えば、以前irteusにjointクラスを追加したときに、コンパイルされたもの同士で
クラスのメンバ変数がかわってるのがあると、若干問題があった気がします。

k-okada · 2016-07-21T05:14:19Z

2016-07-21 12:46 GMT+09:00 Shunichi Nozawa notifications@github.com:

例えば、以前irteusにjointクラスを追加したときに、コンパイルされたもの同士で
クラスのメンバ変数がかわってるのがあると、若干問題があった気がします。

お，たしかに，試してもらえると嬉しいです．

◉ Kei Okada

snozawa · 2016-07-21T11:20:42Z

ちなみに、robot-frame->robot-model, scene -> scene-modelのように
クラス名をバッティングさせないようにするのはいかがですか？

k-okada · 2016-07-22T01:33:31Z

うーん，無理やりな名前しかでてこないんですよね．どこかの時点で，rbrainのinterpolatorの親クラスをirteusの者に変えたらいいと思うので，そうしたら，
interpolatorクラスはrbrainからなくなって，minjerk-interpolator
も引数の名前をかえるようなメソッドをつくって互換性を保つようなものになるんだと思う．

◉ Kei Okada

On Thu, Jul 21, 2016 at 8:20 PM, Shunichi Nozawa notifications@github.com
wrote:

ちなみに、robot-frame->robot-model, scene -> scene-modelのように
クラス名をバッティングさせないようにするのはいかがですか？

—
You are receiving this because you authored the thread.
Reply to this email directly, view it on GitHub
#391 (comment), or mute
the thread
https://github.com/notifications/unsubscribe-auth/AAeG3JhwB6a5g_egfE-2kJi_KrMnDreRks5qX1YLgaJpZM4JQdsJ
.

snozawa · 2016-07-22T04:48:19Z

interpolatorクラスはrbrainからなくなって，minjerk-interpolator も引数の名前をかえるようなメソッドをつくって互換性を保つようなものになるんだと思う．

これは、

spline-interpolatorはそのままrbrainに残し
linearはirteusのものでおそらく事足りるのでrbrainから削除
minjerk-interpolatorはirteusにはないけどrbrainにはあるメソッドなどをrbrain側で追加

といったかんじになりますでしょうか。

k-okada · 2016-07-22T05:43:00Z

minjerk もirtuesに追加しているつもりです
https://github.com/euslisp/jskeus/pull/391/files#diff-c546f3a6798054fa253ce131ef3b2857R301

とりあえずjskeusにもrbrainも同じ名前のクラスを入れても大丈夫に見える．

k-okada@kokada-t440s:~/prog/euslib$ irteusgl
configuring by
"/home/k-okada/catkin_ws/ws_euslisp/devel/share/euslisp/jskeus/eus//lib/eusrt.l"
;; readmacro ;; object ;; packsym ;; common ;; constants ;; stream ;;
string ;; loader ;; pprint ;; process ;; hashtab ;; array ;; mathtran ;;
eusdebug ;; eusforeign ;; coordinates ;; tty ;; history ;; toplevel ;;
trans ;; comp ;; builtins ;; par ;; intersection ;; geoclasses ;; geopack
;; geobody ;; primt ;; compose ;; polygon ;; viewing ;; viewport ;;
viewsurface ;; hid ;; shadow ;; bodyrel ;; dda ;; helpsub ;; eushelp ;;
xforeign ;; Xdecl ;; Xgraphics ;; Xcolor ;; Xeus ;; Xevent ;; Xpanel ;;
Xitem ;; Xtext ;; Xmenu ;; Xscroll ;; Xcanvas ;; Xtop ;; Xapplwin
connected to Xserver DISPLAY=:0
X events are being asynchronously monitored.
;; pixword ;; RGBHLS ;; convolve ;; piximage ;; pbmfile ;;
image_correlation ;; oglforeign ;; gldecl ;; glconst ;; glforeign ;;
gluconst ;; gluforeign ;; glxconst ;; glxforeign ;; eglforeign ;; eglfunc
;; glutil ;; gltexture ;; glprim ;; gleus ;; glview ;; toiv-undefined ;;
fstringdouble irtmath irtutil irtc irtgeoc irtgraph pgsql irtgeo euspqp pqp
irtscene irtmodel irtdyna irtrobot irtsensor irtbvh irtcollada
irtpointcloud irtx eusjpeg euspng png irtimage irtglrgb
;; extending gcstack 0x4d8f1d0[16374] --> 0x51f43f0[32748] top=3cbf
irtgl irtglc irtviewer
EusLisp 9.19(d69cf76 1.0.12) for Linux64 created on kokada-t440s(Fri Jul 8
10:11:58 JST 2016)
1.irteusgl$ (setq l (instance minjerk-interpolator :init))
(send l :reset :position-list (list #f(1 2 3) #f(3 4 5) #f(1 2 3))
:time-list (list 0.1 0.18))
(send l :start-interpolation)
(while (send l :interpolatingp) (send l :pass-time 0.02) (print (send l
:position)))
#<minjerk-interpolator #X504bfa8>
2.irteusgl$ (#f(0.0 0.0 0.0) #f(0.0 0.0 0.0) #f(0.0 0.0 0.0))
3.irteusgl$ t
4.irteusgl$ #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(2.79297 3.79297 4.79297)
#f(2.0 3.0 4.0)
#f(1.20703 2.20703 3.20703)
#f(1.0 2.0 3.0)
nil
5.irteusgl$ describe l
plist=nil
position-list=(#f(1.0 2.0 3.0)
               #f(3.0 4.0 5.0)
               #f(1.0 2.0 3.0))
time-list=(0.1 0.18)
position=#f(1.0 2.0 3.0)
time=0.0
segmnet-num=nil
segment-time=0.0
segment=0
interpolatingp=nil
velocity=#f(-25.0 -25.0 -25.0)
acceleration=#f(-312.5 -312.5 -312.5)
velocity-list=(#f(0.0 0.0 0.0)
               #f(0.0 0.0 0.0)
               #f(0.0 0.0 0.0))
acceleration-list=(#f(0.0 0.0 0.0)
                   #f(0.0 0.0 0.0)
                   #f(0.0 0.0 0.0))
nil
6.irteusgl$ jsk


;; load-ext-lib: jskeus (Fri Jul 22 14:18:09 2016)
ckdtree not loaded
fftlib not loaded
svm not loaded
jsk matlib graph kdtree galib neurolib eusrapid rapid kalmanlib lmeds icp
optimization octave preview queue searchlib dplib backprop bayesian qlearn
soundlib sendmail linearproblem pfilter euscorba galatea jskc win32api
cvoxel jskgeoc regexp festival listener talk unittest eusjasper jasper
;; load-ext-lib: jskeusunix (Fri Jul 22 14:18:10 2016)
jskunix sericom uptime
;; load-ext-lib: jskeusgeo (Fri Jul 22 14:18:28 2016)
jskgeo pickport kdraw primtpatch primtapp vclip pfitting sample_poisson
;; load-ext-lib: jskeusimage (Fri Jul 22 14:18:12 2016)
jpeg_compress_struct jpeg_decompress_struct jskimage
;; load-ext-lib: jskeusx (Fri Jul 22 14:18:42 2016)
jskx pickview winspect browse gnuplotlib graphwin
;; load-ext-lib: jskeusgl (Fri Jul 22 14:18:51 2016)
jskgl rnurbs voxel jskglc nil
7.irteusgl$ brain

;; load-ext-lib: rbraineus (Fri Jul 22 14:25:43 2016)
graph not loaded
inertia basicshape basicsensor iklib pathlib scene motionplan basicmodel
state action actionunit
;; extending gcstack 0x51f43f0[32738] --> 0x66d5c90[65476] top=7e43
animation autobalancer servoq tendon wire basicape bipedrobot basicqape
basicdrawon basicio benet collision conversion compensation dynamics
extractcoords filter_classes ikview ladder passiveJmactuators
robotJmpartsmaker motioneditor robots silhouette sensormodel stable statedb
task tree universal utils convertJmtoJmirtmodel
;; load-ext-lib: rbrainio (Fri Jul 22 14:25:57 2016)
obj2eus wrl2eus eusdads dadsudf eushrp eusdxf eusmqo eusiv eusbvh eusmatlab
;; load-ext-lib: rbrvirtual (Fri Jul 22 14:25:42 2016)
virtual_window virtual_vision_unit virtual_interpolator
virtual_control_unit virtual_bbi virtual_objects virtual_world
virtual_environment
;; load-ext-lib: rbraineusgeo (Fri Jul 22 14:25:44 2016)
rbrgeo
;; load-ext-lib: rbraineusx (Fri Jul 22 14:25:53 2016)
mewindow jointcontroller statedbwin trwin threadview pvext ikviewx benetwin
statenetwin
;; load-ext-lib: rbraineusgl (Fri Jul 22 14:25:58 2016)
rbrgl nil
8.irteusgl$ (setq l (instance minjerk-interpolator :init))
(send l :reset :angle-vector-list (list #f(1 2 3) #f(3 4 5) #f(1 2 3))
:step-list (list 5 4 :dummy))
(send l :start-interpolation)
(while (send l :interpolatingp) (send l :pass-time 0.02) (print (send l
:angle-vector)))
#<minjerk-interpolator #X6ba28b8>
9.irteusgl$ #f(0.0 0.0 0.0)
10.irteusgl$ t
11.irteusgl$ #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(2.79297 3.79297 4.79297)
#f(2.0 3.0 4.0)
#f(1.20703 2.20703 3.20703)
#f(1.0 2.0 3.0)
nil
12.irteusgl$ describe l
plist=nil
angle-vector-list=(#f(1.0 2.0 3.0)
                   #f(3.0 4.0 5.0)
                   #f(1.0 2.0 3.0))
step-list=(5 4 :dummy)
angle-vector=#f(1.0 2.0 3.0)
sec/step=0.02
time-list=(0.1 0.18)
time=0.0
segment-time=0.0
segment=0
segment-num=2
interpolatingp=nil
pos=#f(1.0 2.0 3.0)
vel=#f(7.958079e-13 7.958079e-13 1.023182e-12)
acc=#f(5.093170e-11 5.093170e-11 5.820766e-11)
vel-vector-list=(#f(0.0 0.0 0.0)
                 #f(0.0 0.0 0.0)
                 #f(0.0 0.0 0.0))
acc-vector-list=(#f(0.0 0.0 0.0)
                 #f(0.0 0.0 0.0)
                 #f(0.0 0.0 0.0))
mode=:absolute
nil

◉ Kei Okada

2016-07-22 13:48 GMT+09:00 Shunichi Nozawa notifications@github.com:

interpolatorクラスはrbrainからなくなって，minjerk-interpolator
も引数の名前をかえるようなメソッドをつくって互換性を保つようなものになるんだと思う．

これは、

spline-interpolatorはそのままrbrainに残し

linearはirteusのものでおそらく事足りるのでrbrainから削除

minjerk-interpolatorはirteusにはないけどrbrainにはあるメソッドなどをrbrain側で追加

といったかんじになりますでしょうか。

—
You are receiving this because you authored the thread.
Reply to this email directly, view it on GitHub
#391 (comment), or mute
the thread
https://github.com/notifications/unsubscribe-auth/AAeG3AK1EbSk2KrxIh7guyQ-jcr73z7Bks5qYEuTgaJpZM4JQdsJ
.

snozawa · 2016-07-22T06:26:03Z

minjerk もirtuesに追加しているつもりです

すいません、

minjerk-interpolatorはirteusにはないけどrbrainにはあるメソッドなどをrbrain側で追加

は、minjerk-interpolatorはirteusに追加はされているけど、
元々rbrainにあったものよりいくつかメソッドを洗練させてけずっているので、
minjekr-interpolatorのメソッドのうちrbrainには元々ありirteusには現状追加されてないものは、
rbrain側に残しておく、という意味でした。

irteusに追加した状態で(jsk)``(rbrain)まで動いているのであれば、このPRでOKそうです。

k-okada · 2016-07-27T11:21:40Z

doc, test code 追加しました．

snozawa · 2016-07-29T01:10:09Z

LGTM.
テストは、経由点のチェックがはいってるんですね。

* add linear/minjerk-interpolator * add irteus/test/interpolator.l * add doc for interpolators

add linear/minjerk-interpolator

fa4a1d5

k-okada mentioned this pull request Jul 20, 2016

interpolator #389

Closed

k-okada mentioned this pull request Jul 20, 2016

[wip] add end-coords-interpolation jsk-ros-pkg/jsk_pr2eus#244

Closed

k-okada added 2 commits July 27, 2016 20:06

add irteus/test/interpolator.l

53336f3

add doc for interpolators

1a7f416

k-okada merged commit 1ec6012 into euslisp:master Aug 2, 2016

k-okada deleted the add_minjerk branch August 2, 2016 06:42

mmurooka mentioned this pull request Oct 16, 2016

(send *ri* :angle-vector)した後のGOALまでの中間姿勢を知りたい jsk-ros-pkg/jsk_robot#686

Closed

iory pushed a commit to iory/jskeus that referenced this pull request Mar 22, 2018

add linear/minjerk-interpolator (euslisp#391)

729688a

* add linear/minjerk-interpolator * add irteus/test/interpolator.l * add doc for interpolators

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

add linear/minjerk-interpolator #391

add linear/minjerk-interpolator #391

k-okada commented Jul 20, 2016

snozawa commented Jul 20, 2016

k-okada commented Jul 20, 2016

snozawa commented Jul 20, 2016

snozawa commented Jul 21, 2016

k-okada commented Jul 21, 2016

snozawa commented Jul 21, 2016

k-okada commented Jul 21, 2016

snozawa commented Jul 21, 2016

k-okada commented Jul 22, 2016

snozawa commented Jul 22, 2016

k-okada commented Jul 22, 2016

snozawa commented Jul 22, 2016

k-okada commented Jul 27, 2016

snozawa commented Jul 29, 2016

add linear/minjerk-interpolator #391

add linear/minjerk-interpolator #391

Conversation

k-okada commented Jul 20, 2016

snozawa commented Jul 20, 2016

k-okada commented Jul 20, 2016

snozawa commented Jul 20, 2016

snozawa commented Jul 21, 2016

k-okada commented Jul 21, 2016

snozawa commented Jul 21, 2016

k-okada commented Jul 21, 2016

snozawa commented Jul 21, 2016

k-okada commented Jul 22, 2016

snozawa commented Jul 22, 2016

k-okada commented Jul 22, 2016

snozawa commented Jul 22, 2016

k-okada commented Jul 27, 2016

snozawa commented Jul 29, 2016