Sampling planner with MoveTo stage/MoveRelative provides trajectory with all zero timestamps, and no velocities/acc, but same task works with cartesian planner

[robotics-qc]

Hello! I have just recently picked up using moveit task constructor and had the pick&place example working. I decided to make a simpler demo that builds many simple demo tasks that demonstrate the usage of different stages. Using the same environment where the pick and place demo runs and works - i created another node which executes my freshly defined task.

I created a simple task : Task flow :

• Current stage
• MoveTo 'ready' position using cartesian planner
• MoveRelative with Cartesian planner : end eff moves back on it's x axis by 2 units
• MoveTo 'ready' position with sampling planner
• MoveRelative with Sampling planner : end eff moves back on it's x axis by 2 units
• MoveTo 'ready' position with sampling planner

The point of this is to show the syntax of MoveRelative and MoveTo stages, as well as demonstrate the conceptual difference between a cartesian planner and sampling planner.

Planning is successful and the visualization shows that it behaves as expected. However, whilst executing, whenever I use a sampling planner - in either of the stages at any point in the program - the execution is aborted. But the odd thing is it works in the pick and place example based code I have running in the same package if I run that node instead of this one. The settings for the planners/stages/everything is the same except for the task and it's substages.

My node's terminal output :

Output in the RViz terminal:

If I use the cartesian planner for everything, then there is no error and planning + execution is completed If I use sampling planner then the planning is successful but whichever stage I use the sampling planner, the execution fails for the above reason.

Finally I also logged the solutions being sent out on /solutions for both the scenarios and the working pick and place example which also uses the sampling planner.

I see that in the trajectory all the timestamps in my error case are at zero - not incrementing, but in the other two scenarios it does increase. Moreover, the velocity and acceleration fields in my error case are also empty.

error case (sampling planner) :

Cartesian only:

My Code for the task generation is simple: This works in planning - and fails execution at the first sampling planner based stage. Am I doing something wrong or is this some sort of a bug? And in the pick and place example, sampling planners are used only in connect stages, never in the moveTo or moveRelative stages so is it an inherent limitation? I tried changing one of the MoveRelatives in the pick and place example to sampling and that seemed to work - and that is just another node in a file in the same package and environment i'm running. i.e : The controllers, settings all are the same.

