added functions to reset model

実験をしていて，うまく行かない点があったので修正しました．
GRASP時にモデルがずれていたので，
- そのモデルを治す方法の実装
- そもそもきちんとANGLEVECTORが行ってるかを確認する出力の追加
を行いました．