Code Documentation

Planning-Space Modules

class pybullet_industrial_path_planner.pbi_state_space.PbiStateSpace(robot: RobotBase)[source]

Custom real vector state space for robot joint configurations.

Bounds are defined based on the robot’s joint limits. The state space supports conversion between OMPL states and joint configurations.

robot

The robot instance.

Type:: RobotBase

joint_order

Ordered list of movable joint names.

Type:: list

dict_to_list(joint_dict: dict) → list[source]

A dictionary of joint names and values is converted to an ordered list.

Parameters:: joint_dict (dict) – Mapping from joint names to values.
Returns:: Ordered joint values as per self._joint_order.
Return type:: list

list_to_state(joint_values: list) → State[source]

A list of joint values is converted to an OMPL state.

Parameters:: joint_values (list) – Ordered list of joint values.
Returns:: The corresponding OMPL state.
Return type:: ob.State

path_to_joint_path(path: Path, interpolation_precision: float) → JointPath[source]

Converts an OMPL path to a joint configuration path using a specified interpolation precision.

Parameters:

path (ob.Path) – OMPL path containing states.
interpolation_precision (float) – Interpolation precision.

Returns:

The resulting joint configuration path.

Return type:

JointPath

setBounds(lower_limit: list, upper_limit: list) → None[source]

The state space bounds are set using the provided joint limits.

Parameters:

lower_limit (list) – Lower limits for each joint.
upper_limit (list) – Upper limits for each joint.

state_to_list(state: State) → list[source]

An OMPL state is converted to a list of joint values.

Parameters:: state (ob.State) – The OMPL state.
Returns:: A list of joint values.
Return type:: list

class pybullet_industrial_path_planner.pbi_object_mover.PbiObjectMover(urdf: list = None, position_offset: list = None, orientation_offset: list = None)[source]

Manage objects to be relocated in the PyBullet simulation.

Objects are defined by URDF identifiers and pose offsets relative to a given input pose. This class enables sampling of robot configurations while including coupled tools and grasped objects.

Parameters:

urdf (list, optional) – List of URDF identifiers.
position_offset (list, optional) – List of 3D position offsets.
orientation_offset (list, optional) – List of quaternion offsets.

add_object(urdf, position_offset=None, orientation_offset=None)[source]

Add a movable object with pose offsets relative to the reference.

Parameters:

urdf – URDF identifier of the object.
position_offset (np.array, optional) – 3D position offset. Defaults to [0, 0, 0].
orientation_offset (np.array, optional) – Quaternion offset [x, y, z, w]. Defaults to [0, 0, 0, 1].

match_moving_objects(position, orientation)[source]

Align all stored objects with a given reference pose.

Applies each object’s relative offset to the input pose to compute the new base pose and updates the object in the simulation.

Parameters:

position (np.array) – Reference position in world coordinates.
orientation (np.array) – Reference orientation (quaternion).

class pybullet_industrial_path_planner.pbi_space_information.PbiSpaceInformation(state_space: PbiStateSpace, object_mover: PbiObjectMover = None)[source]

Extend OMPL SpaceInformation with robot-specific functionality.

This includes robot configuration updates and interaction with moving simulation objects.

robot

The robot instance.

Type:: RobotBase

state_space

Custom state space implementation.

Type:: PbiStateSpace

object_mover

Optional object mover instance.

Type:: PbiObjectMover

set_object_mover(object_mover: PbiObjectMover) → None[source]

The object mover is set to update moving objects in the simulation.

Parameters:: object_mover (PbiObjectMover) – The object mover instance.

set_state(state: State) → None[source]

The robot configuration is updated based on the given state. Moving objects are updated if applicable.

Parameters:: state (ob.State) – The state to be applied.

class pybullet_industrial_path_planner.pbi_state_validity_checker.PbiStateValidityChecker(si: PbiSpaceInformation, collision_check_function, constraint_function=None, clearance_function=None)[source]

A state is validated by updating the robot configuration and performing collision and constraint tests.

si

The robot’s space information.

Type:: PbiSpaceInformation

collision_check_function

Returns True if the state is collision free.

Type:: callable

constraint_function

Returns True if constraints are met.

Type:: callable

clearance_function

Returns a clearance value.

Type:: callable

clearance(state: State)[source]

Clearance is computed for a state via collision tests.

Parameters:: state (ob.State) – The state to evaluate.
Returns:: The computed cost, where lower clearance yields a higher cost.
Return type:: ob.Cost

isValid(state: State) → bool[source]

State validity is determined by applying clearance, constraints, and collision tests.

Parameters:: state (ob.State) – The state to validate.
Returns:: True if the state is valid; False otherwise.
Return type:: bool

set_clearance_function(clearance_function)[source]

Set the function to compute clearance.

Parameters:: clearance_function (callable) – The function to compute clearance.

set_collision_check_function(collision_check_function)[source]

Set the function to check for collisions.

Parameters:: collision_check_function (callable) – The function to check for collisions.

set_constraint_function(constraint_function)[source]

Set the function to check for constraints.

Parameters:: constraint_function (callable) – The function to check for constraints.

Objective modules

class pybullet_industrial_path_planner.pbi_clearance_objective.PbiClearanceObjective(si: PbiSpaceInformation)[source]

Optimization objective based on state clearance.

