diff --git a/doc/irtmodel.tex b/doc/irtmodel.tex index 8435ebe65..217101d16 100644 --- a/doc/irtmodel.tex +++ b/doc/irtmodel.tex @@ -1605,8 +1605,9 @@ \subsubsection{三軸関節ロボットを使ったヤコビアン,逆運動 う場合の例を示す.ここでは,\verb|:calc-jacobian-from-link-list|のオプ ショナル引数として\verb|:rotation-axis|, \verb|:translation-axis| があり,それぞれ位置,姿勢での拘束条件を示す. -\verb|t|は三軸拘束,\verb|nil|は拘束なし,その他に\verb|:x|, -\verb|:y|, \verb|:z|を指定することができる. +\verb|t|は三軸拘束,\verb|nil|は拘束なしを示し,その他に拘束なしの軸を +エンドエフェクタの座標系で\verb|:x|, \verb|:y|, \verb|:z|, \verb|:xy|, +\verb|:yz|, \verb|:zx|と指定することができる. {\baselineskip=10pt \begin{verbatim} diff --git a/irteus/irtgeo.l b/irteus/irtgeo.l index c005a053e..c43a28e11 100644 --- a/irteus/irtgeo.l +++ b/irteus/irtgeo.l @@ -55,7 +55,7 @@ ) (:difference-position (coords &key (translation-axis t)) - "return difference in position of given coords, translation-axis can take (:x, :y, :z, :xy, :yz, :zx)." + "return difference in position of given coords, translation-axis can take (:x, :y, :z, :xy, :yz, :zx, :yx, :zy, :xz)." (let ((dif-pos (send self :inverse-transform-vector (send coords :worldpos)))) (case @@ -69,7 +69,7 @@ dif-pos)) (:difference-rotation (coords &key (rotation-axis t)) - "return difference in rotation of given coords, rotation-axis can take (:x, :y, :z, :xx, :yy, :zz, :xm, :ym, :zm)" + "return difference in rotation of given coords, rotation-axis can take (:x, :y, :z, :xy, :yz, :zx, :yx, :zy, :xz, :xx, :yy, :zz, :xm, :ym, :zm)" (labels ((need-mirror-for-nearest-axis (coords0 coords1 axis) @@ -88,6 +88,22 @@ (setq dif-rot (transform (transpose (send self :worldrot)) (scale (acos (v. a0 a1)) (normalize-vector (v* a0 a1)))))) + ((:xy :yx :yz :zy :zx :xz) + (let ((ax1 (case rotation-axis ((:xy :yx) :z) ((:yz :zy) :x) ((:zx :xz) :y))) + (ax2 (case rotation-axis ((:xy :yx) :x) ((:yz :zy) :y) ((:zx :xz) :z)))) + (setq a0 (send self :axis ax1) + a1 (send coords :axis ax1)) + (setq dif-rot + (transform (transpose (send self :worldrot)) + (scale (acos (v. a0 a1)) (normalize-vector (v* a0 a1))))) + (let ((self-coords (if (= 0.0 (norm dif-rot)) + self + (send (send self :copy-worldcoords) :rotate (norm dif-rot) (normalize-vector dif-rot))))) + (setq a0 (send self-coords :axis ax2) + a1 (send coords :axis ax2)) + (setq dif-rot + (transform (transpose (send self-coords :worldrot)) + (scale (acos (v. a0 a1)) (normalize-vector (v* a0 a1)))))))) ((:xx :yy :zz) (let ((axis (case rotation-axis (:xx :x) (:yy :y) (:zz :z))) a0 a2) (setq a0 (send self :axis axis))