Local Navigation Planner for Legged Robots

Overview

ANYmal Rough Terrain Planner

Sampling based path planning for ANYmal, based on 2.5D height maps. More detailed instructions still to come.

The paper detailing this work is available on the ETH Research Collection.

Author: Lorenz Wellhausen

Maintainer: Lorenz Wellhausen, [email protected]

©2021 ETH Zurich

If you use this work in an academic context, please cite:

@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}
}

Build

Build Status

Dependencies

art_planner

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. 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-melodic-ompl ros-melodic-grid-map-core

Warning: Do NOT install the libompl-dev package from PPA as that one is an imcompatible version which breaks things.

art_planner_ros

The dependencies of the ROS interface can be installed with the following command:

sudo apt install ros-melodic-actionlib ros-melodic-geometry-msgs ros-melodic-grid-map-msgs ros-melodic-grid-map-ros ros-melodic-nav-msgs ros-melodic-roscpp ros-melodic-tf2-geometry-msgs ros-melodic-tf2-ros

Usage

We provide a launch file which should be everything you need, if you work with ANYmal.

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.

Configuration

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.

TODO

Although the planner is overall pretty 🔥 🔥 🔥 💯 🔥 🔥 🔥 some things are still 💩 .

Known issues

  • Catkinized ODE version might be pulled in as dependency by other packages in workspace 🎳
