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.