ComputeIK fails when any collision objects are in the scene.

[ralphieraccoon]

I've got a tool swapping function using moveit task constructor:

bool swap_tool(std::string tool)
{

    std::shared_ptr<moveit::task_constructor::solvers::CartesianPath>
        cartesian_planner(
            new moveit::task_constructor::solvers::CartesianPath());

    cartesian_planner->setMaxVelocityScalingFactor(1.0);
    cartesian_planner->setMaxAccelerationScalingFactor(1.0);
    cartesian_planner->setStepSize(.01);
    cartesian_planner->setIKFrame(this->get_parameter("link_name").as_string());

    std::shared_ptr<moveit::task_constructor::solvers::PipelinePlanner>
        sampling_planner(new moveit::task_constructor::solvers::PipelinePlanner(
            std::make_shared<rclcpp::Node>(this->get_name())));
    sampling_planner->setProperty("goal_joint_tolerance", 1e-5);
    sampling_planner->setMaxVelocityScalingFactor(1.0);
    sampling_planner->setMaxAccelerationScalingFactor(1.0);

    moveit::task_constructor::TaskPtr task(
        new moveit::task_constructor::Task());

    task->stages()->setName("tool change");
    task->loadRobotModel(std::make_shared<rclcpp::Node>(this->get_name()));
    task->setProperty("group", this->get_parameter("group_name").as_string());
    task->setProperty("eef", this->get_parameter("link_name").as_string());
    task->setProperty("ik_frame", this->get_parameter("link_name").as_string());

    std::unique_ptr<moveit::task_constructor::stages::CurrentState>
        current_state(new moveit::task_constructor::stages::CurrentState(
            "current state"));

    moveit::task_constructor::Stage *monitor_ptr = current_state.get();

    geometry_msgs::msg::Vector3Stamped vec;
    vec.header.frame_id = this->get_parameter("link_name").as_string();

    double tool_approach_offset =
        this->get_parameter("tool_approach_offset").as_double();

    tf2::Quaternion orientation;
    orientation.setRPY(0.0f, M_PI, M_PI);

    std::vector<double> tool_box_origin =
        this->get_parameter("tool_box_origin").as_double_array();

    std::unique_ptr<moveit::task_constructor::stages::MoveRelative>
        tool_approach(new moveit::task_constructor::stages::MoveRelative(
            "approach tool", cartesian_planner));
    vec.vector.z = tool_approach_offset;
    tool_approach->setIKFrame(this->get_parameter("link_name").as_string());
    tool_approach->setForwardedProperties({"ik_frame"});
    tool_approach->setDirection(vec);
    tool_approach->properties().configureInitFrom(
        moveit::task_constructor::Stage::PARENT);
    tool_approach->setMinMaxDistance(tool_approach_offset,
                                     tool_approach_offset + 0.05);

    std::unique_ptr<moveit::task_constructor::stages::MoveRelative>
        tool_retract(new moveit::task_constructor::stages::MoveRelative(
            "retract tool", cartesian_planner));
    vec.vector.z = -tool_approach_offset;
    tool_retract->setIKFrame(this->get_parameter("link_name").as_string());
    tool_retract->setForwardedProperties({"ik_frame"});
    tool_retract->setDirection(vec);
    tool_retract->properties().configureInitFrom(
        moveit::task_constructor::Stage::PARENT);
    tool_retract->setMinMaxDistance(tool_approach_offset - 0.05,
                                    tool_approach_offset);

    std::unique_ptr<moveit::task_constructor::stages::GeneratePose>
        tool_location(new moveit::task_constructor::stages::GeneratePose(
            "tool location"));

    geometry_msgs::msg::PoseStamped pose;

    pose.header.frame_id = "world";

    geometry_msgs::msg::Point tool_origin =
        (!active_tool_.empty()) ? tools_.at(active_tool_).tool_origin
                                : tools_.at(tool).tool_origin;

    pose.pose.position.x = tool_box_origin[0] + tool_origin.x;
    pose.pose.position.y = tool_box_origin[1] + tool_origin.y;
    pose.pose.position.z = tool_box_origin[2] + tool_origin.z;

    pose.pose.orientation = tf2::toMsg(orientation);

    tool_location->setPose(pose);
    tool_location->setMonitoredStage(monitor_ptr);

    std::unique_ptr<moveit::task_constructor::stages::ComputeIK>
        tool_location_ik(new moveit::task_constructor::stages::ComputeIK(
            "tool location IK", std::move(tool_location)));
    tool_location_ik->setMaxIKSolutions(8);
    tool_location_ik->setMinSolutionDistance(1.0);
    tool_location_ik->setIKFrame(Eigen::Isometry3d().Identity(),
                                 this->get_parameter("link_name").as_string());
    tool_location_ik->properties().configureInitFrom(
        moveit::task_constructor::Stage::PARENT, {"eef", "group"});
    tool_location_ik->properties().configureInitFrom(
        moveit::task_constructor::Stage::INTERFACE, {"target_pose"});

    std::unique_ptr<moveit::task_constructor::stages::Connect>
        location_connector(new moveit::task_constructor::stages::Connect(
            "move to tool",
            moveit::task_constructor::stages::Connect::GroupPlannerVector{
                {this->get_parameter("group_name").as_string(),
                 sampling_planner}}));
    location_connector->setTimeout(5.0);
    location_connector->properties().configureInitFrom(
        moveit::task_constructor::Stage::PARENT);

    task->add(std::move(current_state));
    task->add(std::move(location_connector));
    task->add(std::move(tool_approach));
    task->add(std::move(tool_location_ik));

    if (!plan_and_execute_task(task))
    {
        // result->success = false;
        // goal_handle->abort(result);
        return false;
    }

    auto msg = std_msgs::msg::Bool();
    msg.data = (!active_tool_.empty()) ? false : true;
    this->chuck_magnet_switch_->publish(msg);

    rclcpp::Clock().sleep_for(rclcpp::Duration(5, 0));

    moveit_task_constructor_msgs::msg::Solution solution;

    task->solutions().front()->toMsg(solution, &task->introspection());

    std::unique_ptr<moveit::task_constructor::stages::FixedState> end_state(
        new moveit::task_constructor::stages::FixedState("end state"));

    planning_scene::PlanningScenePtr current_plan(
        new planning_scene::PlanningScene(task->getRobotModel()));
    current_plan->setCurrentState(solution.start_scene.robot_state);

    end_state->setState(current_plan);

    if (!active_tool_.empty() && !tool.empty())
    { // Pick new tool up.

        RCLCPP_INFO(this->get_logger(), "pick new tool up.");

        task->clear();

        current_state.reset(
            new moveit::task_constructor::stages::CurrentState("current state"));

        monitor_ptr = current_state.get();

        std::unique_ptr<moveit::task_constructor::stages::Connect>
            pick_place_connector(new moveit::task_constructor::stages::Connect(
                "move to next tool",
                moveit::task_constructor::stages::Connect::GroupPlannerVector{
                    {this->get_parameter("group_name").as_string(),
                     cartesian_planner}}));
        pick_place_connector->setTimeout(5.0);
        pick_place_connector->properties().configureInitFrom(
            moveit::task_constructor::Stage::PARENT);

        tool_location.reset(
            new moveit::task_constructor::stages::GeneratePose("tool location"));

        geometry_msgs::msg::PoseStamped pose;

        pose.header.frame_id = "world";

        geometry_msgs::msg::Point tool_origin = tools_.at(tool).tool_origin;

        pose.pose.position.x = tool_box_origin[0] + tool_origin.x;
        pose.pose.position.y = tool_box_origin[1] + tool_origin.y;
        pose.pose.position.z = tool_box_origin[2] + tool_origin.z;

        pose.pose.orientation = tf2::toMsg(orientation);

        tool_location->setMonitoredStage(monitor_ptr);
        tool_location->setPose(pose);

        tool_location_ik.reset(new moveit::task_constructor::stages::ComputeIK(
            "tool location IK", std::move(tool_location)));
        tool_location_ik->setMaxIKSolutions(8);
        tool_location_ik->setMinSolutionDistance(1.0);
        tool_location_ik->setIKFrame(
            Eigen::Isometry3d().Identity(),
            this->get_parameter("link_name").as_string());
        tool_location_ik->properties().configureInitFrom(
            moveit::task_constructor::Stage::PARENT, {"eef", "group"});
        tool_location_ik->properties().configureInitFrom(
            moveit::task_constructor::Stage::INTERFACE, {"target_pose"});

        tool_approach.reset(new moveit::task_constructor::stages::MoveRelative(
            "approach tool", cartesian_planner));
        vec.vector.z = tool_approach_offset;
        tool_approach->setIKFrame(this->get_parameter("link_name").as_string());
        tool_approach->setForwardedProperties({"ik_frame"});
        tool_approach->setDirection(vec);
        tool_approach->properties().configureInitFrom(
            moveit::task_constructor::Stage::PARENT);
        tool_approach->setMinMaxDistance(tool_approach_offset,
                                         tool_approach_offset + 0.05);

        task->add(std::move(current_state));
        task->add(std::move(tool_retract));
        task->add(std::move(pick_place_connector));
        task->add(std::move(tool_approach));
        task->add(std::move(tool_location_ik));
        if (!plan_and_execute_task(task))
        {
            // result->success = false;
            // goal_handle->abort(result);
            return false;
        }

        auto msg = std_msgs::msg::Bool();
        msg.data = true;
        this->chuck_magnet_switch_->publish(msg);

        rclcpp::Clock().sleep_for(rclcpp::Duration(5, 0));

        tool_retract.reset(new moveit::task_constructor::stages::MoveRelative(
            "retract tool", cartesian_planner));
        vec.vector.z = -tool_approach_offset;
        tool_retract->setIKFrame(this->get_parameter("link_name").as_string());
        tool_retract->setForwardedProperties({"ik_frame"});
        tool_retract->setDirection(vec);
        tool_retract->properties().configureInitFrom(
            moveit::task_constructor::Stage::PARENT);
        tool_retract->setMinMaxDistance(tool_approach_offset - 0.05,
                                        tool_approach_offset);
    }

    task->clear();

    current_state.reset(
        new moveit::task_constructor::stages::CurrentState("current state"));

    monitor_ptr = current_state.get();

    std::unique_ptr<moveit::task_constructor::stages::Connect> end_connector(
        new moveit::task_constructor::stages::Connect(
            "move to end",
            moveit::task_constructor::stages::Connect::GroupPlannerVector{
                {this->get_parameter("group_name").as_string(),
                 sampling_planner}}));
    end_connector->setTimeout(5.0);
    end_connector->properties().configureInitFrom(
        moveit::task_constructor::Stage::PARENT);

    task->add(std::move(current_state));
    task->add(std::move(tool_retract));
    task->add(std::move(end_connector));
    task->add(std::move(end_state));
    if (!plan_and_execute_task(task))
    {
        return false;
    }
    active_tool_ = tool;
    vaccuum_gripper_ = this->create_publisher<std_msgs::msg::Bool>(
        "/tools/" + tool + "/" + tool + "_gripper/turn_on", 10);
    return true;
}

