how to call PlanningScene from moveit_py?

[adamanov]

Hello,

I am having difficulty figuring out how to create a Python instance from the PlanningScene class to use functions like apply_collision_object and remove_all_collision_objects.

In a similar manner to functions such as:

def get_planning_component(self, *args, **kwargs) -> Any: ...
def get_planning_scene_monitor(self, *args, **kwargs) -> Any: ...
def get_robot_model(self, *args, **kwargs) -> Any: ...
def get_trajectory_execution_manager(self, *args, **kwargs) -> Any: ...

Is there a way to create an instance from the PlanningScene class to utilize its functions? I would appreciate it if you could share an example demonstrating how to do this.

Thanks!

[adamanov]

Figured it out :) Basically bellow with planning_scene_monitor.read_write() as scene: , scene will be part of PlanningScene

def add_collision_objects(planning_scene_monitor):
    """Helper function that adds collision objects to the planning scene."""
    object_positions = [
        (0.15, 0.1, 0.5),
        (0.25, 0.0, 1.0),
        (-0.25, -0.3, 0.8),
        (0.25, 0.3, 0.75),
    ]
    object_dimensions = [
        (0.1, 0.4, 0.1),
        (0.1, 0.4, 0.1),
        (0.2, 0.2, 0.2),
        (0.15, 0.15, 0.15),
    ]

    with planning_scene_monitor.read_write() as scene:
        collision_object = CollisionObject()
        collision_object.header.frame_id = "world"
        collision_object.id = "boxes"

        for position, dimensions in zip(object_positions, object_dimensions):
            box_pose = Pose()
            box_pose.position.x = position[0]
            box_pose.position.y = position[1]
            box_pose.position.z = position[2]

            box = SolidPrimitive()
            box.type = SolidPrimitive.BOX
            box.dimensions = dimensions

            collision_object.primitives.append(box)
            collision_object.primitive_poses.append(box_pose)
            collision_object.operation = CollisionObject.ADD

        scene.apply_collision_object(collision_object)
        scene.current_state.update()  # Important to ensure the scene is updated

add_collision_objects(_planning_scene_monitor)