MoveIt Task Constructor: RRTConnect Unable to Sample Valid States on ROS2 Humble

Question

MoveIt Task Constructor: RRTConnect Unable to Sample Valid States on ROS2 Humble

Introduction: I am developing a robotic application using ROS 2 and MoveIt for a Motoman GP4 robot. The project involves creating a task constructor node (MTCTaskNode) that receives commands and executes predefined tasks. The setup includes:

• MTCTaskNode Class: Manages task creation and execution using MoveIt Task Constructor. It handles commands through a ROS 2 service (task_command) and validates goal poses.
• Task Command Client: A client node that sends commands to MTCTaskNode, triggering the robot to move to predefined positions.

Problem Description: I have created a task constructor node (MTCTaskNode) to execute predefined tasks based on commands. However, I'm encountering an error during the execution of the move_to_target stage:

[task_constructor_example-6] [ERROR] [1722878843.757147134] [ompl]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:252 - gp4_arm/gp4_arm: Unable to sample any valid states for goal tree

Possible Issue: I suspect the issue might be related to the robot configuration, specifically the addition of a new link to the last joint of the robot. This could be causing a disconnect or misconfiguration that prevents the planner from finding valid states.

Relevant Functions in MTCTaskNode:

doTask: This function handles the execution of tasks based on commands:

void MTCTaskNode::doTask(const std::string& command)
{
    if (!rclcpp::ok()) return;

    geometry_msgs::msg::PoseStamped target_pose;

    if (command == "home") {
        target_pose = createPose(0.0, 0.0, 0.0);
    } else if (command == "pointA") {
        target_pose = createPose(0.1, 0.0, 0.1);
    } else if (command == "pointB") {
        target_pose = createPose(0.1, 0.1, 0.1);
    } else if (command == "pointC") {
        target_pose = createPose(0.0, 0.1, 0.1);
    } else {
        RCLCPP_ERROR(node_->get_logger(), "Unknown command: %s", command.c_str());
        return;
    }

    task_ = createTask(target_pose);

    try {
        task_.init();
    } 
    catch (const mtc::InitStageException& e) {
        RCLCPP_ERROR(node_->get_logger(), "Task initialization failed: %s", e.what());
        return;
    } 
    catch (const std::exception &e) {
        RCLCPP_ERROR(node_->get_logger(), "Standard exception during task initialization: %s", e.what());
        return;
    } 
    catch (...) {
        RCLCPP_ERROR(node_->get_logger(), "Unknown exception during task initialization");
        return;
    }

    if (!task_.plan(5)) {
        RCLCPP_ERROR(node_->get_logger(), "Task planning failed");
        return;
    }

    task_.introspection().publishSolution(*task_.solutions().front());

    auto result = task_.execute(*task_.solutions().front());
    if (result.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) {
        RCLCPP_ERROR(node_->get_logger(), "Task execution failed");
    }
}

createPose: This function generates a pose based on the given coordinates:

geometry_msgs::msg::PoseStamped MTCTaskNode::createPose(double x, double y, double z)
{
    geometry_msgs::msg::PoseStamped pose;
    pose.header.frame_id = "base_link";
    pose.header.stamp = node_->now();
    pose.pose.position.x = x;
    pose.pose.position.y = y;
    pose.pose.position.z = z;

    tf2::Quaternion q;
    q.setRPY(0, 0, 0);
    pose.pose.orientation = tf2::toMsg(q);

    return pose;
}

createTask: This function sets up the task with the necessary stages:

mtc::Task MTCTaskNode::createTask(const geometry_msgs::msg::PoseStamped& target_pose)
{
    mtc::Task task;
    task.stages()->setName("demo task");

    try {
        task.loadRobotModel(node_);
    } 
    catch (const std::exception &e) {
        RCLCPP_ERROR(node_->get_logger(), "Exception during robot model loading: %s", e.what());
        throw;
    } 
    catch (...) {
        RCLCPP_ERROR(node_->get_logger(), "Unknown exception during robot model loading");
        throw;
    }

    const std::string arm_group_name = "gp4_arm";
    const std::string pick_group_name = "group_1/joint_6";

    task.setProperty("group", arm_group_name);
    task.setProperty("ik_frame", pick_group_name);

    auto stage_state_current = std::make_unique<mtc::stages::CurrentState>("current");
    task.add(std::move(stage_state_current));

    addMoveStage(task, "move_to_target", target_pose);

    return task;
}

addMoveStage: This function adds a move stage to the task:

void MTCTaskNode::addMoveStage(mtc::Task& task, const std::string& name, const geometry_msgs::msg::PoseStamped& target_pose)
{
    auto sampling_planner = std::make_shared<mtc::solvers::PipelinePlanner>(node_);
    
    try {
        auto stage = std::make_unique<mtc::stages::MoveTo>(name, sampling_planner);
        stage->setGoal(target_pose);
        stage->setGroup("gp4_arm");
        stage->setIKFrame("group_1/joint_6");
        task.add(std::move(stage));
    } 
    catch (const std::exception &e) {
        RCLCPP_ERROR(node_->get_logger(), "Exception during adding move stage: %s", e.what());
    } 
    catch (...) {
        RCLCPP_ERROR(node_->get_logger(), "Unknown exception during adding move stage");
    }
}

Output

After initializing the task and sending it to be published and executed, I receive the following console output:

[task_constructor_example-6] [INFO] [1722878842.748705901] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'gp4_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[task_constructor_example-6] [ERROR] [1722878843.757147134] [ompl]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:252 - gp4_arm/gp4_arm: Unable to sample any valid states for goal tree
[task_constructor_example-6] [WARN] [1722878843.757191082] [moveit.ompl_planning.model_based_planning_context]: Timed out: 1.0s ≥ 1.0s
[task_constructor_example-6] [INFO] [1722878843.761033140] [moveit.ompl_planning.model_based_planning_context]: Unable to solve the planning problem
[task_constructor_example-6]   0  - ←   0 →   -  0 / demo task
[task_constructor_example-6]     1  - ←   1 →   -  0 / current
[task_constructor_example-6]     -  0 →   0 →   -  0 / move_to_target
[task_constructor_example-6] Failing stage(s):
[task_constructor_example-6] move_to_target (0/1): TIMED_OUT

In RViz, under the Motion Planning Tasks Task Tree, it seems that a large number of the properties for the move_to_target stage are left undefined:

Request for help: I am struggling to identify the exact cause of the planner's failure to sample valid states. I suspect it might be due to the robot configuration, particularly the added link on the last joint. Any insights or suggestions to troubleshoot and resolve this issue would be greatly appreciated.

Thank you!

Answer 1

The reason this is happening is because the robot is unable to move to the specified position. (0,0,0) is inside the robot base so it cannot find the goal tree because there is no trajectory that could allow the robot to move to that position without a collision. (0,0,0) is relative to the base_link not the end effector.