PlanningWorld

class mplib.PlanningWorld

Bases: pybind11_object

Planning world for collision checking

Mimicking MoveIt2’s planning_scene::PlanningScene, collision_detection::World, moveit::core::RobotState, collision_detection::CollisionEnv

https://moveit.picknik.ai/main/api/html/classplanning__scene_1_1PlanningScene.html https://moveit.picknik.ai/main/api/html/classcollision__detection_1_1World.html https://moveit.picknik.ai/main/api/html/classmoveit_1_1core_1_1RobotState.html https://moveit.picknik.ai/main/api/html/classcollision__detection_1_1CollisionEnv.html

__init__(
self: mplib.pymp.PlanningWorld,
articulations: list[mplib.pymp.ArticulatedModel],
objects: list[mplib.pymp.collision_detection.fcl.FCLObject] = [],
) → None

Constructs a PlanningWorld with given (planned) articulations and objects

Parameters:

  • articulations – list of planned articulated models

  • objects – list of non-articulated collision objects

add_articulation(
self: mplib.pymp.PlanningWorld,
model: mplib.pymp.ArticulatedModel,
planned: bool = False,
) → None

Adds an articulation (ArticulatedModelPtr) to world

Parameters:

  • model – articulated model to be added

  • planned – whether the articulation is being planned

add_object(*args, **kwargs)

Overloaded function.

  1. add_object(self: mplib.pymp.PlanningWorld, fcl_obj: mplib.pymp.collision_detection.fcl.FCLObject) -> None

Adds an non-articulated object containing multiple collision objects (FCLObjectPtr) to world

Parameters:

fcl_obj – FCLObject to be added

  1. add_object(self: mplib.pymp.PlanningWorld, name: str, collision_object: mplib.pymp.collision_detection.fcl.CollisionObject) -> None

Adds an non-articulated object (CollisionObjectPtr) with given name to world

Parameters:

  • name – name of the collision object

  • collision_object – collision object to be added

add_point_cloud(
self: mplib.pymp.PlanningWorld,
name: str,
vertices: numpy.ndarray[numpy.float64[m, 3]],
resolution: float = 0.01,
) → None

Adds a point cloud as a collision object with given name to world

Parameters:

  • name – name of the point cloud collision object

  • vertices – point cloud vertices matrix

  • resolution – resolution of the point in octomap::OcTree

attach_box(
self: mplib.pymp.PlanningWorld,
size: numpy.ndarray[numpy.float64[3, 1]],
art_name: str,
link_id: int,
pose: mplib.pymp.Pose,
) → None

Attaches given box to specified link of articulation (auto touch_links)

Parameters:

  • size – box side length

  • art_name – name of the planned articulation to attach to

  • link_id – index of the link of the planned articulation to attach to

  • pose – attached pose (relative pose from attached link to object)

attach_mesh(
self: mplib.pymp.PlanningWorld,
mesh_path: str,
art_name: str,
link_id: int,
pose: mplib.pymp.Pose,
*,
convex: bool = False,
) → None

Attaches given mesh to specified link of articulation (auto touch_links)

Parameters:

  • mesh_path – path to a mesh file

  • art_name – name of the planned articulation to attach to

  • link_id – index of the link of the planned articulation to attach to

  • pose – attached pose (relative pose from attached link to object)

  • convex – whether to load mesh as a convex mesh. Default: False.

attach_object(*args, **kwargs)

Overloaded function.

  1. attach_object(self: mplib.pymp.PlanningWorld, name: str, art_name: str, link_id: int, touch_links: list[str]) -> None

Attaches existing non-articulated object to specified link of articulation at its current pose. If the object is currently attached, disallow collision between the object and previous touch_links.

Updates acm_ to allow collisions between attached object and touch_links.

Parameters:

  • name – name of the non-articulated object to attach

  • art_name – name of the planned articulation to attach to

  • link_id – index of the link of the planned articulation to attach to

  • touch_links – link names that the attached object touches

Raises:

