Execute subtrajectories with MTC Python API

[matthias88]

Hi, is there a possibility to execute subtrajectories directly over MTC task.execute method? My current workaround is to publish the solution to an moveit_py node (with Introspection enabled, for accessing the solution infos), access the subtrajectories and execute it. Isn't there a more elegant solution without this "overhead" of starting another node, which sends the (sub)solutions to the move_group node? Why do I need this: I want to realize automatic and step mode and I have some external IOs which have to be set in between the subtrajectories, related to #194 and #626. Thanks in advance

[rhaschke]

Of course, you can send a subsolution for execution as well. The Task::execute() method takes any solution as argument:

moveit_task_constructor/core/src/task.cpp

Line 285 in dc8399f

moveit::core::MoveItErrorCode Task::execute(const SolutionBase& s) {

I guess, you already tried that. What issues did you run into? Don't know how to access subtrajectories?
Use dynamic_cast to switch based on the concrete type and then use

moveit_task_constructor/core/include/moveit/task_constructor/storage.h

Line 402 in dc8399f

const container_type& solutions() const { return subsolutions_; }

or

moveit_task_constructor/core/include/moveit/task_constructor/storage.h

Line 434 in dc8399f

const SolutionBase* wrapped() const { return wrapped_; }

Maybe your problem is that you are running python code and those classes are not yet wrapped?
Then, you could use solution.toMsg, access the subsolution(s) in the message and send them to the ExecuteTaskSolution action, like the C++ code does.
Obviously, it would make sense to provide this missing functionality. However, I won't have time to implement this before the weekend. So, if you find a solution along the sketched roadmap, please file a PR.

[matthias88]

Thanks a lot for your fast reply. Yes, my problem is, that I am using python. But thanks to your hint I should be able to implement a modified action client for sending subtrajectories directly to the ExecuteTaskSolution action. I am going to file a PR, as soon I have a clean and working solution.

[matthias88]

Just a short update: principally it works, but I have no idea why I always get id=0 and stage_id = 0 within the subtrajectory.info, the comments and costs coincides with the originally published solution per each subtrajectory. If I subscribe the topic /solution directly with the RVIZ2 plugin, then valid id and stage_id are given.

class TaskActionServer(Node):
    def __init__(self):
        super().__init__('task_action_server')
       
        self.mtc_node = rclcpp.Node("task_constructor")

    def test_task(self):
        # Define task
        task = core.Task()   
        ...
        ...     
        if task.plan():
            task.enableIntrospection()
            task.publish(task.solutions[0])
            solutionmsg = task.solutions[0].toMsg()
            
            if self.b_automatic:
                task.execute(task.solutions[0])
            else:
                # step mode
                step_solution = copy.deepcopy(solutionmsg)
                for i in range(len(solutionmsg.sub_trajectory)):
                    step_solution.sub_trajectory.clear()
                    step_solution.sub_trajectory.append(solutionmsg.sub_trajectory[i])
                    # execute step
                    self.get_logger().info(f"ID: {solutionmsg.sub_trajectory[i].info.id}")    
                    self.get_logger().info(f"Stage_ID: {solutionmsg.sub_trajectory[i].info.stage_id}")
                    self.get_logger().info(f"Cost: {solutionmsg.sub_trajectory[i].info.cost}")
                    self.get_logger().info(f"Comment: {solutionmsg.sub_trajectory[i].info.comment}")
                    self.error_code = self.mtc_task_executor.send_goal_mtc(step_solution)

Implementation action client task executor:

class MTCExecutorClient(Node):
    def __init__(self):
        super().__init__('moveit_task_constructor_executor_')

        self.action_client_mtc_task_execute = ActionClient(self, ExecuteTaskSolution, 'execute_task_solution', callback_group=MutuallyExclusiveCallbackGroup())

    def send_goal_mtc(self, solution_msg):
        goal_msg = ExecuteTaskSolution.Goal()
        goal_msg.solution = solution_msg

        if not self.action_client_mtc_task_execute.wait_for_server(timeout_sec=0.5):
            self.get_logger().error("Failed to connect to the 'execute_task_solution' action server")
            return MoveItErrorCodes.FAILURE
            
        self._send_goal_future = self.action_client_mtc_task_execute.send_goal_async(goal_msg)
        rclpy.spin_until_future_complete(self, self._send_goal_future)

        goal_handle = self._send_goal_future.result()
        if not goal_handle.accepted:
            self.get_logger().info("Send goal call failed")
            return MoveItErrorCodes.FAILURE

        self.get_logger().info("Goal accepted")

        self._get_result_future = goal_handle.get_result_async()
        rclpy.spin_until_future_complete(self, self._get_result_future)

        try:
            result = self._get_result_future.result()
            self.get_logger().info(f"Result: {result.result.error_code}")
        except Exception as e:
            self.get_logger().error(f"Get result call failed: {e}")
            return MoveItErrorCodes.FAILURE
                        
        return result.result.error_code

[rhaschke]

I don't see a reason, why IDs should be zero. Both lines

            task.publish(task.solutions[0])
            solutionmsg = task.solutions[0].toMsg()

should generate exactly the same message (except for task_id) as they both use toMsg():

moveit_task_constructor/core/src/introspection.cpp

Lines 193 to 196 in dc8399f

	void Introspection::fillSolution(moveit_task_constructor_msgs::msg::Solution& msg, const SolutionBase& s) {
	s.toMsg(msg, this);
	msg.task_id = impl->task_id_;
	}

However, I didn't have time to reproduce the code and look deeper.

[matthias88]

Thanks for your answer. Here you can see my output if I compare /solution topic with solutionmsg:

[matthias88]

@rhaschke I would like to ask, if you were already able to reproduce the code and examine the cause for this issue?