Include base placement for kinematic benchmarks

[marioprats]

Many goal poses that are not reachable from a given robot base pose, could be reachable from another base placement. Ideally we would need to know if a target is reachable independently of the base position. I see different possibilities:

1. Include virtual d.o.f on the base_link and use them as part of a group.
2. Somehow encode the base pose as part of the start state (inside the RobotState msg?), and then from rviz allow setting multiple base poses as with goals.
3. Doing it at the benchmark level, with a set of parameters that specify a sampling region. Then call the benchmark for each sample.

I like 1) because it avoids sampling, although it may not be so consistent with the existing code... I guess the fastest would be 3) if there is a way to set the root transform of a chain (kinematic_state::setRootTransform probably??). If this functionality is not going to be needed outside the kinematic analysis, this is probably the best option.

What do you think?

[sachinchitta]

Here's the way we last did it: http://kforge.ros.org/Sushi/trac/wiki/Navigation/FindBasePoses

• generate a bunch of goal poses in a set of concentric circles around an object taking into account an approximate notion of the distance the arm can reach out to. All orientations were pointing towards the object. The poses were offset based on whether you were doing the left or right arm.
• check the goal poses for collisions, eliminate the ones that are in collision
• choose the first one that is not in collision

Issues with this approach:

• the representation of reachability was not very good, we ended up with awkward poses sometimes
• you almost want a semantic notion of a good base pose (e.g. right in front of a table or shelf, facing its edge)

I would suggest using the same approach we did for Sushi but with a better representation for reachability than what we had.

[isucan]

I think we have to do variant 1) from the list that Mario suggests. It seems to me the others are a bit of a hack.
(btw, the base pose can be encoded as a multi_dof joint as part of the RobotState message)

• From the point of view of changing the benchmark or the config files, this is trivial: just update the group we are planning for (perhaps add it to the SRDF)
• As Sachin mentions, there needs to be a method for finding base poses. We should do that through the robot-specific constraint sampler plugin. There is in the moveit_pr2 repo, pr2_moveit_plugins package, pr2_moveit_constraint_sampler library.
  This is an empty implementation now (not even included in compilation).
  What it needs to become is something that allocates samplers for JointStateGroup instances based on specified constraints. In this sampler however we can make use of semantic information about the robot. In particular, if the constraints happen to be for one of the arms, and we also need to sample a base pose, we can sample base poses in a particular region for particular ranges of the yaw. I have old some code to start from if you'd like to take a crack at this. We do not need to cover all cases. We should add them on a need basis. This means that we will just have an if statement in the sampler that checks whether we are indeed in the case of finding a sample for the base + an arm, and handle that. We can make this more complex later on.

[fmessmer]

I started something to address the issue of finding "whole body"-IKs.
Find the respective topic on the moveit-users mailinglist:
https://groups.google.com/forum/#!topic/moveit-users/rN0r0dV0pxk