Cleaning up and extracting tests from mm_control

Author: Federico-PizarroBejarano

mm_control/CMakeLists.txt

@@ -8,9 +8,11 @@ catkin_python_setup()
 catkin_package()
 
 catkin_install_python(PROGRAMS
-            scripts/test_constraints.py
-            scripts/test_cost_functions.py
             scripts/mpc_generate_c_code.py
-            scripts/test_mpc.py
+            tests/test_constraints.py
+            tests/test_cost_functions.py
+            tests/test_mpc.py
+            tests/test_robot_model.py
+            tests/test_SO3.py
             DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
           )