Added debugging code to look for bad q/qd/forces/limits/etc. in joints