Modified SnapPlanner and GreedyIKPlanner to raise collision-specific errors. Added tests for collision-specific errors in PlantoConfiguration and PlanToEndEffectorOffset

Author: Gilwoo Lee

src/prpy/planning/snap.py

@@ -80,6 +80,10 @@ def PlanToEndEffectorPose(self, robot, goal_pose, **kw_args):
         @param goal_pose desired end-effector pose
         @return traj
         """
+        
+        from .exceptions import CollisionPlanningError,SelfCollisionPlanningError
+        from openravepy import CollisionReport
+        
         ikp = openravepy.IkParameterizationType
         ikfo = openravepy.IkFilterOptions
 
@@ -92,13 +96,33 @@ def PlanToEndEffectorPose(self, robot, goal_pose, **kw_args):
             ik_param, ikfo.CheckEnvCollisions,
             ikreturn=False, releasegil=True
         )
-
-        if ik_solution is None:
+        
+        if ik_solution is None: 
+            # FindIKSolutions is slower than FindIKSolution, 
+            # so call this only to identify and raise error when there is no solution
+            ik_solutions = manipulator.FindIKSolutions(
+                    ik_param,
+                    ikfo.IgnoreSelfCollisions,
+                    ikreturn=False,
+                    releasegil=True
+                )
+
+            for q in ik_solutions:
+                robot.SetActiveDOFValues(q)
+                report = CollisionReport()
+                if self.env.CheckCollision(robot, report=report):
+                    raise CollisionPlanningError.FromReport(report) 
+                elif robot.CheckSelfCollision(report=report):
+                    raise SelfCollisionPlanningError.FromReport(report)
+            
             raise PlanningError('There is no IK solution at the goal pose.')
 
         return self._Snap(robot, ik_solution, **kw_args)
 
     def _Snap(self, robot, goal, **kw_args):
+        from .exceptions import CollisionPlanningError,SelfCollisionPlanningError,JointLimitError
+        from openravepy import CollisionReport
+
         Closed = openravepy.Interval.Closed
 
         start = robot.GetActiveDOFValues()
@@ -110,13 +134,45 @@ def _Snap(self, robot, goal, **kw_args):
         params = openravepy.Planner.PlannerParameters()
         params.SetRobotActiveJoints(robot)
         params.SetGoalConfig(goal)
-        check = params.CheckPathAllConstraints(start, goal, [], [], 0., Closed)
-
-        # The function returns a bitmask of ConstraintFilterOptions flags,
-        # indicating which constraints are violated. We'll abort if any
-        # constraints are violated.
-        if check != 0:
-            raise PlanningError('Straight line trajectory is not valid.')
+        check = params.CheckPathAllConstraints(start, goal, [], [], 0., Closed,
+            options = 15, returnconfigurations = True)
+
+        # If valid, above function returns (0,None, None, 0) 
+        # if not, it returns (1, invalid_config ,[], time_when_invalid),
+        # in which case we identify and raise appropriate error. 
+        if check[0] != 0:
+            q = check[1]
+
+            # Check for joint limit violation
+            q_limit_min, q_limit_max = robot.GetActiveDOFLimits()
+
+            lower_position_violations = (q < q_limit_min)
+            if lower_position_violations.any():
+                index = lower_position_violations.nonzero()[0][0]
+                raise JointLimitError(robot,
+                    dof_index=active_indices[index],
+                    dof_value=q[index],
+                    dof_limit=q_limit_min[index],
+                    description='position')
+
+            upper_position_violations = (q> q_limit_max)
+            if upper_position_violations.any():
+                index = upper_position_violations.nonzero()[0][0]
+                raise JointLimitError(robot,
+                    dof_index=active_indices[index],
+                    dof_value=q[index],
+                    dof_limit=q_limit_max[index],
+                    description='position')
+
+            # Check for collision
+            robot.SetActiveDOFValues(q)
+            report = CollisionReport()
+            if self.env.CheckCollision(robot, report=report):
+                raise CollisionPlanningError.FromReport(report) 
+            elif robot.CheckSelfCollision(report=report):
+                raise SelfCollisionPlanningError.FromReport(report)
+
+            raise PlanningError('Straight line trajectory is not valid.' )
 
         # Create a two-point trajectory that starts at our current
         # configuration and takes us to the goal.

src/prpy/planning/workspace.py

@@ -154,7 +154,14 @@ def PlanWorkspacePath(self, robot, traj, timelimit=5.0,
         @param timelimit timeout in seconds
         @return qtraj configuration space path
         """
