Connect OpenRAVE grasping module via YARP somehow

[rsantos88]

From @rsantos88 on November 23, 2017 15:48

After doing totally functional the simulation of the teo-grasping example in OpenRave, I am thinking to create a new YarpPluging, passing the python code to C++ and adapting it.
The purpose is to create a Yarp port which sends the information of the trajectory calculated by OpenRave to the robot.
@jgvictores do you think it's ok?

Copied from original issue: roboticslab-uc3m/openrave-yarp-plugins#40

[rsantos88]

From @jgvictores on November 24, 2017 10:18

Sounds good. A couple of comments:

1. The Python layer sometimes has high-level functions that would take ages to migrate to C++. If that is the case of the specific functions you're using, I'm okay with having a Python component as part of our architecture. Conclusion: Before anything, first study the viability of this python -> c++ migration.
2. Currently, we are usually starting with a C++ OpenRAVE plugin. From there, we can open a YARP Port or connect to a YARP plugin if existing interfaces exist or we consider it's worth it to develop a new one. We can study this later on.

[rsantos88]

Some important information about thats points:

• add C++ API for grasping rdiankov/openrave#184
• http://openrave.programmingvision.com/wiki/index.php/GraspModuleIK

[rsantos88]

From @jgvictores on November 29, 2017 21:4

Ran the example you mention, here's some relevant output:

grasp 111/112
found good grasp
grasping finished in 1292.753110s

('validgrasps is ', 14, 'validindicees is ', 14)
trying to grasp de object (attempt: 0)
moving robot arm to grasp
[basemanipulation.cpp:815 _MoveToHandPosition] failed to find goal
try again:  Planning Error
MoveToHandPosition
trying to grasp de object (attempt: 1)
moving robot arm to grasp
[basemanipulation.cpp:815 _MoveToHandPosition] failed to find goal
try again:  Planning Error
MoveToHandPosition
trying to grasp de object (attempt: 2)
moving robot arm to grasp
[basemanipulation.cpp:815 _MoveToHandPosition] failed to find goal
try again:  Planning Error
MoveToHandPosition
trying to grasp de object (attempt: 3)
moving robot arm to grasp
[basemanipulation.cpp:815 _MoveToHandPosition] failed to find goal
try again:  Planning Error
MoveToHandPosition
trying to grasp de object (attempt: 4)
moving robot arm to grasp
[basemanipulation.cpp:815 _MoveToHandPosition] failed to find goal
try again:  Planning Error
MoveToHandPosition
trying to grasp de object (attempt: 5)
moving robot arm to grasp
[basemanipulation.cpp:815 _MoveToHandPosition] failed to find goal
try again:  Planning Error
MoveToHandPosition
trying to grasp de object (attempt: 6)
moving robot arm to grasp
traj has 45 waypoints
WayPoint 0: array([ 0.,  0.,  0.,  0.,  0.,  0.,  0.,  0.,  0.,  0.,  0.,  0.,  0.,
        0.,  0.,  0.,  0.,  1.])
WayPoint 1: array([ -3.56733843e-04,   2.16227067e-03,   1.74126225e-03,
         4.52014247e-04,  -2.39173249e-03,   4.07028902e-03,
         3.55207193e-03,   2.00948496e-03,  -5.59154324e-02,
         3.38920184e-01,   2.72930179e-01,   7.08499421e-02,
        -3.74886653e-01,   6.37988168e-01,   5.56761412e-01,
         3.14972136e-01,   1.27597634e-02,   1.00000000e+00])
WayPoint 2: array([ 0.01615828,  0.01965283,  0.01956112, -0.0162815 , -0.03213552,
        0.04268585, -0.05661849,  0.08061498,  0.59349215,  0.23041124,
        0.30712024, -0.61553903, -0.59329713,  0.61897871, -2.51536068,
        2.2436988 ,  0.06144244,  1.        ])
WayPoint 3: array([ 0.02240737,  0.02180319,  0.02256411, -0.02277256, -0.03805321,
        0.04897889, -0.0833481 ,  0.10383726,  0.69593874,  0.21329353,
        0.31251385, -0.72381959, -0.62775224,  0.67952162, -3.        ,
        2.547963  ,  0.00969279,  1.        ])
WayPoint 4: array([ 0.03352498,  0.02469158,  0.0271221 , -0.03435407, -0.0474616 ,
        0.05941184, -0.12654902,  0.14378344,  0.84814085,  0.18786221,
        0.320527  , -0.88468905, -0.67894125,  0.76946856, -3.        ,
        3.        ,  0.01440031,  1.        ])
