disable check that herb is extremely close to traj start position

[ClintLiddick]

Because OWD published last commanded position, not actual position,
but ros_control and rewd_controllers publish actual position,
the check in herbpy that current position is extremely close to the
planning start position almost always fails.

However, this has not been a problem in practice, and we have been
running with this code commented out for over a month.

Do we want to change the tolerance, or delete this check?

[mkoval]

We should definitely not disable this check. The trajectory-following controller does not currently reject trajectories that start far from the robot's current configuration. Such a trajectory could cause the robot to linearly interpolate in configuration space without any collision checking.

I do think we should add this check to ReWD - this check should be enforced even if bypass HerbPy while executing trajectories. There are already tolerance parameters in there - we just have to check if the tolerance is violated and abort the trajectory.

We should still keep this check in simulation, since there is no controller to do our dirty work for us.

[ClintLiddick]

We have personalrobotics/rewd_controllers#4 open for this in rewd_controllers, but I guess for that change and this one we should empirically determine what the tolerances should be. I'll setup something to get that data next week. I'm closing this in the meantime then, and will open one with updated tolerances.

As a note, @vinitha910 and I discoved a similar issue in prpy at https://github.com/personalrobotics/prpy/blob/5b7a2dd75944c38df0cc91ec2fbdbd180e89b534/src/prpy/tsr/tsr.py#L36. That epsilon is used (in block_sorting) via the contains method to verify our current position is within 1 mm of our planned TSR. I'll have to look at how and where that method is used, and figure out the right place to make a change.

[mkoval]

A reasonable upper bound may be DOF resolution. There isn't much point to collision check at a higher resolution than the controller can achieve resolution.

I believe the tolerance on contains is just to detail with numerical imprecision introduced by the distance calculation and (possibly) inverse kinematics. I don't think we should increase this just because the check is (rightfully) failing somewhere. Instead, we should adjust that specific TSR to be more lenient: we're clearly okay with larger Bws since the demo currently works. 😉

On a second thought: Why do we even perform a contains check in the block_sorting demo?

[ClintLiddick]

Yeah, that's the problem. We shouldn't be figuring out which TSR we ended up planning to by calling contains with the end-effector position.

[jeking04]

We use the contains method to determine which block to Grab. We are sending the planners TSRs for every block and letting the planner find a solution to any block. This is nice in that it allows the planners to be as fast as possible, and removes the need for a higher level planner that is selecting the particular block to pick. However, we need a way to know which block is being picked so we can have the robot Grab the block kinbody and move it to the bin. Using the contains method allows us to make this determination. I think this is the most correct thing to do other than modify all planners to return the id of the TSR that the goal was sampled from.

[jeking04]

@mkoval points out that one option is to change the block sorting demo to call contains against the last configuration in the trajectory instead of against the configuration achieved/reported by the robot. This will basically match what was happening when we used owd - i.e. the robot used to report its configuration as the configuration in the trajectory. This might be the simplest solution for the particular problem in the block sorting demo.