This package is a rust implementation of PythonRobotics.
Build
git clone https://github.com/rsasaki0109/RustRobotics.git
cd RustRobotics
cargo build
Run (Example)
cargo run --bin a_star
cargo run --bin rrt
cargo run --bin inverted_pendulum_lqr
cargo run --bin two_joint_arm_control
- Localization
- Mapping
- SLAM
- Path Planning
- A* Algorithm
- Theta* Algorithm
- Jump Point Search (JPS)
- Bezier Path Planning
- Cubic Spline
- D* Lite
- Dynamic Window Approach
- Dijkstra Algorithm
- Informed RRT*
- Model Predictive Trajectory Generator
- Potential Field Algorithm
- Quintic Polynomials
- Rapidly-Exploring Random Trees (RRT)
- RRT*
- Reeds-Shepp Path
- PRM (Probabilistic Road-Map)
- Voronoi Road-Map
- Frenet Optimal Trajectory
- State Lattice Planner
- Path Tracking
- Inverted Pendulum
- Arm Navigation
Red:GPS, Brue:Ground Truth, Green:EKF, Yellow:Dead Reckoning
cargo run --bin ekf
Blue: GPS, Red: Ground Truth, Green: Particle Filter, Yellow: Dead Reckoning
cargo run --bin particle_filter
Blue: Ground Truth, Red: UKF Estimate, Black: Dead Reckoning, Green: GPS Observations, Red Ellipse: Uncertainty
cargo run --bin unscented_kalman_filter
Grid-based probabilistic localization using RFID landmarks. The algorithm maintains a probability distribution over a 2D grid and updates it based on motion and observations.
Blue: True path, Yellow: Dead Reckoning, Green: Histogram Filter estimate, Black: RFID landmarks
cargo run --bin histogram_filter
cargo run --bin ndt
Occupancy grid mapping using Gaussian distribution. Higher probability near obstacles.
cargo run --bin gaussian_grid_map
Occupancy grid mapping using ray casting. Free space (0.5), Occupied (1.0), Unknown (0.0).
cargo run --bin ray_casting_grid_map
Red: Reference points, Blue: Initial points, Green: Aligned points
cargo run --bin icp_matching
Particle filter based SLAM (Simultaneous Localization and Mapping). Each particle maintains its own map of landmarks using EKF.
Blue: True path, Yellow: Dead Reckoning, Green: FastSLAM estimate, Black: True landmarks, Cyan: Estimated landmarks
cargo run --bin fastslam1
Extended Kalman Filter based SLAM. Maintains a joint state vector of robot pose and landmark positions with full covariance matrix.
cargo run --bin ekf_slam
Pose graph optimization for SLAM. Constructs a graph of robot poses connected by odometry and observation constraints, then optimizes the graph using iterative methods.
cargo run --bin graph_based_slam
Blue: Start, Red: Goal, Green: Path, Gray: Obstacles
cargo run --example a_star
Any-angle path planning algorithm. Unlike A* which restricts movement to grid edges, Theta* allows paths at any angle by checking line-of-sight between nodes.
cargo run --example theta_star
Optimized pathfinding algorithm for uniform-cost grids. Reduces the number of nodes to explore by identifying and jumping to key "jump points" instead of examining all neighbors.
cargo run --example jps
Blue: Start, Red: Goal, Green: Path
cargo run --bin bezier_path_planning
Black: Control points, Green: Path
cargo run --bin csp
Black: Obstacles, Green: Trajectory, Yellow: Predicted trajectory
cargo run --bin dwa
Blue: Start, Red: Goal, Green: Path, Black: Obstacles
cargo run --bin d_star_liteD* Lite is an incremental heuristic search algorithm for path planning in dynamic environments. It's particularly efficient for replanning when the environment changes.
cargo run --bin dijkstra
Blue: Start, Red: Goal, Green: Path, Black: Tree
cargo run --bin informed_rrt_star
Green: Path
cargo run --bin model_predictive_trajectory_generator
Blue: Start, Red: Goal, Green: Path, Gray: Obstacles
cargo run --bin potential_field
Blue: Start, Red: Goal, Green: Path
cargo run --bin quintic_polynomials
Sampling-based path planning algorithm that builds a tree by randomly sampling the configuration space.
Blue: Start, Red: Goal, Green: Path, Gray: Tree
cargo run --bin rrt
Optimized version of RRT that rewires the tree to find shorter paths. Asymptotically optimal.
Blue: Start, Red: Goal, Green: Path, Gray: Tree
cargo run --bin rrt_star
Blue: Start, Red: Goal, Green: Path
cargo run --bin reeds_shepp_path
Sampling-based path planning using random samples and k-nearest neighbor connections.
Blue: Start, Red: Goal, Green: Path, Gray: Samples and edges, Black: Obstacles
cargo run --bin prm
Path planning using Voronoi diagram vertices as waypoints. Provides paths that maximize clearance from obstacles.
Blue: Start, Red: Goal, Green: Path, Cyan: Voronoi vertices, Black: Obstacles
cargo run --bin voronoi_road_map
Optimal trajectory planning in Frenet coordinate frame. Widely used in autonomous driving for lane keeping and obstacle avoidance.
Gray: Reference path, Green: Optimal trajectory, Black: Obstacles, Red: Vehicle
cargo run --bin frenet_optimal_trajectory
Lattice-based motion planning that searches over a pre-computed set of motion primitives. Generates smooth, dynamically feasible trajectories by connecting state lattice primitives.
cargo run --example state_lattice
Black: Planned path, Green: Tracked path
cargo run --bin lqr_steer_control
Green: Path, Red: Start and Goal
cargo run --bin move_to_pose
Black: Planned path, Green: Tracked path
cargo run --bin pure_pursuit
Black: Planned path, Green: Tracked path
cargo run --bin stanley_controller
Path tracking using rear wheel feedback steering control. Combines heading error and lateral error with path curvature feedforward.
Blue: Reference path, Red: Vehicle trajectory, Green: Waypoints
cargo run --example rear_wheel_feedback
Model Predictive Control for path tracking using linearized bicycle model. Predicts future states and optimizes control inputs over a horizon.
Gray: Reference path, Blue: Tracked trajectory, Green: Prediction horizon, Red: Vehicle
cargo run --bin mpc
Cart-pendulum animation showing LQR control stabilization. Blue: Cart, Black: Pendulum. Multiple frames overlaid to show time progression from initial angle to stabilized state.
cargo run --bin inverted_pendulum_lqr
Two joint arm to a point control simulation using inverse kinematics.
Black: Arm links, Red: Joints (shoulder, elbow, end effector), Green: Target position
cargo run --bin two_joint_arm_control
Finite state machine for robot behavior management with states, transitions, guards, and actions
cargo run --bin state_machine