Move To Stored States? / Support Warehouse Targets

[gautz]

I want to use the MoveTo stage to be able to plan to a (teached-in) goals saved in the warehouse ros mongodb. Is there an already implemented way to access those stored states from moveit_task_constructor?

If not I plan to use MotionPlanningFrame::loadStoredStates() to load the states locally. Do you see a better way?

[rhaschke]

No, we don't yet have a stage to load states from the database. You cannot access the MotionPlanningFrame from MTC easily: the former is instantiated in rviz, while the latter in your planning node. However, you can use similar code as in the function loadStoredStates(), namely RobotStateStorage::getKnownRobotStates().

[v4hn]

adding support to the warehouse to MoveTo (or a derived stage) would be feasible (around here), but to keep things simpler I would propose to have a new MonitoringGenerator, e.g. WarehouseState and monitor&Connect it to the previous state.

Such a stage sounds like a clean addition to this repository :+1:

[rhaschke]

I agree with @v4hn that a new stage is the way to go. I would derive WarehouseState from FixedState, however. But I agree that we should adapt FixedState to become a MonitoringGenerator to allow adapting only a (sub) group of the robot. @gautz: Could you provide a PR for a WarehouseState?

[v4hn]

But I agree that we should adapt FixedState to become a MonitoringGenerator to allow adapting only a (sub) group of the robot.

That should still be a new State MonitoringFixedState, otherwise we would loose the option to use FixedState to seed a Task (especially relevant for testing).

@gautz: Could you provide a PR for a WarehouseState?

If you don't have time/interest to work on this for upstream, please say so too. Someone else might find the time to work on that at some point.

[gautz]

For the purpose of my welding application, I had some troubles with the FixedState stage, we currently only use MoveTo stages. Thus my current code extends the MoveTo stage with a connectDB() function, a loadStoredStates(const std::string &pattern) and a setGoal(const std::string &pattern, const std::string &named_stored_state) which takes the .* (name of the table in the db?) and the name of the stored state as input. I guess this is not the cleanest way but it works.

Could you also explain what is the advantage/purpose of using fixed_states in combination with connect stages? Using only MoveTo stages also works closer to writing a "conventional robot program", e.g. moveLinear, moveCircular, etc. If I manage to switch our application from moveTo to {FixedState + Move relative + Connect}, then I could probably work on a PR. Otherwise I guess I would not find time for it.

[v4hn]

For the purpose of my welding application, I had some troubles with the FixedState stage, we currently only use MoveTo stages.

Feel free to create issues, but it's probably misuse one your end in this case. FixedState is supposed to replace CurrentState when you already know the full planning scene you want to work with. That can lead to problems, e.g., if you use it in multiple places in one Task and parts of the scene change in between them.

Thus my current code extends the MoveTo stage with a connectDB() function, a loadStoredStates(const std::string &pattern) and a setGoal(const std::string &pattern, const std::string &named_stored_state) which takes the .* (name of the table in the db?) and the name of the stored state as input. I guess this is not the cleanest way but it works.

Yeah, one advantage of having a separate Stage for it is that you can connectDB() in Stage::init and have a clear setGoal with only one meaning. You basically cheated because setGoal(const std::string&) already exists.

Could you also explain what is the advantage/purpose of using fixed_states in combination with connect stages? Using only MoveTo stages also works closer to writing a "conventional robot program", e.g. moveLinear, moveCircular, etc.

The main advantage of MTC is that you are not restricted to "conventional" programs, but can handle inverse inference without much more complex code. CurrentState-MoveTo can handle more general goals (e.g. Cartesian target volumes), but if you only use explicit joint space targets, it is basically equivalent to CurrentState-Connect-<SceneOfCurrentStateModifiedToNewTargetRobotState>.

If I manage to switch our application from moveTo to {FixedState + Move relative + Connect}, then I could probably work on a PR. Otherwise I guess I would not find time for it.

Ok.