artofnothingness / mppic Goto Github PK

https://clearpathrobotics.com/blog/2022/05/indiana-university-explores-collision-free-navigation-in-cluttered-environments/

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

• no clear understanding about hot spots

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

• help with gpu extension
• help with other tensor libs extension

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

• Feedback from 2-3 users happy with performance
• Rename for Nav2 naming conventions (nav2_mppi_controller)
• Add to metapackage list
• Add documentation page on website ros-navigation/docs.nav2.org#378
• Add to plugins list ros-navigation/docs.nav2.org#378
• Add to migration guide ros-navigation/docs.nav2.org#378
• Add to Discourse announcement
• Transfer tickets: Log-Normal distribution + Zero-mean + SMPPI + Acceleration characteristics

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

• Goal Angle scores the differences in yaw angles
• Goal scores translational distances
• Obstacle scores normalized cost 0-1.0
• Path Angle scores differences in angles
• Approx / Ref Path are translational distances

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:

• Path asks robot to reverse direction in the middle of some plan: MPPI does so around the right point in the right direction
• Path asks the robot to start in reverse, MPPI does so rather than rotating forward to follow
• Path asks for an in-place rotation, MPPI approximately does so (though, the least important of the 3)

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?

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

• explain
• separate from ref critic (already done in art branch)

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.

• Footprint collision checking
• Publishers use std::move
• Support Y for omni robots (still requires some tests, but seems to work)
• Pluginize Critics
• Pluginize Model Not required.
• Parameters handle lifecycle transition multiple times
• A readme with the run-time rate / other information / parameters / link to relevant paper
• Use of ament / colcon /package.xml for building so its easy for users
• 20 Hz for 2 iterations // improve runtime performance for stability and quality behavior
• Tuning out of the box parameters to give a decent baseline (Differential done - omni needed)
• hardware testing / feedback
• Smoothness of controls (critic? more iterations? faster cycling than 20hz?)
• Support ackermann minimum turning radius constraints #42
• Coverage for PathAngleCritic and ApproxReferenceTrajectoryCritic
• Dynamic parameters (almost done)

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!