planning_scene.isStateColliding() function reports a collision even though there is no collision [closed]

edit

I am using planning_scene.isStateColliding() to check for collisions in the planning scene.

Here's my initial environment 1. I have a mesh part placed on a turntable. It is added to the scene as a collision object. 2. Initially the part is added at the center of the turntable. 3. I have a fanuc robot properly configured using moveit setup assistant and works perfectly

My goal: 1. Turn the turntable by some angle, and rotate the part with respect to the center of the turntable. While rotating the part, checking if the part is colliding with the robot at discrete intervals (lets say I need to rotate 0.5 radians. I am discretising this 0.5 rad into steps of 0.17 rad and checking for state collision at each step). I am using planning_scene.isStateColliding() to check if there are any collisions with the robot

At some point in my code: 1. I moved the mesh part to a new pose far away from any collisions with the scene and applied the changes using apply_planning_scene service and published changes using planning_scene_diff_publisher.

Here's what I am doing next:

I receive planning_scene_msg_part_collision of moveit_msgs:planning_scene type from a different method. I got this planning_scene_msg by making a service request to get_planning_scene

I create a planning_scene object as below

  robot_model_loader::RobotModelLoader robot_model_loader_("robot_description");
  const moveit::core::RobotModelPtr& kinematic_model_ = robot_model_loader_.getModel();
  planning_scene::PlanningScene planning_scene_part_collision(kinematic_model_);

I update the robot state to a desired state

planning_scene_msg_part_collision.robot_state.joint_state.position = full_state;

I am transforming the collision object to a new pose. This new pose is not on the turntable and far away to cause any collisions with environment

planning_scene_msg_part_collision.world.collision_objects[0].mesh_poses[0].position.x = intermediate_poses[idx].position.x;
planning_scene_msg_part_collision.world.collision_objects[0].mesh_poses[0].position.y = intermediate_poses[idx].position.y;
planning_scene_msg_part_collision.world.collision_objects[0].mesh_poses[0].position.z = intermediate_poses[idx].position.z;
planning_scene_msg_part_collision.world.collision_objects[0].mesh_poses[0].orientation.x = intermediate_poses[idx].orientation.x;
planning_scene_msg_part_collision.world.collision_objects[0].mesh_poses[0].orientation.y = intermediate_poses[idx].orientation.y;
planning_scene_msg_part_collision.world.collision_objects[0].mesh_poses[0].orientation.z = intermediate_poses[idx].orientation.z;
planning_scene_msg_part_collision.world.collision_objects[0].mesh_poses[0].orientation.w = intermediate_poses[idx].orientation.w;

Now I'm creating the planning_scene_part_collision from planning_scene_msg_part_collision

planning_scene_part_collision.usePlanningSceneMsg(planning_scene_msg_part_collision);

I am then using the isStateColliding() to check if there is a collision between the part and the robot while rotating the part in discrete steps.

collision_exist = planning_scene_part_collision.isStateColliding(planning_scene_part_collision.getCurrentState());

My Issue

When my robot moves close to the turntable, since the part is not on the turntable anymore, I expect isStateColliding to report No collisions, but it reports that there is a collision as if the part never moved from its initial state. Ironically when I use the move_group.plan() to plan a path, the planner effectively works with no issues.

Where is the problem? Is my approach correct? Am I missing to update the planning scene anywhere? Thanks in Advance.

I verified my whole pipeline work as expected except for this below line

collision_exist = planning_scene_part_collision.isStateColliding(planning_scene_part_collision.getCurrentState());