kylc/planning-kit

planning-kit

planning-kit is an experimental toolbox implementing sampling-based motion planning for high-dimensional systems. The toolbox is exposed in the form of a Rust library with Python bindings.

Setup

Download a recent .whl from GitHub Releases
Run pip install planning_kit_py<...>.whl (replace <...> with the actual filename)
Test it: python -c 'import planning_kit'

Usage

Plan around a sphere obstacle in $\mathbb{R}^3$ using the RRT-Connect planner:

planning-kit/demo/01_intro.py

Lines 1 to 51 in 7579c0d

    
           #!/usr/bin/env python3 
        
           from math import sqrt 
        
           import numpy as np 
        
           import rerun as rr 
        
           import planning_kit as pkit 
        
           from planning_kit import StateSpace, Constraint 
        
           # We can construct a Euclidean space, from which samples will be drawn. Because 
        
           # this space is defined in the native Rust code, sampling is computationally 
        
           # efficient. 
        
           # 
        
           # We define lower and upper boundaries of the space in R^3. 
        
           euclidean = StateSpace.euclidean(-np.ones(3), np.ones(3)) 
        
           R = 0.95 
        
           # A sample is valid if it lies outside a sphere of radius 0.95. 
        
           def is_valid(s): 
        
               return sqrt(s[0] ** 2 + s[1] ** 2 + s[2] ** 3) > R 
        
           # Now we can use a variety of the built-in sampling-based motion planning 
        
           # algorithms. 
        
           start = -np.ones(3) 
        
           goal = np.ones(3) 
        
           tree = pkit.rrt_connect( 
        
               euclidean,  # the search space 
        
               is_valid,  # is a particular sample valid? 
        
               start=start,  # the start state 
        
               goal=goal,  # the goal state 
        
               discretization=0.01,  # motion validation resolution 
        
               # planner-specific parameters 
        
               steering_dist=0.1, 
        
               n=10000, 
        
           ) 
        
           # Log everything to Rerun! 
        
           rr.init("pk.01_intro", spawn=True) 
        
           rr.log_view_coordinates("space", xyz="FLU", timeless=True) 
        
           rr.log_points("space/rrt_tree", positions=[p for p in tree.points()]) 
        
           rr.log_line_strip( 
        
               "space/shortest_path", 
        
               positions=tree.shortest_path(euclidean, start, goal).points(), 
        
           ) 
        
           # Sample the surface of the sphere so we can visualize the keep-out region. 
        
           xs = np.random.uniform(low=-1.0, high=1.0, size=(1000, 3)) 
        
           xs = R * xs / np.linalg.norm(xs, axis=1)[:, None] 
        
           rr.log_points("space/sphere", positions=xs)

Details

State Space

A state space (or configuration space) is defined by all possible configurations of the system. Arbitrary state space definitions are possible for use with the library.

For example, to define a Euclidean space (which has equivalent behavior to the built-in StateSpace.euclidean):

planning-kit/demo/02_custom_space.py

Lines 1 to 32 in 7579c0d

    
           #!/usr/bin/env python3 
        
           import numpy as np 
        
           import rerun as rr 
        
           from planning_kit import StateSpace 
        
           class MyEuclideanSpace: 
        
               def __init__(self, dim): 
        
                   self.dim = dim 
        
               def sample_uniform(self): 
        
                   return np.random.uniform(0.0, 1.0, self.dim) 
        
               def sample_uniform_near(self, near, dist): 
        
                   return near + np.random.uniform(-dist, dist, self.dim) 
        
               def distance(self, a, b): 
        
                   return np.linalg.norm(np.subtract(a, b)) 
        
               def interpolate(self, a, b, alpha): 
        
                   return (1.0 - alpha) * np.asarray(a) + alpha * np.asarray(b) 
        
           # Once we wrap our custom implementation in a StateSpace object, we can use it 
        
           # as any other. 
        
           space = StateSpace.custom(MyEuclideanSpace(dim=3)) 
        
           # Log everything to Rerun! 
        
           rr.init("pk.02_custom_space", spawn=True) 
        
           rr.log_view_coordinates("space", xyz="FLU", timeless=True) 
        
           rr.log_points("space", positions=[space.sample_uniform() for _ in range(0, 1000)])

Constraints

Planning subject to manifold constraints is implemented via the unifying framework IMACS (implicit manifold configuration space) presented in [KiMK19]. Projection is used to adhere samples from an ambient space to the constrained manifold while preserving the planner properties of probabilistic completeness and asymptotic optimality.

In order to converge on an adhering sample, Newton's method is used to approximate the roots of the constraints. Constraint Jacobians (required for Newton's method) are automatically computed via central finite difference or complex-step differentiation, or are provided analytically.

For example, we can define a constraint which confines the state space to the surface of a unit sphere. With this constraint, we can define a state space which is automatically projected into the manifold. This state space can be used like any other, e.g. for use in motion planning algorithms.

planning-kit/demo/03_constraints.py