-        from .exceptions import TimeoutPlanningError
+        from .exceptions import (
+            TimeoutPlanningError, 
+            CollisionPlanningError, 
+            SelfCollisionPlanningError,
+            JointLimitError)
+        from openravepy import CollisionReport
+        from numpy import linalg as LA
+
 
         with robot:
             manip = robot.GetActiveManipulator()
@@ -171,17 +178,20 @@ def PlanWorkspacePath(self, robot, traj, timelimit=5.0,
 
             # Smallest CSpace step at which to give up
             min_step = min(robot.GetActiveDOFResolutions())/100.
-            ik_options = openravepy.IkFilterOptions.CheckEnvCollisions
-
+            ik_options = openravepy.IkFilterOptions.CheckEnvCollisions 
             start_time = time.time()
             epsilon = 1e-6
 
+
             try:
+                collision_error = None 
                 while t < traj.GetDuration() + epsilon:
                     # Check for a timeout.
                     current_time = time.time()
                     if (timelimit is not None and
                             current_time - start_time > timelimit):
+                        if collision_error is not None: 
+                            raise collision_error 
                         raise TimeoutPlanningError(timelimit)
 
                     # Hypothesize new configuration as closest IK to current
@@ -193,12 +203,33 @@ def PlanWorkspacePath(self, robot, traj, timelimit=5.0,
                         releasegil=True
                     )
 
+                    # FindIKSolutions is slower than FindIKSolution, 
+                    # so call this only to identify error when there is no solution
+                    if qnew is None: 
+                        ik_solutions = manip.FindIKSolutions(
+                            openravepy.matrixFromPose(traj.Sample(t+dt)[0:7]),
+                            openravepy.IkFilterOptions.IgnoreSelfCollisions,
+                            ikreturn=False,
+                            releasegil=True
+                        )
+
+                        # update collision_error to contain collision info.
+                        for q in ik_solutions:
+                            robot.SetActiveDOFValues(q)
+                            report = CollisionReport()
+                            if self.env.CheckCollision(robot, report=report):
+                                collision_error = CollisionPlanningError.FromReport(report) 
+                            elif robot.CheckSelfCollision(report=report):
+                                collision_error = SelfCollisionPlanningError.FromReport(report)
+                            
                     # Check if the step was within joint DOF resolution.
                     infeasible_step = True
                     if qnew is not None:
                         # Found an IK
                         step = abs(qnew - qcurr)
                         if (max(step) < min_step) and qtraj:
+                            if collision_error is not None: 
+                                raise collision_error
                             raise PlanningError('Not making progress.')
                         infeasible_step = any(step > robot.GetActiveDOFResolutions())
                     if infeasible_step:

tests/planning/methods/PlanToConfiguration.py

@@ -1,5 +1,9 @@
 from prpy.clone import CloneException
 from prpy.planning.base import PlanningError
+from prpy.planning.exceptions import (
+    CollisionPlanningError,
+    SelfCollisionPlanningError,
+    JointLimitError)
 
 class PlanToConfigurationTest(object):
     def test_PlanToConfiguration_StartInCollision_Throws(self):
@@ -80,3 +84,17 @@ def test_PlanToConfiguration_GoalIsFeasible_FindsSolution(self):
             self.config_feasible_start, self.config_feasible_goal)
 
 
