Merge pull request #221 from personalrobotics/trajopt_tests

Trajopt tests and ConstraintViolationPlanningError modified.

Author: mkoval

src/prpy/planning/exceptions.py

@@ -8,6 +8,22 @@ class UnsupportedPlanningError(PlanningError):
 
 
 class ConstraintViolationPlanningError(PlanningError):
+    def __init__(self, 
+                constraint_name, 
+                threshold = None,
+                violation_by = None, 
+                base_message='Violates constraint'):
+        self.constraint_name = constraint_name 
+        self.threshold = threshold
+        self.violation_by = violation_by
+
+        super(ConstraintViolationPlanningError, self).__init__(
+            '{:s}: {:s}'.format(
+                base_message,
+                self.constraint_name
+            )
+        )
+
     pass
 
 

tests/planning/methods/PlanToEndEffectorOffset.py

@@ -4,6 +4,7 @@
 from prpy.planning.exceptions import (
     CollisionPlanningError,
     SelfCollisionPlanningError,
+    ConstraintViolationPlanningError,
     JointLimitError)
 
 
@@ -113,18 +114,37 @@ def test_PlanToEndEffectorOffset_GoalInSelfCollision_Throws(self):
 
 class PlanToEndEffectorOffsetCollisionTest(object):
     """ Expects collision-specific error"""
+    def setUp(self):
+        self.direction = numpy.array([ 0., 0., 1. ])
+        self.distance = 0.1
 
     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)):
+            JointLimitError,
+            ConstraintViolationPlanningError
+            )):
+            self.planner.PlanToEndEffectorOffset(
+                self.robot, direction=self.direction, distance=self.distance)
+
+    def test_PlanToEndEffectorOffset_GoalInEnvCollision_Throws_CollisionError(self):
+        # Setup
+        with self.env:
+            self.robot.SetActiveDOFValues(
+                PlanToEndEffectorOffsetTest.config_infeasible_env_movement)
+
+        # Test/Assert
+        with self.assertRaises((CollisionPlanningError,
+            SelfCollisionPlanningError,
+            JointLimitError,
+            ConstraintViolationPlanningError,
+            )):
             self.planner.PlanToEndEffectorOffset(
-                self.robot, direction=self.direction, distance=self.distance)
+                self.robot, direction=self.direction, distance=self.distance)

tests/planning/test_TrajoptPlanner.py

@@ -5,14 +5,14 @@
 )
 from methods.PlanToConfiguration import PlanToConfigurationTestCollisionTest
 from planning_helpers import BasePlannerTest
-from prpy.planning.snap import SnapPlanner
 from unittest import TestCase
 from or_trajopt import TrajoptPlanner
 
+
 class TrajoptPlannerTest(BasePlannerTest,
                          PlanToConfigurationTest,
-                         PlanToConfigurationStraightLineTest,
                          PlanToEndEffectorPoseTest,
+                         PlanToConfigurationTestCollisionTest,
                          TestCase):
     planner_factory = TrajoptPlanner