remove redundant code, fix bounds in those loops that still come from fortran
remove redundant code, fix bounds in those loops that still come from
fortran

robustrobotics/eigen,robustrobotics/eigen,robustrobotics/eigen,robustrobotics/eigen