Define benchmark as robot-specific start state and set of target gripper poses

[mateiciocarlie]

We want to define a benchmark using:

• a start state which is robot specific, and specified in joint space
• a set of target poses, specified in Cartesian space, over a set of planning scenes. This set must be usable by all robots that we want to run the benchmark against.
• the name of a link used to achieve the target poses

The benchmark tool should allow us to define and run this benchmark.

Note that this is different from the current notion of saved "query" as:

• only the start state is in joint space and robot specific
• the goal states are in Cartesian space (IK is performed as part of the benchmark) and they are shared by all robots we run the benchmark against

In parallel, in a different ticket, we are adding the ability to save the target poses using the rviz plugin.

helping: @isucan

[isucan]

Just as a note, the moveit_ros_warehouse package allows storage of planning scenes, goal poses (or arrays of goal poses) and individual robot states (used for definition of start states).

I would like to split this ticket in two, as I believe we need to do two things:

• benchmark planning plugins (the functionality described in this ticket is is there, but not yet tested)
• benchmark planning pipelines

For the first part, we just load a planning plugin and test it as is. That means that if the planner can only operate in Joint space, then we only test joint space; no IK is done. I'll set up some instructions on how to run things.

For the second part we test a planning pipeline, which allows running IK, fixing input states, parameterize the solution with timestamps (each of these additional steps is optional). This code does not yet exist.

[isucan]

This is now implemented as part of the Groovy distribution. I am starting a different ticket for benchmarking pipelines

[mateiciocarlie]

OK, got some more clarifications. Thanks @isucan :)

The benchmark as defined in the ticket can be done now, with the condition that the planning plugin itself can handle goals which specify just end-effector poses (as constraints). Our current plugin can, so we're good.

The long term solution is to allow plugins + adapters, so the adapter will do the IK if the plugin can not handle it. This is not on our critical path now.

[mateiciocarlie]

Re-opening this, as the following part is not yet addressed: "a set of target poses, specified in Cartesian space, over a set of planning scenes. This set must be usable by all robots that we want to run the benchmark against".

Right now, planning scenes are still robot-specific. This issue might go in another ticket though, that this one will then depend on.

[isucan]

Actually, that is incorrect. The set or target poses have nothing to do with the planning scenes or with the robot itself. The described functionality is already available.

[mateiciocarlie]

Well, sure, but it is impossible (or at least not as useful) to run the becnhmark without a planning scene, so even if goal poses are robot independent, we still need the planning scenes to be independent as well.

We should probably close this ticket though and open another one on making planning scenes robot-independent?

[isucan]

The planning scene is robot specific in the sense that it includes the collision matrix, the robot model and its configuration. This information is needed for planning, so I do not see how we can have planning scenes that are robot independent. Do you mean the PlanningSceneWorld? That is a separate message, and it is robot independent. That information can be read on its own from the database and can be copied to other planning scenes, thus making the use of different environments possible for different robots. The only issues that arise are if you have different robots named the same way, but that I believe is user error.

[mateiciocarlie]

Got it.

The question, then: how do we run a benchmark across multiple robots using the exact same PlanningSceneWorlds for all of them?

[isucan]

The following should work: use the same names for the planning scenes to load (in the .cfg file) but have different URDF loaded on the param server.
However you cannot include multiple robots in the same benchmark -- it would have to be multiple benchmarks.

[isucan]

Perhaps the confusing thing here is that the PlanningScene is always saved and when the robot is different only the PlanningSceneWorld is read. We can make it so that we can explicitly store just PlanningSceneWorld. That would perhaps be more elegant as well. What do you think?