States with higher clearance from obstacles are preferred. The cost is inversely proportional to the clearance: lower clearance results in higher cost, encouraging safer paths. Total path cost is evaluated as the integral of state costs along the path.

Args: si (PbiSpaceInformation): Space information that provides

access to the state validity checker and robot model.

stateCost(state: State) → Cost[source]

Compute the cost of a state based on its clearance.

A higher clearance results in a lower cost. The cost is defined as the reciprocal of the clearance to penalize states near obstacles.

Parameters:: state (ob.State) – The state to evaluate.
Returns:: Inverse clearance as cost; zero if clearance is None.
Return type:: ob.Cost

class pybullet_industrial_path_planner.pbi_endeffector_path_length_objective.PbiEndeffectorPathLengthObjective(si: PbiSpaceInformation)[source]

Optimization objective based on the end-effector path length in the robots workspace.

The cost is computed as the spatial and angular distance between consecutive end-effector poses, combining translation and rotation.

Parameters:: si (PbiSpaceInformation) – Space information including robot model and methods for state conversion and setting.

costToGo(state: State, goal: Goal) → Cost[source]

Heuristic estimate from a given state to the goal.

Uses the same metric as motionCost to evaluate distance between end-effector poses.

Parameters:

state (ob.State) – Current planning state.
goal (ob.Goal) – Goal representation or concrete goal state.

Returns:

Estimated cost-to-go.

Return type:

ob.Cost

motionCost(s1: State, s2: State) → Cost[source]

Compute the cost between two states based on workspace distance.

Translation is measured as Euclidean distance, rotation as quaternion angular distance. Both components are combined into a scalar cost value.

Parameters:

s1 (ob.State) – Start state.
s2 (ob.State) – Target state.

Returns:

Combined translational and rotational motion cost.

Return type:

ob.Cost

stateCost(state: State) → Cost[source]

Cost for an individual state.

Always returns zero since only motion cost is considered.

Parameters:: state (ob.State) – State to evaluate.
Returns:: Always zero.
Return type:: ob.Cost

class pybullet_industrial_path_planner.pbi_multi_optimization_objective.PbiMultiOptimizationObjective(si: PbiSpaceInformation, weighted_objective_list: list)[source]

Optimization objective based on a weighted combination of multiple robot-specific cost terms.

Parameters:

si (PbiSpaceInformation) – Space information including robot model.
weighted_objective_list (list) – List of (objective, weight) pairs, where each objective is a class and weight is a float.

Helpers and utilities modules

class pybullet_industrial_path_planner.pbi_simple_setup.PbiSimpleSetup(robot: RobotBase, collision_check_function: callable, object_mover: PbiObjectMover = None, constraint_function: callable = None, clearance_function: callable = None, planner_type: callable = None, objective: callable = None, name: str = None)[source]

Simple setup to solve and configure a path planning problem.

The state space, space information, validity checker, optimization objective, and planner are configured for the given robot instance.

Parameters:

robot (RobotBase) – The robot instance.
collision_check_function (callable) – Function to perform collision checks.
planner_type (callable, optional) – Callable returning a planner instance.
constraint_function (callable, optional) – Additional constraint checker.
clearance_function (callable, optional) – Function providing state clearance.
objective (class, optional) – Optimization objective class.
object_mover (PbiObjectMover, optional) – Simulation object manager.
name (str, optional) – Custom setup name.

get_path_cost_value(path: Path, objective=None)[source]

The cost of a path is computed using the optimization objective.

Parameters:: ob.Path – The OMPL path to evaluate.
Returns:: The cost of the path.
Return type:: float

plan_start_goal(start: dict, goal: dict, allowed_time: float = 5.0, simplify: bool = True) → tuple[source]

Plans a path from start to goal configuration and returns the result as a Joint Path instance.

Parameters:

start (dict) – Starting joint configuration.
goal (dict) – Goal joint configuration.
allowed_time (float) – Maximum planning time in seconds.
simplify (float) – Factor for solution simplification

Returns:

(solved (bool), JointPath or None) indicating whether: a valid path was found and the corresponding joint path.

Return type:

tuple

setOptimizationObjective(objective: callable) → None[source]

The optimization objective for the planner is set.

Parameters:: objective (class or None) – The optimization objective class.

setPlanner(planner_type: callable) → None[source]

The planner is configured.

Parameters:: planner_type – The planner class/type to be instantiated.

setStartAndGoalStates(start: dict, goal: dict) → None[source]

Defines start and goal states for planning.

Parameters:

start (dict) – Starting joint configuration.
goal (dict) – Goal joint configuration.

set_clearance_function(clearance_function: callable) → None[source]

Assigns the clearance function for state evaluation.

Parameters:: clearance_function – Callable that returns a clearance value.

set_collision_check_function(collision_check_function: callable) → None[source]

Sets the collision check function for validity tests.

Parameters:: collision_check_function – Callable that verifies collision-free states.

set_constraint_function(constraint_function: callable) → None[source]

Updates the validity checker’s constraint function.

Parameters:: constraint_function – Callable used to check additional constraints.

set_interpolation_precision(precision: float) → None[source]

The interpolation precision is set for path generation.

Parameters:: precision (float) – Distance between interpolated states.

set_object_mover(object_mover: PbiObjectMover) → None[source]

Sets the object mover for the space information.

Parameters:: object_mover – PbiObjectMover instance for managing objects.