+class PlanToConfigurationTestCollisionTest(object): 
+    """ Expects collision-specific error"""
+
+    def test_PlanToConfiguration_GoalInCollision_Throws_Collision(self):
+        # Setup
+        with self.env:
+            self.robot.SetActiveDOFValues(self.config_feasible_start)
+
+        # Test/Assert
+        with self.assertRaises((CollisionPlanningError,
+            SelfCollisionPlanningError,
+            JointLimitError)):
+                self.planner.PlanToConfiguration(
+                self.robot, self.config_env_collision)

tests/planning/methods/PlanToEndEffectorOffset.py

@@ -1,6 +1,10 @@
 import numpy
 from prpy.clone import CloneException
 from prpy.planning.base import PlanningError
+from prpy.planning.exceptions import (
+    CollisionPlanningError,
+    SelfCollisionPlanningError,
+    JointLimitError)
 
 
 class PlanToEndEffectorOffsetTest(object):
@@ -106,3 +110,21 @@ def test_PlanToEndEffectorOffset_GoalInCollision_Throws(self):
             self.planner.PlanToEndEffectorOffset(
                 self.robot, direction=numpy.array([0., 0., -1.]),
                 distance=0.1)
+
+class PlanToEndEffectorOffsetCollisionTest(object):
+    """ Expects collision-specific error"""
+
+    def test_PlanToEndEffectorOffset_StartInCollision_Throws_CollisionError(self):
+        # Setup
+        with self.env:
+            self.robot.SetActiveDOFValues(self.config_feasible_goal)
+            goal_ik = self.manipulator.GetEndEffectorTransform()
+
+            self.robot.SetActiveDOFValues(self.config_env_collision)
+
+        # Test/Assert
+        with self.assertRaises((CollisionPlanningError,
+            SelfCollisionPlanningError,
+            JointLimitError)):
+            self.planner.PlanToEndEffectorOffset(
+                self.robot, direction=self.direction, distance=self.distance)

tests/planning/test_GreedyIKPlanner.py

@@ -1,10 +1,12 @@
 from methods import PlanToEndEffectorOffsetTest
+from methods.PlanToEndEffectorOffset import PlanToEndEffectorOffsetCollisionTest
 from planning_helpers import BasePlannerTest
 from prpy.planning.workspace import GreedyIKPlanner
 from unittest import TestCase
 
 
 class GreedyIKPlannerTest(BasePlannerTest,
                              PlanToEndEffectorOffsetTest,
+                             PlanToEndEffectorOffsetCollisionTest,
                              TestCase):
     planner_factory = GreedyIKPlanner 

tests/planning/test_SnapPlanner.py

@@ -2,6 +2,7 @@
     PlanToConfigurationTest,
     PlanToConfigurationStraightLineTest,
 )
+from methods.PlanToConfiguration import PlanToConfigurationTestCollisionTest
 from planning_helpers import BasePlannerTest
 from prpy.planning.snap import SnapPlanner
 from unittest import TestCase
@@ -10,6 +11,7 @@
 class SnapPlannerTest(BasePlannerTest,
                       PlanToConfigurationTest,
                       PlanToConfigurationStraightLineTest,
+                      PlanToConfigurationTestCollisionTest,
                       TestCase):
     planner_factory = SnapPlanner
 

tests/planning/test_VectorFieldPlanner.py

@@ -1,10 +1,12 @@
 from methods import PlanToEndEffectorOffsetTest
+from methods.PlanToEndEffectorOffset import PlanToEndEffectorOffsetCollisionTest
 from planning_helpers import BasePlannerTest
 from prpy.planning.vectorfield import VectorFieldPlanner
 from unittest import TestCase
 
 
 class VectorFieldPlannerTest(BasePlannerTest,
                              PlanToEndEffectorOffsetTest,
+                             PlanToEndEffectorOffsetCollisionTest,
                              TestCase):
     planner_factory = VectorFieldPlanner