How to make our own path before grasping the object

[rsantos88]

Openrave is able to do operations of grasping, calling to high level functions. For example, it can calculate the trajectory avoiding the obstacles, But, for us, it's interesting to tell openrave what type of path we want to do before the manipulator grasps the object. Maybe we prefer to do a different path, going through other points before reaching the object

[jgvictores]

While there could be many ad-hoc short-term hacks, the goal of roboticslab-uc3m/kinematics-dynamics#134 is to develop our own unified Cartesian-space trajectory class(es), which will serve to interface KDL, openrave, and all other dependencies we may have in the future.
Please keep in close contact with that issue, and also highly-related roboticslab-uc3m/questions-and-answers#40 which should also log joint-space trajectory discussions.

[jgvictores]

WIP at roboticslab-uc3m/kinematics-dynamics#134 (comment)

[rsantos88]

While there could be many ad-hoc short-term hacks, the goal of roboticslab-uc3m/kinematics-dynamics#134 is to develop our own unified Cartesian-space trajectory class(es), which will serve to interface KDL, openrave, and all other dependencies we may have in the future.

So in that case, I'll put this issue pending of roboticslab-uc3m/kinematics-dynamics#134

[rsantos88]

I've been thinking in my "free time", how to do this task.
If we want to move our TCP over the space, making our own path and passing the TCP through the desired points, we need to make our own planner. For this reason, the first thing is to understand the concepts of interpolation of two points or the use of splines in python.
Here, I've programed two examples in python of interpolation:

• In this first program you have a simple equation y = 2*x²+1 and the program will interpolate to get 10 equidistant points and its values (x, y). Example: ejemplo-scipy-interpolar.py

Preview:

(x, y) = (1.0, 3.0)
(x, y) = (2.0, 9.0)
(x, y) = (3.0, 19.0)
(x, y) = (4.0, 33.0)
(x, y) = (5.0, 51.0)
(x, y) = (6.0, 73.0)
(x, y) = (7.0, 99.0)
(x, y) = (8.0, 129.0)
(x, y) = (9.0, 163.0)
(x, y) = (10.0, 201.0)

• The second example, and more interesting for a planner allow us to give different points to the program, the resolution of the interpolation (number of points that we want to get) and the degree of the spline to get a smooth function. The program will connect all the points and return us the intermediate points, making a smoother curve depending of the spline degree. Example: ejemplo-trazadores.py

Preview:

Trazador bidimensional con interpolación 1.0
--------------------------------------------
Elija los puntos por los que desea pasar: 
5
indique las coordenadas (x,y) del pto 0
x: 
1
y: 
2
indique las coordenadas (x,y) del pto 1
x: 
4 
y: 
8
indique las coordenadas (x,y) del pto 2
x: 
7
y: 
2
indique las coordenadas (x,y) del pto 3
x: 
10
y: 
15
indique las coordenadas (x,y) del pto 4
x: 
13
y: 
4
-------------------------------------------
Indique la resolución de la interpolación: 
20
Indique el grado del trazador (Ej: 1-lineal, 2-cuadrado, 3-cúbico.... 5)
2
Pto 0: (x, y) = (1.0, 2.0)
Pto 1: (x, y) = (1.63157894737, 4.80411555204)
Pto 2: (x, y) = (2.26315789474, 6.78638702018)
Pto 3: (x, y) = (2.89473684211, 7.94681440443)
Pto 4: (x, y) = (3.52631578947, 8.28539770479)
Pto 5: (x, y) = (4.15789473684, 7.80213692125)
Pto 6: (x, y) = (4.78947368421, 6.49703205382)
Pto 7: (x, y) = (5.42105263158, 4.37008310249)
Pto 8: (x, y) = (6.05263157895, 2.30937870993)
Pto 9: (x, y) = (6.68421052632, 1.72861100119)
Pto 10: (x, y) = (7.31578947368, 2.64590423427)
Pto 11: (x, y) = (7.94736842105, 5.06125840918)
Pto 12: (x, y) = (8.57894736842, 8.95180055402)
Pto 13: (x, y) = (9.21052631579, 12.5334388603)
Pto 14: (x, y) = (9.84210526316, 14.6853977048)
Pto 15: (x, y) = (10.4736842105, 15.4076770875)
Pto 16: (x, y) = (11.1052631579, 14.7002770083)
Pto 17: (x, y) = (11.7368421053, 12.5631974674)
Pto 18: (x, y) = (12.3684210526, 8.99643846458)
Pto 19: (x, y) = (13.0, 4.0)

[jgvictores]

Very cool! Lots of thing can be done with this!

Just to be very purist with technical terms, I'd say that this is a: 2D Cartesian space path generator.

Adding velocity profiles (which can be done separately, e.g. via things we are preparing in roboticslab-uc3m/kinematics-dynamics#134) would make it a trajectory generator.
I wouldn't call it a planner in the current state, because planning usually involves some kind of optimization, algorithm for decision taking, or some kind of problem solving based on additional constraints.

PS: Visualization always helps! :-)
...despite our empty dedicated repo. ^^

[rsantos88]

