Add constraint grouping routine
Working on joint limit constraint