12345qiupeng
/
mpc_local_planner
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

hybrid costs added

master
Christoph Rösmann 2020-05-26 18:53:55 +02:00
parent ec6a0c4406
commit 203220501a
2 changed files with 25 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
mpc_local_planner/cfg/test_mpc_optim_node.yaml
View File

@ -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

23
mpc_local_planner/src/controller.cpp
View File

@ -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,15 +592,37 @@ 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)
{
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")
{
bool via_points_ordered = false;