あんまりQiitaぽくないネタだけど...
クラウドで管理できるマークダウンで数式書けるものを探していたのでテスト．
勉強不足がヒドいので，正しいかどうかわかりません．
シミュレーション動画 (2015/9/25 追記)
システム
アーム手先の位置姿勢を $\boldsymbol{p_{ee}}$, ロボットの状態を $\bar{\boldsymbol{q}}$ とする． $n \geq 4$ とする．
\begin{align}
\boldsymbol{p} &=
\begin{bmatrix}
x_{ee} & y_{ee} & z_{ee} & \psi_{ee} & \theta_{ee} & \phi_{ee}
\end{bmatrix}^\mathrm{T}
\in \mathrm{R}^{6}
\\
\bar{\boldsymbol{q}} &=
\begin{bmatrix}
\theta_1 & ... & \theta_n & x_{base} & y_{base} & \theta_{base}
\end{bmatrix}^\mathrm{T}
\in \mathrm{R}^{n+3}
\\
\end{align}
同次変換行列を考えて，
\begin{align}
{}^{origin}T_{ee}
&= {}^{origin}T_{base} {}^{base}T_{arm} {}^{arm}T_{joint1} \cdots {}^{jointn}T_{ee}
\end{align}
から，${}^{origin}T_{ee}$ の右上 $3\times3$ の回転行列と，左端の縦ベクトルから $
\boldsymbol{p}$ は
\begin{align}
\boldsymbol{p} &= \boldsymbol{f}(\bar{\boldsymbol{q}})
\end{align}
こんなかんじに書ける．
で，
\begin{align}
\dot{\boldsymbol{p}}
&= \frac{\partial \boldsymbol{f}(\bar{\boldsymbol{q}})}{\partial \bar{\boldsymbol{q}}}\dot{\bar{\boldsymbol{q}}} \\
&= \bar{J}(\bar{\boldsymbol{q}})\dot{\bar{\boldsymbol{q}}}
\quad
\left(\bar{J} \in \mathrm{R}^{6 \times (n+3)}\right)
\end{align}
ここで，
\dot{\boldsymbol{q}} =
\begin{bmatrix}
\dot{\theta_1} & ... & \dot{\theta_n} & v & \omega
\end{bmatrix}^\mathrm{T}
とすると，
\dot{\bar{\boldsymbol{q}}}
=
\begin{bmatrix}
\mathrm{I}_n & 0 & 0 \\
0 \cdots 0 & cos(\theta_{base}) & 0 \\
0 \cdots 0 & sin(\theta_{base}) & 0 \\
0 \cdots 0 & 0 & 1
\end{bmatrix}
\dot{\boldsymbol{q}}
=
T_{nh}
\dot{\boldsymbol{q}}
\quad
\left(T_{nh} \in \mathrm{R}^{(n+3) \times (n+2)}\right)
とできるので，結局システムは
\begin{align}
\dot{\boldsymbol{p}}
&= \bar{J}(\bar{\boldsymbol{q}}) \dot{\bar{\boldsymbol{q}}}
= \bar{J}(\bar{\boldsymbol{q}}) T_{nh} \bar{\boldsymbol{q}} \\
&= J(\bar{\boldsymbol{q}}) \dot{\boldsymbol{q}}
\quad
\left(J = \bar{J}(\bar{\boldsymbol{q}}) T_{nh} \in \mathrm{R}^{6 \times (n+2)}\right)
\end{align}
と表せられる．
制御
入力 $\boldsymbol{u}$ を $\dot{\boldsymbol{q}}$ として，目標を $\boldsymbol{p}_d$ とすると，
\boldsymbol{u} =
J^{\dagger} \left(\dot{\boldsymbol{p}_d} - K_p(\boldsymbol{p} - \boldsymbol{p}_d) \right)
と入力を作れば，指数関数で収束する．
$n = 4$ なら $J^{\dagger}$ は逆行列だけど，$n > 4$ なら擬似逆行列で冗長系になって，
\boldsymbol{u} =
J^{\dagger} \left(\dot{\boldsymbol{p}_d} - K_p(\boldsymbol{p} - \boldsymbol{p}_d) \right)
+ (I_{n+2} - J^{\dagger} J) \boldsymbol{k}_{r}
と，任意のベクトル $\boldsymbol{k}_{r} \in \mathrm{R}^{n+2}$ を含む項が追加できて，
\boldsymbol{k}_{r} =
\alpha \left( \frac{\partial V(\boldsymbol{q})}{\partial \boldsymbol{q}} \right)^\mathrm{T}
とすれば，評価関数 $V(\boldsymbol{q})$ を増大 or 減少させる方向に制御できる．（向きは $\alpha$ の符号による）
教科書とかであるのは，$V(\boldsymbol{q})$ を可操作度とかにして，その増大を狙う．
↑↑↑ ここまでM1の授業で習ったレベル ↑↑↑
↓↓↓ ここからなんか自分で考えたこと(常識？) ↓↓↓
で，よくわからんのは，$V(\boldsymbol{q})$ の $\boldsymbol{q}$ ってなんやねん！ってこと．
ちゃんと言えば，$v$ と $\omega$ の積分したもの的なものはなんなのか．
まぁ，左右の車輪の回転角速度と$v$ と $\omega$ は線形なので，左右の車輪の回転角度に写像してやることは可能なんだけど．
問題は，評価関数 $V$ を例えば可操作度とかにすると，
V(\boldsymbol{q})
= \sqrt{det(J(\bar{\boldsymbol{q}}) J(\bar{\boldsymbol{q}})^{\mathrm{T}} )}
で，$\boldsymbol{q}$ が陽に出てこなくて，$\boldsymbol{k}_{r}$ を計算する微分がめんどくさい．
ということで，
\begin{align}
\frac{\partial V(\boldsymbol{q})}{\partial \boldsymbol{q}}
&= \frac{1}{2 \sqrt{ det{(J J^{\mathrm{T}})}} } \frac{\partial det(J J^{\mathrm{T}})}{\partial \boldsymbol{q}} \\
&= \frac{1}{2\sqrt{det{(J_{sq}}(\bar{\boldsymbol{q}}))}}
\frac{\partial det(J_{sq}(\bar{\boldsymbol{q}}))}{\partial \bar{\boldsymbol{q}}}
\frac{\partial \bar{\boldsymbol{q}}}{\partial \boldsymbol{q}}
\quad \left( J_{sq}(\bar{\boldsymbol{q}}) = J(\bar{\boldsymbol{q}}) J(\bar{\boldsymbol{q}})^{\mathrm{T}} \right)
\end{align}
ここで，こんなことやっていいのかわからんけど，ノンホロの $\dot{\bar{\boldsymbol{q}}} = T_{nh} \dot{\boldsymbol{q}}$ の式から $T_{nh} = \frac{\partial \bar{\boldsymbol{q}}}{\partial \boldsymbol{q}}$ じゃね？ってことで
\begin{align}
\frac{\partial V(\boldsymbol{q})}{\partial \boldsymbol{q}}
&=
\frac{1}{2\sqrt{det(J_{sq}(\bar{\boldsymbol{q}}))}}
\frac{\partial det(J_{sq}(\bar{\boldsymbol{q}}))}{\partial \bar{\boldsymbol{q}}}
T_{nh}
\end{align}
http://homepage2.nifty.com/eman/relativity/detdif.html から微分のところを要素ごとに見ると，
\begin{align}
\left( \frac{\partial det(J_{sq}(\bar{\boldsymbol{q}}))}{\partial \bar{\boldsymbol{q}}} \right)_k
&=
\frac{\partial det(J_{sq}(\bar{\boldsymbol{q}}))}{\partial \bar{q}_k} \\
&=
det(J_{sq}) \sum_{i, j} ({J_{sq}}^{-1})_{ij} \frac{\partial (J_{sq})_{ij}}{\partial \bar{q}_k} \\
&=
det(J_{sq}) \sum_{i, j} ({J_{sq}}^{-1})_{ij}
\left(\frac{\partial (J)_{ij}}{\partial \bar{q}_k} (J)_{ji} + (J)_{ij} \frac{\partial (J)_{ji}}{\partial \bar{q}_k} \right)
\end{align}
ということで
\begin{align}
\frac{\partial V(\boldsymbol{q})}{\partial \boldsymbol{q}}
&=
\frac{\sqrt{det(J_{sq}(\bar{\boldsymbol{q}}))}}{2}
\left(\sum_{i, j} ({J_{sq}}^{-1})_{ij} \frac{\partial (J_{sq})_{ij}}{\partial \bar{q}_k} \right)
T_{nh}
\\
&=
\frac{\sqrt{det(J_{sq}(\bar{\boldsymbol{q}}))}}{2}
\left(\sum_{i, j} ({J_{sq}}^{-1})_{ij}
\left(\frac{\partial (J)_{ij}}{\partial \bar{q}_k} (J)_{ji} + (J)_{ij} \frac{\partial (J)_{ji}}{\partial \bar{q}_k} \right)\right)
T_{nh}
\end{align}
行列式の微分はちょー厳しいけど，
\frac{\partial (J_{sq})_{ij}}{\partial \bar{q}_k}
\quad or \quad
\frac{\partial (J)_{ij}}{\partial \bar{q}_k} (J)_{ji} + (J)_{ij} \frac{\partial (J)_{ji}}{\partial \bar{q}_k}
ならがんばってmatlabのシンボリック計算で書き下せる．$det(J_{sq}), ({J_{sq}}^{-1})_{ij}$ は都度都度計算してやれば，なんとかmatlabでシミュレータは作れる．
最後に
正しいのかマジでわからん... 適当にやり過ぎな気がする...
ちゃんとノンホロノミックを勉強しないといけないんだろうなとおもうとキツい．
Qiitaの数式便利だけど，やっぱりサジェストとか補完ないと書きにくい．
プレビューで常に確認しながら書いてると，プレビューが重すぎてめちゃくちゃストレスだし，
プレビューなしで数式ガッと書き終えてからプレビューで確認すると，カッコ忘れてるとかしょーもないミスを探すのが大変になる．
バリバリ $TeX$ 書ける人ならプレビューなしでいっちゃうのがいいんだろうけど，自分にはハードル高いっすね...