New set of unit tests for updated cost utilities. Still needs some testing.
Added a mutable_joint_state_publisher, fixed some bugs
Added a getEulerZYX to the Pose6d
Added a new transform interface to talk to the mutable joint state publisher