Merge pull request #228 from DavidB-CMU/master

Added method for collision checking trajectories.

Author: mkoval

src/prpy/planning/snap.py

@@ -29,23 +29,25 @@
 # POSSIBILITY OF SUCH DAMAGE.
 import numpy
 import openravepy
-from ..util import SetTrajectoryTags
-from base import BasePlanner, PlanningError, PlanningMethod, Tags
+from prpy.util import SetTrajectoryTags
+from prpy.planning.base import BasePlanner
+from prpy.planning.base import PlanningError
+from prpy.planning.base import PlanningMethod
+from prpy.planning.base import Tags
 
 
 class SnapPlanner(BasePlanner):
     """Planner that checks the straight-line trajectory to the goal.
 
     SnapPlanner is a utility planner class that collision checks the
-    straight-line trajectory to the goal. If that trajectory is invalid, i.e..
-    due to an environment or self collision, the planner immediately returns
-    failure by raising a PlanningError. Collision checking is performed using
-    the standard CheckPathAllConstraints method.
-
-    SnapPlanner is intended to be used only as a "short circuit" to speed up
-    planning between nearby configurations. This planner is most commonly used
-    as the first item in a Sequence meta-planner to avoid calling a motion
-    planner when the trivial solution is valid.
+    straight-line trajectory to the goal. If that trajectory is invalid,
+    e.g. due to an environment or self collision, the planner
+    immediately returns failure by raising a PlanningError.
+
+    SnapPlanner is intended to be used only as a "short circuit" to
+    speed-up planning between nearby configurations. This planner is
+    most commonly used as the first item in a Sequence meta-planner to
+    avoid calling a motion planner when the trivial solution is valid.
     """
     def __init__(self):
         super(SnapPlanner, self).__init__()
@@ -56,34 +58,35 @@ def __str__(self):
     @PlanningMethod
     def PlanToConfiguration(self, robot, goal, **kw_args):
         """
-        Attempt to plan a straight line trajectory from the robot's current
-        configuration to the goal configuration. This will fail if the
-        straight-line path is not collision free.
+        Attempt to plan a straight line trajectory from the robot's
+        current configuration to the goal configuration. This will
+        fail if the straight-line path is not collision free.
 
         @param robot
         @param goal desired configuration
         @return traj
         """
+
         return self._Snap(robot, goal, **kw_args)
 
     @PlanningMethod
     def PlanToEndEffectorPose(self, robot, goal_pose, **kw_args):
         """
-        Attempt to plan a straight line trajectory from the robot's current
-        configuration to a desired end-effector pose. This happens by finding
-        the closest IK solution to the robot's current configuration and
-        attempts to snap there (using PlanToConfiguration) if possible. In the
-        case of a redundant manipulator, no attempt is made to check other IK
-        solutions.
+        Attempt to plan a straight line trajectory from the robot's
+        current configuration to a desired end-effector pose. This
+        happens by finding the closest IK solution to the robot's
+        current configuration and attempts to snap there
+        (using PlanToConfiguration) if possible.
+        In the case of a redundant manipulator, no attempt is
+        made to check other IK solutions.
 
         @param robot
         @param goal_pose desired end-effector pose
         @return traj
         """
-        
-        from .exceptions import CollisionPlanningError,SelfCollisionPlanningError
-        from openravepy import CollisionReport
-        
+        from prpy.planning.exceptions import CollisionPlanningError
+        from prpy.planning.exceptions import SelfCollisionPlanningError
+
         ikp = openravepy.IkParameterizationType
         ikfo = openravepy.IkFilterOptions
 
@@ -99,17 +102,16 @@ def PlanToEndEffectorPose(self, robot, goal_pose, **kw_args):
 
         if ik_solution is None: 
             # FindIKSolutions is slower than FindIKSolution, 