Continuing with this work and with the last examples, I've been doing other python application named 3D Cartesian space path generator.
Basically it's the same as the other example but now it returns us the 3D points generated from the path obtained from the points we have given to the application.

E.g: We want to make a path using only 6 points in cartesian coordinates:
[1, 2, 3], [2, 1, 2], [4, 2, 6], [6, 3, 4], [3, 7, 0], [8, 9, 4]

We'll use a cubic spline (degree two) and the resolution of the path will be 100 points.
You can see the values in the end:

Preview:

--------------------------------------------
3D Cartesian space path generator 1.0
--------------------------------------------
indicate the total number of points which you want to pass through: 
6
indicate coordinates (x,y,z) of the point: 1
x: 
1
y: 
2
z: 
3
indicate coordinates (x,y,z) of the point: 2
x: 
2
y: 
1
z: 
2
indicate coordinates (x,y,z) of the point: 3
x: 
4
y: 
2
z: 
6
indicate coordinates (x,y,z) of the point: 4
x: 
6
y: 
3
z: 
4
indicate coordinates (x,y,z) of the point: 5
x: 
3
y: 
7
z: 
0
indicate coordinates (x,y,z) of the point: 6
x: 
8
y: 
9
z: 
4
----------
resulting matrix:
[[ 1.  2.  3.]
 [ 2.  1.  2.]
 [ 4.  2.  6.]
 [ 6.  3.  4.]
 [ 3.  7.  0.]
 [ 8.  9.  4.]]
----------
transposed matrix:
[[ 1.  2.  4.  6.  3.  8.]
 [ 2.  1.  2.  3.  7.  9.]
 [ 3.  2.  6.  4.  0.  4.]]
