artofnothingness / mppic Goto Github PK
View Code? Open in Web Editor NEWLicense: MIT License
License: MIT License
Seems like they are using nav stack
Too high move forward cost after addition prefer forward critic i guess
I don't know what happened, but all the sudden the robots run directly into walls and when the path replans, there's too big of an angle and the robot stops dead in its tracks rotates a bit, then continues and ends up in loops of that 3-4 times in a row.
I'm using just the default parameter we have in the readme file that I've been using when testing the PRs pending, clearly something significant has changed in the last couple of PRs but I don't know what.
I also upgraded to 22.04/Rolling. Note ros-navigation/navigation2#3014 but I'm seeing it run straight into static obstacles in the static map that is showing in the costmap fine.
See #77 (comment)
A good place to start is a critic to weight going the full speed, with probably a low weight to just incentivize but not force.
I see this PR in the other repo FastSense/mppic#11
Is this worth pulling in here? While I haven't tested yet, it seemed like reversing was already not particularly a problem -- but I could be wrong if you're starting at a path heading the opposite from the robot heading so reversing would allow tracking the path without turning around.
Hi
Yeah i mean to use log-mppi in nav2, specially in humble edition, i've changed noise part
But due to the log-mppi from ros1, https://github.com/IhabMohamed/log-MPPI_ros/blob/main/mppi_control/launch/mppi_launcher.launch,
i should change the std part and lambda part
The thing i want to ask you is when using log-mppi in ros2 humble with nav2, i guess i have to change the parameters of these
vx_std: 0.5
vy_std: 0.5
wz_std: 0.8
temperature: 0.25
but i don't have any idea to well form the parameter values
Ant suggestions?
Best regards
Just some thoughts came up
Now ref critic is about O(BTP) (batch_size x time_step x path_len)
I start doubting that we really need critic that penalize exact distance between current batch point and path line segment (between path points)
Probably local goal critic with some offset plus simple goal critic would be enough.
Plus consider local goal as the farthest nearest goal to batches last point (now we searching for farthest nearest goal to all batches points)
I'm seeing this after pulling in the latest changes on develop
Starting >>> mppic
--- stderr: mppic
Traceback (most recent call last):
File "/opt/ros/rolling/share/ament_cmake_core/cmake/package_templates/templates_2_cmake.py", line 21, in <module>
from ament_package.templates import get_environment_hook_template_path
ModuleNotFoundError: No module named 'ament_package'
CMake Error at /opt/ros/rolling/share/ament_cmake_core/cmake/ament_cmake_package_templates-extras.cmake:41 (message):
execute_process(/usr/bin/python3.8
/opt/ros/rolling/share/ament_cmake_core/cmake/package_templates/templates_2_cmake.py
/home/steve/Documents/nav2_ws/build/mppic/ament_cmake_package_templates/templates.cmake)
returned error code 1
Call Stack (most recent call first):
/opt/ros/rolling/share/ament_cmake_core/cmake/ament_cmake_coreConfig.cmake:41 (include)
/opt/ros/rolling/share/ament_cmake/cmake/ament_cmake_export_dependencies-extras.cmake:15 (find_package)
/opt/ros/rolling/share/ament_cmake/cmake/ament_cmakeConfig.cmake:41 (include)
CMakeLists.txt:32 (find_package)
Probably more path follow + understanding why goal and path follow are so sensitive for forward driving forces
That would help with stability if users couldn't define some model_dt != control_frequency so when we apply the control shift it doesn't shift by the right amount as the next starting point. We should just straight up require that to be true
I added simple profiling mechanism on each critic in critic manager in art branch, but i think we need to have more general and involved approach
This controller has been updated and merged into Nav2: https://navigation.ros.org/configuration/packages/configuring-mppic.html
As such, while this project repository is absolutely critical and led to its inclusion into Nav2 and to the amazing work and efforts by @artofnothingness, the version in Nav2's repository fixes a number of bugs major and minor and will continue to receive community improvements and be maintained alongside the rest of the navigation stack into the future.
Was planning to do some tuning for omni robots, while building got an issue.
ros2_nav_ws/src/mppic/src/critics/path_align_critic.cpp:112:16: error: unused variable ‘is_path_longer_than_trajectory’ [-Werror=unused-variable]
112 | bool is_path_longer_than_trajectory = true;
Of course I can give a PR. but thought should report it here as well.
But maybe this variable was planned for something ?
Thread 16 "controller_serv" received signal SIGSEGV, Segmentation fault.
[Switching to Thread 0x7fffc2ffd700 (LWP 23325)]
0x00007fffe43a4997 in auto xt::reduce_immediate<xt::xreducer_functors<xt::norm_sq<xt::xtensor_container<xt::uvector<double, xsimd::aligned_allocator<double, 32ul> >, 2ul, (xt::layout_type)1, xt::xtensor_expression_tag>&, std::array<unsigned long, 1ul>, std::tuple<xt::evaluation_strategy::immediate_type>, 0>(xt::xtensor_container<xt::uvector<double, xsimd::aligned_allocator<double, 32ul> >, 2ul, (xt::layout_type)1, xt::xtensor_expression_tag>&, std::array<unsigned long, 1ul>&&, std::tuple<xt::evaluation_strategy::immediate_type>)::{lambda(double const&, double const&)#1}, xt::const_value<double>, std::plus<double> >, xt::xtensor_container<xt::uvector<double, xsimd::aligned_allocator<double, 32ul> >, 2ul, (xt::layout_type)1, xt::xtensor_expression_tag>&, std::array<unsigned long, 1ul>, std::tuple<xt::evaluation_strategy::immediate_type>&>(xt::xtensor_container<xt::uvector<double, xsimd::aligned_allocator<double, 32ul> >, 2ul, (xt::layout_type)1, xt::xtensor_expression_tag>&, std::array<unsigned long, 1ul>&&, std::array<unsigned long, 1ul>&&, std::tuple<xt::evaluation_strategy::immediate_type>&) ()
from /home/neobotix/mp_400_workspace/install/mppic/lib/libcritics.so
This happens when the controller tries to reach the goal location at a goal angle roughly around >90 degrees from the start orientation. Note that, I have an high weight cost for prefer_forward_critic
...
Parameters are as follows:
FollowPath:
plugin: "mppi::Controller"
time_steps: 15
model_dt: 0.1
batch_size: 300
vx_std: 0.2
vy_std: 0.2
wz_std: 1.0
vx_max: 0.5
vy_max: 0.5
wz_max: 0.7
iteration_count: 1
prune_distance: 1.2
transform_tolerance: 0.1
temperature: 0.25
motion_model: "DiffDrive"
visualize: false
AckermannConstrains:
min_turning_r: 0.
critics: ["ObstaclesCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic" ]
GoalCritic:
enabled: true
cost_power: 1
cost_weight: 4.0
GoalAngleCritic:
enabled: true
cost_power: 1
cost_weight: 3.0
threshold_to_consider_goal_angle: 0.20
ObstaclesCritic:
enabled: true
cost_power: 2
cost_weight: 1.25
consider_footprint: false
collision_cost: 2000.0
PathAlignCritic:
enabled: true
cost_power: 1
cost_weight: 2.0
path_point_step: 1
trajectory_point_step: 2
PathFollowCritic:
enabled: true
cost_power: 1
cost_weight: 2.0
offset_from_furthest: 6
max_path_ratio: 0.40
PathAngleCritic:
enabled: true
cost_power: 1
cost_weight: 2.0
offset_from_furthest: 4
PreferForwardCritic:
enabled: true
cost_power: 1
cost_weight: 20.0
let me know, if you need more details!
Penalize for difference between trajectories velocities (mean?), and maximum velocity
CMake Error at benchmark/CMakeLists.txt:1 (find_package):
By not providing "Findbenchmark.cmake" in CMAKE_MODULE_PATH this project
has asked CMake to find a package configuration file provided by
"benchmark", but CMake did not find one.
Could not find a package configuration file provided by "benchmark" with
any of the following names:
Is there something missing in readme after the updates?
I'm trying in Galactic..
Sorry if I missed something blindly!
While robot moving try to change dynamic param. Get this error
[container_isolated-6] [INFO] [1649365719.116922575] [lifecycle_manager_navigation]: Have not received a heartbeat from controller_server.
[component_container_isolated-6] [ERROR] [1649365719.116979220] [lifecycle_manager_navigation]: CRITICAL FAILURE: SERVER controller_server IS DOWN after not receiving a heartbeat for 4000 ms. Shutting down related nodes
Reproduced on MPPI and DWB
Add the installation prefix of "benchmark" to CMAKE_PREFIX_PATH or set
"benchmark_DIR" to a directory containing one of the above files. If
"benchmark" provides a separate development package or SDK, be sure it has
been installed.
what should I do to solve this problem
It seems to me that sometimes robot does a little wobbles following the path
From the gif, it would be nice if the Omni had some preference for moving forward or not "twirling" while moving
https://github.com/ros-planning/navigation2/blob/main/nav2_dwb_controller/dwb_critics/src/twirling.cpp
https://github.com/ros-planning/navigation2/blob/main/nav2_dwb_controller/dwb_critics/src/prefer_forward.cpp
I think prefer forward would be the opposite of #2 so perhaps that's not needing explicit separation for Omni if that is created. I would think it would be preferable to use mostly X velocities and Y when needed, but I'm not actually sure. I haven't worked with Omni robots before in a deployed setting. It might be good to chat with other users
Support maximum curvature contraint on controls so that the system is able to work with ackermann vehicles
nav2_mppi_controller
)Sometimes robot does quite dangerous moves near obstacle
todo:
discuss current impl of obstacle critic and possible rework. May be just change temperature/costs is enough ?
Looking over the critics
Maybe it would be good to normalize these terms so that when we assign the weights its not proportional to absolute values? Let me know what you think, this is just a suggestion. I don't think its critical but a nice to have.
But more consequentially, for the differences in angles, how are the angles normalized? They are setup as abs(A - B)
which doesn't protect against wrap around to represent the shortest distance? ros/angles
can help here https://github.com/ros/angles/blob/master/angles/include/angles/angles.h#L103-L106. That seems like more of a potential issue. Also in the prefer forward critic as well.
It's ok that we somehow estimate effectiveness of this controller, but having some test scenarios with metrics, would help alot with estimating this more precisely
Hi. i'm really interested in your works
is there any version of log mppi which have migrated to ros2 humble?
Best regards :)
I think that one might be a little more involved, but its nothing crazy I suspect.
I think we need to not use some critics in close proximity to the goal & make some minimum spacing requirements for the trajectory points, else on approach all 60 time steps or whatever are going to be in the span of a few centimeters, which I suspect is the cause of issues getting right up to the goal. I think we need to make some special rules about on approach to the final target.
But we should find root cause of this behavior, but auditing critics to see what are used on approach to the goal and using only the goal-driving critics would be a good place to start. If its not the critics (unless bug in implementation of goal critics?), then its the trajectories most likely that need some special rules to get to a stop at a pose
Essentially, some of the planners in Nav2 are kinematically feasible containing specific points to reverse direction to achieve some path. It is differentiated by having a cusp point and the direction of the arrows on the path are reversed. So the path yaws will be pointing in the opposite direction than a forward-moving request.
I'd like to make sure this can respect those, approximately speaking. The 3 tests:
Though, this could be something to be dealt with at a later date. Just wanted it documented. This is the lowest priority of the remaining tickets
I'm trying to use the "Omni" option for the motion_model parameter, but I get this error shown in the issue topic, when I try to send the goal. I just use the same parameters from the readme except the motion model.
Is there something, I'm missing?
In file included from /opt/ros/rolling/include/rclcpp_lifecycle/rclcpp_lifecycle/lifecycle_node.hpp:996,
from /home/steve/Documents/nav2_ws/src/mppic/include/mppic/parameters_handler.hpp:15,
from /home/steve/Documents/nav2_ws/src/mppic/src/parameters_handler.cpp:2:
/opt/ros/rolling/include/rclcpp_lifecycle/rclcpp_lifecycle/lifecycle_node_impl.hpp: In member function ‘void mppi::ParametersHandler::getParam(SettingT&, const string&, ParamT, mppi::ParameterType) [with SettingT = bool; ParamT = bool]’:
/opt/ros/rolling/include/rclcpp_lifecycle/rclcpp_lifecycle/lifecycle_node_impl.hpp:256:21: warning: ‘param_in’ may be used uninitialized [-Wmaybe-uninitialized]
256 | rclcpp::Parameter param(name, parameter);
| ^~~~~
In file included from /home/steve/Documents/nav2_ws/src/mppic/src/parameters_handler.cpp:2:
/home/steve/Documents/nav2_ws/src/mppic/include/mppic/parameters_handler.hpp:136:10: note: ‘param_in’ was declared here
136 | ParamT param_in;
| ^~~~~~~~
Right now, we use the offsets from the furthest reachable point. That will obviously change depending on the number of trajectory points and their dt for the distance that represents.
If we make it a distance in front of the furthest point, integrated along path length (and find the nearest point), then we can generalize it from other settings and give a physical meaning to the parameter.
Also, since those critics score based on distance from the last point to the path point, it is directly analogous to the cost in the objective function.
todo
Highest compute portions of the program at ~20, 40, 12, 20%, respectively (with different base measurements, but illustrative of their relative size to others)
The gifs are 27mb and 4mb, respectively. This is quite alot for cloning, it would be good to compress these to be smaller so that we don't have 31mb to download for the source
After talking to @tkkim-robot about the issues wrt #80 (due to his expertise as the first-author of the SMPPI paper), he mentioned in passing adding some zero-mean samples into our optimization problem. With more research, I think this is a good idea.
Algorithm 1 on page 8 of this paper introduces the idea of having some trajectories of just noise values without adding in the previous optimal control.
This enables the IT-MPC algorithm to reset itself if a disturbance destroys the effectiveness of the previously computed control sequence
Basically, if we get in a bad spot, some zero-mean samples can help reset the optimization problem softly to continue making progress. We have a heavier handed version of this where we reset the optimization problem when all of the samples are in collision, but won't until all are in collision. This helps potentially for the bad-but-not-terrible case where many are in collision but not strictly all.
Additionally, on occasion I see this problem:
Where we go down the "wrong" path solution space because we're going full-speed towards 1 goal and it was changed to another direction. Noises from the previous optimal trajectory won't have the robot slow and stop, because it can reach around the pillar in another solution space to try to follow the path.
I think something like this could remove that behavior, or at least have it only when it truly is just better by providing some options near the robot to follow the path more directly. I am concerned about feasibility of this kind of thing though - we'd be going from full speed potentially to 0. We have velocity smoothers below the controllers to help smooth out infeasible requests, but full-speed to zero (or even reversing) would be an extreme case and may require several smoothed-time steps for the robot to comply with MPPI's request. I suppose that happens right now when we do a hard reset, but those are few and far between and analog to a critical failure mode.
This is easy to implement, but don't want to play with it until after #80 is resolved (1 thing at a time). I suspect with all of our cost functions we may need to play with this a bit to make a hand-off between them in the right situations smooth.
There are some suggested features/fixes/updates to be done to consider this controller as a part of Nav2 Stack.
PathAngleCritic
and ApproxReferenceTrajectoryCritic
I am current using a 4-wheel differential drive AMR and trying to config the parameters for motion model like wheel_base
However when I look into the source code it seems there is no way to input such kinds of parameters and the implementation
in motion_models.hpp seems empty for class DiffDriveMotionModel
Any help on this? Many thanks!
A declarative, efficient, and flexible JavaScript library for building user interfaces.
🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
An Open Source Machine Learning Framework for Everyone
The Web framework for perfectionists with deadlines.
A PHP framework for web artisans
Bring data to life with SVG, Canvas and HTML. 📊📈🎉
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
Some thing interesting about web. New door for the world.
A server is a program made to process requests and deliver data to clients.
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
Some thing interesting about visualization, use data art
Some thing interesting about game, make everyone happy.
We are working to build community through open source technology. NB: members must have two-factor auth.
Open source projects and samples from Microsoft.
Google ❤️ Open Source for everyone.
Alibaba Open Source for everyone
Data-Driven Documents codes.
China tencent open source team.