/motion-planning-practice

Fast Python motion planning algorithm implementations with demos in pybullet

Primary LanguagePythonMIT LicenseMIT

Motion Planning Practice

Fast motion planning algorithm implementations with hovercraft and KUKA 7-DOF arm demos in pybullet!

Custom maze builder is also supported!

Algorithms

  • A*
  • RRT
  • RRT*
  • Bidirectional RRT
  • Kinodynamic RRT
  • Bidirectional Kinodynamic RRT
  • Shortcut Path Smoothing
  • Nonlinear State Steering via CasADi
  • Fast NN query with lazy-rebuilt KD Tree
  • Iterative Inverse Kinematics
  • Secondary IK tasks through Jacobian null space projection

Quick Start

  1. Clone this repo

    git clone https://github.com/silvery107/kinodynamic-rrt-pybullet.git

  2. Install required python packages

    chmod +x install.sh
./install.sh

  3. Run the hovercraft demo!

    python demo.py # default to Kinodynamic RRT

    Choose a different planning algorithm by setting --algo, choices are ["RRT", "BiRRT", "KdRRT", "Astar", "BiKdRRT"].

  4. Customize your maze. Just edit the maze matrix in maze_layout.json and have fun. Make sure the goal (shown in green) is feasible.

  5. Run the KUKA iiwa arm demo!

    python demo_kuka.py # default to Bidirectional RRT

    Choose a different planning algorithm by setting --algo, choices are ["RRT", "BiRRT"]. The start and goal joint configurations are solved through iterative IK from task space positions.

Gallery

demo_kuka.mp4

Known Issues

  • Nonlinear state steering could fail due to the kinodynamic infeasibility.
  • Iterative IK solver can converge to sub-optimal solutions due to joint limits, singularities or different initial guesses.