It works perfectly when I have no collision objects in the planning scene. However, I need the arm to avoid colliding with static objects in the scene when performing the tool change.

But when I add an object into the planning scene using the following in the class constructor:

rclcpp::Publisher<moveit_msgs::msg::PlanningScene>::SharedPtr
  planning_scene_diff_publisher =
      this->create_publisher<moveit_msgs::msg::PlanningScene>(
          "/planning_scene", 1);

moveit_msgs::msg::PlanningScene planning_scene;

planning_scene.is_diff = true;

moveit_msgs::msg::CollisionObject boundaries;

boundaries.header.frame_id = "world";
boundaries.id = "boundary";
boundaries.operation = boundaries.ADD;

shape_msgs::msg::SolidPrimitive rail_box;
geometry_msgs::msg::Pose rail_box_pose;

rail_box_pose.position.x = -1.355 / 2;
rail_box_pose.position.y = 0.640 / 2;
rail_box_pose.position.z = 0.082 / 2;

rail_box_pose.orientation.x = 0.0;
rail_box_pose.orientation.y = 0.0;
rail_box_pose.orientation.z = 0.0;
rail_box_pose.orientation.w = 1.0;

rail_box.type = rail_box.BOX;
rail_box.dimensions.resize(3);
rail_box.dimensions[0] = 1.355;
rail_box.dimensions[1] = 0.640;
rail_box.dimensions[2] = 0.01;

