robotics_algorithms

This repository contains my pure-python implementation of essential algorithms that make a mobile manipulator (and other robots) move. It is mainly for my own personal learnings, which means the implementation may not be the cleanest or the most stable.

While it is true that a lot of algorithms have been implemented by other projects, this repo serves these main benefits:

1. It achieves a good balance of width and depth. It covers a wide range of topics, while focusing on only key algorithms in each field.
2. It is implemented with modular structure that cleanly separates algorithms from problems (like OMPL), at the same time emphasizing connection between different algorithms. For example, the design reflects that planing under uncertainties, optimal control and RL share the same underlying MDP formulation.
3. Serves as a single source of truth of various algorithms so that I no longer need to search all over the Internet.

Scope

It should include popular and representative algorithms from robot dynamics, state estimation, planning, control and learning.

Requirement

python 3.10

How to use

• Run pip install -e .
• Run various scripts inside examples folder.

For example, to run a* to find the shortest path between start and goal in a grid world

python examples/planning/path_planning/test_a_star.py

News

• 30/09/2026: Added B-RRT* and PlanarRobotArm model (v0.14.0)
• 21/06/2026: Added iLQR and DDP for trajectory optimisation (v0.13.0).
• 08/06/2026: Added direct shooting and direct collocation trajectory optimisation (v0.12.0).
• 26/03/2025: Added DWA (v0.11.2).
• 20/02/2025: Added LQR and convex MPC for planar quadrotor (v0.11.1).

Status

This repository is undergoing significant development. Here is the status checklist.

Algorithms

• Robot Kinematic and Dynamics
  • Differential drive
    • Car
  • Cartpole
  • Double Integrator
  • Inverted Pendulum
  • Arm
    • FK and IK
    • Velocity Kinematics and Inverse velocity kinematics
  • Planar Quadrotor
    • Quadrotor
  • Planar Quadruped
    • Quadruped
• State Estimation
  • Localizaion
    • Discrete bayes filter
    • Kalman filter
    • Extended Kalman filter
    • Particle filter (MCL)
    • AMCL
  • SLAM
    • EKF SLAM
    • Fast SLAM
    • Graph-based SLAM
• Planning
  • Path Planning
    • Dijkstra
    • A-star
    • Hybrid A-star
  • Coverage Path Planning
    • BA*
  • Motion Planning
    • PRM
    • RRT / RRT-Connect
    • RRT*
    • BRRT*
    • Informed RRT*
    • BIT*
  • MDP
    • Value iteration
    • policy iteration
    • Policy tree search
    • MCTS
  • POMDP
    • Belief tree search
    • SARSOP
    • DESPOT
• Control
  • Classical control
    • PID
  • Optimal Control
    • LQR
    • MPPI
    • CEM-MPC
    • Convex-MPC
  • Trajectory optimization
    • Direct collocation
    • Direct shooting
    • iLQR
    • DDP
  • Navigation Path Follow Control
    • Regulated Pure Pursuit
    • Dynamic Window Approach
    • Time-elastic Band
• Imitation learning
  • ACT
  • Diffusion-policy
• Reinforcement learning
  • Tabular
    • On-policy MC
    • Off-policy MC
    • On-policy TD (SARSA)
    • Off-policy TD (Q-learning)
  • Function approximation
• Environments
  • Frozen lake (MDP)
  • Cliff walking (MDP)
  • Windy gridworld (MDP)
  • 1D navigation with double integrator
    • Deterministic and fully-observable
    • Stochastic and partially-observable
  • 2D navigation with omni-directional robot
    • Deterministic and fully-observable
  • 2D navigation with differential drive
    • Deterministic and fully-observable
    • Stochastic and partially-observable
    • With obstacle-distance cost
  • 2D localization
  • 2D SLAM
  • Multi-arm bandits (POMDP)
  • Tiger (POMDP)

In addition to the algorithm itself, also implement several realistic robotics problems that often require additional domain-specific components and strategies.

• Systems
  • Navigation
    • Cost aware planning
    • SE2 Pose path tracking
  • Manipulation
    • Cartesian planning
    • TOPP
  • Locomotion

Known issues

• EKF gives high localisation error at some instances.
• MCTS is not stable.
• Recursive feasibility in convex MPC