Merge pull request #227 from personalrobotics/bugfix/trajopt_tests

Fixed Trajopt tests

Author: jeking04

tests/planning/methods/PlanToConfiguration.py

@@ -86,7 +86,7 @@ def test_PlanToConfiguration_StraightLineIsFeasible_FindsSolution(self):
 class PlanToConfigurationCompleteTest(object):
     def test_PlanToConfiguration_GoalIsFeasible_FindsSolution(self):
         self.PlanFromStartToGoalConfiguration(
-            self.config_feasible_start, self.config_feasible_goal)
+            self.config_feasible_start2, self.config_feasible_goal)
 
 
 class PlanToConfigurationTestCollisionTest(object): 

tests/planning/methods/PlanToEndEffectorPose.py

@@ -10,7 +10,7 @@ def test_PlanToEndEffectorPose_GoalIsFeasible_FindsSolution(self):
             self.robot.SetActiveDOFValues(self.config_feasible_goal)
             goal_ik = self.manipulator.GetEndEffectorTransform()
 
-            self.robot.SetActiveDOFValues(self.config_feasible_start)
+            self.robot.SetActiveDOFValues(self.config_feasible_start2)
 
         # Test
         path = self.planner.PlanToEndEffectorPose(self.robot, goal_ik)
@@ -19,7 +19,7 @@ def test_PlanToEndEffectorPose_GoalIsFeasible_FindsSolution(self):
         self.ValidatePath(path)
 
         first_waypoint = path.GetWaypoint(0)
-        assert_allclose(first_waypoint, self.config_feasible_start)
+        assert_allclose(first_waypoint, self.config_feasible_start2)
 
         with self.env:
             last_waypoint = path.GetWaypoint(path.GetNumWaypoints() - 1)

tests/planning/planning_helpers.py

@@ -10,6 +10,15 @@ class BasePlannerTest(object):
         +2.35061574,  0.61043555,  0.85000000,  1.80684444, -0.08639935,
         -0.69750474,  1.31656172
     ])
+
+    # This configuration is easier for Trajopt (and harder for most other
+    # planners) because teh pole introduces a local minimum (maybe?).
+    config_feasible_start2 = numpy.array([
+        -2.61799387,  0.66440338,  0.19853743,  2.00944286, -0.08639925,
+        -0.69750467,  1.31656153
+    ])
+
+
     config_feasible_goal = numpy.array([
         -0.84085883,  1.44573701,  0.20000000,  1.72620231, -0.81124757,
         -1.39363597,  1.29233111

tests/planning/test_TrajoptPlanner.py

@@ -3,7 +3,10 @@
     PlanToConfigurationStraightLineTest,
     PlanToEndEffectorPoseTest,
 )
-from methods.PlanToConfiguration import PlanToConfigurationTestCollisionTest
+from methods.PlanToConfiguration import (
+    PlanToConfigurationTestCollisionTest,
+    PlanToConfigurationCompleteTest,
+)
 from planning_helpers import BasePlannerTest
 from unittest import TestCase
 from or_trajopt import TrajoptPlanner
@@ -12,7 +15,9 @@
 class TrajoptPlannerTest(BasePlannerTest,
                          PlanToConfigurationTest,
                          PlanToEndEffectorPoseTest,
+                         PlanToConfigurationCompleteTest,
                          PlanToConfigurationTestCollisionTest,
+                         PlanToConfigurationStraightLineTest,
                          TestCase):
     planner_factory = TrajoptPlanner