search

roboticslab-uc3m / openrave-yarp-plugins

OpenRAVE plugins to interface OpenRAVE with YARP.
https://robots.uc3m.es/openrave-yarp-plugins/
GNU Lesser General Public License v2.1
3 stars 1 forks source link

Review YarpOpenraveControlBoard for collision check #82

Open jgvictores opened 6 years ago

jgvictores commented 6 years ago

Review YarpOpenraveControlboard for collision check.

Might make YarpOpenraveControlboardCollision obsolete, so blocks #53.

jgvictores commented 6 years ago

Related: #80

jgvictores commented 6 years ago

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 commented 6 years ago

PS: Line comes from _CheckConfiguration.

jgvictores commented 6 years ago

Blocks #80 in addition to mentioned #53.

Described changes implemented at bfea7d40b6d43dbfece0f16da2f0a30e8c243c81.

jgvictores commented 6 years ago

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

jgvictores commented 6 years ago

Further experiments demonstrate:

  1. controlboard can be created before convexdecomp (see ccd8a3afa0dd2b0b05f2cf9a02a5a63604d9edf1), issue was missing SetCheckCollisions 1.
  2. Adding SetThrowExceptions 1 as in 12ea290f82cbdb4691b669e9e7746e628c1bbb61 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
jgvictores commented 6 years ago

More thoughts involve the issue that there is no awareness of which actuation is causing the collision, so all robot joints should be stopped. Seems like a task for a separate OpenRAVE plugin.

jgvictores commented 6 years ago

Old WIP at issue-82-collision may be deleted, new WIP at issue-82-collision-or.

jgvictores commented 6 years ago

Think about RegisterCollisionCallback:

jgvictores commented 6 years ago

Unfortunately, the callback mechanism only works enabling the ODE physics engine (not only a collision checker). Output achieved:

[success] OpenraveCollisionStop.cpp:42 _IgnoreCollisionCallback(): Here: (teoSim:r65)x(teoSim:r63), contacts=16
[success] OpenraveCollisionStop.cpp:42 _IgnoreCollisionCallback(): Here: (teoSim:r60)x(teoSim:r62), contacts=16
[success] OpenraveCollisionStop.cpp:42 _IgnoreCollisionCallback(): Here: (teoSim:r55)x(teoSim:r53), contacts=16
[success] OpenraveCollisionStop.cpp:42 _IgnoreCollisionCallback(): Here: (teoSim:r50)x(teoSim:r52), contacts=16
[success] OpenraveCollisionStop.cpp:42 _IgnoreCollisionCallback(): Here: (teoSim:r31)x(teoSim:cintura), contacts=16
[success] OpenraveCollisionStop.cpp:42 _IgnoreCollisionCallback(): Here: (teoSim:r31)x(teoSim:cintura), contacts=16
jgvictores commented 6 years ago

Unfortunately, the callback mechanism only works enabling the ODE physics engine (not only a collision checker). Output achieved:

And no improvement with pIndivControl->SendCommand(outputStr,"SetCheckCollisions 1");. ODE does something, even with PQP checker.

jgvictores commented 6 years ago

Looks like FCL and Bullet implementations also do this. However, to not force the user to have these, may succumb to the temptation of implementing a separate thread.

jgvictores commented 6 years ago

Tried some more things because PQP code looks like it should call the callbacks.

Test 1:

    collisionChecker = RaveCreateCollisionChecker(env,'pqp')
    collisionChecker.SetCollisionOptions(CollisionOptions.Contacts)
    env.SetCollisionChecker(collisionChecker)

