アルデバランが提供する線形代数ライブラリであるalmathというのが存在するらしい。こちらのレポジトリで提供されています。

インストール

pythonで使用する場合は以下でインストール可能。

$ pip install almath

公式からnaoqi SDK落としてきてパスを通している人はalmathも入ってるっぽいので何もせず使えます。3系で使いたい場合は一応用意されているので同様にインストール可能ですが、naoqi SDKのパスが通ってるとimport時にエラーが起きるっぽいです。(~~naoqiは早く3系に移行してくれ~~)。パス通ってない状態だとpipコマンドでいれれば多分問題なく使えると思います。

使い方

import almath

# (x, y, θ)
vector2d = almath.Pose2D(3.0, 0, 0)
# (x, y, z, roll, pitch, yaw)
vector6d = almath.Position6D(2.0, 1.0, 0, 0, 0, 0)

# convert to simultaneous transform matrix from vector
t_vector2d = almath.transformFromPose2D(vector2d)
t_vector6d = almath.transformFromPosition6D(vector6d)

t_result = t_vector2d * t_vector6d
# convert to vector from simultaneous transform matrix
result = almath.position6DFromTransform(t_result)
# get element of vector
print("[{},{},{},{},{},{}]".format(result.x,result.y,result.z,result.wx,result.wy,result.wz))

各ベクトル初期化時は引数を入れないとすべて0で初期化されます。Eigenとかと同様、各要素はa.xとかで取ることが可能です。リストとして扱いたい場合は以下のようにするとリスト型へ変換が可能です。

list(almath.Position6D().toVector())
# almath.Position6D().toVector()自体はタプル型として扱われる

その他使い方

# calculate norm
almath.Position6D().norm()

# calculate normalize
almath.Position6D().normalize()

# calculate distance between two vectors
a = almath.Position6D(2,0,0,0,0,0)
b = almath.Position6D(2,5,0,0,0,0)
almath.distance(a,b)

# get rotate matrix
ax = almath.Position3D(1,0,0)
ay = almath.Position3D(0,0,0)
az = almath.Position3D(0,0,0)
rotation = almath.rotationFromAxesXYZ(ax, ay, az)

# get quaternion
rotation3D = almath.rotation3DFromRotation(rotation)
almath.quaternionFromRotation3D(rotation3D)

# generate Quaternion
almath.Quaternion(1,0,0,0)
# Quaternion(w, x, y, z)

その他いろいろできますが全体のAPIを追い切れていないのでまた後日追記しようと思います。
(API見てると占有格子地図で使いそうな名前のAPIとかもあったんでALNavigationとかで使ってるのかな)

結論

以外と便利かも？？？？