From c3603fefd3a7329e1d4002ac51023817f92d70a7 Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Sun, 24 May 2020 10:52:31 +0900 Subject: [PATCH 1/4] [irtgeo.l] enable :rotation-axis :xy :yz :zx :yx :zy :xz --- irteus/irtgeo.l | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/irteus/irtgeo.l b/irteus/irtgeo.l index c005a053e..69ada011a 100644 --- a/irteus/irtgeo.l +++ b/irteus/irtgeo.l @@ -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, :xx, :yy, :zz, :xm, :ym, :zm)" (labels ((need-mirror-for-nearest-axis (coords0 coords1 axis) @@ -88,6 +88,19 @@ (setq dif-rot (transform (transpose (send self :worldrot)) (scale (acos (v. a0 a1)) (normalize-vector (v* a0 a1)))))) + ((:xy :yx :yz :zy :zx :xz) + (setq a0 (send self :axis (case rotation-axis ((:xy :yx) :z) ((:yz :zy) :x) ((:zx :xz) :y))) + a1 (send coords :axis (case rotation-axis ((:xy :yx) :z) ((:yz :zy) :x) ((:zx :xz) :y)))) + (setq dif-rot + (transform (transpose (send self :worldrot)) + (scale (acos (v. a0 a1)) (normalize-vector (v* a0 a1))))) + (let ((self-coords (send (send self :copy-worldcoords) :rotate (norm dif-rot) dif-rot))) + (setq a0 (send self-coords :axis (case rotation-axis ((:xy :yx) :x) ((:yz :zy) :y) ((:zx :xz) :z))) + a1 (send coords :axis (case rotation-axis ((:xy :yx) :x) ((:yz :zy) :y) ((:zx :xz) :z)))) + (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)) From 85c060d39f347120dc426c8cd1d37f85cd2d3fbc Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Mon, 25 May 2020 09:54:56 +0900 Subject: [PATCH 2/4] [irtgeo.l] fix documentation of :difference-position and :difference-rotation --- irteus/irtgeo.l | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/irteus/irtgeo.l b/irteus/irtgeo.l index 69ada011a..a2232b7c8 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, :xy, :yz, :zx, :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) From 815f8b46e9b7ed047e85a4a8aa8972f93dbc449c Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Mon, 25 May 2020 18:02:52 +0900 Subject: [PATCH 3/4] [iregeo.l :difference-rotation] use let, and fix 0 norm condition --- irteus/irtgeo.l | 25 ++++++++++++++----------- 1 file changed, 14 insertions(+), 11 deletions(-) diff --git a/irteus/irtgeo.l b/irteus/irtgeo.l index a2232b7c8..c43a28e11 100644 --- a/irteus/irtgeo.l +++ b/irteus/irtgeo.l @@ -89,18 +89,21 @@ (transform (transpose (send self :worldrot)) (scale (acos (v. a0 a1)) (normalize-vector (v* a0 a1)))))) ((:xy :yx :yz :zy :zx :xz) - (setq a0 (send self :axis (case rotation-axis ((:xy :yx) :z) ((:yz :zy) :x) ((:zx :xz) :y))) - a1 (send coords :axis (case rotation-axis ((:xy :yx) :z) ((:yz :zy) :x) ((:zx :xz) :y)))) - (setq dif-rot - (transform (transpose (send self :worldrot)) - (scale (acos (v. a0 a1)) (normalize-vector (v* a0 a1))))) - (let ((self-coords (send (send self :copy-worldcoords) :rotate (norm dif-rot) dif-rot))) - (setq a0 (send self-coords :axis (case rotation-axis ((:xy :yx) :x) ((:yz :zy) :y) ((:zx :xz) :z))) - a1 (send coords :axis (case rotation-axis ((:xy :yx) :x) ((:yz :zy) :y) ((:zx :xz) :z)))) + (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-coords :worldrot)) - (scale (acos (v. a0 a1)) (normalize-vector (v* a0 a1)))))) - ) + (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)) From 85d63f9ea070604d17ca961e38084f06e761e316 Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Tue, 26 May 2020 19:30:36 +0900 Subject: [PATCH 4/4] [doc/irtmodel.tex]add document for translation/rotation-axis --- doc/irtmodel.tex | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) 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}