Sampling based path planning for ANYmal, based on 2.5D height maps using learned motion cost.
Author: Lorenz Wellhausen
Maintainer: Lorenz Wellhausen, [email protected]
ยฉ2021 ETH Zurich
If you use this work in an academic context, please cite:
Use with motion cost (Default, prm_motion_cost
) Paper link:
@inproceedings{wellhausen2023artplanner,
title={ArtPlanner: Robust Legged Robot Navigation in the Field},
author={Wellhausen, Lorenz and Hutter, Marco},
booktitle={Field Robotics},
year={2023}
}
Use without motion cost (lazy_prm_star_min_update
) Paper link:
@inproceedings{wellhausen2021rough,
title={Rough Terrain Navigation for Legged Robots using Reachability Planning and Template Learning},
author={Wellhausen, Lorenz and Hutter, Marco},
booktitle={2021 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2021)},
year={2021}
}
The base package has the following dependencies:
You can install them from source if you want to use the planner independent of ROS. If you're using the ROS interface, installation is even easier and can be done through PPA (and OpenCV should be installed by default). We ship our own catkinized version of ODE, which has modifications in the height field collision checking. This means that if you build this package in a workspace with other packages which also require ODE this version might be used.
sudo apt install ros-noetic-ompl ros-noetic-grid-map-core
Warning: Do NOT install the libompl-dev
package from PPA which is an imcompatible version and breaks things.
The dependencies of the ROS interface can be installed with the following command:
sudo apt install ros-noetic-actionlib ros-noetic-geometry-msgs ros-noetic-grid-map-msgs ros-noetic-grid-map-ros ros-noetic-nav-msgs ros-noetic-roscpp ros-noetic-tf2-geometry-msgs ros-noetic-tf2-ros
Tested on ROS noetic.
This package contains everything related to motion cost inference. This is all done in Python and Pytorch, therefore, you need to install these Python packages:
pip3 install opencv-python rospkg
Also, install Pytorch (tested with version 1.13). For example with:
pip3 install torch torchvision torchaudio
We provide a launch file which should be everything you need, if you work with ANYmal. Note that the ANYmal simulation stack is only available through ANYmal Research and I can therefore not provide a minimal working solution.
roslaunch art_planner_ros art_planner.launch
In case you do not have your own path follower, you can use our hacky and unsupported path follower.
rosrun art_planner_ros path_follower.py
For this one to work you need to manually start your desired motion controller.
The config file which is loaded when following the instructions above is located in art_planner_ros/config/params.yaml
.
It has extensive comments describing the function of each parameter.
The defaults should be fine for ANYmal C.
You can use the 2D Nav Goal
in RViz to set a goal pose for the planner.
By default, prm_motion_cost
is used as planning method. This uses a learned motion cost as planning objective, as described in wellhausen2023artplanner
.
The included neural network weights for computing the motion cost are trained for blind and perceptive policies of the ANYmal robot.
Unforunately, we cannot provide the environment for you to retrain it for your robot.
Therefore, if you want to try this planner on a robot with very different traversability characteristics than ANYmal, you might be better off using the purely geometric lazy_prm_star_min_update
method, as described in wellhausen2021rough
.
We recommend using the elevation_mapping_cupy package for height mapping.
It's what we use as input to the planner and will provide a traversability
layer by default, which is used to restrict steppable terrain.
It can also output an upper_bound
layer, which computes the maximum terrain height in unobserved map cells via ray-tracing and improves navigation in complex terrain.
We know that elevation_mapping also works, if you only have a CPU but will not provide traversability
or upper_bound
.
~elevation_map
: Expects a grid_map_msgs::GridMap
message containing the height map used for planning
The robot pose is obtained from the TF tree (see robot/base_frame
in the config file).
~path
: Outputs a nav_msgs::Path
message with the computed path.
~map
: Outputs the height map used for computing the published path as grid_map_msgs::GridMap
, with additional layers created during planning (such as sampling probability and inpainted traversability).
These are a lot of layers, which makes the published message quite heavy. Therefore, only subscribe to it, when you also really want to look at it (it's only published if there are subscribers).
~planning_time
: Publishes the time used for planning for the latest output path as std_msgs::Float64
.
~plan_to_goal
: Custom action art_planner_msgs::PlanToGoalAction
to contiuously plan towards a goal. Can be called with a 2D Nav Goal
in Rviz using art_planner_ros/scripts/plan_to_goal_client.py
.
~plan
: A nav_msgs::GetPlan
service to request a single path. The tolerance
field is ignored in favor of the planner/start_goal_search/goal_radius
parameter.
Although the planner is overall pretty ๐ฅ๐ฅ๐ฅ๐ฏ๐ฅ๐ฅ๐ฅ some things are still ๐ฉ.
- Catkinized ODE version might be pulled in as dependency by other packages in workspace ๐ณ
- This method does not use LLMs or RL, so using it basically makes you a boomer ๐ด