-------------------------------------------
Indicate the degree of the spline (1 <= k <= 5) default is 3
2
-------------------------------------------
Indicate number of interpolation points (resolution) : 
100
-------------------------------------------
points resulting from the trajectory: 
(x, y, z) = (0.993778867108, 1.8767115432, 2.47144663373)
(x, y, z) = (1.11681767363, 1.75835675114, 2.50908137715)
(x, y, z) = (1.24024155777, 1.64942897717, 2.55456259915)
(x, y, z) = (1.36405051955, 1.54992822131, 2.60789029974)
(x, y, z) = (1.48824455895, 1.45985448354, 2.66906447893)
(x, y, z) = (1.61282367598, 1.37920776388, 2.7380851367)
(x, y, z) = (1.73778787064, 1.30798806231, 2.81495227307)
(x, y, z) = (1.86313714292, 1.24619537885, 2.89966588802)
(x, y, z) = (1.98887149284, 1.19382971348, 2.99222598156)
(x, y, z) = (2.11499092038, 1.15089106622, 3.0926325537)
(x, y, z) = (2.24149542555, 1.11737943705, 3.20088560442)
(x, y, z) = (2.36838500835, 1.09329482599, 3.31698513374)
(x, y, z) = (2.49565966878, 1.07863723302, 3.44093114164)
(x, y, z) = (2.62331940683, 1.07340665815, 3.57272362813)
(x, y, z) = (2.75136422251, 1.07760310139, 3.71236259322)
(x, y, z) = (2.87979411582, 1.09122656272, 3.85984803689)
(x, y, z) = (3.00860908676, 1.11427704215, 4.01517995916)
(x, y, z) = (3.13780913533, 1.14675453968, 4.17835836001)
(x, y, z) = (3.26731909243, 1.18848946025, 4.34870919221)
(x, y, z) = (3.39534238805, 1.23542841726, 4.51012247)
(x, y, z) = (3.52111574392, 1.28584931699, 4.6557538166)
(x, y, z) = (3.64463916004, 1.33975215946, 4.78560323199)
(x, y, z) = (3.76591263641, 1.39713694465, 4.89967071618)
(x, y, z) = (3.88493617304, 1.45800367256, 4.99795626918)
(x, y, z) = (4.00170976991, 1.52235234321, 5.08045989097)
(x, y, z) = (4.11623342703, 1.59018295658, 5.14718158156)
(x, y, z) = (4.22850714441, 1.66149551268, 5.19812134096)
(x, y, z) = (4.33853092203, 1.73629001151, 5.23327916915)
(x, y, z) = (4.44630475991, 1.81456645306, 5.25265506615)
(x, y, z) = (4.55182865804, 1.89632483735, 5.25624903194)
(x, y, z) = (4.65510261641, 1.98156516436, 5.24406106654)
(x, y, z) = (4.75612663504, 2.0702874341, 5.21609116993)
(x, y, z) = (4.85490071392, 2.16249164656, 5.17233934213)
(x, y, z) = (4.95142485305, 2.25817780176, 5.11280558313)
(x, y, z) = (5.04569905243, 2.35734589968, 5.03748989292)
(x, y, z) = (5.13648500601, 2.45986775243, 4.94766238179)
(x, y, z) = (5.21641535707, 2.56498070046, 4.85087962701)
(x, y, z) = (5.28427256756, 2.67255870575, 4.74839043747)
(x, y, z) = (5.34005663748, 2.78260176829, 4.64019481317)
(x, y, z) = (5.38376756682, 2.89510988808, 4.5262927541)
(x, y, z) = (5.41540535559, 3.01008306514, 4.40668426027)
(x, y, z) = (5.43497000379, 3.12752129945, 4.28136933168)
(x, y, z) = (5.44246151141, 3.24742459102, 4.15034796832)
(x, y, z) = (5.43787987845, 3.36979293984, 4.01362017021)
(x, y, z) = (5.42122510492, 3.49462634592, 3.87118593733)
(x, y, z) = (5.39249719082, 3.62192480926, 3.72304526968)
(x, y, z) = (5.35169613614, 3.75168832985, 3.56919816728)
(x, y, z) = (5.29882194089, 3.8839169077, 3.40964463011)
(x, y, z) = (5.23387460507, 4.0186105428, 3.24438465817)
(x, y, z) = (5.15685412867, 4.15576923517, 3.07341825148)
(x, y, z) = (5.06776051169, 4.29539298478, 2.89674541002)
(x, y, z) = (4.96659375415, 4.43748179166, 2.7143661338)
(x, y, z) = (4.85335385602, 4.58203565579, 2.52628042282)
(x, y, z) = (4.72804081733, 4.72905457718, 2.33248827707)
(x, y, z) = (4.59065463806, 4.87853855582, 2.13298969656)
(x, y, z) = (4.44119531821, 5.03048759172, 1.92778468129)
(x, y, z) = (4.28608394724, 5.18340222631, 1.72174599417)
(x, y, z) = (4.14118052166, 5.33357881895, 1.52690928796)
(x, y, z) = (4.00722208893, 5.48084525368, 1.3438338848)
(x, y, z) = (3.88420864904, 5.62520153048, 1.17251978468)
(x, y, z) = (3.772140202, 5.76664764937, 1.01296698761)
(x, y, z) = (3.6710167478, 5.90518361033, 0.865175493583)
(x, y, z) = (3.58083828644, 6.04080941337, 0.729145302604)
(x, y, z) = (3.50160481792, 6.17352505849, 0.604876414671)
(x, y, z) = (3.43331634225, 6.30333054569, 0.492368829784)
(x, y, z) = (3.37597285943, 6.43022587497, 0.391622547943)
(x, y, z) = (3.32957436944, 6.55421104633, 0.302637569149)
(x, y, z) = (3.2941208723, 6.67528605977, 0.2254138934)
(x, y, z) = (3.269612368, 6.79345091529, 0.159951520697)
(x, y, z) = (3.25604885655, 6.90870561289, 0.106250451041)
(x, y, z) = (3.25343033794, 7.02105015257, 0.0643106844302)
(x, y, z) = (3.26175681217, 7.13048453432, 0.0341322208658)
(x, y, z) = (3.28102827925, 7.23700875816, 0.0157150603475)
(x, y, z) = (3.31124473917, 7.34062282408, 0.00905920287518)
(x, y, z) = (3.35240619193, 7.44132673207, 0.014164648449)
(x, y, z) = (3.40451263754, 7.53912048214, 0.0310313970688)
(x, y, z) = (3.46756407599, 7.6340040743, 0.0596594487347)
(x, y, z) = (3.54156050728, 7.72597750853, 0.100048803447)
(x, y, z) = (3.62650193142, 7.81504078484, 0.152199461205)
(x, y, z) = (3.7223883484, 7.90119390323, 0.216111422009)
(x, y, z) = (3.82921975822, 7.9844368637, 0.291784685859)
(x, y, z) = (3.94699616089, 8.06476966625, 0.379219252755)
(x, y, z) = (4.0757175564, 8.14219231088, 0.478415122697)
(x, y, z) = (4.21538394475, 8.21670479759, 0.589372295686)
(x, y, z) = (4.36599532595, 8.28830712638, 0.71209077172)
(x, y, z) = (4.52755169999, 8.35699929725, 0.846570550801)
(x, y, z) = (4.70005306688, 8.42278131019, 0.992811632927)
(x, y, z) = (4.8834994266, 8.48565316522, 1.1508140181)
(x, y, z) = (5.07789077918, 8.54561486233, 1.32057770632)
(x, y, z) = (5.28322712459, 8.60266640151, 1.50210269758)
(x, y, z) = (5.49950846285, 8.65680778277, 1.69538899189)
(x, y, z) = (5.72673479395, 8.70803900612, 1.90043658925)
(x, y, z) = (5.9649061179, 8.75636007154, 2.11724548965)
(x, y, z) = (6.21402243468, 8.80177097904, 2.3458156931)
(x, y, z) = (6.47408374432, 8.84427172862, 2.5861471996)
(x, y, z) = (6.74509004679, 8.88386232029, 2.83824000914)
(x, y, z) = (7.02704134211, 8.92054275403, 3.10209412173)
(x, y, z) = (7.31993763027, 8.95431302985, 3.37770953736)
(x, y, z) = (7.62377891128, 8.98517314774, 3.66508625604)
(x, y, z) = (7.93856518513, 9.01312310772, 3.96422427776)
-------------------------------------------

And the graphic solution: