Generalisation for other arms

After these changes I was able to get 1 successful grasp with Schunk PowerBall arm and WSG-50 gripper (with grasp_tester_fast).
I've committed the changes in a new branch, since I've worked with 0.7.2 version. Would I benefit from the changes in the groovy-devel branch?
I'm still struggling with performing the actual grasp (getting [ERROR]: Starting state is in collision, can't plan).