Addapting turtlebot_arm_block_manipulation package to work with moveit
(still a lot to do)