fix issue #56, #59

[hmatsushita]

issue #56 と #59 の修正を実施いたしました。

なお、 #58 で返答を受けた関節軌道の開始時間の件ですが、現行のコントローラの実装では、各軌道の目標時間となった時点で関節角度設定を行っているため、暫定対応として開始時間に軌道データの先頭の目標時間を加算したものを使用しております。

本来であれば目標時間より軌道を開始し、各軌道の目標時間に指定の関節角度となるような制御を行わなければならないという事は理解しており、今後そのようにコントローラを改修する所存です。