moveit::task_constructor::Task moveRelativeStageTask(){

        //Relevant groups and frames
        const auto& arm_group_name = "panda_arm";
        const auto& hand_frame = "panda_hand";
        const auto& hand_group_name = "hand";

        // Initialize the task
        moveit::task_constructor::Task task;
        task.setName("Sample MoveTo task");

        //add propoerties : These are like variables we can add to the task, can be anything. Some stages expect some properties to be given, such as 'group' for moveRelative
        task.setProperty("group", arm_group_name);
        task.setProperty("eef", hand_group_name);
        task.setProperty("ik_frame", hand_frame);

        // Expects a shared pointer to the node class - 'this' is not a shared pointer, so within a Node subclass we can get the shared reference using this function 'shared_from_this()'
        // Needs to be loaded before task.init() is called to initialize the task later
        task.loadRobotModel(shared_from_this());

        // Sampling planner and a cartesian planner example - can try out either
        auto cartesian_planner = std::make_shared<moveit::task_constructor::solvers::CartesianPath>();
        cartesian_planner->setMaxVelocityScalingFactor(1.0);
        cartesian_planner->setMaxAccelerationScalingFactor(1.0);
        cartesian_planner->setStepSize(.005);

        auto sampling_planner = std::make_shared<moveit::task_constructor::solvers::PipelinePlanner>(shared_from_this());
        sampling_planner->setProperty("goal_joint_tolerance", 1e-5); 

        // Current stage : This is the current state of the robot and scene - this is a generator stage
        auto current_stage = std::make_unique<moveit::task_constructor::stages::CurrentState>("current");
            task.add(std::move(current_stage));
            // Pointer to the currrent state
            moveit::task_constructor::Stage* current_state_ptr = current_stage.get(); 

        // MoveTo Stage allows us to move a planning group to a pre-defined or specified RobotState : pre-defined states can be found in the SRDF
        auto move_to_ready_stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("MoveTo predefined position (ready pose)", cartesian_planner);
            move_to_ready_stage->setGroup(arm_group_name);
            move_to_ready_stage->setGoal("ready"); // SRDF has a 'ready' state defined
            move_to_ready_stage->setTimeout(10);
            // move_to_ready_stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT,{ "eef", "group", "ik_frame" });
            task.add(std::move(move_to_ready_stage));

        // MoveRelative stage : is a propagator stage that passes on the information to adjacent stages
        // Also moves the robot frame sepcified relative to it's current position/frame by a given value and orientation
        auto move_relative_with_cartesian_planner_stage =  std::make_unique<moveit::task_constructor::stages::MoveRelative>("MoveRelative with cartesian planner", cartesian_planner );
            //set properties
            move_relative_with_cartesian_planner_stage->properties().set("marker_ns", "approach_object");
            move_relative_with_cartesian_planner_stage->properties().set("link", hand_frame);
            move_relative_with_cartesian_planner_stage->setMinMaxDistance(0.1, 0.15);

            // Inherent properties of chioce from parent stage/task
            // move_relative_with_cartesian_planner_stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT,{ "eef", "group", "ik_frame" });
            move_relative_with_cartesian_planner_stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
            // move_relative_with_cartesian_planner_stage->setGroup(arm_group_name); - this also works if we do not want to inherit the 'group' property from parent stage

            // move in a specified transformation wrt to a given frame
            geometry_msgs::msg::Vector3Stamped vec_cartesian;
            vec_cartesian.header.frame_id = hand_frame;
            vec_cartesian.vector.x = -2.0;
            move_relative_with_cartesian_planner_stage->setDirection(vec_cartesian);
            task.add(std::move(move_relative_with_cartesian_planner_stage));

        // Move To allows us to move a planning group to a pre-defined or specified RobotState : pre-defined states can be found in the SRDF
        move_to_ready_stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("MoveTo predefined position (ready pose)", sampling_planner);
            move_to_ready_stage->setGroup(arm_group_name);
            move_to_ready_stage->setGoal("ready");
            move_to_ready_stage->setIKFrame(hand_frame);
            // move_to_ready_stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT,{ "eef", "group", "ik_frame" });

            task.add(std::move(move_to_ready_stage));

        // Move Relative stage but with a different planner
        auto move_relative_with_sampling_planner_stage =  std::make_unique<moveit::task_constructor::stages::MoveRelative>("MoveRelative with sampling planner", cartesian_planner );
            //set properties
            move_relative_with_sampling_planner_stage->properties().set("marker_ns", "approach_object");
            move_relative_with_sampling_planner_stage->properties().set("link", hand_frame);
            move_relative_with_sampling_planner_stage->setMinMaxDistance(0.1, 0.15);

            // Inherent properties of chioce from parent stage/task
            move_relative_with_sampling_planner_stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });

            // move in a specified transformation wrt to a given frame
            geometry_msgs::msg::Vector3Stamped vec_sampling;
            vec_sampling.header.frame_id = hand_frame;
            vec_sampling.vector.x = -2.0;
            move_relative_with_sampling_planner_stage->setDirection(vec_sampling);
            task.add(std::move(move_relative_with_sampling_planner_stage));

        // Move To allows us to move a planning group to a pre-defined or specified RobotState : pre-defined states can be found in the SRDF
        move_to_ready_stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("MoveTo predefined position (ready pose)", cartesian_planner);
            move_to_ready_stage->setGroup(arm_group_name);
            move_to_ready_stage->setGoal("ready");
            task.add(std::move(move_to_ready_stage));

    return task;
    }

Any other information needed can be provided! I'd appreciate any help in this.

Aside from this, as I learn I am creating sample tasks using merger/Alternatives/Fallbacks(which I cant find info or get to work) and other stages so people can understand MTC more easily!

[robotics-qc]

Nevermind - I solved it. If anyone else is facing this, the issue seems to be that we need to create the node with the override options otherwise sampling planner does not work.

    rclcpp::NodeOptions options;
    options.automatically_declare_parameters_from_overrides(true); // Turns out this is essnential and I somehow missed it while writing my node

    std::shared_ptr<CustomMtcPipeline> mtc_node = std::make_shared<CustomMtcPipeline>(options);

[rhaschke]

The reason for this behavior is that TimeParameterization is configured via the ROS parameter planning_plugin, which is not declared and thus not available without that flag.