started adding kdl/jacobian control
small changes. more easy type conversion.