WayPoint 5: array([ 0.13373999,  0.0340284 ,  0.05422999, -0.13934608, -0.11232169,
        0.13986377, -0.36395912,  0.38119354,  1.6845655 ,  0.04810474,
        0.36456315, -1.76874503, -0.9602498 ,  1.26377078, -3.        ,
        3.        ,  0.0791367 ,  1.        ])
WayPoint 6: array([  1.33976062e-01,   3.40351280e-02,   5.42811073e-02,
        -1.39594174e-01,  -1.12456353e-01,   1.40041017e-01,
        -3.64379736e-01,   3.81614162e-01,   1.68293289e+00,
         4.78571373e-02,   3.64641166e-01,  -1.77031130e+00,
        -9.60748197e-01,   1.26464653e+00,  -3.00000000e+00,
         3.00000000e+00,   1.40205707e-04,   1.00000000e+00])
WayPoint 7: array([ 0.14145019,  0.03423307,  0.05591999, -0.14769471, -0.11682699,
        0.1458101 , -0.37791439,  0.39514881,  1.63039865,  0.03988962,
        0.36188452, -1.82071097, -0.97678548,  1.29282649, -3.        ,
        3.        ,  0.00451155,  1.        ])
WayPoint 8: array([ 0.16725723,  0.03515199,  0.06192802, -0.17994258, -0.13378169,
        0.16846912, -0.42843901,  0.44567343,  1.43428922,  0.06923595,
        0.35159401, -2.00885205, -1.03665239,  1.39802181, -3.        ,
        3.        ,  0.01684154,  1.        ])
WayPoint 9: array([ 0.24044773,  0.04468757,  0.08571277, -0.30569946, -0.21786039,
        0.28563973, -0.64496317,  0.6621976 ,  0.59385879,  0.19500015,
        0.30749385, -1.47593799, -1.29321308,  1.84883825, -3.        ,
        3.        ,  0.07217472,  1.        ])
WayPoint 10: array([ 0.24701886,  0.04728912,  0.0895473 , -0.32374988, -0.23416539,
        0.30948627, -0.68284933,  0.70008376,  0.44680508,  0.21700565,
        0.29977746, -1.38269173, -1.28899643,  1.92771953, -3.        ,
        3.        ,  0.01262872,  1.        ])
WayPoint 11: array([ 0.25318207,  0.05148496,  0.09485297, -0.34747966, -0.25735172,
        0.3452583 , -0.73693932,  0.75108537,  0.23685682,  0.24842286,
        0.28876078, -1.24956426, -1.28297634,  2.04033816, -3.        ,
        2.65741776,  0.01803   ,  1.        ])
WayPoint 12: array([ 0.25096871,  0.0655886 ,  0.1081437 , -0.39941479, -0.31920522,
        0.45160142, -0.82364298,  0.85765289, -0.32809683,  0.33296399,
        0.25911579, -0.89132908, -1.26677676,  2.34338572, -0.57413756,
        1.7355569 ,  0.04851725,  1.        ])
WayPoint 13: array([ 0.24605641,  0.07057378,  0.11202321, -0.41275986, -0.33817154,
        0.48668685, -0.83223903,  0.88363784, -0.42002923,  0.42626016,
        0.33171977, -1.14107857, -1.62172631,  3.        , -0.73501031,
        2.22185816,  0.01313229,  0.        ])
WayPoint 14: array([ 0.22388081,  0.09307835,  0.12953648, -0.47300354, -0.4237912 ,
        0.64507299, -0.87104418,  1.00094169, -0.42002923,  0.42626016,
        0.33171977, -1.14107857, -1.62172631,  3.        , -0.73501031,
        2.22185816,  0.05279538,  0.        ])
WayPoint 15: array([  2.22837478e-01,   9.41371542e-02,   1.30360455e-01,
        -4.75837921e-01,  -4.27819483e-01,   6.52524831e-01,
        -8.72869910e-01,   1.00646067e+00,  -4.02264727e-01,
         4.08232135e-01,   3.17690187e-01,  -1.09281838e+00,
        -1.55313785e+00,   2.87311954e+00,  -7.03924166e-01,
         2.12788804e+00,   2.53760911e-03,   1.00000000e+00])
WayPoint 16: array([ 0.20635879,  0.11086413,  0.14254674, -0.50234958, -0.47604062,
        0.74291732, -0.87085001,  1.06020544, -0.66841386,  0.6785785 ,
        0.47409575, -0.62973748, -1.57995963,  3.        ,  0.83516408,
        1.36409986,  0.03078176,  1.        ])
