Merge pull request #27 from reem-utils/chang_branch

Chang branch
Merge branch 'master' of https://github.com/reem-utils/robocup2014 into chang_branch
Adding Beta version of SM manip_to_pose