ValueError – if non-articulated object with given name does not exist or if planned articulation with given name does not exist

  1. attach_object(self: mplib.pymp.PlanningWorld, name: str, art_name: str, link_id: int) -> None

Attaches existing non-articulated object to specified link of articulation at its current pose. If the object is not currently attached, automatically sets touch_links as the name of self links that collide with the object in the current state.

Updates acm_ to allow collisions between attached object and touch_links.

If the object is already attached, the touch_links of the attached object is preserved and acm_ remains unchanged.

Parameters:

  • name – name of the non-articulated object to attach

  • art_name – name of the planned articulation to attach to

  • link_id – index of the link of the planned articulation to attach to

Raises:

ValueError – if non-articulated object with given name does not exist or if planned articulation with given name does not exist

  1. attach_object(self: mplib.pymp.PlanningWorld, name: str, art_name: str, link_id: int, pose: mplib.pymp.Pose, touch_links: list[str]) -> None

Attaches existing non-articulated object to specified link of articulation at given pose. If the object is currently attached, disallow collision between the object and previous touch_links.

Updates acm_ to allow collisions between attached object and touch_links.

Parameters:

  • name – name of the non-articulated object to attach

  • art_name – name of the planned articulation to attach to

  • link_id – index of the link of the planned articulation to attach to

  • pose – attached pose (relative pose from attached link to object)

  • touch_links – link names that the attached object touches

Raises:

ValueError – if non-articulated object with given name does not exist or if planned articulation with given name does not exist

  1. attach_object(self: mplib.pymp.PlanningWorld, name: str, art_name: str, link_id: int, pose: mplib.pymp.Pose) -> None

Attaches existing non-articulated object to specified link of articulation at given pose. If the object is not currently attached, automatically sets touch_links as the name of self links that collide with the object in the current state.

Updates acm_ to allow collisions between attached object and touch_links.

If the object is already attached, the touch_links of the attached object is preserved and acm_ remains unchanged.

Parameters:

  • name – name of the non-articulated object to attach

  • art_name – name of the planned articulation to attach to

  • link_id – index of the link of the planned articulation to attach to

  • pose – attached pose (relative pose from attached link to object)

Raises:

ValueError – if non-articulated object with given name does not exist or if planned articulation with given name does not exist

  1. attach_object(self: mplib.pymp.PlanningWorld, name: str, p_geom: mplib.pymp.collision_detection.fcl.CollisionGeometry, art_name: str, link_id: int, pose: mplib.pymp.Pose, touch_links: list[str]) -> None

Attaches given object (w/ p_geom) to specified link of articulation at given pose. This is done by removing the object and then adding and attaching object. As a result, all previous acm_ entries with the object are removed

Parameters:

  • name – name of the non-articulated object to attach

  • p_geom – pointer to a CollisionGeometry object

  • art_name – name of the planned articulation to attach to

  • link_id – index of the link of the planned articulation to attach to

  • pose – attached pose (relative pose from attached link to object)

  • touch_links – link names that the attached object touches

  1. attach_object(self: mplib.pymp.PlanningWorld, name: str, p_geom: mplib.pymp.collision_detection.fcl.CollisionGeometry, art_name: str, link_id: int, pose: mplib.pymp.Pose) -> None

Attaches given object (w/ p_geom) to specified link of articulation at given pose. This is done by removing the object and then adding and attaching object. As a result, all previous acm_ entries with the object are removed. Automatically sets touch_links as the name of self links that collide with the object in the current state (auto touch_links).

Parameters:

  • name – name of the non-articulated object to attach

  • p_geom – pointer to a CollisionGeometry object

  • art_name – name of the planned articulation to attach to

  • link_id – index of the link of the planned articulation to attach to

  • pose – attached pose (relative pose from attached link to object)

attach_sphere(
self: mplib.pymp.PlanningWorld,
radius: float,
art_name: str,
link_id: int,
pose: mplib.pymp.Pose,
) → None

Attaches given sphere to specified link of articulation (auto touch_links)