boundaries.primitives.push_back(rail_box);
boundaries.primitive_poses.push_back(rail_box_pose);

planning_scene.world.collision_objects.push_back(boundaries);

planning_scene_diff_publisher->publish(planning_scene);

ComputeIK fails every time, regardless of where the collision object is, even if it's nowhere near the end effector or the arm and no IK plan could possibly collide with it. Here's the relevant logs:

[robot_arm_core-17]   0  - ←   0 →   -  0 / tool change
[robot_arm_core-17]     1  - ←   1 →   -  1 / current state
[robot_arm_core-17]     -  1 →   0 ←   0  - / move to tool
[robot_arm_core-17]     0  - ←   0 ←   0  - / approach tool
[robot_arm_core-17]     0  - ←   0 →   -  0 / tool location IK
[robot_arm_core-17]       1  - ←   1 →   -  1 / tool location
[robot_arm_core-17] Failing stage(s):
[robot_arm_core-17] tool location IK (0/1)
[robot_arm_core-17] [ERROR] [1738260711.215896384] [robot_arm_core]: Task constructor plan failed: PLANNING_FAILED

Thanks!

[rhaschke]

Probably this is related to your setup. Is there any error message from ComputeIK which could explain the failure? Did you look at the failure comment reported in the rviz GUI?

