diff --git a/python/examples/__pycache__/robotiq_gripper.cpython-310.pyc b/python/examples/__pycache__/robotiq_gripper.cpython-310.pyc
deleted file mode 100644
index 122384842b716013f5bd36e61282ee1c6cc548bf..0000000000000000000000000000000000000000
Binary files a/python/examples/__pycache__/robotiq_gripper.cpython-310.pyc and /dev/null differ
diff --git a/python/examples/drawing_from_input_drawing.py b/python/examples/drawing_from_input_drawing.py
index e3213a2cf4e897c76c0fd8336d3dc6f8d3ede2ab..612c14539df1393e1c53bc8eb615a94079d2aa4f 100644
--- a/python/examples/drawing_from_input_drawing.py
+++ b/python/examples/drawing_from_input_drawing.py
@@ -253,8 +253,8 @@ if __name__ == "__main__":
assert args.mm_into_board > 0.0 and args.mm_into_board < 5.0
args.mm_into_board = args.mm_into_board / 1000
print(args)
- clikController = getClikController(args)
robot = RobotManager(args)
+ clikController = getClikController(args, robot)
if args.pinocchio_only:
rand_pertb = np.zeros(8)
rand_pertb[:6] = np.random.random(6) * 0.1
diff --git a/python/examples/joint_trajectory.csv b/python/examples/joint_trajectory.csv
index 5b8f74a4dc8a0755426e3f64379e226548f9d048..6e4bc966156f0327a12f9f3ddd821ffbba402ba1 100644
--- a/python/examples/joint_trajectory.csv
+++ b/python/examples/joint_trajectory.csv
@@ -1,183 +1,12 @@
-0.00000,0.93845,-1.46316,-1.40613,-1.05605,2.05935,-0.42868
-0.05495,0.93841,-1.46317,-1.40614,-1.05604,2.05938,-0.42872
-0.10989,0.93837,-1.46317,-1.40615,-1.05604,2.05940,-0.42875
-0.16484,0.93834,-1.46318,-1.40616,-1.05603,2.05942,-0.42877
-0.21978,0.93831,-1.46319,-1.40616,-1.05602,2.05944,-0.42879
-0.27473,0.93828,-1.46319,-1.40616,-1.05602,2.05946,-0.42881
-0.32967,0.93827,-1.46319,-1.40617,-1.05601,2.05946,-0.42882
-0.38462,0.93827,-1.46318,-1.40617,-1.05601,2.05946,-0.42882
-0.43956,0.93829,-1.46317,-1.40616,-1.05602,2.05943,-0.42879
-0.49451,0.93833,-1.46316,-1.40615,-1.05602,2.05941,-0.42877
-0.54945,0.94162,-1.46188,-1.40514,-1.05663,2.05679,-0.42599
-0.60440,0.94633,-1.46034,-1.40345,-1.05762,2.05314,-0.42210
-0.65934,0.96110,-1.45514,-1.40004,-1.06010,2.04208,-0.41023
-0.71429,0.96696,-1.45285,-1.39930,-1.06083,2.03771,-0.40558
-0.76923,0.97236,-1.45106,-1.39815,-1.06173,2.03372,-0.40134
-0.82418,0.99662,-1.44066,-1.39826,-1.06358,2.01597,-0.38260
-0.87912,1.00312,-1.43783,-1.39852,-1.06399,2.01120,-0.37764
-0.93407,1.01069,-1.43436,-1.39923,-1.06431,2.00565,-0.37190
-0.98901,1.03109,-1.42384,-1.40357,-1.06416,1.99070,-0.35659
-1.04396,1.03416,-1.42220,-1.40436,-1.06408,1.98844,-0.35430
-1.09890,1.03624,-1.42106,-1.40496,-1.06400,1.98691,-0.35275
-1.15385,1.03726,-1.42050,-1.40527,-1.06395,1.98616,-0.35200
-1.20879,1.04878,-1.41339,-1.41007,-1.06289,1.97767,-0.34347
-1.26374,1.04883,-1.41336,-1.41009,-1.06289,1.97764,-0.34343
-1.31868,1.05067,-1.41208,-1.41112,-1.06260,1.97628,-0.34208
-1.37363,1.05375,-1.40981,-1.41308,-1.06203,1.97400,-0.33981
-1.42857,1.05685,-1.40730,-1.41547,-1.06128,1.97171,-0.33753
-1.48352,1.05723,-1.40694,-1.41586,-1.06114,1.97143,-0.33725
-1.53846,1.05836,-1.40581,-1.41709,-1.06071,1.97059,-0.33641
-1.59341,1.05948,-1.40467,-1.41837,-1.06026,1.96977,-0.33559
-1.64835,1.06029,-1.40369,-1.41956,-1.05982,1.96916,-0.33499
-1.70330,1.07022,-1.38772,-1.44116,-1.05144,1.96178,-0.32768
-1.75824,1.07330,-1.38128,-1.45046,-1.04773,1.95949,-0.32541
-1.81319,1.07521,-1.37752,-1.45581,-1.04561,1.95806,-0.32400
-1.86813,1.07766,-1.37149,-1.46478,-1.04201,1.95622,-0.32219
-1.92308,1.08496,-1.34982,-1.49779,-1.02870,1.95075,-0.31680
-1.97802,1.08687,-1.34034,-1.51297,-1.02248,1.94931,-0.31536
-2.03297,1.08765,-1.33677,-1.51863,-1.02018,1.94871,-0.31478
-2.08791,1.08782,-1.33590,-1.52001,-1.01961,1.94859,-0.31465
-2.14286,1.08816,-1.33331,-1.52427,-1.01785,1.94832,-0.31439
-2.19780,1.08826,-1.33242,-1.52573,-1.01725,1.94825,-0.31431
-2.25275,1.08839,-1.32982,-1.53012,-1.01542,1.94815,-0.31420
-2.30769,1.08815,-1.32783,-1.53370,-1.01390,1.94831,-0.31435
-2.36264,1.08743,-1.32478,-1.53937,-1.01145,1.94884,-0.31485
-2.41758,1.07796,-1.31761,-1.55757,-1.00287,1.95581,-0.32158
-2.47253,1.07419,-1.31575,-1.56313,-1.00017,1.95859,-0.32429
-2.52747,1.07179,-1.31457,-1.56667,-0.99845,1.96036,-0.32601
-2.58242,1.05474,-1.31213,-1.58178,-0.99044,1.97291,-0.33836
-2.63736,1.04854,-1.31152,-1.58687,-0.98771,1.97746,-0.34289
-2.69231,1.04435,-1.31127,-1.59003,-0.98599,1.98053,-0.34596
-2.74725,1.03792,-1.31135,-1.59415,-0.98365,1.98523,-0.35068
-2.80220,1.02013,-1.31400,-1.60160,-0.97883,1.99823,-0.36389
-2.85714,1.01458,-1.31499,-1.60372,-0.97743,2.00227,-0.36804
-2.91209,1.00724,-1.31672,-1.60583,-0.97585,2.00761,-0.37356
-2.96703,0.99540,-1.32115,-1.60660,-0.97442,2.01619,-0.38253
-3.02198,0.99124,-1.32270,-1.60690,-0.97391,2.01920,-0.38569
-3.07692,0.98823,-1.32397,-1.60688,-0.97364,2.02137,-0.38799
-3.13187,0.98100,-1.32724,-1.60650,-0.97314,2.02660,-0.39353
-3.18681,0.97790,-1.32877,-1.60613,-0.97301,2.02883,-0.39592
-3.24176,0.97470,-1.33047,-1.60554,-0.97297,2.03113,-0.39838
-3.29670,0.97003,-1.33304,-1.60458,-0.97295,2.03449,-0.40200
-3.35165,0.96503,-1.33572,-1.60368,-0.97288,2.03808,-0.40588
-3.40659,0.96402,-1.33627,-1.60348,-0.97287,2.03881,-0.40667
-3.46154,0.95349,-1.34301,-1.59982,-0.97347,2.04635,-0.41490
-3.51648,0.95241,-1.34372,-1.59942,-0.97355,2.04712,-0.41574
-3.57143,0.94928,-1.34585,-1.59813,-0.97381,2.04936,-0.41821
-3.62637,0.94405,-1.34960,-1.59569,-0.97439,2.05309,-0.42233
-3.68132,0.93994,-1.35266,-1.59359,-0.97492,2.05602,-0.42559
-3.73626,0.93789,-1.35422,-1.59249,-0.97521,2.05748,-0.42722
-3.79121,0.93697,-1.35495,-1.59193,-0.97537,2.05813,-0.42795
-3.84615,0.92745,-1.36381,-1.58412,-0.97784,2.06489,-0.43555
-3.90110,0.92454,-1.36693,-1.58103,-0.97890,2.06696,-0.43789
-3.95604,0.91913,-1.37356,-1.57390,-0.98146,2.07079,-0.44227
-4.01099,0.90474,-1.39758,-1.54395,-0.99298,2.08097,-0.45402
-4.06593,0.89803,-1.40912,-1.52935,-0.99871,2.08570,-0.45954
-4.12088,0.87738,-1.45433,-1.46737,-1.02396,2.10019,-0.47682
-4.17582,0.87143,-1.46740,-1.44931,-1.03148,2.10435,-0.48185
-4.23077,0.86544,-1.47993,-1.43221,-1.03861,2.10852,-0.48694
-4.28571,0.84436,-1.52459,-1.37096,-1.06446,2.12310,-0.50505
-4.34066,0.84005,-1.53462,-1.35674,-1.07060,2.12607,-0.50880
-4.39560,0.83705,-1.54126,-1.34746,-1.07461,2.12812,-0.51142
-4.45055,0.83540,-1.54508,-1.34206,-1.07696,2.12926,-0.51287
-4.50549,0.83178,-1.55368,-1.32977,-1.08230,2.13174,-0.51604
-4.56044,0.82471,-1.56996,-1.30674,-1.09233,2.13657,-0.52227
-4.61538,0.82109,-1.57923,-1.29322,-1.09829,2.13904,-0.52548
-4.67033,0.81921,-1.58407,-1.28615,-1.10142,2.14033,-0.52715
-4.72527,0.81762,-1.58858,-1.27940,-1.10442,2.14141,-0.52858
-4.78022,0.81728,-1.58950,-1.27803,-1.10503,2.14164,-0.52888
-4.83516,0.81623,-1.59254,-1.27345,-1.10706,2.14236,-0.52982
-4.89011,0.81621,-1.59259,-1.27338,-1.10710,2.14237,-0.52983
-4.94505,0.81604,-1.59312,-1.27257,-1.10746,2.14248,-0.52999
-5.00000,0.81603,-1.59317,-1.27249,-1.10749,2.14249,-0.53000
-5.05495,0.81585,-1.59380,-1.27152,-1.10793,2.14262,-0.53017
-5.10989,0.81583,-1.59384,-1.27144,-1.10796,2.14263,-0.53018
-5.16484,0.81582,-1.59389,-1.27137,-1.10800,2.14263,-0.53019
-5.21978,0.81582,-1.59393,-1.27130,-1.10803,2.14264,-0.53019
-5.27473,0.81735,-1.59459,-1.26858,-1.10937,2.14161,-0.52887
-5.32967,0.81875,-1.59493,-1.26660,-1.11037,2.14067,-0.52766
-5.38462,0.83542,-1.58930,-1.26107,-1.11395,2.12940,-0.51327
-5.43956,0.84030,-1.58724,-1.26031,-1.11460,2.12608,-0.50910
-5.49451,0.84382,-1.58530,-1.26059,-1.11469,2.12368,-0.50609
-5.54945,0.84977,-1.58167,-1.26178,-1.11452,2.11961,-0.50102
-5.60440,0.86461,-1.57253,-1.26512,-1.11391,2.10941,-0.48848
-5.65934,0.87012,-1.56857,-1.26748,-1.11318,2.10560,-0.48386
-5.71429,0.87556,-1.56470,-1.26980,-1.11246,2.10184,-0.47931
-5.76923,0.88942,-1.55405,-1.27724,-1.10996,2.09219,-0.46780
-5.82418,0.89581,-1.54928,-1.28052,-1.10888,2.08772,-0.46255
-5.87912,0.90221,-1.54459,-1.28369,-1.10786,2.08324,-0.45730
-5.93407,0.91325,-1.53539,-1.29127,-1.10516,2.07548,-0.44831
-5.98901,0.91669,-1.53246,-1.29377,-1.10426,2.07305,-0.44553
-6.04396,0.91965,-1.53016,-1.29553,-1.10367,2.07095,-0.44313
-6.09890,0.92487,-1.52590,-1.29904,-1.10243,2.06726,-0.43892
-6.15385,0.92751,-1.52371,-1.30090,-1.10178,2.06539,-0.43681
-6.20879,0.93014,-1.52149,-1.30283,-1.10109,2.06353,-0.43470
-6.26374,0.93093,-1.52078,-1.30348,-1.10085,2.06297,-0.43407
-6.31868,0.93670,-1.51474,-1.30983,-1.09840,2.05886,-0.42944
-6.37363,0.93759,-1.51377,-1.31088,-1.09799,2.05823,-0.42873
-6.42857,0.93846,-1.51277,-1.31200,-1.09755,2.05761,-0.42804
-6.48352,0.94343,-1.50626,-1.31987,-1.09437,2.05406,-0.42407
-6.53846,0.94797,-1.49828,-1.33073,-1.08984,2.05080,-0.42044
-6.59341,0.94800,-1.49823,-1.33079,-1.08981,2.05079,-0.42042
-6.64835,0.94995,-1.49424,-1.33648,-1.08741,2.04938,-0.41886
-6.70330,0.95449,-1.47425,-1.36889,-1.07332,2.04609,-0.41516
-6.75824,0.95498,-1.47198,-1.37258,-1.07172,2.04573,-0.41475
-6.81319,0.95592,-1.46576,-1.38300,-1.06717,2.04504,-0.41397
-6.86813,0.95629,-1.44199,-1.42504,-1.04869,2.04469,-0.41350
-6.92308,0.95622,-1.43615,-1.43544,-1.04413,2.04472,-0.41350
-6.97802,0.95462,-1.41562,-1.47284,-1.02775,2.04580,-0.41459
-7.03297,0.95413,-1.41007,-1.48298,-1.02332,2.04614,-0.41494
-7.08791,0.95343,-1.40608,-1.49049,-1.02002,2.04662,-0.41545
-7.14286,0.95139,-1.39595,-1.50973,-1.01159,2.04804,-0.41696
-7.19780,0.95019,-1.39000,-1.52101,-1.00668,2.04888,-0.41786
-7.25275,0.94968,-1.38744,-1.52585,-1.00457,2.04923,-0.41824
-7.30769,0.94911,-1.38511,-1.53033,-1.00261,2.04963,-0.41867
-7.36264,0.94845,-1.38288,-1.53470,-1.00070,2.05009,-0.41917
-7.41758,0.94822,-1.38214,-1.53615,-1.00006,2.05026,-0.41934
-7.47253,0.94737,-1.38014,-1.54026,-0.99824,2.05086,-0.41999
-7.52747,0.94705,-1.37942,-1.54175,-0.99759,2.05108,-0.42024
-7.58242,0.94673,-1.37892,-1.54286,-0.99709,2.05131,-0.42048
-7.63736,0.94630,-1.37835,-1.54419,-0.99649,2.05161,-0.42082
-7.69231,0.94374,-1.37639,-1.54954,-0.99400,2.05342,-0.42282
-7.74725,0.94302,-1.37590,-1.55093,-0.99335,2.05394,-0.42338
-7.80220,0.94115,-1.37508,-1.55379,-0.99199,2.05526,-0.42484
-7.85714,0.94092,-1.37500,-1.55411,-0.99183,2.05542,-0.42502
-7.91209,0.93810,-1.37500,-1.55629,-0.99067,2.05742,-0.42724
-7.96703,0.93756,-1.37503,-1.55666,-0.99046,2.05780,-0.42767
-8.02198,0.93577,-1.37530,-1.55760,-0.98992,2.05907,-0.42908
-8.07692,0.92872,-1.37787,-1.55868,-0.98886,2.06406,-0.43468
-8.13187,0.92613,-1.37898,-1.55880,-0.98859,2.06589,-0.43675
-8.18681,0.92339,-1.38030,-1.55868,-0.98841,2.06783,-0.43894
-8.24176,0.91389,-1.38615,-1.55616,-0.98869,2.07453,-0.44658
-8.29670,0.90999,-1.38875,-1.55479,-0.98895,2.07727,-0.44974
-8.35165,0.90710,-1.39077,-1.55362,-0.98922,2.07930,-0.45208
-8.40659,0.89358,-1.40279,-1.54386,-0.99231,2.08878,-0.46314
-8.46154,0.88810,-1.40785,-1.53961,-0.99371,2.09261,-0.46766
-8.51648,0.88376,-1.41230,-1.53548,-0.99515,2.09564,-0.47125
-8.57143,0.88119,-1.41495,-1.53303,-0.99601,2.09743,-0.47339
-8.62637,0.87199,-1.42516,-1.52302,-0.99964,2.10383,-0.48108
-8.68132,0.87114,-1.42612,-1.52208,-0.99998,2.10442,-0.48179
-8.73626,0.86863,-1.42917,-1.51889,-1.00118,2.10616,-0.48390
-8.79121,0.86626,-1.43215,-1.51570,-1.00239,2.10780,-0.48590
-8.84615,0.86526,-1.43354,-1.51413,-1.00300,2.10850,-0.48675
-8.90110,0.86346,-1.43639,-1.51070,-1.00435,2.10975,-0.48827
-8.95604,0.86343,-1.43643,-1.51065,-1.00437,2.10977,-0.48829
-9.01099,0.86113,-1.44156,-1.50364,-1.00724,2.11136,-0.49026
-9.06593,0.86046,-1.44344,-1.50094,-1.00836,2.11183,-0.49083
-9.12088,0.85960,-1.44764,-1.49431,-1.01117,2.11244,-0.49159
-9.17582,0.85930,-1.44994,-1.49053,-1.01279,2.11265,-0.49186
-9.23077,0.85915,-1.45229,-1.48650,-1.01453,2.11276,-0.49200
-9.28571,0.85928,-1.45381,-1.48371,-1.01576,2.11268,-0.49190
-9.34066,0.86640,-1.46518,-1.45740,-1.02771,2.10783,-0.48604
-9.39560,0.86798,-1.46768,-1.45160,-1.03037,2.10675,-0.48475
-9.45055,0.87141,-1.47135,-1.44211,-1.03476,2.10440,-0.48193
-9.50549,0.88524,-1.48141,-1.41237,-1.04880,2.09486,-0.47059
-9.56044,0.89056,-1.48635,-1.39904,-1.05507,2.09118,-0.46626
-9.61538,0.89406,-1.48922,-1.39096,-1.05889,2.08875,-0.46343
-9.67033,0.90127,-1.49475,-1.37501,-1.06647,2.08373,-0.45761
-9.72527,0.90334,-1.49610,-1.37087,-1.06846,2.08229,-0.45594
-9.78022,0.90393,-1.49651,-1.36964,-1.06905,2.08188,-0.45546
-9.83516,0.90469,-1.49697,-1.36818,-1.06976,2.08135,-0.45485
-9.89011,0.90472,-1.49699,-1.36813,-1.06978,2.08133,-0.45483
-9.94505,0.90486,-1.49706,-1.36787,-1.06990,2.08123,-0.45472
-10.00000,0.90489,-1.49708,-1.36782,-1.06993,2.08121,-0.45469
+0.00000,0.89672,-1.14483,-1.60108,-1.19515,2.08894,-0.46310
+0.90909,0.86014,-1.15888,-1.59376,-1.20231,2.11354,-0.49311
+1.81818,0.78343,-1.22114,-1.54642,-1.22208,2.16489,-0.55978
+2.72727,0.73242,-1.29426,-1.47359,-1.24837,2.19812,-0.60696
+3.63636,0.72427,-1.30838,-1.45836,-1.25398,2.20334,-0.61473
+4.54545,0.71705,-1.32065,-1.44531,-1.25883,2.20795,-0.62166
+5.45455,0.71180,-1.32990,-1.43534,-1.26254,2.21129,-0.62674
+6.36364,0.70894,-1.33521,-1.42951,-1.26470,2.21309,-0.62951
+7.27273,0.70580,-1.34125,-1.42281,-1.26719,2.21509,-0.63258
+8.18182,0.70304,-1.34727,-1.41584,-1.26974,2.21683,-0.63528
+9.09091,0.69707,-1.36317,-1.39643,-1.27676,2.22059,-0.64116
+10.00000,0.69705,-1.36322,-1.39637,-1.27678,2.22060,-0.64118
diff --git a/python/examples/path_in_pixels.csv b/python/examples/path_in_pixels.csv
index c1306d726f334da5e341ebffbdd3cae3deb15229..fd83f2220341f3a17b375383f21daea0337fdad7 100644
--- a/python/examples/path_in_pixels.csv
+++ b/python/examples/path_in_pixels.csv
@@ -1,183 +1,12 @@
-0.48757,0.61206
-0.48757,0.61206
-0.48609,0.61358
-0.48461,0.61510
-0.48017,0.61662
-0.47869,0.62117
-0.46832,0.62573
-0.46239,0.63029
-0.44461,0.64548
-0.44017,0.64700
-0.43424,0.65307
-0.42832,0.65915
-0.40758,0.67130
-0.39869,0.67434
-0.39276,0.68041
-0.35869,0.69256
-0.34980,0.69560
-0.33943,0.69864
-0.31128,0.70472
-0.30684,0.70472
-0.30388,0.70472
-0.30240,0.70472
-0.28610,0.70472
-0.28610,0.70472
-0.28314,0.70168
-0.27869,0.70016
-0.27425,0.69712
-0.27425,0.69256
-0.27277,0.69105
-0.27129,0.68953
-0.27129,0.68497
-0.25647,0.66522
-0.25351,0.65459
-0.24906,0.65155
-0.24610,0.64244
-0.22981,0.61510
-0.22684,0.59991
-0.22388,0.59535
-0.22388,0.59383
-0.22388,0.58928
-0.22388,0.58776
-0.22388,0.58320
-0.22684,0.57864
-0.22832,0.57257
-0.24610,0.55130
-0.25203,0.54523
-0.25351,0.54067
-0.27425,0.52092
-0.28017,0.51333
-0.28462,0.50877
-0.29203,0.50270
-0.31277,0.48903
-0.31869,0.48447
-0.32758,0.47991
-0.34388,0.47687
-0.34832,0.47384
-0.35276,0.47384
-0.36165,0.47080
-0.36610,0.47080
-0.37054,0.47080
-0.37647,0.46928
-0.38239,0.46624
-0.38387,0.46624
-0.39869,0.46624
-0.40017,0.46624
-0.40461,0.46624
-0.41202,0.46624
-0.41795,0.46624
-0.42091,0.46624
-0.42239,0.46776
-0.43721,0.47232
-0.44165,0.47687
-0.45054,0.48295
-0.47869,0.50573
-0.49350,0.51333
-0.54535,0.55282
-0.56165,0.56194
-0.57794,0.56953
-0.63424,0.59839
-0.64609,0.60598
-0.65498,0.60902
-0.65942,0.61206
-0.66979,0.61814
-0.69053,0.62725
-0.70090,0.63484
-0.70683,0.63788
-0.71127,0.64244
-0.71275,0.64244
-0.71571,0.64548
-0.71571,0.64548
-0.71571,0.64700
-0.71571,0.64700
-0.71571,0.64852
-0.71423,0.64852
-0.71275,0.65003
-0.71127,0.65155
-0.67720,0.66370
-0.67275,0.66522
-0.63868,0.67738
-0.62979,0.68041
-0.62238,0.68041
-0.61201,0.68345
-0.58831,0.69408
-0.57794,0.69408
-0.56905,0.69712
-0.54535,0.70168
-0.53498,0.70472
-0.52461,0.70775
-0.50535,0.70775
-0.49943,0.70775
-0.49498,0.71079
-0.48609,0.71079
-0.48165,0.71079
-0.47720,0.71079
-0.47572,0.70927
-0.46535,0.70472
-0.46387,0.70320
-0.46239,0.70168
-0.45350,0.69560
-0.44609,0.68345
-0.44609,0.68345
-0.44313,0.67738
-0.43276,0.64700
-0.43128,0.64396
-0.42832,0.63484
-0.41647,0.59839
-0.41350,0.58928
-0.40313,0.55586
-0.40017,0.54675
-0.40017,0.53915
-0.39573,0.52092
-0.39276,0.51029
-0.39128,0.50573
-0.39128,0.50118
-0.39128,0.49662
-0.39128,0.49510
-0.39276,0.49054
-0.39276,0.48903
-0.39573,0.48751
-0.39721,0.48599
-0.40313,0.47991
-0.40461,0.47839
-0.40906,0.47536
-0.41054,0.47536
-0.42091,0.47536
-0.42239,0.47536
-0.42684,0.47536
-0.44017,0.47536
-0.44461,0.47536
-0.44906,0.47536
-0.46387,0.47536
-0.46980,0.47536
-0.47424,0.47536
-0.49646,0.47991
-0.50535,0.47991
-0.51276,0.48295
-0.51720,0.48295
-0.53350,0.48599
-0.53498,0.48599
-0.53942,0.48903
-0.54387,0.49054
-0.54535,0.49358
-0.54831,0.49814
-0.54831,0.49814
-0.55128,0.50877
-0.55128,0.51333
-0.55128,0.52244
-0.55128,0.52700
-0.55128,0.53156
-0.54831,0.53611
-0.53942,0.56649
-0.53942,0.57257
-0.53498,0.58320
-0.52461,0.61814
-0.52461,0.63333
-0.52165,0.64244
-0.51720,0.66067
-0.51424,0.66522
-0.51424,0.66674
-0.51276,0.66826
-0.51276,0.66826
-0.51128,0.66826
-0.51128,0.66826
+0.20381,0.36380
+0.25473,0.36700
+0.35332,0.38197
+0.44758,0.42152
+0.46491,0.43007
+0.48116,0.43434
+0.49308,0.43862
+0.49958,0.44183
+0.50716,0.44503
+0.51366,0.45038
+0.53100,0.46320
+0.53100,0.46320
diff --git a/python/ur_simple_control/__pycache__/__init__.cpython-312.pyc b/python/ur_simple_control/__pycache__/__init__.cpython-312.pyc
index 39cc8b4f8587f07e50e07745e3c0e25cbebd5262..962f7ffb2839b04d64de59bffc301462294b51dd 100644
Binary files a/python/ur_simple_control/__pycache__/__init__.cpython-312.pyc and b/python/ur_simple_control/__pycache__/__init__.cpython-312.pyc differ
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-311.pyc b/python/ur_simple_control/__pycache__/managers.cpython-311.pyc
index a9cb9681827fd5bf80a51b5ad0c1febe78fd9923..f483a01458224dd004f83b838d10cbd14cccf706 100644
Binary files a/python/ur_simple_control/__pycache__/managers.cpython-311.pyc and b/python/ur_simple_control/__pycache__/managers.cpython-311.pyc differ
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc
index 3313d6f721c2d15fc7500a8604808aa9f66a3c50..3cb97171b2bb96a925d27897af75a8930c1df8fe 100644
Binary files a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc and b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc differ
diff --git a/python/ur_simple_control/basics/__pycache__/__init__.cpython-312.pyc b/python/ur_simple_control/basics/__pycache__/__init__.cpython-312.pyc
index 43d425d803da5fdfb5ad1ba1bb8709ae74398a8b..90f7a9633aa4ec8f8144b86339c2c17b3825f0c6 100644
Binary files a/python/ur_simple_control/basics/__pycache__/__init__.cpython-312.pyc and b/python/ur_simple_control/basics/__pycache__/__init__.cpython-312.pyc differ
diff --git a/python/ur_simple_control/basics/__pycache__/basics.cpython-312.pyc b/python/ur_simple_control/basics/__pycache__/basics.cpython-312.pyc
index 7d4c1b2825ca761c742f7999cc5dbd5cd7d21b2f..5f7f84bdf9b600f8d0c911ebd0919c211ea1c011 100644
Binary files a/python/ur_simple_control/basics/__pycache__/basics.cpython-312.pyc and b/python/ur_simple_control/basics/__pycache__/basics.cpython-312.pyc differ
diff --git a/python/ur_simple_control/clik/__pycache__/__init__.cpython-312.pyc b/python/ur_simple_control/clik/__pycache__/__init__.cpython-312.pyc
index 61a7479eba52df830deb8cbe9f5dc2f9142f0c8c..c00dbcf1872f6817c3bd5e64964ec57c2189cb2d 100644
Binary files a/python/ur_simple_control/clik/__pycache__/__init__.cpython-312.pyc and b/python/ur_simple_control/clik/__pycache__/__init__.cpython-312.pyc differ
diff --git a/python/ur_simple_control/clik/clik.py b/python/ur_simple_control/clik/clik.py
index 6d4481429b58d4babe58ae64cdafd6e30859b2e9..1b140aeaa8f61a415e26bda0975b9c1a73cfb34d 100644
--- a/python/ur_simple_control/clik/clik.py
+++ b/python/ur_simple_control/clik/clik.py
@@ -8,6 +8,7 @@ import time
from qpsolvers import solve_qp
import argparse
import importlib
+import proxsuite
if importlib.util.find_spec('shapely'):
from ur_simple_control.path_generation.planner import path2D_to_timed_SE3, pathPointFromPathParam
@@ -40,7 +41,7 @@ def getClikArgs(parser):
# TODO add the rest
parser.add_argument('--clik-controller', type=str, \
help="select which click algorithm you want", \
- default='dampedPseudoinverse', choices=['dampedPseudoinverse', 'jacobianTranspose', 'invKinmQP'])
+ default='dampedPseudoinverse', choices=['dampedPseudoinverse', 'jacobianTranspose', 'invKinmQP', 'QPproxsuite'])
###########################################
# force sensing and feedback parameters #
@@ -90,7 +91,8 @@ def jacobianTranspose(J, err_vector):
# TODO: put something into q of the QP
# also, put in lb and ub
-def invKinmQP(J, err_vector, lb=None, ub=None):
+# this one is with qpsolvers
+def invKinmQP(J, err_vector, lb=None, ub=None, past_qd=None):
"""
invKinmQP
---------
@@ -118,12 +120,47 @@ def invKinmQP(J, err_vector, lb=None, ub=None):
# TODO: you probably want limits here
#lb = None
#ub = None
+ #lb *= 20
+ #ub *= 20
h = None
+ # (n_vars, n_eq_constraints, n_ineq_constraints)
+ #qp.init(H, g, A, b, C, l, u)
+ #print(J.shape)
+ #print(q.shape)
+ #print(A.shape)
+ #print(b.shape)
+# NOTE: you want to pass the previous solver, not recreate it every time
+######################
+# solve it
#qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="ecos")
- qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="quadprog")
+ #qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="quadprog", verbose=True, initvals=np.ones(len(lb)))
+ #if not (past_qd is None):
+ # qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="proxqp", verbose=False, initvals=past_qd)
+ #else:
+ # qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="proxqp", verbose=False, initvals=J.T@err_vector)
+ #qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="proxqp", verbose=True, initvals=0.01*J.T@err_vector)
+ #qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="quadprog", verbose=False, initvals=0.01*J.T@err_vector)
+ qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="quadprog", verbose=False)
+ #qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="proxqp")
return qd
+def QPproxsuite(qp, lb, ub, J, err_vector):
+ # proxsuite does lb <= Cx <= ub
+ qp.settings.initial_guess = (
+ proxsuite.proxqp.InitialGuess.WARM_START_WITH_PREVIOUS_RESULT
+)
+ #qp.update(g=q, A=A, b=h, l=lb, u=ub)
+ qp.update(A=J, b=err_vector)
+ qp.solve()
+ qd = qp.results.x
+
+ if qp.results.info.status == proxsuite.proxqp.PROXQP_PRIMAL_INFEASIBLE:
+ #if np.abs(qp.results.info.duality_gap) > 0.1:
+ print("didn't solve shit")
+ qd = None
+ return qd
+
# TODO: calculate nice q (in QP) as the secondary objective
# this requires getting the forward kinematics hessian,
# a.k.a jacobian derivative w.r.t. joint positions dJ/dq .
@@ -156,10 +193,8 @@ class CostManipulability:
res += JqJpk[k,:]
res *= self.calc(q)
return res
-"""
# use this as a starting point for finite differencing
-"""
def numdiff(func, x, eps=1e-6):
f0 = copy.copy(func(x))
xe = x.copy()
@@ -172,10 +207,8 @@ def numdiff(func, x, eps=1e-6):
return np.stack(fs,axis=1)
else:
return np.matrix(fs)
-"""
# and here's example usage
-"""
# Tdiffq is used to compute the tangent application in the configuration space.
Tdiffq = lambda f,q: Tdiff1(f,lambda q,v:pin.integrate(robot.model,q,v),robot.model.nv,q)
c=costManipulability
@@ -219,7 +252,7 @@ def QPManipMax(J, err_vector, lb=None, ub=None):
qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="quadprog")
return qd
-def getClikController(args):
+def getClikController(args, robot):
"""
getClikController
-----------------
@@ -242,7 +275,30 @@ def getClikController(args):
#if controller_name == "invKinm_PseudoInv_half":
# return invKinm_PseudoInv_half
if args.clik_controller == "invKinmQP":
- return invKinmQP
+ lb = -1 * np.array(robot.model.velocityLimit, dtype='double')
+ # we do additional clipping
+ lb = np.clip(lb, -1 * robot.max_qd, robot.max_qd)
+ ub = np.array(robot.model.velocityLimit, dtype='double')
+ ub = np.clip(ub, -1 * robot.max_qd, robot.max_qd)
+ return partial(invKinmQP, lb=lb, ub=ub)
+ if args.clik_controller == "QPproxsuite":
+ H = np.eye(robot.model.nv)
+ g = np.zeros(robot.model.nv)
+ G = np.eye(robot.model.nv)
+ A = np.eye(6, robot.model.nv)
+ b = np.ones(6) * 0.1
+ # proxsuite does lb <= Cx <= ub
+ C = np.eye(robot.model.nv)
+ lb = -1 * np.array(robot.model.velocityLimit, dtype='double')
+ # we do additional clipping
+ lb = np.clip(lb, -1 * robot.max_qd, robot.max_qd)
+ ub = np.array(robot.model.velocityLimit, dtype='double')
+ ub = np.clip(ub, -1 * robot.max_qd, robot.max_qd)
+ qp = proxsuite.proxqp.dense.QP(robot.model.nv, 6, robot.model.nv)
+ qp.init(H, g, A, b, G, lb, ub)
+ qp.solve()
+ return partial(QPproxsuite, qp, lb, ub)
+
#if controller_name == "invKinmQPSingAvoidE_kI":
# return invKinmQPSingAvoidE_kI
#if controller_name == "invKinmQPSingAvoidE_kM":
@@ -264,6 +320,7 @@ def controlLoopClik(robot : RobotManager, clik_controller, i, past_data):
"""
breakFlag = False
log_item = {}
+ save_past_item = {}
q = robot.getQ()
T_w_e = robot.getT_w_e()
# first check whether we're at the goal
@@ -273,15 +330,23 @@ def controlLoopClik(robot : RobotManager, clik_controller, i, past_data):
breakFlag = True
J = pin.computeFrameJacobian(robot.model, robot.data, q, robot.ee_frame_id)
# compute the joint velocities based on controller you passed
+ #qd = clik_controller(J, err_vector, past_qd=past_data['dqs_cmd'][-1])
qd = clik_controller(J, err_vector)
+ if qd is None:
+ print(i)
+ qd = dampedPseudoinverse(1e-2, J, err_vector)
+ else:
+ print(i, "actually worked")
robot.sendQd(qd)
log_item['qs'] = q.reshape((robot.model.nq,))
log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
log_item['dqs_cmd'] = qd.reshape((robot.model.nv,))
+ log_item['err_norm'] = np.linalg.norm(err_vector).reshape((1,))
# we're not saving here, but need to respect the API,
# hence the empty dict
- return breakFlag, {}, log_item
+ save_past_item['dqs_cmd'] = qd.reshape((robot.model.nv,))
+ return breakFlag, save_past_item, log_item
def controlLoopClikDualArm(robot : RobotManager, clik_controller, goal_transform, i, past_data):
@@ -339,7 +404,7 @@ def controlLoopClikDualArm(robot : RobotManager, clik_controller, goal_transform
# -----------
# """
# robot.Mgoal = copy.deepcopy(goal)
-# clik_controller = getClikController(args)
+# clik_controller = getClikController(args, robot)
# controlLoop = partial(controlLoopClikDualArm, robot, clik_controller, goal_transform)
# # we're not using any past data or logging, hence the empty arguments
# log_item = {
@@ -428,7 +493,7 @@ def moveUntilContact(args, robot, speed):
does clik until it feels something with the f/t sensor
"""
assert type(speed) == np.ndarray
- clik_controller = getClikController(args)
+ clik_controller = getClikController(args, robot)
controlLoop = partial(moveUntilContactControlLoop, args, robot, speed, clik_controller)
# we're not using any past data or logging, hence the empty arguments
log_item = {'wrench' : np.zeros(6)}
@@ -447,15 +512,19 @@ def moveL(args, robot : RobotManager, goal_point):
"""
#assert type(goal_point) == pin.pinocchio_pywrap.SE3
robot.Mgoal = copy.deepcopy(goal_point)
- clik_controller = getClikController(args)
+ clik_controller = getClikController(args, robot)
controlLoop = partial(controlLoopClik, robot, clik_controller)
# we're not using any past data or logging, hence the empty arguments
log_item = {
'qs' : np.zeros(robot.model.nq),
'dqs' : np.zeros(robot.model.nv),
'dqs_cmd' : np.zeros(robot.model.nv),
+ 'err_norm' : np.zeros(1),
}
- loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_item)
+ save_past_dict = {
+ 'dqs_cmd' : np.zeros(robot.model.nv),
+ }
+ loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_item)
loop_manager.run()
def moveLDualArm(args, robot : RobotManager, goal_point, goal_transform, run=True):
@@ -468,7 +537,7 @@ def moveLDualArm(args, robot : RobotManager, goal_point, goal_transform, run=Tru
"""
#assert type(goal_point) == pin.pinocchio_pywrap.SE3
robot.Mgoal = copy.deepcopy(goal_point)
- clik_controller = getClikController(args)
+ clik_controller = getClikController(args, robot)
controlLoop = partial(controlLoopClikDualArm, robot, clik_controller, goal_transform)
# we're not using any past data or logging, hence the empty arguments
log_item = {
@@ -528,7 +597,7 @@ def cartesianPathFollowingWithPlannerControlLoop(args, robot : RobotManager, pat
path_pol, path2D_untimed = data
path2D_untimed = np.array(path2D_untimed).reshape((-1,2))
# should be precomputed somewhere but this is nowhere near the biggest problem
- max_base_v = np.linalg.norm(robot.model.velocityLimit[:2])
+ max_base_v = np.linalg.norm(robot.model.robot.model.velocityLimit[:2])
# 1) make 6D path out of [[x0,y0],...]
# that represents the center of the cart
@@ -544,9 +613,9 @@ def cartesianPathFollowingWithPlannerControlLoop(args, robot : RobotManager, pat
# NOTE: obviously not path following but i want to see things working here
SEerror = T_w_e.actInv(pathSE3[-1])
err_vector = pin.log6(SEerror).vector
- lb = -1*robot.model.velocityLimit
+ lb = -1*robot.model.robot.model.velocityLimit
lb[1] = -0.001
- ub = robot.model.velocityLimit
+ ub = robot.model.robot.model.velocityLimit
ub[1] = 0.001
#vel_cmd = invKinmQP(J, err_vector, lb=lb, ub=ub)
vel_cmd = dampedPseudoinverse(0.002, J, err_vector)
@@ -626,7 +695,7 @@ def compliantMoveL(args, robot, goal_point, run=True):
"""
# assert type(goal_point) == pin.pinocchio_pywrap.SE3
robot.Mgoal = copy.deepcopy(goal_point)
- clik_controller = getClikController(args)
+ clik_controller = getClikController(args, robot)
controlLoop = partial(controlLoopCompliantClik, args, robot)
# we're not using any past data or logging, hence the empty arguments
log_item = {
@@ -746,7 +815,7 @@ def controlLoopClikDualArmsOnly(robot : RobotManager, clik_controller, goal_tran
# compute the joint velocities based on controller you passed
qd_left = clik_controller(J_left, err_vector_left)
qd_right = clik_controller(J_right, err_vector_right)
- #lb, ub = (-0.05 * robot.model.velocityLimit, 0.05 * robot.model.velocityLimit)
+ #lb, ub = (-0.05 * robot.model.robot.model.velocityLimit, 0.05 * robot.model.robot.model.velocityLimit)
#qd_left = invKinmQP(J_left, err_vector_left, lb=lb[3:], ub=ub[3:])
#qd_right = invKinmQP(J_right, err_vector_right, lb=lb[3:], ub=ub[3:])
qd = qd_left + qd_right
@@ -771,7 +840,7 @@ def moveLDualArmsOnly(args, robot : RobotManager, goal_point, goal_transform, ru
"""
#assert type(goal_point) == pin.pinocchio_pywrap.SE3
robot.Mgoal = copy.deepcopy(goal_point)
- clik_controller = getClikController(args)
+ clik_controller = getClikController(args, robot)
controlLoop = partial(controlLoopClikDualArmsOnly, robot, clik_controller, goal_transform)
# we're not using any past data or logging, hence the empty arguments
log_item = {
@@ -789,7 +858,7 @@ if __name__ == "__main__":
args = get_args()
robot = RobotManager(args)
Mgoal = robot.defineGoalPointCLI()
- clik_controller = getClikController(args)
+ clik_controller = getClikController(args, robot)
controlLoop = partial(controlLoopClik, robot, clik_controller)
# we're not using any past data or logging, hence the empty arguments
loop_manager = ControlLoopManager(robot, controlLoop, args, {}, {})
diff --git a/python/ur_simple_control/dmp/__pycache__/__init__.cpython-312.pyc b/python/ur_simple_control/dmp/__pycache__/__init__.cpython-312.pyc
index 36d923a88e1d3b72459e16e37f40e8b62362d845..2f9b2824737ebed52a6982e9abb3a3a070851fdd 100644
Binary files a/python/ur_simple_control/dmp/__pycache__/__init__.cpython-312.pyc and b/python/ur_simple_control/dmp/__pycache__/__init__.cpython-312.pyc differ
diff --git a/python/ur_simple_control/dmp/__pycache__/dmp.cpython-312.pyc b/python/ur_simple_control/dmp/__pycache__/dmp.cpython-312.pyc
index 7a66c5c8e1c46e94ce711f546a338bab792ad7b9..496b8c705ee1e32c4b4865057475df6ae5f3db9c 100644
Binary files a/python/ur_simple_control/dmp/__pycache__/dmp.cpython-312.pyc and b/python/ur_simple_control/dmp/__pycache__/dmp.cpython-312.pyc differ
diff --git a/python/ur_simple_control/optimal_control/create_pinocchio_casadi_ocp.py b/python/ur_simple_control/optimal_control/create_pinocchio_casadi_ocp.py
index be9364a9c2fc6fb82e42c56bd4f6ad00f3db643f..f2af8fd3d639420a72173bf182165d0e4ebf82f8 100644
--- a/python/ur_simple_control/optimal_control/create_pinocchio_casadi_ocp.py
+++ b/python/ur_simple_control/optimal_control/create_pinocchio_casadi_ocp.py
@@ -95,11 +95,12 @@ def createCasadiIKObstacleAvoidanceOCP(args, robot : RobotManager, T_goal: pin.S
# here you need to define other obstacles, namely the table (floor)
# it's obviously going to be a plane
# alternatively just forbid z-axis of end-effector to be negative
- obstacles = []
-# obstacles = [
-# SimpleNamespace(radius=0.01, pos=np.array([-0.4, 0.2 + s, 0.5]), name=f"obs_{i_s}")
-# for i_s, s in enumerate(np.arange(-0.5, 0.5, 0.25))
-# ]
+ obstacles_sphere = [
+ SimpleNamespace(radius=0.05, pos=np.array([0.0, 0.3, 0.0 + s]), name=f"obs_{i_s}")
+ for i_s, s in enumerate(np.arange(0.0, 0.5, 0.125))
+ ]
+ for obstacle in obstacles_sphere:
+ robot.visualizer_manager.sendCommand({"obstacle_sphere" : [obstacle.radius, obstacle.pos]})
# define the optimizer/solver
opti = casadi.Opti()
@@ -135,11 +136,15 @@ def createCasadiIKObstacleAvoidanceOCP(args, robot : RobotManager, T_goal: pin.S
ellipse.e_pos = casadi.Function(
f"e{ellipse.name}", [cq, cpos], [cdata.oMi[ellipse.id].inverse().act(casadi.SX(cpos))]
)
- for obstacle in obstacles:
+ for obstacle in obstacles_sphere:
for q in var_qs:
# obstacle position in ellipsoid (joint) frame
#e_pos = e.e_pos(var_q, o.pos)
e_pos = ellipse.e_pos(q, obstacle.pos)
+ # pretend obstacle is a point, and then
+ # do no intersect with the point
+ # TODO: make different equations for different obstacles,
+ # most importantly box
opti.subject_to((e_pos - ellipse.center).T @ ellipse.A @ (e_pos - ellipse.center) >= 1)
# now that the ocp has been transcribed as nlp,
diff --git a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
index f056d26c6ef76efcbaae5bdccc0ee827770594b6..fa5f37ab5db0c676be3261ea10cc89e083182edc 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
@@ -119,7 +119,6 @@ def CrocoEndEffectorPathFollowingMPCControlLoop(args, robot : RobotManager, solv
T_w_e = robot.getT_w_e()
p = T_w_e.translation[:2]
- path_planner.sendCommandViaSharedMemory(p)
# NOTE: it's pointless to recalculate the path every time
# if it's the same 2D path
@@ -127,6 +126,7 @@ def CrocoEndEffectorPathFollowingMPCControlLoop(args, robot : RobotManager, solv
if type(path_planner) == types.FunctionType:
pathSE3 = path_planner(T_w_e, i)
else:
+ path_planner.sendCommandViaSharedMemory(p)
data = path_planner.getData()
if data == None:
if args.debug_prints:
diff --git a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
index 16315d1fbae8bda9f7a3fd5ab43cc05923fcff20..d5576b49c1dc3391896f1fb3d9fa85235c79820d 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
@@ -438,10 +438,11 @@ def createBaseAndEEPathFollowingOCP(args, robot : RobotManager, x0):
"""
if robot.robot_name != "yumi":
T_w_e = robot.getT_w_e()
+ path_ee = [T_w_e] * args.n_knots
else:
T_w_e_left, T_w_e_right = robot.getT_w_e()
+ path_ee = [T_w_e_left] * args.n_knots
# TODO: have a different reference for each arm
- path_ee = [T_w_e_left] * args.n_knots
path_base = [np.append(x0[:2], 0.0)] * args.n_knots
# underactuation is done by setting max-torque to 0 in the robot model,
# and since we torques are constrained we're good
diff --git a/python/ur_simple_control/path_generation/mobile_manipulation_challenge.egg-info/PKG-INFO b/python/ur_simple_control/path_generation/mobile_manipulation_challenge.egg-info/PKG-INFO
new file mode 100644
index 0000000000000000000000000000000000000000..154e4bc923f226e57a41c1fac37a45df6a9e1681
--- /dev/null
+++ b/python/ur_simple_control/path_generation/mobile_manipulation_challenge.egg-info/PKG-INFO
@@ -0,0 +1,8 @@
+Metadata-Version: 2.1
+Name: mobile_manipulation_challenge
+Version: 1.0
+Requires-Dist: numpy
+Requires-Dist: scipy
+Requires-Dist: matplotlib
+Requires-Dist: shapely
+Requires-Dist: pyyaml
diff --git a/python/ur_simple_control/path_generation/mobile_manipulation_challenge.egg-info/SOURCES.txt b/python/ur_simple_control/path_generation/mobile_manipulation_challenge.egg-info/SOURCES.txt
new file mode 100644
index 0000000000000000000000000000000000000000..f8a90ef64c3032b6949d0a85f7ab8160c3209a1d
--- /dev/null
+++ b/python/ur_simple_control/path_generation/mobile_manipulation_challenge.egg-info/SOURCES.txt
@@ -0,0 +1,7 @@
+README.md
+setup.py
+mobile_manipulation_challenge.egg-info/PKG-INFO
+mobile_manipulation_challenge.egg-info/SOURCES.txt
+mobile_manipulation_challenge.egg-info/dependency_links.txt
+mobile_manipulation_challenge.egg-info/requires.txt
+mobile_manipulation_challenge.egg-info/top_level.txt
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/mobile_manipulation_challenge.egg-info/dependency_links.txt b/python/ur_simple_control/path_generation/mobile_manipulation_challenge.egg-info/dependency_links.txt
new file mode 100644
index 0000000000000000000000000000000000000000..8b137891791fe96927ad78e64b0aad7bded08bdc
--- /dev/null
+++ b/python/ur_simple_control/path_generation/mobile_manipulation_challenge.egg-info/dependency_links.txt
@@ -0,0 +1 @@
+
diff --git a/python/ur_simple_control/path_generation/mobile_manipulation_challenge.egg-info/requires.txt b/python/ur_simple_control/path_generation/mobile_manipulation_challenge.egg-info/requires.txt
new file mode 100644
index 0000000000000000000000000000000000000000..e1986006b0f59c3449690b8710882e63b42fb071
--- /dev/null
+++ b/python/ur_simple_control/path_generation/mobile_manipulation_challenge.egg-info/requires.txt
@@ -0,0 +1,5 @@
+numpy
+scipy
+matplotlib
+shapely
+pyyaml
diff --git a/python/ur_simple_control/path_generation/mobile_manipulation_challenge.egg-info/top_level.txt b/python/ur_simple_control/path_generation/mobile_manipulation_challenge.egg-info/top_level.txt
new file mode 100644
index 0000000000000000000000000000000000000000..8b137891791fe96927ad78e64b0aad7bded08bdc
--- /dev/null
+++ b/python/ur_simple_control/path_generation/mobile_manipulation_challenge.egg-info/top_level.txt
@@ -0,0 +1 @@
+
diff --git a/python/ur_simple_control/path_generation/setup.py b/python/ur_simple_control/path_generation/setup.py
index df2f0c78df00acfbde143eed1cd5a2a9b32fb2ac..dad99868742763693ded4b22c3a8a6e1d09acdad 100644
--- a/python/ur_simple_control/path_generation/setup.py
+++ b/python/ur_simple_control/path_generation/setup.py
@@ -1,6 +1,6 @@
from setuptools import setup, find_packages
-setup(name='mobile_manipulation_challenge',
+setup(name='starworlds',
version='1.0',
packages=find_packages(),
install_requires=[
diff --git a/python/ur_simple_control/path_generation/star_navigation/config/load_config.py b/python/ur_simple_control/path_generation/star_navigation/config/load_config.py
index 0858ef11f503890a5566fb8a424662bc0902dcce..28e0590ea5d6e0405a5f787d339d3a5566fe0712 100644
--- a/python/ur_simple_control/path_generation/star_navigation/config/load_config.py
+++ b/python/ur_simple_control/path_generation/star_navigation/config/load_config.py
@@ -34,7 +34,8 @@ def load_config(scene_id=None, robot_type_id=None, ctrl_param_file=None, verbosi
name=robot_type)
x0 = scene.p0
elif robot_params['model'] == 'Unicycle':
- robot = Unicycle(radius=robot_params['radius'], vel_min=[robot_params['lin_vel_min'], -robot_params['ang_vel_max']],
+ #robot = Unicycle(radius=robot_params['radius'], vel_min=[robot_params['lin_vel_min'], -robot_params['ang_vel_max']],
+ robot = Unicycle(width=robot_params['radius'], vel_min=[robot_params['lin_vel_min'], -robot_params['ang_vel_max']],
vel_max=[robot_params['lin_vel_max'], robot_params['ang_vel_max']],
name=robot_type)
try:
diff --git a/python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/PKG-INFO b/python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/PKG-INFO
index 21a305dc4d6e70e9b3fda722cf1e3cd64d5275d1..f45c70919684dc135f35391ab63f8d9e19989cbb 100644
--- a/python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/PKG-INFO
+++ b/python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/PKG-INFO
@@ -1,10 +1,3 @@
Metadata-Version: 2.1
-Name: star-navigation
+Name: star_navigation
Version: 1.0
-Summary: UNKNOWN
-Home-page: UNKNOWN
-License: UNKNOWN
-Platform: UNKNOWN
-
-UNKNOWN
-
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/LICENSE b/python/ur_simple_control/path_generation/star_navigation/starworlds/LICENSE
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/README.md b/python/ur_simple_control/path_generation/star_navigation/starworlds/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/__init__.py b/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..52cb0934227437bf233de1e81164eac4f85fa2fe
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/__init__.py
@@ -0,0 +1,6 @@
+from .obstacle import Obstacle, Frame
+from .starshaped_obstacle import StarshapedObstacle
+from .ellipse import Ellipse
+from .polygon import Polygon
+from .starshaped_polygon import StarshapedPolygon
+from .starshaped_primitive_combination import StarshapedPrimitiveCombination
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/ellipse.py b/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/ellipse.py
new file mode 100644
index 0000000000000000000000000000000000000000..17f216ea08bfd56748b3be73dbb0ffe8d9155481
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/ellipse.py
@@ -0,0 +1,217 @@
+from obstacles import StarshapedObstacle, Frame
+import numpy as np
+import matplotlib.pyplot as plt
+import matplotlib.patches as patches
+from utils import is_ccw, tic, toc
+import shapely
+
+
+class Ellipse(StarshapedObstacle):
+ def __init__(self, a, xr=None, n_pol=20, **kwargs):
+ self._a = np.array(a, float)
+ self._a2 = np.square(self._a)
+ self._n_pol = n_pol
+ # self._area = np.pi * self._a[0] * self._a[1]
+ if xr is None:
+ xr = np.zeros(2)
+ super().__init__(xr=xr, **kwargs)
+ self.enclosing_ball_diameter = max(2*self._a[0], 2*self._a[1])
+
+ def copy(self, id, name):
+ if (id == 'duplicate' or id == 'd'):
+ id = self.id()
+ return Ellipse(id=id, name=name, a=self._a, xr=self._xr, n_pol=self._n_pol, motion_model=self._motion_model)
+
+ def dilated_obstacle(self, padding, id="new", name=None):
+ cp = self.copy(id, name)
+ cp._a += padding
+ cp._a2 = np.square(cp._a)
+ cp.enclosing_ball_diameter = max(2 * cp._a[0], 2 * cp._a[1])
+ if self._polygon is not None:
+ cp._polygon = self._polygon.buffer(padding, cap_style=1, join_style=1)
+ cp._polygon_global_pose = None
+ cp._polygon_global = None
+ cp._kernel = cp._polygon
+ return cp
+
+ def init_plot(self, ax=None, show_reference=True, show_name=False, **kwargs):
+ if ax is None:
+ _, ax = plt.subplots(subplot_kw={'aspect': 'equal'})
+ # Default facecolor
+ if "fc" not in kwargs and "facecolor" not in kwargs:
+ kwargs["fc"] = 'lightgrey'
+ line_handles = []
+ # Boundary
+ line_handles += [patches.Ellipse(xy=[0, 0], width=2*self._a[0], height=2*self._a[1], angle=0, **kwargs)]
+ ax.add_patch(line_handles[-1])
+ # Reference point
+ line_handles += ax.plot(*self._xr, '+', color='k') if show_reference else [None]
+ # Name
+ line_handles += [ax.text(*self._xr, self._name)] if show_name else [None]
+ return line_handles, ax
+
+ def update_plot(self, line_handles, frame=Frame.GLOBAL):
+ pos, rot = self.pos(frame), self.rot(frame)
+ line_handles[0].center = pos
+ line_handles[0].angle = np.rad2deg(rot)
+ if line_handles[1] is not None:
+ line_handles[1].set_data(*self.xr(frame))
+ if line_handles[2] is not None:
+ line_handles[2].set_position(self.xr(frame))
+
+ def draw(self, frame=Frame.GLOBAL, **kwargs):
+ line_handles, ax = self.init_plot(**kwargs)
+ self.update_plot(line_handles, frame)
+ return line_handles, ax
+
+ def boundary_mapping(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
+ x_obstacle = self.transform(x, input_frame, Frame.OBSTACLE)
+
+ intersect_obstacle = self.line_intersection([self._xr, self._xr+1.1*self.enclosing_ball_diameter*self.reference_direction(x_obstacle, Frame.OBSTACLE, Frame.OBSTACLE)],
+ input_frame=Frame.OBSTACLE, output_frame=Frame.OBSTACLE)
+ if not intersect_obstacle:
+ return None
+ if len(intersect_obstacle) == 1:
+ return self.transform(intersect_obstacle[0], Frame.OBSTACLE, output_frame)
+ else:
+ if np.linalg.norm(intersect_obstacle[0]-x_obstacle) < np.linalg.norm(intersect_obstacle[1]-x_obstacle):
+ return self.transform(intersect_obstacle[0], Frame.OBSTACLE, output_frame)
+ else:
+ return self.transform(intersect_obstacle[1], Frame.OBSTACLE, output_frame)
+
+ def normal(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL, x_is_boundary=False):
+ b_obstacle = self.transform(x, input_frame, Frame.OBSTACLE) if x_is_boundary else self.boundary_mapping(x, input_frame, Frame.OBSTACLE)
+ if b_obstacle is None:
+ return None
+ n_obstacle = np.array([self._a2[1] * b_obstacle[0], self._a2[0] * b_obstacle[1]])
+ n_obstacle = n_obstacle / np.linalg.norm(n_obstacle)
+ return self.rotate(n_obstacle, Frame.OBSTACLE, output_frame)
+
+ def point_location(self, x, input_frame=Frame.GLOBAL):
+ x_obstacle = self.transform(x, input_frame, Frame.OBSTACLE)
+ loc = (x_obstacle[0] / self._a[0]) ** 2 + (x_obstacle[1] / self._a[1]) ** 2 - 1
+ return loc
+
+ def line_intersection(self, line, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
+ # Transform line points to left/right points in obstacle ellipse coordinates
+ l0_obstacle = self.transform(line[0], input_frame, Frame.OBSTACLE)
+ l1_obstacle = self.transform(line[1], input_frame, Frame.OBSTACLE)
+ l_left_obstacle = l0_obstacle if l0_obstacle[0] < l1_obstacle[0] else l1_obstacle
+ l_right_obstacle = l1_obstacle if l0_obstacle[0] < l1_obstacle[0] else l0_obstacle
+
+
+ vertical_line = abs(l_right_obstacle[0]-l_left_obstacle[0]) < 1e-4
+ if not vertical_line:
+ # Line parameters
+ m = (l_right_obstacle[1] - l_left_obstacle[1]) / (l_right_obstacle[0] - l_left_obstacle[0])
+ c = l_left_obstacle[1] - m * l_left_obstacle[0]
+ vertical_line = abs(m) > 100
+
+ # Special case with vertical line
+ if vertical_line:
+ if l_right_obstacle[0] < -self._a[0] or l_right_obstacle[0] > self._a[0]:
+ return []
+
+ l_top_obstacle = l_right_obstacle if l_right_obstacle[1] > l_left_obstacle[1] else l_left_obstacle
+ l_bottom_obstacle = l_left_obstacle if l_right_obstacle[1] > l_left_obstacle[1] else l_right_obstacle
+ x_intersect_top_obstacle, x_intersect_bottom_obstacle = np.array([0, self._a[1]]), np.array([0, -self._a[1]])
+ x_intersect_top = self.transform(x_intersect_top_obstacle, Frame.OBSTACLE, output_frame)
+ x_intersect_bottom = self.transform(x_intersect_bottom_obstacle, Frame.OBSTACLE, output_frame)
+ if l_top_obstacle[1] >= self._a[1] and l_bottom_obstacle[1] <= -self._a[1]:
+ return [x_intersect_top, x_intersect_bottom]
+ elif l_top_obstacle[1] >= self._a[1] and l_bottom_obstacle[1] <= self._a[1]:
+ return [x_intersect_top]
+ elif l_top_obstacle[1] >= -self._a[1] and l_bottom_obstacle[1] <= -self._a[1]:
+ return [x_intersect_bottom]
+ else:
+ return []
+
+ # obstacle ellipse coefficients at intersection with line m*x+c
+ kx2 = self._a2[0] * m**2 + self._a2[1]
+ kx = 2 * self._a2[0] * m * c
+ k1 = self._a2[0] * (c**2 - self._a2[1])
+
+ # TODO: Stable fix for finding intersection
+ discriminant = self._a2[0] * m**2 + self._a2[1] - c**2
+ if discriminant < 0:
+ return []
+ elif discriminant == 0:
+ tmp_x = -kx / (2 * kx2)
+ x_intersect_obstacle = np.array([tmp_x, m * tmp_x + c])
+ return [self.transform(x_intersect_obstacle, Frame.OBSTACLE, output_frame)]
+ else:
+ in_sqrt = kx ** 2 / (4 * kx2 ** 2) - k1 / kx2
+ if np.isclose(in_sqrt, 0):
+ in_sqrt = 0
+ tmp_x = -kx / (2 * kx2) - np.sqrt(in_sqrt)
+ x_intersect_left_obstacle = np.array([tmp_x, m * tmp_x + c])
+ tmp_x = -kx / (2 * kx2) + np.sqrt(in_sqrt)
+ x_intersect_right_obstacle = np.array([tmp_x, m * tmp_x + c])
+ x_intersect_left = self.transform(x_intersect_left_obstacle, Frame.OBSTACLE, output_frame)
+ x_intersect_right = self.transform(x_intersect_right_obstacle, Frame.OBSTACLE, output_frame)
+
+ if l_right_obstacle[0] < x_intersect_left_obstacle[0] or x_intersect_right_obstacle[0] < l_left_obstacle[0] or \
+ x_intersect_left_obstacle[0] < l_left_obstacle[0] < l_right_obstacle[0] < x_intersect_right_obstacle[0]:
+ return []
+ elif x_intersect_left_obstacle[0] < l_left_obstacle[0]:
+ return [x_intersect_right]
+ elif l_right_obstacle[0] < x_intersect_right_obstacle[0]:
+ return [x_intersect_left]
+ else:
+ return [x_intersect_left, x_intersect_right]
+
+ def tangent_points(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
+ x_obstacle = self.transform(x, input_frame, Frame.OBSTACLE)
+ if not self.exterior_point(x_obstacle, Frame.OBSTACLE):
+ return []
+ px, py = x_obstacle
+
+ # Special case with vertical tangent
+ a2 = self._a**2
+ vertical_tangent = abs(a2[0] - px**2) < 1e-5
+ if vertical_tangent:
+ m2 = (py**2-a2[1]) / (2*px*py)
+ x2 = (px * m2 ** 2 - py * m2) / (a2[1] / a2[0] + m2 ** 2)
+ y2 = m2 * (x2 - px) + py
+ tp1_obstacle = np.array([px, 0])
+ tp2_obstacle = np.array([x2, y2])
+ else:
+ tmp = px**2 - a2[0]
+ c1 = (px * py) / tmp
+ c2 = (a2[1] - py**2) / tmp
+ tmp = np.sqrt(c1**2+c2)
+ m1, m2 = c1 + tmp, c1 - tmp
+ tmp = a2[1] / a2[0]
+ m1_sqr = m1**2
+ m2_sqr = m2**2
+ x1 = (px * m1_sqr - py * m1) / (tmp + m1_sqr)
+ x2 = (px * m2_sqr - py * m2) / (tmp + m2_sqr)
+ y1 = m1 * (x1 - px) + py
+ y2 = m2 * (x2 - px) + py
+ tp1_obstacle = np.array([x1, y1])
+ tp2_obstacle = np.array([x2, y2])
+
+ tp1 = self.transform(tp1_obstacle, Frame.OBSTACLE, output_frame)
+ tp2 = self.transform(tp2_obstacle, Frame.OBSTACLE, output_frame)
+
+ if is_ccw(x, tp1, tp2):
+ tp1, tp2 = tp2, tp1
+
+ return [tp1, tp2]
+
+ # def area(self):
+ # return self._area
+
+ # ------------ Private methods ------------ #
+ def _check_convexity(self):
+ self._is_convex = True
+
+ def _compute_kernel(self):
+ self._kernel = self._polygon
+
+ def _compute_polygon_representation(self):
+ # logprint(str(self) + ": " + str(self._polygon), 0)
+ t = np.linspace(0, 2 * np.pi, self._n_pol, endpoint=False)
+ a = self._a + 1e-3 # Add offset to adjust for polygon approximation
+ polygon = np.vstack((a[0] * np.cos(t), a[1] * np.sin(t))).T
+ self._polygon = shapely.geometry.Polygon(polygon)
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/motion_model.py b/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/motion_model.py
new file mode 100644
index 0000000000000000000000000000000000000000..64e4746494cc9c7b3f8d43459f8dfdff44807bf3
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/motion_model.py
@@ -0,0 +1,119 @@
+import numpy as np
+from abc import ABC, abstractmethod
+
+
+class MotionModel(ABC):
+
+ def __init__(self, pos=None, rot=0):
+ self._pos = np.array([0., 0.]) if pos is None else np.array(pos, dtype='float64')
+ self._rot = rot
+ self._t = 0.
+
+ def move(self, obs_self, dt):
+ xy_vel = np.array(self.lin_vel())
+ rot_vel = self.rot_vel()
+ prev_pos, prev_rot = self._pos.copy(), self._rot
+ self._pos += xy_vel * dt
+ self._rot += rot_vel * dt
+ self._t += dt
+
+ def set_pos(self, pos):
+ self._pos = pos
+
+ def set_rot(self, rot):
+ self._rot = rot
+
+ def pos(self):
+ return self._pos
+
+ def rot(self):
+ return self._rot
+
+ @abstractmethod
+ def lin_vel(self):
+ pass
+
+ @abstractmethod
+ def rot_vel(self):
+ pass
+
+
+class Static(MotionModel):
+
+ def lin_vel(self):
+ return np.zeros(2)
+
+ def rot_vel(self):
+ return 0.
+
+
+class SinusVelocity(MotionModel):
+
+ # If cartesian_coords: (x1,x2) vel are Cartesian (x,y) vel.
+ # Else: (x1,x2) vel are linear and rotational (lin,ang) vel.
+ def __init__(self, pos=None, rot=0, cartesian_coords=True, x1_mag=0., x1_period=0, x2_mag=0., x2_period=0):
+ super().__init__(pos, rot)
+ self.cartesian_coords = cartesian_coords
+ self.x1_vel_mag = x1_mag
+ self.x1_vel_freq = 2 * np.pi / x1_period if x1_period else 0
+ self.x2_vel_mag = x2_mag
+ self.x2_vel_freq = 2 * np.pi / x2_period if x2_period else 0
+
+ def lin_vel(self):
+ if self.cartesian_coords:
+ return np.array([self.x1_vel_mag * np.cos(self.x1_vel_freq * self._t),
+ self.x2_vel_mag * np.cos(self.x2_vel_freq * self._t)])
+ else:
+ rot_mat = np.array([[np.cos(self._rot), -np.sin(self._rot)], [np.sin(self._rot), np.cos(self._rot)]])
+ return rot_mat.dot([self.x1_vel_mag * np.cos(self.x1_vel_freq * self._t), 0])
+
+ def rot_vel(self):
+ if self.cartesian_coords:
+ return 0.
+ else:
+ return np.array(self.x2_vel_mag * np.cos(self.x2_vel_freq * self._t))
+
+
+class Interval(MotionModel):
+
+ def __init__(self, init_pos, time_pos):
+ self.pos_point = np.array([init_pos] + [p for _, p in time_pos])
+ self.time_point = np.cumsum([0] + [t for t, _ in time_pos])
+ super().__init__(init_pos, 0)
+
+ def lin_vel(self):
+ if self._t > self.time_point[-1]:
+ vel_norm = np.linalg.norm((self.pos_point[-1] - self.pos_point[-2]) / (self.time_point[-1] - self.time_point[-2]))
+ dir = self.pos_point[-1] - self.pos()
+ if np.linalg.norm(dir) > vel_norm:
+ dir /= np.linalg.norm(dir)
+ return vel_norm * dir
+ idx = np.argmax(self._t < self.time_point)
+ return (self.pos_point[idx] - self.pos_point[idx-1]) / (self.time_point[idx] - self.time_point[idx-1])
+
+ def rot_vel(self):
+ return 0.
+
+
+class Waypoints(MotionModel):
+
+ def __init__(self, init_pos, waypoints, vel, wp_thresh=0.2):
+ self.waypoints = np.array([init_pos] + waypoints)
+ self.vel = vel
+ self._wp_idx = 0
+ self.wp_thresh = wp_thresh
+ super().__init__(init_pos, 0)
+
+ def lin_vel(self):
+ if not self._wp_idx < len(self.waypoints):
+ return np.zeros(2)
+ dir = self.waypoints[self._wp_idx] - self.pos()
+ wp_dist = np.linalg.norm(dir)
+ if wp_dist < self.wp_thresh:
+ self._wp_idx += 1
+ return self.lin_vel()
+ dir /= wp_dist
+ return self.vel * dir
+
+ def rot_vel(self):
+ return 0.
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/obstacle.py b/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/obstacle.py
new file mode 100644
index 0000000000000000000000000000000000000000..7789611537fe4a9621377a6521a8c1d6ae239f7d
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/obstacle.py
@@ -0,0 +1,201 @@
+from abc import ABC, abstractmethod
+import numpy as np
+import shapely
+from shapely import affinity as sh_affinity
+from utils import affine_transform
+from copy import deepcopy
+from enum import Enum
+
+
+class Frame(Enum):
+ GLOBAL = 1
+ OBSTACLE = 2
+
+ class InvalidFrameError(Exception):
+ pass
+
+
+class Obstacle(ABC):
+ """ Abstract base class of obstacles
+ """
+ id_counter = 0
+
+ # obs_id <0: temp object, obs_id=0: new object, obs_id>0: existing object with id #obs_id
+ def __init__(self, motion_model=None, is_convex=None, is_starshaped=None, id='new', name=None, compute_polygon=False):
+ self._id = None
+ self._is_convex = is_convex
+ self._is_starshaped = is_starshaped
+ # Pose of local frame in global frame
+ self._motion_model = motion_model # if motion_model is not None else mm.Static([0., 0.], 0.)
+ # Initialize id and name
+ self._set_id_name(id, name)
+ self._polygon = None # Polygon in obstacle frame
+ self._polygon_global = None # Polygon in global frame if static obstacle
+ self._polygon_global_pose = None # Obstacle pose corresponding to global polygon
+ if compute_polygon:
+ self._compute_global_polygon_representation()
+
+ def __str__(self): return self._name
+
+ def copy(self, id='temporary', name=None):
+ ob = deepcopy(self)
+ if not (id == 'duplicate' or id == 'd'):
+ ob._set_id_name(id, name)
+ return ob
+
+ def pos(self, output_frame=Frame.GLOBAL):
+ if output_frame == Frame.OBSTACLE or self._motion_model is None:
+ return np.zeros(2)
+ if output_frame == Frame.GLOBAL:
+ return self._motion_model.pos()
+
+ def rot(self, output_frame=Frame.GLOBAL):
+ if output_frame == Frame.OBSTACLE or self._motion_model is None:
+ return 0.
+ if output_frame == Frame.GLOBAL:
+ return self._motion_model.rot()
+
+ def interior_point(self, x, input_frame=Frame.GLOBAL):
+ return True if self.point_location(x, input_frame) < 0 else False
+
+ def exterior_point(self, x, input_frame=Frame.GLOBAL):
+ return True if self.point_location(x, input_frame) > 0 else False
+
+ def boundary_point(self, x, input_frame=Frame.GLOBAL):
+ return np.isclose(self.point_location(x, input_frame), 0.)
+
+ def move(self, dt):
+ if self._motion_model is not None:
+ self._motion_model.move(self, dt)
+
+ def id(self): return self._id
+
+ def polygon(self, output_frame=Frame.GLOBAL):
+ if self._polygon is None:
+ self._compute_polygon_representation()
+ # self._compute_global_polygon_representation()
+ if output_frame == Frame.OBSTACLE or self._motion_model is None:
+ return self._polygon
+ elif output_frame == Frame.GLOBAL:
+ current_pose = [*self._motion_model.pos(), self._motion_model.rot()]
+ if not current_pose == self._polygon_global_pose:
+ self._polygon_global_pose = current_pose
+ c, s = np.cos(current_pose[2]), np.sin(current_pose[2])
+ trans_matrix = np.array([[c, -s, current_pose[0]], [s, c, current_pose[1]], [0, 0, 1]])
+ affinity_matrix = [trans_matrix[0, 0], trans_matrix[0, 1], trans_matrix[1, 0], trans_matrix[1, 1], trans_matrix[0, 2], trans_matrix[1, 2]]
+ self._polygon_global = sh_affinity.affine_transform(self._polygon, affinity_matrix)
+ return self._polygon_global
+ else:
+ raise Frame.InvalidFrameError
+
+ def intersects(self, other):
+ return self.polygon().intersects(other.polygon())
+
+ def transform(self, x, input_frame, output_frame):
+ if input_frame == output_frame or self._motion_model is None:
+ return x
+ elif input_frame == Frame.OBSTACLE and output_frame == Frame.GLOBAL:
+ return self._transform_obstacle2global(x)
+ elif input_frame == Frame.GLOBAL and output_frame == Frame.OBSTACLE:
+ return self._transform_global2obstacle(x)
+ else:
+ raise Frame.InvalidFrameError
+
+ def rotate(self, x, input_frame, output_frame):
+ if input_frame == output_frame or self._motion_model is None:
+ return x
+ elif input_frame == Frame.OBSTACLE and output_frame == Frame.GLOBAL:
+ return self._rotate_obstacle2global(x)
+ elif input_frame == Frame.GLOBAL and output_frame == Frame.OBSTACLE:
+ return self._rotate_global2obstacle(x)
+ else:
+ raise Frame.InvalidFrameError
+
+ def is_convex(self):
+ # Check if convexity already has been computed
+ if self._is_convex is None:
+ self._check_convexity()
+ return self._is_convex
+
+ def is_starshaped(self):
+ if self._is_starshaped is None:
+ if self.is_convex():
+ self._is_starshaped = True
+ else:
+ # TODO: Add check for starshapedness. Currently default to not starshaped
+ self._is_starshaped = False
+ return self._is_starshaped
+
+ def set_motion_model(self, motion_model):
+ self._motion_model = motion_model
+
+ # ------------ Private methods ------------ #
+ def _set_id_name(self, id, name=None):
+ if id == 'new' or id == 'n':
+ Obstacle.id_counter += 1
+ self._id = Obstacle.id_counter
+ elif id == 'temporary' or id == 'temp' or id == 't':
+ self._id = None
+ elif isinstance(id, int) and 0 < id <= Obstacle.id_counter:
+ self._id = id
+ else:
+ print("Invalid id '" + str(id) + "' in set_id. Create temporary obstacle.")
+ self._id = None
+ self._name = name if name else str(self._id)
+
+ def _rotate_obstacle2global(self, x_obstacle):
+ rot = self._motion_model.rot()
+ return affine_transform(x_obstacle, rotation=rot, translation=[0, 0])
+
+ def _rotate_global2obstacle(self, x_global):
+ rot = self._motion_model.rot()
+ return affine_transform(x_global, rotation=rot, translation=[0, 0], inverse=True)
+
+ def _transform_obstacle2global(self, x_obstacle):
+ pos, rot = (self._motion_model.pos(), self._motion_model.rot())
+ return affine_transform(x_obstacle, rotation=rot, translation=pos)
+
+ def _transform_global2obstacle(self, x_global):
+ pos, rot = (self._motion_model.pos(), self._motion_model.rot())
+ return affine_transform(x_global, rotation=rot, translation=pos, inverse=True)
+
+ def _compute_global_polygon_representation(self):
+ self._compute_polygon_representation()
+ if self._motion_model is None:
+ self._polygon_global = self._polygon
+ if self._motion_model.__class__.__name__ == 'Static':
+ pos, rot = (self._motion_model.pos(), self._motion_model.rot())
+ c, s = np.cos(rot), np.sin(rot)
+ trans_matrix = np.array([[c, -s, pos[0]], [s, c, pos[1]], [0, 0, 1]])
+ affinity_matrix = [trans_matrix[0, 0], trans_matrix[0, 1], trans_matrix[1, 0], trans_matrix[1, 1],
+ trans_matrix[0, 2], trans_matrix[1, 2]]
+ self._polygon_global = shapely.affinity.affine_transform(self._polygon, affinity_matrix)
+
+ # ------------ Abstract methods ------------ #
+ @abstractmethod
+ def draw(self):
+ pass
+
+ @abstractmethod
+ def point_location(self, x, input_frame=Frame.GLOBAL):
+ pass
+
+ @abstractmethod
+ def dilated_obstacle(self, padding, id="new", name=None):
+ pass
+
+ @abstractmethod
+ def line_intersection(self, line, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
+ pass
+
+ @abstractmethod
+ def tangent_points(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
+ pass
+
+ @abstractmethod
+ def _check_convexity(self):
+ pass
+
+ @abstractmethod
+ def _compute_polygon_representation(self):
+ pass
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/polygon.py b/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/polygon.py
new file mode 100644
index 0000000000000000000000000000000000000000..d8ec63a62b891136497967a50f588d08dd1b8343
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/polygon.py
@@ -0,0 +1,158 @@
+from obstacles import Obstacle, Frame
+from utils import is_cw, is_ccw, is_collinear, tic, toc
+import shapely
+import numpy as np
+import matplotlib.pyplot as plt
+import matplotlib.patches as patches
+
+
+class Polygon(Obstacle):
+
+ def __init__(self, polygon, **kwargs):
+ super().__init__(**kwargs)
+ self._polygon = shapely.geometry.Polygon(polygon)
+ self._polygon = shapely.ops.orient(self._polygon)
+ self._pol_bounds = self._polygon.bounds
+ self._compute_global_polygon_representation()
+ self.vertices = np.array(self._polygon.exterior.coords[:-1])
+ self.circular_vertices = np.array(self._polygon.exterior.coords)
+
+ def init_plot(self, ax=None, show_name=False, **kwargs):
+ if ax is None:
+ _, ax = plt.subplots(subplot_kw={'aspect': 'equal'})
+ if "fc" not in kwargs and "facecolor" not in kwargs:
+ kwargs["fc"] = 'lightgrey'
+ if 'show_reference' in kwargs:
+ del kwargs['show_reference']
+ line_handles = []
+ # Boundary
+ line_handles += [patches.Polygon(np.random.rand(3, 2), **kwargs)]
+ ax.add_patch(line_handles[-1])
+ # Name
+ line_handles += [ax.text(0, 0, self._name)] if show_name else [None]
+ return line_handles, ax
+
+ def extreme_points(self, frame=Frame.GLOBAL):
+ vertices = np.asarray(self.polygon(frame).exterior.coords)[:-1, :]
+ return [vertices[i] for i in range(vertices.shape[0])]
+
+ def update_plot(self, line_handles, frame=Frame.GLOBAL):
+ polygon = self.polygon(frame)
+ boundary = np.vstack((polygon.exterior.xy[0], polygon.exterior.xy[1])).T
+ line_handles[0].set_xy(boundary)
+ if line_handles[1] is not None:
+ line_handles[1].set_position(self.pos(frame))
+
+ def draw(self, frame=Frame.GLOBAL, **kwargs):
+ line_handles, ax = self.init_plot(**kwargs)
+ self.update_plot(line_handles, frame)
+ return line_handles, ax
+
+ def dilated_obstacle(self, padding, id="new", name=None):
+ cp = self.copy(id, name)
+ cp._polygon = cp._polygon.buffer(padding, cap_style=1, join_style=1)
+ cp._pol_bounds = cp._polygon.bounds
+ cp.vertices = np.array(cp._polygon.exterior.coords[:-1])
+ cp.circular_vertices = np.array(cp._polygon.exterior.coords)
+ cp._polygon_global_pose = None
+ cp._polygon_global = None
+ return cp
+
+ def point_location(self, x, input_frame=Frame.GLOBAL):
+ x_obstacle = self.transform(x, input_frame, Frame.OBSTACLE)
+ xmin, ymin, xmax, ymax = self._pol_bounds
+ if not (xmin < x_obstacle[0] < xmax and ymin < x_obstacle[1] < ymax):
+ return 1
+ x_sh = shapely.geometry.Point(x_obstacle)
+ if self._polygon.contains(x_sh):
+ return -1
+ if self._polygon.exterior.contains(x_sh):
+ return 0
+ return 1
+
+ def line_intersection(self, line, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
+ l0_obstacle = self.transform(line[0], input_frame, Frame.OBSTACLE)
+ l1_obstacle = self.transform(line[1], input_frame, Frame.OBSTACLE)
+ intersection_points_shapely = shapely.geometry.LineString([l0_obstacle, l1_obstacle]).intersection(self._polygon.exterior)
+ if intersection_points_shapely.is_empty:
+ return []
+ if intersection_points_shapely.geom_type == 'Point':
+ intersection_points_obstacle = [np.array([intersection_points_shapely.x, intersection_points_shapely.y])]
+ elif intersection_points_shapely.geom_type == 'MultiPoint':
+ intersection_points_obstacle = [np.array([p.x, p.y]) for p in intersection_points_shapely.geoms]
+ elif intersection_points_shapely.geom_type == 'LineString':
+ intersection_points_obstacle = [np.array([ip[0], ip[1]]) for ip in intersection_points_shapely.coords]
+ elif intersection_points_shapely.geom_type == 'MultiLineString':
+ intersection_points_obstacle = [np.array([ip[0], ip[1]]) for line in intersection_points_shapely.geoms for ip in line.coords]
+ else:
+ print(intersection_points_shapely)
+ return [self.transform(ip, Frame.OBSTACLE, output_frame) for ip in intersection_points_obstacle]
+
+ def tangent_points(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
+
+ x_obstacle = self.transform(x, input_frame, Frame.OBSTACLE)
+ t0 = tic()
+ phi = np.arctan2(self.circular_vertices[:, 1] - x_obstacle[1], self.circular_vertices[:, 0] - x_obstacle[0])
+ phi[phi < 0] += 2 * np.pi
+ t1 = toc(t0)
+ t0 = tic()
+ phi_diff = np.diff(phi)
+ t2 = toc(t0)
+ t0 = tic()
+ phi_decrease_idcs = phi_diff > np.pi
+ phi_increase_idcs = phi_diff < -np.pi
+ t3 = toc(t0)
+ t0 = tic()
+ phi_decrease_idcs = np.flatnonzero(phi_decrease_idcs)
+ phi_increase_idcs = np.flatnonzero(phi_increase_idcs)
+ for i in phi_decrease_idcs:
+ phi[i+1:] -= 2*np.pi
+ for i in phi_increase_idcs:
+ phi[i+1:] += 2*np.pi
+ t4 = toc(t0)
+
+ t0 = tic()
+
+ i_min, i_max = np.argmin(phi), np.argmax(phi)
+
+ if abs(phi[0] - phi[-1]) > 0.00001:
+ # Interior point
+ return []
+ if (phi[i_max] - phi[i_min]) >= 2*np.pi:
+ # Blocked exterior point
+ return []
+ t5 = toc(t0)
+
+ t0 = tic()
+ tp1_obstacle = self.circular_vertices[i_max]
+ tp2_obstacle = self.circular_vertices[i_min]
+
+ tp1 = self.transform(tp1_obstacle, Frame.OBSTACLE, output_frame)
+ tp2 = self.transform(tp2_obstacle, Frame.OBSTACLE, output_frame)
+
+ tend = toc(t0)
+ # print(sum([t1*100,t2*100,t3*100,t4*100,t5*100,tend*100*0]))
+ return [tp1, tp2]
+
+ def area(self):
+ return self._polygon.area
+
+ # ------------ Private methods ------------ #
+ def _check_convexity(self):
+ v = np.asarray(self._polygon.exterior.coords)[:-1, :]
+ i = 0
+ N = v.shape[0]
+ # Make sure first vertice is not collinear
+ while is_collinear(v[i-1, :], v[i, :], v[(i+1) % N, :]):
+ i += 1
+ if i > N:
+ raise RuntimeError("Bad polygon shape. All vertices collinear")
+ # All vertices must be either cw or ccw when iterating through for convexity
+ if is_cw(v[i-1, :], v[i, :], v[i+1, :]):
+ self._is_convex = not any([is_ccw(v[j-1, :], v[j, :], v[(j+1) % N, :]) for j in range(v.shape[0])])
+ else:
+ self._is_convex = not any([is_cw(v[j-1, :], v[j, :], v[(j+1) % N, :]) for j in range(v.shape[0])])
+
+ # Not needed
+ def _compute_polygon_representation(self):
+ pass
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/starshaped_obstacle.py b/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/starshaped_obstacle.py
new file mode 100644
index 0000000000000000000000000000000000000000..122ab8f02c24f2b1c0d57754353c639f4d5a3fae
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/starshaped_obstacle.py
@@ -0,0 +1,85 @@
+from abc import abstractmethod
+import numpy as np
+from obstacles import Obstacle, Frame
+from shapely import affinity as sh_affinity
+
+
+class StarshapedObstacle(Obstacle):
+
+ def __init__(self, xr, **kwargs):
+ super().__init__(is_starshaped=True, **kwargs)
+ self._xr = np.array(xr) # Reference point in obstacle frame
+ self._kernel = None
+ self._kernel_global = None # Kernel in global frame
+ self._kernel_global_pose = None # Obstacle pose corresponding to global kernel
+
+ def xr(self, output_frame=Frame.GLOBAL):
+ return self.transform(self._xr, Frame.OBSTACLE, output_frame)
+
+ def set_xr(self, xr, input_frame=Frame.OBSTACLE, safe_set=False):
+ new_xr = self.transform(xr, input_frame, Frame.OBSTACLE)
+ if safe_set:
+ k = self.kernel()
+ if not k.exterior_point(new_xr, Frame.OBSTACLE):
+ self._xr = new_xr
+ else:
+ self._xr = new_xr
+
+ def reference_direction(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
+ dir = self.transform(x, input_frame, output_frame) - self.transform(self._xr, Frame.OBSTACLE, output_frame)
+ if not np.any(dir):
+ print("reference_direction for xr is not defined")
+ return dir / np.linalg.norm(dir, axis=x.ndim - 1)
+
+ # interior point: <1. exterior point: >1. boundary point: 1.
+ def distance_function(self, x, input_frame=Frame.GLOBAL):
+ x_obstacle = self.transform(x, input_frame, Frame.OBSTACLE)
+ dist_func = (np.linalg.norm(x_obstacle - self._xr, axis=x.ndim - 1) / (
+ np.linalg.norm(self.boundary_mapping(x_obstacle, input_frame=Frame.OBSTACLE, output_frame=Frame.OBSTACLE)
+ - self._xr, axis=x.ndim - 1))) ** 2
+ return dist_func
+
+ # interior point: <0. exterior point: >0. boundary point: 0.
+ def point_location(self, x, input_frame=Frame.GLOBAL):
+ return np.sign(self.distance_function(x, input_frame)-1.)
+
+ def kernel(self, output_frame=Frame.GLOBAL):
+ if self._kernel is None:
+ self._compute_kernel()
+ if output_frame == Frame.OBSTACLE or self._motion_model is None:
+ return self._kernel
+ elif output_frame == Frame.GLOBAL:
+ current_pose = [*self._motion_model.pos(), self._motion_model.rot()]
+ if not current_pose == self._kernel_global_pose:
+ self._kernel_global_pose = current_pose
+ c, s = np.cos(current_pose[2]), np.sin(current_pose[2])
+ trans_matrix = np.array([[c, -s, current_pose[0]], [s, c, current_pose[1]], [0, 0, 1]])
+ affinity_matrix = [trans_matrix[0, 0], trans_matrix[0, 1], trans_matrix[1, 0], trans_matrix[1, 1], trans_matrix[0, 2], trans_matrix[1, 2]]
+ self._kernel_global = sh_affinity.affine_transform(self._kernel, affinity_matrix)
+ return self._kernel_global
+ else:
+ raise Frame.InvalidFrameError
+
+ def vel_intertial_frame(self, x):
+ if self._motion_model is None:
+ return np.zeros(2)
+ omega = self._motion_model.rot_vel()
+ lin_vel_omega = np.cross(np.hstack(([0, 0], omega)), np.hstack((self.reference_direction(x), 0)))[:2]
+ return self._motion_model.lin_vel() + lin_vel_omega
+
+ # ------------ Abstract methods ------------ #
+ @abstractmethod
+ def normal(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL, x_is_boundary=False):
+ pass
+
+ @abstractmethod
+ def boundary_mapping(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
+ pass
+
+ # @abstractmethod
+ # def signed_boundary_distance(self, x, input_frame=Frame.GLOBAL):
+ # pass
+
+ @abstractmethod
+ def _compute_kernel(self):
+ pass
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/starshaped_polygon.py b/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/starshaped_polygon.py
new file mode 100644
index 0000000000000000000000000000000000000000..85eba3ce218671de85d625c612a6fa9494377efe
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/starshaped_polygon.py
@@ -0,0 +1,866 @@
+from obstacles import Frame, StarshapedObstacle, Polygon
+import matplotlib.pyplot as plt
+import numpy as np
+import shapely
+from utils import is_cw, is_ccw, is_collinear, orientation_val, get_intersection, is_between, tic, toc
+
+
+class StarshapedPolygon(Polygon, StarshapedObstacle):
+
+ def __init__(self, polygon, xr=None, **kwargs):
+ super().__init__(polygon, xr=xr, **kwargs)
+ if xr is None:
+ self._compute_kernel()
+ if self._kernel.contains(self._kernel.centroid):
+ self._xr = np.array(self._kernel.centroid.coords[0])
+ else:
+ self._xr = np.array(self._kernel.representative_point().coords[0])
+ else:
+ self._xr = np.array(xr)
+ self.vertex_angles = None
+ self._update_vertex_angles()
+ self.enclosing_ball_diameter = self._polygon.bounds[2]-self._polygon.bounds[0] + self._polygon.bounds[3]-self._polygon.bounds[1]
+
+ def copy(self, id, name):
+ if (id == 'duplicate' or id == 'd'):
+ id = self.id()
+ return StarshapedPolygon(id=id, name=name, polygon=self._polygon, xr=self._xr, motion_model=self._motion_model)
+
+ # Note: Does not recompute the kernel
+ def dilated_obstacle(self, padding, id="new", name=None):
+ cp = self.copy(id, name)
+ cp._polygon = cp._polygon.buffer(padding, cap_style=1, join_style=1)
+ cp._pol_bounds = cp._polygon.bounds
+ cp.enclosing_ball_diameter = max(cp._polygon.bounds[2]-cp._polygon.bounds[0], cp._polygon.bounds[3]-cp._polygon.bounds[1])
+ cp.vertices = np.array(cp._polygon.exterior.coords[:-1])
+ cp.circular_vertices = np.array(cp._polygon.exterior.coords)
+ cp._polygon_global_pose = None
+ cp._polygon_global = None
+ cp._update_vertex_angles()
+ return cp
+
+ def distance_function(self, x, input_frame=Frame.GLOBAL):
+ x_obstacle = self.transform(x, input_frame, Frame.OBSTACLE)
+ dist_center = np.linalg.norm(x_obstacle - self._xr, axis=x.ndim - 1)
+ local_radius = np.linalg.norm(self.boundary_mapping(x_obstacle, input_frame=Frame.OBSTACLE, output_frame=Frame.OBSTACLE)
+ - self._xr, axis=x.ndim - 1)
+ if dist_center < local_radius:
+ # Return proportional inside to have -> [0, 1]
+ return (dist_center / local_radius) ** 2
+ else:
+ return ((dist_center - local_radius) + 1) ** 2
+
+
+ def boundary_mapping(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
+ x_obstacle = self.transform(x, input_frame, Frame.OBSTACLE)
+ intersect_obstacle = self.line_intersection([self._xr, self._xr+1.1*self.enclosing_ball_diameter*self.reference_direction(x_obstacle, Frame.OBSTACLE, Frame.OBSTACLE)],
+ input_frame=Frame.OBSTACLE, output_frame=Frame.OBSTACLE)
+ if not intersect_obstacle:
+ return None
+ if len(intersect_obstacle) == 1:
+ return self.transform(intersect_obstacle[0], Frame.OBSTACLE, output_frame)
+ else:
+ if np.linalg.norm(intersect_obstacle[0]-x_obstacle) < np.linalg.norm(intersect_obstacle[1]-x_obstacle):
+ return self.transform(intersect_obstacle[0], Frame.OBSTACLE, output_frame)
+ else:
+ return self.transform(intersect_obstacle[1], Frame.OBSTACLE, output_frame)
+
+ def normal(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL, x_is_boundary=False, type='edge'):
+ x_obstacle = self.transform(x, input_frame, Frame.OBSTACLE)
+ angle = np.arctan2(x_obstacle[1]-self._xr[1], x_obstacle[0]-self._xr[0])
+ v_idx = np.argmax(self.vertex_angles > angle)
+
+ if v_idx == self.vertices.shape[0]:
+ v_idx = 0
+
+ if type == 'edge':
+ n_obstacle = np.array([self.vertices[v_idx, 1] - self.vertices[v_idx - 1, 1], self.vertices[v_idx - 1, 0] - self.vertices[v_idx, 0]])
+ n_obstacle /= np.linalg.norm(n_obstacle)
+ elif type == 'weighted_edges':
+ edge_neighbors = [(self.vertices[(v_idx - 2 + i) % self.vertices.shape[0]], self.vertices[(v_idx - 1 + i) % self.vertices.shape[0]]) for i in range(3)]
+ edge_neighbors_normal = np.array([[e[1][1] - e[0][1],
+ e[0][0] - e[1][0]] for e in edge_neighbors])
+ edge_closest = [shapely.ops.nearest_points(shapely.geometry.LineString(e),
+ shapely.geometry.Point(x_obstacle))[0].coords[0] for e in edge_neighbors]
+
+ dist = [np.linalg.norm(np.array(e)-x_obstacle) for e in edge_closest]
+ w = np.array([1/(d+1e-6) for d in dist])
+ w /= sum(w)
+ n_obstacle = edge_neighbors_normal.T.dot(w)
+ return self.rotate(n_obstacle, Frame.OBSTACLE, output_frame)
+
+
+ #directional weighted mean (see Appendix A) of normal vectors of the surface tiles ni(ξ), the weights wi,
+ # and with respect to the reference direction r
+ # n_vert = self.vertices.shape[0]
+ # r = self.reference_direction(x, input_frame=Frame.OBSTACLE, output_frame=Frame.OBSTACLE)
+ # B = np.array((r, (-r[1], r[0])))
+ # ws = np.zeros(n_vert)
+ # kappa = np.zeros(n_vert)
+ # p = 3
+ # vi_min = np.inf
+ #
+ # edge_r = np.zeros((n_vert, 2))
+ # _, ax = self.draw(frame=Frame.OBSTACLE)
+ # ax.plot(*x_obstacle, 'ko')
+ # for i in range(n_vert):
+ # # surface_i = np.array([self.vertices[v_idcs[i] - 1, :], self.vertices[v_idcs[i], :]])
+ # # edge_normal_i = np.array([-(surface_i[0, 1] - surface_i[1, 1]), surface_i[0, 0] - surface_i[1, 0]])
+ # edge_normal_i = np.array([self.vertices[i, 1] - self.vertices[i - 1, 1], self.vertices[i - 1, 0] - self.vertices[i, 0]])
+ # edge_normal_i /= np.linalg.norm(edge_normal_i)
+ # # rotated_edge_normal_i = Rpn.T.dot(edge_normal_i)
+ #
+ # pi = np.array(
+ # shapely.ops.nearest_points(shapely.geometry.LineString([self.vertices[i-1, :], self.vertices[i, :]]),
+ # shapely.geometry.Point(x_obstacle))[0].coords[0])
+ # # ax.quiver(*pi, *edge_normals[i, :], color='k')
+ # edge_r[i, :] = [np.mean([self.vertices[i, 0], self.vertices[i - 1, 0]]),
+ # np.mean([self.vertices[i, 1], self.vertices[i - 1, 1]])]
+ # vi = x_obstacle - pi
+ # ei = (vi - edge_normal_i.dot(vi)*edge_normal_i) * np.sign(vi.dot(x_obstacle-edge_r[i, :]))
+ # phi = np.arccos(ei.dot(vi) / (np.linalg.norm(ei) * np.linalg.norm(vi))) * np.sign(edge_normal_i.dot(vi))
+ #
+ # if phi > 0:
+ # ws[i] = 0
+ # else:
+ # ws[i] = (np.pi / phi) ** p - 1
+ #
+ # # if np.linalg.norm(vi) < vi_min:
+ # # ws = np.zeros(n_vert)
+ # # ws[i] = 1
+ # # vi_min = np.linalg.norm(vi)
+ #
+ #
+ #
+ # # if np.all(np.isclose(pi, x_obstacle)):
+ # # ws[i] = -1
+ # # else:
+ # # ws[i] = 1 / np.linalg.norm(x_obstacle - pi) ** 2
+ # rotated_edge_normal_i = B.T.dot(edge_normal_i)
+ #
+ # kappa[i] = 0 if rotated_edge_normal_i[0] == 1 else np.arccos(rotated_edge_normal_i[0]) * np.sign(
+ # rotated_edge_normal_i[1])
+ #
+ # # v_idx += 1
+ # # if np.any(ws < 0):
+ # # ws[np.nonzero(ws > 0)] = 0
+ # # ws[np.nonzero(ws)] = 1
+ # #
+ # ws = ws / np.sum(ws)
+ # # n_obstacle2 =
+ #
+ # kappa_bar = ws.dot(kappa)
+ # tmp_vec = [np.cos(abs(kappa_bar)), np.sin(abs(kappa_bar)) * np.sign(kappa_bar)]
+ # n_obstacle = B.dot(tmp_vec)
+ #
+ # return self.rotate(n_obstacle, Frame.OBSTACLE, output_frame)
+
+ def set_xr(self, xr, input_frame=Frame.OBSTACLE, safe_set=False):
+ super().set_xr(xr, input_frame, safe_set)
+ self._update_vertex_angles()
+
+ def init_plot(self, ax=None, show_reference=True, show_name=False, **kwargs):
+ line_handles, ax = super().init_plot(ax=ax, show_name=show_name, **kwargs)
+ if ax is None:
+ _, ax = plt.subplots(subplot_kw={'aspect': 'equal'})
+ # Reference point
+ line_handles += ax.plot(0, 0, '+', color='k') if show_reference else [None]
+ return line_handles, ax
+
+ def update_plot(self, line_handles, frame=Frame.GLOBAL):
+ super().update_plot(line_handles, frame)
+ if line_handles[1] is not None:
+ line_handles[1].set_position(self.xr(frame))
+ if line_handles[2] is not None:
+ line_handles[2].set_data(*self.xr(frame))
+
+ def _update_vertex_angles(self):
+ self.vertex_angles = np.arctan2(self.vertices[:, 1] - self._xr[1], self.vertices[:, 0] - self._xr[0])
+ idcs = np.argsort(self.vertex_angles)
+ self.vertex_angles = self.vertex_angles[idcs]
+ self.vertices = self.vertices[idcs, :]
+ self.circular_vertices = np.vstack((self.vertices, self.vertices[0, :]))
+ self.vertex_angles = np.hstack((self.vertex_angles, self.vertex_angles[0] + 2 * np.pi))
+
+ def _compute_kernel(self, verbose=False, debug=False):
+ if self.is_convex():
+ self._kernel = self._polygon
+ return
+
+ # Returns true if vertex v[i, :] is reflex
+ def is_reflex(i):
+ return is_cw(v[i - 1, :], v[i, :], v[(i + 1) % v.shape[0], :])
+
+ # Show polygon
+ def draw(xk, F1_idx, L1_idx, i, xk_bounded):
+ v_ext = np.vstack((v, v[0, :]))
+ xk_ext = np.vstack((xk, xk[0, :])) if xk_bounded else xk
+ plt.plot(v_ext[:, 0], v_ext[:, 1], label='v', marker='.')
+ # plt.plot(v_ext[:, 0], v_ext[:, 1], label='v')
+ # plt.text(v_ext[0, 0], v_ext[0, 1], 'v0')
+ plt.text(xk[0, 0], xk[0, 1], 'xk0')
+ axes = plt.gca()
+ xlim, ylim = axes.get_xlim(), axes.get_ylim()
+ plt.plot(xk_ext[:, 0], xk_ext[:, 1], label='xk')
+ plt.plot(xk[F1_idx, 0], xk[F1_idx, 1], marker='o', c='c', label='F1')
+ end_point = v[i, :] + INF_VAL * (xk[F1_idx, :] - v[i, :]) / np.linalg.norm(xk[F1_idx, :] - v[i, :])
+ plt.plot([v[i, 0], end_point[0]], [v[i, 1], end_point[1]], 'c--')
+ plt.plot(xk[L1_idx, 0], xk[L1_idx, 1], marker='o', c='b', label='L1')
+ end_point = v[i, :] + INF_VAL * (xk[L1_idx, :] - v[i, :]) / np.linalg.norm(xk[L1_idx, :] - v[i, :])
+ plt.plot([v[i, 0], end_point[0]], [v[i, 1], end_point[1]], 'b--')
+ plt.plot(v[i, 0], v[i, 1], marker='x', c='r', ms=12, label='v{}'.format(i))
+ axes.set_xlim(xlim)
+ axes.set_ylim(ylim)
+
+ def point_left_of_line(point, line_head, line_tail):
+ return is_ccw(line_head, line_tail, point)
+
+ def point_right_of_line(point, line_head, line_tail):
+ return is_cw(line_head, line_tail, point)
+
+ pol = shapely.ops.orient(self._polygon)
+ v = np.asarray(pol.exterior.coords)[:-1, :]
+
+ # Remove points in line
+ v_idcs = np.arange(v.shape[0])
+ for i in range(v.shape[0]):
+ if is_collinear(v[v_idcs[i - 2], :], v[v_idcs[i - 1], :], v[v_idcs[i], :]):
+ v_idcs[i - 1] = v_idcs[i - 2]
+ v_idcs = np.unique(v_idcs)
+ v = v[v_idcs, :]
+ if is_collinear(v[-2, :], v[-1, :], v[0, :]):
+ v = v[:-1]
+
+ N = v.shape[0]
+ for i in range(N):
+ # Order v to have v[:, 0] as reflex vertex
+ if is_reflex(i):
+ v = np.vstack((v[i:, :], v[:i, :]))
+ break
+
+ INF_VAL = 300.
+ # Initial step
+ F1 = v[0, :] + INF_VAL * (v[0, :] - v[1, :]) / np.linalg.norm(v[0, :] - v[1, :])
+ L1 = v[0, :] + INF_VAL * (v[0, :] - v[-1, :]) / np.linalg.norm(v[0, :] - v[-1, :])
+ xk = np.vstack((F1, v[0, :], L1))
+ F1_idx = 0 # Index of point F1 in xk
+ L1_idx = 2 # Index of point L1 in xk
+ xk_bounded = False
+
+ if verbose:
+ print("------")
+ print(0)
+ print("F1:", end=" ")
+ print(F1)
+ print("L1:", end=" ")
+ print(L1)
+ if debug:
+ draw(xk, F1_idx, L1_idx, 0, xk_bounded)
+ plt.show()
+
+ for i in range(1, N):
+
+ if verbose:
+ print("------")
+ print(i)
+ print("F1 index: {}".format(F1_idx))
+ print("L1 index: {}".format(L1_idx))
+
+ L1 = xk[L1_idx, :]
+ F1 = xk[F1_idx, :]
+ vi = v[i, :]
+ vi_1 = v[(i + 1) % N, :]
+
+ # (1) vi is reflex
+ if is_reflex(i):
+ # (1.2) F1 lies to the left of line ->(vi->vi+1)->vi+1
+ start_point = vi - INF_VAL * (vi_1 - vi) / np.linalg.norm(vi_1 - vi)
+ if point_left_of_line(F1, start_point, vi_1):
+ case = 2
+ if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ plt.plot([start_point[0], vi_1[0]], [start_point[1], vi_1[1]], marker='*', color='y',
+ label="->(v{}->v{})->v{}".format(i, i + 1, i + 1))
+ plt.title("F1 lies to the left of line ->(v{}->v{})->v{}".format(i, i + 1, i + 1))
+ plt.show()
+ if verbose:
+ print("(1.2) - F1 ({},{}) lies to the left of line ->(v{}->v{})->v{}".format(F1[0], F1[0], i,
+ i + 1, i + 1))
+ # (1.1) F1 lies on or to the right of line ->(vi->vi+1)->vi+1
+ else:
+ case = 1
+ if verbose:
+ print(
+ "(1.1) - F1 ({},{}) lies on or to the right of line ->(v{}->v{})->v{}".format(F1[0], F1[0],
+ i, i + 1,
+ i + 1))
+ print("Scan xk ccw from F1 until we reach edge intersecting line ->(v{}->v{})->v{}".format(i,
+ i + 1,
+ i + 1))
+ # Scan xk ccw from F1 to L1 until we reach edge intersecting line ->(vi->vi+1)->vi+1
+ w1 = None
+ idx_offsets = range(1, xk.shape[0] + 1)
+ for off in idx_offsets:
+ t = (F1_idx + off) % xk.shape[0]
+ if not xk_bounded and t == 0:
+ break
+ # Get intersection w1 of current edge and line ->(vi->vi+1)->vi+1
+ wt_prev = xk[t - 1, :]
+ wt = xk[t, :]
+ w1 = get_intersection(wt_prev, wt, start_point, vi_1)
+ if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ col = 'r' if w1 is None else 'g'
+ plt.plot([wt_prev[0], wt[0]], [wt_prev[1], wt[1]], marker='*', color=col, label="edge")
+ plt.plot([start_point[0], vi_1[0]], [start_point[1], vi_1[1]], marker='*', color='y',
+ label="->(v{}->v{})->v{}".format(i, i + 1, i + 1))
+ plt.title(
+ "Scan xk ccw from F1 until we reach edge intersecting line ->(v{}->v{})->v{}".format(i,
+ i + 1,
+ i + 1))
+ if w1 is not None:
+ if debug:
+ plt.plot(w1[0], w1[1], 's', label="w1", color='g')
+ plt.legend()
+ plt.show()
+ break
+ if debug:
+ plt.legend()
+ plt.show()
+
+ if t == L1_idx:
+ break
+ # If no intersecting line is reached no kernel exists
+ if w1 is None:
+ # if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ plt.title('No kernel found.. Polygon not starshaped.')
+ plt.legend()
+ plt.show()
+ return False
+ if verbose:
+ print("Found intersection ({},{}) at line ->(v{}->v{})->v{}".format(w1[0], w1[1], i, i + 1,
+ i + 1))
+ print("Scan xk cw from F1 until we reach edge intersecting line ->(v{}->v{})->v{}".format(i,
+ i + 1,
+ i + 1))
+ # Scan xk cw from F1 until we reach edge intersecting line ->(vi->vi+1)->vi+1
+ w2 = None
+ idcs_list = np.flip(np.roll(np.arange(xk.shape[0]), -F1_idx - 1)) if xk_bounded else range(F1_idx,
+ 0, -1)
+ # for s in range(F1_idx, 0, -1):
+ for s in idcs_list:
+ ws = xk[s, :]
+ ws_prev = xk[s - 1, :]
+ # Get intersection w2 of edge and line ->(vi->vi+1)->vi+1
+ w2 = get_intersection(ws_prev, ws, start_point, vi_1)
+
+ if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ plt.plot(w1[0], w1[1], 's', label="w1", color='g')
+ col = 'r' if w2 is None else 'g'
+ plt.plot([ws_prev[0], ws[0]], [ws_prev[1], ws[1]], marker='*', color=col, label="edge")
+ plt.plot([start_point[0], vi_1[0]], [start_point[1], vi_1[1]], marker='*', color='y',
+ label="->(v{}->v{})->v{}".format(i, i + 1, i + 1))
+ plt.title(
+ "Scan xk cw from F1 until we reach edge intersecting line ->(v{}->v{})->v{}".format(i,
+ i + 1,
+ i + 1))
+
+ if w2 is not None:
+ if xk_bounded and s > t:
+ alpha = xk[xk.shape[0]:, :] # Empty array
+ beta = xk[t:s, :]
+ L1_idx = L1_idx - t + 2
+ else:
+ alpha = xk[:s, :]
+ beta = xk[t:, :]
+ L1_idx = L1_idx - t + s + 2
+ if debug:
+ print("xk: ", end=" ")
+ print(xk)
+ print("alpha: ", end=" ")
+ print(alpha)
+ print("beta: ", end=" ")
+ print(beta)
+ print("w1: ", end=" ")
+ print(w1)
+ print("w2: ", end=" ")
+ print(w2)
+ plt.plot(w1[0], w1[1], 's', label="w1", color='k')
+ plt.plot(w2[0], w2[1], 's', label="w2", color='g')
+ plt.plot(alpha[:, 0], alpha[:, 1], 'r--', label="alpha")
+ plt.plot(beta[:, 0], beta[:, 1], 'k--', label="beta")
+ plt.legend()
+ plt.show()
+ xk = np.vstack((alpha, w2, w1, beta))
+ w1_idx = alpha.shape[0] + 1
+ w2_idx = alpha.shape[0]
+ ####### F1 index reassignment should not be necessary for this case
+ F1_idx = w2_idx
+ # L1_idx = L1_idx - t + s + 2
+
+ if verbose:
+ print("Update 1")
+ print("Found intersection ({},{}) at line ->(v{}->v{})->v{}".format(w2[0], w2[1], i,
+ i + 1, i + 1))
+ break
+ if debug:
+ plt.legend()
+ plt.show()
+ # If no intersecting line is reached
+ if w2 is None:
+ # Test if xk+1 is bounded
+ # If slope ->(vi->vi+1)->vi+1 is comprised between the slopes of initial and final half lines of xk,
+ if (orientation_val(xk[-2, :], xk[-1, :], start_point) * orientation_val(vi_1, start_point,
+ xk[0, :])) > 0:
+ beta = xk[t:, :]
+ if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ plt.plot(w1[0], w1[1], 's', label="w1", color='g')
+ plt.plot(xk[:2, 0], xk[:2, 1], '--c', label="initial half line")
+ plt.plot(xk[-2:, 0], xk[-2:, 1], '--m', label="final half line")
+ plt.plot([start_point[0], vi_1[0]], [start_point[1], vi_1[1]], marker='*', color='y',
+ label="->(v{}->v{})->v{}".format(i, i + 1, i + 1))
+ plt.title(
+ "->(v{}->v{})->v{} between initial and finial half lines of xk".format(
+ i, i + 1, i + 1))
+ plt.plot(beta[:, 0], beta[:, 1], 'k--', label="beta")
+ plt.legend()
+ plt.show()
+ if verbose:
+ print("Update 2 - xk still unbounded")
+ # then xk+1= ->(vi->vi+1)->w1->beta is also unbounded.
+ xk = np.vstack((start_point, w1, beta))
+ w1_idx = 1
+ # xk_bounded = False
+ F1_idx = 0
+ L1_idx -= t - 1
+
+
+ else:
+ # otherwise scan xk cw from xk[-1,:] until we reach edge intersecting line ->(vi->vi+1)->vi+1
+ if verbose:
+ print(
+ "Scan xk cw from end until we reach edge intersecting line ->(v{}->v{})->v{}".format(
+ i, i + 1, i + 1))
+ w2 = None
+ if xk_bounded:
+ r = xk.shape[0]
+ w2 = get_intersection(xk[0, :], xk[r - 1, :], start_point, vi_1)
+ if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ plt.plot(w1[0], w1[1], 's', label="w1", color='g')
+ col = 'r' if w2 is None else 'g'
+ plt.plot([xk[r - 1, 0], xk[0, 0]], [xk[r - 1, 1], xk[0, 1]], marker='*', color=col,
+ label="edge")
+ plt.plot([start_point[0], vi_1[0]], [start_point[1], vi_1[1]], marker='*',
+ color='y',
+ label="->(v{}->v{})->v{}".format(i, i + 1, i + 1))
+ plt.title(
+ "Scan xk cw from xk[-1, :] until we reach edge intersecting line ->(v{}->v{})->v{}".format(
+ i, i + 1, i + 1))
+ if w2 is not None:
+ delta = xk[t:r, :]
+ if debug:
+ print(t, r)
+ plt.plot(delta[:, 0], delta[:, 1], 'k--', label="delta")
+ plt.plot(w1[0], w1[1], 's', label="w1", color='k')
+ plt.plot(w2[0], w2[1], 's', label="w2", color='g')
+ plt.legend()
+ plt.show()
+ if debug:
+ plt.legend()
+ plt.show()
+ if w2 is None:
+ for r in range(xk.shape[0] - 1, 0, -1):
+ # Get intersection w2 of edge and line ->(vi->vi+1)->vi+1
+ w2 = get_intersection(xk[r, :], xk[r - 1, :], start_point, vi_1)
+ if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ plt.plot(w1[0], w1[1], 's', label="w1", color='g')
+ col = 'r' if w2 is None else 'g'
+ plt.plot([xk[r - 1, 0], xk[r, 0]], [xk[r - 1, 1], xk[r, 1]], marker='*',
+ color=col, label="edge")
+ plt.plot([start_point[0], vi_1[0]], [start_point[1], vi_1[1]], marker='*',
+ color='y', label="->(v{}->v{})->v{}".format(i, i + 1, i + 1))
+ plt.title(
+ "Scan xk cw from xk[-1, :] until we reach edge intersecting line ->(v{}->v{})->v{}".format(
+ i, i + 1, i + 1))
+ if w2 is not None:
+ delta = xk[t:r, :]
+ if debug:
+ print(t, r)
+ plt.plot(delta[:, 0], delta[:, 1], 'k--', label="delta")
+ plt.plot(w1[0], w1[1], 's', label="w1", color='k')
+ plt.plot(w2[0], w2[1], 's', label="w2", color='g')
+ plt.legend()
+ plt.show()
+ break
+ if debug:
+ plt.legend()
+ plt.show()
+ if verbose:
+ print("Update 3")
+ print("Found intersection ({},{}) at line ->(v{}->v{})->v{}".format(w2[0], w2[1], i,
+ i + 1, i + 1))
+ # Set xk as delta-w2-w1
+ xk = np.vstack((delta, w2, w1))
+ w1_idx = delta.shape[0] + 1
+ w2_idx = delta.shape[0]
+ xk_bounded = True
+
+ F1_idx = 0
+ L1_idx = min(L1_idx - t, xk.shape[0] - 1)
+
+ # F1 update
+ if case == 1:
+ # If ->(vi->vi+1)->vi+1 has just one intersection with xk F1 = startpoint
+ if w2 is None:
+ F1_idx = 0
+ # Otherwise F1 = w2
+ else:
+ F1_idx = w2_idx
+ if case == 2:
+ # Scan xk ccw from F1 until find vertex wt s.t. wt+1 lies to the
+ # right of vi+1->(vi+1->wt)->. Let F1 = wt.
+ idx_offsets = range(xk.shape[0])
+ for off in idx_offsets:
+ t = (F1_idx + off) % xk.shape[0]
+ w_next = xk[(t + 1) % xk.shape[0], :]
+ line_end_point = vi_1 + INF_VAL * (xk[t, :] - vi_1)
+ if point_right_of_line(w_next, vi_1, line_end_point):
+ F1_idx = t
+ break
+
+ # Check update of previous L1 index for new xk
+ # if not np.isclose(np.linalg.norm(L1 - xk[L1_idx, :]), 0):
+ # print("BAD L1 update [{},{}] -> [{},{}]".format(xk[L1_idx, 0], xk[L1_idx, 1], L1[0], L1[1]))
+ # plt.figure(2)
+ # draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ # plt.show()
+
+ # L1 update
+ # scan xk ccw from L1 until find vertex wu s.t. wu+1 lies to the
+ # left of vi+1->(vi+1->wu)->. Let L1 = wu.
+ idx_offsets = range(xk.shape[0])
+ for off in idx_offsets:
+ u = (L1_idx + off) % xk.shape[0]
+ w_next = xk[(u + 1) % xk.shape[0], :]
+ line_end_point = vi_1 + INF_VAL * (xk[u, :] - vi_1)
+ if point_left_of_line(w_next, vi_1, line_end_point):
+ L1_idx = u
+ break
+
+ else:
+ if verbose:
+ print("(2)")
+ # Endpoint for line vi->(vi->vi+1)->
+ end_point = vi + INF_VAL * (vi_1 - vi) / np.linalg.norm(vi_1 - vi)
+
+ # (2.2) L1 lies to the left of line vi->(vi->vi+1)->
+ if point_left_of_line(L1, vi, end_point):
+ case = 2
+ if verbose:
+ print("(2.2) - L1 ({},{}) lies to the left of line v{}->(v{}->v{})->".format(L1[0], L1[1], i, i,
+ i + 1))
+ # xk stays the same
+
+ if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ plt.plot([end_point[0], vi[0]], [end_point[1], vi[1]], marker='*', color='y',
+ label="v{}->(v{}->v{})->".format(i, i, i + 1))
+ plt.title("L1 lies to the left of line v{}->(v{}->v{})->".format(i, i, i + 1))
+ plt.show()
+
+ # (2.1) L1 lies on or to the right of line vi->(vi->vi+1)->
+ else:
+ case = 1
+ if verbose:
+ print(
+ "(2.1) - L1 ({},{}) lies on or to the right of line v{}->(v{}->v{})->".format(L1[0], L1[1],
+ i, i, i + 1))
+ print(
+ "Scan xk cw from L1 until we reach F1 or an edge intersecting line v{}->(v{}->v{})->".format(
+ i, i, i + 1))
+ # Scan xk cw from L1 until we reach F1 or an edge intersecting line vi->(vi->vi+1)->
+ idx_offsets = range(xk.shape[0])
+ w1 = None
+ for off in idx_offsets:
+ t = L1_idx - off
+ # If circular
+ if t < 0:
+ t += xk.shape[0]
+ if t == F1_idx:
+ break
+ # Get intersection w1 of edge and line vi->(vi->vi+1)->
+ w1 = get_intersection(xk[t, :], xk[t - 1, :], vi, end_point)
+ if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ col = 'r' if w1 is None else 'g'
+ plt.plot([xk[t - 1, 0], xk[t, 0]], [xk[t - 1, 1], xk[t, 1]], marker='*', color=col,
+ label="edge")
+ plt.plot([end_point[0], v[i, 0]], [end_point[1], v[i, 1]], marker='*', color='y',
+ label="v{}->(v{}->v{})->".format(i, i, i + 1))
+ plt.title(
+ "Scan xk cw from L1 until we reach F1 or an edge intersecting line v{}->(v{}->v{})->".format(
+ i,
+ i,
+ i + 1))
+ if w1 is not None:
+ if debug:
+ plt.plot(w1[0], w1[1], 's', label="w1", color='g')
+ plt.legend()
+ plt.show()
+ break
+ if debug:
+ plt.legend()
+ plt.show()
+ # If no intersecting line is reached no kernel exists
+ if w1 is None:
+ # if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ plt.title('No kernel found. Polygon not starshaped.')
+ plt.show()
+ return False
+
+ if verbose:
+ print("Found intersection ({},{}) at line v{}->(v{}->v{})-> for edge ({},{})-({},{})".format(
+ w1[0], w1[1], i, i, i + 1, xk[t - 1, 0], xk[t - 1, 1], xk[t, 0], xk[t, 1]))
+ print("Scan xk ccw from L1 until we reach edge intersecting line v{}->(v{}->v{})->".format(i,
+ i,
+ i + 1))
+ # Scan xk ccw from L1 until we reach edge w2 intersecting line vi->(vi->vi+1)->
+ w2 = None
+ idx_offsets = range(1, xk.shape[0] + 1)
+ for off in idx_offsets:
+ s = (L1_idx + off) % xk.shape[0]
+ if not xk_bounded and s == 0:
+ break
+ # Get intersection w2 of edge and line vi->(vi->vi+1)->
+ w2 = get_intersection(xk[s - 1, :], xk[s, :], vi, end_point)
+ if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ col = 'r' if w2 is None else 'g'
+ plt.plot([xk[s - 1, 0], xk[s, 0]], [xk[s - 1, 1], xk[s, 1]], marker='*', color=col,
+ label="edge")
+ plt.plot([end_point[0], v[i, 0]], [end_point[1], v[i, 1]], marker='*', color='y',
+ label="v{}->(v{}->v{})->".format(i, i, i + 1))
+ plt.title(
+ "Scan xk ccw from L1 until we reach edge intersecting line v{}->(v{}->v{})->".format(i,
+ i,
+ i + 1))
+ if w2 is not None:
+ if xk_bounded and t > s:
+ alpha = xk[xk.shape[0]:, :] # Empty array
+ beta = xk[s:t, :]
+ else:
+ alpha = xk[:t, :]
+ beta = xk[s:, :]
+ if debug:
+ print("xk: ", end=" ")
+ print(xk)
+ print("alpha: ", end=" ")
+ print(alpha)
+ print("beta: ", end=" ")
+ print(beta)
+ print("w1: ", end=" ")
+ print(w1)
+ print("w2: ", end=" ")
+ print(w2)
+ plt.plot(w1[0], w1[1], 's', label="w1", color='k')
+ plt.plot(w2[0], w2[1], 's', label="w2", color='g')
+ plt.plot(alpha[:, 0], alpha[:, 1], 'r--', label="alpha")
+ plt.plot(beta[:, 0], beta[:, 1], 'k--', label="beta")
+ plt.legend()
+ plt.show()
+ if verbose:
+ print("Update 1")
+ print(
+ "Found intersection ({},{}) at line v{}->(v{}->v{})-> for edge ({},{})-({},{})".format(
+ w2[0], w2[1], i, i, i + 1, xk[s, 0], xk[s, 1], xk[(s + 1) % xk.shape[0], 0],
+ xk[(s + 1) % xk.shape[0], 1]))
+
+ xk = np.vstack((alpha, w1, w2, beta))
+ w1_idx = alpha.shape[0]
+ w2_idx = alpha.shape[0] + 1
+ break
+ if debug:
+ plt.legend()
+ plt.show()
+ # If no intersecting line is reached
+ if w2 is None:
+ # Test if xk+1 is bounded
+ # If slope vi->(vi->vi+1)-> is comprised between the slopes of initial and final half lines of xk,
+ if not xk_bounded and ((orientation_val(xk[-2, :], xk[-1, :], end_point) * orientation_val(vi,
+ end_point,
+ xk[0,
+ :])) > 0):
+ alpha = xk[:t, :]
+ if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ plt.plot(w1[0], w1[1], 's', label="w1", color='g')
+ plt.plot(xk[:2, 0], xk[:2, 1], '--c', label="initial half line")
+ plt.plot(xk[-2:, 0], xk[-2:, 1], '--m', label="final half line")
+ plt.plot([end_point[0], v[i, 0]], [end_point[1], v[i, 1]], marker='*', color='y',
+ label="v{}->(v{}->v{})->".format(i, i, i + 1))
+ plt.title(
+ "v{}->(v{}->v{})-> between initial and final half lines of xk".format(
+ i, i, i + 1))
+ plt.plot(alpha[:, 0], alpha[:, 1], 'r--', label="alpha")
+ plt.legend()
+ plt.show()
+ if verbose:
+ print("Update 2 - xk still unbounded")
+ # then xk+1= alpha->w1->(vi->vi+1)-> is also unbounded.
+ xk = np.vstack((alpha, w1, end_point))
+ w1_idx = alpha.shape[0]
+ L1_idx = xk.shape[0] - 1
+ F1_idx = 0
+ else:
+ if verbose:
+ print(
+ "Scan xk ccw from start until we reach edge intersecting line v{}->(v{}->v{})->".format(
+ i,
+ i,
+ i + 1))
+ # otherwise scan xk ccw from xk[0,:] until we reach edge intersecting line vi->(vi->vi+1)->
+ w2 = None
+ for r in range(1, xk.shape[0] + 1):
+ # Get intersection w2 of edge and line vi->(vi->vi+1)->
+ w2 = get_intersection(xk[r - 1, :], xk[r % xk.shape[0], :], vi, end_point)
+ if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ col = 'r' if w2 is None else 'g'
+ plt.plot([xk[r - 1, 0], xk[r % xk.shape[0], 0]],
+ [xk[r - 1, 1], xk[r % xk.shape[0], 1]], marker='*', color=col,
+ label="edge")
+ plt.plot([end_point[0], v[i, 0]], [end_point[1], v[i, 1]], marker='*', color='y',
+ label="v{}->(v{}->v{})->".format(i, i, i + 1))
+ plt.title(
+ "Scan xk ccw from xk[0,:] until we reach edge intersecting line v{}->(v{}->v{})->".format(
+ i,
+ i,
+ i + 1))
+ if w2 is not None:
+ delta = xk[r:t, :]
+ if debug:
+ plt.plot(w2[0], w2[1], 's', label="w2", color='g')
+ plt.legend()
+ plt.show()
+ break
+ if debug:
+ plt.legend()
+ plt.show()
+ if verbose:
+ print("Update 3")
+ print("Found intersection ({},{}) at line v{}->(v{}->v{})->".format(w2[0], w2[1], i, i,
+ i + 1))
+ # Set xk as delta-w1-vi-vi+1-w2
+ xk = np.vstack((delta, w1, w2))
+ w1_idx = delta.shape[0]
+ w2_idx = delta.shape[0] + 1
+ xk_bounded = True
+ F1_idx = 0
+ L1_idx = min(L1_idx - t, xk.shape[0] - 1)
+
+ # F1 update
+
+ # If vi+1 in vi->(vi->vi+1)->w1,
+ if case == 2 or is_between(vi, vi_1, w1):
+ # scan xk ccw from F1 until find vertex wt s.t. wt+1 lies to the
+ # right of vi+1->(vi+1->wt)->. Let F1 = wt.
+ idx_offsets = range(xk.shape[0])
+ for off in idx_offsets:
+ t = (F1_idx + off) % xk.shape[0]
+ w_next = xk[(t + 1) % xk.shape[0], :]
+ line_end_point = vi_1 + INF_VAL * (xk[t, :] - vi_1)
+ if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ plt.plot([vi_1[0], line_end_point[0]], [vi_1[1], line_end_point[1]], '--',
+ label='v{}->(v{}->wt)'.format(i + 1, i + 1))
+ c = 'g' if point_right_of_line(w_next, vi_1, line_end_point) else 'r'
+ plt.plot(w_next[0], w_next[1], color=c, marker='s', label='w_t+1')
+ plt.title(
+ "Update F1\n scan xk ccw from F1 until find vertex wt s.t. wt+1 lies to the right of v{}->(v{}->wt)->.".format(
+ i + 1, i + 1))
+ plt.legend()
+ plt.show()
+ if point_right_of_line(w_next, vi_1, line_end_point):
+ F1_idx = t
+ break
+ else:
+ F1_idx = w1_idx
+
+ # L1 update
+ if case == 1:
+ if w2 is not None:
+ # If vi+1 in vi->(vi->vi+1)->w2
+ if is_between(vi, vi_1, w2):
+ L1_idx = w2_idx
+ else:
+ # scan xk ccw from w2 until find vertex wu s.t. wu+1 lies to the
+ # left of vi+1->(vi+1->wu)->. Let L1 = wu.
+ idx_offsets = range(xk.shape[0])
+ for off in idx_offsets:
+ u = (w2_idx + off) % xk.shape[0]
+ w_next = xk[(u + 1) % xk.shape[0], :]
+ line_end_point = vi_1 + INF_VAL * (xk[u, :] - vi_1)
+ if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ plt.plot([vi_1[0], line_end_point[0]], [vi_1[1], line_end_point[1]], '--',
+ label='v{}->(v{}->wt)'.format(i + 1, i + 1))
+ c = 'g' if point_left_of_line(w_next, vi_1, line_end_point) else 'r'
+ plt.plot(w_next[0], w_next[1], color=c, marker='s', label='w_t+1')
+ plt.title(
+ "Update L1\n scan xk ccw from w2 until find vertex wu s.t. wu+1 lies to the left of v{}->(v{}->wt)->.".format(
+ i + 1, i + 1))
+ plt.legend()
+ plt.show()
+ if point_left_of_line(w_next, vi_1, line_end_point):
+ L1_idx = u
+ break
+
+ # (2.1.2) line vi->(vi->vi+1)-> intersects xk just in w1
+ else:
+ L1_idx = xk.shape[0] - 1
+ if case == 2:
+ # print("(2.1.2)")
+ if xk_bounded:
+ # scan xk ccw from w2 until find vertex wu s.t. wu+1 lies to the
+ # left of vi+1->(vi+1->wu)->. Let L1 = wu.
+ idcs_list = np.roll(np.arange(xk.shape[0]), -L1_idx) if xk_bounded else range(L1_idx,
+ xk.shape[0] - 1)
+ for u in idcs_list:
+ w_next = xk[(u + 1) % xk.shape[0], :]
+ line_end_point = vi_1 + INF_VAL * (xk[u, :] - vi_1)
+ if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ plt.plot([vi_1[0], line_end_point[0]], [vi_1[1], line_end_point[1]], '--',
+ label='v{}->(v{}->wt)'.format(i + 1, i + 1))
+ c = 'g' if point_left_of_line(w_next, vi_1, line_end_point) else 'r'
+ plt.plot(w_next[0], w_next[1], color=c, marker='s', label='w_t+1')
+ plt.title(
+ "Update L1\n scan xk ccw from w2 until find vertex wu s.t. wu+1 lies to the left of v{}->(v{}->wt)->.".format(
+ i + 1, i + 1))
+ plt.legend()
+ plt.show()
+ if point_left_of_line(w_next, vi_1, line_end_point):
+ L1_idx = u
+ break
+
+ if verbose:
+ print("F1:", end=" ")
+ print(F1)
+ print("L1:", end=" ")
+ print(L1)
+ print("xk:", end=" ")
+ print(xk)
+ if debug or verbose:
+ draw(xk, F1_idx, L1_idx, 0, xk_bounded)
+ plt.show()
+ _, idx = np.unique(xk, axis=0, return_index=True)
+ xk = xk[np.sort(idx), :]
+ self._kernel = shapely.geometry.Polygon(xk)
+
+
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/starshaped_primitive_combination.py b/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/starshaped_primitive_combination.py
new file mode 100644
index 0000000000000000000000000000000000000000..6fdf2cd7ba907711261851bd22c60a6ab5d1b95d
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/starshaped_primitive_combination.py
@@ -0,0 +1,226 @@
+import shapely
+import numpy as np
+from obstacles import Frame, StarshapedObstacle, StarshapedPolygon
+from utils import is_ccw, is_cw, draw_shapely_polygon
+import matplotlib.pyplot as plt
+
+# Note: Local == Global frame
+class StarshapedPrimitiveCombination(StarshapedObstacle):
+
+ def __init__(self, obstacle_cluster, hull_cluster, xr, **kwargs):
+ self._obstacle_cluster = obstacle_cluster
+ self._hull_cluster = hull_cluster
+ super().__init__(xr=xr, **kwargs)
+ self.vertices = None
+ self.circular_vertices = None
+ self.vertex_angles = None
+
+ def obstacle_cluster(self):
+ return self._obstacle_cluster
+
+ def hull_cluster(self):
+ return self._hull_cluster
+
+ def dilated_obstacle(self, padding, id="new", name=None):
+ pass
+
+ def point_location(self, x, input_frame=Frame.GLOBAL):
+ locs = [obs.point_location(x, input_frame=Frame.GLOBAL) for obs in self._obstacle_cluster] + \
+ [self._hull_cluster_point_location(x)]
+ if any([l < 0 for l in locs]):
+ # Interior point
+ return -1
+ if any([l == 0 for l in locs]):
+ # Boundary point
+ return 0
+ # Exterior point
+ return 1
+
+ def line_intersection(self, line, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
+ intersection_points = []
+ for o in self._obstacle_cluster:
+ intersection_points += o.line_intersection(line, Frame.GLOBAL, Frame.GLOBAL)
+ intersection_points += self._hull_cluster_line_intersections(line)
+ return intersection_points
+
+ # TODO: Fix if needed. Currently not considering hull.
+ def tangent_points(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
+ tp, tp_candidates = [], []
+ for obs in self._obstacle_cluster:
+ tp_candidates += obs.tangent_points(x, Frame.GLOBAL, Frame.GLOBAL)
+ for i in range(len(tp_candidates)):
+ if all([is_ccw(x, tp_candidates[i], tp_candidates[j]) for j in range(len(tp_candidates)) if j is not i]) or \
+ all([is_cw(x, tp_candidates[i], tp_candidates[j]) for j in range(len(tp_candidates)) if j is not i]):
+ tp += [tp_candidates[i]]
+ return tp
+
+ def _compute_kernel(self):
+ self._kernel = StarshapedPolygon(self.polygon(), xr=self.xr(), id="temp").kernel()
+
+ def _check_convexity(self):
+ self._is_convex = StarshapedPolygon(self.polygon(), xr=self.xr(), id="temp").is_convex()
+
+ def boundary_mapping(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
+ # intersection_points = [p for ps in self.line_intersection([self._xr, self._xr+10*(x-self._xr)]) for p in ps]
+ intersection_points = self.line_intersection([self._xr, self._xr+10*(x-self._xr)])
+ if not intersection_points:
+ return None
+ dist_intersection_points = [np.linalg.norm(ip - self._xr) for ip in intersection_points]
+ return intersection_points[np.argmax(dist_intersection_points)]
+
+ def vel_intertial_frame(self, x):
+ boundary_obs_idx = 0
+ max_dist = -1
+ for i, ps in enumerate(self.line_intersection([self._xr, self._xr+10*(x-self._xr)])):
+ o_intersection_dist = max([np.linalg.norm(p-self._xr) for p in ps] + [-1])
+ if o_intersection_dist > max_dist:
+ boundary_obs_idx = i
+ max_dist = o_intersection_dist
+ if boundary_obs_idx >= len(self._obstacle_cluster):
+ boundary_obs_idx -= len(self._obstacle_cluster)
+ return self._obstacle_cluster[boundary_obs_idx].vel_intertial_frame(x)
+
+ def normal(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL, x_is_boundary=False, type='weigthed_polygon_approx'):
+ if type == 'sub_normal':
+ boundary_obs_idx = 0
+ max_dist = -1
+ line = [self._xr, self._xr+10*(x-self._xr)]
+ for i, o in enumerate(self._obstacle_cluster):
+ intersection_points = o.line_intersection(line, Frame.GLOBAL, Frame.GLOBAL)
+ for p in intersection_points:
+ p_dist = np.linalg.norm(p - self._xr)
+ if p_dist > max_dist:
+ max_dist = p_dist
+ boundary_obs_idx = i
+
+ hull_ip = self._hull_cluster_line_intersections(line)
+ if hull_ip:
+ for p in hull_ip:
+ p_dist = np.linalg.norm(p - self._xr)
+ if p_dist > max_dist:
+ max_dist = p_dist
+ boundary_obs_idx = len(self._obstacle_cluster)
+ if boundary_obs_idx < len(self._obstacle_cluster):
+ return self._obstacle_cluster[boundary_obs_idx].normal(x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL)
+ else:
+ boundary_obs_idx -= len(self._obstacle_cluster)
+ hull_vertices = np.array(self._hull_cluster.exterior.coords[:-1])
+ vertex_angles = np.array([np.arctan2(v[1] - self._xr[1], v[0] - self._xr[0]) for v in hull_vertices]).flatten()
+ idcs = np.argsort(vertex_angles)
+ vertex_angles = vertex_angles[idcs]
+ hull_vertices = hull_vertices[idcs, :]
+ vertex_angles = np.hstack((vertex_angles, vertex_angles[0] + 2 * np.pi))
+ hull_vertices = np.vstack((hull_vertices, hull_vertices[0, :]))
+
+ angle = np.arctan2(x[1] - self._xr[1], x[0] - self._xr[0])
+ v_idx = np.argmax(vertex_angles > angle)
+ # Adjust for circular self.vertices (self.vertices[0] == self.vertices[-1])
+ if v_idx == 0:
+ v_idx = -1
+ n = np.array([hull_vertices[v_idx, 1] - hull_vertices[v_idx - 1, 1],
+ hull_vertices[v_idx - 1, 0] - hull_vertices[v_idx, 0]])
+ n /= np.linalg.norm(n)
+ return n
+ elif type == 'polygon_approx' or type == 'weigthed_polygon_approx':
+ if self.vertices is None:
+ self._update_vertex_angles()
+ angle = np.arctan2(x[1] - self._xr[1], x[0] - self._xr[0])
+ v_idx = np.argmax(self.vertex_angles > angle)
+
+ if v_idx == self.vertices.shape[0]:
+ v_idx = 0
+
+ if type == 'polygon_approx':
+ n = np.array([self.vertices[v_idx, 1] - self.vertices[v_idx - 1, 1],
+ self.vertices[v_idx - 1, 0] - self.vertices[v_idx, 0]])
+ else:
+ edge_neighbors = [(self.vertices[(v_idx - 2 + i) % self.vertices.shape[0]],
+ self.vertices[(v_idx - 1 + i) % self.vertices.shape[0]]) for i in range(3)]
+ edge_neighbors_normal = np.array([[e[1][1] - e[0][1],
+ e[0][0] - e[1][0]] for e in edge_neighbors])
+ edge_closest = [shapely.ops.nearest_points(shapely.geometry.LineString(e),
+ shapely.geometry.Point(x))[0].coords[0] for e in
+ edge_neighbors]
+
+ dist = [np.linalg.norm(np.array(e) - x) for e in edge_closest]
+ w = np.array([1 / (d + 1e-10) for d in dist])
+ w /= sum(w)
+ n = edge_neighbors_normal.T.dot(w)
+
+ n /= np.linalg.norm(n)
+ return n
+
+ def set_xr(self, xr, input_frame=Frame.OBSTACLE, safe_set=False):
+ super().set_xr(xr, input_frame, safe_set)
+ self._update_vertex_angles()
+
+ def init_plot(self, ax=None, show_reference=True, show_name=False, **kwargs):
+ if ax is None:
+ _, ax = plt.subplots(subplot_kw={'aspect': 'equal'})
+ if "fc" not in kwargs and "facecolor" not in kwargs:
+ kwargs["fc"] = 'lightgrey'
+
+ line_handles = []
+
+ lh, _ = draw_shapely_polygon(self.polygon(), ax=ax, **kwargs)
+ line_handles += lh
+
+ # Reference point
+ line_handles += ax.plot(*self.xr(), '+', color='k') if show_reference else [None]
+ # Name
+ line_handles += [ax.text(*self.xr(), self._name)] if show_name else [None]
+ return line_handles, ax
+
+ def update_plot(self, line_handles):
+ pass
+
+ def draw(self, ax=None, show_reference=True, show_name=False, **kwargs):
+ line_handles, ax = self.init_plot(ax, show_reference, show_name, **kwargs)
+ self.update_plot(line_handles)
+ return line_handles, ax
+
+ def _hull_cluster_point_location(self, x):
+ if self._hull_cluster is None:
+ return 1
+ x_sh = shapely.geometry.Point(x)
+ if self._hull_cluster.contains(x_sh):
+ return -1
+ if self._hull_cluster.disjoint(x_sh):
+ return 1
+ return 0
+
+ def _hull_cluster_line_intersections(self, line):
+ if self._hull_cluster is None:
+ return []
+ line_sh = shapely.geometry.LineString(line)
+ intersection_points_shapely = line_sh.intersection(self._hull_cluster.exterior)
+ if intersection_points_shapely.is_empty:
+ return []
+ if intersection_points_shapely.geom_type == 'Point':
+ return [np.array([intersection_points_shapely.x, intersection_points_shapely.y])]
+ elif intersection_points_shapely.geom_type == 'MultiPoint':
+ return [np.array([p.x, p.y]) for p in intersection_points_shapely.geoms]
+ elif intersection_points_shapely.geom_type == 'LineString':
+ return [np.array([ip[0], ip[1]]) for ip in intersection_points_shapely.coords]
+ elif intersection_points_shapely.geom_type == 'MultiLineString':
+ return [np.array([l[0], l[1]]) for line in intersection_points_shapely.geoms for
+ l in line.coords]
+ else:
+ print("[_hull_cluster_line_intersections]: Shapely geom_type not covered!")
+ print(intersection_points_shapely)
+
+ def _update_vertex_angles(self):
+ self.vertices = np.array(self._polygon.exterior.coords[:-1])
+ self.circular_vertices = np.array(self._polygon.exterior.coords)
+ self.vertex_angles = np.arctan2(self.vertices[:, 1] - self._xr[1], self.vertices[:, 0] - self._xr[0])
+ idcs = np.argsort(self.vertex_angles)
+ self.vertex_angles = self.vertex_angles[idcs]
+ self.vertices = self.vertices[idcs, :]
+ self.circular_vertices = np.vstack((self.vertices, self.vertices[0, :]))
+ self.vertex_angles = np.hstack((self.vertex_angles, self.vertex_angles[0] + 2 * np.pi))
+
+ def _compute_polygon_representation(self):
+ obs_pol = [obs.polygon() for obs in self._obstacle_cluster]
+ if self._hull_cluster is not None:
+ obs_pol += [self._hull_cluster]
+ self._polygon = shapely.ops.unary_union(obs_pol)
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/requirements.txt b/python/ur_simple_control/path_generation/star_navigation/starworlds/requirements.txt
new file mode 100644
index 0000000000000000000000000000000000000000..3689a74ef0eb7796d52dc466d5aafaa6c624e28f
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/requirements.txt
@@ -0,0 +1,4 @@
+numpy
+scipy
+matplotlib
+shapely
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/setup.py b/python/ur_simple_control/path_generation/star_navigation/starworlds/setup.py
new file mode 100644
index 0000000000000000000000000000000000000000..67761811b674817cb9f0d2dea2db43c8dba03227
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/setup.py
@@ -0,0 +1,14 @@
+from setuptools import setup, find_packages
+
+setup(name='starworlds',
+ version='1.0',
+ packages=find_packages(),
+ install_requires=[
+ 'pyyaml',
+ 'numpy',
+ 'scipy',
+ 'matplotlib',
+ 'shapely'
+ ]
+
+)
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/starshaped_hull/__init__.py b/python/ur_simple_control/path_generation/star_navigation/starworlds/starshaped_hull/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..45406b14bb497117f7b86c4941c8e4849f3160e8
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/starshaped_hull/__init__.py
@@ -0,0 +1,2 @@
+from .starshaped_hull import admissible_kernel, kernel_starshaped_hull
+from .cluster_and_starify import ObstacleCluster, get_intersection_clusters, cluster_and_starify, draw_clustering, draw_adm_ker, draw_star_hull
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/starshaped_hull/cluster_and_starify.py b/python/ur_simple_control/path_generation/star_navigation/starworlds/starshaped_hull/cluster_and_starify.py
new file mode 100644
index 0000000000000000000000000000000000000000..2e825e86581a3bbecb805d3ea1351a1a6a5c95ed
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/starshaped_hull/cluster_and_starify.py
@@ -0,0 +1,500 @@
+import shapely
+import numpy as np
+from obstacles import Frame, StarshapedPrimitiveCombination, Ellipse, StarshapedPolygon
+from utils import is_ccw, is_collinear, equilateral_triangle, Cone, tic, toc, draw_shapely_polygon
+from scipy.spatial import ConvexHull
+import starshaped_hull as sh
+import matplotlib.pyplot as plt
+
+
+class ObstacleCluster:
+ def __init__(self, obstacles):
+ self.name = '_'.join([str(o.id()) for o in obstacles])
+ self.obstacles = obstacles
+ self.cluster_obstacle = None if len(obstacles) > 1 else obstacles[0]
+ self.kernel_points = None
+ self.admissible_kernel = None
+ self._polygon = None
+ self._polygon_excluding_hull = None
+
+ def __str__(self):
+ return self.name
+
+ def polygon(self):
+ if self._polygon is None:
+ if self.cluster_obstacle is None:
+ print("[OBSTACLE CLUSTER]: WARNING, cluster_obstacle must be defined before accessing polygon.")
+ else:
+ self._polygon = self.cluster_obstacle.polygon()
+ return self._polygon
+
+ def polygon_excluding_hull(self):
+ if self._polygon_excluding_hull is None:
+ self._polygon_excluding_hull = shapely.ops.unary_union([o.polygon() for o in self.obstacles])
+ return self._polygon_excluding_hull
+
+ def draw(self, ax=None):
+ if self.cluster_obstacle is not None:
+ ax, _ = self.cluster_obstacle.draw(ax=ax, fc="green")
+ for obs in self.obstacles:
+ ax, _ = obs.draw(ax=ax)
+ return ax, _
+
+
+def get_intersection_clusters(clusters):
+ No = len(clusters)
+ intersection_idcs = []
+
+ # Use polygon approximations for intersection check
+ cluster_polygons = [cl.polygon() for cl in clusters]
+
+ # Find intersections
+ intersections_exist = False
+ for i in range(No):
+ intersection_idcs += [[i]]
+ for j in range(i + 1, No):
+ if cluster_polygons[i].intersects(cluster_polygons[j]):
+ intersection_idcs[i] += [j]
+ intersections_exist = True
+
+ if not intersections_exist:
+ return clusters, intersections_exist
+
+ # Cluster intersecting obstacles
+ for i in range(No - 1, 0, -1):
+ for j in range(i - 1, -1, -1):
+ found = False
+ for l_j in intersection_idcs[j]:
+ if l_j in intersection_idcs[i]:
+ found = True
+ break
+ if found:
+ intersection_idcs[j] = list(set(intersection_idcs[j] + intersection_idcs[i]))
+ intersection_idcs[i] = []
+ break
+
+ # Create obstacle clusters
+ cluster_obstacles = [cl.obstacles for cl in clusters]
+ new_clusters = []
+ for i in range(No):
+ if intersection_idcs[i]:
+ new_clusters += [ObstacleCluster([o for j in intersection_idcs[i] for o in cluster_obstacles[j]])]
+
+ return new_clusters, intersections_exist
+
+
+def compute_kernel_points(cl, x, xg, epsilon, cl_prev, workspace):
+ triangle_center_prev = np.mean(cl_prev.kernel_points, axis=0) if cl_prev else None
+
+ t0 = tic()
+ ts = {}
+ # Find triangle center selection set (TCSS)
+ tcss = cl.admissible_kernel
+ # If obstacle is in exterior of workspace limit TCSS to workspace exterior
+ if not cl.polygon_excluding_hull().within(workspace.polygon()):
+ tcss_tmp = tcss.difference(workspace.polygon())
+ tcss_tmp = tcss_tmp.intersection(cl.polygon_excluding_hull()) # NOTE: Added for exlcluding undesired extremas in other workspace exterior than close to obstacles
+ if tcss_tmp.area > 1e-6:
+ tcss = tcss_tmp
+ tcss_tmp = tcss
+ ts['ws check'] = toc(t0)
+ # Try to use intersection of all obstacle kernels in cluster if all starshaped
+ if all([o.is_starshaped() for o in cl.obstacles]):
+ for o in cl.obstacles:
+ tcss_tmp = tcss_tmp.intersection(o.kernel())
+ ts['kernel intersection'] = toc(t0)-list(ts.values())[-1]
+ # Else, try to use union of all obstacles in cluster
+ if tcss_tmp.area < 1e-6:
+ tcss_tmp = tcss.intersection(cl.polygon_excluding_hull())
+ ts['cluster intersection'] = toc(t0)-list(ts.values())[-1]
+ if tcss_tmp.area > 1e-6:
+ tcss = tcss_tmp
+
+ # If not tc from previous iteraion, use closest point to TCSS in ad ker as triangle center
+ if triangle_center_prev is None:
+ tc, _ = shapely.ops.nearest_points(cl.admissible_kernel, tcss.centroid)
+ triangle_center = np.array(tc.coords[0])
+ while is_collinear(x, xg, triangle_center):
+ triangle_center += np.random.uniform(-1e-4, 1e-4, 2)
+ # Else, use previous triangle center if still in selection set
+ elif tcss.contains(shapely.geometry.Point(triangle_center_prev))\
+ and not is_collinear(x, xg, triangle_center_prev):
+ triangle_center = triangle_center_prev
+ # Else, try to maintain triangle center on same side of l(x,xg) as previous time step
+ else:
+ x_xg_line = shapely.geometry.LineString([x, xg])
+ splitted_tcss = shapely.ops.split(tcss, x_xg_line).geoms
+ triangle_center_selection_set = splitted_tcss[0]
+ if len(splitted_tcss) > 1:
+ for i in range(1, len(splitted_tcss)):
+ if is_ccw(x, xg, splitted_tcss[i].centroid.coords[0]) == is_ccw(x, xg, triangle_center_prev):
+ triangle_center_selection_set = splitted_tcss[i]
+ break
+ tc, _ = shapely.ops.nearest_points(cl.admissible_kernel, triangle_center_selection_set.centroid)
+ triangle_center = np.array(tc.coords[0])
+
+ ts['tc selection'] = toc(t0)-list(ts.values())[-1]
+
+ # if cl.name == '5_6_7':
+ # hs, _ = draw_shapely_polygon(tcss, plt.gca(), fc='g')
+ # hs += plt.plot(*triangle_center, 'kd')
+ # while not plt.waitforbuttonpress(): pass
+ # [h.remove() for h in hs]
+
+ # Select kernel points as largest equilateral triangle in TCSS (with maximum side length epsilon)
+ if tcss.geom_type == 'Polygon':
+ dist = tcss.exterior.distance(shapely.geometry.Point(triangle_center))
+ else:
+ tc = shapely.geometry.Point(triangle_center)
+ dist = min([p.exterior.distance(tc) for p in tcss.geoms])
+ triangle_length = min(epsilon, 0.9 * dist)
+ kernel_points = equilateral_triangle(triangle_center, triangle_length)
+ ts['triangle generation'] = toc(t0)-list(ts.values())[-1]
+ tot_time = sum(ts.values())
+ for k in ts.keys():
+ ts[k] = int(ts[k] / tot_time * 100)
+ # print(ts)
+ return kernel_points
+
+def extract_cluster(cl, cl_list):
+ if cl_list is None:
+ return None
+ for cl_i in cl_list:
+ if cl.name == cl_i.name:
+ return cl_i
+ return None
+
+# Input: Convex obstacles, excluding points x and xg, kernel width epsilon
+def cluster_and_starify(obstacles, x, xg, epsilon, workspace=None, max_compute_time=np.inf, previous_clusters=None,
+ make_convex=False, exclude_obstacles=False, max_iterations=np.inf, verbose=False,
+ timing_verbose=False, return_history=False):
+ t0 = tic()
+
+ if workspace is None:
+ workspace = Ellipse([1e10, 1e10])
+
+ # Exit flags
+ INCOMPLETE = 0
+ COMPLETE = 1
+ MAX_COMPUTE_TIME_CONVEX_HULL = 2
+
+ # Variable initialization
+ kernel_time, hull_time, cluster_time, convex_time = [0.], [0.], [0.], 0.
+ adm_ker_robot_cones = {obs.id(): None for obs in obstacles}
+ adm_ker_goal_cones = {obs.id(): None for obs in obstacles}
+ adm_ker_obstacles = {obs.id(): {} for obs in obstacles}
+ cluster_iterations = []
+ n_iter = 0
+
+ def default_result():
+ if return_history:
+ cl_history = cluster_iterations if n_iter > 0 else [ObstacleCluster([o]) for o in obstacles]
+ return [ObstacleCluster([o]) for o in obstacles], [0.] * 4, INCOMPLETE, n_iter, cl_history
+ else:
+ return [ObstacleCluster([o]) for o in obstacles], [0.] * 4, INCOMPLETE, n_iter
+
+ # Compute admissible kernel for all obstacles
+ for o in obstacles:
+ if max_compute_time < toc(t0):
+ return default_result()
+ # Find admissible kernel
+ adm_ker_robot_cones[o.id()] = sh.admissible_kernel(o, x)
+ adm_ker_goal_cones[o.id()] = sh.admissible_kernel(o, xg)
+
+ if adm_ker_robot_cones[o.id()] is None:
+ if verbose:
+ print("[Cluster and Starify]: Robot position is not a free exterior point of obstacle " + str(o.id()))
+ return default_result()
+
+ if adm_ker_goal_cones[o.id()] is None:
+ if verbose:
+ print("[Cluster and Starify]: Goal position is not a free exterior point of obstacle " + str(o.id()))
+ return default_result()
+
+ # Admissible kernel when excluding points of other obstacles
+ if exclude_obstacles:
+ for o_ex in obstacles:
+ if o_ex.id() == o.id():
+ continue
+ o_x_exclude = o_ex.extreme_points()
+ if not all([o.exterior_point(x_ex) for x_ex in o_x_exclude]):
+ adm_ker_obstacles[o.id()][o_ex.id()] = None
+ continue
+ adm_ker_obstacles[o.id()][o_ex.id()] = sh.admissible_kernel(o, o_x_exclude[0]).polygon()
+ for v in o_x_exclude[1:]:
+ adm_ker_obstacles[o.id()][o_ex.id()] = adm_ker_obstacles[o.id()][o_ex.id()].intersection(
+ sh.admissible_kernel(o, v).polygon())
+ init_kernel_time = toc(t0)
+
+ # -- First iteration -- #
+
+ ker_sel_time, hull_compute_time, star_obj_time = [0], [0], [0]
+ # Initialize clusters as single obstacles
+ clusters = []
+ for o in obstacles:
+ # Ensure not xr in l(x,xg)
+ # while is_collinear(x, o.xr(), xg):
+ # o.set_xr(o.xr(output_frame=Frame.OBSTACLE) + np.random.normal(0, 0.01, 2))
+ cl = ObstacleCluster([o])
+
+ # New---
+ t1 = tic()
+ cl.admissible_kernel = adm_ker_robot_cones[o.id()].intersection(adm_ker_goal_cones[o.id()])
+ kernel_time[0] += toc(t1)
+
+ t1 = tic()
+ cl_prev = extract_cluster(cl, previous_clusters)
+ # if cl_prev is None:
+ # cl_prev.kernel_points = equilateral_triangle(o.xr(), epsilon)
+ cl.kernel_points = compute_kernel_points(cl, x, xg, epsilon, cl_prev, workspace=workspace)
+ ker_sel_time[0] += toc(t1)
+ t1 = tic()
+
+ # -- Compute starshaped hull of cluster
+ cl_id = "new" if cl_prev is None else cl_prev.cluster_obstacle.id()
+ cluster_hull_extensions = sh.kernel_starshaped_hull(cl.obstacles, cl.kernel_points)
+ hull_compute_time[0] += toc(t1)
+ t1 = tic()
+ k_centroid = np.mean(cl.kernel_points, axis=0)
+ if cluster_hull_extensions is None:
+ cl.cluster_obstacle = o
+ cl.cluster_obstacle.set_xr(k_centroid, input_frame=Frame.GLOBAL)
+ else:
+ # Non-starshaped polygons are included in the cluster hull
+ # cl_obstacles = [o] if o.is_starshaped() else []
+ cl.cluster_obstacle = StarshapedPrimitiveCombination(cl.obstacles, cluster_hull_extensions, xr=k_centroid,
+ id=cl_id)
+ star_obj_time[0] += toc(t1)
+ # cl.polygon()
+ # if o.is_starshaped and o.kernel().contains(shapely.geometry.Point(k_centroid)):
+ # cl.cluster_obstacle
+ # cl.cluster_obstacle = StarshapedPolygon(cl._polygon, xr=k_centroid, id=o.id())
+ # hull_time[0] += toc(t1)
+
+
+ clusters += [cl]
+
+
+ # if not o.is_starshaped():
+ # cl = clusters[-1]
+ # t1 = tic()
+ # cl.admissible_kernel = adm_ker_robot_cones[o.id()].intersection(adm_ker_goal_cones[o.id()])
+ # kernel_time[0] += toc(t1)
+ #
+ # t1 = tic()
+ # cl_prev = None
+ # if previous_clusters:
+ # for p_cl in previous_clusters:
+ # if cl.name == p_cl.name:
+ # cl_prev = p_cl
+ # cl.kernel_points = compute_kernel_points(cl, x, xg, epsilon, cl_prev, workspace=workspace)
+ # # -- Compute starshaped hull of cluster
+ # k_centroid = np.mean(cl.kernel_points, axis=0)
+ # cl._polygon = sh.kernel_starshaped_hull(o, cl.kernel_points)
+ # cl.cluster_obstacle = StarshapedPolygon(cl._polygon, xr=k_centroid, id=o.id())
+ # hull_time[0] += toc(t1)
+ hull_time[0] = ker_sel_time[0] + hull_compute_time[0] + star_obj_time[0]
+
+ # Set cluster history
+ cluster_history = {cl.name: cl for cl in clusters}
+ if return_history:
+ cluster_iterations += [clusters]
+
+ # -- Cluster obstacles such that no intersection exists
+ t1 = tic()
+ clusters, intersection_exists = get_intersection_clusters(clusters)
+ cluster_time[0] = toc(t1)
+
+ n_iter = 1
+ # -- End first iteration -- #
+
+ while intersection_exists:
+ kernel_time += [0.]
+ hull_time += [0.]
+ cluster_time += [0.]
+ ker_sel_time += [0.]
+ hull_compute_time += [0.]
+ star_obj_time += [0.]
+
+ # Check compute time
+ if max_compute_time < toc(t0):
+ if verbose:
+ print("[Cluster and Starify]: Max compute time.")
+ cluster_iterations += [clusters]
+ return default_result()
+
+ # Find starshaped obstacle representation for each cluster
+ for i, cl in enumerate(clusters):
+ # If cluster already calculated keep it
+ if cl.name in cluster_history:
+ clusters[i] = cluster_history[cl.name]
+ continue
+
+ # ----------- Admissible Kernel ----------- #
+ t1 = tic()
+
+ # If cluster is two convex obstacles
+ # if len(cl.obstacles) == 2 and cl.obstacles[0].is_convex() and cl.obstacles[1].is_convex():
+ # cl.admissible_kernel = cl.obstacles[0].polygon().intersection(cl.obstacles[1].polygon()).buffer(0.01)
+ # else:
+ adm_ker_robot = Cone.list_intersection([adm_ker_robot_cones[o.id()] for o in cl.obstacles], same_apex=True)
+ adm_ker_goal = Cone.list_intersection([adm_ker_goal_cones[o.id()] for o in cl.obstacles], same_apex=True)
+ cl.admissible_kernel = adm_ker_robot.intersection(adm_ker_goal)
+
+ if cl.admissible_kernel.is_empty or cl.admissible_kernel.area < 1e-6:
+ if verbose:
+ print("[Cluster and Starify]: Could not find disjoint starshaped obstacles. Admissible kernel empty for the cluster " + cl.name)
+ cluster_iterations += [clusters]
+ return default_result()
+
+ # Exclude other obstacles
+ adm_ker_o_ex = cl.admissible_kernel
+ if exclude_obstacles:
+ for o in cl.obstacles:
+ for o_ex in obstacles:
+ if o_ex.id() == o.id() or adm_ker_obstacles[o.id()][o_ex.id()] is None:
+ continue
+ adm_ker_o_ex = adm_ker_o_ex.intersection(adm_ker_obstacles[o.id()][o_ex.id()])
+ if not (adm_ker_o_ex.is_empty or adm_ker_o_ex.area < 1e-6):
+ cl.admissible_kernel = adm_ker_o_ex
+
+ kernel_time[n_iter] += toc(t1)
+ # ----------- End Admissible Kernel ----------- #
+
+ # ----------- Starshaped Hull ----------- #
+ t1 = tic()
+ # Check if cluster exist in previous cluster
+ cl_prev = None
+ if previous_clusters:
+ for p_cl in previous_clusters:
+ if cl.name == p_cl.name:
+ cl_prev = p_cl
+
+ # -- Kernel points selection
+ cl.kernel_points = compute_kernel_points(cl, x, xg, epsilon, cl_prev, workspace=workspace)
+ ker_sel_time[n_iter] += toc(t1)
+ t1 = tic()
+
+ # -- Compute starshaped hull of cluster
+ cl_id = "new" if cl_prev is None else cl_prev.cluster_obstacle.id()
+ cluster_hull_extensions = sh.kernel_starshaped_hull(cl.obstacles, cl.kernel_points)
+ hull_compute_time[n_iter] += toc(t1)
+
+ t1 = tic()
+ k_centroid = np.mean(cl.kernel_points, axis=0)
+ # Non-starshaped polygons are included in the cluster hull
+ cl_obstacles = [o for o in cl.obstacles if o.is_starshaped()]
+ cl.cluster_obstacle = StarshapedPrimitiveCombination(cl_obstacles, cluster_hull_extensions, xr=k_centroid, id=cl_id)
+ cl.polygon()
+
+ star_obj_time[n_iter] += toc(t1)
+ # ----------- End Starshaped Hull ----------- #
+
+ # -- Add cluster to history
+ cluster_history[cl.name] = cl
+
+ hull_time[n_iter] = ker_sel_time[n_iter] + hull_compute_time[n_iter] + star_obj_time[n_iter]
+
+ if return_history:
+ cluster_iterations += [clusters]
+
+ # ----------- Clustering ----------- #
+ t1 = tic()
+ # -- Cluster star obstacles such that no intersection exists
+ clusters, intersection_exists = get_intersection_clusters(clusters)
+ cluster_time[n_iter] = toc(t1)
+ # ----------- End Clustering ----------- #
+
+ n_iter += 1
+
+ if n_iter >= max_iterations:
+ break
+
+ # ----------- Make Convex ----------- #
+
+ if make_convex:
+ # Make convex if no intersection occurs
+ t1 = tic()
+ for j, cl in enumerate(clusters):
+ if not cl.cluster_obstacle.is_convex():
+ # Check compute time
+ if max_compute_time < toc(t0):
+ if verbose:
+ print("[Cluster and Starify]: Max compute time in convex hull.")
+ return clusters, [sum(cluster_time), init_kernel_time+sum(kernel_time), sum(hull_time), toc(t1)], MAX_COMPUTE_TIME_CONVEX_HULL, n_iter
+
+ v = np.array(cl.polygon().exterior.coords)[:-1, :]
+ hull = ConvexHull(v)
+ hull_polygon = shapely.geometry.Polygon(v[hull.vertices, :])
+ if not any([hull_polygon.contains(shapely.geometry.Point(x_ex)) for x_ex in [x, xg]]) and not any(
+ [hull_polygon.intersects(clusters[k].polygon()) for k in range(len(clusters)) if k != j]):
+ # clusters[j].cluster_obstacle = StarshapedPrimitiveCombination(cl.obstacles, hull_polygon, cl.cluster_obstacle.xr(Frame.GLOBAL))
+ clusters[j].cluster_obstacle = StarshapedPolygon(hull_polygon, xr=cl.cluster_obstacle.xr(Frame.GLOBAL))
+
+ convex_time = toc(t1)
+ # ----------- End Make Convex ----------- #
+
+ timing_vec = [sum(cluster_time), init_kernel_time+sum(kernel_time), sum(hull_time), convex_time]
+ if timing_verbose:
+
+ print("------------\nTotal timing (Cluster/AdmKer/StHull/ConvHull): {:.1f} [{:.1f}, {:.1f}, {:.1f}, {:.1f}]".format(sum(timing_vec), *timing_vec))
+ print("Init kernel timing: {:.1f}".format(init_kernel_time))
+ for i in range(n_iter):
+ print("Iteration {} timing (Cluster/AdmKer/StHull): [{:.1f}, {:.1f}, {:.1f}]".format(i, cluster_time[i], kernel_time[i], hull_time[i]))
+ print("\t Hull timing divide: (KerSel/Hull/Obj) calculation [{:.1f}, {:.1f}, {:.1f}]".format(ker_sel_time[i], hull_compute_time[i], star_obj_time[i]))
+
+ if return_history:
+ return clusters, timing_vec, COMPLETE, n_iter, cluster_iterations
+ else:
+ return clusters, timing_vec, COMPLETE, n_iter
+
+
+def draw_clustering(clusters, p, pg, xlim=None, ylim=None):
+ color = plt.cm.rainbow(np.linspace(0, 1, len(clusters)))
+ fig, ax = plt.subplots()
+ for i, cl in enumerate(clusters):
+ [o.draw(ax=ax, fc=color[i], show_reference=False, ec='k', alpha=0.8) for o in cl.obstacles]
+ ax.plot(*p, 'ko')
+ ax.plot(*pg, 'k*')
+ if xlim is not None:
+ ax.set_xlim(xlim)
+ if ylim is not None:
+ ax.set_ylim(ylim)
+ return fig, ax
+
+
+def draw_star_hull(clusters, p, pg, xlim=None, ylim=None):
+ fig, ax = plt.subplots()
+ for cl in clusters:
+ if cl.cluster_obstacle is not None:
+ cl.cluster_obstacle.draw(ax=ax, fc='g', alpha=0.8)
+ draw_shapely_polygon(cl.polygon_excluding_hull(), ax=ax, fc='lightgrey', ec='k')
+ else:
+ draw_shapely_polygon(cl.polygon_excluding_hull(), ax=ax, fc='r', ec='k')
+ ax.plot(*p, 'ko')
+ ax.plot(*pg, 'k*')
+ if xlim is not None:
+ ax.set_xlim(xlim)
+ if ylim is not None:
+ ax.set_ylim(ylim)
+ return fig, ax
+
+
+def draw_adm_ker(clusters, p, pg, xlim=None, ylim=None):
+ valid_adm_ker_cls = [cl for cl in clusters if not (cl.admissible_kernel is None or cl.admissible_kernel.is_empty)]
+ n_col = int(np.sqrt(len(valid_adm_ker_cls))) + 1
+ fig, axs = plt.subplots(n_col, n_col)
+ for i, cl in enumerate(valid_adm_ker_cls):
+ ax_i = axs[i//n_col, i%n_col]
+ [o.draw(ax=ax_i, show_name=1, show_reference=0, fc='lightgrey', ec='k', alpha=0.8) for o in cl.obstacles]
+ if not cl.admissible_kernel.geom_type == 'Point':
+ draw_shapely_polygon(cl.admissible_kernel, ax=ax_i, fc='y', alpha=0.3)
+ if cl.kernel_points is not None:
+ draw_shapely_polygon(shapely.geometry.Polygon(cl.kernel_points), ax=ax_i, fc='g', alpha=0.6)
+ ax_i.plot(*p, 'ko')
+ ax_i.plot(*pg, 'k*')
+ if xlim is not None:
+ ax_i.set_xlim(xlim)
+ if ylim is not None:
+ ax_i.set_ylim(ylim)
+ return fig, axs
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/starshaped_hull/starshaped_hull.py b/python/ur_simple_control/path_generation/star_navigation/starworlds/starshaped_hull/starshaped_hull.py
new file mode 100644
index 0000000000000000000000000000000000000000..8fde482f9edaf717ffcb82f170b9bacc158c3714
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/starshaped_hull/starshaped_hull.py
@@ -0,0 +1,207 @@
+import shapely
+import numpy as np
+from obstacles import Polygon
+from utils import is_ccw, is_cw, line, ray, Cone, convex_hull
+import matplotlib.pyplot as plt
+
+
+def admissible_kernel(obstacle, x):
+ # Find tangents of obstacle through x
+ tps = obstacle.tangent_points(x)
+ if not tps:
+ # Interior point
+ return None
+ return Cone(x, x-tps[0], x-tps[1])
+
+
+# Computes the starshaped hull of a list of obstacles for specified kernel points
+def kernel_starshaped_hull(obstacles, kernel_points):
+ if not type(obstacles) is list:
+ if obstacles.is_convex():
+ return convex_kernel_starshaped_hull(obstacles, kernel_points)
+ if issubclass(obstacles.__class__, Polygon):
+ return polygon_kernel_starshaped_hull(obstacles.polygon(), kernel_points)
+ else:
+ print("[kernel_starshaped_hull]: Bad obstacle class.")
+ print(obstacles)
+
+ sub_pols = [kernel_starshaped_hull(o, kernel_points) for o in obstacles]
+ hull_polygon = shapely.ops.unary_union(sub_pols)
+ if hull_polygon.is_empty:
+ return None
+ return hull_polygon
+
+
+def convex_kernel_starshaped_hull(convex_obstacle, kernel_points):
+ tps = []
+ for k in kernel_points:
+ tps += convex_obstacle.tangent_points(k)
+ if not tps:
+ return shapely.geometry.Polygon([])
+
+ tps = np.unique(tps,axis=0)
+ ch_points = np.vstack((tps, kernel_points))
+ pol = convex_hull(ch_points)
+ return shapely.geometry.Polygon(pol)
+
+
+# TODO: Improve computational consideration
+def polygon_kernel_starshaped_hull(polygon, kernel_points, debug=0):
+ kernel_points = kernel_points.reshape((kernel_points.size//2, 2))
+
+ if kernel_points.shape[0] > 2:
+ # NOTE: Assumes kernel points convex
+ # convex_kernel_subset = shapely.geometry.Polygon(kernel_points[ConvexHull(kernel_points).vertices, :])
+ convex_kernel_subset = shapely.geometry.Polygon(kernel_points)
+
+ # polygon_sh = polygon.polygon() # Shapely represenation of polygon
+ # vertices = np.asarray(polygon_sh.exterior.coords)[:-1, :] # Vertices of polygon
+ vertices = np.asarray(polygon.exterior.coords)[:-1, :] # Vertices of polygon
+ star_vertices = [] # Vertices of starshaped hull polygon
+ v_bar = kernel_points[0].copy() # Last vertex of starshaped hull polygon
+ e1_idx = 0
+ e2_idx = 0
+ k_centroid = np.mean(kernel_points, axis=0)
+ k_included = [False] * kernel_points.shape[0]
+
+ # Arrange vertices such that v_1 is the one with largest x-value and vendv1v2 is CCW, (assumes no collinear vertices in P)
+ start_idx = np.argmax(vertices[:, 0])
+ vertices = np.roll(vertices, -start_idx, axis=0)
+ if is_cw(vertices[-1], vertices[0], vertices[1]):
+ vertices = np.flip(vertices, axis=0)
+ vertices = np.roll(vertices, 1, axis=0)
+ # print("Initial sort: {:.1f}".format(toc(t0)))
+
+ # Iterate through all vertices
+ for v_idx, v in enumerate(vertices):
+ adjust_e1 = False
+ # Check if no ray r(v,kv) intersects with interior of polygon
+ if all([ray(v, k, v).disjoint(polygon) for k in kernel_points]):
+ # Add current vertex
+ if kernel_points.shape[0] < 3 or not convex_kernel_subset.contains(shapely.geometry.Point(v)):
+ star_vertices += [v]
+ if star_vertices:
+ # Intersections of lines l(k,v) and l(e1,e2)
+ e1, e2 = star_vertices[e1_idx], star_vertices[e2_idx]
+ e1_e2 = line(e1, e2)
+
+ for k in kernel_points:
+ kv_e1e2_intersect = line(k,v).intersection(e1_e2)
+ # Adjust to closest intersection to e2
+ if not kv_e1e2_intersect.is_empty:
+ adjust_e1 = True
+ e1_candidate = np.array([kv_e1e2_intersect.x, kv_e1e2_intersect.y])
+ if np.linalg.norm(e2 - e1_candidate) < np.linalg.norm(e2 - star_vertices[e1_idx]):
+ star_vertices[e1_idx] = e1_candidate
+
+ if not adjust_e1:
+ for k_idx, k in enumerate(kernel_points):
+ kps = [kp for kp in kernel_points if not np.array_equal(kp, k)]
+ kv_P_intersect = line(k, v).intersection(polygon)
+
+ # If l(k,v) intersects interior of P
+ if not kv_P_intersect.is_empty:
+ # Find last intersection of l(k,v) and polygon boundary
+ if kv_P_intersect.geom_type == 'LineString':
+ intersection_points = [np.array([ip[0], ip[1]]) for ip in kv_P_intersect.coords]
+ elif kv_P_intersect.geom_type == 'MultiLineString':
+ intersection_points = [np.array([ip[0], ip[1]]) for l in kv_P_intersect.geoms for ip in
+ l.coords]
+ elif kv_P_intersect.geom_type == 'GeometryCollection':
+ intersection_points = []
+ for g in kv_P_intersect.geoms:
+ if g.geom_type == 'Point':
+ intersection_points += [np.array(g.coords[0])]
+ if kv_P_intersect.geom_type == 'LineString':
+ intersection_points += [np.array([ip[0], ip[1]]) for ip in g.coords]
+ else:
+ intersection_points = []
+
+ u = None
+ u_v = None
+ for u_candidate in intersection_points:
+ u_v = line(u_candidate, v)
+ if u_v.disjoint(polygon):
+ u = u_candidate
+ break
+ if u is None:
+ continue
+
+ # If no ray r(u,k'v) intersect with interior of polygon
+ if not any([ray(u, kp, v).intersects(polygon) for kp in kps]):
+ # Adjust u if l(k',v_bar) intersects l(u,v)
+ for kp in kps:
+ kpvb_uv_intersect = line(kp, v_bar).intersection(u_v)
+ if not kpvb_uv_intersect.is_empty:
+ u = np.array([kpvb_uv_intersect.x, kpvb_uv_intersect.y])
+ # Append u to P*
+ star_vertices += [u]
+ # Update last augmented edge
+ e1_idx, e2_idx = len(star_vertices)-1, len(star_vertices)-2
+ # Swap last vertices if not CCW
+ if is_ccw(u, v, vertices[v_idx-1]):
+ # if is_ccw(v_bar, v, u):
+ # if is_cw(k_centroid, v, u):
+ star_vertices[-2], star_vertices[-1] = star_vertices[-1], star_vertices[-2]
+ e1_idx, e2_idx = e2_idx, e1_idx
+ adjust_e1 = True
+ else:
+ # Check if no ray r(k,k'v) intersect with interior of polygon
+ if (not k_included[k_idx]) and (not any([ray(k, kp, v).intersects(polygon) for kp in kps])):
+ k_included[k_idx] = True
+ # Append k to P*
+ star_vertices += [k]
+ # Update last augmented edge
+ e1_idx, e2_idx = len(star_vertices)-1, len(star_vertices)-2
+ # Swap last vertices if not CCW
+ if is_ccw(k, v, vertices[v_idx-1]):
+ # if is_cw(k_centroid, v, k):
+ star_vertices[-2], star_vertices[-1] = star_vertices[-1], star_vertices[-2]
+ e1_idx, e2_idx = e2_idx, e1_idx
+ adjust_e1 = True
+ # Update v_bar
+ v_bar = star_vertices[-1]
+
+ # Visualize debug information
+ if debug == 1:
+ plt.plot(*k_centroid, 'ko')
+ plt.plot(*polygon.exterior.xy, 'k')
+ plt.plot([p[0] for p in star_vertices], [p[1] for p in star_vertices], 'g-o', linewidth=2)
+ [plt.plot(*k, 'kx') for k in kernel_points]
+ [plt.plot(*line(k,v).xy, 'k--') for k in kernel_points]
+ if adjust_e1:
+ plt.plot(*star_vertices[e1_idx], 'ys')
+ plt.show()
+
+ # Check not added kernel points if they should be included
+ for j in range(len(star_vertices)):
+ v, vp = star_vertices[j - 1], star_vertices[j]
+ for k_idx, k in enumerate(kernel_points):
+ if (not k_included[k_idx]) and is_cw(k, v, vp):
+ k_included[k_idx] = True
+ # Insert k
+ star_vertices = star_vertices[:j] + [k] + star_vertices[j:]
+ # Visualize debug information
+ if debug == 1:
+ plt.plot(*k_centroid, 'ko')
+ plt.plot(*polygon.exterior.xy, 'k')
+ plt.plot([p[0] for p in star_vertices], [p[1] for p in star_vertices], 'g-o', linewidth=2)
+ [plt.plot(*ki, 'kx') for ki in kernel_points]
+ plt.plot(*line(k, v).xy, 'r--*')
+ plt.plot(*line(k, vp).xy, 'r--*')
+ plt.plot(*k, 'go')
+ plt.show()
+ # print("Final kernel check: {:.1f}".format(toc(t0)))
+
+ if debug:
+ ax = plt.gca()
+ ax.plot(*polygon.exterior.xy, 'k')
+ ax.plot([p[0] for p in star_vertices] + [star_vertices[0][0]],
+ [p[1] for p in star_vertices] + [star_vertices[0][1]], 'g-o', linewidth=2)
+ # [ax.plot(star_vertices[i][0], star_vertices[i][1], 'r*') for i in augmented_vertex_idcs]
+ [ax.plot(*zip(k, sv), 'y--') for sv in star_vertices for k in kernel_points]
+ ax.plot(*k_centroid, 'bs')
+ plt.show()
+
+ return shapely.geometry.Polygon(star_vertices)
+
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/starworlds.egg-info/PKG-INFO b/python/ur_simple_control/path_generation/star_navigation/starworlds/starworlds.egg-info/PKG-INFO
new file mode 100644
index 0000000000000000000000000000000000000000..4dee13fc6c070e01a3bca7b804984f6d9645b9f5
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/starworlds.egg-info/PKG-INFO
@@ -0,0 +1,9 @@
+Metadata-Version: 2.1
+Name: starworlds
+Version: 1.0
+License-File: LICENSE
+Requires-Dist: pyyaml
+Requires-Dist: numpy
+Requires-Dist: scipy
+Requires-Dist: matplotlib
+Requires-Dist: shapely
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/starworlds.egg-info/SOURCES.txt b/python/ur_simple_control/path_generation/star_navigation/starworlds/starworlds.egg-info/SOURCES.txt
new file mode 100644
index 0000000000000000000000000000000000000000..bad89832078a424410d19018171475813882ed9a
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/starworlds.egg-info/SOURCES.txt
@@ -0,0 +1,28 @@
+LICENSE
+README.md
+requirements.txt
+setup.py
+obstacles/__init__.py
+obstacles/ellipse.py
+obstacles/motion_model.py
+obstacles/obstacle.py
+obstacles/polygon.py
+obstacles/starshaped_obstacle.py
+obstacles/starshaped_polygon.py
+obstacles/starshaped_primitive_combination.py
+starshaped_hull/__init__.py
+starshaped_hull/cluster_and_starify.py
+starshaped_hull/starshaped_hull.py
+starworlds.egg-info/PKG-INFO
+starworlds.egg-info/SOURCES.txt
+starworlds.egg-info/dependency_links.txt
+starworlds.egg-info/requires.txt
+starworlds.egg-info/top_level.txt
+tests/test_cluster_and_starify.py
+tests/test_obstacles.py
+tests/test_starshaped_hull.py
+tests/test_timing.py
+tests/test_utils.py
+utils/__init__.py
+utils/cg.py
+utils/misc.py
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/starworlds.egg-info/dependency_links.txt b/python/ur_simple_control/path_generation/star_navigation/starworlds/starworlds.egg-info/dependency_links.txt
new file mode 100644
index 0000000000000000000000000000000000000000..8b137891791fe96927ad78e64b0aad7bded08bdc
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/starworlds.egg-info/dependency_links.txt
@@ -0,0 +1 @@
+
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/starworlds.egg-info/requires.txt b/python/ur_simple_control/path_generation/star_navigation/starworlds/starworlds.egg-info/requires.txt
new file mode 100644
index 0000000000000000000000000000000000000000..f849890421afe2baa719e8822ed041b4c0ea68da
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/starworlds.egg-info/requires.txt
@@ -0,0 +1,5 @@
+pyyaml
+numpy
+scipy
+matplotlib
+shapely
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/starworlds.egg-info/top_level.txt b/python/ur_simple_control/path_generation/star_navigation/starworlds/starworlds.egg-info/top_level.txt
new file mode 100644
index 0000000000000000000000000000000000000000..35cb7de772c37f5866607798fbfd9e7e835bff72
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/starworlds.egg-info/top_level.txt
@@ -0,0 +1,3 @@
+obstacles
+starshaped_hull
+utils
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/tests/test_cluster_and_starify.py b/python/ur_simple_control/path_generation/star_navigation/starworlds/tests/test_cluster_and_starify.py
new file mode 100644
index 0000000000000000000000000000000000000000..0461e20f4aaa1b680a8ce87348f9b1700086117f
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/tests/test_cluster_and_starify.py
@@ -0,0 +1,129 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from obstacles import Ellipse, StarshapedPolygon
+from obstacles import motion_model as mm
+from utils import generate_convex_polygon, draw_shapely_polygon
+from starshaped_hull import cluster_and_starify, draw_clustering, draw_adm_ker, draw_star_hull
+import shapely
+
+
+def test_cluster_and_starify():
+ n_obstacles = 40
+ ellipse_fraction = .5
+ ell_radius_mean, ell_radius_std = 1, 0.2
+ n_vertices, pol_box = 8, [2, 2]
+ target_scene_coverage = 0.3
+ epsilon = 0.2
+
+ np.random.seed(0)
+
+ def random_scene_point(scene_width):
+ return np.random.rand(2) * scene_width
+
+ def select_x_xg(scene_width, obstacles):
+ x = random_scene_point(scene_width)
+ while any([o.interior_point(x) for o in obstacles]):
+ x = random_scene_point(scene_width)
+ xg = random_scene_point(scene_width)
+ while any([o.interior_point(xg) for o in obstacles]):
+ xg = random_scene_point(scene_width)
+ return x, xg
+
+ # Generate obstacles
+ Nell = int(n_obstacles * ellipse_fraction)
+ Npol = n_obstacles - Nell
+ obstacles = [Ellipse(a=np.random.normal(ell_radius_mean, ell_radius_std, 2), n_pol=10) for j in range(Nell)]
+ obstacles += [StarshapedPolygon(generate_convex_polygon(n_vertices, pol_box), xr=[0, 0], is_convex=True) for j in range(Npol)]
+
+ # Compute area data
+ obstacle_area = sum([o.area() for o in obstacles])
+
+ # Setup scene
+ scene_width = np.sqrt(obstacle_area / target_scene_coverage * 0.9)
+
+ for j in range(Nell):
+ # obstacles[j].set_motion_model(ob.Static(random_scene_point(res['scene_width'][i])))
+ obstacles[j].set_motion_model(mm.Static(random_scene_point(scene_width - 2 * ell_radius_mean) + ell_radius_mean))
+ for j in range(Npol):
+ # obstacles[Nell+j].set_motion_model(ob.Static(random_scene_point(res['scene_width'][i])))
+ obstacles[Nell + j].set_motion_model(mm.Static(random_scene_point(scene_width - pol_box[0]) + pol_box[0] / 2))
+ [obs.polygon() for obs in obstacles]
+
+ # Select collision free robot and goal positions
+ x, xg = select_x_xg(scene_width, obstacles)
+ # Cluster and starify
+ clusters, timing, flag, n_iter, cluster_history = cluster_and_starify(obstacles, x, xg, epsilon, make_convex=0,
+ verbose=1, return_history=1, timing_verbose=1)
+
+ # Draw iteration steps
+ for i, clusters_i in enumerate(cluster_history[1:]):
+ _, ax = draw_clustering(clusters_i, x, xg, xlim=[0, scene_width], ylim=[0, scene_width])
+ ax.set_title("Clustering, Iteration {}/{}".format(i+1, len(cluster_history)))
+ fig, axs = draw_adm_ker(clusters_i, x, xg, xlim=[-0.2*scene_width, 1.2*scene_width], ylim=[-0.2*scene_width, 1.2*scene_width])
+ fig.suptitle("Admissible Kernel, Iteration {}/{}".format(i+1, len(cluster_history)))
+ _, ax = draw_star_hull(clusters_i, x, xg, xlim=[0, scene_width], ylim=[0, scene_width])
+ ax.set_title("Starshaped Hull, Iteration {}/{}".format(i+1, len(cluster_history)))
+
+
+def test_moving_cluster():
+ obstacles = [
+ Ellipse([1, 1], motion_model=mm.Static([-1, 0.3])),
+ Ellipse([1, 1], motion_model=mm.Static([0., 0.3])),
+ Ellipse([1, 1], motion_model=mm.Static([1, 0.3]))
+ ]
+ p0 = np.array([0.1, -2.5])
+ pg = np.array([0.1, 2.5])
+ epsilon = 0.2
+ xlim = [-3, 3]
+ ylim = [-3, 3]
+
+ clusters, timing, flag, n_iter = cluster_and_starify(obstacles, p0, pg, epsilon)
+
+ fig, axs = plt.subplots(1, 3)
+ [o.draw(ax=axs[0], show_reference=0, fc='lightgrey', ec='k', alpha=0.8) for o in obstacles]
+ axs[0].plot(*p0, 'ko')
+ axs[0].plot(*pg, 'k*')
+ axs[0].plot(*clusters[0].cluster_obstacle.xr(), 'gd')
+ axs[0].plot(*zip(p0, pg), '--')
+ axs[0].set_xlim(xlim)
+ axs[0].set_ylim(ylim)
+
+ xr_prev = clusters[0].cluster_obstacle.xr()
+ obstacles[0].set_motion_model(mm.Static([-1, -0.3]))
+ obstacles[1].set_motion_model(mm.Static([0., -0.3]))
+ obstacles[2].set_motion_model(mm.Static([1, -0.3]))
+ clusters, timing, flag, n_iter = cluster_and_starify(obstacles, p0, pg, epsilon, previous_clusters=clusters)
+
+ [o.draw(ax=axs[1], show_reference=0, fc='lightgrey', ec='k', alpha=0.8) for o in obstacles]
+ axs[1].plot(*p0, 'ko')
+ axs[1].plot(*pg, 'k*')
+ axs[1].plot(*xr_prev, 'sk')
+ axs[1].plot(*clusters[0].cluster_obstacle.xr(), 'gd')
+ axs[1].plot(*zip(p0, pg), '--')
+ axs[1].set_xlim(xlim)
+ axs[1].set_ylim(ylim)
+
+ xr_prev = clusters[0].cluster_obstacle.xr()
+ obstacles[0].set_motion_model(mm.Static([-1, -1]))
+ obstacles[1].set_motion_model(mm.Static([0., -1]))
+ obstacles[2].set_motion_model(mm.Static([1, -1]))
+ clusters, timing, flag, n_iter = cluster_and_starify(obstacles, p0, pg, epsilon, previous_clusters=clusters)
+
+ x_xg_line = shapely.geometry.LineString([p0, pg])
+ kernel_selection_set = shapely.ops.split(clusters[0].cluster_obstacle.polygon(), x_xg_line).geoms[0]
+
+ [o.draw(ax=axs[2], show_reference=0, fc='lightgrey', ec='k', alpha=0.8) for o in obstacles]
+ draw_shapely_polygon(kernel_selection_set, ax=axs[2], hatch='///', fill=False, linestyle='None')
+ axs[2].plot(*p0, 'ko')
+ axs[2].plot(*pg, 'k*')
+ axs[2].plot(*xr_prev, 'sk')
+ axs[2].plot(*clusters[0].cluster_obstacle.xr(), 'gd')
+ axs[2].plot(*zip(p0, pg), '--')
+ axs[2].set_xlim(xlim)
+ axs[2].set_ylim(ylim)
+
+
+if (__name__) == "__main__":
+ test_cluster_and_starify()
+ # test_moving_cluster()
+ plt.show()
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/tests/test_obstacles.py b/python/ur_simple_control/path_generation/star_navigation/starworlds/tests/test_obstacles.py
new file mode 100644
index 0000000000000000000000000000000000000000..2839f4036d8d58afac21a42b6d0c7daf42e4c8b4
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/tests/test_obstacles.py
@@ -0,0 +1,147 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from obstacles import Ellipse, Polygon, StarshapedPolygon, StarshapedPrimitiveCombination, Frame
+from obstacles import motion_model as mm
+from utils import generate_convex_polygon, draw_shapely_polygon, generate_star_polygon
+import shapely.geometry
+
+
+def test_ellipse():
+ ell_axes = [2, 1]
+ ell_pos = [0, 0.5]
+ xlim = [ell_pos[0] - 2 * ell_axes[0], ell_pos[0] + 2 * ell_axes[0]]
+ ylim = [ell_pos[1] - 2 * ell_axes[1], ell_pos[1] + 2 * ell_axes[1]]
+ ell = Ellipse(ell_axes, xr=[0, .9], motion_model=mm.Static(ell_pos, 1))
+ while True:
+ x = np.array([np.random.uniform(*xlim),np.random.uniform(*ylim)])
+ if ell.exterior_point(x):
+ break
+ b = ell.boundary_mapping(x)
+ n = ell.normal(x)
+ tp = ell.tangent_points(x)
+ dir = ell.reference_direction(x)
+
+ _, ax = ell.draw()
+ ax.plot(*zip(ell.xr(Frame.GLOBAL), x), 'k--o')
+ if b is not None:
+ ax.plot(*b, 'y+')
+ ax.quiver(*b, *n)
+ if tp:
+ ax.plot(*zip(x, tp[0]), 'g:')
+ ax.plot(*zip(x, tp[1]), 'g:')
+ ax.quiver(*ell.xr(Frame.GLOBAL), *dir, color='c', zorder=3)
+ ax.set_xlim(xlim)
+ ax.set_ylim(ylim)
+
+
+def test_nonstar_polygon():
+ pass
+
+
+def test_star_polygon():
+ avg_radius = 1
+ xlim = [-2*avg_radius, 2*avg_radius]
+ ylim = xlim
+ pol = StarshapedPolygon(generate_star_polygon([0, 0], avg_radius, irregularity=0.3, spikiness=0.5, num_vertices=10))
+
+ while True:
+ x = np.array([np.random.uniform(*xlim), np.random.uniform(*ylim)])
+ if pol.exterior_point(x):
+ break
+ b = pol.boundary_mapping(x)
+ n = pol.normal(x)
+ tp = pol.tangent_points(x)
+ dir = pol.reference_direction(x)
+
+ _, ax = pol.draw()
+ ax.plot(*zip(pol.xr(Frame.GLOBAL), x), 'k--o')
+ if b is not None:
+ ax.plot(*b, 'y+')
+ ax.quiver(*b, *n)
+ if tp:
+ ax.plot(*zip(x, tp[0]), 'g:')
+ ax.plot(*zip(x, tp[1]), 'g:')
+ ax.quiver(*pol.xr(Frame.GLOBAL), *dir, color='c', zorder=3)
+
+ for i in np.linspace(0, 2 * np.pi, 100):
+ x = pol.xr() + 100*np.array([np.cos(i), np.sin(i)])
+ b = pol.boundary_mapping(x)
+ n = pol.normal(b)
+ ax.quiver(*b, *n)
+ ax.set_xlim(xlim)
+ ax.set_ylim(ylim)
+ print("es")
+
+def test_star_primitive_combination():
+ n_ellipses = 3
+ n_polygons = 2
+ n_vertices = 6
+ box_width = 2
+
+ ell_axes = [0.7, 0.4]
+ pol_box = [box_width, box_width]
+
+ xlim = [-box_width, box_width]
+ ylim = [-box_width, box_width]
+ ellipses = [Ellipse(ell_axes) for i in range(n_ellipses)]
+ polygons = [StarshapedPolygon(generate_convex_polygon(n_vertices, pol_box), is_convex=True)
+ for i in range(n_polygons)]
+ obstacles = ellipses + polygons
+ while True:
+ # Generate new positions
+ for obs in obstacles:
+ obs.set_motion_model(mm.Static(np.random.uniform(-0.2*box_width, 0.2*box_width, 2), np.random.uniform(0, 2*np.pi)))
+
+ # Identify if all obstacle form a single cluster
+ kernel = obstacles[0].polygon()
+ for obs in obstacles[1:]:
+ kernel = kernel.intersection(obs.polygon())
+
+ if not kernel.is_empty:
+ break
+ else:
+ _, ax = plt.subplots()
+ [obs.draw(ax=ax, fc='r', alpha=0.2, ec='k') for obs in obstacles]
+ ax.set_xlim(xlim)
+ ax.set_ylim(ylim)
+ plt.show()
+
+ xr = np.array(kernel.representative_point().coords[0])
+ star_obs = StarshapedPrimitiveCombination(obstacles, shapely.geometry.Polygon([]), xr)
+
+ while True:
+ x = np.array([np.random.uniform(*xlim), np.random.uniform(*ylim)])
+ if star_obs.exterior_point(x):
+ break
+ b = star_obs.boundary_mapping(x)
+ n = star_obs.normal(x)
+ tp = star_obs.tangent_points(x)
+ dir = star_obs.reference_direction(x)
+
+ _, ax = star_obs.draw()
+ draw_shapely_polygon(kernel, ax=ax, fc='g')
+ ax.plot(*zip(star_obs.xr(Frame.GLOBAL), x), 'k--o')
+ if b is not None:
+ ax.plot(*b, 'y+')
+ ax.quiver(*b, *n)
+ if tp:
+ ax.plot(*zip(x, tp[0]), 'g:')
+ ax.plot(*zip(x, tp[1]), 'g:')
+ ax.quiver(*star_obs.xr(Frame.GLOBAL), *dir, color='c', zorder=3)
+
+ for i in np.linspace(0, 2 * np.pi, 100):
+ x = star_obs.xr() + np.array([np.cos(i), np.sin(i)])
+ b = star_obs.boundary_mapping(x)
+ n = star_obs.normal(b)
+ # ax.quiver(*b, *n)
+
+ ax.set_xlim(xlim)
+ ax.set_ylim(ylim)
+
+
+if (__name__) == "__main__":
+ # test_ellipse()
+ # test_nonstar_polygon()
+ # test_star_polygon()
+ test_star_primitive_combination()
+ plt.show()
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/tests/test_starshaped_hull.py b/python/ur_simple_control/path_generation/star_navigation/starworlds/tests/test_starshaped_hull.py
new file mode 100644
index 0000000000000000000000000000000000000000..670a63616cdea33d59d825bb4067a08765a0b00d
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/tests/test_starshaped_hull.py
@@ -0,0 +1,53 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from starshaped_hull import admissible_kernel
+from utils import draw_shapely_polygon, Cone
+from obstacles import Polygon, Ellipse
+from obstacles import motion_model as mm
+
+def test_admissible_kernel():
+ pol = Polygon([[0, 0], [1, 0], [2, 2], [2, 4], [-0.75, 1], [-1, 4], [-2, 2], [-2, 1], [-1.5, 0], [-2, -1]])
+
+ x_exclude = np.array([0, 2])
+
+ ad_ker = admissible_kernel(pol, x_exclude)
+
+ _, ax = pol.draw()
+ draw_shapely_polygon(ad_ker.polygon(), ax=ax, fc='y', alpha=0.6)
+ ax.plot(x_exclude[0], x_exclude[1], 'ro')
+ ax.set_xlim([-3, 6])
+ ax.set_ylim([-1, 5])
+
+
+def test_adm_ker_ellipse():
+ ell = Ellipse([1.08877265, 1.06673487], motion_model=mm.Static([4.61242385, 1.87941425]))
+ p = np.array([3.641344, 4.87955125])
+ pg = [0.73012219, 8.95180958]
+ adm_ker_p = admissible_kernel(ell, p)# Cone.list_intersection([adm_ker_robot_cones[o.id()] for o in cl.obstacles])
+ adm_ker_pg = admissible_kernel(ell, pg)
+ ad_ker = adm_ker_p.intersection(adm_ker_pg)
+
+ _, ax = ell.draw()
+ draw_shapely_polygon(ad_ker, ax=ax, fc='y', alpha=0.6)
+ draw_shapely_polygon(adm_ker_p.polygon(), ax=ax, fc='None', ec='k')
+ draw_shapely_polygon(adm_ker_pg.polygon(), ax=ax, fc='None', ec='k')
+ ax.plot(*p, 'rx')
+ ax.plot(*pg, 'rx')
+ ax.set_xlim(0, 10)
+ ax.set_ylim(0, 10)
+
+
+def test_kernel_starshaped_hull_single():
+ pass
+
+
+def test_kernel_starshaped_hull_cluster():
+ pass
+
+
+if (__name__) == "__main__":
+ test_admissible_kernel()
+ test_adm_ker_ellipse()
+ test_kernel_starshaped_hull_single()
+ test_kernel_starshaped_hull_cluster()
+ plt.show()
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/tests/test_timing.py b/python/ur_simple_control/path_generation/star_navigation/starworlds/tests/test_timing.py
new file mode 100644
index 0000000000000000000000000000000000000000..068ff3bcb7db0fa8077c7586bf29ec78c75f1d8a
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/tests/test_timing.py
@@ -0,0 +1,173 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from obstacles import Ellipse, StarshapedPolygon
+from obstacles import motion_model as mm
+from utils import generate_convex_polygon
+from starshaped_hull import cluster_and_starify, draw_clustering, draw_adm_ker, draw_star_hull
+import shapely
+
+
+def test_cluster_and_starify_compute():
+ par = {'N_samples': 1000, 'pol_Nvert': 10, 'ell_n_pol': 30, 'ell_fraction': 0.5, 'pol_box': [2, 2],
+ 'ell_radius_mean': 1., 'ell_radius_std': 0.2, 'target_scene_coverage': 0.25,
+ 'No_min': 5, 'No_max': 50, 'rd_seed': 0, 'epsilon': 0.1}
+
+ plot_fail = 0
+ # ----- Data generation ------ #
+ np.random.seed(par['rd_seed'])
+ # Result data
+ res = {'compute_time': np.zeros((par['N_samples'], 4)), 'n_iter': np.zeros(par['N_samples'], dtype=np.int32),
+ 'No': np.zeros(par['N_samples'], dtype=np.int32), 'Ncl': np.zeros(par['N_samples'], dtype=np.int32),
+ 'obstacle_area': np.zeros(par['N_samples']), 'obstacle_coverage': np.zeros(par['N_samples']),
+ 'scene_width': np.zeros(par['N_samples']), 'scene_coverage': np.zeros(par['N_samples'])}
+
+ def random_scene_point(scene_width):
+ return np.random.rand(2) * scene_width
+
+ def select_x_xg(scene_width, obstacles):
+ x = random_scene_point(scene_width)
+ while any([o.interior_point(x) for o in obstacles]):
+ x = random_scene_point(scene_width)
+ xg = random_scene_point(scene_width)
+ while any([o.interior_point(xg) for o in obstacles]):
+ xg = random_scene_point(scene_width)
+ return x, xg
+
+ for i in range(par['N_samples']):
+ # Generate obstacles
+ res['No'][i] = np.random.randint(par['No_min'], par['No_max'] + 1)
+ Nell = int(res['No'][i] * par['ell_fraction'])
+ Npol = res['No'][i] - Nell
+ obstacles = [
+ Ellipse(a=np.random.normal(par['ell_radius_mean'], par['ell_radius_std'], 2), n_pol=par['ell_n_pol'])
+ for j in range(Nell)]
+ obstacles += [StarshapedPolygon(generate_convex_polygon(par['pol_Nvert'], par['pol_box']), xr=[0, 0],
+ is_convex=True) for j in range(Npol)]
+
+ # Compute area data
+ res['obstacle_area'][i] = sum([o.area() for o in obstacles])
+
+ # Setup scene
+ res['scene_width'][i] = np.sqrt(res['obstacle_area'][i] / par['target_scene_coverage'] * 0.85)
+
+ for j in range(Nell):
+ # obstacles[j].set_motion_model(ob.Static(random_scene_point(res['scene_width'][i])))
+ obstacles[j].set_motion_model(mm.Static(
+ random_scene_point(res['scene_width'][i] - 2 * par['ell_radius_mean']) + par['ell_radius_mean']))
+ for j in range(Npol):
+ # obstacles[Nell+j].set_motion_model(ob.Static(random_scene_point(res['scene_width'][i])))
+ obstacles[Nell + j].set_motion_model(
+ mm.Static(random_scene_point(res['scene_width'][i] - par['pol_box'][0]) + par['pol_box'][0] / 2))
+
+ # Compute coverage data
+ res['obstacle_coverage'][i] = shapely.ops.unary_union([o.polygon() for o in obstacles]).area
+ res['scene_coverage'][i] = res['obstacle_coverage'][i] / (res['scene_width'][i] ** 2)
+
+ flag = 0
+ n_failures = -1
+ while flag == 0:
+ n_failures += 1
+ # Select collision free robot and goal positions
+ x, xg = select_x_xg(res['scene_width'][i], obstacles)
+ # Cluster and starify
+ clusters, timing, flag, res['n_iter'][i] = cluster_and_starify(obstacles, x, xg, par['epsilon'],
+ exclude_obstacles=0, make_convex=0,
+ max_iterations=100,
+ verbose=0)
+
+ star_obstacles = [cl.cluster_obstacle for cl in clusters]
+ res['compute_time'][i, :] = timing
+ res['Ncl'][i] = len(clusters)
+
+ if plot_fail and flag == 0:
+ _, ax_i = plt.subplots()
+ [o.draw(ax=ax_i, fc='g', alpha=0.8) for o in star_obstacles]
+ [o.draw(ax=ax_i, show_name=1, show_reference=0, ec='k', linestyle='--') for o in obstacles]
+ ax_i.plot(*x, 'rx', markersize=16)
+ ax_i.plot(*xg, 'rx', markersize=16)
+ ax_i.set_xlim([0, res['scene_width'][i]])
+ ax_i.set_ylim([0, res['scene_width'][i]])
+ if flag == 0:
+ ax_i.set_title(
+ 'Fail\nSample: {}, #O: {}, #Cl: {}, Time: {:.1f}, It: {}, Scene coverage: {:.2f}({:.2f})'.format(
+ i, res['No'][i], res['Ncl'][i], sum(res['compute_time'][i, :]), res['n_iter'][i],
+ res['scene_coverage'][i], par['target_scene_coverage']))
+ else:
+ ax_i.set_title(
+ 'Sample: {}, #O: {}, #Cl: {}, Time: {:.1f}, It: {}, Scene coverage: {:.2f}({:.2f})'.format(
+ i, res['No'][i], res['Ncl'][i], sum(res['compute_time'][i, :]), res['n_iter'][i],
+ res['scene_coverage'][i], par['target_scene_coverage']))
+ plt.show()
+
+ if n_failures == 5:
+ break
+
+ # ----- Postprocessing ------ #
+ total_compute_time = np.sum(res['compute_time'], axis=1)
+ binc_niter = np.bincount(res['n_iter'])[min(res['n_iter']):]
+ print(np.vstack((np.arange(min(res['n_iter']), min(res['n_iter']) + len(binc_niter)), binc_niter)))
+
+ cl = ['r', 'g', 'b', 'y', 'k', 'c']
+ mk = ['o', '+', '^', 'x', 'd', '8']
+ tct_it = [None] * len(binc_niter)
+ No_it = [None] * len(binc_niter)
+ ct_it = [[]] * len(binc_niter)
+ scene_coverage_it = [None] * len(binc_niter)
+ for i in range(len(binc_niter)):
+ scene_coverage_it[i] = np.ma.masked_where(res['n_iter'] != min(res['n_iter']) + i, res['scene_coverage'])
+ tct_it[i] = np.ma.masked_where(res['n_iter'] != min(res['n_iter']) + i, total_compute_time)
+ No_it[i] = np.ma.masked_where(res['n_iter'] != min(res['n_iter']) + i, res['No'])
+ for j in range(3):
+ ma = np.ma.masked_where(res['n_iter'] != min(res['n_iter']) + i, res['compute_time'][:, j])
+ ct_it[i] += [ma.data]
+
+ timename = ['Clustering', 'Adm Ker', 'St Hull']
+ if max(res['No']) - min(res['No']) > 0:
+ _, axs = plt.subplots(3)
+ for j in range(3):
+ for i in range(len(binc_niter)):
+ axs[j].scatter(No_it[i], ct_it[i][j], c=cl[i], marker=mk[i])
+ axs[j].set_xlabel('No'), axs[j].set_ylabel('Time [ms]')
+ axs[j].set_title(timename[j])
+ plt.tight_layout()
+
+ _, axs = plt.subplots(1, 2)
+ # ax.scatter(res['No'], total_compute_time)
+ for i in range(len(binc_niter)):
+ axs[0].scatter(No_it[i], tct_it[i], c=cl[i], marker=mk[i], s=np.square(scene_coverage_it[i] * 20))
+ axs[1].scatter(scene_coverage_it[i], tct_it[i], c=cl[i], marker=mk[i], s=No_it[i] * 2)
+
+ _, ax = plt.subplots()
+ ax.scatter(res['No'], np.divide(res['compute_time'][:, 0], total_compute_time), color='r')
+ ax.scatter(res['No'], np.divide(res['compute_time'][:, 1], total_compute_time), color='g')
+ ax.scatter(res['No'], np.divide(res['compute_time'][:, 2], total_compute_time), color='b')
+ ax.set_title("Clustering fraction of total"), ax.set_xlabel('No'), ax.set_ylabel('Cl time / Tot time')
+
+ _, axs = plt.subplots(2, 2)
+ axs[0, 0].scatter(res['No'], res['obstacle_area'])
+ axs[0, 0].scatter(res['No'], res['obstacle_coverage']), axs[0, 0].set_title('Obstacle area')
+ axs[0, 0].set_xlabel('#Obstacles'), axs[1, 0].set_ylabel('Obstacle area')
+ axs[1, 0].scatter(res['scene_coverage'], res['n_iter']), axs[1, 0].set_title('#Iterations')
+ axs[1, 0].set_xlabel('Scene coverage'), axs[1, 0].set_ylabel('#Iterations')
+ axs[0, 1].scatter(res['No'], np.divide(res['Ncl'], res['No'])), axs[0, 1].set_title('#Clusters')
+ axs[0, 1].set_xlabel('#Obstacles'), axs[0, 1].set_ylabel('#Clusters/#Obstacles')
+ axs[1, 1].scatter(res['scene_coverage'], np.divide(res['Ncl'], res['No'])), axs[1, 1].set_title('#Clusters/#No')
+ axs[1, 1].set_xlabel('Scene coverage'), axs[1, 1].set_ylabel('#Clusters/#Obstacles')
+ plt.tight_layout()
+
+ scene_coverage = res['scene_coverage'] * 100
+ cov_span = max(scene_coverage) - min(scene_coverage)
+ binwidth = 1
+ _, ax = plt.subplots()
+ ax.hist(scene_coverage, bins=int(cov_span / binwidth),
+ color='blue', edgecolor='black')
+ ymin, ymax = ax.get_ylim()
+ ax.plot([par['target_scene_coverage'] * 100, par['target_scene_coverage'] * 100], [ymin, ymax], 'r--')
+ ax.set_title('Histogram with Binwidth = %d' % binwidth, size=30)
+ ax.set_xlabel('Coverage', size=22)
+ ax.set_ylabel('Samples', size=22)
+
+
+if (__name__) == "__main__":
+ test_cluster_and_starify_compute()
+ plt.show()
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/tests/test_utils.py b/python/ur_simple_control/path_generation/star_navigation/starworlds/tests/test_utils.py
new file mode 100644
index 0000000000000000000000000000000000000000..54cac6a5e2d79d03452cf847b41117d1674dd59b
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/tests/test_utils.py
@@ -0,0 +1,106 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from utils import Cone, draw_shapely_polygon
+
+
+def test_affine_transformation():
+ pass
+
+
+def test_point_in_triangle():
+ pass
+
+
+def test_orientation():
+ pass
+
+
+def test_equilateral_triangle():
+ pass
+
+
+def test_convex_hull():
+ pass
+
+
+def test_generate_convex_polygon():
+ pass
+
+
+def test_generate_star_polygon():
+ pass
+
+
+def test_cone():
+ n_cones_sqrt = 4
+ n_cones_same_apex = 4
+ n_points = 8
+ n_several_cone_intersection = 2
+ boxlim = [-1, 1]
+
+ fig_int_sev, axs_int_sev = plt.subplots(n_cones_sqrt, n_cones_sqrt)
+ fig_int, axs_int = plt.subplots(n_cones_sqrt, n_cones_sqrt)
+ fig_p, axs_p = plt.subplots(n_cones_sqrt, n_cones_sqrt)
+ for i in range(n_cones_sqrt ** 2):
+ # Test cone intersection
+ c_params = np.random.uniform(*boxlim, (2, 3))
+ c1 = Cone(c_params[:, 0], c_params[:, 1] - c_params[:, 0], c_params[:, 2] - c_params[:, 0])
+ c2_params = np.random.uniform(*boxlim, (2, 3))
+ c2_apex = c_params[:, 0] if i < n_cones_same_apex else c2_params[:, 0]
+ c2 = Cone(c2_apex, c2_params[:, 1] - c2_apex, c2_params[:, 2] - c2_apex)
+ c_several_intersect = [c1, c2]
+ for j in range(1, n_several_cone_intersection):
+ c3_params = np.random.uniform(*boxlim, (2, 3))
+ c3_apex = c_params[:, 0] if i < n_cones_same_apex else c3_params[:, 0]
+ c_several_intersect += [Cone(c3_apex, c3_params[:, 1] - c3_apex, c3_params[:, 2] - c3_apex)]
+
+ two_cones_intersection = c1.intersection(c2)
+ several_cones_intersection = Cone.list_intersection(c_several_intersect, same_apex=i<n_cones_same_apex)
+
+ # Two cones intersection plot
+ fig_int.suptitle("Intersection of two cones")
+ ax_int_i = axs_int[i//n_cones_sqrt, i%n_cones_sqrt]
+ c1.draw(ax=ax_int_i, color='b', alpha=0.2, ray_color='b')
+ c2.draw(ax=ax_int_i, color='r', alpha=0.2, ray_color='r')
+ if not two_cones_intersection.is_empty and not two_cones_intersection.geom_type == 'Point':
+ draw_shapely_polygon(two_cones_intersection, ax=ax_int_i, color='k', alpha=0.2, hatch='///')
+ ax_int_i.set_xlim(1.2 * np.array(boxlim))
+ ax_int_i.set_ylim(1.2 * np.array(boxlim))
+
+ # Several cones intersection plot
+ fig_int_sev.suptitle("Intersection of several cones")
+ color = plt.cm.rainbow(np.linspace(0, 1, 2*n_several_cone_intersection))
+ ax_int_sev_i = axs_int_sev[i//n_cones_sqrt, i%n_cones_sqrt]
+ for j, c in enumerate(c_several_intersect):
+ c.draw(ax=ax_int_sev_i, color=color[j], alpha=0.2, ray_color=color[j])
+ if not several_cones_intersection.is_empty:
+ draw_shapely_polygon(several_cones_intersection, ax=ax_int_sev_i, color='k', alpha=0.2, hatch='///')
+ ax_int_sev_i.set_xlim(1.2 * np.array(boxlim))
+ ax_int_sev_i.set_ylim(1.2 * np.array(boxlim))
+
+ # Test point in cone
+ fig_p.suptitle("Point in cone")
+ ax_p_i = axs_p[i//n_cones_sqrt, i%n_cones_sqrt]
+ xs = np.linspace(*boxlim, n_points)
+ ys = np.linspace(*boxlim, n_points)
+ XS, YS = np.meshgrid(xs, ys)
+ c1.draw(ax=ax_p_i, color='b', alpha=0.2, ray_color='b')
+ for j in range(n_points):
+ for k in range(n_points):
+ col = 'g' if c1.point_in_cone([XS[j, k], YS[j, k]]) else 'r'
+ ax_p_i.plot(XS[j, k], YS[j, k], marker='.', color=col)
+ ax_p_i.set_xlim(1.2 * np.array(boxlim))
+ ax_p_i.set_ylim(1.2 * np.array(boxlim))
+
+
+if (__name__) == "__main__":
+ test_convex_hull()
+ test_orientation()
+ test_affine_transformation()
+ test_equilateral_triangle()
+ test_orientation()
+ test_generate_convex_polygon()
+ test_generate_star_polygon()
+ test_point_in_triangle()
+ test_cone()
+ plt.show()
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/utils/__init__.py b/python/ur_simple_control/path_generation/star_navigation/starworlds/utils/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..428e2f7fa52d681e0fec32febc1c69117e92bbb4
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/utils/__init__.py
@@ -0,0 +1,2 @@
+from .misc import *
+from .cg import *
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/utils/cg.py b/python/ur_simple_control/path_generation/star_navigation/starworlds/utils/cg.py
new file mode 100644
index 0000000000000000000000000000000000000000..bb7525d45bbaef9404d59fb15fa2c32d9e666f12
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/utils/cg.py
@@ -0,0 +1,593 @@
+import numpy as np
+import shapely.geometry
+import shapely.ops
+import matplotlib.pyplot as plt
+from typing import List, Tuple
+from utils import draw_shapely_polygon
+
+DEFAULT_RAY_INFINITY_LENGTH = 100000.
+COLLINEAR_THRESHOLD = 1e-10
+
+
+def affine_transform(x, rotation, translation, inverse=False):
+ if inverse:
+ x_t = [x[0] - translation[0], x[1] - translation[1]]
+ if rotation == 0:
+ return np.array(x_t)
+ c, s = np.cos(rotation), np.sin(rotation)
+ return np.array([c*x_t[0] + s*x_t[1], -s*x_t[0] + c*x_t[1]])
+ else:
+ if rotation != 0:
+ c, s = np.cos(rotation), np.sin(rotation)
+ return np.array([c*x[0]-s*x[1]+translation[0], s*x[0]+c*x[1]+translation[1]])
+ else:
+ return np.array([x[0]+translation[0], x[1]+translation[1]])
+
+
+# Line segment from a to b, excluding a and b
+def line(a, b):
+ return shapely.geometry.LineString([a + .0001 * (b - a), a + .9999 * (b - a)])
+
+
+# Random point on the triangle with vertices a, b and c
+def point_in_triangle(a, b, c):
+ """
+ """
+ x, y = np.random.rand(2)
+ q = abs(x - y)
+ s, t, u = q, 0.5 * (x + y - q), 1 - 0.5 * (q + x + y)
+ return s * a[0] + t * b[0] + u * c[0], s * a[1] + t * b[1] + u * c[1]
+
+# TODO: Dynamic length of ray
+# Ray emanating from a in direction of b->c
+def ray(a, b, c, ray_inf_length=DEFAULT_RAY_INFINITY_LENGTH):
+ return shapely.geometry.LineString([a + .0001 * (c - b), a + ray_inf_length * (c - b)])
+
+
+def np_orientation_val(a, b, c):
+ return (b[:, 1] - a[:, 1]) * (c[:, 0] - b[:, 0]) - (b[:, 0] - a[:, 0]) * (c[:, 1] - b[:, 1])
+
+
+def orientation_val(a, b, c):
+ return (b[1] - a[1]) * (c[0] - b[0]) - (b[0] - a[0]) * (c[1] - b[1])
+
+
+def is_collinear(a, b, c):
+ return abs(orientation_val(a, b, c)) < COLLINEAR_THRESHOLD
+
+
+def is_cw(a, b, c):
+ return orientation_val(a, b, c) > COLLINEAR_THRESHOLD
+
+
+def is_ccw(a, b, c):
+ return orientation_val(a, b, c) < -COLLINEAR_THRESHOLD
+
+
+# TODO: Use [NOT is_cw and NOT is_ccw] instead?
+# Returns true if point b is between point and b
+def is_between(a, b, c):
+ return np.isclose(np.linalg.norm(a-b) + np.linalg.norm(c-b), np.linalg.norm(a-c), rtol=1e-7)
+
+
+def intersect(a1, a2, b1, b2):
+ return (is_ccw(a1, b1, b2) != is_ccw(a2, b1, b2) and is_ccw(a1, a2, b1) != is_ccw(a1, a2, b2))
+
+
+class Point:
+
+ def __init__(self, x: float, y: float):
+ self.x = x
+ self.y = y
+ self.xy = [x, y]
+
+ def __str__(self):
+ return "Point {:s}".format(str(self.xy))
+
+ def __iter__(self):
+ return iter(self.xy)
+
+ def __getitem__(self, item):
+ return self.xy[item]
+
+ def draw(self, ax=None, marker='o', **kwargs):
+ if ax is None:
+ _, ax = plt.subplots(subplot_kw={'aspect': 'equal'})
+ handles = ax.plot(self.x, self.y, marker=marker, **kwargs)
+ return handles, ax
+
+
+class Line:
+
+ def __init__(self, p1: Point, p2: Point):
+ self.p1 = p1
+ self.p2 = p2
+
+ def __str__(self):
+ return "Line --({:.2f},{:.2f})--({:.2f},{:.2f})--".format(self.p1.x, self.p1.y, self.p2.x, self.p2.y)
+
+ def line_intersection(self, other: 'Line') -> Point:
+ self_dx = self.p1.x - self.p2.x
+ other_dx = other.p1.x - other.p2.x
+ self_dy = self.p1.y - self.p2.y
+ other_dy = other.p1.y - other.p2.y
+ den = self_dx * other_dy - self_dy * other_dx
+ if abs(den) < 1e-10:
+ # Parallel or coincident lines
+ return None
+ tmp1 = self.p1.x * self.p2.y - self.p1.y * self.p2.x
+ tmp2 = other.p1.x * other.p2.y - other.p1.y * other.p2.x
+ ip_x = (tmp1 * other_dx - self_dx * tmp2) / den
+ ip_y = (tmp1 * other_dy - self_dy * tmp2) / den
+ return Point(ip_x, ip_y)
+
+ def intersects(self, other):
+ return (is_ccw(self.p1, other.p1, other.p1) != is_ccw(self.p2, other.p1, other.p1) and is_ccw(self.p1, self.p2, other.p1) != is_ccw(self.p1, self.p2, other.p2))
+
+
+class LineSegment(Line):
+
+ def __str__(self):
+ return "Line segment ({:.2f},{:.2f})--({:.2f},{:.2f})".format(self.p1.x, self.p1.y, self.p2.x, self.p2.y)
+
+ def line_segment_intersection(self, other: 'LineSegment') -> Point:
+ self_dx = self.p1.x - self.p2.x
+ other_dx = other.p1.x - other.p2.x
+ self_dy = self.p1.y - self.p2.y
+ other_dy = other.p1.y - other.p2.y
+ p1_dx = self.p1.x - other.p1.x
+ p1_dy = self.p1.y - other.p1.y
+ den = self_dx * other_dy - self_dy * other_dx
+ if abs(den) < 1e-10:
+ # Parallel or coincident lines
+ return None
+ t = (p1_dx * other_dy - p1_dy * other_dx) / den
+ u = (p1_dx * self_dy - p1_dy * self_dx) / den
+ if t < 0 or t > 1 or u < 0 or u > 1:
+ return None
+ ip_x = self.p1.x - t * self_dx
+ ip_y = self.p1.y - t * self_dy
+ return Point(ip_x, ip_y)
+
+class Ray(Line):
+
+ def __str__(self):
+ return "Ray ({:.2f},{:.2f})--({:.2f},{:.2f})--".format(self.p1.x, self.p1.y, self.p2.x, self.p2.y)
+
+ def ray_intersection(self, other: 'Ray') -> Point:
+ self_dx = self.p1.x - self.p2.x
+ other_dx = other.p1.x - other.p2.x
+ self_dy = self.p1.y - self.p2.y
+ other_dy = other.p1.y - other.p2.y
+ p1_dx = self.p1.x - other.p1.x
+ p1_dy = self.p1.y - other.p1.y
+ den = self_dx * other_dy - self_dy * other_dx
+ if abs(den) < 1e-10:
+ # Parallel or coincident lines
+ return None
+ t = (p1_dx * other_dy - p1_dy * other_dx) / den
+ u = (p1_dx * self_dy - p1_dy * self_dx) / den
+ if t < 0 or u < 0:
+ return None
+ ip_x = self.p1.x - t * self_dx
+ ip_y = self.p1.y - t * self_dy
+ return Point(ip_x, ip_y)
+
+ def draw(self, ax=None, linestyle='--', color='k', markersize=16, **kwargs):
+ if ax is None:
+ _, ax = plt.subplots(subplot_kw={'aspect': 'equal'})
+ handles = ax.plot(*zip(self.p1, self.p2), linestyle=linestyle, color=color, **kwargs)
+ orient = np.rad2deg(np.arctan2(self.p2.y-self.p1.y, self.p2.x-self.p1.x))
+ handles += ax.plot(*self.p2, marker=(3, 0, orient-90), markersize=markersize, linestyle='None', color=color)
+ return handles, ax
+
+
+# Get intersection of line a and line b
+def get_intersection(a1, a2, b1, b2):
+ if not intersect(a1, a2, b1, b2):
+ if is_between(b1, a1, b2):
+ return a1
+ if is_between(b1, a2, b2):
+ return a2
+ if is_between(a1, b1, a2):
+ return b1
+ if is_between(a1, b2, a2):
+ return b2
+ return None
+ da = a2 - a1
+ db = b2 - b1
+ dp = a1 - b1
+ dap = np.array([-da[1], da[0]])
+ denom = np.dot(dap, db)
+ num = np.dot(dap, dp)
+ return (num / denom.astype(float)) * db + b1
+
+
+def equilateral_triangle(centroid, side_length, rot=0):
+ triangle = np.array(centroid) + np.array([[0, 1 / np.sqrt(3) * side_length],
+ [1 / 2 * side_length, -1 / (2*np.sqrt(3)) * side_length],
+ [-1 / 2 * side_length, -1 / (2*np.sqrt(3)) * side_length]])
+ if not rot:
+ return triangle
+ c,s = np.cos(rot), np.sin(rot)
+ return np.array([[c * triangle[0, 0] - s * triangle[0, 1], s * triangle[0, 0] + c * triangle[0, 1]],
+ [c * triangle[1, 0] - s * triangle[1, 1], s * triangle[1, 0] + c * triangle[1, 1]],
+ [c * triangle[2, 0] - s * triangle[2, 1], s * triangle[2, 0] + c * triangle[2, 1]]])
+
+
+def convex_hull(points):
+ n = len(points)
+
+ # Find the leftmost point
+ l = 0
+ for i in range(1, n):
+ if points[i][0] < points[l][0]:
+ l = i
+ elif points[i][0] == points[l][0]:
+ if points[i][1] > points[l][1]:
+ l = i
+
+ hull = []
+ '''
+ Start from leftmost point, keep moving counterclockwise
+ until reach the start point again. This loop runs O(h)
+ times where h is number of points in result or output.
+ '''
+ p = l
+ while (True):
+ # Add current point to result
+ hull.append([points[p][0], points[p][1]])
+
+ '''
+ Search for a point 'q' such that orientation(p, q,
+ x) is counterclockwise for all points 'x'. The idea
+ is to keep track of last visited most counterclock-
+ wise point in q. If any point 'i' is more counterclock-
+ wise than q, then update q.
+ '''
+ q = (p + 1) % n
+
+ for i in range(n):
+
+ # If i is more counterclockwise
+ # than current q, then update q
+ if is_ccw(points[p], points[i], points[q]):
+ q = i
+ '''
+ Now q is the most counterclockwise with respect to p
+ Set p as q for next iteration, so that q is added to
+ result 'hull'
+ '''
+ p = q
+
+ # While we don't come to first point
+ if (p == l):
+ break
+
+ return hull
+
+
+class Cone:
+ bb_width = 1e6
+ bottom_right = Point(bb_width, -bb_width)
+ top_right = Point(bb_width, bb_width)
+ bottom_left = Point(-bb_width, -bb_width)
+ top_left = Point(-bb_width, bb_width)
+ right_line = Line(bottom_right, top_right)
+ top_line = Line(top_left, top_right)
+ left_line = Line(top_left, bottom_left)
+ bottom_line = Line(bottom_left, bottom_right)
+ bb_edges = [bottom_line, right_line, top_line, left_line]
+ bb_corners = [bottom_right.xy, top_right.xy, top_left.xy, bottom_left.xy]
+ bb_corner_angles = np.pi / 4 * np.array([1, 3, 5, 7])
+
+ def __init__(self, apex, dir1, dir2):
+ self.apex = np.array(apex)
+ self.dir1 = np.array(dir1)
+ self.dir2 = np.array(dir2)
+ self.is_convex = is_ccw([0, 0], self.dir1, self.dir2)
+ self.ray1 = Ray(Point(*self.apex), Point(*(self.apex+self.dir1)))
+ self.ray2 = Ray(Point(*self.apex), Point(*(self.apex+self.dir2)))
+
+ def __str__(self):
+ return "Cone: ({:s}, {:s}, {:s})".format(str(self.apex), str(self.dir1), str(self.dir2))
+
+ def polygon(self) -> shapely.geometry.Polygon:
+
+ angle1 = np.arctan2(self.dir1[1], self.dir1[0]) + np.pi
+ angle2 = np.arctan2(self.dir2[1], self.dir2[0]) + np.pi
+
+ if Cone.bb_corner_angles[0] <= angle1 < Cone.bb_corner_angles[1]:
+ ray1_intersection_idx = 0
+ elif Cone.bb_corner_angles[1] <= angle1 < Cone.bb_corner_angles[2]:
+ ray1_intersection_idx = 1
+ elif Cone.bb_corner_angles[2] <= angle1 < Cone.bb_corner_angles[3]:
+ ray1_intersection_idx = 2
+ else:
+ ray1_intersection_idx = 3
+
+ if Cone.bb_corner_angles[0] <= angle2 < Cone.bb_corner_angles[1]:
+ ray2_intersection_idx = 0
+ elif Cone.bb_corner_angles[1] <= angle2 < Cone.bb_corner_angles[2]:
+ ray2_intersection_idx = 1
+ elif Cone.bb_corner_angles[2] <= angle2 < Cone.bb_corner_angles[3]:
+ ray2_intersection_idx = 2
+ else:
+ ray2_intersection_idx = 3
+
+ r1_border = self.ray1.ray_intersection(Cone.bb_edges[ray1_intersection_idx])
+ r2_border = self.ray2.ray_intersection(Cone.bb_edges[ray2_intersection_idx])
+ if r1_border is None:
+ ray1_intersection_idx = (ray1_intersection_idx + 1) % 4
+ r1_border = self.ray1.ray_intersection(Cone.bb_edges[ray1_intersection_idx])
+ if r1_border is None:
+ ray1_intersection_idx = (ray1_intersection_idx + 2) % 4
+ r1_border = self.ray1.ray_intersection(Cone.bb_edges[ray1_intersection_idx])
+ if r1_border is None:
+ print("SOMETHING WRONG!")
+ if r2_border is None:
+ ray2_intersection_idx = (ray2_intersection_idx + 1) % 4
+ r2_border = self.ray2.ray_intersection(Cone.bb_edges[ray2_intersection_idx])
+ if r2_border is None:
+ ray2_intersection_idx = (ray2_intersection_idx + 2) % 4
+ r2_border = self.ray2.ray_intersection(Cone.bb_edges[ray2_intersection_idx])
+ if r2_border is None:
+ print("SOMETHING WRONG!")
+
+ vertices = [self.apex, r1_border.xy]
+ c_idx = ray1_intersection_idx
+ if not (self.is_convex and (ray1_intersection_idx == ray2_intersection_idx)):
+ while True:
+ vertices += [Cone.bb_corners[c_idx]]
+ c_idx = (c_idx + 1) % 4
+ if c_idx == ray2_intersection_idx:
+ break
+ vertices += [r2_border.xy]
+
+ return shapely.geometry.Polygon(vertices)
+
+ def point_in_cone(self, x):
+ if self.is_convex:
+ return is_ccw(self.apex, self.ray1.p2, x) and is_cw(self.apex, self.ray2.p2, x)
+ else:
+ return not (is_ccw(self.apex, self.ray2.p2, x) and is_cw(self.apex, self.ray1.p2, x))
+
+ def intersection(self, other: 'Cone', same_apex: bool=False) -> shapely.geometry.Polygon:
+ if same_apex:
+ intersection_list = self.intersection_same_apex(other)
+ if len(intersection_list) == 1:
+ return intersection_list[0].polygon()
+ else:
+ return shapely.ops.unary_union([i.polygon() for i in intersection_list])
+ else:
+ return self.polygon().intersection(other.polygon())
+
+ def intersection_same_apex(self, other: 'Cone') -> List['Cone']:
+ other_dir1_in_self = self.point_in_cone(other.apex+other.dir1)
+ other_dir2_in_self = self.point_in_cone(other.apex+other.dir2)
+ self_dir1_in_other = other.point_in_cone(self.apex+self.dir1)
+
+ if other_dir1_in_self and other_dir2_in_self:
+ # Check several cones
+ if self_dir1_in_other:
+ return [Cone(self.apex, self.dir1, other.dir2),
+ Cone(self.apex, other.dir1, self.dir2)]
+ else:
+ return [other]
+ elif other_dir1_in_self:
+ return [Cone(self.apex, other.dir1, self.dir2)]
+ elif other_dir2_in_self:
+ return [Cone(self.apex, self.dir1, other.dir2)]
+ elif self_dir1_in_other:
+ return [self]
+ else:
+ return []
+
+ @staticmethod
+ def list_intersection(cones: List['Cone'], same_apex=False) -> shapely.geometry.Polygon:
+ if not same_apex:
+ intersection = cones[0].polygon()
+ for c in cones[1:]:
+ intersection = intersection.intersection(c.polygon())
+ return intersection
+
+ # List of cones
+ cones_intersect = [cones[0]]
+ for c in cones[1:]:
+ # Find intersection of current cones and next in list
+ cones_intersect_new = []
+ for i, ci in enumerate(cones_intersect):
+ cones_intersect_new += ci.intersection_same_apex(c)
+ cones_intersect = cones_intersect_new
+
+ # If empty intersection
+ if not cones_intersect:
+ return shapely.geometry.Polygon([])
+
+ if len(cones_intersect) == 1:
+ ret = cones_intersect[0].polygon()
+ return ret
+ else:
+ return shapely.ops.unary_union([c.polygon() for c in cones_intersect])
+
+ def draw(self, ax=None, ray_color='k', **kwargs):
+ handles, ax = draw_shapely_polygon(self.polygon(), ax=ax, **kwargs)
+ if ray_color is not None:
+ handles += ax.plot(*zip(self.apex, self.apex + Cone.bb_width * self.dir1), linestyle='--', color=ray_color)
+ handles += ax.plot(*zip(self.apex, self.apex + Cone.bb_width * self.dir2), linestyle='-', color=ray_color)
+ return handles, ax
+
+
+def generate_convex_polygon(n, box=None):
+ if box is None:
+ box = [1, 1]
+
+ # Generate two lists of random X and Y coordinates
+ xPool = [box[0]*np.random.uniform() for i in range(n)]
+ yPool = [box[0]*np.random.uniform() for i in range(n)]
+
+ # Sort them
+ xPool = np.sort(xPool)
+ yPool = np.sort(yPool)
+
+ # Isolate the extreme points
+ minX = xPool[0]
+ maxX = xPool[-1]
+ minY = yPool[0]
+ maxY = yPool[-1]
+
+ # Divide the interior points into two chains & Extract the vector components
+ lastTop = minX
+ lastBot = minX
+
+ xVec, yVec = [], []
+ for i in range(1, n-1):
+ if np.random.randint(2):
+ xVec += [xPool[i] - lastTop]
+ lastTop = xPool[i]
+ else:
+ xVec += [lastBot - xPool[i]]
+ lastBot = xPool[i]
+
+ xVec += [maxX - lastTop]
+ xVec += [lastBot - maxX]
+
+ lastLeft = minY
+ lastRight = minY
+
+ for i in range(1, n - 1):
+ if np.random.randint(2):
+ yVec += [yPool[i] - lastLeft]
+ lastLeft = yPool[i]
+ else:
+ yVec += [lastRight - yPool[i]]
+ lastRight = yPool[i]
+
+ yVec += [maxY - lastLeft]
+ yVec += [lastRight - maxY]
+
+ # Randomly pair up the X- and Y-components
+ np.random.shuffle(yVec)
+
+
+ # Combine the paired up components into vectors
+ # vec = [[xVec[i], yVec[i]] for i in range(n)]
+
+ # Sort the vectors by angle
+ angle = [np.arctan2(yVec[i], xVec[i]) for i in range(n)]
+ xVec = [x for _, x in sorted(zip(angle, xVec))]
+ yVec = [y for _, y in sorted(zip(angle, yVec))]
+
+ # Lay them end-to-end
+ x, y = 0, 0
+ minPolygonX, minPolygonY = 0, 0
+ points = []
+
+ for i in range(n):
+ points += [[x, y]]
+ x += xVec[i]
+ y += yVec[i]
+
+ minPolygonX = min(minPolygonX, x)
+ minPolygonY = min(minPolygonY, y)
+
+ xShift = minX - minPolygonX
+ yShift = minY - minPolygonY
+
+
+ # Move the polygon to have center in origin
+ for i in range(n):
+ points[i][0] += xShift - box[0]/2
+ points[i][1] += yShift - box[1]/2
+
+ xr = np.mean(points, axis=0)
+ for i in range(n):
+ points[i][0] -= xr[0]
+ points[i][1] -= xr[1]
+
+ if not shapely.geometry.box(-box[0],-box[1],box[0],box[1],).contains(shapely.geometry.Polygon(points)):
+ _, ax = plt.subplots()
+ draw_shapely_polygon(shapely.geometry.Polygon(points), ax=ax)
+ ax.set_xlim([-2*box[0], 2*box[0]])
+ ax.set_ylim([-2*box[1], 2*box[1]])
+ plt.show()
+
+ return points
+
+def generate_star_polygon(center: Tuple[float, float], avg_radius: float,
+ irregularity: float, spikiness: float,
+ num_vertices: int) -> List[Tuple[float, float]]:
+ """
+ Start with the center of the polygon at center, then creates the
+ polygon by sampling points on a circle around the center.
+ Random noise is added by varying the angular spacing between
+ sequential points, and by varying the radial distance of each
+ point from the centre.
+
+ Args:
+ center (Tuple[float, float]):
+ a pair representing the center of the circumference used
+ to generate the polygon.
+ avg_radius (float):
+ the average radius (distance of each generated vertex to
+ the center of the circumference) used to generate points
+ with a normal distribution.
+ irregularity (float):
+ variance of the spacing of the angles between consecutive
+ vertices.
+ spikiness (float):
+ variance of the distance of each vertex to the center of
+ the circumference.
+ num_vertices (int):
+ the number of vertices of the polygon.
+ Returns:
+ List[Tuple[float, float]]: list of vertices, in CCW order.
+ """
+ # Parameter check
+ if irregularity < 0 or irregularity > 1:
+ raise ValueError("Irregularity must be between 0 and 1.")
+ if spikiness < 0 or spikiness > 1:
+ raise ValueError("Spikiness must be between 0 and 1.")
+
+ irregularity *= 2 * np.pi / num_vertices
+ spikiness *= avg_radius
+ angle_steps = random_angle_steps(num_vertices, irregularity)
+
+ # now generate the points
+ points = []
+ angle = np.random.uniform(0, 2 * np.pi)
+ for i in range(num_vertices):
+ radius = np.clip(np.random.normal(avg_radius, spikiness), 0, 2 * avg_radius)
+ point = (center[0] + radius * np.cos(angle),
+ center[1] + radius * np.sin(angle))
+ points.append(point)
+ angle += angle_steps[i]
+
+ return points
+
+def random_angle_steps(steps: int, irregularity: float) -> List[float]:
+ """Generates the division of a circumference in random angles.
+
+ Args:
+ steps (int):
+ the number of angles to generate.
+ irregularity (float):
+ variance of the spacing of the angles between consecutive vertices.
+ Returns:
+ List[float]: the list of the random angles.
+ """
+ # generate n angle steps
+ angles = []
+ lower = (2 * np.pi / steps) - irregularity
+ upper = (2 * np.pi / steps) + irregularity
+ cumsum = 0
+ for i in range(steps):
+ angle = np.random.uniform(lower, upper)
+ angles.append(angle)
+ cumsum += angle
+
+ # normalize the steps so that point 0 and point n+1 are the same
+ cumsum /= (2 * np.pi)
+ for i in range(steps):
+ angles[i] /= cumsum
+ return angles
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/utils/misc.py b/python/ur_simple_control/path_generation/star_navigation/starworlds/utils/misc.py
new file mode 100644
index 0000000000000000000000000000000000000000..05e09b0376a01818bd92cee24da50dcb64d46f8c
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/utils/misc.py
@@ -0,0 +1,51 @@
+import matplotlib.pyplot as plt
+import matplotlib.patches as patches
+import time
+import inspect
+import traceback
+import shapely
+import numpy as np
+
+def tic():
+ return time.time()
+
+
+def toc(t0):
+ return (time.time()-t0) * 1000
+
+
+def draw_shapely_polygon(pol, ax=None, xlim=None, ylim=None, **kwargs):
+ if ax is None:
+ _, ax = plt.subplots(subplot_kw={'aspect': 'equal'})
+ pol_list = []
+ handles = []
+ if pol.geom_type == 'Polygon':
+ pol_list += [pol]
+ else:
+ for p in pol.geoms:
+ if p.geom_type == 'Polygon':
+ pol_list += [p]
+ for p in pol_list:
+ if xlim is not None and ylim is not None:
+ pol_plot = p.intersection(shapely.geometry.box(xlim[0] - 1, ylim[0] - 1, xlim[1] + 1, ylim[1] + 1))
+ else:
+ pol_plot = p
+ handles += [patches.Polygon(xy=np.vstack((pol_plot.exterior.xy[0], pol_plot.exterior.xy[1])).T, **kwargs)]
+ ax.add_patch(handles[-1])
+ if xlim is not None:
+ ax.set_xlim(xlim)
+ if ylim is not None:
+ ax.set_ylim(ylim)
+ return handles, ax
+
+
+def logprint(message=None, print_stack=0):
+ callerframerecord = inspect.stack()[1] # 0 represents this line
+ # 1 represents line at caller
+ frame = callerframerecord[0]
+ info = inspect.getframeinfo(frame)
+ if print_stack:
+ traceback.print_stack()
+ print(info.function + ", line: " + str(info.lineno))
+ if message:
+ print(message)
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/PKG-INFO b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/PKG-INFO
index c335b9889a6cd24920756fa4ba62ccd215516f12..07f336d2c0b679dcde8ec721f9f86020d2445382 100644
--- a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/PKG-INFO
+++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/PKG-INFO
@@ -1,10 +1,10 @@
Metadata-Version: 2.1
-Name: starworld-tunnel-mpc
+Name: starworld_tunnel_mpc
Version: 1.0
-Summary: UNKNOWN
-Home-page: UNKNOWN
-License: UNKNOWN
-Platform: UNKNOWN
-
-UNKNOWN
-
+Requires-Dist: pyyaml
+Requires-Dist: numpy
+Requires-Dist: scipy
+Requires-Dist: matplotlib
+Requires-Dist: shapely
+Requires-Dist: casadi
+Requires-Dist: opengen
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/SOURCES.txt b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/SOURCES.txt
index 0be315e9b16ce2ac41854e6f504645a804a3ed61..b0c2aa0fddd985d62b3cf85d9950633b36b964f1 100644
--- a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/SOURCES.txt
+++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/SOURCES.txt
@@ -26,5 +26,7 @@ starworld_tunnel_mpc.egg-info/SOURCES.txt
starworld_tunnel_mpc.egg-info/dependency_links.txt
starworld_tunnel_mpc.egg-info/requires.txt
starworld_tunnel_mpc.egg-info/top_level.txt
+tests/test_motion_control.py
+tests/test_soads.py
visualization/__init__.py
visualization/scene_gui.py
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/requires.txt b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/requires.txt
index c75a0c3a268cd969c655a6052e60cad8589bab36..057d7bc86ef9612bd1d1b733c611737b82c5d542 100644
--- a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/requires.txt
+++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/requires.txt
@@ -1,7 +1,7 @@
-casadi
-matplotlib
-numpy
-opengen
pyyaml
+numpy
scipy
+matplotlib
shapely
+casadi
+opengen
diff --git a/python/ur_simple_control/path_generation/starworlds/starworlds.egg-info/PKG-INFO b/python/ur_simple_control/path_generation/starworlds/starworlds.egg-info/PKG-INFO
index 70433406afc348c8d243871e3bac6a7fc3c41903..4dee13fc6c070e01a3bca7b804984f6d9645b9f5 100644
--- a/python/ur_simple_control/path_generation/starworlds/starworlds.egg-info/PKG-INFO
+++ b/python/ur_simple_control/path_generation/starworlds/starworlds.egg-info/PKG-INFO
@@ -1,11 +1,9 @@
Metadata-Version: 2.1
Name: starworlds
Version: 1.0
-Summary: UNKNOWN
-Home-page: UNKNOWN
-License: UNKNOWN
-Platform: UNKNOWN
License-File: LICENSE
-
-UNKNOWN
-
+Requires-Dist: pyyaml
+Requires-Dist: numpy
+Requires-Dist: scipy
+Requires-Dist: matplotlib
+Requires-Dist: shapely
diff --git a/python/ur_simple_control/path_generation/starworlds/starworlds.egg-info/requires.txt b/python/ur_simple_control/path_generation/starworlds/starworlds.egg-info/requires.txt
index 16fb7174f640ff4a00107b606bbf4abef70ae6e2..f849890421afe2baa719e8822ed041b4c0ea68da 100644
--- a/python/ur_simple_control/path_generation/starworlds/starworlds.egg-info/requires.txt
+++ b/python/ur_simple_control/path_generation/starworlds/starworlds.egg-info/requires.txt
@@ -1,5 +1,5 @@
-matplotlib
-numpy
pyyaml
+numpy
scipy
+matplotlib
shapely
diff --git a/python/ur_simple_control/robot_descriptions/TODO_PUT_IN_FILES_FROM_HERE_INTO_YUMI_LOCAL_URDF b/python/ur_simple_control/robot_descriptions/TODO_PUT_IN_FILES_FROM_HERE_INTO_YUMI_LOCAL_URDF
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/python/ur_simple_control/robot_descriptions/__pycache__/__init__.cpython-312.pyc b/python/ur_simple_control/robot_descriptions/__pycache__/__init__.cpython-312.pyc
index 723cdbade5d2d11d0a64fb97184f739e1c29fade..115adf251a4a1d068f6e90128ff1d264ae83feaa 100644
Binary files a/python/ur_simple_control/robot_descriptions/__pycache__/__init__.cpython-312.pyc and b/python/ur_simple_control/robot_descriptions/__pycache__/__init__.cpython-312.pyc differ
diff --git a/python/ur_simple_control/robot_descriptions/yumi_local.urdf b/python/ur_simple_control/robot_descriptions/yumi_local.urdf
new file mode 100644
index 0000000000000000000000000000000000000000..47f8d9f0da2f7ba7f20723b9d841ff8064c27dca
--- /dev/null
+++ b/python/ur_simple_control/robot_descriptions/yumi_local.urdf
@@ -0,0 +1,1160 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- | This document was autogenerated by xacro from yumi.xacro | -->
+<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
+<!-- =================================================================================== -->
+<robot name="abb_yumi">
+ <!-- - - - YuMi - - - -->
+ <link name="base_link">
+ <visual>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/visual/base_link.stl"/>
+ </geometry>
+ <material name="light_grey">
+ <color rgba="0.6 0.6 0.6 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/collision/base_link.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="-0.122236 0.028996 0.017672"/>
+ <mass value="30.0"/>
+ <inertia ixx="1.61664025" ixy="0.0" ixz="0.0" iyy="1.90523836" iyz="0.0" izz="1.34940618"/>
+ </inertial>
+ </link>
+ <link name="base">
+ </link>
+ <joint name="base_joint" type="fixed">
+ <parent link="base_link"/>
+ <child link="base"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ </joint>
+ <gazebo reference="base_link">
+ <material>Gazebo/White</material>
+ <turnGravityOff>False</turnGravityOff>
+ </gazebo>
+ <link name="robl_base_link">
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <mass value="0.5"/>
+ <inertia ixx="0.00083333" ixy="0.0" ixz="0.0" iyy="0.00083333" iyz="0.0" izz="0.00083333"/>
+ </inertial>
+ </link>
+ <link name="robl_link_1">
+ <visual>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/visual/rob_1_link.stl"/>
+ </geometry>
+ <material name="light_grey">
+ <color rgba="0.6 0.6 0.6 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/collision/rob_1_link.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="-0.0102903 0.0225876 0.0585935"/>
+ <mass value="0.9629233"/>
+ <inertia ixx="0.0030634" ixy="0.00025149" ixz="0.00052912" iyy="0.00302651" iyz="-0.00085524" izz="0.00165401"/>
+ </inertial>
+ </link>
+ <link name="robl_link_2">
+ <visual>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/visual/rob_2_link.stl"/>
+ </geometry>
+ <material name="light_grey">
+ <color rgba="0.6 0.6 0.6 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/collision/rob_2_link.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0178625 -0.0146039 0.0642901"/>
+ <mass value="1.4105"/>
+ <inertia ixx="0.00705656" ixy="-0.00022304" ixz="-0.00110629" iyy="0.00698719" iyz="-0.00115557" izz="0.00227954"/>
+ </inertial>
+ </link>
+ <link name="robl_link_3">
+ <visual>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/visual/rob_3_link.stl"/>
+ </geometry>
+ <material name="light_grey">
+ <color rgba="0.6 0.6 0.6 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/collision/rob_3_link.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0201733 0.0266755 0.0620484"/>
+ <mass value="0.7187233"/>
+ <inertia ixx="0.00156497" ixy="-0.00022216" ixz="-0.00042641" iyy="0.00177346" iyz="-0.00041747" izz="0.00113318"/>
+ </inertial>
+ </link>
+ <link name="robl_link_4">
+ <visual>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/visual/rob_4_link.stl"/>
+ </geometry>
+ <material name="light_grey">
+ <color rgba="0.6 0.6 0.6 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/collision/rob_4_link.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0559113 -0.015687 0.0226745"/>
+ <mass value="1.1807964"/>
+ <inertia ixx="0.00187228" ixy="-0.00093911" ixz="-0.00117309" iyy="0.00529598" iyz="-0.00029449" izz="0.00518088"/>
+ </inertial>
+ </link>
+ <link name="robl_link_5">
+ <visual>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/visual/rob_5_link.stl"/>
+ </geometry>
+ <material name="light_grey">
+ <color rgba="0.6 0.6 0.6 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/collision/rob_5_link.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0708236 0.0278214 -0.0074085"/>
+ <mass value="0.3434158"/>
+ <inertia ixx="0.000364277603" ixy="-0.000253381412" ixz="0.000131795094" iyy="0.000720976878" iyz="5.00695671e-05" izz="0.000787541057"/>
+ </inertial>
+ </link>
+ <link name="robl_link_6">
+ <visual>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/visual/rob_6_link.stl"/>
+ </geometry>
+ <material name="light_grey">
+ <color rgba="0.6 0.6 0.6 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/collision/rob_6_link.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="-0.0123616 -0.009411 0.0113342"/>
+ <mass value="0.4703021"/>
+ <inertia ixx="0.0005976" ixy="3.69e-05" ixz="-7.41e-05" iyy="0.0006515" iyz="-2.32e-05" izz="0.0005894"/>
+ </inertial>
+ </link>
+ <link name="robl_link_7">
+ <visual>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/visual/rob_7_link.stl"/>
+ </geometry>
+ <material name="light_grey">
+ <color rgba="0.6 0.6 0.6 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/collision/rob_7_link.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="-0.01155172 0.00013113 7.43e-06"/>
+ <mass value="0.04167441"/>
+ <inertia ixx="2.75680598e-05" ixy="0.0" ixz="0.0" iyy="2.03557822e-05" iyz="0.0" izz="2.03553446e-05"/>
+ </inertial>
+ </link>
+ <link name="robl_flange">
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <mass value="0.001"/>
+ <inertia ixx="1.66666667e-08" ixy="0.0" ixz="0.0" iyy="1.66666667e-08" iyz="0.0" izz="1.66666667e-08"/>
+ </inertial>
+ </link>
+ <link name="robl_tool0">
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <mass value="0.001"/>
+ <inertia ixx="1.66666667e-08" ixy="0.0" ixz="0.0" iyy="1.66666667e-08" iyz="0.0" izz="1.66666667e-08"/>
+ </inertial>
+ </link>
+ <joint name="robl_base_joint" type="fixed">
+ <parent link="base_link"/>
+ <child link="robl_base_link"/>
+ <origin rpy="-0.9790773504912591 0.5692042289529107 -0.8254185631456783" xyz="0.047607 0.070008 0.411486"/>
+ </joint>
+ <joint name="robl_joint_1" type="revolute">
+ <parent link="robl_base_link"/>
+ <child link="robl_link_1"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="10.0" lower="-2.9408797896104453" upper="2.9408797896104453" velocity="3.141592653589793"/>
+ <dynamics damping="1.06" friction="0.0"/>
+ </joint>
+ <joint name="robl_joint_2" type="revolute">
+ <parent link="robl_link_1"/>
+ <child link="robl_link_2"/>
+ <origin rpy="0.0 0.0 0.0" xyz="-0.03 0.0 0.1075"/>
+ <axis xyz="0 1 0"/>
+ <limit effort="10.0" lower="-2.504547476611863" upper="0.7592182246175333" velocity="3.141592653589793"/>
+ <dynamics damping="1.09" friction="0.0"/>
+ </joint>
+ <joint name="robl_joint_3" type="revolute">
+ <parent link="robl_link_2"/>
+ <child link="robl_link_3"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.03 0.0 0.1603"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="10.0" lower="-2.9408797896104453" upper="2.9408797896104453" velocity="3.141592653589793"/>
+ <dynamics damping="0.61" friction="0.0"/>
+ </joint>
+ <joint name="robl_joint_4" type="revolute">
+ <parent link="robl_link_3"/>
+ <child link="robl_link_4"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0405 0.0 0.0912"/>
+ <axis xyz="0 1 0"/>
+ <limit effort="10.0" lower="-2.155481626212997" upper="1.3962634015954636" velocity="3.141592653589793"/>
+ <dynamics damping="0.08" friction="0.0"/>
+ </joint>
+ <joint name="robl_joint_5" type="revolute">
+ <parent link="robl_link_4"/>
+ <child link="robl_link_5"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.1396 0.0 0.0405"/>
+ <axis xyz="1 0 0"/>
+ <limit effort="1.0" lower="-5.061454830783556" upper="5.061454830783556" velocity="6.981317007977318"/>
+ <dynamics damping="0.08" friction="0.0"/>
+ </joint>
+ <joint name="robl_joint_6" type="revolute">
+ <parent link="robl_link_5"/>
+ <child link="robl_link_6"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.1254 0.0 -0.027"/>
+ <axis xyz="0 1 0"/>
+ <limit effort="1.0" lower="-1.53588974175501" upper="2.356194490192345" velocity="6.981317007977318"/>
+ <dynamics damping="0.08" friction="0.0"/>
+ </joint>
+ <joint name="robl_joint_7" type="revolute">
+ <parent link="robl_link_6"/>
+ <child link="robl_link_7"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.036 0.0 0.027"/>
+ <axis xyz="1 0 0"/>
+ <limit effort="1.0" lower="-3.9968039870670147" upper="3.9968039870670147" velocity="6.981317007977318"/>
+ <dynamics damping="0.039" friction="0.0"/>
+ </joint>
+ <joint name="robl_joint_7_flange" type="fixed">
+ <parent link="robl_link_7"/>
+ <child link="robl_flange"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ </joint>
+ <joint name="robl_flange_tool0" type="fixed">
+ <parent link="robl_flange"/>
+ <child link="robl_tool0"/>
+ <origin rpy="0.0 1.5707963267948966 0.0" xyz="0.0 0.0 0.0"/>
+ </joint>
+ <transmission name="robl_transmission_1">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="robl_joint_1">
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="robl_joint_1_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="robl_transmission_2">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="robl_joint_2">
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="robl_joint_2_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="robl_transmission_3">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="robl_joint_3">
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="robl_joint_3_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="robl_transmission_4">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="robl_joint_4">
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="robl_joint_4_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="robl_transmission_5">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="robl_joint_5">
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="robl_joint_5_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="robl_transmission_6">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="robl_joint_6">
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="robl_joint_6_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="robl_transmission_7">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="robl_joint_7">
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="robl_joint_7_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <gazebo reference="robl_base_joint">
+ <preserveFixedJoint>True</preserveFixedJoint>
+ </gazebo>
+ <gazebo reference="robl_link_1">
+ <material>Gazebo/Grey</material>
+ <mu1 value="1.0"/>
+ <mu2 value="1.0"/>
+ <kp value="10000000.0"/>
+ <kd value="1000.0"/>
+ <fdir1 value="1 0 0"/>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robl_link_2">
+ <material>Gazebo/Grey</material>
+ <mu1 value="1.0"/>
+ <mu2 value="1.0"/>
+ <kp value="10000000.0"/>
+ <kd value="1000.0"/>
+ <fdir1 value="1 0 0"/>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robl_link_3">
+ <material>Gazebo/Grey</material>
+ <mu1 value="1.0"/>
+ <mu2 value="1.0"/>
+ <kp value="10000000.0"/>
+ <kd value="1000.0"/>
+ <fdir1 value="1 0 0"/>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robl_link_4">
+ <material>Gazebo/Grey</material>
+ <mu1 value="1.0"/>
+ <mu2 value="1.0"/>
+ <kp value="10000000.0"/>
+ <kd value="1000.0"/>
+ <fdir1 value="1 0 0"/>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robl_link_5">
+ <material>Gazebo/Grey</material>
+ <mu1 value="1.0"/>
+ <mu2 value="1.0"/>
+ <kp value="10000000.0"/>
+ <kd value="1000.0"/>
+ <fdir1 value="1 0 0"/>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robl_link_6">
+ <material>Gazebo/Grey</material>
+ <mu1 value="1.0"/>
+ <mu2 value="1.0"/>
+ <kp value="10000000.0"/>
+ <kd value="1000.0"/>
+ <fdir1 value="1 0 0"/>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robl_link_7">
+ <mu1 value="1.0"/>
+ <mu2 value="1.0"/>
+ <kp value="10000000.0"/>
+ <kd value="1000.0"/>
+ <fdir1 value="1 0 0"/>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robl_flange">
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robl_tool0">
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robl_flange_tool0">
+ <preserveFixedJoint>True</preserveFixedJoint>
+ </gazebo>
+ <gazebo>
+ <plugin filename="libabb_gazebo_libegm.so" name="gazebo_robl">
+ <controller_rate>250.0</controller_rate>
+ <base_link>robl_base_link</base_link>
+ <end_effector_link>robl_tool0</end_effector_link>
+ <egm_server_ip>127.0.0.1</egm_server_ip>
+ <egm_server_port>6511</egm_server_port>
+ <joint name="robl_joint_1" position="0.0"/>
+ <joint name="robl_joint_2" position="-2.2689280275926285"/>
+ <external_joint name="robl_joint_3" position="2.356194490192345"/>
+ <joint name="robl_joint_4" position="0.5235987755982988"/>
+ <joint name="robl_joint_5" position="0.0"/>
+ <joint name="robl_joint_6" position="0.6981317007977318"/>
+ <joint name="robl_joint_7" position="0.0"/>
+ </plugin>
+ </gazebo>
+ <link name="robr_base_link">
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <mass value="0.5"/>
+ <inertia ixx="0.00083333" ixy="0.0" ixz="0.0" iyy="0.00083333" iyz="0.0" izz="0.00083333"/>
+ </inertial>
+ </link>
+ <link name="robr_link_1">
+ <visual>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/visual/rob_1_link.stl"/>
+ </geometry>
+ <material name="light_grey">
+ <color rgba="0.6 0.6 0.6 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/collision/rob_1_link.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="-0.0102903 0.0225876 0.0585935"/>
+ <mass value="0.9629233"/>
+ <inertia ixx="0.0030634" ixy="0.00025149" ixz="0.00052912" iyy="0.00302651" iyz="-0.00085524" izz="0.00165401"/>
+ </inertial>
+ </link>
+ <link name="robr_link_2">
+ <visual>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/visual/rob_2_link.stl"/>
+ </geometry>
+ <material name="light_grey">
+ <color rgba="0.6 0.6 0.6 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/collision/rob_2_link.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0178625 -0.0146039 0.0642901"/>
+ <mass value="1.4105"/>
+ <inertia ixx="0.00705656" ixy="-0.00022304" ixz="-0.00110629" iyy="0.00698719" iyz="-0.00115557" izz="0.00227954"/>
+ </inertial>
+ </link>
+ <link name="robr_link_3">
+ <visual>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/visual/rob_3_link.stl"/>
+ </geometry>
+ <material name="light_grey">
+ <color rgba="0.6 0.6 0.6 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/collision/rob_3_link.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0201733 0.0266755 0.0620484"/>
+ <mass value="0.7187233"/>
+ <inertia ixx="0.00156497" ixy="-0.00022216" ixz="-0.00042641" iyy="0.00177346" iyz="-0.00041747" izz="0.00113318"/>
+ </inertial>
+ </link>
+ <link name="robr_link_4">
+ <visual>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/visual/rob_4_link.stl"/>
+ </geometry>
+ <material name="light_grey">
+ <color rgba="0.6 0.6 0.6 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/collision/rob_4_link.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0559113 -0.015687 0.0226745"/>
+ <mass value="1.1807964"/>
+ <inertia ixx="0.00187228" ixy="-0.00093911" ixz="-0.00117309" iyy="0.00529598" iyz="-0.00029449" izz="0.00518088"/>
+ </inertial>
+ </link>
+ <link name="robr_link_5">
+ <visual>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/visual/rob_5_link.stl"/>
+ </geometry>
+ <material name="light_grey">
+ <color rgba="0.6 0.6 0.6 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/collision/rob_5_link.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0708236 0.0278214 -0.0074085"/>
+ <mass value="0.3434158"/>
+ <inertia ixx="0.000364277603" ixy="-0.000253381412" ixz="0.000131795094" iyy="0.000720976878" iyz="5.00695671e-05" izz="0.000787541057"/>
+ </inertial>
+ </link>
+ <link name="robr_link_6">
+ <visual>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/visual/rob_6_link.stl"/>
+ </geometry>
+ <material name="light_grey">
+ <color rgba="0.6 0.6 0.6 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/collision/rob_6_link.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="-0.0123616 -0.009411 0.0113342"/>
+ <mass value="0.4703021"/>
+ <inertia ixx="0.0005976" ixy="3.69e-05" ixz="-7.41e-05" iyy="0.0006515" iyz="-2.32e-05" izz="0.0005894"/>
+ </inertial>
+ </link>
+ <link name="robr_link_7">
+ <visual>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/visual/rob_7_link.stl"/>
+ </geometry>
+ <material name="light_grey">
+ <color rgba="0.6 0.6 0.6 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/collision/rob_7_link.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="-0.01155172 0.00013113 7.43e-06"/>
+ <mass value="0.04167441"/>
+ <inertia ixx="2.75680598e-05" ixy="0.0" ixz="0.0" iyy="2.03557822e-05" iyz="0.0" izz="2.03553446e-05"/>
+ </inertial>
+ </link>
+ <link name="robr_flange">
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <mass value="0.001"/>
+ <inertia ixx="1.66666667e-08" ixy="0.0" ixz="0.0" iyy="1.66666667e-08" iyz="0.0" izz="1.66666667e-08"/>
+ </inertial>
+ </link>
+ <link name="robr_tool0">
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <mass value="0.001"/>
+ <inertia ixx="1.66666667e-08" ixy="0.0" ixz="0.0" iyy="1.66666667e-08" iyz="0.0" izz="1.66666667e-08"/>
+ </inertial>
+ </link>
+ <joint name="robr_base_joint" type="fixed">
+ <parent link="base_link"/>
+ <child link="robr_base_link"/>
+ <origin rpy="0.9790773504912591 0.5692042289529107 0.8254185631456783" xyz="0.047607 -0.070008 0.411486"/>
+ </joint>
+ <joint name="robr_joint_1" type="revolute">
+ <parent link="robr_base_link"/>
+ <child link="robr_link_1"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="10.0" lower="-2.9408797896104453" upper="2.9408797896104453" velocity="3.141592653589793"/>
+ <dynamics damping="1.06" friction="0.0"/>
+ </joint>
+ <joint name="robr_joint_2" type="revolute">
+ <parent link="robr_link_1"/>
+ <child link="robr_link_2"/>
+ <origin rpy="0.0 0.0 0.0" xyz="-0.03 0.0 0.1075"/>
+ <axis xyz="0 1 0"/>
+ <limit effort="10.0" lower="-2.504547476611863" upper="0.7592182246175333" velocity="3.141592653589793"/>
+ <dynamics damping="1.09" friction="0.0"/>
+ </joint>
+ <joint name="robr_joint_3" type="revolute">
+ <parent link="robr_link_2"/>
+ <child link="robr_link_3"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.03 0.0 0.1603"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="10.0" lower="-2.9408797896104453" upper="2.9408797896104453" velocity="3.141592653589793"/>
+ <dynamics damping="0.61" friction="0.0"/>
+ </joint>
+ <joint name="robr_joint_4" type="revolute">
+ <parent link="robr_link_3"/>
+ <child link="robr_link_4"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0405 0.0 0.0912"/>
+ <axis xyz="0 1 0"/>
+ <limit effort="10.0" lower="-2.155481626212997" upper="1.3962634015954636" velocity="3.141592653589793"/>
+ <dynamics damping="0.08" friction="0.0"/>
+ </joint>
+ <joint name="robr_joint_5" type="revolute">
+ <parent link="robr_link_4"/>
+ <child link="robr_link_5"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.1396 0.0 0.0405"/>
+ <axis xyz="1 0 0"/>
+ <limit effort="1.0" lower="-5.061454830783556" upper="5.061454830783556" velocity="6.981317007977318"/>
+ <dynamics damping="0.08" friction="0.0"/>
+ </joint>
+ <joint name="robr_joint_6" type="revolute">
+ <parent link="robr_link_5"/>
+ <child link="robr_link_6"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.1254 0.0 -0.027"/>
+ <axis xyz="0 1 0"/>
+ <limit effort="1.0" lower="-1.53588974175501" upper="2.356194490192345" velocity="6.981317007977318"/>
+ <dynamics damping="0.08" friction="0.0"/>
+ </joint>
+ <joint name="robr_joint_7" type="revolute">
+ <parent link="robr_link_6"/>
+ <child link="robr_link_7"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.036 0.0 0.027"/>
+ <axis xyz="1 0 0"/>
+ <limit effort="1.0" lower="-3.9968039870670147" upper="3.9968039870670147" velocity="6.981317007977318"/>
+ <dynamics damping="0.039" friction="0.0"/>
+ </joint>
+ <joint name="robr_joint_7_flange" type="fixed">
+ <parent link="robr_link_7"/>
+ <child link="robr_flange"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ </joint>
+ <joint name="robr_flange_tool0" type="fixed">
+ <parent link="robr_flange"/>
+ <child link="robr_tool0"/>
+ <origin rpy="0.0 1.5707963267948966 0.0" xyz="0.0 0.0 0.0"/>
+ </joint>
+ <transmission name="robr_transmission_1">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="robr_joint_1">
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="robr_joint_1_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="robr_transmission_2">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="robr_joint_2">
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="robr_joint_2_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="robr_transmission_3">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="robr_joint_3">
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="robr_joint_3_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="robr_transmission_4">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="robr_joint_4">
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="robr_joint_4_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="robr_transmission_5">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="robr_joint_5">
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="robr_joint_5_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="robr_transmission_6">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="robr_joint_6">
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="robr_joint_6_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="robr_transmission_7">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="robr_joint_7">
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="robr_joint_7_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <gazebo reference="robr_base_joint">
+ <preserveFixedJoint>True</preserveFixedJoint>
+ </gazebo>
+ <gazebo reference="robr_link_1">
+ <material>Gazebo/Grey</material>
+ <mu1 value="1.0"/>
+ <mu2 value="1.0"/>
+ <kp value="10000000.0"/>
+ <kd value="1000.0"/>
+ <fdir1 value="1 0 0"/>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robr_link_2">
+ <material>Gazebo/Grey</material>
+ <mu1 value="1.0"/>
+ <mu2 value="1.0"/>
+ <kp value="10000000.0"/>
+ <kd value="1000.0"/>
+ <fdir1 value="1 0 0"/>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robr_link_3">
+ <material>Gazebo/Grey</material>
+ <mu1 value="1.0"/>
+ <mu2 value="1.0"/>
+ <kp value="10000000.0"/>
+ <kd value="1000.0"/>
+ <fdir1 value="1 0 0"/>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robr_link_4">
+ <material>Gazebo/Grey</material>
+ <mu1 value="1.0"/>
+ <mu2 value="1.0"/>
+ <kp value="10000000.0"/>
+ <kd value="1000.0"/>
+ <fdir1 value="1 0 0"/>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robr_link_5">
+ <material>Gazebo/Grey</material>
+ <mu1 value="1.0"/>
+ <mu2 value="1.0"/>
+ <kp value="10000000.0"/>
+ <kd value="1000.0"/>
+ <fdir1 value="1 0 0"/>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robr_link_6">
+ <material>Gazebo/Grey</material>
+ <mu1 value="1.0"/>
+ <mu2 value="1.0"/>
+ <kp value="10000000.0"/>
+ <kd value="1000.0"/>
+ <fdir1 value="1 0 0"/>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robr_link_7">
+ <mu1 value="1.0"/>
+ <mu2 value="1.0"/>
+ <kp value="10000000.0"/>
+ <kd value="1000.0"/>
+ <fdir1 value="1 0 0"/>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robr_flange">
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robr_tool0">
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robr_flange_tool0">
+ <preserveFixedJoint>True</preserveFixedJoint>
+ </gazebo>
+ <gazebo>
+ <plugin filename="libabb_gazebo_libegm.so" name="gazebo_robr">
+ <controller_rate>250.0</controller_rate>
+ <base_link>robr_base_link</base_link>
+ <end_effector_link>robr_tool0</end_effector_link>
+ <egm_server_ip>127.0.0.1</egm_server_ip>
+ <egm_server_port>6512</egm_server_port>
+ <joint name="robr_joint_1" position="0.0"/>
+ <joint name="robr_joint_2" position="-2.2689280275926285"/>
+ <external_joint name="robr_joint_3" position="-2.356194490192345"/>
+ <joint name="robr_joint_4" position="0.5235987755982988"/>
+ <joint name="robr_joint_5" position="0.0"/>
+ <joint name="robr_joint_6" position="0.6981317007977318"/>
+ <joint name="robr_joint_7" position="0.0"/>
+ </plugin>
+ </gazebo>
+ <!-- - - - Grippers - - - -->
+ <link name="robl_sg_base_link">
+ <visual>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_myumi_smart_gripper_description/meshes/sg/visual/base_link.dae"/>
+ </geometry>
+ <material name="light_grey">
+ <color rgba="0.6 0.6 0.6 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_myumi_smart_gripper_description/meshes/sg/collision/base_link.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="-0.0002 -0.007 0.0426"/>
+ <mass value="0.35"/>
+ <inertia ixx="8.73e-05" ixy="-2e-07" ixz="-4e-07" iyy="9.11e-05" iyz="-8.7e-06" izz="2.33e-05"/>
+ </inertial>
+ </link>
+ <link name="robl_sg_vacuum_1_frame_link">
+ </link>
+ <link name="robl_sg_vacuum_2_frame_link">
+ </link>
+ <link name="robl_sg_camera_frame_link">
+ </link>
+ <joint name="robl_sg_vacuum_1_frame_joint" type="fixed">
+ <parent link="robl_sg_base_link"/>
+ <child link="robl_sg_vacuum_1_frame_link"/>
+ <origin rpy="0.0 1.5707963267948966 0.0" xyz="0.0274 0.0185 0.0375"/>
+ </joint>
+ <joint name="robl_sg_vacuum_2_frame_joint" type="fixed">
+ <parent link="robl_sg_base_link"/>
+ <child link="robl_sg_vacuum_2_frame_link"/>
+ <origin rpy="-3.141592653589793 1.5707963267948966 0.0" xyz="-0.0274 0.0185 0.0375"/>
+ </joint>
+ <joint name="robl_sg_camera_frame_joint" type="fixed">
+ <parent link="robl_sg_base_link"/>
+ <child link="robl_sg_camera_frame_link"/>
+ <origin rpy="-1.5707963267948966 1.5707963267948966 0.0" xyz="-0.0073 0.0283 0.0351"/>
+ </joint>
+ <gazebo reference="robl_sg_base_link">
+ <material>Gazebo/White</material>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <link name="robl_sg_servo_frame_link">
+ </link>
+ <link name="robl_sg_fingers_frame_link">
+ </link>
+ <link name="robl_sg_finger_1_base_link">
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.00139 -0.014982"/>
+ <mass value="0.05"/>
+ <inertia ixx="3.08333333e-06" ixy="0.0" ixz="0.0" iyy="6.56666667e-06" iyz="0.0" izz="4.01666667e-06"/>
+ </inertial>
+ </link>
+ <link name="robl_sg_finger_2_base_link">
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.00139 -0.014982"/>
+ <mass value="0.05"/>
+ <inertia ixx="3.08333333e-06" ixy="0.0" ixz="0.0" iyy="6.56666667e-06" iyz="0.0" izz="4.01666667e-06"/>
+ </inertial>
+ </link>
+ <link name="robl_sg_finger_1_link">
+ <visual>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_myumi_smart_gripper_description/meshes/sg/visual/finger_link.stl"/>
+ </geometry>
+ <material name="light_grey">
+ <color rgba="0.6 0.6 0.6 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_myumi_smart_gripper_description/meshes/sg/collision/finger_link.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.011259 -0.004784 0.017852"/>
+ <mass value="0.01"/>
+ <inertia ixx="3.55e-06" ixy="-4.5e-07" ixz="1.68e-06" iyy="4.53e-06" iyz="6.8e-07" izz="1.54e-06"/>
+ </inertial>
+ </link>
+ <link name="robl_sg_finger_2_link">
+ <visual>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_myumi_smart_gripper_description/meshes/sg/visual/finger_link.stl"/>
+ </geometry>
+ <material name="light_grey">
+ <color rgba="0.6 0.6 0.6 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_myumi_smart_gripper_description/meshes/sg/collision/finger_link.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.011259 -0.004784 0.017852"/>
+ <mass value="0.01"/>
+ <inertia ixx="3.55e-06" ixy="-4.5e-07" ixz="1.68e-06" iyy="4.53e-06" iyz="6.8e-07" izz="1.54e-06"/>
+ </inertial>
+ </link>
+ <joint name="robl_sg_servo_frame_joint" type="fixed">
+ <parent link="robl_sg_base_link"/>
+ <child link="robl_sg_servo_frame_link"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.084"/>
+ </joint>
+ <joint name="robl_sg_fingers_frame_joint" type="fixed">
+ <parent link="robl_sg_base_link"/>
+ <child link="robl_sg_fingers_frame_link"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.1142"/>
+ </joint>
+ <joint name="robl_sg_finger_1_base_joint" type="fixed">
+ <parent link="robl_sg_base_link"/>
+ <child link="robl_sg_finger_1_base_link"/>
+ <origin rpy="0.0 0.0 0.0" xyz="-0.0125 0.0065 0.084"/>
+ </joint>
+ <joint name="robl_sg_finger_2_base_joint" type="fixed">
+ <parent link="robl_sg_base_link"/>
+ <child link="robl_sg_finger_2_base_link"/>
+ <origin rpy="0.0 0.0 3.141592653589793" xyz="0.0125 -0.0065 0.084"/>
+ </joint>
+ <joint name="robl_sg_finger_1_joint" type="fixed">
+ <parent link="robl_sg_finger_1_base_link"/>
+ <child link="robl_sg_finger_1_link"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ </joint>
+ <joint name="robl_sg_finger_2_joint" type="fixed">
+ <parent link="robl_sg_finger_2_base_link"/>
+ <child link="robl_sg_finger_2_link"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ </joint>
+ <gazebo reference="robl_sg_finger_1_base_link">
+ <material>Gazebo/White</material>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robl_sg_finger_2_base_link">
+ <material>Gazebo/White</material>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robl_sg_finger_1_link">
+ <material>Gazebo/Grey</material>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robl_sg_finger_2_link">
+ <material>Gazebo/Grey</material>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <joint name="robl_sg_joint" type="fixed">
+ <parent link="robl_link_7"/>
+ <child link="robl_sg_base_link"/>
+ <origin rpy="0.0 1.5707963267948966 0.0" xyz="0.0 0.0 0.0"/>
+ </joint>
+ <link name="robr_sg_base_link">
+ <visual>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_myumi_smart_gripper_description/meshes/sg/visual/base_link.dae"/>
+ </geometry>
+ <material name="light_grey">
+ <color rgba="0.6 0.6 0.6 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_myumi_smart_gripper_description/meshes/sg/collision/base_link.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="-0.0002 -0.007 0.0426"/>
+ <mass value="0.35"/>
+ <inertia ixx="8.73e-05" ixy="-2e-07" ixz="-4e-07" iyy="9.11e-05" iyz="-8.7e-06" izz="2.33e-05"/>
+ </inertial>
+ </link>
+ <link name="robr_sg_vacuum_1_frame_link">
+ </link>
+ <link name="robr_sg_vacuum_2_frame_link">
+ </link>
+ <link name="robr_sg_camera_frame_link">
+ </link>
+ <joint name="robr_sg_vacuum_1_frame_joint" type="fixed">
+ <parent link="robr_sg_base_link"/>
+ <child link="robr_sg_vacuum_1_frame_link"/>
+ <origin rpy="0.0 1.5707963267948966 0.0" xyz="0.0274 0.0185 0.0375"/>
+ </joint>
+ <joint name="robr_sg_vacuum_2_frame_joint" type="fixed">
+ <parent link="robr_sg_base_link"/>
+ <child link="robr_sg_vacuum_2_frame_link"/>
+ <origin rpy="-3.141592653589793 1.5707963267948966 0.0" xyz="-0.0274 0.0185 0.0375"/>
+ </joint>
+ <joint name="robr_sg_camera_frame_joint" type="fixed">
+ <parent link="robr_sg_base_link"/>
+ <child link="robr_sg_camera_frame_link"/>
+ <origin rpy="-1.5707963267948966 1.5707963267948966 0.0" xyz="-0.0073 0.0283 0.0351"/>
+ </joint>
+ <gazebo reference="robr_sg_base_link">
+ <material>Gazebo/White</material>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <link name="robr_sg_servo_frame_link">
+ </link>
+ <link name="robr_sg_fingers_frame_link">
+ </link>
+ <link name="robr_sg_finger_1_base_link">
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.00139 -0.014982"/>
+ <mass value="0.05"/>
+ <inertia ixx="3.08333333e-06" ixy="0.0" ixz="0.0" iyy="6.56666667e-06" iyz="0.0" izz="4.01666667e-06"/>
+ </inertial>
+ </link>
+ <link name="robr_sg_finger_2_base_link">
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.00139 -0.014982"/>
+ <mass value="0.05"/>
+ <inertia ixx="3.08333333e-06" ixy="0.0" ixz="0.0" iyy="6.56666667e-06" iyz="0.0" izz="4.01666667e-06"/>
+ </inertial>
+ </link>
+ <link name="robr_sg_finger_1_link">
+ <visual>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_myumi_smart_gripper_description/meshes/sg/visual/finger_link.stl"/>
+ </geometry>
+ <material name="light_grey">
+ <color rgba="0.6 0.6 0.6 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_myumi_smart_gripper_description/meshes/sg/collision/finger_link.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.011259 -0.004784 0.017852"/>
+ <mass value="0.01"/>
+ <inertia ixx="3.55e-06" ixy="-4.5e-07" ixz="1.68e-06" iyy="4.53e-06" iyz="6.8e-07" izz="1.54e-06"/>
+ </inertial>
+ </link>
+ <link name="robr_sg_finger_2_link">
+ <visual>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_myumi_smart_gripper_description/meshes/sg/visual/finger_link.stl"/>
+ </geometry>
+ <material name="light_grey">
+ <color rgba="0.6 0.6 0.6 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_myumi_smart_gripper_description/meshes/sg/collision/finger_link.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.011259 -0.004784 0.017852"/>
+ <mass value="0.01"/>
+ <inertia ixx="3.55e-06" ixy="-4.5e-07" ixz="1.68e-06" iyy="4.53e-06" iyz="6.8e-07" izz="1.54e-06"/>
+ </inertial>
+ </link>
+ <joint name="robr_sg_servo_frame_joint" type="fixed">
+ <parent link="robr_sg_base_link"/>
+ <child link="robr_sg_servo_frame_link"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.084"/>
+ </joint>
+ <joint name="robr_sg_fingers_frame_joint" type="fixed">
+ <parent link="robr_sg_base_link"/>
+ <child link="robr_sg_fingers_frame_link"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.1142"/>
+ </joint>
+ <joint name="robr_sg_finger_1_base_joint" type="fixed">
+ <parent link="robr_sg_base_link"/>
+ <child link="robr_sg_finger_1_base_link"/>
+ <origin rpy="0.0 0.0 0.0" xyz="-0.0125 0.0065 0.084"/>
+ </joint>
+ <joint name="robr_sg_finger_2_base_joint" type="fixed">
+ <parent link="robr_sg_base_link"/>
+ <child link="robr_sg_finger_2_base_link"/>
+ <origin rpy="0.0 0.0 3.141592653589793" xyz="0.0125 -0.0065 0.084"/>
+ </joint>
+ <joint name="robr_sg_finger_1_joint" type="fixed">
+ <parent link="robr_sg_finger_1_base_link"/>
+ <child link="robr_sg_finger_1_link"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ </joint>
+ <joint name="robr_sg_finger_2_joint" type="fixed">
+ <parent link="robr_sg_finger_2_base_link"/>
+ <child link="robr_sg_finger_2_link"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ </joint>
+ <gazebo reference="robr_sg_finger_1_base_link">
+ <material>Gazebo/White</material>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robr_sg_finger_2_base_link">
+ <material>Gazebo/White</material>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robr_sg_finger_1_link">
+ <material>Gazebo/Grey</material>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robr_sg_finger_2_link">
+ <material>Gazebo/Grey</material>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <joint name="robr_sg_joint" type="fixed">
+ <parent link="robr_link_7"/>
+ <child link="robr_sg_base_link"/>
+ <origin rpy="0.0 1.5707963267948966 0.0" xyz="0.0 0.0 0.0"/>
+ </joint>
+</robot>
diff --git a/python/ur_simple_control/util/__pycache__/__init__.cpython-312.pyc b/python/ur_simple_control/util/__pycache__/__init__.cpython-312.pyc
index 11e5fceb19b1ac1e286cd8e4d4939298f13090c7..388924771e93bdbcaeb01a6e13de839f5951627b 100644
Binary files a/python/ur_simple_control/util/__pycache__/__init__.cpython-312.pyc and b/python/ur_simple_control/util/__pycache__/__init__.cpython-312.pyc differ
diff --git a/python/ur_simple_control/util/__pycache__/get_model.cpython-311.pyc b/python/ur_simple_control/util/__pycache__/get_model.cpython-311.pyc
index 5683b3c8a5c1c9547d5161bdd4dbf9898905d83e..aa9f8054b344ab0333701bfe20c412a83188c33e 100644
Binary files a/python/ur_simple_control/util/__pycache__/get_model.cpython-311.pyc and b/python/ur_simple_control/util/__pycache__/get_model.cpython-311.pyc differ
diff --git a/python/ur_simple_control/util/__pycache__/get_model.cpython-312.pyc b/python/ur_simple_control/util/__pycache__/get_model.cpython-312.pyc
index 42e86edcddac775dd8dd65d294a9d30d13a39372..4431c6814afa3ccd929c1c6f3ebfff7f72ffa3be 100644
Binary files a/python/ur_simple_control/util/__pycache__/get_model.cpython-312.pyc and b/python/ur_simple_control/util/__pycache__/get_model.cpython-312.pyc differ
diff --git a/python/ur_simple_control/util/__pycache__/logging_utils.cpython-312.pyc b/python/ur_simple_control/util/__pycache__/logging_utils.cpython-312.pyc
index 78ef9cb72fde8a2173f7b05fe10cb5bb70c9784d..a70360a9ceaab78c4f4dbc30377f789bb8b68029 100644
Binary files a/python/ur_simple_control/util/__pycache__/logging_utils.cpython-312.pyc and b/python/ur_simple_control/util/__pycache__/logging_utils.cpython-312.pyc differ
diff --git a/python/ur_simple_control/util/encapsulating_ellipses.py b/python/ur_simple_control/util/encapsulating_ellipses.py
index ec0e6acb98ae99faa26c8dcf30acc53c46780ab0..80dd2081177d25516a243a6ecea25ef8adc89fda 100644
--- a/python/ur_simple_control/util/encapsulating_ellipses.py
+++ b/python/ur_simple_control/util/encapsulating_ellipses.py
@@ -115,6 +115,19 @@ def visualizeVertices(args, robot : RobotManager):
# TODO: make this for every robot.
def computeEncapsulatingEllipses(args, robot : RobotManager):
+ """
+ computeEncapsulatingEllipses
+ ----------------------------
+ make convex approximations of the robot's links
+ so that (self-) collision avoidace can be calculated more quickly
+ and easily.
+ this includes the need to group related links because otherwise
+ we have multiple ellipses for the same part of the robot.
+ the grouping of links has to be hardcoded and done manually
+ and as it is non-trivial to make the grouping algorithmically,
+ and since this is done once per robot there's no point
+ to writing the algorithm (more work overall).
+ """
model, collision_model, visual_model, data = (robot.model, robot.collision_model, robot.visual_model, robot.data)
viz = MeshcatVisualizer(model=model, collision_model=collision_model, visual_model=visual_model)
#q = np.zeros(model.nq)
@@ -127,18 +140,57 @@ def computeEncapsulatingEllipses(args, robot : RobotManager):
pin.computeAllTerms(model,data,q,np.zeros(model.nv))
time.sleep(3)
+ # vertex groups will have to be hardcoded per robot,
+ # there's no getting around it
+ vertices_grps = []
+ vertices_grps_indeces = [
+ #[0, 1], [2], [3], [4,5,6,7,8,9,10]
+ #[0, 1], [2], [3,4], [5,6,7,8,9,10]
+ [0, 1], [2], [3,4], [5,6,7,8,9,10]
+ ]
+ vertices_grps_joint_parents = [collision_model.geometryObjects[0].parentJoint,
+ collision_model.geometryObjects[2].parentJoint,
+ collision_model.geometryObjects[3].parentJoint,
+ #collision_model.geometryObjects[4].parentJoint]
+ collision_model.geometryObjects[5].parentJoint]
+ # index of parent joint needs to be here
+ # we also maybe need parent frames
+ #vertices_grps_joint_parents_indeces = [collision_model.geometryObjects[0].]
+ # we'll np.vstack related vertices into vertices_grp
+ for v in vertices_grps_indeces:
+ # put them into their group
+ vertices_grp = np.empty((0,3))
+ for j in v:
+ geom = collision_model.geometryObjects[j]
+ # plot em in meshcat
+ vertices = collision_model.geometryObjects[j].geometry.vertices()
+ for i in np.arange(0, vertices.shape[0]):
+ viz.addSphere(f"world/point_{i}", 5e-3, [1, 0, 0, 0.8])
+ vertix_pose = pin.SE3.Identity()
+ vertix_pose.translation = vertices[i]
+ #vertix_pose = data.oMi[geom.parentJoint].act(vertix_pose)
+ vertix_pose = data.oMf[geom.parentFrame].act(geom.placement.act(vertix_pose))
+ #viz.applyConfiguration(f"world/point_{i}", np.array(vertices[i].tolist() + [1, 0, 0, 0]))
+ viz.applyConfiguration(f"world/point_{i}", vertix_pose)
+
+ vertices_in_joint_frame = []
+ for g_v in vertices:
+ # g_v is the vertex v expressed in the geometry frame.
+ # Convert point from geometry frame to joint frame
+ j_v = geom.placement.act(g_v)
+ vertices_in_joint_frame.append(j_v)
+ vertices_in_joint_frame = np.array(vertices_in_joint_frame)
+
+ #vertices_grp = np.vstack((vertices_grp, collision_model.geometryObjects[j]))
+ vertices_grp = np.vstack((vertices_grp, vertices_in_joint_frame))
+ vertices_grps.append(vertices_grp)
+
+
ellipses = []
- for i, geom in enumerate(collision_model.geometryObjects):
- vertices = geom.geometry.vertices()
-
- for i in np.arange(0, vertices.shape[0]):
- viz.addSphere(f"world/point_{i}", 5e-3, [1, 0, 0, 0.8])
- vertix_pose = pin.SE3.Identity()
- vertix_pose.translation = vertices[i]
- #vertix_pose = data.oMi[geom.parentJoint].act(vertix_pose)
- vertix_pose = data.oMf[geom.parentFrame].act(geom.placement.act(vertix_pose))
- #viz.applyConfiguration(f"world/point_{i}", np.array(vertices[i].tolist() + [1, 0, 0, 0]))
- viz.applyConfiguration(f"world/point_{i}", vertix_pose)
+ # go over grouped vertices and compute their ellipse fits
+ #for i, geom in enumerate(collision_model.geometryObjects):
+ # vertices = geom.geometry.vertices()
+ for i, vertices in enumerate(vertices_grps):
cw = casadi.SX.sym("w", 3)
exp = casadi.Function("exp3", [cw], [cpin.exp3(cw)])
@@ -164,13 +216,15 @@ def computeEncapsulatingEllipses(args, robot : RobotManager):
A = R @ casadi.diag(1 / var_r**2) @ R.T
totalcost = var_r[0] * var_r[1] * var_r[2]
-
opti.subject_to(var_r >= 0)
-
- for g_v in vertices:
+# for g_v in vertices:
+ for j_v in vertices:
# g_v is the vertex v expressed in the geometry frame.
# Convert point from geometry frame to joint frame
- j_v = geom.placement.act(g_v)
+
+# print(j_v)
+# j_v = geom.placement.act(g_v)
+
# Constraint the ellipsoid to be including the point
opti.subject_to((j_v - var_c).T @ A @ (j_v - var_c) <= 1)
@@ -194,14 +248,15 @@ def computeEncapsulatingEllipses(args, robot : RobotManager):
# TODO: add placement=pin.SE3(P, ellipse.center) and id=robot.model.getJointId(e.name)
ellipse = SimpleNamespace(
- name=model.names[geom.parentJoint],
+ name=model.names[vertices_grps_joint_parents[i]],
A=sol_A,
center=sol_c)
ellipses.append(ellipse)
e, P = np.linalg.eig(sol_A)
ellipse_placement = pin.SE3(P, ellipse.center)
#ellipse_placement = data.oMf[geom.parentFrame].act(geom.placement.act(ellipse_placement))
- ellipse_placement = data.oMf[geom.parentFrame].act(ellipse_placement)
+ #ellipse_placement = data.oMf[geom.parentFrame].act(ellipse_placement)
+ ellipse_placement = data.oMi[vertices_grps_joint_parents[i]].act(ellipse_placement)
viz.addEllipsoid(f"el_{ellipse.name}", sol_r, [0.3, 0.9, 0.3, 0.3])
viz.applyConfiguration(f"el_{ellipse.name}", ellipse_placement)
print(ellipse)
@@ -259,7 +314,7 @@ if __name__ == "__main__":
args = get_args()
robot = RobotManager(args)
- computeEncapsulatingEllipses(args,robot)
+ computeEncapsulatingEllipses(args, robot)
# get expected behaviour here (library can't know what the end is - you have to do this here)
if not args.pinocchio_only:
diff --git a/python/ur_simple_control/util/get_model.py b/python/ur_simple_control/util/get_model.py
index 1b088c6fb6652603b1fa3000fae720df099e807e..b691c5ee6a2e8c8f89af042bbec9bae40d17c4da 100644
--- a/python/ur_simple_control/util/get_model.py
+++ b/python/ur_simple_control/util/get_model.py
@@ -262,14 +262,14 @@ def heron_approximation():
# TODO: make these parameters the same as in mpc_params in the planner
model_mobile_base.velocityLimit[0] = 2
# TODO: PUT THE CONSTRAINTS BACK!!!!!!!!!!!!!!!
- #model_mobile_base.velocityLimit[1] = 0
- model_mobile_base.velocityLimit[1] = 2
+ model_mobile_base.velocityLimit[1] = 0
+ #model_mobile_base.velocityLimit[1] = 2
model_mobile_base.velocityLimit[2] = 2
# TODO: i have literally no idea what reasonable numbers are here
model_mobile_base.effortLimit[0] = 200
# TODO: PUT THE CONSTRAINTS BACK!!!!!!!!!!!!!!!
- #model_mobile_base.effortLimit[1] = 0
- model_mobile_base.effortLimit[1] = 2
+ model_mobile_base.effortLimit[1] = 0
+ #model_mobile_base.effortLimit[1] = 2
model_mobile_base.effortLimit[2] = 200
#print("OBJECT_JOINT_ID",OBJECT_JOINT_ID)
#body_inertia = pin.Inertia.FromBox(args.box_mass, box_dimensions[0],
diff --git a/python/ur_simple_control/visualize/__pycache__/__init__.cpython-312.pyc b/python/ur_simple_control/visualize/__pycache__/__init__.cpython-312.pyc
index f047d1996c3cab390860f9ade0a64f578c4b48bf..ef461beee0f1f95021b9d5b6ac3926ed0403e6b3 100644
Binary files a/python/ur_simple_control/visualize/__pycache__/__init__.cpython-312.pyc and b/python/ur_simple_control/visualize/__pycache__/__init__.cpython-312.pyc differ
diff --git a/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc b/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc
index 876bff867c79676f94a98cbd0ad7a9d2f1e30182..31e46a697ef6ce9f033463f59951d89785f53a96 100644
Binary files a/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc and b/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc differ
diff --git a/python/ur_simple_control/visualize/meshcat_viewer_wrapper/visualizer.py b/python/ur_simple_control/visualize/meshcat_viewer_wrapper/visualizer.py
index 6dbd513fa60369776efa95a04450ec052d10030f..bdd34e3d75b5b149898fc2439ffb31bc8530a29e 100644
--- a/python/ur_simple_control/visualize/meshcat_viewer_wrapper/visualizer.py
+++ b/python/ur_simple_control/visualize/meshcat_viewer_wrapper/visualizer.py
@@ -80,13 +80,22 @@ class MeshcatVisualizer(PMV):
material = materialFromColor(color)
self.viewer[name].set_object(meshcat.geometry.Box(dims), material)
- def addObstacle(self, pose, dims):
+ def addBoxObstacle(self, pose, dims):
color = [0.5, 0.5, 0.5, 0.8]
obstacle_name = f"world/obstacle_{self.n_obstacles}"
self.addBox(obstacle_name, dims, color)
self.applyConfiguration(obstacle_name, pose)
self.n_obstacles += 1
+ def addSphereObstacle(self, radius, position):
+ color = [0.5, 0.5, 0.5, 0.8]
+ obstacle_name = f"world/obstacle_{self.n_obstacles}"
+ self.addSphere(obstacle_name, radius, color)
+ pose = pin.SE3.Identity()
+ pose.translation = np.array(position)
+ self.applyConfiguration(obstacle_name, pose)
+ self.n_obstacles += 1
+
def addEllipsoid(self, name, dims, color):
material = materialFromColor(color)
diff --git a/python/ur_simple_control/visualize/visualize.py b/python/ur_simple_control/visualize/visualize.py
index 07a69d24f5feb4660518f485243a175af8b09e68..862d3bd585a676ee6597129e874c3f8f6b744260 100644
--- a/python/ur_simple_control/visualize/visualize.py
+++ b/python/ur_simple_control/visualize/visualize.py
@@ -123,6 +123,8 @@ def realTimePlotter(args, log_item, queue):
ax.set_ylim(bottom=-20.0, top=20.0)
if 'tau' in data_key:
ax.set_ylim(bottom=-2.0, top=2.0)
+ if 'err' in data_key:
+ ax.set_ylim(bottom=-2.0, top=2.0)
axes_and_updating_artists[data_key] = AxisAndArtists(ax, {})
for j in range(log_item[data_key].shape[0]):
# the comma is because plot retuns ax, sth_unimportant.
@@ -169,6 +171,8 @@ def realTimePlotter(args, log_item, queue):
def manipulatorVisualizer(model, collision_model, visual_model, args, cmd, queue):
viz = MeshcatVisualizer(model=model, collision_model=collision_model, visual_model=visual_model)
viz.loadViewerModel()
+ # display the initial pose
+ viz.display(cmd["q"])
# set shapes we know we'll use
meshcat_shapes.frame(viz.viewer["Mgoal"], opacity=0.5)
meshcat_shapes.frame(viz.viewer["T_w_e"], opacity=0.5)
@@ -194,9 +198,12 @@ def manipulatorVisualizer(model, collision_model, visual_model, args, cmd, queue
viz.display(cmd["q"])
if key == "point":
viz.addPoint(cmd["point"])
- if key == "obstacle":
+ if key == "obstacle_sphere":
+ # stupid and evil but there is no time
+ viz.addSphereObstacle(cmd["obstacle_sphere"][0], cmd["obstacle_sphere"][1])
+ if key == "obstacle_box":
# stupid and evil but there is no time
- viz.addObstacle(cmd["obstacle"][0], cmd["obstacle"][1])
+ viz.addBoxObstacle(cmd["obstacle_box"][0], cmd["obstacle_box"][1])
if key == "path":
# stupid and evil but there is no time
viz.addPath("", cmd["path"])