Parameters:

  • radius – sphere radius

  • art_name – name of the planned articulation to attach to

  • link_id – index of the link of the planned articulation to attach to

  • pose – attached pose (relative pose from attached link to object)

check_collision(
self: mplib.pymp.PlanningWorld,
request: mplib.pymp.collision_detection.fcl.CollisionRequest = <mplib.pymp.collision_detection.fcl.CollisionRequest object at 0x7f5817c81fb0>,
) → list[mplib.pymp.collision_detection.WorldCollisionResult]

Check full collision (calls checkSelfCollision() and checkRobotCollision())

Parameters:

request – collision request params.

Returns:

List of WorldCollisionResult objects

check_robot_collision(
self: mplib.pymp.PlanningWorld,
request: mplib.pymp.collision_detection.fcl.CollisionRequest = <mplib.pymp.collision_detection.fcl.CollisionRequest object at 0x7f581d0708f0>,
) → list[mplib.pymp.collision_detection.WorldCollisionResult]

Check collision with other scene bodies in the world (planned articulations with attached objects collide against unplanned articulations and scene objects)

Parameters:

request – collision request params.

Returns:

List of WorldCollisionResult objects

check_self_collision(
self: mplib.pymp.PlanningWorld,
request: mplib.pymp.collision_detection.fcl.CollisionRequest = <mplib.pymp.collision_detection.fcl.CollisionRequest object at 0x7f58256eba30>,
) → list[mplib.pymp.collision_detection.WorldCollisionResult]

Check for self collision (including planned articulation self-collision, planned articulation-attach collision, attach-attach collision)

Parameters:

request – collision request params.

Returns:

List of WorldCollisionResult objects

detach_object(
self: mplib.pymp.PlanningWorld,
name: str,
also_remove: bool = False,
) → bool

Detaches object with given name. Updates acm_ to disallow collision between the object and touch_links.

Parameters:

  • name – name of the non-articulated object to detach

  • also_remove – whether to also remove object from world

Returns:

True if success, False if the object with given name is not attached

distance(
self: mplib.pymp.PlanningWorld,
request: mplib.pymp.collision_detection.fcl.DistanceRequest = <mplib.pymp.collision_detection.fcl.DistanceRequest object at 0x7f581735a070>,
) → mplib.pymp.collision_detection.WorldDistanceResult

Compute the minimum distance-to-all-collision (calls distanceSelf() and distanceRobot())

Parameters:

request – distance request params.

Returns:

a WorldDistanceResult object

distance_robot(
self: mplib.pymp.PlanningWorld,
request: mplib.pymp.collision_detection.fcl.DistanceRequest = <mplib.pymp.collision_detection.fcl.DistanceRequest object at 0x7f5817c69170>,
) → mplib.pymp.collision_detection.WorldDistanceResult

Compute the minimum distance-to-collision between a robot and the world

Parameters:

request – distance request params.

Returns:

a WorldDistanceResult object

distance_self(
self: mplib.pymp.PlanningWorld,
request: mplib.pymp.collision_detection.fcl.DistanceRequest = <mplib.pymp.collision_detection.fcl.DistanceRequest object at 0x7f581734bd30>,
) → mplib.pymp.collision_detection.WorldDistanceResult

Get the minimum distance to self-collision given the robot in current state

Parameters:

request – distance request params.

Returns:

a WorldDistanceResult object

distance_to_collision(self: mplib.pymp.PlanningWorld) → float

Compute the minimum distance-to-all-collision. Calls distance().

Note that this is different from MoveIt2’s planning_scene::PlanningScene::distanceToCollision() where self-collisions are ignored.

Returns:

minimum distance-to-all-collision

distance_to_robot_collision(self: mplib.pymp.PlanningWorld) → float

The distance between the robot model at current state to the nearest collision (ignoring self-collisions). Calls distanceRobot().

Returns:

minimum distance-to-robot-collision

distance_to_self_collision(self: mplib.pymp.PlanningWorld) → float