Lines 1 to 33 in 7579c0d

    
           #!/usr/bin/env python3 
        
           import numpy as np 
        
           import rerun as rr 
        
           from planning_kit import Constraint, StateSpace 
        
           def sinusoid_xy_plane(amp=1.0, freq=1.0): 
        
               def inner(p): 
        
                   s = np.sin(2.0 * np.pi * freq * p[0]) 
        
                   c = np.cos(2.0 * np.pi * freq * p[1]) 
        
                   z = amp * (s + c) 
        
                   return [p[2] - z] 
        
               return inner 
        
           constraint = Constraint(3, 1, sinusoid_xy_plane(amp=0.1, freq=0.707)) 
        
           ambient = StateSpace.euclidean(-np.ones(3), np.ones(3)) 
        
           projected = StateSpace.projected(ambient, constraint) 
        
           # Log everything to Rerun! 
        
           rr.init("pk.03_constraints", spawn=True) 
        
           rr.log_view_coordinates("space", xyz="FLU", timeless=True) 
        
           rr.log_points( 
        
               "space/ambient", 
        
               positions=[ambient.sample_uniform() for _ in range(0, 500)], 
        
               colors=[0.3, 0.3, 0.3], 
        
           ) 
        
           rr.log_points( 
        
               "space/constrained", positions=[projected.sample_uniform() for _ in range(0, 2000)] 
        
           )

Planning

Planner	Category	Optimal	Reference
PRM	Multi-query	❌	[KSLO96]
RRT	Single-query	❌	[LaVa98]
RRT-Connect	Single-query	❌	[KuLa00]

Post-processing

TODO

Application

One potential application is to plan a joint-space trajectory for a robot arm which does not collide with obstacles in the environment. A predefined setup for this problem is defined in planning_kit.KinematicChainProblem. An example demonstrating its usage is provided:

planning-kit/demo/04_arm_obstacle.py

Lines 1 to 68 in 7579c0d

    
           #!/usr/bin/env python3 
        
           import sys 
        
           import numpy as np 
        
           from scipy.interpolate import BSpline, make_interp_spline 
        
           import rerun as rr 
        
           import rerun_ext as rr_ext 
        
           import trimesh 
        
           import planning_kit as pkit 
        
           def load_mesh(path): 
        
               """Helper function to build a planning_kit.Mesh object using the trimesh 
        
               library's loading functionality.""" 
        
               mesh = trimesh.load_mesh(path) 
        
               vertices = np.asarray(mesh.vertices).flatten() 
        
               indices = np.asarray(mesh.faces).flatten() 
        
               return pkit.Mesh(vertices, indices) 
        
           urdf_model_path = sys.argv[1] 
        
           collision_mesh_path = sys.argv[2] 
        
           ee_frame = sys.argv[3] 
        
           with open(urdf_model_path, "r") as f: 
        
               urdf = f.read() 
        
           collision_mesh = load_mesh(collision_mesh_path) 
        
           # Build a pre-baked problem definition for soliving a collision-free joint-space 
        
           # trajectory of a kinematic chain. 
        
           arm_problem = pkit.KinematicChainProblem(urdf, collision_mesh) 
        
           # Define our search space. 
        
           nq = arm_problem.nq() 
        
           space = pkit.StateSpace.euclidean(-3.14 * np.ones(nq), 3.14 * np.ones(nq)) 
        
           # Define our start and goal configurations as joint angles. 
        
           q_init = np.zeros(nq) 
        
           q_goal = np.zeros(nq) 
        
           q_goal[0] = 3.0 
        
           # Perform an RRT-Connect search. This can easily be replaced with a different 
        
           # planning algorithm because the specifics of the search space and state 
        
           # validation are abstracted. 
        
           graph = pkit.rrt_connect_problem( 
        
               space, 
        
               arm_problem, 
        
               start=q_init, 
        
               goal=q_goal, 
        
               discretization=0.01, 
        
               steering_dist=0.1, 
        
           ) 
        
           # Find the points along the shortest path. 
        
           path = graph.shortest_path(space, q_init, q_goal) 
        
           # Perform a naive joint-space B-spline interpolation to get a smooth path. 
        
           splx = np.linspace(0.0, 1.0, num=len(path.points())) 
        
           spl = make_interp_spline(splx, path.points()) 
        
           # Log everything to Rerun! 
        
           rr.init("pk.04_arm_obstacle", spawn=True) 
        
           rr.log_view_coordinates("robot", xyz="FLU", timeless=True) 
        
           viz_mod = rr_ext.load_urdf_from_file(urdf_model_path) 
        
           rr_ext.log_arm_problem_scene(viz_mod, collision_mesh, arm_problem, graph, spl, ee_frame)

04_arm_obstacle.webm

References

[KiMK19]: Z. Kingston, M. Moll, and L. E. Kavraki, "Exploring implicit spaces for constrained sampling-based planning," The International Journal of Robotics Research, vol. 38, no. 10–11, pp. 1151–1178, Sep. 2019, doi: 10.1177/0278364919868530.

