Kinovaでベクトルを変えようとしたときにdimensionがあっていないと言われる

[a-ichikura]

README.mdにしたがって

"load://package/kinova-interface.l"
 kinova-init :type :gen3_lite_gen3_lite_2f
 (send *kinova* :angle-vector #f(0.0 15.0 180.0 -130.0 0.0 55.0 90.0))

までは問題なくできますが、
(send *ri* :angle-vector (send *kinova* :angle-vector) 2000)
しようとすると

Call Stack (max depth: 20):
  0: at (v- av prev-av)
  1: at (let* ((prev-av (send self :state :potentio-vector :wait-until-update t)) (diff-av (v- av prev-av))) (when (send self :check-continuous-joint-move-over-180 diff-av) (let* (avs (minjerk (instance minjerk-interpolator :init)) (scale-av (send self :sub-angle-vector av prev-av)) dist div) (setq dist (abs (find-extream (coerce diff-av cons) #'abs #'>=))) (setq div (round (/ dist 120.0))) (send minjerk :reset :position-list (list prev-av (v+ prev-av scale-av)) :time-list (list tm)) (send minjerk :start-interpolation) (send minjerk :pass-time (/ tm div)) (dotimes (i div) (setq avs (append avs (list (send minjerk :pass-time (/ tm div)))))) (send* self :angle-vector-sequence-raw avs (make-list div :initial-element (/ tm div)) ctype start-time args) (return-from :angle-vector-raw (car (last avs))))) (send-super* :angle-vector av tm ctype start-time :minjerk-interpolation t args))
  2: at (apply #'send self :angle-vector-raw av tm ctype start-time args)
  3: at (apply #'send self :angle-vector-raw av tm ctype start-time args)
  4: at (send* self :angle-vector-raw av tm ctype start-time args)
  5: at (return-from :angle-vector (send* self :angle-vector-raw av tm ctype start-time args))
  6: at (progn (return-from :angle-vector (send* self :angle-vector-raw av tm ctype start-time args)))
  7: at (if (and (not (boundp '*co*)) (send self :simulation-modep)) (progn (return-from :angle-vector (send* self :angle-vector-raw av tm ctype start-time args))))
  8: at (when (and (not (boundp '*co*)) (send self :simulation-modep)) (return-from :angle-vector (send* self :angle-vector-raw av tm ctype start-time args)))
  9: at (let ((ctype controller-type) (start-time 0) (start-offset-time nil) (clear-velocities t)) (if (= (length args) 1) (setq ctype (car args) args (cdr args))) (if (and (>= (length args) 2) (null (member (car args) '(:start-time :clear-velocities)))) (setq ctype (car args) start-time (cadr args) args (cddr args))) (if (member :start-time args) (setq start-time (cadr (member :start-time args)))) (if (member :start-offset-time args) (setq start-offset-time (cadr (member :start-offset-time args)))) (if (member :clear-velocities args) (setq clear-velocities (cadr (member :clear-velocities args)))) (when (and (not (boundp '*co*)) (send self :simulation-modep)) (return-from :angle-vector (send* self :angle-vector-raw av tm ctype start-time args))) (when (not (numberp tm)) (warn ":angle-vector tm is not a number, use :angle-vector av tm args")) (send* self :angle-vector-motion-plan av :ctype ctype :move-arm :rarm :total-time tm :start-offset-time (if start-offset-time start-offset-time start-time) :clear-velocities clear-velocities :minjerk-interpolation nil args))
  10: at (send *ri* :angle-vector (send *kinova* :angle-vector) 2000)
  11: at #<compiled-code #X55d1eba7ce18>
/opt/ros/melodic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: vector dimension mismatch in (v- av prev-av)

というエラーが出ました。
また、最初の状態(initしかしていない状態)でpotentio-vectorを見てみたところ、

11.E3-irteusgl$ send *ri* :state :potentio-vector
#f(0.0 -16.0 75.0 0.0 -60.0 0.0)

になっていて、6次元のvectorであるように見えます。*ri*と*kinova*の次元が異なっているように思ったのですが、どうしたら良いのでしょうか。

[tkmtnt7000]

また、最初の状態(initしかしていない状態)でpotentio-vectorを見てみたところ、

11.E3-irteusgl$ send *ri* :state :potentio-vector
#f(0.0 -16.0 75.0 0.0 -60.0 0.0)

になっていて、6次元のvectorであるように見えます。

gen3_lite_gen3_lite_2fに対応するGen3 Lite Robotは6自由度アームなので6次元のvectorなのは問題ありません．gen3_lite_gen3_lite_2fの場合には(send *kinova* :angle-vector #f(0 0 0 0 0 90))のように6次元で関節角度を与える必要があります．

Readmeで7次元の関節角度を与えるようになっているのは，LiteのつかないKINOVA Gen3 アームが7自由度なのでそちらに合わせて記述されたからだと思われます．6自由度アームの場合と7自由度アームの場合の違いが書かれていないのはわかりにくいので，

;; gen3_robotiq_2f_140 and gen3_robotiq_2f_85
 (send *kinova* :angle-vector #f(0.0 15.0 180.0 -130.0 0.0 55.0 90.0))
;; gen3_lite_gen3_lite_2f
 (send *kinova* :angle-vector #f(0.0 15.0 180.0 -130.0 0.0 90.0))

みたいな感じで該当部分のReadmeを書き換えたほうがいいかもしれません．(いい感じの記述と関節角度にしてPR出していただけると助かります．)

[tkmtnt7000]

README.mdにしたがって

"load://package/kinova-interface.l"
kinova-init :type :gen3_lite_gen3_lite_2f
(send kinova :angle-vector #f(0.0 15.0 180.0 -130.0 0.0 55.0 90.0))
までは問題なくできますが、

これが問題なくできるのはロボットモデルに関しては，
https://github.com/euslisp/jskeus/blob/317c29c67d52648af485c4833a7fbfd6d8345415/irteus/irtmodel.l#L819-L824
にあるようにjoint-listに入っているjointの数の分だけベクトルの中身の数値を読みに行っているので，jointの数以上の次元数を入力した場合にはそれを超えた分はすべて無視されるからだと考えられます．