[ralphieraccoon]

No other error messages on the log as far as I can see, no failure comments in the rviz GUI (screenshot below):

Here are the robot arm URDF and SRDF if they are of any help:

<?xml version="1.0" encoding="UTF-8"?>
<!--This does not replace URDF, and is not an extension of URDF.
    This is a format for representing semantic information about the robot structure.
    A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
-->
<robot name="fairino3_v6_robot">
    <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
    <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
    <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
    <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
    <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
    <group name="fairino3_v6_group">
        <joint name="j1"/>
        <joint name="j2"/>
        <joint name="j3"/>
        <joint name="j4"/>
        <joint name="j5"/>
        <joint name="j6"/>
        <joint name="eef"/>
        <chain base_link="base_link" tip_link="end_effector"/>
    </group>
    <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
    <group_state name="pos1" group="fairino3_v6_group">
        <joint name="j1" value="2.4468"/>
        <joint name="j2" value="-1.6214"/>
        <joint name="j3" value="1.5465"/>
        <joint name="j4" value="-1.5877"/>
        <joint name="j5" value="-1.6368"/>
        <joint name="j6" value="0"/>
    </group_state>
    <group_state name="pos2" group="fairino3_v6_group">
        <joint name="j1" value="0.8606"/>
        <joint name="j2" value="-1.4189"/>
        <joint name="j3" value="1.5465"/>
        <joint name="j4" value="-1.7227"/>
        <joint name="j5" value="-1.5018"/>
        <joint name="j6" value="0"/>
    </group_state>
    <end_effector name="end_effector" parent_link="end_effector" group="fairino3_v6_group" />
    <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
    <disable_collisions link1="base_link" link2="shoulder_link" reason="Adjacent"/>
    <disable_collisions link1="base_link" link2="upperarm_link" reason="Never"/>
    <disable_collisions link1="forearm_link" link2="upperarm_link" reason="Adjacent"/>
    <disable_collisions link1="forearm_link" link2="wrist1_link" reason="Adjacent"/>
    <disable_collisions link1="shoulder_link" link2="upperarm_link" reason="Adjacent"/>
    <disable_collisions link1="shoulder_link" link2="wrist1_link" reason="Never"/>
    <disable_collisions link1="wrist1_link" link2="wrist2_link" reason="Adjacent"/>
    <disable_collisions link1="wrist1_link" link2="wrist3_link" reason="Never"/>
    <disable_collisions link1="wrist2_link" link2="wrist3_link" reason="Adjacent"/>
