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.

Author: psigen

src/prpy/clone.py

@@ -92,6 +92,25 @@ def __init__(self, parent_env, clone_env=None, destroy_on_exit=None,
                 with self.clone_parent:
                     self.clone_env.Clone(self.clone_parent, self.options)
 
+                    # Due to a bug in the OpenRAVE clone API, we need to regrab
+                    # objects in cloned environments because they might have
+                    # incorrectly computed 'ignore' flags.
+                    # TODO: Remove this block if OpenRAVE cloning is fixed.
+                    for robot in self.clone_parent.GetRobots():
+                        if len(robot.GetGrabbed()):
+                            # Since ignore lists are computed from current
+                            # pose,  calling RegrabAll() from a pose that is
+                            # in self-collision may ignore collisions.
+                            if robot.CheckSelfCollision():
+                                raise CloneException(
+                                    'Unable to compute self-collisions'
+                                    ' correctly. Robot {:s} was cloned'
+                                    ' while in collision.'
+                                    .format(robot.GetName())
+                                )
+                            cloned_robot = self.clone_env.Cloned(robot)
+                            cloned_robot.RegrabAll()
+
             # Required for InstanceDeduplicator to call CloneBindings for
             # PrPy-annotated classes.
             setattr(self.clone_env, 'clone_parent', self.clone_parent)
@@ -101,23 +120,6 @@ def ClonedWrapper(*instances):
                 return Cloned(*instances, into=self.clone_env)
             setattr(self.clone_env, 'Cloned', ClonedWrapper)
 
-            # Due to a bug in the OpenRAVE clone API, we need to regrab
-            # objects in cloned environments because they might have
-            # incorrectly computed 'ignore' flags.
-            # TODO(pkv): Remove this block once OpenRAVE cloning is fixed.
-            for robot in self.clone_parent.GetRobots():
-                # Since the new ignore lists are computed from the current
-                # pose,  calling RegrabAll() from a pose that is in
-                # SelfCollision may incorrectly ignore collisions.
-                if robot.CheckSelfCollision():
-                    raise CloneException(
-                        'Unable to compute self-collisions correctly. '
-                        'Robot {:s} was cloned while in collision.'
-                        .format(robot.GetName())
-                    )
-                cloned_robot = self.clone_env.Cloned(robot)
-                cloned_robot.RegrabAll()
-
     def __enter__(self):
         if self.lock:
             self.clone_env.Lock()