Skip to content

Commit

Permalink
Merge pull request #501 from YoheiKakiuchi/pr_fix_coordinates_doc
Browse files Browse the repository at this point in the history 
Fix documentation for coordinate class
  • Loading branch information
@k-okada
k-okada authored Jun 27, 2023
2 parents 96fbc78 + 398ab65 commit 25a473c
Show file tree
Hide file tree
Showing 3 changed files with 213 additions and 2 deletions.
2 changes: 1 addition & 1 deletion doc/jlatex/jmatrix.tex
View file
Original file line number Diff line number Diff line change
Expand Up @@ -712,7 +712,7 @@ \subsubsection*{座標系を変更するメソッド}

wrt == :world のとき、 $T \leftarrow AT$

wrt == W (coordinates class) のとき、$T \leftarrow$ $\left( WAW \right)^{-1} T$
wrt == W (coordinates class) のとき、$T \leftarrow$ $\left( W A W^{-1} \right) T$
}

\end{refdesc}
Expand Down
2 changes: 1 addition & 1 deletion doc/latex/matrix.tex
View file
Original file line number Diff line number Diff line change
Expand Up @@ -733,7 +733,7 @@ \subsubsection*{Methods modifying itself}

when wrt == :world, $T \leftarrow AT$

when wrt == W (coordinates class), $T \leftarrow$ $\left( WAW \right)^{-1} T$
when wrt == W (coordinates class), $T \leftarrow$ $\left( W A W^{-1} \right) T$
}

\end{refdesc}
Expand Down
211 changes: 211 additions & 0 deletions test/coords.l
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,217 @@
-- undefined function quaternion2matrix , exitting...
-- See jskeus/irteus/coords.l~%")

#-:word-size=64
(defvar *current-eps* *epsilon*)

#+:word-size=64
(defvar *current-eps* (* *epsilon* 1e-8))

(deftest test-make-new-coords
(let ((pos (float-vector 100 200 300))
(rot (rpy-matrix 0.2 0.3 0.5))
(cds (make-coords))
cds-new0 cds-new1)

(setq cds-new0 (make-coords))
(send cds-new0 :newcoords cds)

(setq cds-new1 (make-coords))
(send cds-new1 :newcoords rot pos)

(assert (eq (send cds :pos) (send cds-new0 :pos)))
(assert (eq (send cds :rot) (send cds-new0 :rot)))

(assert (eq pos (send cds-new1 :pos)))
(assert (eq rot (send cds-new1 :rot)))
))

(deftest test-convert-vector
(let* ((vec0 (float-vector 100 200 300))
(vec1 (float-vector 50 100 500))
(mat (rpy-matrix 0.1 0.2 0.3))
(pos (float-vector 10 20 30))
(cds (make-coords :pos pos :rot mat))
)
(assert (eps-v= (send cds :rotate-vector vec0)
(transform mat vec0) *current-eps*))
(assert (eps-v= (send cds :rotate-vector vec1)
(transform mat vec1) *current-eps*))
#| ;; :inverse-rotate-vector is a method in irteus
(assert (eps-v= (send cds :inverse-rotate-vector vec0)
(transform vec0 mat) *current-eps*))
(assert (eps-v= (send cds :inverse-rotate-vector vec1)
(transform vec1 mat) *current-eps*))
|#
(assert (eps-v= (send cds :transform-vector vec0)
(v+ (transform mat vec0) pos) *current-eps*))
(assert (eps-v= (send cds :transform-vector vec1)
(v+ (transform mat vec1) pos) *current-eps*))

(assert (eps-v= (send cds :inverse-transform-vector vec0)
(transform (transpose mat) (v- vec0 pos)) *current-eps*))
(assert (eps-v= (send cds :inverse-transform-vector vec1)
(transform (transpose mat) (v- vec1 pos)) *current-eps*))
))

