Safe Planning for Articulated Robots Using Reachability-based Obstacle Avoidance With Spheres

Introduction

This paper addresses the challenge of real-time safe motion planning of manipulators by proposing Safe Planning for Articulated Robots Using Reachability-based Obstacle Avoidance With Spheres (SPARROWS).

SPARROWS is a receding-horizon trajectory planner that utilizes the combination of a novel reachable set representation and an exact signed distance function to generate provably-safe motion plans.

At runtime, SPARROWS uses parameterized trajectories to compute reachable sets composed entirely of spheres that overapproximate the swept volume of the robot's motion. SPARROWS then performs trajectory optimization to select a safe trajectory that is guaranteed to be collision-free.

We demonstrate that SPARROWS' novel reachable set is significantly less conservative than previous approaches. We also demonstrate that SPARROWS outperforms a variety of state-of-the-art methods in solving challenging motion planning tasks in cluttered environments.

Dependency

Python dependencies can be installed directly from environment.yaml using conda environments:

conda env create --file environment.yaml
conda activate sparrows

Solving the conda environment can take up to 15 minutes. If you don't have a CUDA capable device, you can use the environment-cpu.yaml file instead. This installs the CPU-only version of PyTorch. For MACOS, you will need to modify the cpu environment to remove open3d and then build and install python-fcl and open3d from source manually after activating the environment.

Note that the environment includes:

• zonopy which provides functionalities used for reachability analysis
• zonopy-robots which provides functionalities for specifying and loading robots

MATLAB and CORA 2021 are used to compute Joint Reachable Set in forward_occupancy/jrs_trig/gen_jrs_trig with the provided MATLAB scripts.

Reproducing Results

Generating Joint Reachable Sets (JRS)

From forward_occupancy/jrs_trig/gen_jrs_trig, run create_jrs_trig_tensor.m so that the JRS files will be saved in jrs_trig_tensor_saved/. The variable d_kai in create_jrs_trig_tensor.m corresponds to the acceleration range in the paper.

Pre-generated versions for pi/6, pi/12, and pi/24 can be found in this drive link here, but the corresponding folders (e.g. jrs_trig_tensor_saved_pi_24) will need to be renamed to jrs_trig_tensor_saved and moved to the correct location to match the generated output.

Running a single-trial example

With python run_statistics_planning_3d.py --planner sphere --n_obs 20 --time_limit 0.5 --n_envs 1 --video, the user can run a single trial of SPARROWS with 20 obstacles under 0.5 second time limit. Note that the --video argument is specified for rendering a planning video. A video folder planning_videos/ will be made by the script to store the video.

Running Planning Experiments for SPARROWS and ARMTD

IPOPT is required to run the planning experiments as the framework to solve non-linear programming problems. While cyipopt in the python dependencies provides IPOPT in python interface, linear solvers such as MA27 are additionally obtained following the instructions from IPOPT. Experiments in the paper are run with MA27 linear solver.

We assume the MA27 (ma27) linear solver is available and attempt to run with it as default. If it is not available, you may use MUMPS (mumps) but performance may suffer. You can specify the linear solver to use in our shell scripts. or manually using the --solver option

To reproduce the single-arm planning experiments on random scenarios, run bash run_3d_planning.sh; to reproduce the multi-arm planning experiments on random scenarios, run bash run_multi_robot_planning.sh. The results will be in planning_results/ as generated by the planning program.

To reproduce the single-arm planning experiments on hard scenarios, run bash run_scenario_planning.sh. The results will be in scenario_planning_results/ as generated by the planning program.

For multi robot planning, you will need to copy the respective urdf in kinova_scenarios/extras to where gen3.urdf in the zonopy-robots package kinova folder is located.

Recreating Figures

We generate our figures using a pipeline to export base Blender files which we then add cameras or materials to. Since the Blender code adds quite a bit of bloat, we keep it separate. Please clone the blender-color branch to use those visualization environments. The Blender file used to generate the kitchen scene along with a handful of others can be found here (Warning, they are big!).

Credits

• zonopy referred some part of CORA.

Citation