-            # so call this only to identify and raise error when there is no solution
+            # 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
-                )
-
+                                                    ik_param,
+                                                    ikfo.IgnoreSelfCollisions,
+                                                    ikreturn=False,
+                                                    releasegil=True)
             for q in ik_solutions:
                 robot.SetActiveDOFValues(q)
-                report = CollisionReport()
+                report = openravepy.CollisionReport()
                 if self.env.CheckCollision(robot, report=report):
                     raise CollisionPlanningError.FromReport(report) 
                 elif robot.CheckSelfCollision(report=report):
@@ -120,81 +122,70 @@ def PlanToEndEffectorPose(self, robot, goal_pose, **kw_args):
         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()
-        active_indices = robot.GetActiveDOFIndices()
-
-        # Use the CheckPathAllConstraints helper function to collision check
-        # the straight-line trajectory. We pass dummy values for dq0, dq1,
-        # and timeelapsed since this is a purely geometric check.
-        params = openravepy.Planner.PlannerParameters()
-        params.SetRobotActiveJoints(robot)
-        params.SetGoalConfig(goal)
-        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.
+        from prpy.util import CheckJointLimits
+        from prpy.util import GetLinearCollisionCheckPts
+        #from prpy.util import SampleTimeGenerator
+        from prpy.util import VanDerCorputSampleGenerator
+        from prpy.planning.exceptions import CollisionPlanningError
+        from prpy.planning.exceptions import SelfCollisionPlanningError
+
+        # Create a two-point trajectory between the
+        # current configuration and the goal.
+        # (a straight line in joint space)
         traj = openravepy.RaveCreateTrajectory(self.env, '')
         cspec = robot.GetActiveConfigurationSpecification('linear')
         active_indices = robot.GetActiveDOFIndices()
 
+        # Check the start position is within joint limits,
+        # this can throw a JointLimitError
+        start = robot.GetActiveDOFValues()
+        CheckJointLimits(robot, start)
+
+        # Add the start waypoint
         start_waypoint = numpy.zeros(cspec.GetDOF())
         cspec.InsertJointValues(start_waypoint, start, robot,
                                 active_indices, False)
-
         traj.Init(cspec)
         traj.Insert(0, start_waypoint.ravel())
 
-        # Make the trajectory end at the goal configuration, as long as it
-        # was not identical to the start configuration.
+        # Make the trajectory end at the goal configuration, as
+        # long as it is not in collision and is not identical to
+        # the start configuration.
+        CheckJointLimits(robot, goal)
         if not numpy.allclose(start, goal):
             goal_waypoint = numpy.zeros(cspec.GetDOF())
             cspec.InsertJointValues(goal_waypoint, goal, robot,
                                     active_indices, False)
             traj.Insert(1, goal_waypoint.ravel())
 
-        # Tag the return trajectory as smooth (in joint space) and return it.
+        # Get joint configurations to check
+        # Note: this returns a python generator, and if the
+        # trajectory only has one waypoint then only the
+        # start configuration will be collisioned checked.
+        #
+        # Sampling function:
+        # 'linear'
+        #linear = SampleTimeGenerator
+        # 'Van der Corput'
+        vdc = VanDerCorputSampleGenerator
+        checks = GetLinearCollisionCheckPts(robot, \
+                                            traj, \
+                                            norm_order=2, \
+                                            sampling_func=vdc)
+
+        # Run constraint checks at DOF resolution:
+        for t, q in checks:
+            # Set the joint positions
+            # Note: the planner is using a cloned 'robot' object
+            robot.SetActiveDOFValues(q)
+
+            # Check for collisions
+            report = openravepy.CollisionReport()
+            if self.env.CheckCollision(robot, report=report):
+                raise CollisionPlanningError.FromReport(report)
+            elif robot.CheckSelfCollision(report=report):
+                raise SelfCollisionPlanningError.FromReport(report)
+
+        # Tag the return trajectory as smooth (in joint space).
         SetTrajectoryTags(traj, {Tags.SMOOTH: True}, append=True)
         return traj