Comments (7)
increase prune dist to help with path alignment?
from mppic.
I found the issue, the last critic to still use the path ratio metric is the PathFollow critic making it only score intermittently. Since we prune the path based on costmap size, that metric is bad measure of the distance to the actual goal pose which isn't going to be on the path at all for most of the navigation request. I'm going to update it with the measure I replaced all the other critics with and then it should score more regularly to be retuned in proportion to the Goal critic to balance driving to the goal at the end of the pruned path with the mid-point from the PathFollow offset. Maybe even remove the Goal Critic from being used outside of approach to goal.
EZ-PZ. Retuning will be difficult(ish) but should have a solution to this in #99 shortly in a couple of days. Easy code fix, just need to make sure it doesn't introduce other issues and retune for reasonable non-short-cutting behavior.
from mppic.
Mhm, well fixing the PathFollow helps, but I now see that the path align critic has some very strange features. With it removed, the robot moves much slower and actually tracks the path more closely. I need to dig into why the PathAlign critic has any forward-driving characteristics at all (and if there are some issues here why its not aligning as well with the path).
Edit: it seems like the if(segment_short(s))
line is called for nearly all things, which is very strange and means that the distance metrics from the path are probably not right if we're not actually computing u
, so its not surprising to me then that the path align critic isn't fully working. Something's not quite right here.
const auto && P2_P1_norm_sq = xt::eval(P2_P1_dx * P2_P1_dx + P2_P1_dy * P2_P1_dy);
const auto segment_short = P2_P1_norm_sq < 1e-3f;
These seem pretty straight forwardly correct and path points should be ~5cm apart, not less than 1mm.
Edit2: sqrt(1e-3) = 0.032 which is suspiciously close to what the actual path resolution might be. I lowered this to 1e-6 so that its 1mm of non-square distance apart. This may have been an oversight I made at some point when optimizing for reducing sqrts. That seems to make us actually now evaluate u
but still has a forward driving force I don't understand and isn't well aligning the path even with a high weight
Edit3: the forward-driving characteristics are because the PathAlign population of furthest_reached_path_point
is higher than that of the setPathFurthestPointIfNotSet
for use in the other critics. So when that's not used, then the value is different impacting those critics functions. That's something to look into later. Still trying to figure out why it is the path align critic seems to do such a poor job of path-alignment. Whether I set the weight to 0 or 5000, I see little to no difference.
from mppic.
Found it! Obstacle critic was set to cost = <cost>
instead of +=
so everything prior to it was being zeroed out and not counted (e.g. Path Align). With that patch, we get its results considered now. Will now retune tomorrow and find the bug in setPathFurthestPointIfNotSet
/ furthest_reached_path_point
from mppic.
@artofnothingness I found that in the PathAlign critic, the condition of u < 0 or u >= 1
represents >99% of the samples distances taken for the shortest distance to the path. That means all the interpolation stuff we're doing isn't even being functionally used. I think that means we can simply this critic to be simply path_points - trajectory_points
and find the minimum for the axes without the need for any of what this critic is doing. And potentially removing the 3x nested for loop which should speed things up here.
Even without tensorifying it, just removing all the U stuff makes it run at 30hz with 2000 samples with every single path and trajectory point with no appreciable change in path tracking quality 😄 Tensorified can be simplified to
const auto T_x = xt::view(data.trajectories.x, xt::all(), xt::all(), xt::newaxis());
const auto T_y = xt::view(data.trajectories.y, xt::all(), xt::all(), xt::newaxis());
const auto P_x = xt::view(data.path.x, xt::range(_, -1)); // path points
const auto P_y = xt::view(data.path.y, xt::range(_, -1)); // path points
const size_t time_steps = T_x.shape(1);
auto diffs_x = T_x - P_x;
auto diffs_y = T_y - P_y;
// size should = (batches, timesteps, subtraction values)
auto && dists_sq = xt::eval(diffs_x * diffs_x + diffs_y * diffs_y);
auto mins = xt::amin(dists_sq, {1}); // size (batches, timesteps)
auto mean_dists = xt::sum(xt::sqrt(mins), {1}, immediate) / time_steps; // size (batches)
data.costs += xt::pow(mean_dists * weight_, power_);
Which also considers all points without needing steps. I'm ultimately going to go with the non-tensor version though - the amin
takes a ton of time and I'm not sure why. It takes a solid 12 ms by itself putting it back in the range of the old performance and a bit worse. Non-tensor version runs in << 1ms and puts the typical steady-state run rate at ~70hz with 2000 samples. Maybe there's something I'm doing wrong here, but ultimately its probably not worth thrashing over for days on end if I have another solution that works well.
from mppic.
The shortcutting also appears to be from the Path Angle critic - since we look very far ahead for the path angle to apply the critic, it can create shortcutting. Its needed to balance a local minima condition, but I've shown by removing it (without triggering that condition) path tracking increases significantly. Some reworking here will likely be all we need in conjunction with the changes above to call this a wrap.
from mppic.
#106 to resolve
from mppic.
Related Issues (20)
- Use integrated path distance for path follower point in path follow and path angle HOT 1
- Staging for additional to Nav2 project HOT 2
- Motion model have no implementation? HOT 3
- lack benchmark
- Nav2 MPPI Controller Merged - See Project HOT 2
- migration to ros2 humble? HOT 2
- to use log mppi with nav2 HOT 1
- Compiling error on 22.04/Rolling
- Require model_dt = control_frequency? HOT 3
- Extremely poor performance in master HOT 1
- Optimize Code to run real-time by removing or reducing CPU jitter HOT 44
- CMake Error at benchmark/CMakeLists.txt:1 (find_package): HOT 2
- Segmentation fault with Controller Server caused by Xtensor(?) HOT 25
- Add rosdep keys for libxsimd-dev HOT 1
- Approach to Goal Behavior/Smoothness/Converging HOT 8
- Achieving full speed on straight-a-ways HOT 18
- Support reversing when path includes reversing segments with the Smac Planners (or other feasible planners) HOT 1
- Consider log-form distribution HOT 7
- Add in some small number of samples around zero-mean HOT 1
Recommend Projects
-
React
A declarative, efficient, and flexible JavaScript library for building user interfaces.
-
Vue.js
🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
-
Typescript
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
-
TensorFlow
An Open Source Machine Learning Framework for Everyone
-
Django
The Web framework for perfectionists with deadlines.
-
Laravel
A PHP framework for web artisans
-
D3
Bring data to life with SVG, Canvas and HTML. 📊📈🎉
-
Recommend Topics
-
javascript
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
-
web
Some thing interesting about web. New door for the world.
-
server
A server is a program made to process requests and deliver data to clients.
-
Machine learning
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
-
Visualization
Some thing interesting about visualization, use data art
-
Game
Some thing interesting about game, make everyone happy.
Recommend Org
-
Facebook
We are working to build community through open source technology. NB: members must have two-factor auth.
-
Microsoft
Open source projects and samples from Microsoft.
-
Google
Google ❤️ Open Source for everyone.
-
Alibaba
Alibaba Open Source for everyone
-
D3
Data-Driven Documents codes.
-
Tencent
China tencent open source team.
from mppic.