Improved OOP, simplified class a bit, cleaned up
Improved OOP, simplified class a bit, cleaned up

ScazLab/human_robot_collaboration,ScazLab/baxter_collaboration,ScazLab/human_robot_collaboration,ScazLab/human_robot_collaboration,ScazLab/baxter_collaboration,ScazLab/human_robot_collaboration,ScazLab/baxter_collaboration,ScazLab/baxter_collaboration