Merge pull request #120 from personalrobotics/bugfix/tsr_planner_gil

IK GIL release.

Author: mkoval

src/prpy/planning/ik.py

@@ -63,7 +63,9 @@ def PlanToIK(self, robot, goal_pose, ranker=ik_ranking.JointLimitAvoidance,
             ik_param = IkParameterization(
                 goal_pose, IkParameterizationType.Transform6D)
             ik_solutions = manipulator.FindIKSolutions(
-                ik_param, IkFilterOptions.CheckEnvCollisions)
+                ik_param, IkFilterOptions.CheckEnvCollisions,
+                ikreturn=False, releasegil=True
+            )
 
         if ik_solutions.shape[0] == 0:
             raise PlanningError('There is no IK solution at the goal pose.')

src/prpy/planning/snap.py

@@ -88,7 +88,10 @@ def PlanToEndEffectorPose(self, robot, goal_pose, **kw_args):
         manipulator = robot.GetActiveManipulator()
         current_config = robot.GetDOFValues(manipulator.GetArmIndices())
         ik_param = openravepy.IkParameterization(goal_pose, ikp.Transform6D)
-        ik_solution = manipulator.FindIKSolution(ik_param, ikfo.CheckEnvCollisions)
+        ik_solution = manipulator.FindIKSolution(
+            ik_param, ikfo.CheckEnvCollisions,
+            ikreturn=False, releasegil=True
+        )
 
         if ik_solution is None:
             raise PlanningError('There is no IK solution at the goal pose.')

src/prpy/planning/tsr.py

@@ -119,7 +119,9 @@ def PlanToTSR(self, robot, tsrchains, tsr_timeout=2.0,
             ik_param = IkParameterization(
                 tsrchain.sample(), IkParameterizationType.Transform6D)
             ik_solution = manipulator.FindIKSolutions(
-                ik_param, IkFilterOptions.CheckEnvCollisions)
+                ik_param, IkFilterOptions.CheckEnvCollisions,
+                ikreturn=False, releasegil=True
+            )
             if ik_solution.shape[0] > 0:
                 ik_solutions.append(ik_solution)
 

src/prpy/planning/workspace.py

@@ -185,7 +185,9 @@ def PlanWorkspacePath(self, robot, traj, timelimit=5.0,
                     qcurr = robot.GetActiveDOFValues()  # Configuration at t.
                     qnew = manip.FindIKSolution(
                         openravepy.matrixFromPose(traj.Sample(t+dt)[0:7]),
-                        ik_options
+                        ik_options,
+                        ikreturn=False,
+                        releasegil=True
                     )
 
                     # Check if the step was within joint DOF resolution.