The minimum distance to self-collision given the robot in current state. Calls distanceSelf().

Returns:

minimum distance-to-self-collision

get_allowed_collision_matrix(
self: mplib.pymp.PlanningWorld,
) → mplib.pymp.collision_detection.AllowedCollisionMatrix

Get the current allowed collision matrix

get_articulation(
self: mplib.pymp.PlanningWorld,
name: str,
) → mplib.pymp.ArticulatedModel

Gets the articulation (ArticulatedModelPtr) with given name

Parameters:

name – name of the articulated model

Returns:

the articulated model with given name or None if not found.

get_articulation_names(self: mplib.pymp.PlanningWorld) → list[str]

Gets names of all articulations in world (unordered)

get_attached_object(
self: mplib.pymp.PlanningWorld,
name: str,
) → mplib.pymp.AttachedBody

Gets the attached body (AttachedBodyPtr) with given name

Parameters:

name – name of the attached body

Returns:

the attached body with given name or None if not found.

get_object(
self: mplib.pymp.PlanningWorld,
name: str,
) → mplib.pymp.collision_detection.fcl.FCLObject

Gets the non-articulated object (FCLObjectPtr) with given name

Parameters:

name – name of the non-articulated object

Returns:

the object with given name or None if not found.

get_object_names(self: mplib.pymp.PlanningWorld) → list[str]

Gets names of all objects in world (unordered)

get_planned_articulations(
self: mplib.pymp.PlanningWorld,
) → list[mplib.pymp.ArticulatedModel]

Gets all planned articulations (ArticulatedModelPtr)

has_articulation(self: mplib.pymp.PlanningWorld, name: str) → bool

Check whether the articulation with given name exists

Parameters:

name – name of the articulated model

Returns:

True if exists, False otherwise.

has_object(self: mplib.pymp.PlanningWorld, name: str) → bool

Check whether the non-articulated object with given name exists

Parameters:

name – name of the non-articulated object

Returns:

True if exists, False otherwise.

is_articulation_planned(self: mplib.pymp.PlanningWorld, name: str) → bool

Check whether the articulation with given name is being planned

Parameters:

name – name of the articulated model

Returns:

True if exists, False otherwise.

is_object_attached(self: mplib.pymp.PlanningWorld, name: str) → bool

Check whether the non-articulated object with given name is attached

Parameters:

name – name of the non-articulated object

Returns:

True if it is attached, False otherwise.

is_state_colliding(self: mplib.pymp.PlanningWorld) → bool

Check if the current state is in collision (with the environment or self collision).

Returns:

True if collision exists

print_attached_body_pose(self: mplib.pymp.PlanningWorld) → None

Prints global pose of all attached bodies

remove_articulation(self: mplib.pymp.PlanningWorld, name: str) → bool

Removes the articulation with given name if exists. Updates acm_

Parameters:

name – name of the articulated model

Returns:

True if success, False if articulation with given name does not exist

remove_object(self: mplib.pymp.PlanningWorld, name: str) → bool

Removes (and detaches) the collision object with given name if exists. Updates acm_

Parameters:

name – name of the non-articulated collision object

Returns:

True if success, False if the non-articulated object with given name does not exist

set_articulation_planned(
self: mplib.pymp.PlanningWorld,
name: str,
planned: bool,
) → None

Sets articulation with given name as being planned

Parameters:

  • name – name of the articulated model

  • planned – whether the articulation is being planned

Raises:

ValueError – if the articulation with given name does not exist

set_qpos(
self: mplib.pymp.PlanningWorld,
name: str,
qpos: numpy.ndarray[numpy.float64[m, 1]],
) → None

Set qpos of articulation with given name

Parameters:

  • name – name of the articulated model

  • qpos – joint angles of the movegroup only // FIXME: double check

set_qpos_all(
self: mplib.pymp.PlanningWorld,
state: numpy.ndarray[numpy.float64[m, 1]],
) → None

Set qpos of all planned articulations