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

fixed wrong angular computation in midpoint differences

master
Christoph Rösmann 2020-05-20 11:03:09 +02:00
parent a77e1cbcc8
commit 1dbc6e122d
1 changed files with 4 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
mpc_local_planner/include/mpc_local_planner/optimal_control/fd_collocation_se2.h
View File

@ -94,7 +94,10 @@ class MidpointDiffCollocationSE2 : public corbo::FiniteDifferencesCollocationInt
assert(error.size() == x1.size());
assert(dt > 0 && "dt must be greater then zero!");
system.dynamics(0.5 * (x1 + x2), u1, error);
Eigen::VectorXd midpoint = 0.5 * (x1 + x2);
// fix angular component
midpoint.coeffRef(2) = interpolate_angle(x1.coeffRef(2), x2.coeffRef(2), 0.5);
system.dynamics(midpoint, u1, error);
error.head(2) -= (x2.head(2) - x1.head(2)) / dt;
error.coeffRef(2) -= normalize_theta(x2.coeffRef(2) - x1.coeffRef(2)) / dt;
if (x1.size() > 3)