Skip to content

Commit 798fdc9

Browse files
committed
Merge branch 'master' into feature/more_planningmethods
2 parents 74c07e5 + f27f454 commit 798fdc9

File tree

1 file changed

+19
-17
lines changed

1 file changed

+19
-17
lines changed

src/prpy/clone.py

Lines changed: 19 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -92,6 +92,25 @@ def __init__(self, parent_env, clone_env=None, destroy_on_exit=None,
9292
with self.clone_parent:
9393
self.clone_env.Clone(self.clone_parent, self.options)
9494

95+
# Due to a bug in the OpenRAVE clone API, we need to regrab
96+
# objects in cloned environments because they might have
97+
# incorrectly computed 'ignore' flags.
98+
# TODO: Remove this block if OpenRAVE cloning is fixed.
99+
for robot in self.clone_parent.GetRobots():
100+
if len(robot.GetGrabbed()):
101+
# Since ignore lists are computed from current
102+
# pose, calling RegrabAll() from a pose that is
103+
# in self-collision may ignore collisions.
104+
if robot.CheckSelfCollision():
105+
raise CloneException(
106+
'Unable to compute self-collisions'
107+
' correctly. Robot {:s} was cloned'
108+
' while in collision.'
109+
.format(robot.GetName())
110+
)
111+
cloned_robot = self.clone_env.Cloned(robot)
112+
cloned_robot.RegrabAll()
113+
95114
# Required for InstanceDeduplicator to call CloneBindings for
96115
# PrPy-annotated classes.
97116
setattr(self.clone_env, 'clone_parent', self.clone_parent)
@@ -101,23 +120,6 @@ def ClonedWrapper(*instances):
101120
return Cloned(*instances, into=self.clone_env)
102121
setattr(self.clone_env, 'Cloned', ClonedWrapper)
103122

104-
# Due to a bug in the OpenRAVE clone API, we need to regrab
105-
# objects in cloned environments because they might have
106-
# incorrectly computed 'ignore' flags.
107-
# TODO(pkv): Remove this block once OpenRAVE cloning is fixed.
108-
for robot in self.clone_parent.GetRobots():
109-
# Since the new ignore lists are computed from the current
110-
# pose, calling RegrabAll() from a pose that is in
111-
# SelfCollision may incorrectly ignore collisions.
112-
if robot.CheckSelfCollision():
113-
raise CloneException(
114-
'Unable to compute self-collisions correctly. '
115-
'Robot {:s} was cloned while in collision.'
116-
.format(robot.GetName())
117-
)
118-
cloned_robot = self.clone_env.Cloned(robot)
119-
cloned_robot.RegrabAll()
120-
121123
def __enter__(self):
122124
if self.lock:
123125
self.clone_env.Lock()

0 commit comments

Comments
 (0)