"Goal State is in Collision" Error Despite Disabling Collisions

[ammaraljodah]

I am working with the MoveIt Task Constructor to set up a robotic task sequence and am encountering a persistent collision detection issue despite explicitly disabling collisions for certain links. I have tried disabling global collisions for a specific object called ("tool1") against all links in the planning scene, but the task still fails with a "Goal state is in collision" error. I've confirmed that the collision disabling stage is executed before any planning or motion execution stages. Below is the relevant part of my code where I set up the collision disabling:

my task setup:

// task setup 
moveit::task_constructor::TaskPtr task_;
task_.reset();
task_.reset(new moveit::task_constructor::Task());
Task &t = *task_;
t.stages()->setName("demo task");
t.loadRobotModel(node_);

// start with the current state
auto stage_state_current = std::make_unique<mtc::stages::CurrentState>("current");
t.add(std::move(stage_state_current));

This is how I add the collision object:

// Create the tool as a collision object
moveit_msgs::msg::CollisionObject tool1;
tool1.id = "tool1";
tool1.header.frame_id = "base_link";
tool1.primitives.resize(1);
tool1.primitives[0].type = shape_msgs::msg::SolidPrimitive::CYLINDER;
tool1.primitives[0].dimensions = {tools["tool1"].length, tools["tool1"].radius};

geometry_msgs::msg::Pose pose;
pose.position.x = tools["tool1"].pose_x;
pose.position.y = tools["tool1"].pose_y;
pose.position.z = tools["tool1"].pose_z; 

tf2::Quaternion myQuaternion;
// Rotate 90 degrees around the Y-axis
myQuaternion.setRPY(tools["tool1"].pose_roll, tools["tool1"].pose_pitch, tools["tool1"].pose_yaw);
// Convert quaternion to message format
pose.orientation = tf2::toMsg(myQuaternion);

tool1.pose = pose;

moveit::planning_interface::PlanningSceneInterface psi;
psi.applyCollisionObject(tool1);

This is how I disable the collision

// Setting up the ModifyPlanningScene stage to disable collisions 
auto disable_collision = std::make_unique<mtc::stages::ModifyPlanningScene>(
"disable collision for tool1 and grippers/fingers"); 

disable_collision->allowCollisions( "tool1", all_links, false); 

t.add(std::move(disable_collision)); 
// Subsequent stages where the robot plans and executes movements 
// ... code that sets up the task and motion planning ... 

Then closing the fingers on the object:

auto stage = std::make_unique<stages::MoveTo>("close fingers", interpolation_planner);
        stage->setGroup(fingers_group);
        stage->setGoal("close");
        t.add(std::move(stage));

Despite this configuration, the stage named "close fingers" reports a collision. I have checked the execution order and used RViz to visually inspect the scene. All links are included, and debugging outputs confirm that the stage is processed.

Here is what I have already checked:

• Execution order of stages.

• Correctness of all_links vector (contains all relevant link names).

• RViz visual inspection shows no unexpected overlaps.

• Increased debugging verbosity around collision settings application.

• Tested with different motion planners.

I am using ROS2 humble, and MoveIt 2.

Could there be a misunderstanding on my part regarding how collision settings are supposed to be applied, or might there be a bug or limitation in the software versions I am using?

Thank you!

[rhaschke]

Allow collisions, not disallow them:

-disable_collision->allowCollisions( "tool1", all_links, false);
+disable_collision->allowCollisions( "tool1", all_links, true);

[ammaraljodah]

I want the gripper to grasp the tool, thus I need to disable collision, is that the right logic?

void moveit::task_constructor::stages::ModifyPlanningScene::allowCollisions (const std::string & first,
const T & second,
bool    enable_collision )

[gavanderhoorn]

Cross-posted to RSE it seems: Persistent "Goal State is in Collision" Error Despite Disabling Collisions

[LeroyR]

Can you guys check the posted answer

Allow collisions, not disallow them:

-disable_collision->allowCollisions( "tool1", all_links, false);
+disable_collision->allowCollisions( "tool1", all_links, true);

you are not disabling collisions, you set allow_collision to false

[ammaraljodah]

-disable_collision->allowCollisions( "tool1", all_links, false);
+disable_collision->allowCollisions( "tool1", all_links, true);

Yes that fixed the issue, many thanks @rhaschke