merged, successfully storing joint positions and velocities in the KDL::JntArray for q_ & qdot_