Review YarpOpenraveControlBoard for collision check

[jgvictores]

Review YarpOpenraveControlboard for collision check.

Might make YarpOpenraveControlboardCollision obsolete, so blocks #53.

[jgvictores]

Related: #80

[jgvictores]

Adding to YarpOpenraveControlboard/DeviceDriverImpl.cpp:

std::string outputStr;
pIndivControl->SendCommand(outputStr,"SetCheckCollisions 1");
CD_DEBUG("SetCheckCollisions[%d] returned: %s\n",i,outputStr.c_str());

And moving in examples/python/openraveYarpPluginLoader-controlboard-collision-sim.py to load the controlboard after:

    # Convex Decomposition padding
    teo_robot = env.GetRobots()[0]
    cdmodel = databases.convexdecomposition.ConvexDecompositionModel(teo_robot)

    # Load the ConvexDecomposition model, if it does not exit in the database generate it.
    if not cdmodel.load(): # It always load the model with zero padding
        # If not already in the database. Generate:
        cdmodel.generate(padding=0.02)

    cdmodel.save()
    print 'Finished saving'

    print 'Setting robot...'
    cdmodel.setrobot()
    print 'Finish setrobot'

Got the following line at collisions during runtime:

[idealcontroller.cpp:526 _ReportError] self collsion in trajectory: (teoSim:r55)x(teoSim:r65), contacts=0, time=0.130000

Which corresponds to this line, which is just a warning (no movement stop). This means we need changes at the OR (e.g. inside the "idealcontroller" SimulationStep) or ORYP level.

[jgvictores]

PS: Line comes from _CheckConfiguration.

[jgvictores]

Blocks #80 in addition to mentioned #53.

Described changes implemented at bfea7d4.

[jgvictores]

Last commit wasn't working (as in "outputting the warning"), had to commit 1c9de50 (SetCheckCollisions -> SetCheckCollisions 1)

[jgvictores]

Further experiments demonstrate:

1. controlboard can be created before convexdecomp (see ccd8a3a), issue was missing SetCheckCollisions 1.
2. Adding SetThrowExceptions 1 as in 12ea290 the old warning is now an error (it is handled in environment-core.h, do not know how to catch it elsewhere):

[environment-core.h:2577 _SimulationThread] simulation thread exception: openrave (Assert): self collsion in trajectory: (teoSim:cintura)x(teoSim:r52), contacts=0, time=2.010000