Feature/local planner mpc cpp #83
Conversation
…yper overdamped in the future for safety reasons. but im now starting the attack run on introducing c++ port
Jawad12256
left a comment
Jawad12256
left a comment
There was a problem hiding this comment.
lmk if any issues or qs.
|
|
||
| Eigen::Vector2d obs_future = obs_pos + obs_vel * 0.1; | ||
| Eigen::Vector2d diff = current_state.head<2>() - obs_future; | ||
| double dist = diff.norm(); |
There was a problem hiding this comment.
If this diff is (0,0) it can lead to division by zero errors. Also if the diff is very small like (1e-12, 1e-11) and it's affected by noise, you will have a very large acceleration in a potentially undesirable direction which might cause issues. Suggestion:
if (dist > 1e-6) {
repulsion += diff / dist * force_mag;
}
| for (const auto& obs : obstacles) { | ||
| Eigen::Vector2d obs_pos(obs[0], obs[1]); | ||
| Eigen::Vector2d obs_vel(obs[2], obs[3]); | ||
| double radius = obs[4]; |
There was a problem hiding this comment.
Add safeguard to double check obstacle vector size >= 5. Suggestion:
if (obs.size() < 5) continue;
| // 2. Calculate Target Velocity | ||
| Eigen::Vector2d ref_vel = Eigen::Vector2d::Zero(); | ||
|
|
||
| if (is_arriving || dist_to_goal < 0.15) { |
There was a problem hiding this comment.
|| dist_to_goal < 0.15 is redundant because you already are checking dist_to_goal < 0.40 with is_arriving
idk if you want to set it as two fences at 0.15 and 0.40, maybe something like:
<0.15 --> ref vel set to zero
0.15<dist<0.40 --> ref vel set the same as the else, and then multiplied by coefficient of 0.5
>0.4 --> (same as existing else block)
|
|
||
| if (dist < safety * 1.2) { | ||
| double violation = std::max(0.0, safety * 1.2 - dist); | ||
| double force_mag = 50.0 * std::exp(violation * 10.0); |
There was a problem hiding this comment.
Move 1.2, 50.0 and 10.0 to MPCConfig under names safety_margin_coeff, repulsion_strength and exponential_factor respectively
| #include <iostream> | ||
| #include <cmath> | ||
| #include <algorithm> | ||
|
|
There was a problem hiding this comment.
Probably worth documenting something like:
"This class is a heuristic approximation of the full MPC solver and shares its interface and configuration, but does not solve an optimization problem."
| Eigen::Vector2d obs_vel(obs[2], obs[3]); | ||
| double radius = obs[4]; | ||
|
|
||
| Eigen::Vector2d obs_future = obs_pos + obs_vel * 0.1; |
There was a problem hiding this comment.
Python MPC predicts it like this:
time_k = k * dt
ox_k = ox_start + ovx * time_k
but the C++ implementation looks to be independent of DT, so to make the behaviours match you could try something like
double lookahead = config.DT;
obs_future = obs_pos + obs_vel * lookahead;
| double robot_radius = 0.09; | ||
| double obstacle_buffer_ratio = 1.25; | ||
| double safety_vel_coeff = 0.15; | ||
| }; |
There was a problem hiding this comment.
Add the extra config variables here (see the cpp file for my comments on that)
| self._slow_planner = BisectorPlanner(game, friendly_colour, env) | ||
| self._game = game | ||
| self._fast_planner = DynamicWindowPlanner(game) | ||
| self._fast_planner = OmnidirectionalMPC() |
There was a problem hiding this comment.
CRITICAL: OmnidirectionalMPC() doesn't seem to have a path_to() method that will return a 'plan' in the correct format. You can see further down in the code it's expecting to get the "waypoint" attribute of whatever path_to() returns. Some code rewriting needed to properly integrate the outputs of OmnidirectionalMPC() controller
|
|
||
| # Avoid division by zero | ||
| fps = 60.0 / time_diff if time_diff > 0 else 0.0 | ||
| avg_latency_ms = (_total_compute_time / 60) * 1000 |
There was a problem hiding this comment.
Why are we dividing by 60? perf_counter gives _total_compute_time in seconds, so should just be:
avg_latency_ms = _total_compute_time * 1000
| if is_arriving or curr_dist < 0.15: | ||
| ref_vel = np.zeros(2) | ||
| else: | ||
| dir_vec = (np.array(goal_pos) - current_state[0:2]) / curr_dist |
There was a problem hiding this comment.
Needs a divide by zero safeguard. Suggestion:
if curr_dist > 1e-6:
dir_vec = (goal_pos - current_state[0:2]) / curr_dist
else:
dir_vec = np.zeros(2)
Code Review for Shi Hao 12/01/2026