Next: 3 Euslispクラスシステム Up: ソフトウェア特論講義資料ロボットのソフトウェア Previous: 1 ロボットシステムのソフトウェア

2 ベクトル行列計算

euslispにはCommonLispの関数のよくつかう関数以外に，ロボットプログラミング用ということで，ベクトル行列計算用の組み込み関数があります．要素が浮動小数に決まっているベクトルとしてfloat-vectorタイプというものがあり，float-vectorという関数で作ります．行列も要素が浮動小数に決まっている2次元配列です． make-matrixで作ります．行列，ベクタのサイズは自由です．


solvalou<inaba> eus
EusLisp 7.15 created on Thu Jan 13 00:54:12 1994
eus$ (setq v1 (float-vector 1 2 3 4))
#f(1.0 2.0 3.0 4.0)
eus$ (setq v2 (float-vector 5 6 7 8))
#f(5.0 6.0 7.0 8.0)
eus$ (float-vector-p v1)  ;;; 型がfloat-vectorか
t

eus$ (v+ v1 v2)         ;;; 足し算
#f(6.0 8.0 10.0 12.0)
eus$ (v- v1 v2)         ;;; 引き算
#f(-4.0 -4.0 -4.0 -4.0)
eus$ (v. v1 v2)                ;;; 内積
70.0
eus$ (scale 3 v2)       ;;; 定数倍
#f(15.0 18.0 21.0 24.0)

eus$ (norm v1)          ;;; 長さ
5.47722
eus$ (normalize-vector v1)  ;;; 長さ1に正規化
#f(0.182574 0.365148 0.547723 0.730297)

eus$ (distance v1 v2)   ;;; 距離
8.0

eus$ (setq m1 (make-matrix 4 4
          '((2 1 1 0)
            (1 1 1 1)
            (5 3 2 1)
            (4 2 3 1))))  ;;; 初期値を与える．
#2f((2.0 1.0 1.0 0.0)
    (1.0 1.0 1.0 1.0)
    (5.0 3.0 2.0 1.0)
    (4.0 2.0 3.0 1.0))

eus$ (m* m1 m1)
#2f((10.0 6.0 5.0 2.0)
    (12.0 7.0 7.0 3.0)
    (27.0 16.0 15.0 6.0)
    (29.0 17.0 15.0 6.0))

eus$ (transform m1 v1)  ;;; 行列へ右からかける
#f(7.0 10.0 21.0 21.0)
eus$ (transform v1 m1)  ;;; 行列へ左からかける
#f(35.0 20.0 21.0 9.0)

eus$ (inverse-matrix m1)   ;;; 逆行列
#2f((-3.0 -2.0 1.0 1.0)
    (5.0 3.0 -1.0 -2.0)
    (2.0 1.0 -1.0 -1.788139e-07)
    (-4.0 -1.0 1.0 1.0))
eus$ (transpose m1)        ;;; 転置行列
#2f((2.0 1.0 5.0 4.0)
    (1.0 1.0 3.0 2.0)
    (1.0 1.0 2.0 3.0)
    (0.0 1.0 1.0 1.0))

eus$ (setq u (unit-matrix 3))  ;;; 単位行列
#2f((1.0 0.0 0.0) (0.0 1.0 0.0) (0.0 0.0 1.0))
eus$ (unit-matrix 2)
#2f((1.0 0.0) (0.0 1.0))
eus$ (unit-matrix 1)
#2f((1.0))

eus$ (setq a (float-vector 1 2 3))
#f(1.0 2.0 3.0)
eus$ (setq b (float-vector 2 -1 0))
#f(2.0 -1.0 0.0)
eus$ (v* a b)                ;;; 外積
#f(3.0 6.0 -5.0)

三次元空間ベクトルの回転に関していろいろな関数が定義されています．


eus$ pi
3.14159
eus$ (/ pi 6)
0.523599

eus$ (setq r0 (rotation-matrix (/ pi 6) :x))
#2f((1.0 0.0 0.0) (0.0 0.866025 -0.5) (0.0 0.5 0.866025))
eus$ (matrix-row r0 0)
#f(1.0 0.0 0.0)
eus$ (matrix-row r0 1)
#f(0.0 0.866025 -0.5)
eus$ (matrix-column r0 2)
#f(0.0 -0.5 0.866025)

eus$ (setq r (unit-matrix 3))
#2f((1.0 0.0 0.0) (0.0 1.0 0.0) (0.0 0.0 1.0))
eus$ (setq r1 (rotate-matrix r (/ pi 6) :x))
#2f((1.0 0.0 0.0) (0.0 0.866025 -0.5) (0.0 0.5 0.866025))

eus$ (rpy-angle r1)
((0.0 0.0 0.523599) (3.14159 -3.14159 -2.61799))
eus$ (rotation-angle r1)
(0.523599 #f(1.0 0.0 0.0))

eus$ (setq r2 (euler-matrix (/ pi 6) 0 0))
#2f((0.866025 -0.5 0.0) (0.5 0.866025 0.0) (0.0 0.0 1.0))
eus$ (euler-angle r2)
((0.0 0.0 0.523599) (3.14159 0.0 -2.61799))
eus$ (rotation-angle r2)
(0.523599 #f(0.0 0.0 1.0))
eus$ (rpy-angle r2)
((0.523599 0.0 0.0) (3.66519 -3.14159 3.14159))

LU分解などの関数などもあります．

generated through LaTeX2HTML. M.Inaba 平成18年5月6日