(deftest test-create-target-coordinates
(let* ((pos0 (float-vector 100 200 300))
(pos1 (float-vector 50 100 500))
(pos2 (float-vector 10 -10 20))
(mat0 (rpy-matrix 0.1 0.2 0.3))
(mat1 (rpy-matrix 0.05 0.1 0.4))
(mat2 (rpy-matrix -0.1 0.2 -0.05))
(cds (make-coords :pos pos0 :rot mat0))
(cds1 (make-coords :pos pos1 :rot mat1))
(cds2 (make-coords :pos pos2 :rot mat2))
)
(let ((res (send cds :inverse-transformation)))
(assert (eps-v= (array-entity (send res :rot))
(array-entity (transpose mat0)) *current-eps*))
(assert (eps-v= (send res :pos)
(transform (transpose mat0) (v- pos0)) *current-eps*))
)
(let ((res0 (send cds :transformation cds1))
(res1 (send (send cds :inverse-transformation) :transform cds1)))
(assert (eps-v= (array-entity (send res0 :rot)) (array-entity (send res1 :rot)) *current-eps*))
(assert (eps-v= (send res0 :pos) (send res1 :pos) *current-eps*))
)
(let ((res0 (send cds :transformation cds1 :world))
(res1 (send (send cds1 :copy-coords)
:transform (send cds :inverse-transformation))))
(assert (eps-v= (array-entity (send res0 :rot)) (array-entity (send res1 :rot)) *current-eps*))
(assert (eps-v= (send res0 :pos) (send res1 :pos) *current-eps*))
)
(let ((res0 (send cds :transformation cds1 cds2))
(res1 (send (send (send cds2 :inverse-transformation) :transform cds1)
:transform
(send (send cds :inverse-transformation) :transform cds2))))
(assert (eps-v= (array-entity (send res0 :rot)) (array-entity (send res1 :rot)) *current-eps*))
(assert (eps-v= (send res0 :pos) (send res1 :pos) *current-eps*))
)
))

(deftest test-move-to
(let* ((pos0 (float-vector 100 200 300))
(pos1 (float-vector 50 100 500))
(pos2 (float-vector 10 -10 20))
(mat0 (rpy-matrix 0.1 0.2 0.3))
(mat1 (rpy-matrix 0.05 0.1 0.4))
(mat2 (rpy-matrix -0.1 0.2 -0.05))
(cds (make-coords :pos pos0 :rot mat0))
(cds1 (make-coords :pos pos1 :rot mat1))
(cds2 (make-coords :pos pos2 :rot mat2))
(tmp-l (send cds :copy-coords))
(tmp-w (send cds :copy-coords))
(tmp-o (send cds :copy-coords))
)
(send tmp-l :move-to cds1)
(let ((res0 tmp-l)
(res1 (send (send cds :copy-coords) :transform cds1)))
(assert (eps-v= (array-entity (send res0 :rot)) (array-entity (send res1 :rot)) *current-eps*))
(assert (eps-v= (send res0 :pos) (send res1 :pos) *current-eps*))
)
;;
(send tmp-w :move-to cds1 :world)
(assert (eq (send tmp-w :pos) (send cds1 :pos)))
(assert (eq (send tmp-w :rot) (send cds1 :rot)))
;;
(send tmp-o :move-to cds1 cds2)
(let ((res0 tmp-o)
(res1 (send (send cds2 :copy-coords) :transform cds1)))
(assert (eps-v= (array-entity (send res0 :rot)) (array-entity (send res1 :rot)) *current-eps*))
(assert (eps-v= (send res0 :pos) (send res1 :pos) *current-eps*))
)
))

(deftest test-translate
(let* ((pos0 (float-vector 100 200 300))
(pos1 (float-vector 50 100 500))
(pos2 (float-vector 10 -10 20))
(mat0 (rpy-matrix 0.1 0.2 0.3))
(mat1 (rpy-matrix 0.05 0.1 0.4))
(mat2 (rpy-matrix -0.1 0.2 -0.05))
(cds (make-coords :pos pos0 :rot mat0))
(cds1 (make-coords :pos pos1 :rot mat1))
(cds2 (make-coords :pos pos2 :rot mat2))
(tmp-l (send cds :copy-coords))
(tmp-w (send cds :copy-coords))
(tmp-o (send cds :copy-coords))
)
(send tmp-l :translate pos1)
(assert (eps-v= (send tmp-l :pos)
(v+ pos0 (transform mat0 pos1)) *current-eps*))
;;
(send tmp-w :translate pos1 :world)
(assert (eps-v= (send tmp-w :pos) (v+ pos0 pos1) *current-eps*))
;;
(send tmp-o :translate pos1 cds2)
(assert (eps-v= (send tmp-o :pos)
(v+ pos0 (transform mat2 pos1)) *current-eps*))
))

