from TOPP import Utilities
import TOPP
import numpy as np

def TOPPInterface(trajectorystring, discrtimestep, a, b, c):
        Ndim = 2
        vmax = 1e5*np.ones(Ndim) ## no velocity constraints
        sdbeg_min, sdbeg_max = 0.0 , 0.0
        topp_inst = TOPP.QuadraticConstraints(trajectorystring, discrtimestep, vmax, list(a), list(b), list(c))
        x = topp_inst.solver
        ret = x.RunVIP(sdbeg_min,sdbeg_max)
        print "discrtimestep",discrtimestep,"TOPP return code:",ret,
        if ret==1:
                print "(Success)",
                semin = x.sdendmin
                semax = x.sdendmax
                print "sd_end: [",semin,",",semax,"]"
        else:
                print "(Failure)"

trajectorystring = """0.181818
2
-8.60663863714e-19 2.19999381518 0.000102049482337 -0.000374181435274
0.0 -1.36189708232e-05 0.000224713018583 -0.000823947734803
0.090909
2
0.4 2.19999381518 -0.000102049482358 0.0018709071768
-4.23516473627e-22 -1.36189708232e-05 -0.000224713018583 0.00411973867401
0.090909
2
0.6 2.20002164686 0.000408197929498 -0.00710944727209
4.23516473627e-22 4.76663978811e-05 0.00089885207433 -0.0156550069613
0.090909
2
0.8 2.19991959738 -0.00153074223562 0.0265668819108
-1.69406589451e-21 -0.000177046620701 -0.00337069527874 0.058500289171
0.090909
2
1.0 2.20029996363 0.00571477101278 -0.0991580803692
0.0 0.000660520084924 0.0125839290406 -0.218346149723
0.090909
2
1.2 2.1988805481 -0.0213283418152 0.370065439564
2.71050543121e-20 -0.002465033719 -0.0469650208838 0.81488430972
0.090909
2
1.4 2.20417784396 0.0795985962476 -1.38110367788
-5.42101086243e-20 0.00919961479106 0.175276154494 -3.04119108916
0.090909
2
1.6 2.18440807606 -0.297066043175 5.15434927197
2.16840434497e-19 -0.0343334254452 -0.654139597094 11.3498800469
0.181818
2
1.8 2.25818985181 1.10866557645 -19.23629341
0.0 0.12813408699 2.44128223388 -42.3583290985"""

discrtimestep = 0.05
a =np.array( [[2.19999381518291, -1.3618970778241531e-05, -2.19999381518291,
1.3618970778241531e-05], [2.200001213770359, 2.6727230240498897e-06,
-2.200001213770359, -2.6727230240498897e-06], [2.200002999636299,
6.605200849244305e-06, -2.200002999636299, -6.605200849244305e-06],
[2.1999991727807098, -1.8215373476008889e-06, -2.1999991727807098,
1.8215373476008889e-06], [2.1999919597377526, -1.7704662070139386e-05,
-2.1999919597377526, 1.7704662070139386e-05], [2.20000599154158,
1.3193377984959673e-05, -2.20000599154158, -1.3193377984959673e-05],
[2.2000280481456116, 6.17620326831401e-05, -2.2000280481456116,
-6.17620326831401e-05], [2.199957378879067, -9.385173268526804e-05,
-2.199957378879067, 9.385173268526804e-05], [2.1999136599530926,
-0.0001901208326916509, -2.1999136599530926, 0.0001901208326916509],
[2.2002496578737976, 0.0005497467809913568, -2.2002496578737976,
-0.0005497467809913568], [2.200204872066884, 0.0004511284085179754,
-2.200204872066884, -0.0004511284085179754], [2.19870959202647,
-0.0028414790962626767, -2.19870959202647, 0.0028414790962626767],
[2.1998568833319925, -0.00031514298484846496, -2.1998568833319925,
0.00031514298484846496], [2.205578264367929, 0.012283341330840953,
-2.205578264367929, -0.012283341330840953], [2.1975298768172298,
-0.005439212662217453, -2.1975298768172298, 0.005439212662217453],
[2.1788921855701644, -0.0464794194553364, -2.1788921855701644,
0.0464794194553364], [2.2229864912831045, 0.05061626696144616,
-2.2229864912831045, -0.05061626696144616], [2.2703170180125904,
0.15483811390888688, -2.2703170180125904, -0.15483811390888688],
[2.0532922107148686, -0.3230506359723996, -2.0532922107148686,
0.3230506359723996], [1.5477230022673387, -1.4363143223307215,
-1.5477230022673387, 1.4363143223307215], [0.7536093926700032,
-3.184952945166074, -0.7536093926700032, 3.184952945166074]] )
b =np.array( [[0.00020409896444981745, 0.00044942603667070787,
-0.00020409896444981745, -0.00044942603667070787], [9.184453409225208e-05,
0.0002022417167242844, -9.184453409225208e-05, -0.0002022417167242844],
[-2.0409896489822143e-05, -4.494260371650774e-05, 2.0409896489822143e-05,
4.494260371650774e-05], [-0.00013266432707189642, -0.0002921269241572999,
0.00013266432707189642, 0.0002921269241572999], [2.703035278245336e-14,
5.421010862427522e-19, -2.703035278245336e-14, -5.421010862427522e-19],
[0.0005612721530677083, 0.0012359216022039621, -0.0005612721530677083,
-0.0012359216022039621], [-0.00034696824007336615, -0.0007640242631806369,
0.00034696824007336615, 0.0007640242631806369], [-0.002479802421699229,
-0.005460526351555691, 0.002479802421699229, 0.005460526351555691],
[0.0027349261274830297, 0.006022308898012032, -0.0027349261274830297,
-0.006022308898012032], [0.010704990700718328, 0.023572395649308284,
-0.010704990700718328, -0.023572395649308284], [-0.015613570802400115,
-0.03438109184312841, 0.015613570802400115, 0.03438109184312841],
[-0.03256398982408027, -0.07170592422968769, 0.03256398982408027,
0.07170592422968769], [0.07845564204497431, 0.17275936868625608,
-0.07845564204497431, -0.17275936868625608], [0.04619780066842563,
0.10172758351231465, -0.04619780066842563, -0.10172758351231465],
[-0.3681333026963967, -0.8106297432346503, 0.3681333026963967,
0.8106297432346503], [0.1087337234637511, 0.23943172129969237,
-0.1087337234637511, -0.23943172129969237], [1.6550385050538645,
3.6443957353716065, -1.6550385050538645, -3.6443957353716065],
[-1.4550521344563738, -3.2040256328554113, 1.4550521344563738,
3.2040256328554113], [-7.225940157452512, -15.911524362396072,
7.225940157452512, 15.911524362396072], [-12.996828180448663,
-28.619023091936764, 12.996828180448663, 28.619023091936764],
[-18.767716203444802, -41.32652182147743, 18.767716203444802,
41.32652182147743]] )
c =np.array( [[-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.3, -1.0, -0.7], [-1.0, -1.3,
-1.0, -0.7]] )

