Simplifying RobotState class implementation
Added generateJntArray() member function:
   Generate JntArray (KDL type) from JointState::join_state
Added TreeFkSolverPos_recursive