how to call PlanningScene from moveit_py?
adamanov opened this issue · 1 comments
adamanov commented
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 commented
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)