>>42
仮にkリンク目から見たk+1リンク目の相対位置と相対姿勢をそれぞれP(k,k+1),R(k,k+1)とすると、4リンク目の世界座標での位置Pと姿勢Rは
P=R(1,2)P(1,2)+R(1,2)R(2,3)P(2,3)+R(1,2)R(2,3)R(3,4)P(3,4)
R=R(1,2)R(2,3)R(3,4)
です。リンク1の位置と姿勢を原点としています。
あとはRに回転行列を入れていくだけです。
(次の関節位置を見て回転させて足すだけ)
また、ヤコビアンはPを時間で微分して各関節角速度θ'の係数を行列でまとめたものです。

おおよそこんな感じなのですが、慣れないうちはイメージがつかないかと思います。
良い先生の講義が聞けると分かりやすいのですが...

ヤコビアンは時間に余裕ができれば載せようと思います。
おそらくよく分からないと思うので、
分からないところがあれば聞いてください！