WayPoint 17: array([ 0.19415029,  0.12325894,  0.15106283, -0.51069364, -0.50223384,
        0.79242726, -0.85025806,  1.07933858, -0.81110684,  0.82352179,
        0.5579509 , -0.38146166, -1.59433983,  3.        ,  1.66032963,
        0.95460307,  0.01650331,  1.        ])
WayPoint 18: array([ 0.16931444,  0.14847638,  0.16783607, -0.51551436, -0.54148145,
        0.87280748, -0.787825  ,  1.09600919, -1.04277134,  1.05883969,
        0.69409117,  0.02161839, -1.33530701,  3.        ,  3.        ,
        0.28977805,  0.02679341,  1.        ])
WayPoint 19: array([ 0.13777186,  0.18050616,  0.18858242, -0.50936775, -0.57420934,
        0.95436228, -0.7062702 ,  1.09471812, -1.2778211 ,  1.29759625,
        0.83222083,  0.43058857, -1.07248899,  3.        ,  3.        ,
       -0.38476195,  0.02718493,  1.        ])
WayPoint 20: array([  1.37329827e-01,   1.80955037e-01,   1.88869651e-01,
        -5.09218071e-01,  -5.74579337e-01,   9.55398857e-01,
        -7.05233624e-01,   1.09458370e+00,  -1.28080862e+00,
         1.30063089e+00,   8.30385876e-01,   4.35786644e-01,
        -1.06914853e+00,   3.00000000e+00,   3.00000000e+00,
        -3.93335459e-01,   3.45524909e-04,   1.00000000e+00])
WayPoint 21: array([ 0.12697917,  0.19208358,  0.19559655, -0.50507028, -0.58314242,
        0.98036609, -0.68026639,  1.09045089, -1.20660831,  1.37372395,
        0.78618861,  0.560989  , -0.98868928,  3.        ,  3.        ,
       -0.59983951,  0.00832241,  1.        ])
WayPoint 22: array([ 0.11180067,  0.20968015,  0.20553   , -0.49633505, -0.59537301,
        1.02004327, -0.64058921,  1.08034743, -1.08869139,  1.28723806,
        0.71595163,  0.75995683, -0.86082584,  3.        ,  3.        ,
       -0.92800954,  0.01322573,  1.        ])
WayPoint 23: array([ 0.08979637,  0.23668695,  0.22013721, -0.4757171 , -0.61212449,
        1.07958546, -0.57388018,  1.05357744, -0.89043829,  1.14182979,
        0.59786257,  1.09448042, -0.64584971,  2.35539404,  3.        ,
       -1.47976013,  0.02223634,  1.        ])
WayPoint 24: array([ 0.08266496,  0.24600267,  0.22494902, -0.46604318, -0.61718519,
        1.0982607 , -0.54880423,  1.04135329, -0.81591486,  1.08717075,
        0.55347284,  1.22022798, -0.56504009,  2.11308638,  3.        ,
       -1.44514788,  0.00835865,  1.        ])
WayPoint 25: array([ 0.05263498,  0.29297172,  0.24628   , -0.4059257 , -0.63343192,
        1.16835133, -0.39569438,  0.9729908 , -0.36088649,  0.75343126,
        0.28243612,  1.13562864, -0.07162921,  0.6335938 ,  3.        ,
       -1.23381093,  0.05103662,  1.        ])
WayPoint 26: array([-0.02360826,  0.45214653,  0.3059493 , -0.16600537, -0.64856477,
        1.30220849,  0.23810523,  0.71232784, -0.36088649,  0.75343126,
        0.28243612,  1.13562864, -0.07162921,  0.6335938 ,  3.        ,
       -1.23381093,  0.21126653,  1.        ])
WayPoint 27: array([-0.03390728,  0.48078178,  0.31559285, -0.12466517, -0.65117099,
        1.32384376,  0.35061478,  0.67202572, -0.18834802,  0.77365239,
        0.23184318,  1.0689951 , -0.06735709,  0.52018885,  3.        ,
       -0.91545285,  0.03750318,  1.        ])
WayPoint 28: array([-0.03746441,  0.50388004,  0.32185518, -0.09385025, -0.65441759,
        1.33789589,  0.43927021,  0.64867912, -0.05239093,  0.78958626,
        0.19197688,  1.01648911, -0.15236536,  0.43082786,  3.        ,
       -0.66459262,  0.02955181,  1.        ])
WayPoint 29: array([-0.03452168,  0.54315488,  0.34031437, -0.0462461 , -0.66531449,
        1.35535474,  0.5860416 ,  0.62632389,  0.17268962,  0.8159652 ,
        0.56263314,  0.92956407, -0.2930988 ,  0.28288839,  3.        ,
       -0.2492869 ,  0.0489238 ,  1.        ])
