fix initial pose and use cfm

this PR relates to 
https://github.com/start-jsk/rtmros_gazebo/pull/141
https://github.com/start-jsk/rtmros_gazebo/pull/142