Test 2 (can't even move):

    collisionChecker = RaveCreateCollisionChecker(env,'pqp')
    collisionChecker.SetCollisionOptions(CollisionOptions.Distance|CollisionOptions.Contacts)
    env.SetCollisionChecker(collisionChecker)

Both constantly return the following after first attempt of movement:

[success] OpenraveCollisionStop.cpp:42 _IgnoreCollisionCallback(): Here: ()x(), contacts=0
jgvictores commented 6 years ago

Current status (in addition to #83):

jgvictores commented 6 years ago

The fact that When a cdmodel.show() is executed in the code, the execution of the rest of the program stops. Is this normal? had been stated before.

The culprit could be this while True.

jgvictores commented 6 years ago

The solution is to have two meshes: self-collision mesh is high-res, environment collision mesh gets convex decomposition.

In the case of the current teo-openrave-models, there are still self-collisions even with zero padding.

jgvictores commented 6 years ago

Thanks to #83, now model loading is faster. In addition, commenting out the controlboard as in 07ecc4541e6e3232e2c5656fe75ce58f7c10ae4a... has lead to very strange segfault that traces back to the RateThread::start() even with an empty run()... I recall something similar long ago, but couldn't trace it back. Reverting last commit for now.

jgvictores commented 6 years ago

The segfault has been traced to upstream https://github.com/robotology/yarp/issues/1595, which means it wasn't due to race conditions but to the lack of the Network yarp object that was instanced by our other plugin.

jgvictores commented 6 years ago

I recall something similar long ago, but couldn't trace it back. Reverting last commit for now.

So, finally not reverting. Simply added Network yarp object at fbaca6c.

jgvictores commented 6 years ago

Strange things via manipulatorPtr->GetIndependentLinks:

[debug] OpenraveCollisionStop.cpp:153 Open(): * Manipulator [0]: trunk
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [0]: RootWaist
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [1]: r50
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [2]: r51
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [3]: r52
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [4]: r53
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [5]: r5dummy
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [6]: r54
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [7]: r55
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [8]: r5Man
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [9]: r60
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [10]: r61
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [11]: r62
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [12]: r63
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [13]: r6dummy
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [14]: r64
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [15]: r65
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [16]: r6Man
[debug] OpenraveCollisionStop.cpp:153 Open(): * Manipulator [1]: head
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [0]: RootWaist
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [1]: AxialWaist
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [2]: FrontalWaist
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [3]: r10
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [4]: r11
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [5]: r12
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [6]: r13
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [7]: r14
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [8]: RightFrontalWrist
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [9]: r1Man
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [10]: RightThumbProximal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [11]: RightThumbDistal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [12]: RightExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [13]: RightExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [14]: RightInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [15]: RightInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [16]: r20
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [17]: r21
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [18]: r22
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [19]: r23
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [20]: r24
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [21]: LeftFrontalWrist
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [22]: r2Man
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [23]: LeftThumbProximal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [24]: LeftThumbDistal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [25]: LeftExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [26]: LeftExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [27]: LeftInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [28]: LeftInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [29]: r50
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [30]: r51
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [31]: r52
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [32]: r53
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [33]: r5dummy
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [34]: r54
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [35]: r55
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [36]: r5Man
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [37]: r60
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [38]: r61
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [39]: r62
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [40]: r63
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [41]: r6dummy
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [42]: r64
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [43]: r65
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [44]: r6Man
[debug] OpenraveCollisionStop.cpp:153 Open(): * Manipulator [2]: rightArm
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [0]: RootWaist
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [1]: AxialWaist
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [2]: FrontalWaist
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [3]: AxialNeck
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [4]: FrontalNeck
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [5]: r20
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [6]: r21
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [7]: r22
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [8]: r23
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [9]: r24
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [10]: LeftFrontalWrist
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [11]: r2Man
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [12]: LeftThumbProximal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [13]: LeftThumbDistal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [14]: LeftExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [15]: LeftExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [16]: LeftInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [17]: LeftInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [18]: r50
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [19]: r51
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [20]: r52
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [21]: r53
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [22]: r5dummy
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [23]: r54
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [24]: r55
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [25]: r5Man
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [26]: r60
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [27]: r61
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [28]: r62
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [29]: r63
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [30]: r6dummy
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [31]: r64
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [32]: r65
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [33]: r6Man
[debug] OpenraveCollisionStop.cpp:153 Open(): * Manipulator [3]: rightArm_trunk
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [0]: RootWaist
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [1]: r50
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [2]: r51
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [3]: r52
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [4]: r53
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [5]: r5dummy
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [6]: r54
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [7]: r55
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [8]: r5Man
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [9]: r60
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [10]: r61
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [11]: r62
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [12]: r63
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [13]: r6dummy
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [14]: r64
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [15]: r65
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [16]: r6Man
[debug] OpenraveCollisionStop.cpp:153 Open(): * Manipulator [4]: leftArm
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [0]: RootWaist
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [1]: AxialWaist
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [2]: FrontalWaist
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [3]: AxialNeck
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [4]: FrontalNeck
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [5]: r10
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [6]: r11
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [7]: r12
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [8]: r13
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [9]: r14
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [10]: RightFrontalWrist
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [11]: r1Man
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [12]: RightThumbProximal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [13]: RightThumbDistal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [14]: RightExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [15]: RightExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [16]: RightInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [17]: RightInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [18]: r50
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [19]: r51
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [20]: r52
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [21]: r53
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [22]: r5dummy
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [23]: r54
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [24]: r55
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [25]: r5Man
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [26]: r60
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [27]: r61
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [28]: r62
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [29]: r63
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [30]: r6dummy
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [31]: r64
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [32]: r65
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [33]: r6Man
[debug] OpenraveCollisionStop.cpp:153 Open(): * Manipulator [5]: leftArm_trunk
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [0]: RootWaist
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [1]: r50
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [2]: r51
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [3]: r52
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [4]: r53
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [5]: r5dummy
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [6]: r54
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [7]: r55
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [8]: r5Man
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [9]: r60
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [10]: r61
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [11]: r62
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [12]: r63
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [13]: r6dummy
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [14]: r64
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [15]: r65
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [16]: r6Man
[debug] OpenraveCollisionStop.cpp:153 Open(): * Manipulator [6]: rightLeg
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [0]: RootWaist
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [1]: AxialWaist
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [2]: FrontalWaist
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [3]: AxialNeck
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [4]: FrontalNeck
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [5]: r10
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [6]: r11
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [7]: r12
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [8]: r13
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [9]: r14
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [10]: RightFrontalWrist
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [11]: r1Man
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [12]: RightThumbProximal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [13]: RightThumbDistal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [14]: RightExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [15]: RightExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [16]: RightInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [17]: RightInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [18]: r20
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [19]: r21
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [20]: r22
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [21]: r23
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [22]: r24
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [23]: LeftFrontalWrist
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [24]: r2Man
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [25]: LeftThumbProximal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [26]: LeftThumbDistal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [27]: LeftExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [28]: LeftExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [29]: LeftInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [30]: LeftInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [31]: r60
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [32]: r61
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [33]: r62
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [34]: r63
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [35]: r6dummy
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [36]: r64
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [37]: r65
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [38]: r6Man
[debug] OpenraveCollisionStop.cpp:153 Open(): * Manipulator [7]: leftLeg
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [0]: RootWaist
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [1]: AxialWaist
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [2]: FrontalWaist
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [3]: AxialNeck
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [4]: FrontalNeck
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [5]: r10
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [6]: r11
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [7]: r12
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [8]: r13
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [9]: r14
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [10]: RightFrontalWrist
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [11]: r1Man
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [12]: RightThumbProximal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [13]: RightThumbDistal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [14]: RightExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [15]: RightExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [16]: RightInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [17]: RightInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [18]: r20
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [19]: r21
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [20]: r22
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [21]: r23
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [22]: r24
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [23]: LeftFrontalWrist
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [24]: r2Man
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [25]: LeftThumbProximal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [26]: LeftThumbDistal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [27]: LeftExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [28]: LeftExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [29]: LeftInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [30]: LeftInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [31]: r50
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [32]: r51
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [33]: r52
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [34]: r53
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [35]: r5dummy
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [36]: r54
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [37]: r55
[debug] OpenraveCollisionStop.cpp:160 Open(): ** Link [38]: r5Man
jgvictores commented 6 years ago

Less stranger things (but still strange) via manipulatorPtr->GetChildLinks(vectorOfLinkPtr):

[debug] OpenraveCollisionStop.cpp:153 Open(): * Manipulator [0]: trunk
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [0]: FrontalWaist
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [1]: AxialNeck
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [2]: FrontalNeck
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [3]: r10
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [4]: r11
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [5]: r12
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [6]: r13
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [7]: r14
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [8]: RightFrontalWrist
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [9]: r1Man
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [10]: RightThumbProximal
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [11]: RightThumbDistal
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [12]: RightExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [13]: RightExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [14]: RightInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [15]: RightInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [16]: r20
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [17]: r21
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [18]: r22
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [19]: r23
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [20]: r24
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [21]: LeftFrontalWrist
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [22]: r2Man
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [23]: LeftThumbProximal
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [24]: LeftThumbDistal
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [25]: LeftExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [26]: LeftExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [27]: LeftInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [28]: LeftInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:153 Open(): * Manipulator [1]: head
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [0]: FrontalNeck
[debug] OpenraveCollisionStop.cpp:153 Open(): * Manipulator [2]: rightArm
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [0]: r1Man
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [1]: RightFrontalWrist
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [2]: RightThumbProximal
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [3]: RightThumbDistal
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [4]: RightExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [5]: RightExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [6]: RightInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [7]: RightInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:153 Open(): * Manipulator [3]: rightArm_trunk
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [0]: r1Man
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [1]: RightFrontalWrist
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [2]: RightThumbProximal
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [3]: RightThumbDistal
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [4]: RightExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [5]: RightExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [6]: RightInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [7]: RightInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:153 Open(): * Manipulator [4]: leftArm
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [0]: r2Man
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [1]: LeftFrontalWrist
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [2]: LeftThumbProximal
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [3]: LeftThumbDistal
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [4]: LeftExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [5]: LeftExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [6]: LeftInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [7]: LeftInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:153 Open(): * Manipulator [5]: leftArm_trunk
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [0]: r2Man
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [1]: LeftFrontalWrist
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [2]: LeftThumbProximal
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [3]: LeftThumbDistal
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [4]: LeftExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [5]: LeftExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [6]: LeftInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [7]: LeftInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:153 Open(): * Manipulator [6]: rightLeg
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [0]: r5Man
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [1]: r55
[debug] OpenraveCollisionStop.cpp:153 Open(): * Manipulator [7]: leftLeg
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [0]: r6Man
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Link [1]: r65
jgvictores commented 6 years ago

Link names are pretty crazy. This is tracked at https://github.com/roboticslab-uc3m/teo-main/issues/37.

jgvictores commented 6 years ago

For the record, GetChildJoints (this begins to make sense, Children are those that do not belong but can be affected):

[debug] OpenraveCollisionStop.cpp:153 Open(): * Manipulator [0]: trunk
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [0]: q00
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [1]: q01
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [2]: q10
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [3]: q11
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [4]: q12
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [5]: q13
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [6]: q14
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [7]: RightFrontalWristJoint
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [8]: RightThumbProximalJoint
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [9]: RightThumbDistalJoint
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [10]: RightExternalFingerProximalJoint
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [11]: RightExternalFingerDistalJoint
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [12]: RightInternalFingerProximalJoint
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [13]: RightInternalFingerDistalJoint
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [14]: q20
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [15]: q21
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [16]: q22
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [17]: q23
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [18]: q24
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [19]: LeftFrontalWristJoint
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [20]: LeftThumbProximalJoint
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [21]: LeftThumbDistalJoint
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [22]: LeftExternalFingerProximalJoint
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [23]: LeftExternalFingerDistalJoint
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [24]: LeftInternalFingerProximalJoint
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [25]: LeftInternalFingerDistalJoint
[debug] OpenraveCollisionStop.cpp:153 Open(): * Manipulator [1]: head
[debug] OpenraveCollisionStop.cpp:153 Open(): * Manipulator [2]: rightArm
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [0]: RightThumbProximalJoint
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [1]: RightThumbDistalJoint
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [2]: RightExternalFingerProximalJoint
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [3]: RightExternalFingerDistalJoint
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [4]: RightInternalFingerProximalJoint
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [5]: RightInternalFingerDistalJoint
[debug] OpenraveCollisionStop.cpp:153 Open(): * Manipulator [3]: rightArm_trunk
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [0]: RightThumbProximalJoint
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [1]: RightThumbDistalJoint
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [2]: RightExternalFingerProximalJoint
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [3]: RightExternalFingerDistalJoint
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [4]: RightInternalFingerProximalJoint
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [5]: RightInternalFingerDistalJoint
[debug] OpenraveCollisionStop.cpp:153 Open(): * Manipulator [4]: leftArm
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [0]: LeftThumbProximalJoint
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [1]: LeftThumbDistalJoint
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [2]: LeftExternalFingerProximalJoint
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [3]: LeftExternalFingerDistalJoint
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [4]: LeftInternalFingerProximalJoint
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [5]: LeftInternalFingerDistalJoint
[debug] OpenraveCollisionStop.cpp:153 Open(): * Manipulator [5]: leftArm_trunk
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [0]: LeftThumbProximalJoint
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [1]: LeftThumbDistalJoint
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [2]: LeftExternalFingerProximalJoint
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [3]: LeftExternalFingerDistalJoint
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [4]: LeftInternalFingerProximalJoint
[debug] OpenraveCollisionStop.cpp:161 Open(): ** Joint [5]: LeftInternalFingerDistalJoint
[debug] OpenraveCollisionStop.cpp:153 Open(): * Manipulator [6]: rightLeg
[debug] OpenraveCollisionStop.cpp:153 Open(): * Manipulator [7]: leftLeg
jgvictores commented 6 years ago

Better using GetArmIndices mechanism as in: https://github.com/roboticslab-uc3m/openrave-yarp-plugins/blob/f85d8be4ab6c07a3254ed3c43952f554a6fcb233/libraries/YarpPlugins/YarpOpenraveControlboard/DeviceDriverImpl.cpp#L43-L55

Gets:

[debug] OpenraveCollisionStop.cpp:155 Open(): * Manipulator [0]: trunk
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [0]: q30
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [1]: q31
[debug] OpenraveCollisionStop.cpp:155 Open(): * Manipulator [1]: head
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [0]: q00
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [1]: q01
[debug] OpenraveCollisionStop.cpp:155 Open(): * Manipulator [2]: rightArm
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [0]: q10
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [1]: q11
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [2]: q12
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [3]: q13
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [4]: q14
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [5]: RightFrontalWristJoint
[debug] OpenraveCollisionStop.cpp:155 Open(): * Manipulator [3]: rightArm_trunk
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [0]: q30
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [1]: q31
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [2]: q10
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [3]: q11
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [4]: q12
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [5]: q13
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [6]: q14
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [7]: RightFrontalWristJoint
[debug] OpenraveCollisionStop.cpp:155 Open(): * Manipulator [4]: leftArm
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [0]: q20
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [1]: q21
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [2]: q22
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [3]: q23
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [4]: q24
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [5]: LeftFrontalWristJoint
[debug] OpenraveCollisionStop.cpp:155 Open(): * Manipulator [5]: leftArm_trunk
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [0]: q30
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [1]: q31
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [2]: q20
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [3]: q21
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [4]: q22
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [5]: q23
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [6]: q24
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [7]: LeftFrontalWristJoint
[debug] OpenraveCollisionStop.cpp:155 Open(): * Manipulator [6]: rightLeg
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [0]: q50
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [1]: q51
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [2]: q52
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [3]: q53
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [4]: q54
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [5]: q55
[debug] OpenraveCollisionStop.cpp:155 Open(): * Manipulator [7]: leftLeg
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [0]: q60
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [1]: q61
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [2]: q62
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [3]: q63
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [4]: q64
[debug] OpenraveCollisionStop.cpp:170 Open(): ** Joint [5]: q65
jgvictores commented 6 years ago

Interesting list of affected, via 639436df6800d88c79e2608f1593f2d83e3ae64a:

[debug] OpenraveCollisionStop.cpp:155 Open(): * Manipulator [0]: trunk
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [0]: q30
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: AxialWaist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: FrontalWaist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: AxialNeck
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: FrontalNeck
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r10
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r11
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r12
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r13
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r14
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightFrontalWrist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r1Man
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightThumbProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightThumbDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r20
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r21
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r22
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r23
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r24
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftFrontalWrist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r2Man
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftThumbProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftThumbDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [1]: q31
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: FrontalWaist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: AxialNeck
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: FrontalNeck
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r10
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r11
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r12
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r13
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r14
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightFrontalWrist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r1Man
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightThumbProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightThumbDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r20
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r21
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r22
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r23
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r24
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftFrontalWrist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r2Man
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftThumbProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftThumbDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:155 Open(): * Manipulator [1]: head
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [0]: q00
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: AxialNeck
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: FrontalNeck
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [1]: q01
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: FrontalNeck
[debug] OpenraveCollisionStop.cpp:155 Open(): * Manipulator [2]: rightArm
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [0]: q10
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r10
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r11
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r12
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r13
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r14
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightFrontalWrist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r1Man
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightThumbProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightThumbDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [1]: q11
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r11
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r12
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r13
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r14
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightFrontalWrist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r1Man
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightThumbProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightThumbDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [2]: q12
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r12
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r13
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r14
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightFrontalWrist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r1Man
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightThumbProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightThumbDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [3]: q13
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r13
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r14
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightFrontalWrist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r1Man
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightThumbProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightThumbDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [4]: q14
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r14
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightFrontalWrist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r1Man
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightThumbProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightThumbDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [5]: RightFrontalWristJoint
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightFrontalWrist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r1Man
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightThumbProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightThumbDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:155 Open(): * Manipulator [3]: rightArm_trunk
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [0]: q30
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: AxialWaist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: FrontalWaist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: AxialNeck
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: FrontalNeck
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r10
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r11
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r12
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r13
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r14
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightFrontalWrist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r1Man
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightThumbProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightThumbDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r20
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r21
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r22
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r23
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r24
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftFrontalWrist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r2Man
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftThumbProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftThumbDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [1]: q31
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: FrontalWaist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: AxialNeck
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: FrontalNeck
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r10
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r11
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r12
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r13
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r14
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightFrontalWrist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r1Man
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightThumbProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightThumbDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r20
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r21
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r22
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r23
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r24
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftFrontalWrist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r2Man
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftThumbProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftThumbDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [2]: q10
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r10
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r11
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r12
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r13
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r14
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightFrontalWrist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r1Man
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightThumbProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightThumbDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [3]: q11
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r11
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r12
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r13
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r14
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightFrontalWrist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r1Man
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightThumbProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightThumbDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [4]: q12
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r12
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r13
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r14
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightFrontalWrist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r1Man
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightThumbProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightThumbDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [5]: q13
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r13
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r14
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightFrontalWrist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r1Man
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightThumbProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightThumbDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [6]: q14
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r14
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightFrontalWrist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r1Man
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightThumbProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightThumbDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [7]: RightFrontalWristJoint
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightFrontalWrist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r1Man
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightThumbProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightThumbDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:155 Open(): * Manipulator [4]: leftArm
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [0]: q20
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r20
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r21
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r22
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r23
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r24
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftFrontalWrist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r2Man
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftThumbProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftThumbDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [1]: q21
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r21
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r22
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r23
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r24
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftFrontalWrist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r2Man
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftThumbProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftThumbDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [2]: q22
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r22
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r23
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r24
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftFrontalWrist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r2Man
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftThumbProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftThumbDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [3]: q23
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r23
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r24
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftFrontalWrist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r2Man
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftThumbProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftThumbDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [4]: q24
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r24
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftFrontalWrist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r2Man
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftThumbProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftThumbDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [5]: LeftFrontalWristJoint
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftFrontalWrist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r2Man
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftThumbProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftThumbDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:155 Open(): * Manipulator [5]: leftArm_trunk
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [0]: q30
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: AxialWaist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: FrontalWaist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: AxialNeck
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: FrontalNeck
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r10
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r11
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r12
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r13
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r14
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightFrontalWrist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r1Man
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightThumbProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightThumbDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r20
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r21
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r22
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r23
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r24
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftFrontalWrist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r2Man
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftThumbProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftThumbDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [1]: q31
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: FrontalWaist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: AxialNeck
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: FrontalNeck
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r10
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r11
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r12
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r13
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r14
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightFrontalWrist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r1Man
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightThumbProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightThumbDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: RightInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r20
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r21
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r22
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r23
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r24
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftFrontalWrist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r2Man
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftThumbProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftThumbDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [2]: q20
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r20
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r21
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r22
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r23
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r24
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftFrontalWrist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r2Man
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftThumbProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftThumbDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [3]: q21
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r21
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r22
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r23
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r24
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftFrontalWrist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r2Man
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftThumbProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftThumbDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [4]: q22
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r22
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r23
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r24
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftFrontalWrist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r2Man
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftThumbProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftThumbDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [5]: q23
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r23
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r24
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftFrontalWrist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r2Man
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftThumbProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftThumbDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [6]: q24
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r24
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftFrontalWrist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r2Man
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftThumbProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftThumbDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [7]: LeftFrontalWristJoint
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftFrontalWrist
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r2Man
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftThumbProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftThumbDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftExternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftExternalFingerDistal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftInternalFingerProximal
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: LeftInternalFingerDistal
[debug] OpenraveCollisionStop.cpp:155 Open(): * Manipulator [6]: rightLeg
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [0]: q50
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r50
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r51
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r52
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r53
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r5dummy
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r54
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r55
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r5Man
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [1]: q51
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r51
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r52
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r53
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r5dummy
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r54
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r55
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r5Man
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [2]: q52
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r52
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r53
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r5dummy
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r54
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r55
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r5Man
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [3]: q53
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r53
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r5dummy
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r54
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r55
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r5Man
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [4]: q54
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r54
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r55
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r5Man
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [5]: q55
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r55
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r5Man
[debug] OpenraveCollisionStop.cpp:155 Open(): * Manipulator [7]: leftLeg
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [0]: q60
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r60
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r61
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r62
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r63
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r6dummy
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r64
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r65
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r6Man
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [1]: q61
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r61
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r62
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r63
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r6dummy
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r64
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r65
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r6Man
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [2]: q62
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r62
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r63
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r6dummy
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r64
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r65
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r6Man
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [3]: q63
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r63
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r6dummy
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r64
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r65
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r6Man
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [4]: q64
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r64
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r65
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r6Man
[debug] OpenraveCollisionStop.cpp:172 Open(): ** Joint [5]: q65
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r65
[debug] OpenraveCollisionStop.cpp:179 Open(): *** Affects: r6Man
jgvictores commented 6 years ago

A quick hack is to only look at Joint [0] of each manipulator, as it is the base, and all the rest are subsets.

jgvictores commented 6 years ago

Deleting old YarpOpenraveControlboardCollision. For the record, what was being done was:

OpenRAVE::EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment
if(penv->CheckSelfCollision(probot)) {  // Check if we collide.
CD_WARNING("Collision!!! Invalid position. Going back to the initial position\n");

Inspiring but broken anyway, as the current model self-collides a lot.

jgvictores commented 6 years ago

Created OpenraveDumpManipulatorJointLinks as part of the process, for debugging, merged into develop at 7dc7a65daf50f83c7636549dced20a19eda29169.

Will probably integrate --collision into YarpOpenraveControlboard (in line with the issue title) now that we have the info of the links of a manipulator.

jgvictores commented 6 years ago

Already have example prepared for when working: https://github.com/roboticslab-uc3m/openrave-yarp-plugins/blob/18ea4ffdcc4c88ee93ee6945821995f4e5926090/examples/python/openraveYarpPluginLoader-controlboard-collision.py#L17

jgvictores commented 6 years ago

Blocked issue #53 is now closed/invalid.

jgvictores commented 4 years ago

https://github.com/roboticslab-uc3m/openrave-yarp-plugins/commit/bf9e96200c662adbf65b0975b83147b93bf6c801 coops I did it again (ref: https://github.com/roboticslab-uc3m/openrave-yarp-plugins/commit/12ea290f82cbdb4691b669e9e7746e628c1bbb61#diff-187cc21333790ef079f4b46fe577256a where SetCheckCollisions was also present)