</robot>

<?xml version="1.0" encoding="utf-8"?>
<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com) 
     Commit Version: 1.5.1-0-g916b5db  Build Version: 1.5.7152.31018
     For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
<robot
  name="fairino3_v6_robot">
  <link
    name="base_link">
    <inertial>
      <origin
        xyz="-0.00049027 -0.00045095 0.032527"
        rpy="0 0 0" />
      <mass
        value="0.78393" />
      <inertia
        ixx="0.0013854"
        ixy="1.1455E-05"
        ixz="-1.0642E-05"
        iyy="0.0013849"
        iyz="-9.7435E-06"
        izz="0.001977" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://fairino_description/meshes/fairino3_v6/base_link.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.89804 0.91765 0.92941 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://fairino_description/meshes/fairino3_v6/base_link.STL" />
      </geometry>
    </collision>
  </link>
  <link
    name="shoulder_link">
    <inertial>
      <origin
        xyz="-3.8658E-07 -0.0024176 0.13519"
        rpy="0 0 0" />
      <mass
        value="2.1225" />
      <inertia
        ixx="0.0033227"
        ixy="-5.0648E-08"
        ixz="-6.3401E-09"
        iyy="0.0032419"
        iyz="4.3768E-05"
        izz="0.0023129" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://fairino_description/meshes/fairino3_v6/shoulder_link.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.89804 0.91765 0.92941 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://fairino_description/meshes/fairino3_v6/shoulder_link.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="j1"
    type="revolute">
    <origin
      xyz="0 0 0"
      rpy="0 0 0" />
    <parent
      link="base_link" />
    <child
      link="shoulder_link" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="-3.0543"
      upper="3.0543"
      effort="150"
      velocity="3.15" />
    <calibration
      rising="0"
      falling="0" />
    <dynamics
      damping="0"
      friction="0" />
    <safety_controller
      soft_lower_limit="-3.0543"
      soft_upper_limit="3.0543"
      k_position="15"
      k_velocity="10" />
  </joint>
  <link
    name="upperarm_link">
    <inertial>
      <origin
        xyz="-0.14111 3.4656E-06 0.11053"
        rpy="0 0 0" />
      <mass
        value="4.8053" />
      <inertia
        ixx="0.0075945"
        ixy="3.7143E-07"
        ixz="-7.9228E-12"
        iyy="0.0081317"
        iyz="-1.2918E-11"
        izz="0.0059231" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://fairino_description/meshes/fairino3_v6/upperarm_link.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.89804 0.91765 0.92941 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://fairino_description/meshes/fairino3_v6/upperarm_link.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="j2"
    type="revolute">
    <origin
      xyz="0 0 0.14"
      rpy="1.5708 0 0" />
    <parent
      link="shoulder_link" />
    <child
      link="upperarm_link" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="-4.6251"
      upper="1.4835"
      effort="150"
      velocity="3.15" />
    <calibration
      rising="0"
      falling="0" />
    <dynamics
      damping="0"
      friction="0" />
    <safety_controller
      soft_lower_limit="-4.6251"
      soft_upper_limit="1.4835"
      k_position="15"
      k_velocity="10" />
  </joint>
  <link
    name="forearm_link">
    <inertial>
      <origin
        xyz="-0.18987 5.2268E-06 0.0034145"
        rpy="0 0 0" />
      <mass
        value="2.2312" />
      <inertia
        ixx="0.0028852"
        ixy="7.3389E-07"
        ixz="-3.5373E-05"
        iyy="0.0035145"
        iyz="-2.8527E-07"
        izz="0.0029186" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://fairino_description/meshes/fairino3_v6/forearm_link.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.89804 0.91765 0.92941 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://fairino_description/meshes/fairino3_v6/forearm_link.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="j3"
    type="revolute">
    <origin
      xyz="-0.28 0 0"
      rpy="0 0 0" />
    <parent
      link="upperarm_link" />
    <child
      link="forearm_link" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="-2.8274"
      upper="2.8274"
      effort="150"
      velocity="3.15" />
    <calibration
      rising="0"
      falling="0" />
    <dynamics
      damping="0"
      friction="0" />
    <safety_controller
      soft_lower_limit="-2.8274"
      soft_upper_limit="2.8274"
      k_position="15"
      k_velocity="10" />
  </joint>
  <link
    name="wrist1_link">
    <inertial>
      <origin
        xyz="4.5665E-07 -0.0034666 0.098292"
        rpy="0 0 0" />
      <mass
        value="1.5961" />
      <inertia
        ixx="0.0020112"
        ixy="6.8085E-09"
        ixz="2.4945E-08"
        iyy="0.0014524"
        iyz="3.2511E-05"
        izz="0.0019076" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://fairino_description/meshes/fairino3_v6/wrist1_link.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.89804 0.91765 0.92941 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://fairino_description/meshes/fairino3_v6/wrist1_link.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="j4"
    type="revolute">
    <origin
      xyz="-0.24001 0 0"
      rpy="0 0 0" />
    <parent
      link="forearm_link" />
    <child
      link="wrist1_link" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="-4.6251"
      upper="1.4835"
      effort="28"
      velocity="3.2" />
    <calibration
      rising="0"
      falling="0" />
    <dynamics
      damping="0"
      friction="0" />
    <safety_controller
      soft_lower_limit="-4.6251"
      soft_upper_limit="1.4835"
      k_position="15"
      k_velocity="10" />
  </joint>
  <link
    name="wrist2_link">
    <inertial>
      <origin
        xyz="-4.5262E-07 0.0034666 0.098292"
        rpy="0 0 0" />
      <mass
        value="1.5961" />
      <inertia
        ixx="0.0020112"
        ixy="6.8086E-09"
        ixz="-2.4945E-08"
        iyy="0.0014524"
        iyz="-3.2511E-05"
        izz="0.0019076" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://fairino_description/meshes/fairino3_v6/wrist2_link.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.89804 0.91765 0.92941 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://fairino_description/meshes/fairino3_v6/wrist2_link.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="j5"
    type="revolute">
    <origin
      xyz="0 0 0.102"
      rpy="1.5708 0 0" />
    <parent
      link="wrist1_link" />
    <child
      link="wrist2_link" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="-3.0543"
      upper="3.0543"
      effort="28"
      velocity="3.2" />
    <calibration
      rising="0"
      falling="0" />
    <dynamics
      damping="0"
      friction="0" />
    <safety_controller
      soft_lower_limit="-3.0543"
      soft_upper_limit="3.0543"
      k_position="15"
      k_velocity="10" />
  </joint>
  <link
    name="wrist3_link">
    <inertial>
      <origin
        xyz="7.559E-05 1.3858E-05 0.076619"
        rpy="0 0 0" />
      <mass
        value="0.53877" />
      <inertia
        ixx="0.00028785"
        ixy="-2.0584E-09"
        ixz="-1.3892E-07"
        iyy="0.00028896"
        iyz="1.8115E-07"
        izz="0.00042655" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://fairino_description/meshes/fairino3_v6/wrist3_link.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.89804 0.91765 0.92941 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://fairino_description/meshes/fairino3_v6/wrist3_link.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="j6"
    type="revolute">
    <origin
      xyz="0 0 0.102"
      rpy="-1.5708 0 0" />
    <parent
      link="wrist2_link" />
    <child
      link="wrist3_link" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="-3.0543"
      upper="3.0543"
      effort="28"
      velocity="3.2" />
    <calibration
      rising="0"
      falling="0" />
    <dynamics
      damping="0"
      friction="0" />
    <safety_controller
      soft_lower_limit="-3.0543"
      soft_upper_limit="3.0543"
      k_position="15"
      k_velocity="10" />
  </joint>
  <link name="end_effector" />
  <joint name="eef" type="fixed">
      <origin
      xyz="0 0 0.164"
      rpy="0 0 0" />
    <parent
      link="wrist3_link" />
    <child
      link="end_effector" />
  </joint>
