I got the kinematics running but a silly error of let the arm break free. So I reprinted some parts. I used the IK sovler from ROS/moveit: http://moveit.ros.org/wiki/Kinematics/IKFast

I'm very kind of all the people who offered help with this project, I still don't know how to organize that everyone can participate in a good way.