(deftest test-locate
(let* ((pos0 (float-vector 100 200 300))
(pos1 (float-vector 50 100 500))
(pos2 (float-vector 10 -10 20))
(mat0 (rpy-matrix 0.1 0.2 0.3))
(mat1 (rpy-matrix 0.05 0.1 0.4))
(mat2 (rpy-matrix -0.1 0.2 -0.05))
(cds (make-coords :pos pos0 :rot mat0))
(cds1 (make-coords :pos pos1 :rot mat1))
(cds2 (make-coords :pos pos2 :rot mat2))
(tmp-l (send cds :copy-coords))
(tmp-w (send cds :copy-coords))
(tmp-o (send cds :copy-coords))
)
(send tmp-l :locate pos1)
(assert (eps-v= (send tmp-l :pos)
(v+ pos0 (transform mat0 pos1)) *current-eps*))
;;
(send tmp-w :locate pos1 :world)
(assert (eps-v= (send tmp-w :pos) pos1 *current-eps*))
;;
(send tmp-o :locate pos1 cds2)
(assert (eps-v= (send tmp-o :pos)
(v+ pos2 (transform mat2 pos1)) *current-eps*))
))

(deftest test-transform
(let* ((pos0 (float-vector 100 200 300))
(pos1 (float-vector 50 100 500))
(pos2 (float-vector 10 -10 20))
(mat0 (rpy-matrix 0.1 0.2 0.3))
(mat1 (rpy-matrix 0.05 0.1 0.4))
(mat2 (rpy-matrix -0.1 0.2 -0.05))
(cds (make-coords :pos pos0 :rot mat0))
(cds1 (make-coords :pos pos1 :rot mat1))
(cds2 (make-coords :pos pos2 :rot mat2))
(tmp-l (send cds :copy-coords))
(tmp-w (send cds :copy-coords))
(tmp-o (send cds :copy-coords))
)
(send tmp-l :transform cds1)
(let ((rot (m* mat0 mat1))
(pos (v+ pos0 (transform mat0 pos1))))
(assert (eps-v= (array-entity (send tmp-l :rot)) (array-entity rot) *current-eps*))
(assert (eps-v= (send tmp-l :pos) pos *current-eps*))
)
(send tmp-w :transform cds1 :world)
(let ((rot (m* mat1 mat0))
(pos (v+ pos1 (transform mat1 pos0))))
(assert (eps-v= (array-entity (send tmp-w :rot)) (array-entity rot) *current-eps*))
(assert (eps-v= (send tmp-w :pos) pos *current-eps*))
)
(send tmp-o :transform cds1 cds2)
(let ((trs (make-coords)))
(send trs :transform cds2)
(send trs :transform cds1)
(send trs :transform (send cds2 :inverse-transformation))
(send trs :transform cds)
(assert (eps-v= (array-entity (send tmp-o :rot)) (array-entity (send trs :rot)) *current-eps*))
(assert (eps-v= (send tmp-o :pos) (send trs :pos) *current-eps*))
)
))

(eval-when (load eval)
(run-all-tests)
Expand Down

1 comment on commit 25a473c

@k-okada
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thank you for contributing EusLisp documentation

Please check latest documents before merging

PDF version of English manual: manual.pdf
PDF version of Japanese jmanual: jmanual.pdf
HTML version of English manual: manual.html
HTML version of Japanese manual: jmanual.html
Sphinx (ReST) version of English manual: manual.rst

Please sign in to comment.