diff --git a/python/examples/drawing_from_input_drawing.py b/python/examples/drawing_from_input_drawing.py
index 6e85c4f3cbcb8b0cdab7239362bae306ef72c247..05d53990d35dc103fc0de8322f6bb6b84fa170ec 100644
--- a/python/examples/drawing_from_input_drawing.py
+++ b/python/examples/drawing_from_input_drawing.py
@@ -336,7 +336,8 @@ if __name__ == "__main__":
     else:
         print("i hope you know the magic number of marker length + going into board")
         #path = path + np.array([0.0, 0.0, -0.1503])
-        path_points_3D = path_points_3D + np.array([0.0, 0.0, - 0.192792+ 0.003])
+        marker_offset = 0.066
+        path_points_3D = path_points_3D + np.array([0.0, 0.0,  -1 * marker_offset + args.mm_into_board])
 
     # create a joint space trajectory based on the 3D path
     if args.draw_new or args.calibration or args.find_marker_offset:
diff --git a/python/examples/joint_trajectory.csv b/python/examples/joint_trajectory.csv
index f2668f7a2d27fdeefd5fd7f825202cd137175a66..553d3888af1730d39cba390893138d0c52293bd7 100644
--- a/python/examples/joint_trajectory.csv
+++ b/python/examples/joint_trajectory.csv
@@ -1,191 +1,25 @@
-0.00000,0.91603,-1.42930,-1.42367,-1.08082,2.07522,-0.44683
-0.05263,0.91599,-1.42930,-1.42368,-1.08081,2.07525,-0.44687
-0.10526,0.91594,-1.42931,-1.42368,-1.08081,2.07528,-0.44690
-0.15789,0.91590,-1.42932,-1.42369,-1.08081,2.07530,-0.44693
-0.21053,0.91586,-1.42932,-1.42369,-1.08080,2.07533,-0.44696
-0.26316,0.91582,-1.42934,-1.42368,-1.08081,2.07536,-0.44700
-0.31579,0.91577,-1.42936,-1.42367,-1.08081,2.07538,-0.44703
-0.36842,0.91574,-1.42938,-1.42365,-1.08082,2.07540,-0.44706
-0.42105,0.91570,-1.42940,-1.42362,-1.08083,2.07542,-0.44708
-0.47368,0.91570,-1.42943,-1.42357,-1.08086,2.07543,-0.44708
-0.52632,0.91570,-1.42945,-1.42352,-1.08088,2.07542,-0.44708
-0.57895,0.91586,-1.42967,-1.42286,-1.08119,2.07527,-0.44693
-0.63158,0.91806,-1.43128,-1.41719,-1.08387,2.07343,-0.44499
-0.68421,0.92030,-1.43242,-1.41264,-1.08605,2.07165,-0.44307
-0.73684,0.92385,-1.43359,-1.40696,-1.08880,2.06892,-0.44009
-0.78947,0.92605,-1.43420,-1.40373,-1.09037,2.06725,-0.43827
-0.84211,0.93031,-1.43501,-1.39841,-1.09299,2.06407,-0.43477
-0.89474,0.93332,-1.43537,-1.39512,-1.09463,2.06184,-0.43233
-0.94737,0.93765,-1.43588,-1.39055,-1.09691,2.05866,-0.42884
-1.00000,0.94323,-1.43579,-1.38614,-1.09920,2.05460,-0.42438
-1.05263,0.95004,-1.43490,-1.38230,-1.10133,2.04966,-0.41898
-1.10526,0.95555,-1.43363,-1.38028,-1.10259,2.04568,-0.41464
-1.15789,0.96144,-1.43182,-1.37897,-1.10356,2.04142,-0.41002
-1.21053,0.96856,-1.42914,-1.37836,-1.10433,2.03628,-0.40446
-1.26316,0.97170,-1.42790,-1.37822,-1.10461,2.03400,-0.40202
-1.31579,0.97627,-1.42607,-1.37809,-1.10499,2.03070,-0.39848
-1.36842,0.98278,-1.42328,-1.37830,-1.10536,2.02598,-0.39346
-1.42105,0.99140,-1.41921,-1.37930,-1.10555,2.01974,-0.38685
-1.47368,0.99948,-1.41478,-1.38142,-1.10524,2.01387,-0.38068
-1.52632,1.00572,-1.41108,-1.38357,-1.10478,2.00932,-0.37594
-1.57895,1.01185,-1.40707,-1.38639,-1.10404,2.00485,-0.37130
-1.63158,1.01580,-1.40418,-1.38875,-1.10334,2.00197,-0.36832
-1.68421,1.02091,-1.40010,-1.39240,-1.10218,1.99823,-0.36447
-1.73684,1.02605,-1.39555,-1.39689,-1.10068,1.99447,-0.36061
-1.78947,1.03216,-1.38974,-1.40293,-1.09860,1.98999,-0.35603
-1.84211,1.03819,-1.38371,-1.40943,-1.09634,1.98556,-0.35154
-1.89474,1.04316,-1.37869,-1.41491,-1.09444,1.98190,-0.34784
-1.94737,1.04718,-1.37452,-1.41951,-1.09283,1.97894,-0.34486
-2.00000,1.05399,-1.36764,-1.42703,-1.09024,1.97391,-0.33982
-2.05263,1.05700,-1.36464,-1.43030,-1.08912,1.97170,-0.33761
-2.10526,1.06037,-1.36100,-1.43444,-1.08768,1.96920,-0.33513
-2.15789,1.06473,-1.35588,-1.44048,-1.08553,1.96597,-0.33192
-2.21053,1.07328,-1.34348,-1.45637,-1.07969,1.95963,-0.32565
-2.26316,1.07887,-1.33343,-1.47006,-1.07455,1.95547,-0.32156
-2.31579,1.08285,-1.32510,-1.48176,-1.07011,1.95250,-0.31864
-2.36842,1.08539,-1.31949,-1.48971,-1.06710,1.95060,-0.31677
-2.42105,1.08848,-1.31249,-1.49965,-1.06334,1.94829,-0.31452
-2.47368,1.09254,-1.30195,-1.51492,-1.05754,1.94525,-0.31155
-2.52632,1.09618,-1.29151,-1.53020,-1.05175,1.94252,-0.30888
-2.57895,1.09892,-1.28316,-1.54247,-1.04712,1.94046,-0.30688
-2.63158,1.10061,-1.27678,-1.55206,-1.04347,1.93919,-0.30564
-2.68421,1.10151,-1.27244,-1.55871,-1.04092,1.93851,-0.30497
-2.73684,1.10204,-1.26966,-1.56298,-1.03929,1.93810,-0.30457
-2.78947,1.10235,-1.26733,-1.56665,-1.03787,1.93787,-0.30434
-2.84211,1.10244,-1.26656,-1.56785,-1.03741,1.93780,-0.30427
-2.89474,1.10253,-1.26275,-1.57407,-1.03498,1.93772,-0.30418
-2.94737,1.10214,-1.25929,-1.57997,-1.03262,1.93800,-0.30444
-3.00000,1.09954,-1.25473,-1.58892,-1.02887,1.93992,-0.30625
-3.05263,1.09450,-1.25071,-1.59839,-1.02468,1.94365,-0.30981
-3.10526,1.08638,-1.24732,-1.60863,-1.01991,1.94966,-0.31559
-3.15789,1.07369,-1.24538,-1.61925,-1.01457,1.95904,-0.32470
-3.21053,1.05745,-1.24572,-1.62840,-1.00949,1.97101,-0.33647
-3.26316,1.04007,-1.24851,-1.63451,-1.00554,1.98377,-0.34921
-3.31579,1.02828,-1.25093,-1.63794,-1.00317,1.99240,-0.35794
-3.36842,1.00717,-1.25522,-1.64438,-0.99888,2.00778,-0.37374
-3.42105,0.98847,-1.25947,-1.64970,-0.99530,2.02132,-0.38793
-3.47368,0.97217,-1.26373,-1.65366,-0.99250,2.03306,-0.40044
-3.52632,0.96423,-1.26581,-1.65566,-0.99113,2.03876,-0.40659
-3.57895,0.95285,-1.26914,-1.65806,-0.98936,2.04690,-0.41546
-3.63158,0.94132,-1.27226,-1.66104,-0.98739,2.05512,-0.42453
-3.68421,0.92713,-1.27550,-1.66585,-0.98456,2.06518,-0.43579
-3.73684,0.91552,-1.27761,-1.67077,-0.98191,2.07337,-0.44509
-3.78947,0.90558,-1.27906,-1.67566,-0.97940,2.08035,-0.45312
-3.84211,0.89010,-1.28140,-1.68336,-0.97549,2.09117,-0.46574
-3.89474,0.87770,-1.28278,-1.69046,-0.97205,2.09979,-0.47596
-3.94737,0.87080,-1.28303,-1.69530,-0.96982,2.10456,-0.48169
-4.00000,0.86349,-1.28309,-1.70081,-0.96733,2.10960,-0.48779
-4.05263,0.85989,-1.28280,-1.70404,-0.96592,2.11208,-0.49081
-4.10526,0.85775,-1.28253,-1.70613,-0.96502,2.11354,-0.49260
-4.15789,0.85528,-1.28220,-1.70855,-0.96397,2.11524,-0.49468
-4.21053,0.85079,-1.28142,-1.71330,-0.96196,2.11832,-0.49847
-4.26316,0.84544,-1.27989,-1.71990,-0.95921,2.12197,-0.50299
-4.31579,0.83978,-1.27767,-1.72787,-0.95596,2.12582,-0.50780
-4.36842,0.83730,-1.27660,-1.73152,-0.95448,2.12751,-0.50991
-4.42105,0.83427,-1.27515,-1.73622,-0.95259,2.12957,-0.51250
-4.47368,0.83309,-1.27456,-1.73807,-0.95185,2.13036,-0.51351
-4.52632,0.83249,-1.27426,-1.73903,-0.95146,2.13077,-0.51403
-4.57895,0.83246,-1.27424,-1.73908,-0.95144,2.13079,-0.51405
-4.63158,0.83189,-1.27395,-1.73999,-0.95108,2.13118,-0.51454
-4.68421,0.83071,-1.27333,-1.74191,-0.95031,2.13198,-0.51555
-4.73684,0.83062,-1.27329,-1.74205,-0.95026,2.13204,-0.51563
-4.78947,0.83059,-1.27328,-1.74209,-0.95024,2.13206,-0.51566
-4.84211,0.83056,-1.27326,-1.74213,-0.95022,2.13208,-0.51568
-4.89474,0.83053,-1.27325,-1.74218,-0.95021,2.13210,-0.51571
-4.94737,0.83050,-1.27324,-1.74221,-0.95019,2.13212,-0.51573
-5.00000,0.83048,-1.27324,-1.74223,-0.95018,2.13213,-0.51575
-5.05263,0.83045,-1.27325,-1.74224,-0.95018,2.13215,-0.51577
-5.10526,0.83043,-1.27328,-1.74222,-0.95019,2.13217,-0.51579
-5.15789,0.83030,-1.27346,-1.74203,-0.95025,2.13226,-0.51590
-5.21053,0.82482,-1.28333,-1.73092,-0.95398,2.13599,-0.52068
-5.26316,0.81957,-1.29387,-1.71852,-0.95828,2.13955,-0.52528
-5.31579,0.81592,-1.30166,-1.70912,-0.96159,2.14203,-0.52849
-5.36842,0.81343,-1.30771,-1.70153,-0.96432,2.14372,-0.53071
-5.42105,0.80680,-1.32449,-1.68011,-0.97212,2.14821,-0.53660
-5.47368,0.80185,-1.33615,-1.66543,-0.97752,2.15155,-0.54103
-5.52632,0.79932,-1.34200,-1.65810,-0.98024,2.15325,-0.54330
-5.57895,0.79405,-1.35332,-1.64418,-0.98542,2.15679,-0.54804
-5.63158,0.79026,-1.36108,-1.63477,-0.98894,2.15933,-0.55146
-5.68421,0.78520,-1.37153,-1.62204,-0.99374,2.16270,-0.55604
-5.73684,0.77216,-1.39971,-1.58707,-1.00713,2.17137,-0.56795
-5.78947,0.76747,-1.41014,-1.57394,-1.01226,2.17447,-0.57228
-5.84211,0.76480,-1.41651,-1.56572,-1.01550,2.17623,-0.57475
-5.89474,0.76184,-1.42417,-1.55558,-1.01952,2.17819,-0.57750
-5.94737,0.75818,-1.43465,-1.54135,-1.02522,2.18060,-0.58093
-6.00000,0.75454,-1.44745,-1.52312,-1.03259,2.18301,-0.58436
-6.05263,0.75139,-1.45971,-1.50525,-1.03989,2.18508,-0.58735
-6.10526,0.74879,-1.47066,-1.48905,-1.04657,2.18680,-0.58984
-6.15789,0.74694,-1.47882,-1.47684,-1.05163,2.18802,-0.59161
-6.21053,0.74491,-1.48894,-1.46140,-1.05807,2.18936,-0.59356
-6.26316,0.74300,-1.50151,-1.44156,-1.06642,2.19063,-0.59544
-6.31579,0.74189,-1.51392,-1.42112,-1.07510,2.19137,-0.59657
-6.36842,0.74180,-1.52481,-1.40222,-1.08321,2.19146,-0.59674
-6.42105,0.74197,-1.53121,-1.39084,-1.08813,2.19137,-0.59664
-6.47368,0.74224,-1.53396,-1.38575,-1.09034,2.19121,-0.59641
-6.52632,0.74253,-1.53647,-1.38106,-1.09238,2.19103,-0.59617
-6.57895,0.74262,-1.53717,-1.37974,-1.09296,2.19097,-0.59609
-6.63158,0.74272,-1.53787,-1.37841,-1.09353,2.19091,-0.59600
-6.68421,0.74283,-1.53855,-1.37710,-1.09411,2.19084,-0.59591
-6.73684,0.74440,-1.54642,-1.36169,-1.10085,2.18985,-0.59452
-6.78947,0.74525,-1.55015,-1.35427,-1.10411,2.18931,-0.59377
-6.84211,0.74641,-1.55446,-1.34548,-1.10799,2.18857,-0.59273
-6.89474,0.74794,-1.55935,-1.33531,-1.11248,2.18760,-0.59136
-6.94737,0.74935,-1.56353,-1.32650,-1.11640,2.18670,-0.59010
-7.00000,0.75148,-1.56856,-1.31544,-1.12133,2.18533,-0.58818
-7.05263,0.75387,-1.57341,-1.30444,-1.12625,2.18379,-0.58602
-7.10526,0.75687,-1.57734,-1.29445,-1.13077,2.18185,-0.58330
-7.15789,0.76411,-1.58344,-1.27641,-1.13901,2.17715,-0.57675
-7.21053,0.76764,-1.58546,-1.26932,-1.14228,2.17485,-0.57357
-7.26316,0.77007,-1.58666,-1.26478,-1.14439,2.17326,-0.57138
-7.31579,0.77401,-1.58767,-1.25912,-1.14705,2.17067,-0.56782
-7.36842,0.77751,-1.58811,-1.25494,-1.14905,2.16838,-0.56468
-7.42105,0.78121,-1.58822,-1.25118,-1.15086,2.16594,-0.56136
-7.47368,0.78464,-1.58821,-1.24789,-1.15246,2.16367,-0.55829
-7.52632,0.79062,-1.58804,-1.24250,-1.15509,2.15971,-0.55296
-7.57895,0.79616,-1.58765,-1.23795,-1.15732,2.15603,-0.54804
-7.63158,0.80078,-1.58756,-1.23379,-1.15936,2.15295,-0.54396
-7.68421,0.81099,-1.58678,-1.22572,-1.16336,2.14611,-0.53499
-7.73684,0.81677,-1.58588,-1.22203,-1.16524,2.14223,-0.52995
-7.78947,0.82106,-1.58513,-1.21949,-1.16655,2.13934,-0.52622
-7.84211,0.82537,-1.58430,-1.21708,-1.16780,2.13642,-0.52248
-7.89474,0.82875,-1.58339,-1.21567,-1.16857,2.13413,-0.51956
-7.94737,0.83269,-1.58223,-1.21424,-1.16938,2.13146,-0.51617
-8.00000,0.83444,-1.58166,-1.21371,-1.16969,2.13027,-0.51466
-8.05263,0.84322,-1.57836,-1.21190,-1.17088,2.12430,-0.50715
-8.10526,0.84802,-1.57643,-1.21118,-1.17141,2.12101,-0.50305
-8.15789,0.85359,-1.57394,-1.21084,-1.17181,2.11719,-0.49832
-8.21053,0.85734,-1.57218,-1.21078,-1.17200,2.11462,-0.49515
-8.26316,0.86152,-1.56991,-1.21128,-1.17197,2.11174,-0.49162
-8.31579,0.86465,-1.56797,-1.21211,-1.17174,2.10958,-0.48899
-8.36842,0.86761,-1.56582,-1.21348,-1.17127,2.10753,-0.48649
-8.42105,0.87215,-1.56184,-1.21682,-1.16999,2.10439,-0.48269
-8.47368,0.87368,-1.56040,-1.21812,-1.16948,2.10333,-0.48140
-8.52632,0.87516,-1.55885,-1.21968,-1.16886,2.10230,-0.48016
-8.57895,0.87822,-1.55483,-1.22436,-1.16692,2.10017,-0.47760
-8.63158,0.88072,-1.55082,-1.22949,-1.16476,2.09843,-0.47550
-8.68421,0.88477,-1.54120,-1.24340,-1.15878,2.09559,-0.47208
-8.73684,0.88748,-1.53291,-1.25605,-1.15330,2.09369,-0.46979
-8.78947,0.89083,-1.51856,-1.27893,-1.14336,2.09132,-0.46692
-8.84211,0.89201,-1.51349,-1.28702,-1.13985,2.09049,-0.46592
-8.89474,0.89325,-1.50704,-1.29750,-1.13530,2.08960,-0.46485
-8.94737,0.89454,-1.50064,-1.30781,-1.13084,2.08868,-0.46374
-9.00000,0.89592,-1.49353,-1.31932,-1.12588,2.08771,-0.46257
-9.05263,0.89696,-1.48716,-1.32975,-1.12138,2.08696,-0.46166
-9.10526,0.89783,-1.48157,-1.33892,-1.11743,2.08634,-0.46091
-9.15789,0.89866,-1.47601,-1.34805,-1.11351,2.08574,-0.46019
-9.21053,0.89923,-1.47181,-1.35498,-1.11053,2.08533,-0.45969
-9.26316,0.89954,-1.46899,-1.35969,-1.10851,2.08511,-0.45941
-9.31579,0.89979,-1.46619,-1.36440,-1.10649,2.08493,-0.45919
-9.36842,0.89997,-1.46338,-1.36918,-1.10444,2.08479,-0.45902
-9.42105,0.90024,-1.46007,-1.37477,-1.10204,2.08460,-0.45877
-9.47368,0.90040,-1.45807,-1.37814,-1.10060,2.08448,-0.45863
-9.52632,0.90056,-1.45529,-1.38288,-1.09857,2.08436,-0.45848
-9.57895,0.90080,-1.45200,-1.38842,-1.09620,2.08418,-0.45825
-9.63158,0.90146,-1.44752,-1.39571,-1.09312,2.08371,-0.45768
-9.68421,0.90192,-1.44483,-1.40003,-1.09130,2.08338,-0.45729
-9.73684,0.90234,-1.44285,-1.40314,-1.08999,2.08309,-0.45694
-9.78947,0.90263,-1.44150,-1.40526,-1.08911,2.08287,-0.45669
-9.84211,0.90295,-1.44010,-1.40744,-1.08819,2.08265,-0.45642
-9.89474,0.90327,-1.43875,-1.40955,-1.08731,2.08242,-0.45615
-9.94737,0.90361,-1.43734,-1.41172,-1.08641,2.08218,-0.45586
-10.00000,0.90362,-1.43730,-1.41179,-1.08638,2.08217,-0.45585
+0.00000,1.11039,-1.23013,-1.62170,-1.02108,1.93437,-0.29909
+0.41667,1.11035,-1.23011,-1.62174,-1.02106,1.93439,-0.29911
+0.83333,1.11011,-1.23000,-1.62196,-1.02098,1.93454,-0.29927
+1.25000,1.10978,-1.22987,-1.62223,-1.02087,1.93475,-0.29950
+1.66667,1.10939,-1.22974,-1.62252,-1.02075,1.93499,-0.29976
+2.08333,1.10786,-1.22945,-1.62340,-1.02036,1.93598,-0.30082
+2.50000,1.10587,-1.22930,-1.62426,-1.01994,1.93730,-0.30219
+2.91667,1.10163,-1.22958,-1.62535,-1.01929,1.94016,-0.30513
+3.33333,1.07429,-1.24078,-1.62039,-1.01907,1.95955,-0.32456
+3.75000,1.06133,-1.24717,-1.61681,-1.01942,1.96883,-0.33391
+4.16667,1.04725,-1.25502,-1.61189,-1.02018,1.97898,-0.34418
+4.58333,1.01307,-1.27717,-1.59639,-1.02341,2.00376,-0.36958
+5.00000,1.00018,-1.28613,-1.58982,-1.02499,2.01307,-0.37931
+5.41667,0.99126,-1.29307,-1.58416,-1.02654,2.01951,-0.38611
+5.83333,0.98462,-1.29829,-1.57992,-1.02772,2.02430,-0.39120
+6.25000,0.94619,-1.32935,-1.55485,-1.03495,2.05185,-0.42114
+6.66667,0.93695,-1.33717,-1.54837,-1.03696,2.05844,-0.42847
+7.08333,0.91212,-1.36116,-1.52643,-1.04427,2.07601,-0.44844
+7.50000,0.90656,-1.36660,-1.52144,-1.04599,2.07993,-0.45297
+7.91667,0.89635,-1.37651,-1.51249,-1.04907,2.08710,-0.46132
+8.33333,0.88664,-1.38707,-1.50215,-1.05277,2.09389,-0.46934
+8.75000,0.88579,-1.38800,-1.50123,-1.05311,2.09448,-0.47005
+9.16667,0.88494,-1.38893,-1.50032,-1.05344,2.09508,-0.47076
+9.58333,0.88490,-1.38897,-1.50028,-1.05345,2.09510,-0.47079
+10.00000,0.88486,-1.38901,-1.50024,-1.05347,2.09513,-0.47082
diff --git a/python/examples/path_in_pixels.csv b/python/examples/path_in_pixels.csv
index 2bbbb96bcfc2ce71d503d049039c7be9dbd00346..a547354a96600b510c8cddbd40dd12f16f8a1d77 100644
--- a/python/examples/path_in_pixels.csv
+++ b/python/examples/path_in_pixels.csv
@@ -1,191 +1,25 @@
-0.47276,0.57909
-0.47276,0.57909
-0.47276,0.58042
-0.47276,0.58175
-0.47276,0.58308
-0.47572,0.58708
-0.47572,0.58974
-0.47572,0.59507
-0.47276,0.60040
-0.46239,0.61638
-0.45498,0.62304
-0.44906,0.62970
-0.44313,0.63636
-0.43869,0.64169
-0.43276,0.64835
-0.42980,0.65235
-0.42387,0.65901
-0.41943,0.66300
-0.41498,0.66966
-0.40610,0.67499
-0.39573,0.68032
-0.38684,0.68298
-0.37795,0.68565
-0.36758,0.68831
-0.36313,0.68964
-0.35721,0.69231
-0.34832,0.69497
-0.33647,0.69764
-0.32462,0.69764
-0.31573,0.69764
-0.30684,0.69630
-0.30091,0.69364
-0.29351,0.69098
-0.28610,0.68698
-0.27721,0.68298
-0.26832,0.67899
-0.26092,0.67632
-0.25499,0.67366
-0.24462,0.67100
-0.24018,0.66966
-0.23573,0.66567
-0.22981,0.66034
-0.21795,0.64569
-0.21055,0.63237
-0.20462,0.62171
-0.20018,0.61505
-0.19425,0.60706
-0.18684,0.59374
-0.17944,0.58042
-0.17351,0.56976
-0.17055,0.56044
-0.16907,0.55378
-0.16759,0.54978
-0.16759,0.54579
-0.16759,0.54446
-0.16759,0.53780
-0.16907,0.53114
-0.17647,0.52048
-0.18536,0.50982
-0.19573,0.49784
-0.21055,0.48452
-0.22832,0.47120
-0.24758,0.46054
-0.25943,0.45255
-0.27869,0.43523
-0.29647,0.42058
-0.31277,0.40859
-0.32017,0.40193
-0.33202,0.39394
-0.34239,0.38328
-0.35425,0.36863
-0.36313,0.35531
-0.37054,0.34332
-0.38387,0.32601
-0.39276,0.31002
-0.39573,0.29937
-0.40017,0.28871
-0.40017,0.28205
-0.40017,0.27806
-0.40165,0.27406
-0.40313,0.26607
-0.40313,0.25541
-0.40313,0.24342
-0.40313,0.23810
-0.40313,0.23144
-0.40313,0.22877
-0.40313,0.22744
-0.40313,0.22744
-0.40313,0.22611
-0.40313,0.22344
-0.40461,0.22344
-0.40461,0.22478
-0.40461,0.22478
-0.40461,0.22611
-0.40610,0.23010
-0.40758,0.23676
-0.41202,0.24476
-0.42091,0.25541
-0.43128,0.26740
-0.44165,0.27806
-0.45350,0.28738
-0.46239,0.29404
-0.46832,0.30070
-0.48757,0.31535
-0.50239,0.32334
-0.50980,0.32734
-0.52461,0.33400
-0.53498,0.33800
-0.54831,0.34466
-0.58387,0.36330
-0.59720,0.36996
-0.60461,0.37529
-0.61350,0.38195
-0.62535,0.39128
-0.63868,0.40460
-0.65201,0.41658
-0.66386,0.42724
-0.67275,0.43523
-0.68312,0.44589
-0.69497,0.46054
-0.70534,0.47652
-0.71275,0.49251
-0.71720,0.50183
-0.71720,0.50716
-0.71868,0.51116
-0.71868,0.51249
-0.71868,0.51382
-0.71868,0.51515
-0.72312,0.52847
-0.72460,0.53513
-0.72608,0.54312
-0.72757,0.55245
-0.72905,0.56044
-0.72905,0.57110
-0.72905,0.58175
-0.72460,0.59241
-0.71720,0.61239
-0.71127,0.62038
-0.70831,0.62571
-0.70090,0.63237
-0.69497,0.63770
-0.68905,0.64302
-0.68460,0.64835
-0.67720,0.65768
-0.66979,0.66567
-0.66535,0.67366
-0.65201,0.68831
-0.64312,0.69497
-0.63720,0.70030
-0.63127,0.70563
-0.62535,0.70829
-0.61942,0.71229
-0.61646,0.71362
-0.60313,0.72161
-0.59572,0.72561
-0.58683,0.72960
-0.58090,0.73227
-0.57350,0.73360
-0.56757,0.73360
-0.56165,0.73227
-0.55276,0.72960
-0.54979,0.72827
-0.54683,0.72561
-0.54091,0.71895
-0.53646,0.71229
-0.52905,0.69764
-0.52313,0.68698
-0.51276,0.66966
-0.50831,0.66434
-0.50387,0.65634
-0.49794,0.64968
-0.49202,0.64169
-0.48757,0.63370
-0.48313,0.62704
-0.47869,0.62038
-0.47572,0.61505
-0.47424,0.61106
-0.47276,0.60706
-0.47128,0.60306
-0.46832,0.59907
-0.46683,0.59640
-0.46535,0.59241
-0.46239,0.58841
-0.45646,0.58442
-0.45350,0.58175
-0.45054,0.58042
-0.44906,0.57909
-0.44758,0.57776
-0.44609,0.57642
-0.44461,0.57509
-0.44461,0.57509
+0.16462,0.49814
+0.16462,0.49814
+0.16610,0.49814
+0.16759,0.49814
+0.16907,0.49814
+0.17351,0.49814
+0.17796,0.49814
+0.18536,0.49814
+0.22092,0.50270
+0.23573,0.50270
+0.25203,0.50270
+0.29351,0.50270
+0.30980,0.50270
+0.32165,0.50573
+0.33054,0.50573
+0.38387,0.50573
+0.39721,0.50573
+0.43572,0.51181
+0.44461,0.51181
+0.46091,0.51181
+0.47720,0.51637
+0.47869,0.51637
+0.48017,0.51637
+0.48017,0.51637
+0.48017,0.51637
diff --git a/python/examples/point_impedance_control.py b/python/examples/point_impedance_control.py
index 5430e0b4bf296249b80eedb154825fa4f94eacaf..84801cf8cf749aca9d86a7f575986992ce1da7c7 100644
--- a/python/examples/point_impedance_control.py
+++ b/python/examples/point_impedance_control.py
@@ -191,9 +191,21 @@ if __name__ == "__main__":
     #plotFromDict(log_dict, args)
     # plotting is now initiated in stophandler because then we get the plot 
     # even if we end sooner
-    loop_manager.stopHandler(None, None)
+    #loop_manager.stopHandler(None, None)
     # TODO: add some math to analyze path precision
 
+    if not args.pinocchio_only:
+        robot.stopRobot()
+
+    if args.save_log:
+        robot.log_manager.plotAllControlLoops()
+
+    if args.visualize_manipulator:
+        robot.killManipulatorVisualizer()
+    
+    if args.save_log:
+        robot.log_manager.saveLog()
     
 
 
+    #loop_manager.stopHandler(None, None)
diff --git a/python/ur_simple_control/clik/clik.py b/python/ur_simple_control/clik/clik.py
index f73351a625cbabad575d484e6e0b8b65588cbb54..7612509c01d7059305d4df9f10b48707790071a7 100644
--- a/python/ur_simple_control/clik/clik.py
+++ b/python/ur_simple_control/clik/clik.py
@@ -391,7 +391,7 @@ def clikCartesianPathIntoJointPath(args, robot, path, \
         new_q = sim_robot.q.copy() 
         if args.viz_path:
             robot.updateViz(sim_robot.q, sim_robot.T_w_e)
-            time.sleep(0.05)
+            time.sleep(0.02)
         qs.append(new_q[:6])
         # plot this on the real robot