mfurukawa
commented
4 years ago なお，順運動学ですべての角度を0とした場合の姿勢は，順運動学の式が正しいと仮定して以下を得た．
-0.000 0.000 1.000 0.201 1.000 -0.000 0.000 0.000 0.000 1.000 0.000 0.242 0.000 0.000 0.000 1.000
この姿勢を再現するためのOptiTrackのRigidBodyの姿勢を知りたいので，四元数を以下の方法で求める．
右手で右手系の座標系を作り，左手で上記回転行列が表す姿勢を以下の方法で作る 左中指 を右手系Xとし右手のYへ(∵ 上記回転行列が列方向に0,1,0)， 左人差指を右手系Yとし右手のZへ(∵ 上記回転行列が列方向に0,0,1)， 左親指 を右手系Zとし右手のXへ(∵ 上記回転行列が列方向に1,0,0)，
すると，四元数表現で x,y,z = [1,1,1] theta = 90 [deg] の変換で左手の座標系を右手で表した座標系に変換できる．
https://github.com/maeda-lab/Scaledown/blob/2a96767ad56f1b851fba9bb4836c074b6144d3ef/tele_mbed_serial/tele_mbed_serial/tel_kin.cpp#L43
バグを修正しました．姿勢変換行列(DCM)の列方向ノルムが単位行列にならないことから，QuaternionからのDCMへの変換式を疑い修正しました．参照式の出典はこちらです．