Issues
  • Error:   LazyPRMstar: There are no valid initial states!

    Error: LazyPRMstar: There are no valid initial states!

    I am facing some problem during launching art planner. It can't start the planning. Though I am not expert, it looks like the merged pointlouds from 3 lidars on ANYmal_c are creating elevation map on top of it which is a problem. Is there a way to remove (or not included) those points on top of the robot from elevation map?? (Note: I have transformed and merged pointclouds from vlp-16, RSbpearl front and RSbpearl rear into single frame anymal_c.

    [email protected]:~/ANYmal_ws$ roslaunch art_planner_ros art_planner.launch 
    ... logging to /home/arghya/.ros/log/b43862c4-c79a-11ec-923f-4485007e1d87/roslaunch-arghya-Erazer-X7849-MD60379-7913.log
    Checking log directory for disk usage. This may take a while.
    Press Ctrl-C to interrupt
    started roslaunch server http://arghya-Erazer-X7849-MD60379:46873/
    
    SUMMARY
    ========
    
    PARAMETERS
     * /art_planner/objectives/clearance/cost_center: 5
     * /art_planner/objectives/clearance/cost_diag: 1
     * /art_planner/objectives/clearance/cost_lat: 2
     * /art_planner/objectives/clearance/cost_lon: 3
     * /art_planner/objectives/clearance/enable: False
     * /art_planner/objectives/clearance/weight: 10.0
     * /art_planner/objectives/custom_path_length/max_ang_vel: 0.5
     * /art_planner/objectives/custom_path_length/max_lat_vel: 0.1
     * /art_planner/objectives/custom_path_length/max_lon_vel: 0.5
     * /art_planner/objectives/custom_path_length/use_directional_cost: True
     * /art_planner/path_following/local_guidance_mode: walk
     * /art_planner/planner/elevation_layer: elevation
     * /art_planner/planner/lazy_prm_star_min_update/cleanup_when_not_planning: True
     * /art_planner/planner/lazy_prm_star_min_update/height_change_for_update: 0.05
     * /art_planner/planner/lazy_prm_star_min_update/invalidate_updated_graph_components: True
     * /art_planner/planner/n_threads: 4
     * /art_planner/planner/name: lazy_prm_star_min...
     * /art_planner/planner/plan_time: 0.01
     * /art_planner/planner/replan_freq: 1.0
     * /art_planner/planner/simplify_solution: True
     * /art_planner/planner/snap_goal_to_map: True
     * /art_planner/planner/start_goal_search/goal_radius: 0.5
     * /art_planner/planner/start_goal_search/n_iter: 1000
     * /art_planner/planner/start_goal_search/start_radius: 0.2
     * /art_planner/planner/traversability_layer: traversability
     * /art_planner/planner/traversability_thres: 0.5
     * /art_planner/planner/unknown_space_untraversable: True
     * /art_planner/robot/base_frame: anymal_c
     * /art_planner/robot/feet/offset/x: 0.362
     * /art_planner/robot/feet/offset/y: 0.225
     * /art_planner/robot/feet/offset/z: -0.525
     * /art_planner/robot/feet/reach/x: 0.4
     * /art_planner/robot/feet/reach/y: 0.3
     * /art_planner/robot/feet/reach/z: 0.15
     * /art_planner/robot/torso/height: 0.3
     * /art_planner/robot/torso/length: 1.07
     * /art_planner/robot/torso/offset/x: 0.0
     * /art_planner/robot/torso/offset/y: 0.0
     * /art_planner/robot/torso/offset/z: 0.04
     * /art_planner/robot/torso/width: 0.55
     * /art_planner/sampler/max_pitch_pert: 10
     * /art_planner/sampler/max_prob_unknown_samples: 0.1
     * /art_planner/sampler/max_roll_pert: 3.33
     * /art_planner/sampler/sample_from_distribution: True
     * /art_planner/sampler/use_inverse_vertex_density: True
     * /art_planner/sampler/use_max_prob_unknown_samples: True
     * /art_planner/verbose: True
     * /rosdistro: melodic
     * /rosversion: 1.14.13
    
    NODES
      /
        art_planner (art_planner_ros/art_planner_ros_node)
        art_planner_plan_to_goal_client (art_planner_ros/plan_to_goal_client.py)
    
    ROS_MASTER_URI=http://localhost:11311
    
    process[art_planner-1]: started with pid [8180]
    process[art_planner_plan_to_goal_client-2]: started with pid [8181]
    Waiting for plan_to_goal server to appear...
    Found server.
    
    [ INFO] [1651226340.044097554, 419.200000000]: Received goal pose:
    header: 
      seq: 2
      stamp: 418.604000000
      frame_id: odom
    pose: 
      position: 
        x: 25.4173
        y: -4.3785
        z: 0
      orientation: 
        x: 0
        y: 0
        z: 0.0229904
        w: 0.999736
    
    Grid map does not have "traversability" layer. Assuming traversable.
    Invalidated 0 vertices
    Invalidated 0/0 edges
    Initial start state not valid. Trying to sample around it.
    
    Epic fail, dude
    Warning: LazyPRMstar: Skipping invalid start state (invalid state)
             at line 248 in /tmp/binarydeb/ros-melodic-ompl-1.4.2/src/ompl/base/src/Planner.cpp
    Debug:   LazyPRMstar: Discarded start state Compound state [
    RealVectorState [23.8531 -4.45529 -0.376203]
    SO3State [-0.0396322 0.0409409 -0.0359117 0.997729]
    ]
    
    Error:   LazyPRMstar: There are no valid initial states!
             at line 513 in /home/arghya/ANYmal_ws/src/art_planner/art_planner/src/planners/lazy_prm_star_min_update.cpp
    Info:    No solution found after 0.000294 seconds
    

    Screenshot from 2022-04-29 15-58-51

    There is a warning on elevation mapping node:

    [email protected]:~/ANYmal_ws$ roslaunch elevation_mapping_demos anymal_c_elevation_mapping.launch 
    ... logging to /home/arghya/.ros/log/b43862c4-c79a-11ec-923f-4485007e1d87/roslaunch-arghya-Erazer-X7849-MD60379-32616.log
    Checking log directory for disk usage. This may take a while.
    Press Ctrl-C to interrupt
    started roslaunch server http://arghya-Erazer-X7849-MD60379:46457/
    
    SUMMARY
    ========
    
    PARAMETERS
     * /elevation_mapping/enable_visibility_cleanup: False
     * /elevation_mapping/fused_map_publishing_rate: 0.5
     * /elevation_mapping/init_submap_height_offset: 0.01
     * /elevation_mapping/initialization_method: 0
     * /elevation_mapping/initialize_elevation_map: False
     * /elevation_mapping/length_in_x: 6.0
     * /elevation_mapping/length_in_x_init_submap: 1.0
     * /elevation_mapping/length_in_y: 6.0
     * /elevation_mapping/length_in_y_init_submap: 1.0
     * /elevation_mapping/mahalanobis_distance_threshold: 2.5
     * /elevation_mapping/map_frame_id: odom
     * /elevation_mapping/margin_init_submap: 0.3
     * /elevation_mapping/max_variance: 0.05
     * /elevation_mapping/min_update_rate: 2.0
     * /elevation_mapping/min_variance: 0.0001
     * /elevation_mapping/multi_height_noise: 0.001
     * /elevation_mapping/point_cloud_topic: /anymal_c/combine...
     * /elevation_mapping/position_x: 0.0
     * /elevation_mapping/position_y: 0.0
     * /elevation_mapping/postprocessor_pipeline: [{'type': 'gridMa...
     * /elevation_mapping/resolution: 0.1
     * /elevation_mapping/robot_base_frame_id: anymal_c
     * /elevation_mapping/robot_motion_map_update/covariance_scale: 0.01
     * /elevation_mapping/robot_pose_cache_size: 200
     * /elevation_mapping/robot_pose_with_covariance_topic: /loam/pose
     * /elevation_mapping/scanning_duration: 1.0
     * /elevation_mapping/sensor_processor/beam_angle: 0.0006
     * /elevation_mapping/sensor_processor/beam_constant: 0.0015
     * /elevation_mapping/sensor_processor/ignore_points_above: 0.6
     * /elevation_mapping/sensor_processor/min_radius: 0.018
     * /elevation_mapping/sensor_processor/type: laser
     * /elevation_mapping/surface_normal_positive_axis: z
     * /elevation_mapping/target_frame_init_submap: anymal_c
     * /elevation_mapping/time_offset_for_point_cloud: 0.0
     * /elevation_mapping/time_tolerance: 1.0
     * /elevation_mapping/track_point_frame_id: anymal_c
     * /elevation_mapping/track_point_x: 0.0
     * /elevation_mapping/track_point_y: 0.0
     * /elevation_mapping/track_point_z: 0.0
     * /elevation_mapping/visibility_cleanup_rate: 1.0
     * /rosdistro: melodic
     * /rosversion: 1.14.13
    
    NODES
      /
        elevation_mapping (elevation_mapping/elevation_mapping)
    
    ROS_MASTER_URI=http://localhost:11311
    
    process[elevation_mapping-1]: started with pid [344]
    [ WARN] [1651227229.356009704]: Could not find the parameter: `algorithm`. Setting to default value: 'area'.
    [ WARN] [1651227229.358008454]: Could not find the parameter: `parallelization_enabled`. Setting to default value: 'false'.
    [ WARN] [1651227229.358576404]: Could not find the parameter: `thread_number`. Setting to default value: 'automatic'.
    [ INFO] [1651227229.361199922]: Elevation mapping node started.
    [ INFO] [1651227229.387342299]: Elevation map grid resized to 60 rows and 60 columns.
    [ WARN] [1651227229.429014591]: Could not load the input sources configuration from parameter
     /elevation_mapping/input_sources, are you sure it was pushed to the parameter server? Assuming
     that you meant to leave it empty. Not subscribing to any inputs!
    
    [ WARN] [1651227229.430182369]: Parameter 'point_cloud_topic' is deprecated, please use 'input_sources' instead.
    [ INFO] [1651227229.449461756]: Elevation mapping node initializing ... 
    [ WARN] [1651226932.425124466, 488.204000000]: Estimation radius is smaller than allowed by the map resolution (0.050000 < 0.050000)
    
    opened by ArghyaChatterjee 3
  • Working with Simulation

    Working with Simulation

    Hi, thanks for open sourcing your work. I can reproduce this package cerberus_anymal_locomotion. Is there any walk through tutorial on how to integrate this package with ANYmal B/C ??

    opened by ArghyaChatterjee 1
  • Minimum working demo

    Minimum working demo

    Hi, I am looking for a minimum working demonstration with visualisation.

    Your readme states

    We provide a launch file which should be everything you need, if you work with ANYmal.

    and

    You can use the 2D Nav Goal in RViz to set a goal pose for the planner.

    which suggests there is an external ANYmal simulator/ros package that this compatible with. But I am unfamiliar with that.

    Are you able to provide guidance on how to run this with that simulator if it exists?

    opened by justinberi 0
Owner
Robotic Systems Lab - Legged Robotics at ETH Zürich
The Robotic Systems Lab investigates the development of machines and their intelligence to operate in rough and challenging environments.
Robotic Systems Lab - Legged Robotics at ETH Zürich
ModuLiDAR is an all-in-one open-source software for autonomous UGVs and industrial robots.

ModuLiDAR is an all-in-one open-source software for autonomous UGVs and industrial robots. the target industries that ModuLiDAR is working on are farming industry, mining industry, warehouses industry, and construction industry.

null 18 Jun 22, 2022
Otto-SetupAssist provides an Arduino sketch which assist you to build Otto robots.

Otto-SetupAssist Otto-SetupAssist provides an Arduino sketch which assist you to build Otto robots. This sketch provides two features: Move servos to

ROBOT.ICHIBA 1 Oct 20, 2021
Autonomous recorder for vex robots using the PROS API

VEX Robot Recorder Description This is a demo project for the "Recorder" class that allows the user to record and play back past recorded autonomouses

null 2 Jun 14, 2022
Wolf_descriptions - WoLF: Whole-body Locomotion Framework for quadruped robots

WoLF: Whole-body Locomotion Framework for quadruped robots This repo contains a collection of different robots and sensors used in WoLF. Setup See the

Gennaro Raiola 7 Jul 26, 2022
Collection of quadrupedal robots configured to work in CHAMP development framework

zoo This repository contains configuration packages of various quadrupedal robots generated by CHAMP's setup assistant. Installation You need to have

CHAMP 150 Aug 7, 2022
WoLF: Whole-body Locomotion Framework for quadruped robots

WoLF: Whole-body Locomotion Framework for quadruped robots This package contains the navigation stack to be used with WoLF. Mantainers: Federico Rollo

Gennaro Raiola 12 May 17, 2022
Let's upgrade cheap off-the-shelf robotic mowers to modern, smart RTK GPS based lawn mowing robots!

OpenMower Join the Discord server for OpenMower discussion: HERE About the Project ⚠️ DISCLAIMER: IF YOU ARE NOT 100% SURE WHAT YOU ARE DOING, PLEASE

Clemens Elflein 3.2k Aug 9, 2022
Motion planner built upon Tesseract and Trajopt

motion_planner Motion planner built upon Tesseract and Trajopt The abb_example package is similar to the tesseract_ros_examples, but it contain more e

Mohamed Ahmed 8 Jan 18, 2022
Turn 2 esp32 boards into pixhawk telemetry for use with Ardupilot/Ardurover Mission Planner.

RoverLink Turn 2 esp32 boards into pixhawk telemetry for use with Ardupilot/Ardurover Mission Planner I used to use these cheap Chinese 900mhz Pixhawk

Jeffrey Berezin 2 Jan 18, 2022
Online multi-agent trajectory planner using linear safe corridor (LSC)

lsc_planner This package presents an efficient multi-agent trajectory planning algorithm which generates safe trajectories in obstacle-dense environme

Jungwon Park 24 Aug 1, 2022
Official page of MLCPP (IROS'18 @ Barcelona, Spain): Offline Coverage Path Planner

MLCPP: Multi-layer coverage path planner for autonomous structural inspection of high-rise structures The purpose of the algorithm is to inspect high-

Sungwook Jung 12 Jul 19, 2022
An Open Source tripmaster for navigation rallies

Open Rally Computer An open source tripmaster for navigation rallies Description The Open Rally Computer (previously known as Baja Pro) is a complete

Matias Godoy 45 Aug 8, 2022
2D/3D Registration and system integration for image-based navigation of orthopedic robotic applications, inculding femoroplasty, osteonecrosis, etc.

Registration and System Integration Software for Orthopedic Surgical Robotic System This repository contains software programs for image-based registr

Cong Gao 14 Jun 11, 2022
Small Robot, with LIDAR and DepthCamera. Using ROS for Maping and Navigation

?? RoboCop ?? Small Robot, with LIDAR and DepthCamera. Using ROS for Maping and Navigation Made by Clemente Donoso, ?? Chile ???? RoboCop Lateral Fron

Clemente Donoso Krauss 2 Jan 4, 2022
Invariant-ekf - C++ library to implement invariant extended Kalman filtering for aided inertial navigation.

inekf This repository contains a C++ library that implements an invariant extended Kalman filter (InEKF) for 3D aided inertial navigation. This filter

Ross Hartley 240 Aug 5, 2022
Optimization-Based GNSS/INS Integrated Navigation System

OB_GINS Optimization-Based GNSS/INS Integrated Navigation System We open-source OB_GINS, an optimization-based GNSS/INS integrated navigation system.

i2Nav-WHU 211 Aug 8, 2022
Arduino library for basic aerial navigation functions used for processing Euler angles, direction cosine matrices, quaternions, frame conversions, and more.

navduino Arduino library for basic aerial navigation functions used for Euler angles Direction cosine matrices Quaternions Rodrigues Rotation Vectors

PB2 5 Jul 28, 2022
Project to check which Nt/Zw functions your local EDR is hooking

Probatorum EDR Userland Hook Checker Probatorum will check which Nt/Zw functions your local EDR is hooking. Most credit for this code goes to SolomonS

null 146 Aug 9, 2022
Control Heidelberg Wallbox Energy Control over WiFi using ESP8266 and configure your own local load management

< scroll down for English version and additional information > wbec WLAN-Anbindung der Heidelberg WallBox Energy Control über ESP8266 Die Heidelberg W

null 75 Jul 30, 2022