Moved regrab logic into env equality check.
Moved regrab logic into env equality check.

Previously, we did a relatively expensive self-collision and
regrab check on the robot in *any* clone operation, on the off
chance that it was grabbing something that could get messed up.

This moves the `CheckSelfCollision()` and `RegrabAll()` operations
inside the following checks:

1. We are not cloning back into the same environment.
2. The robot is actually grabbing something.

This shaves time off nested cloning calls, as found everywhere
in our planning stack.

Stefanos19/prpy,personalrobotics/prpy