WayPoint 30: array([-0.03398725,  0.54578416,  0.34216466, -0.04326315, -0.66627284,
        1.35624966,  0.59569822,  0.62556544,  0.15937205,  0.81770077,
        0.58701993,  0.92384496, -0.30235816,  0.27315493,  3.        ,
       -0.22196246,  0.00321887,  1.        ])
WayPoint 31: array([-0.03092896,  0.57509228,  0.36836896, -0.01099401, -0.67909832,
        1.36415468,  0.70426372,  0.62309147,  0.00964784,  0.80204649,
        0.8611911 ,  0.85954728, -0.40645744,  0.16372541,  3.        ,
        0.08523552,  0.0361885 ,  1.        ])
WayPoint 32: array([-0.03360149,  0.6055331 ,  0.40696753,  0.02066353, -0.69680158,
        1.37089239,  0.81931541,  0.63260284, -0.14902154,  0.78545696,
        1.15174245,  0.79140816, -0.51677607,  0.1876495 ,  3.        ,
        0.41078685,  0.03835056,  1.        ])
WayPoint 33: array([-0.03419716,  0.6085124 ,  0.41139536,  0.02365574, -0.69878455,
        1.37160941,  0.83070656,  0.63416092, -0.16473123,  0.78381444,
        1.18050963,  0.78466178, -0.5276986 ,  0.1900182 ,  3.        ,
        0.409895  ,  0.00379705,  1.        ])
WayPoint 34: array([-0.04055229,  0.63062084,  0.44801797,  0.04597794, -0.71494907,
        1.37726357,  0.91599427,  0.64571901, -0.28235271,  0.77151664,
        1.39589509,  0.78570804, -0.6094777 ,  0.20775311,  3.        ,
        0.40321756,  0.02842924,  1.        ])
WayPoint 35: array([-0.05184548,  0.65793428,  0.49656028,  0.07356702, -0.73735157,
        1.38786751,  1.00530577,  0.65960234, -0.47633109,  1.0634169 ,
        1.86520604,  1.06774256, -0.89553552,  0.50462539,  3.        ,
        0.52947316,  0.0297705 ,  1.        ])
WayPoint 36: array([-0.05409305,  0.66290239,  0.50498629,  0.07855145, -0.74154951,
        1.39028071,  1.01903175,  0.66206925, -0.50614295,  1.10827804,
        1.81803281,  1.11108747, -0.93949876,  0.55025066,  3.        ,
        0.54887695,  0.00457533,  1.        ])
WayPoint 37: array([-0.06982746,  0.69578259,  0.54962074,  0.10628377, -0.7698795 ,
        1.40840429,  1.09868093,  0.67813648, -0.67913543,  1.36859855,
        1.54429567,  0.97799784, -1.1946089 ,  0.81500529,  3.        ,
        0.66147336,  0.02654973,  1.        ])
WayPoint 38: array([-0.09453605,  0.73835201,  0.59326544,  0.13468079, -0.81241765,
        1.43913054,  1.19346299,  0.70115171, -0.88499549,  1.3261785 ,
        1.21855004,  0.81962193, -1.49818851,  1.13006177,  3.        ,
        0.79546244,  0.03159402,  1.        ])
WayPoint 39: array([-0.09962307,  0.74595438,  0.60010082,  0.13931017, -0.82118995,
        1.44579238,  1.21071082,  0.70579513, -0.88462542,  1.31845918,
        1.15927296,  0.79080171, -1.55343196,  1.18739371,  3.        ,
        0.81984491,  0.00574928,  1.        ])
WayPoint 40: array([ -1.00017038e-01,   7.46541432e-01,   6.00616088e-01,
         1.39661864e-01,  -8.21882731e-01,   1.44632218e+00,
         1.21204192e+00,   7.06160676e-01,  -8.84596751e-01,
         1.31786122e+00,   1.15468121e+00,   7.88569216e-01,
        -1.55771126e+00,   1.19183479e+00,   2.97773231e+00,
         8.21733638e-01,   4.45353717e-04,   1.00000000e+00])
WayPoint 41: array([-0.10859551,  0.75926286,  0.61133253,  0.14707592, -0.83699259,
        1.45835344,  1.23857624,  0.7143319 , -0.8839723 ,  1.30483603,
        1.05466015,  0.73993947, -1.55739338,  1.28857372,  2.49268088,
        0.86287534,  0.00970103,  1.        ])