TOPPInterface( trajectorystring, discrtimestep, a, b, c)

discrtimestep= 0.02
a =np.array( [[2.19999381518291, -1.3618970778241531e-05, -2.19999381518291,
1.3618970778241531e-05], [2.199997448144461, -5.619187361645773e-06,
-2.199997448144461, 5.619187361645773e-06], [2.2000001830705873,
4.0312153636625174e-07, -2.2000001830705873, -4.0312153636625174e-07],
[2.200002019961269, 4.44795587085194e-06, -2.200002019961269,
-4.44795587085194e-06], [2.200002958816506, 6.51531564181129e-06,
-2.200002958816506, -6.51531564181129e-06], [2.200002999636299,
6.605200849244305e-06, -2.200002999636299, -6.605200849244305e-06],
[2.200002142420647, 4.717611493150978e-06, -2.200002142420647,
-4.717611493150978e-06], [2.20000038716955, 8.525475735313205e-07,
-2.20000038716955, -8.525475735313205e-07], [2.199997733883009,
-4.989990909614678e-06, -2.199997733883009, 4.989990909614678e-06],
[2.1999941825610225, -1.2810003956287002e-05, -2.1999941825610225,
1.2810003956287002e-05], [2.1999919597377526, -1.7704662070139386e-05,
-2.1999919597377526, 1.7704662070139386e-05], [2.1999942048263654,
-1.2760975661323534e-05, -2.1999942048263654, 1.2760975661323534e-05],
[2.2000009400922025, 2.0700835651240085e-06, -2.2000009400922025,
-2.0700835651240085e-06], [2.200012165535264, 2.6788515609203277e-05,
-2.200012165535264, -2.6788515609203277e-05], [2.2000264561736866,
5.82565095932526e-05, -2.2000264561736866, -5.82565095932526e-05],
[2.2000280481456116, 6.176203268314014e-05, -2.2000280481456116,
-6.176203268314014e-05], [2.2000125774440837, 2.769553906602724e-05,
-2.2000125774440837, -2.769553906602724e-05], [2.1999800440691026,
-4.394297125808617e-05, -2.1999800440691026, 4.394297125808617e-05],
[2.1999304480206687, -0.00015315349828919973, -2.1999304480206687,
0.00015315349828919973], [2.199890841688836, -0.0002403666636467065,
-2.199890841688836, 0.0002403666636467065], [2.1999136599530926,
-0.0001901208326916509, -2.1999136599530926, 0.0001901208326916509],
[2.200000238733935, 5.256922737742475e-07, -2.200000238733935,
-5.256922737742475e-07], [2.2001505780313635, 0.0003315729112495701,
-2.2001505780313635, -0.0003315729112495701], [2.200353456113208,
0.0007783105635741512, -2.200353456113208, -0.0007783105635741512],
[2.2003981537864887, 0.0008767348657133035, -2.2003981537864887,
-0.0008767348657133035], [2.200204872066884, 0.0004511284085179754,
-2.200204872066884, -0.0004511284085179754], [2.199773610954393,
-0.0004985088080118342, -2.199773610954393, 0.0004985088080118342],
[2.1991043704490156, -0.0019721767838761254, -2.1991043704490156,
0.0019721767838761254], [2.1984949717600983, -0.0033140730456436103,
-2.1984949717600983, 0.0033140730456436103], [2.198731849018569,
-0.002792469186909822, -2.198731849018569, 0.002792469186909822],
[2.1998568833319925, -0.0003151429848484845, -2.1998568833319925,
0.0003151429848484845], [2.201870074700368, 0.004117905560540404,
-2.201870074700368, -0.004117905560540404], [2.20470195525788,
0.01035370816897084, -2.20470195525788, -0.01035370816897084],
[2.2056259112712486, 0.012388259839217135, -2.2056259112712486,
-0.012388259839217135], [2.2032352184576984, 0.007123952895487705,
-2.2032352184576984, -0.007123952895487705], [2.1975298768172298,
-0.005439212662217453, -2.1975298768172298, 0.005439212662217453],
[2.1885098863498427, -0.0253012368338982, -2.1885098863498427,
0.0253012368338982], [2.179351153117117, -0.04546877265426141,
-2.179351153117117, 0.04546877265426141], [2.181525827586392,
-0.040680138228267555, -2.181525827586392, 0.040680138228267555],
[2.196070940308388, -0.008651791689698367, -2.196070940308388,
0.008651791689698367], [2.2229864912831045, 0.05061626696144616,
-2.2229864912831045, -0.05061626696144616], [2.2620305898393163,
0.136591394328683, -2.2620305898393163, -0.136591394328683],
[2.2790966513341577, 0.1741708715079004, -2.2790966513341577,
-0.1741708715079004], [2.2499956086450306, 0.11009035885079266,
-2.2499956086450306, -0.11009035885079266], [2.1747274617719343,
-0.055650143642640676, -2.1747274617719343, 0.055650143642640676],
[2.0532922107148686, -0.3230506359723996, -2.0532922107148686,
0.3230506359723996], [1.8856898554738333, -0.6921111181384841,
-1.8856898554738333, 0.6921111181384841], [1.6719203960488291,
-1.162831590140894, -1.6719203960488291, 1.162831590140894],
[1.411983832439857, -1.7352120519796261, -1.411983832439857,
1.7352120519796261], [1.1058801646469145, -2.4092525036546877,
-1.1058801646469145, 2.4092525036546877], [0.7536093926700032,
-3.184952945166074, -0.7536093926700032, 3.184952945166074]] )
b =np.array( [[0.00020409896444981745, 0.00044942603667070787,
-0.00020409896444981745, -0.00044942603667070787], [0.00015919719244149663,
0.00035055230898875967, -0.00015919719244149663, -0.00035055230898875967],
[0.00011429542020866692, 0.0002516785808124428, -0.00011429542020866692,
-0.0002516785808124428], [6.939364797583724e-05, 0.00015280485263612596,
-6.939364797583724e-05, -0.00015280485263612596], [2.4491875743007537e-05,
5.393112445980911e-05, -2.4491875743007537e-05, -5.393112445980911e-05],
[-2.0409896489822143e-05, -4.494260371650774e-05, 2.0409896489822143e-05,
4.494260371650774e-05], [-6.531166872265182e-05, -0.0001438163318928246,
6.531166872265182e-05, 0.0001438163318928246], [-0.00011021344095548156,
-0.00024269006006914144, 0.00011021344095548156, 0.00024269006006914144],
[-0.00015511521318831124, -0.0003415637882454583, 0.00015511521318831124,
0.0003415637882454583], [-0.00020001698542114092, -0.00044043751642177504,
0.00020001698542114092, 0.00044043751642177504], [2.703035278245336e-14,
5.421010862427522e-19, -2.703035278245336e-14, -5.421010862427522e-19],
[0.00022450886124330147, 0.000494368640881585, -0.00022450886124330147,
-0.000494368640881585], [0.00044901772245957254, 0.0009887372817631696,
-0.00044901772245957254, -0.0009887372817631696], [0.000673526583675844,
0.0014831059226447548, -0.000673526583675844, -0.0014831059226447548],
[0.0005061654325769797, 0.0011145765721693869, -0.0005061654325769797,
-0.0011145765721693869], [-0.00034696824007336377, -0.0007640242631806317,
0.00034696824007336377, 0.0007640242631806317], [-0.0012001019127237098,
-0.0026426250985306554, 0.0012001019127237098, 0.0026426250985306554],
[-0.0020532355853740557, -0.00452122593388068, 0.0020532355853740557,
0.00452122593388068], [-0.0029063692580243994, -0.006399826769230697,
0.0029063692580243994, 0.006399826769230697], [-0.0004530997018110934,
-0.0009977258025064759, 0.0004530997018110934, 0.0009977258025064759],
[0.0027349261274830297, 0.006022308898012032, -0.0027349261274830297,
-0.006022308898012032], [0.0059229519567771454, 0.01304234359853052,
-0.0059229519567771454, -0.01304234359853052], [0.009110977786071266,
0.020062378299049027, -0.009110977786071266, -0.020062378299049027],
[0.008184368486202954, 0.01802198409031961, -0.008184368486202954,
-0.01802198409031961], [-0.0037146011580985634, -0.008179553876404365,
0.0037146011580985634, 0.008179553876404365], [-0.015613570802400115,
-0.03438109184312841, 0.015613570802400115, 0.03438109184312841],
[-0.027512540446701667, -0.06058262980985246, 0.027512540446701667,
0.06058262980985246], [-0.039411510091003216, -0.0867841677765765,
0.039411510091003216, 0.0867841677765765], [-0.01036006345026936,
-0.02281286564649894, 0.01036006345026936, 0.02281286564649894],
[0.03404778929735222, 0.074973251519878, -0.03404778929735222,
-0.074973251519878], [0.07845564204497406, 0.17275936868625552,
-0.07845564204497406, -0.17275936868625552], [0.12286349479259588,
0.270545485852633, -0.12286349479259588, -0.270545485852633],
[0.1290640213413901, 0.2841990488617076, -0.1290640213413901,
-0.2841990488617076], [-0.03666842000453885, -0.08074388183707831,
0.03666842000453885, 0.08074388183707831], [-0.2024008613504678,
-0.44568681253586423, 0.2024008613504678, 0.44568681253586423],
[-0.3681333026963967, -0.8106297432346503, 0.3681333026963967,
0.8106297432346503], [-0.5338657440423247, -1.175572673933434,
0.5338657440423247, 1.175572673933434], [-0.20052723285427154,
-0.44156108151469053, 0.20052723285427154, 0.44156108151469053],
[0.41799467978177374, 0.9204245241140754, -0.41799467978177374,
-0.9204245241140754], [1.0365165924178192, 2.282410129742841,
-1.0365165924178192, -2.282410129742841], [1.6550385050538645,
3.6443957353716065, -1.6550385050538645, -3.6443957353716065],
[2.007480679341317, 4.420473604869002, -2.007480679341317, -4.420473604869002],
[-0.3008745298571305, -0.6625258869472459, 0.3008745298571305,
0.6625258869472459], [-2.609229739055591, -5.745525378763522,
2.609229739055591, 5.745525378763522], [-4.917584948254052,
-10.828524870579798, 4.917584948254052, 10.828524870579798],
[-7.225940157452512, -15.911524362396072, 7.225940157452512,
15.911524362396072], [-9.534295366650973, -20.99452385421235,
9.534295366650973, 20.99452385421235], [-11.842650575849433,
-26.077523346028624, 11.842650575849433, 26.077523346028624],
[-14.151005785047882, -31.16052283784487, 14.151005785047882,
31.16052283784487], [-16.459360994246342, -36.243522329661154,
16.459360994246342, 36.243522329661154], [-18.767716203444802,
-41.32652182147743, 18.767716203444802, 41.32652182147743]] )
c =np.array( [[-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.3, -1.0, -0.7], [-1.0, -1.3,
-1.0, -0.7], [-1.0, -1.3, -1.0, -0.7], [-1.0, -1.3, -1.0, -0.7], [-1.0, -1.3,
-1.0, -0.7]] )