[KSLO96]: L. E. Kavraki, P. Svestka, J.-C. Latombe, and M. H. Overmars, "Probabilistic roadmaps for path planning in high-dimensional configuration spaces," IEEE Transactions on Robotics and Automation, vol. 12, no. 4, pp. 566–580, Aug. 1996, doi: 10.1109/70.508439.

[LaVa98]: S. LaValle, "Rapidly-exploring random trees : a new tool for path planning," The annual research report, 1998, Accessed: May 26, 2023. [Online]. Available: https://www.semanticscholar.org/paper/Rapidly-exploring-random-trees-%3A-a-new-tool-for-LaValle/d967d9550f831a8b3f5cb00f8835a4c866da60ad

[KuLa00]: J. J. Kuffner and S. M. LaValle, "RRT-connect: An efficient approach to single-query path planning," in Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No.00CH37065), San Francisco, CA, USA: IEEE, 2000, pp. 995–1001. doi: 10.1109/ROBOT.2000.844730.

	#!/usr/bin/env python3

	from math import sqrt

	import numpy as np
	import rerun as rr
	import planning_kit as pkit
	from planning_kit import StateSpace, Constraint

	# We can construct a Euclidean space, from which samples will be drawn. Because
	# this space is defined in the native Rust code, sampling is computationally
	# efficient.
	#
	# We define lower and upper boundaries of the space in R^3.
	euclidean = StateSpace.euclidean(-np.ones(3), np.ones(3))

	R = 0.95

	# A sample is valid if it lies outside a sphere of radius 0.95.
	def is_valid(s):
	return sqrt(s[0] 2 + s[1] 2 + s[2] ** 3) > R


	# Now we can use a variety of the built-in sampling-based motion planning
	# algorithms.
	start = -np.ones(3)
	goal = np.ones(3)
	tree = pkit.rrt_connect(
	euclidean, # the search space
	is_valid, # is a particular sample valid?
	start=start, # the start state
	goal=goal, # the goal state
	discretization=0.01, # motion validation resolution
	# planner-specific parameters
	steering_dist=0.1,
	n=10000,
	)

	# Log everything to Rerun!
	rr.init("pk.01_intro", spawn=True)
	rr.log_view_coordinates("space", xyz="FLU", timeless=True)
	rr.log_points("space/rrt_tree", positions=[p for p in tree.points()])
	rr.log_line_strip(
	"space/shortest_path",
	positions=tree.shortest_path(euclidean, start, goal).points(),
	)

	# Sample the surface of the sphere so we can visualize the keep-out region.
	xs = np.random.uniform(low=-1.0, high=1.0, size=(1000, 3))
	xs = R * xs / np.linalg.norm(xs, axis=1)[:, None]
	rr.log_points("space/sphere", positions=xs)

	#!/usr/bin/env python3

	import sys

	import numpy as np
	from scipy.interpolate import BSpline, make_interp_spline
	import rerun as rr
	import rerun_ext as rr_ext
	import trimesh

	import planning_kit as pkit


	def load_mesh(path):
	"""Helper function to build a planning_kit.Mesh object using the trimesh
	library's loading functionality."""
	mesh = trimesh.load_mesh(path)
	vertices = np.asarray(mesh.vertices).flatten()
	indices = np.asarray(mesh.faces).flatten()
	return pkit.Mesh(vertices, indices)


	urdf_model_path = sys.argv[1]
	collision_mesh_path = sys.argv[2]
	ee_frame = sys.argv[3]

	with open(urdf_model_path, "r") as f:
	urdf = f.read()

	collision_mesh = load_mesh(collision_mesh_path)

	# Build a pre-baked problem definition for soliving a collision-free joint-space
	# trajectory of a kinematic chain.
	arm_problem = pkit.KinematicChainProblem(urdf, collision_mesh)

	# Define our search space.
	nq = arm_problem.nq()
	space = pkit.StateSpace.euclidean(-3.14 * np.ones(nq), 3.14 * np.ones(nq))

	# Define our start and goal configurations as joint angles.
	q_init = np.zeros(nq)
	q_goal = np.zeros(nq)
	q_goal[0] = 3.0

	# Perform an RRT-Connect search. This can easily be replaced with a different
	# planning algorithm because the specifics of the search space and state
	# validation are abstracted.
	graph = pkit.rrt_connect_problem(
	space,
	arm_problem,
	start=q_init,
	goal=q_goal,
	discretization=0.01,
	steering_dist=0.1,
	)

	# Find the points along the shortest path.
	path = graph.shortest_path(space, q_init, q_goal)

	# Perform a naive joint-space B-spline interpolation to get a smooth path.
	splx = np.linspace(0.0, 1.0, num=len(path.points()))
	spl = make_interp_spline(splx, path.points())

	# Log everything to Rerun!
	rr.init("pk.04_arm_obstacle", spawn=True)
	rr.log_view_coordinates("robot", xyz="FLU", timeless=True)
	viz_mod = rr_ext.load_urdf_from_file(urdf_model_path)
	rr_ext.log_arm_problem_scene(viz_mod, collision_mesh, arm_problem, graph, spl, ee_frame)