>>735
少し間違えてました。
正しくは、
q1=atan2(||p1×p2||,p1・p2)
です。
atan2()とは逆正接関数です。
https://en.m.wikipedia.org/wiki/Atan2
||p1×p2||は外積の大きさ
p1・p2は内積です。
この計算により、p1とp2のなす角q1が計算できます。

ロボットの機構を定義しておけば、
マニピュレータの場合は地面に固定されているので、関節数と同等の次元数でロボットの状態を表すことができます。
移動ロボットの場合はそれに6次元(基準の位置と姿勢)を加えた空間が状態を表す最小限の空間です。

まとめると、
地面に固定されている場合、n関節であればn次元で表し、
固定されていない場合、n関節であればn+6次元で表す
のがコンパクトと言えます。