Added tests for Trajopt tests (two failures, one error)

[gilwoolee]

Can we ask Pras to take a look into this?

[mkoval]

@lgw903 Can you fix the spacing on this line?

[mkoval]

Here are the tests that fail:

======================================================================
ERROR: test_PlanToEndEffectorPose_GoalIsFeasible_FindsSolution (tests.planning.test_TrajoptPlanner.TrajoptPlannerTest)
----------------------------------------------------------------------
Traceback (most recent call last):
  File "/home/mkoval/herb-ws/src/prpy/tests/planning/methods/PlanToEndEffectorPose.py", line 16, in test_PlanToEndEffectorPose_GoalIsFeasible_FindsSolution
    path = self.planner.PlanToEndEffectorPose(self.robot, goal_ik)
  File "/home/mkoval/herb-ws/src/prpy/src/prpy/planning/base.py", line 153, in __call__
    return call_planner()
  File "/home/mkoval/herb-ws/src/prpy/src/prpy/planning/base.py", line 133, in call_planner
    *args, **kw_args)
  File "/home/mkoval/herb-ws/src/or_trajopt/src/or_trajopt/planning.py", line 380, in PlanToEndEffectorPose
    return self._PlanToIK(robot, pose, **kwargs)
  File "/home/mkoval/herb-ws/src/or_trajopt/src/or_trajopt/planning.py", line 460, in _PlanToIK
    return self._Plan(robot, request, **kwargs)
  File "/home/mkoval/herb-ws/src/or_trajopt/src/or_trajopt/planning.py", line 286, in _Plan
    raise PlanningError("Result was in collision.")
PlanningError: Result was in collision.
-------------------- >> begin captured logging << --------------------
or_trajopt.planning: DEBUG: Optimization took 0.214 seconds
--------------------- >> end captured logging << ---------------------

======================================================================
FAIL: test_PlanToEndEffectorOffset_GoalInCollision_Throws2 (tests.planning.test_TrajoptPlanner.TrajoptPlannerTest)
----------------------------------------------------------------------
Traceback (most recent call last):
  File "/home/mkoval/herb-ws/src/prpy/tests/planning/methods/PlanToEndEffectorOffset.py", line 112, in test_PlanToEndEffectorOffset_GoalInCollision_Throws2
    distance=0.1)
AssertionError: PlanningError not raised
-------------------- >> begin captured logging << --------------------
or_trajopt.planning: DEBUG: Optimization took 0.023 seconds
--------------------- >> end captured logging << ---------------------

======================================================================
FAIL: test_PlanToEndEffectorOffset_SatisfiesConstraint (tests.planning.test_TrajoptPlanner.TrajoptPlannerTest)
----------------------------------------------------------------------
Traceback (most recent call last):
  File "/home/mkoval/herb-ws/src/prpy/tests/planning/methods/PlanToEndEffectorOffset.py", line 63, in test_PlanToEndEffectorOffset_SatisfiesConstraint
    linear_tol=0.005, angular_tol=0.2)
  File "/home/mkoval/herb-ws/src/prpy/tests/planning/planning_helpers.py", line 143, in assertTransformClose
    self.assertLessEqual(distance, linear_tol)
AssertionError: 0.026803204422558135 not less than or equal to 0.005
-------------------- >> begin captured logging << --------------------
or_trajopt.planning: DEBUG: Optimization took 0.020 seconds
--------------------- >> end captured logging << ---------------------

[mkoval]

@psigen Can you take a look at this? Tests (2) and (3), see below, make me particularly nervous because it looks like Trajopt is not correctly validating the output of the optimizer.

1. test_PlanToEndEffectorPose_GoalIsFeasible_FindsSolution could fail because of a local minimum (although I doubt it because it's quite an easy problem).
2. test_PlanToEndEffectorOffset_GoalInCollision_Throws2 seems like a bug in collision checking
3. test_PlanToEndEffectorOffset_SatisfiesConstraint seems like a bug in constraint checking

[gilwoolee]

@mkoval, I accidentally accepted my own pull request. I thought I was "updating" my pull request. Sorry about this.

[Shushman]

Yeah I get similar errors for the tests I made separately for or_trajopt:

• PlanToEndEffectorPose gives Result was in collision
• I just had one test for PlanToEndEffectorOffset and it gives an array mismatch assert error, so I believe that is akin to the one in test_PlanToEndEffectorOffset_SatisfiesConstraint referenced above.

[psigen]

I think these tests might be correctly flagging issues in or_trajopt. I know that PlanToEndEffectorOffset is almost certainly not working correctly, as there wasn't an obvious way to implement the "move until exactly in collision" logic in the optimizer.