Merge pull request #6 from twiniars/master

tool added
Merge branch 'master' of github.com:RCPRG-ros-pkg/irp6_robot
force control fix'es
force ontrol in progress
force cotnrol in progress