pyroboplan.core package

Core pyroboplan module.

This module contains the essential utilities to represent a robot model for motion planning. It makes extensive use of the Pinocchio library but adds planning-specific abstractions over its implementation.

Submodules

pyroboplan.core.planning_context module

class pyroboplan.core.planning_context.PlanningContext(model, visual_model=None, collision_model=None)

Bases: object

Defines a planning context, which contains Pinocchio models and necessary utilities for motion planning.

pyroboplan.core.utils module

Core utilities for robot modeling.

pyroboplan.core.utils.calculate_collision_vector_and_jacobians(model, collision_model, data, collision_data, pair_idx, q)

Given collision and distance results from collision model data, computes the collision vector and collision Jacobians at both collision points.

This is useful for algorithms that perform collision avoidance, such as IK and trajectory optimization.

Note that forward kinematics, collision, and distance checks must be evaluated first to populate the data and collision_data variables.

Parameters:

  • model (pinocchio.Model) – The model to use for Jacobian computation.

  • collision_model (pinocchio.Model) – The model to use for collision checking.

  • data (pinocchio.Data) – The model data to use for Jacobian computation.

  • collision_data (pinocchio.GeometryData) – The collision_model data to use for collision checking.

  • pair_idx (int) – The index of the collision pair from which to extract information.

  • q (array-like) – The joint configuration of the model.

Returns:

A tuple containing collision distance vector from frame1 to frame2, and the collision Jacobians at frame1 and frame2.

Return type:

tuple(array-like, array-like, array-like)

pyroboplan.core.utils.check_collisions_along_path(model, collision_model, q_path, data=None, collision_data=None, distance_padding=0.0)

Checks whether a path consisting of multiple joint configurations is collision-free.

Parameters:

  • model (pinocchio.Model) – The model from which to generate a random state.

  • collision_model (pinocchio.Model) – The model to use for collision checking.

  • q_path (list[array-like]) – A list of joint configurations describing the path.

  • data (pinocchio.Data, optional) – The model data to use for collision checking. If None, data is created automatically.

  • collision_data (pinocchio.GeometryData, optional) – The collision_model data to use for collision checking. If None, data is created automatically.

  • distance_padding (float, optional) – The padding, in meters, to use for distance to nearest collision.

Returns:

True if there are any collisions or minimum distance violations, otherwise False.

Return type:

bool

pyroboplan.core.utils.check_collisions_at_state(model, collision_model, q, data=None, collision_data=None, distance_padding=0.0)

Checks whether a specified joint configuration is collision-free.

Parameters:

  • model (pinocchio.Model) – The model to use for collision checking.

  • collision_model (pinocchio.Model) – The collision model to use for collision checking.

  • q (array-like) – The joint configuration of the model.

  • data (pinocchio.Data, optional) – The model data to use for collision checking. If None, data is created automatically.

  • collision_data (pinocchio.GeometryData, optional) – The collision_model data to use for collision checking. If None, data is created automatically.

  • distance_padding (float, optional) – The padding, in meters, to use for distance to nearest collision.

Returns:

True if there are any collisions or minimum distance violations, otherwise False.

Return type:

bool

pyroboplan.core.utils.check_within_limits(model, q)

Checks whether a particular joint configuration is within the model’s joint limits.

Parameters:

  • model (pinocchio.Model) – The model from which to generate a random state.

  • q (array-like) – The joint configuration for the model.

Returns:

True if the configuration is within joint limits, otherwise False.

Return type:

bool

pyroboplan.core.utils.configuration_distance(q_start, q_end)

Returns the distance between two joint configurations.

Parameters:

  • q_start (array-like) – The start joint configuration.

  • q_end (array-like) – The end joint configuration.

Returns:

The distance between the two joint configurations.

Return type:

float

pyroboplan.core.utils.extract_cartesian_pose(model, target_frame, q, data=None)

Extracts the Cartesian pose of a specified model frame given a joint configuration.

Parameters:

  • model (pinocchio.Model) – The model from which to perform forward kinematics.

  • target_frame (str) – The name of the target frame.

  • q (array-like) – The joint configuration values describing the robot state.

  • data (pinocchio.Data, optional) – The model data to use. If not set, one will be created.

Returns:

The transform describing the Cartesian pose of the specified frame at the provided joint configuration.

Return type:

pinocchio.SE3

pyroboplan.core.utils.extract_cartesian_poses(model, target_frame, q_vec, data=None)

Extracts the Cartesian poses of a specified model frame given a list of joint configurations.

Parameters:

  • model (pinocchio.Model) – The model from which to perform forward kinematics.

  • target_frame (str) – The name of the target frame.

  • q_vec (array-like) – A list of joint configuration values describing the path.

  • data (pinocchio.Data, optional) – The model data to use. If not set, one will be created.

Returns:

A list of transforms describing the Cartesian poses of the specified frame at the provided joint configurations.

