Tests

[Shushman]

A first attempt at tests for OR_TRAJOPT. A summary of the file:

• Provided first level tests for PlanToConfiguration, PlanToEndEffectorPose, PlanToIK, PlanToEndEffectorOffset. Using the test values from or_ompl.
• Miss tests for PlanToTSR as I need to figure out TSRs better.

Issues to note:

• (Possible bug): The test when done from START_CONFIG to GOAL_CONFIG works but not when done from corresponding end effector poses.
• Test for end effector offset does not seem to be working as expected - the (x,y,z) after the offset does not seem to be changed according to the displacement vector.

[mkoval]

These look like a good start.

However, they are very similar to the planner-agnostic tests we have in prpy. Ideally, you should integrate your improvements to these tests into the prpy tests, when possible. This will let us benefit from them on all of our planners that support these methods. Any tests that are planner-specific (e.g. test trajopt-specific functionality) should stay standalone.

Sorry if this was misleading. We typically have two types of tests for planners:

1. a test for the raw planner
2. a test for the prpy wrapper

Unlike most of our planners, we did not write the core C++ code. Therefore, we may only need to test (2).

[mkoval]

@lgw903 added Trajopt tests to PrPy in personalrobotics/prpy#213.

@Shushman Can you take a look at that pull request and add any tests that you think are missing? In particular, I definitely want to verify that we are testing the two bugs that you found.