Add categorizeConstraint function for InverseKin
Minor change on variable name to stay consistent with TrajectoryOptimization
RigidBodyCost can enforce additional constraints
Add a 'categoryString' function to the RigidBodyConstraint. Modify the test files to test such function