Refactor slightly to make setting the initial state easier.
A simple method for creating a new RobotState from an initial state and an end effector pose goal.
Merge branch 'master' of github.com:ibrahima/trajopt_plugin

Conflicts:
	src/move_group_battery.py
Change planner service from trajopt_planner to plan_kinematic_path
Change service in planning_server.py to 'plan_kinematic_path' to match move_group.
Handle cases where the service throws an exception (move_group does this when it can't find a valid plan).