TOPPInterface( trajectorystring, discrtimestep, a, b, c)

discrtimestep= 0.01
a =np.array( [[2.19999381518291, -1.3618970778241531e-05, -2.19999381518291,
1.3618970778241531e-05], [2.1999957439181057, -9.371894771974162e-06,
-2.1999957439181057, 9.371894771974162e-06], [2.199997448144461,
-5.619187361645773e-06, -2.199997448144461, 5.619187361645773e-06],
[2.1999989278619547, -2.360848592198971e-06, -2.1999989278619547,
2.360848592198971e-06], [2.2000001830705873, 4.0312153636625174e-07,
-2.2000001830705873, -4.0312153636625174e-07], [2.200001213770359,
2.6727230240498897e-06, -2.200001213770359, -2.6727230240498897e-06],
[2.200002019961269, 4.44795587085194e-06, -2.200002019961269,
-4.44795587085194e-06], [2.2000026016433183, 5.7288200767724065e-06,
-2.2000026016433183, -5.7288200767724065e-06], [2.200002958816506,
6.51531564181129e-06, -2.200002958816506, -6.51531564181129e-06],
[2.2000030914808333, 6.807442565968591e-06, -2.2000030914808333,
-6.807442565968591e-06], [2.200002999636299, 6.605200849244305e-06,
-2.200002999636299, -6.605200849244305e-06], [2.2000026832829036,
5.908590491638437e-06, -2.2000026832829036, -5.908590491638437e-06],
[2.200002142420647, 4.717611493150978e-06, -2.200002142420647,
-4.717611493150978e-06], [2.200001377049529, 3.0322638537819467e-06,
-2.200001377049529, -3.0322638537819467e-06], [2.20000038716955,
8.525475735313205e-07, -2.20000038716955, -8.525475735313205e-07],
[2.1999991727807098, -1.8215373476008838e-06, -2.1999991727807098,
1.8215373476008838e-06], [2.199997733883009, -4.989990909614678e-06,
-2.199997733883009, 4.989990909614678e-06], [2.199996070476446,
-8.652813112510057e-06, -2.199996070476446, 8.652813112510057e-06],
[2.1999941825610225, -1.2810003956287002e-05, -2.1999941825610225,
1.2810003956287002e-05], [2.1999925210099054, -1.6468740467935427e-05,
-2.1999925210099054, 1.6468740467935427e-05], [2.1999919597377526,
-1.7704662070139386e-05, -2.1999919597377526, 1.7704662070139386e-05],
[2.199992521009906, -1.6468740467935423e-05, -2.199992521009906,
1.6468740467935423e-05], [2.1999942048263654, -1.2760975661323534e-05,
-2.1999942048263654, 1.2760975661323534e-05], [2.199997011187131,
-6.581367650303716e-06, -2.199997011187131, 6.581367650303716e-06],
[2.2000009400922025, 2.0700835651240085e-06, -2.2000009400922025,
-2.0700835651240085e-06], [2.20000599154158, 1.3193377984959673e-05,
-2.20000599154158, -1.3193377984959673e-05], [2.200012165535264,
2.6788515609203277e-05, -2.200012165535264, -2.6788515609203277e-05],
[2.2000194620732536, 4.28554964378548e-05, -2.2000194620732536,
-4.28554964378548e-05], [2.2000264561736866, 5.82565095932526e-05,
-2.2000264561736866, -5.82565095932526e-05], [2.200029384993831,
6.47057732265714e-05, -2.200029384993831, -6.47057732265714e-05],
[2.2000280481456116, 6.176203268314014e-05, -2.2000280481456116,
-6.176203268314014e-05], [2.2000224456290294, 4.942528796295876e-05,
-2.2000224456290294, -4.942528796295876e-05], [2.2000125774440837,
2.769553906602724e-05, -2.2000125774440837, -2.769553906602724e-05],
[2.1999984435907747, -3.427214007654391e-06, -2.1999984435907747,
3.427214007654391e-06], [2.1999800440691026, -4.394297125808617e-05,
-2.1999800440691026, 4.394297125808617e-05], [2.199957378879067,
-9.385173268526804e-05, -2.199957378879067, 9.385173268526804e-05],
[2.1999304480206687, -0.00015315349828919973, -2.1999304480206687,
0.00015315349828919973], [2.199903342750427, -0.00021283931887034545,
-2.199903342750427, 0.00021283931887034545], [2.199890841688836,
-0.0002403666636467065, -2.199890841688836, 0.0002403666636467065],
[2.199894280756391, -0.000232793834920475, -2.199894280756391,
0.000232793834920475], [2.1999136599530926, -0.0001901208326916509,
-2.1999136599530926, 0.0001901208326916509], [2.1999489792789406,
-0.00011234765696023427, -2.1999489792789406, 0.00011234765696023427],
[2.200000238733935, 5.256922737742475e-07, -2.200000238733935,
-5.256922737742475e-07], [2.2000674383180763, 0.00014849921501037584,
-2.2000674383180763, -0.00014849921501037584], [2.2001505780313635,
0.0003315729112495701, -2.2001505780313635, -0.0003315729112495701],
[2.2002496578737976, 0.0005497467809913568, -2.2002496578737976,
-0.0005497467809913568], [2.200353456113208, 0.0007783105635741512,
-2.200353456113208, -0.0007783105635741512], [2.200405552373959,
0.0008930265595605372, -2.200405552373959, -0.0008930265595605372],
[2.2003981537864887, 0.0008767348657133035, -2.2003981537864887,
-0.0008767348657133035], [2.200331260350797, 0.0007294354820324496,
-2.200331260350797, -0.0007294354820324496], [2.200204872066884,
0.0004511284085179754, -2.200204872066884, -0.0004511284085179754],
[2.200018988934749, 4.1813645169880764e-05, -2.200018988934749,
-4.1813645169880764e-05], [2.199773610954393, -0.0004985088080118342,
-2.199773610954393, 0.0004985088080118342], [2.199468738125815,
-0.0011698389510271694, -2.199468738125815, 0.0011698389510271694],
[2.1991043704490156, -0.0019721767838761254, -2.1991043704490156,
0.0019721767838761254], [2.19870959202647, -0.0028414790962626767,
-2.19870959202647, 0.0028414790962626767], [2.1984949717600983,
-0.0033140730456436103, -2.1984949717600983, 0.0033140730456436103],
[2.1985023907574646, -0.003297736409192656, -2.1985023907574646,
0.003297736409192656], [2.198731849018569, -0.002792469186909822,
-2.198731849018569, 0.002792469186909822], [2.199183346543412,
-0.001798271378795097, -2.199183346543412, 0.001798271378795097],
[2.1998568833319925, -0.0003151429848484845, -2.1998568833319925,
0.0003151429848484845], [2.200752459384311, 0.001656915994930016,
-2.200752459384311, -0.001656915994930016], [2.201870074700368,
0.004117905560540404, -2.201870074700368, -0.004117905560540404],
[2.203209729280163, 0.007067825711982681, -2.203209729280163,
-0.007067825711982681], [2.20470195525788, 0.01035370816897084,
-2.20470195525788, -0.01035370816897084], [2.205578264367929,
0.012283341330840953, -2.205578264367929, -0.012283341330840953],
[2.2056259112712486, 0.012388259839217135, -2.2056259112712486,
-0.012388259839217135], [2.204844895967838, 0.010668463694099385,
-2.204844895967838, -0.010668463694099385], [2.2032352184576984,
0.007123952895487705, -2.2032352184576984, -0.007123952895487705],
[2.200796878740829, 0.0017547274433820934, -2.200796878740829,
-0.0017547274433820934], [2.1975298768172298, -0.005439212662217453,
-2.1975298768172298, 0.005439212662217453], [2.193434212686901,
-0.014457867421310815, -2.193434212686901, 0.014457867421310815],
[2.1885098863498427, -0.0253012368338982, -2.1885098863498427,
0.0253012368338982], [2.1829027302272497, -0.037648197825042584,
-2.1829027302272497, 0.037648197825042584], [2.179351153117117,
-0.04546877265426141, -2.179351153117117, 0.04546877265426141],
[2.1788921855701644, -0.0464794194553364, -2.1788921855701644,
0.0464794194553364], [2.181525827586392, -0.040680138228267555,
-2.181525827586392, 0.040680138228267555], [2.1872520791657997,
-0.028070928973054878, -2.1872520791657997, 0.028070928973054878],
[2.196070940308388, -0.008651791689698367, -2.196070940308388,
0.008651791689698367], [2.2079824110141564, 0.017577273621801975,
-2.2079824110141564, -0.017577273621801975], [2.2229864912831045,
0.05061626696144616, -2.2229864912831045, -0.05061626696144616],
[2.2410831811152336, 0.09046518832923417, -2.2410831811152336,
-0.09046518832923417], [2.2620305898393163, 0.136591394328683,
-2.2620305898393163, -0.136591394328683], [2.2763345086097333,
0.16808863164783236, -2.2763345086097333, -0.16808863164783236],
[2.2790966513341577, 0.1741708715079004, -2.2790966513341577,
-0.1741708715079004], [2.2703170180125904, 0.15483811390888724,
-2.2703170180125904, -0.15483811390888724], [2.2499956086450306,
0.11009035885079266, -2.2499956086450306, -0.11009035885079266],
[2.2181324232314785, 0.03992760633361672, -2.2181324232314785,
-0.03992760633361672], [2.1747274617719343, -0.055650143642640676,
-2.1747274617719343, 0.055650143642640676], [2.1197807242663975,
-0.17664289107797945, -2.1197807242663975, 0.17664289107797945],
[2.0532922107148686, -0.3230506359723996, -2.0532922107148686,
0.3230506359723996], [1.975261921117347, -0.49487337832590117,
-1.975261921117347, 0.49487337832590117], [1.8856898554738333,
-0.6921111181384841, -1.8856898554738333, 0.6921111181384841],
[1.7845760137843274, -0.9147638554101486, -1.7845760137843274,
0.9147638554101486], [1.6719203960488291, -1.162831590140894,
-1.6719203960488291, 1.162831590140894], [1.5477230022673387,
-1.4363143223307215, -1.5477230022673387, 1.4363143223307215],
[1.411983832439857, -1.7352120519796261, -1.411983832439857,
1.7352120519796261], [1.2647028865663819, -2.0595247790876163,
-1.2647028865663819, 2.0595247790876163], [1.1058801646469145,
-2.4092525036546877, -1.1058801646469145, 2.4092525036546877],
[0.9355156666814548, -2.7843952256808397, -0.9355156666814548,
2.7843952256808397], [0.7536093926700032, -3.184952945166074,
-0.7536093926700032, 3.184952945166074]] )
b =np.array( [[0.00020409896444981745, 0.00044942603667070787,
-0.00020409896444981745, -0.00044942603667070787], [0.00018164807855791147,
0.0003999891730769181, -0.00018164807855791147, -0.0003999891730769181],
[0.00015919719244149663, 0.00035055230898875967, -0.00015919719244149663,
-0.00035055230898875967], [0.00013674630632508176, 0.00030111544490060124,
-0.00013674630632508176, -0.00030111544490060124], [0.00011429542020866692,
0.0002516785808124428, -0.00011429542020866692, -0.0002516785808124428],
[9.184453409225208e-05, 0.0002022417167242844, -9.184453409225208e-05,
-0.0002022417167242844], [6.939364797583724e-05, 0.00015280485263612596,
-6.939364797583724e-05, -0.00015280485263612596], [4.694276185942238e-05,
0.00010336798854796754, -4.694276185942238e-05, -0.00010336798854796754],
[2.4491875743007537e-05, 5.393112445980911e-05, -2.4491875743007537e-05,
-5.393112445980911e-05], [2.0409896265926968e-06, 4.494260371650741e-06,
-2.0409896265926968e-06, -4.494260371650741e-06], [-2.0409896489822143e-05,
-4.494260371650774e-05, 2.0409896489822143e-05, 4.494260371650774e-05],
[-4.286078260623701e-05, -9.437946780466611e-05, 4.286078260623701e-05,
9.437946780466611e-05], [-6.531166872265182e-05, -0.0001438163318928246,
6.531166872265182e-05, 0.0001438163318928246], [-8.776255483906669e-05,
-0.00019325319598098296, 8.776255483906669e-05, 0.00019325319598098296],
[-0.00011021344095548156, -0.00024269006006914144, 0.00011021344095548156,
0.00024269006006914144], [-0.00013266432707189637, -0.0002921269241572998,
0.00013266432707189637, 0.0002921269241572998], [-0.00015511521318831124,
-0.0003415637882454583, 0.00015511521318831124, 0.0003415637882454583],
[-0.0001775660993047261, -0.0003910006523336168, 0.0001775660993047261,
0.0003910006523336168], [-0.00020001698542114092, -0.00044043751642177504,
0.00020001698542114092, 0.00044043751642177504], [-0.00011225443058110538,
-0.0002471843204407921, 0.00011225443058110538, 0.0002471843204407921],
[2.703035278245336e-14, 5.421010862427522e-19, -2.703035278245336e-14,
-5.421010862427522e-19], [0.00011225443063516574, 0.00024718432044079245,
-0.00011225443063516574, -0.00024718432044079245], [0.00022450886124330147,
0.000494368640881585, -0.00022450886124330147, -0.000494368640881585],
[0.00033676329185143713, 0.0007415529613223775, -0.00033676329185143713,
-0.0007415529613223775], [0.00044901772245957254, 0.0009887372817631696,
-0.00044901772245957254, -0.0009887372817631696], [0.0005612721530677083,
0.0012359216022039621, -0.0005612721530677083, -0.0012359216022039621],
[0.000673526583675844, 0.0014831059226447548, -0.000673526583675844,
-0.0014831059226447548], [0.0007857810142839799, 0.0017302902430855476,
-0.0007857810142839799, -0.0017302902430855476], [0.0005061654325769797,
0.0011145765721693869, -0.0005061654325769797, -0.0011145765721693869],
[7.959859625180916e-05, 0.00017527615449438021, -7.959859625180916e-05,
-0.00017527615449438021], [-0.00034696824007336377, -0.0007640242631806317,
0.00034696824007336377, 0.0007640242631806317], [-0.0007735350763985369,
-0.0017033246808556435, 0.0007735350763985369, 0.0017033246808556435],
[-0.0012001019127237098, -0.0026426250985306554, 0.0012001019127237098,
0.0026426250985306554], [-0.0016266687490488828, -0.003581925516205667,
0.0016266687490488828, 0.003581925516205667], [-0.0020532355853740557,
-0.00452122593388068, 0.0020532355853740557, 0.00452122593388068],
[-0.002479802421699229, -0.005460526351555691, 0.002479802421699229,
0.005460526351555691], [-0.0029063692580243994, -0.006399826769230697,
0.0029063692580243994, 0.006399826769230697], [-0.0020471126164581547,
-0.0045077431527657304, 0.0020471126164581547, 0.0045077431527657304],
[-0.0004530997018110934, -0.0009977258025064759, 0.0004530997018110934,
0.0009977258025064759], [0.0011409132128359688, 0.002512291547752777,
-0.0011409132128359688, -0.002512291547752777], [0.0027349261274830297,
0.006022308898012032, -0.0027349261274830297, -0.006022308898012032],
[0.0043289390421300915, 0.009532326248271284, -0.0043289390421300915,
-0.009532326248271284], [0.0059229519567771454, 0.01304234359853052,
-0.0059229519567771454, -0.01304234359853052], [0.0075169648714242055,
0.01655236094878977, -0.0075169648714242055, -0.01655236094878977],
[0.009110977786071266, 0.020062378299049027, -0.009110977786071266,
-0.020062378299049027], [0.010704990700718328, 0.023572395649308284,
-0.010704990700718328, -0.023572395649308284], [0.008184368486202954,
0.01802198409031961, -0.008184368486202954, -0.01802198409031961],
[0.002234883664052178, 0.004921215106957588, -0.002234883664052178,
-0.004921215106957588], [-0.0037146011580985634, -0.008179553876404365,
0.0037146011580985634, 0.008179553876404365], [-0.00966408598024934,
-0.021280322859766387, 0.00966408598024934, 0.021280322859766387],
[-0.015613570802400115, -0.03438109184312841, 0.015613570802400115,
0.03438109184312841], [-0.02156305562455089, -0.04748186082649043,
0.02156305562455089, 0.04748186082649043], [-0.027512540446701667,
-0.06058262980985246, 0.027512540446701667, 0.06058262980985246],
[-0.03346202526885245, -0.07368339879321448, 0.03346202526885245,
0.07368339879321448], [-0.039411510091003216, -0.0867841677765765,
0.039411510091003216, 0.0867841677765765], [-0.03256398982408027,
-0.07170592422968769, 0.03256398982408027, 0.07170592422968769],
[-0.01036006345026936, -0.02281286564649894, 0.01036006345026936,
0.02281286564649894], [0.01184386292354156, 0.02608019293668981,
-0.01184386292354156, -0.02608019293668981], [0.03404778929735222,
0.074973251519878, -0.03404778929735222, -0.074973251519878],
[0.05625171567116315, 0.12386631010306677, -0.05625171567116315,
-0.12386631010306677], [0.07845564204497406, 0.17275936868625552,
-0.07845564204497406, -0.17275936868625552], [0.10065956841878497,
0.22165242726944429, -0.10065956841878497, -0.22165242726944429],
[0.12286349479259588, 0.270545485852633, -0.12286349479259588,
-0.270545485852633], [0.1450674211664068, 0.31943854443582176,
-0.1450674211664068, -0.31943854443582176], [0.1290640213413901,
0.2841990488617076, -0.1290640213413901, -0.2841990488617076],
[0.04619780066842563, 0.10172758351231465, -0.04619780066842563,
-0.10172758351231465], [-0.03666842000453885, -0.08074388183707831,
0.03666842000453885, 0.08074388183707831], [-0.1195346406775033,
-0.26321534718647127, 0.1195346406775033, 0.26321534718647127],
[-0.2024008613504678, -0.44568681253586423, 0.2024008613504678,
0.44568681253586423], [-0.28526708202343226, -0.6281582778852572,
0.28526708202343226, 0.6281582778852572], [-0.3681333026963967,
-0.8106297432346503, 0.3681333026963967, 0.8106297432346503],
[-0.4509995233693603, -0.9931012085840412, 0.4509995233693603,
0.9931012085840412], [-0.5338657440423247, -1.175572673933434,
0.5338657440423247, 1.175572673933434], [-0.5097881891722942,
-1.1225538843290734, 0.5097881891722942, 1.1225538843290734],
[-0.20052723285427154, -0.44156108151469053, 0.20052723285427154,
0.44156108151469053], [0.1087337234637511, 0.23943172129969237,
-0.1087337234637511, -0.23943172129969237], [0.41799467978177374,
0.9204245241140754, -0.41799467978177374, -0.9204245241140754],
[0.7272556360997966, 1.6014173269284582, -0.7272556360997966,
-1.6014173269284582], [1.0365165924178192, 2.282410129742841,
-1.0365165924178192, -2.282410129742841], [1.3457775487358419,
2.9634029325572238, -1.3457775487358419, -2.9634029325572238],
[1.6550385050538645, 3.6443957353716065, -1.6550385050538645,
-3.6443957353716065], [1.9642994613718872, 4.325388538185989,
-1.9642994613718872, -4.325388538185989], [2.007480679341317,
4.420473604869002, -2.007480679341317, -4.420473604869002],
[0.8533030747420869, 1.8789738589608644, -0.8533030747420869,
-1.8789738589608644], [-0.3008745298571305, -0.6625258869472459,
0.3008745298571305, 0.6625258869472459], [-1.455052134456361,
-3.204025632855383, 1.455052134456361, 3.204025632855383], [-2.609229739055591,
-5.745525378763522, 2.609229739055591, 5.745525378763522], [-3.763407343654822,
-8.287025124671658, 3.763407343654822, 8.287025124671658], [-4.917584948254052,
-10.828524870579798, 4.917584948254052, 10.828524870579798],
[-6.071762552853282, -13.370024616487935, 6.071762552853282,
13.370024616487935], [-7.225940157452512, -15.911524362396072,
7.225940157452512, 15.911524362396072], [-8.380117762051743,
-18.45302410830421, 8.380117762051743, 18.45302410830421], [-9.534295366650973,
-20.99452385421235, 9.534295366650973, 20.99452385421235],
[-10.688472971250203, -23.536023600120487, 10.688472971250203,
23.536023600120487], [-11.842650575849433, -26.077523346028624,
11.842650575849433, 26.077523346028624], [-12.996828180448663,
-28.619023091936764, 12.996828180448663, 28.619023091936764],
[-14.151005785047882, -31.16052283784487, 14.151005785047882,
31.16052283784487], [-15.305183389647112, -33.70202258375301,
15.305183389647112, 33.70202258375301], [-16.459360994246342,
-36.243522329661154, 16.459360994246342, 36.243522329661154],
[-17.613538598845572, -38.78502207556929, 17.613538598845572,
38.78502207556929], [-18.767716203444802, -41.32652182147743,
18.767716203444802, 41.32652182147743]] )
c =np.array( [[-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.0,
-1.0, -1.0], [-1.0, -1.0, -1.0, -1.0], [-1.0, -1.3, -1.0, -0.7], [-1.0, -1.3,
-1.0, -0.7], [-1.0, -1.3, -1.0, -0.7], [-1.0, -1.3, -1.0, -0.7], [-1.0, -1.3,
-1.0, -0.7], [-1.0, -1.3, -1.0, -0.7], [-1.0, -1.3, -1.0, -0.7], [-1.0, -1.3,
-1.0, -0.7], [-1.0, -1.3, -1.0, -0.7], [-1.0, -1.3, -1.0, -0.7]] )

TOPPInterface( trajectorystring, discrtimestep, a, b, c)