</robot>

[ralphieraccoon]

Occasionally, for a reason I can't fathom (some kind of race condition?), the first task does execute, but then the second task fails with the following log:

[robot_arm_core-17] [ERROR] [1738282299.600385473] [robot_arm_core]: Task constructor plan failed: PLANNING_FAILED
[robot_arm_core-17]   0  - ←   0 →   -  0 / tool change
[robot_arm_core-17]     1  - ←   1 →   -  0 / current state
[robot_arm_core-17]     -  0 →   1 →   -  1 / retract tool
[robot_arm_core-17]     -  1 →   0 ←   1  - / move to end
[robot_arm_core-17]     1  - ←   1 →   -  1 / end state
[robot_arm_core-17] Failing stage(s):

No name of which failing stage is given, the log just continues.

Similar screenshot of rviz GUI:

[rhaschke]

Please provide a .zip file comprising a fully self-contained colcon or catkin workspace (you didn't even specify the ROS version you are using). With only a few files provided, one cannot attempt reproducing an issue.

[ralphieraccoon]

My apologies, I'm using ROS Jazzy on Ubuntu 24.04 (Noble Numbat). The /src directory of my colcon workspace is over 800MB so I can't upload it here. You can download a copy from here. Is that OK of do you want the
/build,/install and /log directories as well?

[DaniGarciaLopez]

Jumping into the conversation to comment that I recently experienced something similar, also in Jazzy. The task fails without providing any meaningful message about the stage at which it occurs (no stage is marked as failed, and the logging "Failing stage(s):" doesn't specify any message).

I haven't had time to investigate deeply, but I was able to replicate the issue by simply changing the ik_frame in the ComputeIK stage to one that doesn't exist in the scene. Like @ralphieraccoon, I am also using trac_ik, so maybe there is a problem with this kinematics solver.

I don't think this is related to the former issue, but I hope this helps narrow down the problem when ComputeIK fails silently.

[rhaschke]

@ralphieraccoon: The sources are sufficient. However, please narrow down your workspace to the minimum required to reproduce your issue. Also, I need to know, which launch files to start.

@DaniGarciaLopez: Indeed, the failing stage(s) error message is empty in that case. However, a warning is issued:
WARN 11:15:08 ros.moveit_task_constructor_core.ComputeIK: ik frame unknown in robot: 'foo'

[DaniGarciaLopez]

@DaniGarciaLopez: Indeed, the failing stage(s) error message is empty in that case. However, a warning is issued:
WARN 11:15:08 ros.moveit_task_constructor_core.ComputeIK: ik frame unknown in robot: 'foo'

That's right, I also get the warning. However, when you have a big task with several ComputeIK stages, it is difficult to determine which one failed just by looking at the log. I think it would be much easier to debug if the solution were directly marked as a failure with a meaningful message so you could check in rviz exactly which stage failed.

I just took a look at the code and it should be pretty straightforward to make the changes. I might have time in the following days to prepare a PR if you agree @rhaschke.

[rhaschke]

@DaniGarciaLopez, thanks for offering help. I already implemented the change quickly: #651.

[rhaschke]

@ralphieraccoon: Looking at #650 (comment), a possible reason for your failure might be incompatible states feeding into both ends of the Connect stage. These are silently filtered out. You may enable debug-level logging and see some warnings on the command-line.

[ralphieraccoon]

@ralphieraccoon: The sources are sufficient. However, please narrow down your workspace to the minimum required to reproduce your issue. Also, I need to know, which launch files to start.

@DaniGarciaLopez: Indeed, the failing stage(s) error message is empty in that case. However, a warning is issued: WARN 11:15:08 ros.moveit_task_constructor_core.ComputeIK: ik frame unknown in robot: 'foo'

Hi, I've updated the zip file to only include packages needed, same link as before around 110MB. To reproduce the problem run ros2 launch ampi_project launch_webots.py in the terminal, and in a new window run ros2 action send_goal /swap_tool ampi_project_interfaces/action/SwapTool "{tool: shaft_chuck_2_0mm}".

Bear in mind webots is needed as a dependency, though you could probably comment out parts of the launch file to disable it.

[rhaschke]

Bear in mind webots is needed as a dependency, though you could probably comment out parts of the launch file to disable it.

Please do so.

[ralphieraccoon]

OK, should work without webots now.

[ralphieraccoon]

Thanks for the tip about the debug. I think there are two issues at play here:

• trac_ik isn't always finding non-colliding solutions even when they exist, which causes ComputeIK to fail. This causes this debug message: [introspection]: new solution #3 (tool location IK): inf Collision between 'upperarm_link' and 'boundary' in my case. This is a problem with trac_ik and not the task constructor as @DaniGarciaLopez found out. Switching to a different IK plugin should solve this problem for now.
• When trac_ik does find a non-colliding solution, after the task is cleared for the retract operation this debug message occurs: [Connecting]: move to end: different number of collision objects. I wonder if I need to allow collisions with a ModifyPlanningScene stage, but the tutorial files suggested that wasn't necessary.

[rhaschke]

The latter issue is related to #650 (comment): The states received from the start and end interface of your Connect stage differ in their collision objects, which must not happen.
Unfortunately, your code is too convolved (and not documented) to understand what you are trying to do.
Building your workspace failed for me too.

[ralphieraccoon]

I think I've fixed the problem, it seems to be an issue with the end_state stage, which is a FixedState that captures the CurrentState at the start, to allow the robot to return to the position it started from. When creating the stage a PlanningScene is created from task->getRobotModel() which doesn't include the collision objects:

std::unique_ptr<moveit::task_constructor::stages::FixedState> end_state(
    new moveit::task_constructor::stages::FixedState("end state"));

planning_scene::PlanningScenePtr current_plan(
    new planning_scene::PlanningScene(task->getRobotModel()));
current_plan->setCurrentState(solution.start_scene.robot_state);

end_state->setState(current_plan);

The solution in my case was to add the world argument to the constructor of PlanningScene from a PlanningSceneMonitor I was already using for something else:

planning_scene_monitor::LockedPlanningSceneRW ls(servo_planning_scene_monitor_);
planning_scene::PlanningScenePtr current_plan(
    new planning_scene::PlanningScene(task->getRobotModel(), ls->getWorldNonConst()));

This doesn't feel like the best way to do things for me, perhaps there is a more elegant solution. It would be nice if there was a function to create a FixedState that is a "capture" of a CurrentState so the robot can return to this saved state later on. Perhaps as an additional constructor. I'll look into it when I have time. Or maybe there's already a better solution I don't know about. Thanks @rhaschke and @DaniGarciaLopez for your help, I know it was a struggle at times and apologies for not being as on the ball as I should have been.

[rhaschke]

You could explicitly fetch the current scene using the PlanningSceneInterface (or the corresponding getPlanningScene service).