Remove local codes

研究室内部のコードからコピーしたままになってたコードを、OpenHRP3のライブラリなど使いつつ
書き直しました。
コミット前後で挙動がかわらないはず、なコミットとなります。

よろしくお願いいたします。