From 203220501aba80e3586e6e54174b8441c513a146 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Christoph=20R=C3=B6smann?=
 <christoph.roesmann@tu-dortmund.de>
Date: Tue, 26 May 2020 18:53:55 +0200
Subject: [PATCH] hybrid costs added

---
 .../cfg/test_mpc_optim_node.yaml              |  1 +
 mpc_local_planner/src/controller.cpp          | 25 ++++++++++++++++++-
 2 files changed, 25 insertions(+), 1 deletion(-)

diff --git a/mpc_local_planner/cfg/test_mpc_optim_node.yaml b/mpc_local_planner/cfg/test_mpc_optim_node.yaml
index 1df3b8d..eccb51e 100644
--- a/mpc_local_planner/cfg/test_mpc_optim_node.yaml
+++ b/mpc_local_planner/cfg/test_mpc_optim_node.yaml
@@ -65,6 +65,7 @@ planning:
       state_weights: [2.0, 2.0, 2.0]
       control_weights: [1.0, 1.0]
       integral_form: False
+      hybrid_cost_minimum_time: False
     minimum_time_via_points:
       position_weight: 8.0
       orientation_weight: 0.0
diff --git a/mpc_local_planner/src/controller.cpp b/mpc_local_planner/src/controller.cpp
index 964a0c1..ce489db 100644
--- a/mpc_local_planner/src/controller.cpp
+++ b/mpc_local_planner/src/controller.cpp
@@ -22,6 +22,7 @@
 
 #include <mpc_local_planner/controller.h>
 
+#include <corbo-optimal-control/functions/hybrid_cost.h>
 #include <corbo-optimal-control/functions/minimum_time.h>
 #include <corbo-optimal-control/functions/quadratic_control_cost.h>
 #include <corbo-optimal-control/structured_ocp/discretization_grids/finite_differences_variable_grid.h>
@@ -591,14 +592,36 @@ corbo::StructuredOptimalControlProblem::Ptr Controller::configureOcp(const ros::
         }
         bool integral_form = false;
         nh.param("planning/objective/quadratic_form/integral_form", integral_form, integral_form);
+        bool hybrid_cost_minimum_time = false;
+        nh.param("planning/objective/quadratic_form/hybrid_cost_minimum_time", hybrid_cost_minimum_time, hybrid_cost_minimum_time);
+
         bool q_zero = Q.isZero();
         bool r_zero = R.isZero();
         if (!q_zero && !r_zero)
+        {
+            PRINT_ERROR_COND(hybrid_cost_minimum_time,
+                             "Hybrid minimum time and quadratic form cost is currently only supported for non-zero control weights only. Falling "
+                             "back to quadratic form.");
             ocp->setStageCost(std::make_shared<QuadraticFormCostSE2>(Q, R, integral_form, lsq_solver));
+        }
         else if (!q_zero && r_zero)
+        {
+            PRINT_ERROR_COND(hybrid_cost_minimum_time,
+                             "Hybrid minimum time and quadratic form cost is currently only supported for non-zero control weights only. Falling "
+                             "back to only quadratic state cost.");
             ocp->setStageCost(std::make_shared<QuadraticStateCostSE2>(Q, integral_form, lsq_solver));
+        }
         else if (q_zero && !r_zero)
-            ocp->setStageCost(std::make_shared<corbo::QuadraticControlCost>(R, integral_form, lsq_solver));
+        {
+            if (hybrid_cost_minimum_time)
+            {
+                ocp->setStageCost(std::make_shared<corbo::MinTimeQuadraticControls>(R, integral_form, lsq_solver));
+            }
+            else
+            {
+                ocp->setStageCost(std::make_shared<corbo::QuadraticControlCost>(R, integral_form, lsq_solver));
+            }
+        }
     }
     else if (objective_type == "minimum_time_via_points")
     {