Merge pull request #142 from personalrobotics/bugfix/snap_waypoints

Added a check within snap planner for one-waypoint trajectories.

Author: mkoval

src/prpy/planning/snap.py

@@ -6,7 +6,7 @@
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions are met:
-# 
+#
 # - Redistributions of source code must retain the above copyright notice, this
 #   list of conditions and the following disclaimer.
 # - Redistributions in binary form must reproduce the above copyright notice,
@@ -15,7 +15,7 @@
 # - Neither the name of Carnegie Mellon University nor the names of its
 #   contributors may be used to endorse or promote products derived from this
 #   software without specific prior written permission.
-# 
+#
 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
@@ -32,6 +32,7 @@
 from ..util import SetTrajectoryTags
 from base import BasePlanner, PlanningError, PlanningMethod, Tags
 
+
 class SnapPlanner(BasePlanner):
     """Planner that checks the straight-line trajectory to the goal.
 
@@ -40,15 +41,15 @@ class SnapPlanner(BasePlanner):
     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.
     """
     def __init__(self):
         super(SnapPlanner, self).__init__()
- 
+
     def __str__(self):
         return 'SnapPlanner'
 
@@ -86,7 +87,6 @@ def PlanToEndEffectorPose(self, robot, goal_pose, **kw_args):
         # close to the configuration of the arm, so we don't need to do any
         # custom IK ranking.
         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,
@@ -101,7 +101,7 @@ def PlanToEndEffectorPose(self, robot, goal_pose, **kw_args):
     def _Snap(self, robot, goal, **kw_args):
         Closed = openravepy.Interval.Closed
 
-        curr = robot.GetActiveDOFValues()
+        start = robot.GetActiveDOFValues()
         active_indices = robot.GetActiveDOFIndices()
 
         # Use the CheckPathAllConstraints helper function to collision check
@@ -110,28 +110,32 @@ def _Snap(self, robot, goal, **kw_args):
         params = openravepy.Planner.PlannerParameters()
         params.SetRobotActiveJoints(robot)
         params.SetGoalConfig(goal)
-        check = params.CheckPathAllConstraints(curr, goal, [], [], 0., Closed)
+        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.')
 
-        # Create a two-point trajectory that starts at our current
-        # configuration and takes us to the goal.
-        traj = openravepy.RaveCreateTrajectory(self.env, '')
+        # Create a trajectory that starts at our current configuration.
         cspec = robot.GetActiveConfigurationSpecification()
-        active_indices = robot.GetActiveDOFIndices()
+        traj = openravepy.RaveCreateTrajectory(self.env, '')
+        traj.Init(cspec)
 
-        waypoints = numpy.zeros((2, cspec.GetDOF()))
-        cspec.InsertJointValues(waypoints[0, :], curr, robot,
-                                active_indices, False)
-        cspec.InsertJointValues(waypoints[1, :], goal, robot,
+        start_waypoint = numpy.zeros(cspec.GetDOF())
+        cspec.InsertJointValues(start_waypoint, start, robot,
                                 active_indices, False)
+        traj.Insert(0, start_waypoint.ravel())
 
-        traj.Init(cspec)
-        traj.Insert(0, waypoints.ravel())
+        # Make the trajectory end at the goal configuration, as long as it
+        # was not identical to the start configuration.
+        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.
         SetTrajectoryTags(traj, {Tags.SMOOTH: True}, append=True)
         return traj