Return type:

list[pinocchio.SE3]

pyroboplan.core.utils.get_collision_geometry_ids(model, collision_model, body)

Gets a list of collision geometry model IDs for a specified body name.

Parameters:

  • model (pinocchio.Model) – The model to use for getting frame IDs.

  • collision_model (pinocchio.Model) – The model to use for collision checking.

  • body (str) – The name of the body. This can be directly the name of a geometry in the collision model, or it can be the name of the frame in the main model.

Returns:

A list of collision geometry IDs corresponding to the body name.

Return type:

list[int]

pyroboplan.core.utils.get_collision_pair_indices_from_bodies(model, collision_model, body_list)

Returns a list of all the collision pair indices involving a list of objects.

Parameters:

  • model (pinocchio.Model) – The model to use for getting frame IDs.

  • collision_model (pinocchio.Model) – The model to use for collision checking.

  • body_list (list[str]) – A list containing the names of bodies. These can be directly the name of a geometry in the collision model, or they can be the name of the frame in the main model.

Returns:

The indices of the collision pairs list involving the bodies in the specified list.

Return type:

list[int]

pyroboplan.core.utils.get_minimum_distance_at_state(model, collision_model, q, data=None, collision_data=None)

Gets the minimum distance to collision at a specified state.

Parameters:

  • model (pinocchio.Model) – The model to use for collision checking.

  • collision_model (pinocchio.Model) – The collision model to use for collision checking.

  • q (array-like) – The joint configuration of the model.

  • data (pinocchio.Data, optional) – The model data to use for collision checking. If None, data is created automatically.

  • collision_data (pinocchio.GeometryData, optional) – The collision_model data to use for collision checking. If None, data is created automatically.

Returns:

The minimum distance to collision, in meters.

Return type:

float

pyroboplan.core.utils.get_path_length(q_path)

Returns the configuration distance of a path.

Parameters:

q_path (list[array-like]) – A list of joint configurations describing a path.

Returns:

The total configuration distance of the entire path.

Return type:

float

pyroboplan.core.utils.get_random_collision_free_state(model, collision_model, joint_padding=0.0, distance_padding=0.0, max_tries=100)

Returns a random state that is within the model’s joint limits and is collision-free according to the collision model.

Parameters:

  • model (pinocchio.Model) – The model from which to generate a random state.

  • collision_model (pinocchio.Model) – The model to use for collision checking.

  • joint_padding (float or array-like, optional) – The padding to use around the sampled joint limits.

  • distance_padding (float, optional) – The padding, in meters, to use for distance to nearest collision.

  • max_tries (int, optional) – The maximum number of tries for sampling a collision-free state.

Returns:

A set of randomly generated collision-free joint variables, or None if one cannot be found.

Return type:

array-like

pyroboplan.core.utils.get_random_collision_free_transform(model, collision_model, target_frame, joint_padding=0.0, distance_padding=0.0, max_tries=100)

Returns a random transform for a target frame that is within the model’s joint limits and is collision-free.

Parameters:

  • model (pinocchio.Model) – The model from which to generate a random transform.

  • collision_model (pinocchio.Model) – The model to use for collision checking.

  • target_frame (str) – The name of the frame for which to generate a random transform.

  • joint_padding (float or array-like, optional) – The padding to use around the sampled joint limits.

  • distance_padding (float, optional) – The padding, in meters, to use for distance to nearest collision.

  • max_tries (int, optional) – The maximum number of tries for sampling a collision-free state.

Returns:

A randomly generated transform for the specified target frame.

Return type:

pinocchio.SE3

pyroboplan.core.utils.get_random_state(model, padding=0.0)

Returns a random state that is within the model’s joint limits.

Parameters:

  • model (pinocchio.Model) – The model from which to generate a random state.

  • padding (float or array-like, optional) – The padding to use around the sampled joint limits.

Returns:

A set of randomly generated joint variables.

Return type:

array-like

pyroboplan.core.utils.get_random_transform(model, target_frame, joint_padding=0.0)

Returns a random transform for a target frame that is within the model’s joint limits.

Parameters:

  • model (pinocchio.Model) – The model from which to generate a random transform.

  • target_frame (str) – The name of the frame for which to generate a random transform.

  • joint_padding (float or array-like, optional) – The padding to use around the sampled joint limits.

Returns:

A randomly generated transform for the specified target frame.

Return type:

pinocchio.SE3

pyroboplan.core.utils.set_collisions(model, collision_model, body1, body2, enable)

Sets collision checking between two bodies by searching for their corresponding geometry objects in the collision model.

Parameters:

  • model (pinocchio.Model) – The model to use for getting frame IDs.

  • collision_model (pinocchio.Model) – The model to use for collision checking.

  • body1 (str) – The name of the first body.

  • body2 (str) – The name of the second body.

  • enable (bool) – If True, enables collisions. If False, disables collisions.