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]

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

[rhaschke]

Sorry, I didn't yet manage to look into this. As your main issue, executing subtrajectories is solved (isn't it?), I pushed the remaining issue (id=0) to the mid-term stack only. Is there any stronger urgency to that?

[matthias88]

Yes, you are right - the main issue is solved, but I need the stage id for setting IOs in between. Pull Request #194 would be an alternative solution, but unfortunately it has not been merged yet.

[rhaschke]

I figured out the problem: toMsg() requires the task's introspection object passed as an extra argument.
I pushed a commit allowing to do so: task.solutions[0].toMsg(task.introspection()).

[matthias88]

Thanks a lot. I manually merged master in ros2 branch with your latest commit and it works.