WayPoint 42: array([-0.13063012,  0.79178826,  0.63762184,  0.16552025, -0.87581344,
        1.49047347,  1.30071082,  0.73584063,  0.        ,  0.        ,
        0.        ,  0.        ,  0.        ,  0.        ,  0.        ,
        0.        ,  0.04985362,  1.        ])
WayPoint 43: array([-0.13263823,  0.80396004,  0.64742369,  0.16806471, -0.88927689,
        1.5133858 ,  1.32070602,  0.74715235, -0.13266418,  0.80411733,
        0.64755036,  0.16809759, -0.88945088,  1.51368189,  1.32096441,
        0.74729853,  0.03027364,  0.        ])
WayPoint 44: array([-0.13464634,  0.81613182,  0.65722555,  0.17060917, -0.90274035,
        1.53629813,  1.34070122,  0.75846407,  0.        ,  0.        ,
        0.        ,  0.        ,  0.        ,  0.        ,  0.        ,
        0.        ,  0.03027364,  1.        ])
press any key to releaseinitial values
trying to grasp de object (attempt: 7)
moving robot arm to grasp
[basemanipulation.cpp:815 _MoveToHandPosition] failed to find goal
try again:  Planning Error
MoveToHandPosition
trying to grasp de object (attempt: 8)
moving robot arm to grasp
traj has 24 waypoints

[rsantos88]

From @jgvictores on November 29, 2017 21:41

As spoken this evening, the target is a Cartesian-space pose specified by a 4x4 numpy.array in homogeneous transformation matrix, that is called Tgrasp in this example, but also called Tgoal elsewhere, entered as the matrices parameter.

Regarding the output waypoints, I can make some pretty certain assumptions given my experience displayed in #29. 😂

Let's look at the first waypoint:

WayPoint 0: array([ 0.,  0.,  0.,  0.,  0.,  0.,  0.,  0.,  0.,  0.,  0.,  0.,  0., 0.,  0.,  0.,  0.,  1.])

There are 18 numbers here. Waypoints in trajectories usually use the last field as a plain 1 to express iswaypoint. Waypoints in trajectories usually use the second-last field to express a deltatime (relative increase of timestamp). There are 16 numbers left to describe. While 16 has several different factors, I'd bet for 8x2. This is, two groups describing 8 DoF, which makes sense because TEO's 2 DoF trunk and TEO's 6 DoF arm.

In conclusion, and based on this set of comments from #29, for each waypoint my assumption is:

• Fields 0-7: joint_values of trunk and arm.
• Fields 8-15: joint_velocities of trunk and arm.
• Field 16: deltatime, the relative increase of timestamp.
• Field 17: iswaypoint, always 1.

Note 1: My count started at a c-style 0, so there is no element 18.
Note 2: These assumptions could be contrasted via the serialization techniques of 702743801ebab57c131798e416f3d25f011bedfd (if available in Python, kind of doubt it), or via extracting the ConfigurationSpecification (probably possible, kind of not worth the effort). I'd in fact just assume true.

Corollary: These values are in joint space. This is good, they can be sent directly to the robot in more or less sophisticated ways. A crude implementation would be solely via positionMove calls. A better implementation would previously adjust velocities via setRefSpeed. A very sophisticated implementation would involve respecting deltatimes rigorously as what is attempted with PlaybackThread from the tools repository (but that's in c++ lol).

[rsantos88]

Thanks @jgvictores. This this can be a very important advance and very useful information.
I think it can better to put this issue directly on https://github.com/roboticslab-uc3m/teo-grasp/issues
We decided that at this moment it's not necessary to use a openrave-yarp-plugin

[rsantos88]

As @AlvaroMartinezR noted, the values are in radians.

[rsantos88]

We are progressing in this fields. I've done a bit example with the leftArm_trunk manipulator.
Now, the python example can show the value of each joint per wayponit.
In the first grasp, we can see 48 wayponits. The joint values of the last wayponit are:

WayPoint: 48
Joint number 0 value: -40.388129702571327
Joint number 1 value: 2.9747920777798509
Joint number 2 value: -26.875009609536988
Joint number 3 value: 0.037173734430287882
Joint number 4 value: -8.9038939102282786
Joint number 5 value: -66.351233738028654
Joint number 6 value: -86.438450209396066
Joint number 7 value: -57.777798437040772

The first two joints are from trunk and the rest for the left arm.
This is the result of the grasping simulation pose and the same values in the real robot:

[jgvictores]

Related: roboticslab-uc3m/questions-and-answers#40

[jgvictores]

In fact also blocked by roboticslab-uc3m/kinematics-dynamics#134, where we're developing org:roboticslab-uc3m trajectory classes with Python in mind. 😄

But no worries: It's WIP and on the way to be shipped ASAP!