Minor changes

CMakeLists.txt

@@ -15,7 +15,10 @@ find_package(catkin REQUIRED COMPONENTS
   as_msgs
 )
 
+find_package(Eigen3 REQUIRED)
+
 find_package(OpenMP REQUIRED)
+
 if(OPENMP_FOUND)
   message(STATUS "OPENMP FOUND")
   set(OpenMP_FLAGS ${OpenMP_CXX_FLAGS})

README.md

@@ -3,27 +3,17 @@
 <div align="center">
 <a> <img src="docs/mpc_black.png" alt="Logo" width="855"> </a>
 <h3 align="center">Tailored MPC</h3>
-<p align="left">
-        This pkg is thought to be the main AS controller for CAT15x. Within this repo you will find 3 different controllers (time-variant, spatial-variant and lateral (also time-variant)). The one used for the 2022-2023 season is the lateral approach, leaving the others for a future implementation.
-    <br />
-  </p>
 </div>
 
+Here you can find the main Autonomous Systems controller for CAT15x, the [BCN eMotorsport](https://bcnemotorsport.upc.edu) 2022-23 car. Within this repo you will find 3 different MPC controllers: time-variant, spatial-variant and lateral (also time-variant). The one used for the 2022-2023 season is the lateral approach, leaving the others for a future implementation as they demand much more testing time.
+
 <details>
     <summary>Table of Contents</summary>
     <ol>
         <li>
-        <a href="#getting-started">Getting Started</a>
-        <ul>
-            <li><a href="#usage">Usage</a></li>
-            <li><a href="#branches">Branches</a></li>
-        </ul>
+        <a href="#disclaimer">Disclaimer</a>
         </li>
-        <li><a href="#prerequisites">Prerequisites</a>
-        <ul>
-            <li><a href="#msg-dependencies">Msg dependencies</a></li>
-            <li><a href="#solver-dependencies">Solver dependencies</a></li>
-        </ul>
+        <li><a href="#dependencies">Dependencies</a>
         </li>
         <li>
         <a href="#approach">Approach</a>
@@ -34,18 +24,23 @@
     </ol>
 </details>
 
-# Getting Started
+# Disclaimer
+This is a tailored control solution made for the CAT15x Formula Student vehicle. In order to make a propper use of this algorithm, it's the user duty to make sure the [dynamic model](docs/tfg.pdf) actually approximates the behaviour of the car. 
+
+If you use this control algorithm in a Formula Student competition the **only** thing I ask for is to **ALWAYS REFERENCE** the team ___BCN eMotorsport___.
 
-## Usage
-In order to execute this pkg a [ForcesPro](https://forces.embotech.com/Documentation/index.html) software/hardware license is needed. If you don't have any license available read the __Solvers__ section in [AS README.md](https://bcnemotorsport.upc.edu:81/autonomous-systems-group/autonomous_systems).
+# Dependencies
+* [Ubuntu](https://ubuntu.com/) 20.04
+* [ROS](https://www.ros.org/) Noetic
+* [Embotech](https://www.embotech.com/products/forcespro/overview/) FORCESPRO solver. A Hardware or Software Embotech license is mandatory.
+* [Eigen3](https://eigen.tuxfamily.org)
+* ___as_msgs___: self-defined ROS msgs pkg. You may change it for yours, adapting the [necessary callbacks](include/mpc.hh).
 
-This pkg has a launch file for each dynamic event (or should have) because of the specific parametres used depending on the event. 
+# Approach
 
-An example for executing the autocross controller:
+For specific information on how the lateral controller work read [Tailored MPC](docs/tfg.pdf)'s paper.
 
-```sh
-roslaunch tailored_mpc autoX.launch
-```
+For the sake of simplicity the different controllers are named after their more important characteristic. However, all the specified MPC controllers are curvature-based and follow a simplified non linear bicycle model.
 
 ## Branches
 Here's a quick explanation of this project branches:
@@ -76,23 +71,11 @@ Here's a quick explanation of this project branches:
 
 __NOTE:__ The other branches are still in an early development process. They're not ready for deployment!
 
-# Prerequisites
-
-## Msg dependencies
-This pkg depends on the self-defined [as_msgs](https://bcnemotorsport.upc.edu:81/autonomous-systems-group/autonomous_systems). Make sure to have this msgs on your current workspace.
-
-## Solver dependecies
-As said before, this pkg depends on [Embotech](https://www.embotech.com/products/forcespro/overview/) ForcesPro solver so a Hardware/Software license is needed. See the __Solvers__ section from [AS README.md](https://bcnemotorsport.upc.edu:81/autonomous-systems-group/autonomous_systems) for more information.
-
-# Approach
-
-For specific information on how these controllers work read [TRO](docs/TRO.pdf), [Lateral MPC](docs/KIT_lateral_MPC.pdf) and [Curvature MPC](docs/curvature_MPC.pdf) papers.
-
-For the sake of simplicity the different controllers are named after their more important characteristic. However, all the specified MPC controllers are curvature-based and follow a simplified non linear bicycle model.
-
 # Workflow
 
-In order to solve the Non Linear Optimization Problem (NLOP) stated in [solver declaration](solver/FORCES_problem.m) the following procedure must be followed (in this order):
+Here's a quick summary of the steps taken by the algorithm in each iteration.
+
+In order to solve the Non Linear Optimization Problem (NLOP) defined with the [solver declaration API](solver/FORCES_problem.m) the following procedure is followed (in this order):
 
 * __Initial conditions__: the initial conditions for all the state/control variables must be computed. Usually the only unkown initial conditions are the path tracking variables `n` & `mu` so they must be calculated using the actual car state and the planned trajectory. 
 

solver/codeGen/TailoredSolver.m

@@ -1,4 +1,4 @@
-% TailoredSolver - a fast solver generated by FORCESPRO v6.1.0
+% TailoredSolver - a fast solver generated by FORCESPRO v6.3.0
 %
 %   OUTPUT = TailoredSolver(PARAMS) solves a multistage problem
 %   subject to the parameters supplied in the following struct:

solver/codeGen/TailoredSolver/TailoredSolver.m

@@ -1,4 +1,4 @@
-% TailoredSolver - a fast solver generated by FORCESPRO v6.1.0
+% TailoredSolver - a fast solver generated by FORCESPRO v6.3.0
 %
 %   OUTPUT = TailoredSolver(PARAMS) solves a multistage problem
 %   subject to the parameters supplied in the following struct:

solver/codeGen/TailoredSolver/include/TailoredSolver.h

@@ -19,7 +19,7 @@ jurisdiction in case of any dispute.
 
 */
 
-/* Generated by FORCESPRO v6.1.0 on Monday, May 15, 2023 at 4:52:58 PM */
+/* Generated by FORCESPRO v6.3.0 on Tuesday, December 19, 2023 at 2:31:19 PM */
 #ifndef TailoredSolver_H
 #define TailoredSolver_H
 
@@ -94,21 +94,6 @@ typedef double TailoredSolverinterface_float;
 /* maximum number of supported elements in the filter */
 #define MAX_FILTER_SIZE_TailoredSolver	(150) 
 
-/* maximum number of supported elements in the filter */
-#define MAX_SOC_IT_TailoredSolver			(4) 
-
-/* desired relative duality gap */
-#define SET_ACC_RDGAP_TailoredSolver		(TailoredSolver_float)(0.0001)
-
-/* desired maximum residual on equality constraints */
-#define SET_ACC_RESEQ_TailoredSolver		(TailoredSolver_float)(1E-06)
-
-/* desired maximum residual on inequality constraints */
-#define SET_ACC_RESINEQ_TailoredSolver	(TailoredSolver_float)(1E-06)
-
-/* desired maximum violation of complementarity */
-#define SET_ACC_KKTCOMPL_TailoredSolver	(TailoredSolver_float)(1E-06)
-
 /* whether callback return values should be checked */
 #define EXTFUNC_RETURN_TailoredSolver (0)
 
@@ -308,10 +293,12 @@ typedef struct
 
 
 /* SOLVER FUNCTION DEFINITION -------------------------------------------*/
-/* Time of Solver Generation: (UTC) Monday, May 15, 2023 4:53:02 PM */
-/* User License expires on: (UTC) Monday, September 25, 2023 10:00:00 PM (approx.) (at the time of code generation) */
-/* Solver Static License expires on: (UTC) Monday, September 25, 2023 10:00:00 PM (approx.) */
-/* Solver Id: 5ed8da90-0e85-42f3-a965-89843d08c984 */
+/* Time of Solver Generation: (UTC) Tuesday, December 19, 2023 2:31:26 PM */
+/* User License expires on: (UTC) Sunday, September 8, 2024 10:00:00 PM (approx.) (at the time of code generation) */
+/* Solver Static License expires on: (UTC) Sunday, September 8, 2024 10:00:00 PM (approx.) */
+/* Solver Id: d9462ecc-09c0-4c1a-841c-e467c14e3df8 */
+/* Host Compiler Version: d76500f0 */
+/* Target Compiler Version: d76500f0 */
 /* examine exitflag before using the result! */
 #ifdef __cplusplus
 extern "C" {

solver/codeGen/TailoredSolver/include/TailoredSolver_memory.h

@@ -19,7 +19,7 @@ jurisdiction in case of any dispute.
 
 */
 
-/* Generated by FORCESPRO v6.1.0 on Monday, May 15, 2023 at 4:52:58 PM */
+/* Generated by FORCESPRO v6.3.0 on Tuesday, December 19, 2023 at 2:31:19 PM */
 #ifndef TailoredSolver_MEMORY_H
 #define TailoredSolver_MEMORY_H
 

solver/codeGen/TailoredSolver/interface/TailoredSolver.m

@@ -1,4 +1,4 @@
-% TailoredSolver - a fast solver generated by FORCESPRO v6.1.0
+% TailoredSolver - a fast solver generated by FORCESPRO v6.3.0
 %
 %   OUTPUT = TailoredSolver(PARAMS) solves a multistage problem
 %   subject to the parameters supplied in the following struct:

solver/codeGen/TailoredSolver/interface/TailoredSolverBuildable.m

@@ -50,7 +50,7 @@ function updateBuildInfo(buildInfo, cfg)
             end
         end
 
-        function [output,exitflag,info] = forcesInitOutputsMatlab()
+        function [output, exitflag, info] = forcesInitOutputsMatlab()
             infos_it = coder.nullcopy(zeros(1, 1));
             infos_it2opt = coder.nullcopy(zeros(1, 1));
             infos_res_eq = coder.nullcopy(zeros(1, 1));
@@ -104,7 +104,7 @@ function updateBuildInfo(buildInfo, cfg)
             [output,exitflag,info] = TailoredSolverBuildable.forcesCall(params.lb, params.ub, params.hu, params.xinit, params.x0, params.all_parameters, params.num_of_threads);
         end
 
-        function [output,exitflag,info] = forcesCall(lb, ub, hu, xinit, x0, all_parameters, num_of_threads)
+        function [output, exitflag, info] = forcesCall(lb, ub, hu, xinit, x0, all_parameters, num_of_threads)
             solvername = 'TailoredSolver';
 
 
@@ -116,14 +116,14 @@ function updateBuildInfo(buildInfo, cfg)
                             'all_parameters', double(all_parameters),...
                             'num_of_threads', uint32(num_of_threads));
 
-            [output_c, exitflag_c, info_c] = TailoredSolverBuildable.forcesInitOutputsC();
+            [output_c, exitflag_c, info_c] = TailoredSolverBuildable.forcesInitOutputsC(); %#ok<ASGLU>
 
             headerName = [solvername '.h'];
             coder.cinclude(headerName);
             coder.cinclude([solvername '_memory.h']);
             coder.cinclude([solvername '_adtool2forces.h']);
             % define memory pointer
-            memptr = coder.opaque([solvername '_mem *'], 'HeaderFile', headerName);
+            memptr = coder.opaque([solvername '_mem *'], 'HeaderFile', headerName); %#ok<NASGU>
             memptr = coder.ceval([solvername '_internal_mem'], uint32(0));
             % define solver input information (params, file and casadi)
             coder.cstructname(params, [solvername '_params'], 'extern', 'HeaderFile', headerName);
@@ -168,7 +168,7 @@ function updateBuildInfo(buildInfo, cfg)
     end
 
     methods (Static, Access = private)
-        function [output,exitflag,info] = forcesInitOutputsC()
+        function [output, exitflag, info] = forcesInitOutputsC()
             infos_it = coder.nullcopy(int32(zeros(1, 1)));
             infos_it2opt = coder.nullcopy(int32(zeros(1, 1)));
             infos_res_eq = coder.nullcopy(double(zeros(1, 1)));

solver/codeGen/TailoredSolver/interface/TailoredSolver_build.py

@@ -17,6 +17,7 @@
 #jurisdiction in case of any dispute.
 #
 from distutils.ccompiler import new_compiler
+from distutils import unixccompiler
 c = new_compiler()
 #from numpy.distutils.intelccompiler import IntelCCompiler
 #c = IntelCCompiler()
@@ -43,7 +44,7 @@
 
 # compile into object file
 objdir = os.path.join(os.getcwd(),"TailoredSolver","obj")
-if isinstance(c,distutils.unixccompiler.UnixCCompiler):
+if isinstance(c,unixccompiler.UnixCCompiler):
 	#objects = c.compile([sourcefile], output_dir=objdir, extra_preargs=['-O3','-fPIC','-fopenmp','-mavx'])
 	objects = c.compile([sourcefile], output_dir=objdir, extra_preargs=['-O3','-fPIC','-mavx'])
 	if sys.platform.startswith('linux'):