-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathget_plan.py
executable file
·195 lines (147 loc) · 29.4 KB
/
get_plan.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
import numpy as np
import pdb
import time
import genpy
from genpy import Duration, Time
import actionlib
import rospy
import moveit_commander
import moveit_msgs.msg
from moveit_msgs.msg import RobotTrajectory
from std_msgs.msg import Header
from trajectory_msgs.msg import JointTrajectoryPoint, JointTrajectory, MultiDOFJointTrajectory
def get_trajectory():
header = Header(0, Time(0, 0), '/world')
joint_names = ['l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint']
points = [] # Gonna store all the point and stuff
aPoint = JointTrajectoryPoint([0.40025003616336186, -0.2491647793511671, 1.000300860415198, -0.4990701008914131, 0.5004342554004451, -0.49874137392607665, 1.249717970864447], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [-0.01982512213691088, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [], Duration(0, 0)) # New one
points.append(aPoint)
aPoint = JointTrajectoryPoint([0.39860735560488875, -0.23881920096853956, 0.9988977702074787, -0.5000248247212695, 0.6610596752915067, -0.561025510929823, 1.2337849910050436], [-0.006901816253753126, 0.043467538875644765, -0.005895163701285709, -0.004011326738336254, 0.6748768822104484, -0.26169036146809727, -0.0669433255279816], [-0.019932847876033846, 0.12553678743369312, -0.01702557659342866, -0.011584945573253102, 1.949083796723467, -0.7557770265082578, -0.19333622846587017], [], Duration(0, 407083613))
points.append(aPoint)
aPoint = JointTrajectoryPoint([0.39696467504641564, -0.22847362258591203, 0.9974946799997595, -0.5009795485511259, 0.8216850951825683, -0.6233096479335695, 1.2178520111456401], [-0.011233030116056465, 0.07074546109445691, -0.009594655806667787, -0.0065286226698035976, 1.0983938232748507, -0.42591335430791716, -0.10895340647701067], [-0.01969053578823422, 0.12401070941174995, -0.01681860651890874, -0.011444114099173744, 1.9253899137915313, -0.746589482894772, -0.19098595190440415], [], Duration(0, 575246466))
points.append(aPoint)
aPoint = JointTrajectoryPoint([0.3953219944879426, -0.2181280442032845, 0.9960915897920403, -0.5019342723809822, 0.9823105150736299, -0.6855937849373159, 1.2019190312862367], [-0.013793342147335186, 0.08687026921186665, -0.011781537925204593, -0.008016672732563165, 1.3487475472262345, -0.522990552005277, -0.13378684096172277], [-0.018284302765078284, 0.11515427418438658, -0.015617477197451024, -0.010626813273034514, 1.787884926199727, -0.6932705282008255, -0.17734636609442725], [], Duration(0, 704615138))
points.append(aPoint)
aPoint = JointTrajectoryPoint([0.3936793139294695, -0.20778246582065696, 0.994688499584321, -0.5028889962108386, 1.1429359349646915, -0.7478779219410623, 1.1859860514268332], [-0.015114591421370924, 0.09519147801720479, -0.012910078656263948, -0.0087845811129213, 1.4779426109457332, -0.5730872493671916, -0.14660213725525592], [-0.004150204438956278, 0.026137927490333626, -0.0035448835005149975, -0.0024120934872026195, 0.4058173862243325, -0.15735981078985886, -0.04025440210953797], [], Duration(0, 814943492))
points.append(aPoint)
aPoint = JointTrajectoryPoint([0.39203663337099637, -0.19743688743802942, 0.9932854093766017, -0.503843720040695, 1.303561354855753, -0.8101620589448086, 1.1700530715674298], [-0.014211294207741594, 0.08950252523925545, -0.012138530305876739, -0.008259585933032564, 1.3896159466547369, -0.538837688721589, -0.13784071602970394], [0.01940915582482035, -0.12223858247455997, 0.01657826674665496, 0.011280576426000406, -1.897875876116794, 0.7359206355029813, 0.18825674124509817], [], Duration(0, 922027105))
points.append(aPoint)
aPoint = JointTrajectoryPoint([0.3903939528125233, -0.1870913090554019, 0.9918823191688825, -0.5047984438705513, 1.4641867747468147, -0.8724461959485551, 1.1541200917080263], [-0.011769892598327255, 0.07412661323774834, -0.010053215133909207, -0.006840646454669105, 1.1508895816215987, -0.44626911746920195, -0.11416063868851307], [0.0185747694431182, -0.11698362911877871, 0.015865578357188843, 0.010795632133072684, -1.8162874855926334, 0.7042839089078509, 0.18016371223471775], [], Duration(1, 47591063))
points.append(aPoint)
aPoint = JointTrajectoryPoint([0.3887512722540502, -0.17674573067277435, 0.9904792289611632, -0.5057531677004077, 1.6248121946378764, -0.9347303329523016, 1.1381871118486229], [-0.008423519268768866, 0.053051202440494646, -0.007194921337310815, -0.004895738575382803, 0.8236728148558028, -0.3193874947161778, -0.08170289844992303], [0.019642741687478316, -0.12370970285689857, 0.016777783344606525, 0.011416336229230535, -1.9207164868960274, 0.7447773141780524, 0.1905223788494603], [], Duration(1, 204674676))
points.append(aPoint)
aPoint = JointTrajectoryPoint([0.3871085916955771, -0.16640015229014682, 0.989076138753444, -0.5067078915302641, 1.785437614528938, -0.9970144699560479, 1.1222541319892194], [-0.11754163541600081, 0.03055225654939165, -0.09676920691601898, 0.06911725401836821, 0.4626785254269351, -0.29284985224781485, -0.08036130649232114], [-0.9907849101662483, -0.08637298977250495, -0.813931154377793, 0.649199350903096, -1.4451016780212123, -0.4508410410525392, -0.16388389098149916], [], Duration(1, 461758289))
points.append(aPoint)
aPoint = JointTrajectoryPoint([0.34327730534898854, -0.1624016693648836, 0.9530286984145642, -0.4795021833065058, 1.8430425245600899, -1.0628356490400621, 1.1033283861062229], [-0.30594392438773, 0.02790955182285173, -0.25161240476046004, 0.18989680279762017, 0.4020843034259805, -0.4594341511576059, -0.13210237367034308], [-1.0096649218684268, 0.09210607962552797, -0.8303620328529243, 0.6266904660500351, 1.3269438757954137, -1.516207740554285, -0.43595941006974603], [], Duration(1, 653417728))
points.append(aPoint)
aPoint = JointTrajectoryPoint([0.2994460190024, -0.15840318643962034, 0.9169812580756843, -0.4522964750827475, 1.9006474345912419, -1.1286568281240763, 1.0844026402232265], [-0.43315407529775407, 0.0395142218823121, -0.35623174650574013, 0.26885506611191895, 0.5692692051027621, -0.6504648696755172, -0.1870299651360256], [-0.974300182202657, 0.08887995236583557, -0.8012775945564563, 0.6047398716470238, 1.2804660555778702, -1.4631007236989144, -0.4206894024583155], [], Duration(1, 767801715))
points.append(aPoint)
aPoint = JointTrajectoryPoint([0.25561473265581147, -0.15440470351435712, 0.8809338177368046, -0.4250907668589893, 1.958252344622394, -1.1944780072080903, 1.06547689434023], [-0.5219970681154187, 0.047618870853864945, -0.42929742059521503, 0.3239991593336081, 0.6860303826710337, -0.7838798576448679, -0.22539114605726163], [-0.9209987631757783, 0.08401756224144352, -0.7574417895299855, 0.5716561322721179, 1.2104151010313395, -1.3830583033269161, -0.3976745836886412], [], Duration(1, 858528325))
points.append(aPoint)
aPoint = JointTrajectoryPoint([0.21178344630922288, -0.15040622058909386, 0.8448863773979247, -0.397885058635231, 2.015857254653546, -1.2602991862921045, 1.0465511484572336], [-0.5933182827828264, 0.054125106075931566, -0.48795294829957014, 0.3682676332511333, 0.7797637064375691, -0.8909824967504685, -0.25618666444996013], [-0.8755644240050264, 0.07987284177947339, -0.720075976953931, 0.543455422736822, 1.1507033919212508, -1.3148298294583989, -0.37805666167045965], [], Duration(1, 936675643))
points.append(aPoint)
aPoint = JointTrajectoryPoint([0.16795215996263435, -0.14640773766383064, 0.8088389370590449, -0.3706793504114727, 2.073462164684698, -1.3261203653761187, 1.027625402574237], [-0.6135479295077135, 0.05597054351926722, -0.5045900822779287, 0.38082400364635927, 0.8063503543934292, -0.9213612355663048, -0.2649215473413876], [0.3416496453884437, -0.03116678490122105, 0.2809772707001135, -0.21205904142743726, -0.4490102555775989, 0.5130532176328273, 0.14751961232689062], [], Duration(2, 6720938))
points.append(aPoint)
aPoint = JointTrajectoryPoint([0.12412087361604579, -0.14240925473856741, 0.772791496720165, -0.3434736421877145, 2.13106707471585, -1.3919415444601329, 1.0086996566912405], [-0.5660117678807588, 0.051634085558737455, -0.4654957026004225, 0.3513187107130392, 0.7438763422895095, -0.8499764675566854, -0.24439608732885304], [0.9088500067953361, -0.08290929919466379, 0.7474504886278738, -0.5641155020757922, -1.1944486971994768, 1.3648145889389067, 0.3924289179732993], [], Duration(2, 79610353))
points.append(aPoint)
aPoint = JointTrajectoryPoint([0.08028958726945729, -0.13841077181330416, 0.7367440563812853, -0.31626793396395614, 2.188671984747002, -1.457762723544147, 0.9897739108082442], [-0.48869567780143713, 0.044580971406764966, -0.40190990860086506, 0.3033292683985416, 0.6422643024839187, -0.7338713601011074, -0.21101206428335997], [0.9293834728632471, -0.08478245237616619, 0.7643374877266327, -0.576860450564202, -1.2214346372449594, 1.395649571434903, 0.40129498587336304], [], Duration(2, 162204299))
points.append(aPoint)
aPoint = JointTrajectoryPoint([0.03645830092286867, -0.13441228888804094, 0.7006966160424053, -0.2890622257401979, 2.246276894778154, -1.523583902628161, 0.9708481649252476], [-0.3941555509130418, 0.03595660478137405, -0.32415883470602375, 0.2446490123087505, 0.5180157130020461, -0.5919010201630739, -0.17019093932052295], [0.928349398171504, -0.0846881195298513, 0.7634870507702395, -0.5762186091606837, -1.2200756130285653, 1.3940967076896458, 0.4008484866608147], [], Duration(2, 260325107))
points.append(aPoint)
aPoint = JointTrajectoryPoint([-0.0073729854237198356, -0.13041380596277768, 0.6646491757035256, -0.26185651751643957, 2.303881804809306, -1.5894050817121754, 0.9519224190422512], [-0.27094347865152124, 0.024716657059366768, -0.22282756669904194, 0.1681723225514482, 0.35608525352948006, -0.40687419230523253, -0.11698966316129737], [0.8141596001415495, -0.0742712233874139, 0.6695758226295558, -0.5053419686083862, -1.0700026037634716, 1.222618574781072, 0.35154290427710144], [], Duration(2, 388635427))
points.append(aPoint)
aPoint = JointTrajectoryPoint([-0.0073729854237198356, -0.13041380596277768, 0.6646491757035256, -0.26185651751643957, 2.303881804809306, -1.5894050817121754, 0.9519224190422512], [-0.27094347865152124, 0.024716657059366768, -0.22282756669904194, 0.1681723225514482, 0.35608525352948006, -0.40687419230523253, -0.11698966316129737], [0.8141596001415495, -0.0742712233874139, 0.6695758226295558, -0.5053419686083862, -1.0700026037634716, 1.222618574781072, 0.35154290427710144], [], Duration(2, 388635427))
points.append(aPoint)
aPoint = JointTrajectoryPoint([-0.051204271770308396, -0.12641532303751446, 0.6286017353646457, -0.23465080929268134, 2.361486714840458, -1.6552262607961894, 0.9329966731592547], [-0.08642767604269791, -0.01062470742324065, -0.09576908542918819, 0.034310009402589076, 0.11678617278453993, -0.17488340737610686, -0.34419020333569095], [0.8766859239148772, -0.22249433813249525, 0.5308872109324757, -0.693030108101226, -1.1275427079196372, 0.9692793945024033, -1.984368417750443], [], Duration(2, 607481965))
points.append(aPoint)
aPoint = JointTrajectoryPoint([-0.04295849963299261, -0.13829645622883224, 0.6205379348248243, -0.2513943945618461, 2.3525734224049084, -1.6699581942590886, 0.7520442235256871], [0.037875950784604574, -0.054574539354924956, -0.03704008639784766, -0.0769096212036678, -0.04094212403589139, -0.06766934345392603, -0.8311830550905466], [0.08868035482578092, -0.1277773736155417, -0.08672331483422072, -0.1800713211569061, -0.09585929888524618, -0.15843671944892385, -1.946079417775751], [], Duration(2, 908116931))
points.append(aPoint)
aPoint = JointTrajectoryPoint([-0.03471272749567682, -0.15017758942015003, 0.612474134285003, -0.2681379798310109, 2.3436601299693582, -1.6846901277219877, 0.5710917738921195], [0.054795269212941436, -0.07895317514619063, -0.05358602130895335, -0.11126541543171778, -0.05923111267765656, -0.09789747360026824, -1.202475405249345], [0.08480632083953625, -0.12219537195314249, -0.08293477486133599, -0.17220483912172943, -0.09167165008173128, -0.15151535296342872, -1.8610642211270032], [], Duration(3, 78751897))
points.append(aPoint)
aPoint = JointTrajectoryPoint([-0.026466955358361032, -0.1620587226114678, 0.6044103337451815, -0.28488156510017565, 2.3347468375338085, -1.699422061184887, 0.390139324258552], [0.06480978831001163, -0.09338285295657212, -0.06337953526449347, -0.13160055829513168, -0.0700563375113185, -0.11578945831002227, -1.422242788138354], [0.05553243763958587, -0.08001534326283989, -0.05430692155427501, -0.11276228463980913, -0.060027957127362905, -0.09921450201571722, -1.2186524751916843], [], Duration(3, 213340511))
points.append(aPoint)
aPoint = JointTrajectoryPoint([-0.018221183221045247, -0.17393985580278562, 0.5963465332053601, -0.30162515036934046, 2.3258335450982583, -1.714153994647786, 0.2091868746249843], [0.06835308519459375, -0.09848830354640681, -0.06684463699842741, -0.1387954567877159, -0.07388647504026233, -0.12211992840714456, -1.5], [9.20316105313894e-16, 9.20316105313894e-16, -8.052765921496572e-16, 2.3007902632847348e-15, 2.9795233909537315e-14, -1.6450650382485854e-14, 0.0], [], Duration(3, 333975477))
points.append(aPoint)
aPoint = JointTrajectoryPoint([-0.0017296389464136683, -0.1977021221854212, 0.5802189321257174, -0.33511232090766996, 2.308006960227159, -1.7436178615735844, -0.15271802464215067], [0.06835308519459378, -0.09848830354640675, -0.06684463699842746, -0.138795456787716, -0.07388647504026238, -0.12211992840714464, -1.5], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [], Duration(3, 454610444))
points.append(aPoint)
aPoint = JointTrajectoryPoint([-0.0017296389464136683, -0.1977021221854212, 0.5802189321257174, -0.33511232090766996, 2.308006960227159, -1.7436178615735844, -0.15271802464215067], [0.06835308519459378, -0.09848830354640675, -0.06684463699842746, -0.138795456787716, -0.07388647504026238, -0.12211992840714464, -1.5], [-4.601580526569473e-16, 0.0, 0.0, -3.681264421255578e-15, -3.060051050168699e-14, 1.518521573767926e-14, 0.0], [], Duration(3, 575245410))
points.append(aPoint)
aPoint = JointTrajectoryPoint([0.006516133190902114, -0.20958325537673897, 0.572155131585896, -0.35185590617683477, 2.2990936677916087, -1.7583497950364835, -0.33367047427571817], [0.06420619234998261, -0.09251314616921227, -0.0627892597454442, -0.1303749168080074, -0.06940387862510844, -0.1147110710621645, -1.408996949454322], [-0.06431065730173424, 0.09266366718591125, 0.06289141931525136, 0.13058703979639127, 0.06951680033200822, 0.11489770861322439, 1.4112894198992212], [], Duration(3, 695880377))
points.append(aPoint)
aPoint = JointTrajectoryPoint([0.014761905328217903, -0.22146438856805678, 0.5640913310460746, -0.36859949144599957, 2.290180375356059, -1.7730817284993827, -0.5146229239092861], [0.05348107171600197, -0.07705957982960739, -0.05230082614362801, -0.10859685056185563, -0.057810526901913475, -0.09554952246123474, -1.1736355037321409], [-0.08404016802003961, 0.12109144092743075, 0.08218552986443807, 0.17064911518844725, 0.09084347486456967, 0.15014654090177082, 1.8442510922686415], [], Duration(3, 833174221))
points.append(aPoint)
aPoint = JointTrajectoryPoint([0.0230076774655337, -0.23334552175937456, 0.5560275305062532, -0.3853430767151643, 2.281267082920509, -1.7878136619622818, -0.6955753735428536], [0.03435608366934175, -0.04950284815175394, -0.033597897374007445, -0.06976229840610643, -0.037137312987253125, -0.06138073308769549, -0.7539400066185828], [-0.09060830028335123, 0.13055530349106256, 0.08860871348004853, 0.18398614182203563, 0.09794331738308093, 0.16188119544559812, 1.9883879423744895], [], Duration(4, 8979598))
points.append(aPoint)
aPoint = JointTrajectoryPoint([0.03125344960284948, -0.24522665495069235, 0.5479637299664318, -0.4020866619843291, 2.272353790484959, -1.802545595425181, -0.8765278231764211], [-0.08288302051500455, 0.06948887019629277, 0.13510466168670668, -0.008863346886277447, -0.061644127130387374, -0.0005685877173472777, -0.139957303212649], [-0.6083259705717642, 0.5863679906208645, 0.9089687207279087, 0.2058228565760475, -0.22120554785354596, 0.2231043057688005, 1.9677361922181262], [], Duration(4, 387064327))
points.append(aPoint)
aPoint = JointTrajectoryPoint([-0.026953365120279307, -0.19234885223386677, 0.6384311560561851, -0.3938452264585385, 2.24141156409238, -1.7908072952748726, -0.8148725359874339], [-0.3000205385898108, 0.27255274019047043, 0.4663042640176738, 0.04247956080335738, -0.15948825703016373, 0.06050375973975024, 0.3177953055383251], [-0.9963447765801335, 0.905126363374244, 1.5485600417051961, 0.14107130370734242, -0.5296480453130975, 0.2009282606567707, 1.0553733893788537], [], Duration(4, 697375944))
points.append(aPoint)
aPoint = JointTrajectoryPoint([-0.0851601798434081, -0.1394710495170412, 0.7288985821459383, -0.3856037909327478, 2.210469337699801, -1.7790689951245644, -0.7532172487984468], [-0.471852650648544, 0.4286530965675863, 0.733372801807677, 0.06680907066450707, -0.25083268359133115, 0.0951563501005378, 0.49980763979274917], [-0.9475867510673464, 0.8608322843013578, 1.4727783125323815, 0.1341677113094389, -0.5037287114507245, 0.1910954542933445, 1.003726686496075], [], Duration(4, 838495104))
points.append(aPoint)
aPoint = JointTrajectoryPoint([-0.14336699456653687, -0.08659324680021563, 0.8193660082356915, -0.37736235540695723, 2.179527111307222, -1.767330694974256, -0.6915619616094595], [-0.47157863114884707, 0.4284041643916124, 0.7329469094279977, 0.06677027256070592, -0.25068701725593834, 0.09510108985051759, 0.49951738596199824], [0.951246606831586, -0.864157069070875, -1.478466610928807, -0.13468590605103667, 0.50567425831083, -0.1918335205433505, -1.0076033710267074], [], Duration(4, 948063020))
points.append(aPoint)
aPoint = JointTrajectoryPoint([-0.20157380928966567, -0.033715444083390056, 0.9098334343254447, -0.3691209198811666, 2.1485848849146425, -1.7555923948239478, -0.6299066744204723], [-0.29084880396384616, 0.2642207059358949, 0.4520491767338889, 0.04118094551339351, -0.1546126442594053, 0.05865413827445176, 0.30808018996165476], [1.000272098520772, -0.9086941269732368, -1.5546640470366222, -0.14162736867528025, 0.5317357748200587, -0.20172026557828043, -1.0595333861642031], [], Duration(5, 89369933))
points.append(aPoint)
aPoint = JointTrajectoryPoint([-0.25978062401279445, 0.019162358633435517, 1.000300860415198, -0.360879484355376, 2.1176426585220636, -1.7438540946736394, -0.5682513872314852], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.9904416240782699, -0.8997636625481352, -1.5393851192292083, -0.1402354831771427, 0.5265099818060625, -0.19973779908918415, -1.0491205036204758], [], Duration(5, 432206764))
points.append(aPoint)
### ACTUALLY CREATE THE TRAJ ##
joint_trajectory = JointTrajectory(header, joint_names, points)
newHeader = Header(0, Time(0, 0), '')
multi_dof_joint_trajectory = MultiDOFJointTrajectory(newHeader, [], [])
plan = RobotTrajectory(joint_trajectory, multi_dof_joint_trajectory)
return plan
def get_simple_plan():
header = Header(0, Time(0, 0), '/world')
joint_names = ['l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint']
points = []
aPoint = JointTrajectoryPoint((0.40010140636150293, -0.24941262701698985, 1.0004124485664612, -0.4997714065764134, 0.5008749075576269, -0.4998209962755613, 1.250362580088737), (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0), (-0.7144440176269288, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0), Duration(0, 0))
points.append(aPoint)
aPoint = JointTrajectoryPoint((0.3701010413213245, -0.237216433004318, 1.0004124485664612, -0.4934401455238009, 0.5743228372936882, -0.5563811188230655, 1.1677162171739146), (-0.17755980716212977, 0.07218425022834517, 0.0, 0.03747212709210088, 0.4347080518162636, -0.33475607510612976, -0.48914979008268733), (-0.7240219594706212, 0.29434016137178437, 0.0, 0.1527972085370781, 1.7725755648421089, -1.365009082386119, -1.9945684507695596), Duration(0, 289797186))
points.append(aPoint)
aPoint = JointTrajectoryPoint((0.3401006762811461, -0.22502023899164614, 1.0004124485664612, -0.48710888447118833, 0.6477707670297495, -0.6129412413705697, 1.0850698542590922), (-0.2880742149704434, 0.11711220883886869, 0.0, 0.06079502882920425, 0.7052732415616743, -0.5431104881451616, -0.7935998807002602), (-0.6892898489023036, 0.28022034788306643, 0.0, 0.14546736242947506, 1.6875432122960485, -1.2995281314894562, -1.8988868611961462), Duration(0, 409036620))
points.append(aPoint)
aPoint = JointTrajectoryPoint((0.3101003112409677, -0.21282404497897428, 1.0004124485664612, -0.48077762341857583, 0.7212186967658107, -0.6695013639180738, 1.0024234913442698), (-0.35255601309591583, 0.14332630720637068, 0.0, 0.07440323314696709, 0.8631398065728579, -0.6646789557013306, -0.9712372555168975), (-0.6540669674290382, 0.26590101891630397, 0.0, 0.13803394429160473, 1.6013093374719958, -1.2331219231004833, -1.8018532737299), Duration(0, 501473212))
points.append(aPoint)
aPoint = JointTrajectoryPoint((0.28009994620078926, -0.2006278509663024, 1.0004124485664612, -0.4744463623659633, 0.794666626501872, -0.7260614864655781, 0.9197771284294473), (-0.406837754916044, 0.16539372717600817, 0.0, 0.08585882301707332, 0.9960342414844434, -0.7670171094313484, -1.120775053743913), (-0.7097026153852208, 0.28851884891888946, 0.0, 0.14977526179125936, 1.7375184521422526, -1.338012615670257, -1.9551208738351065), Duration(0, 580305103))
points.append(aPoint)
aPoint = JointTrajectoryPoint((0.2500995811606109, -0.18843165695363054, 1.0004124485664612, -0.46811510131335077, 0.8681145562379333, -0.7826216090130822, 0.8371307655146248), (-0.4534024449997426, 0.18432389664695084, 0.0, 0.0956858104006955, 1.1100355238311277, -0.8548061937972654, -1.2490535687059658), (-0.6120130152602035, 0.248804621623673, 0.0, 0.12915890063401106, 1.4983513994641284, -1.153836998230712, -1.6860011436543565), Duration(0, 649571737))
points.append(aPoint)
aPoint = JointTrajectoryPoint((0.22009921612043246, -0.17623546294095868, 1.0004124485664612, -0.46178384026073827, 0.9415624859739946, -0.8391817315605864, 0.7544844025998024), (-0.4907755786717509, 0.19951737807675612, 0.0, 0.10357301661684516, 1.2015337203457976, -0.9252663037873301, -1.3520107682128484), (-0.5583065814085438, 0.22697108439483424, 0.0, 0.11782472345103574, 1.3668654533888738, -1.0525834809427888, -1.5380482298474696), Duration(0, 712904979))
points.append(aPoint)
aPoint = JointTrajectoryPoint((0.19009885108025404, -0.16403926892828682, 1.0004124485664612, -0.45545257920812576, 1.015010415710056, -0.8957418541080906, 0.67183803968498), (-0.5261778478447592, 0.2139096343957905, 0.0, 0.11104429263928692, 1.2882067783313866, -0.9920107144037812, -1.4495385410303516), (-0.6417590184099651, 0.2608974086624451, 0.0, 0.1354364812888343, 1.5711765916358333, -1.2099175686236372, -1.7679467789254342), Duration(0, 771977037))
points.append(aPoint)
aPoint = JointTrajectoryPoint((0.1600984860400756, -0.15184307491561494, 1.0004124485664612, -0.4491213181555132, 1.0884583454461172, -0.9523019766555949, 0.5891916767701575), (-0.5444951958339217, 0.22135627478080783, 0.0, 0.11490997599866001, 1.3330519422571325, -1.0265446757614107, -1.5), (4.030024984049384e-15, -3.5262718610432106e-15, 0.0, -1.5364470251688274e-14, 3.224019987239507e-14, 4.433027482454322e-14, 0.0), Duration(0, 827074612))
points.append(aPoint)
aPoint = JointTrajectoryPoint((0.1300981209998972, -0.13964688090294308, 1.0004124485664612, -0.4427900571029007, 1.1619062751821785, -1.008862099203099, 0.5065453138553351), (-0.5444951958339216, 0.22135627478080772, 0.0, 0.1149099759986601, 1.3330519422571334, -1.0265446757614094, -1.5), (0.0, 0.0, 0.0, 1.813511242822224e-14, 0.0, 0.0, 0.0), Duration(0, 882172187))
points.append(aPoint)
aPoint = JointTrajectoryPoint((0.10009775595971881, -0.12745068689027123, 1.0004124485664612, -0.43645879605028814, 1.2353542049182398, -1.065422221750603, 0.42389895094051266), (-0.5444951958339221, 0.22135627478080772, 0.0, 0.1149099759986601, 1.3330519422571334, -1.0265446757614114, -1.5), (-1.813511242822224e-14, 0.0, 0.0, -1.813511242822224e-14, 0.0, -7.254044971288896e-14, 0.0), Duration(0, 937269763))
points.append(aPoint)
aPoint = JointTrajectoryPoint((0.07009739091954037, -0.11525449287759937, 1.0004124485664612, -0.43012753499767564, 1.308802134654301, -1.1219823442981074, 0.34125258802569025), (-0.5444951958339221, 0.22135627478080772, 0.0, 0.1149099759986596, 1.3330519422571334, -1.0265446757614114, -1.5), (1.813511242822224e-14, 0.0, 0.0, 0.0, 0.0, 7.254044971288896e-14, 0.0), Duration(0, 992367338))
points.append(aPoint)
aPoint = JointTrajectoryPoint((0.04009702587936198, -0.10305829886492751, 1.0004124485664612, -0.42379627394506314, 1.3822500643903624, -1.1785424668456115, 0.25860622511086784), (-0.5444951958339221, 0.22135627478080797, 0.0, 0.1149099759986601, 1.3330519422571334, -1.0265446757614114, -1.5), (-1.813511242822224e-14, 9.06755621411112e-15, 0.0, 1.813511242822224e-14, 0.0, -7.254044971288896e-14, 0.0), Duration(1, 47464913))
points.append(aPoint)
aPoint = JointTrajectoryPoint((0.010096660839183536, -0.09086210485225563, 1.0004124485664612, -0.4174650128924506, 1.4556979941264236, -1.2351025893931158, 0.17595986219604542), (-0.5444951958339221, 0.22135627478080772, 0.0, 0.1149099759986601, 1.3330519422571334, -1.0265446757614114, -1.5), (1.813511242822224e-14, -1.813511242822224e-14, 0.0, -1.813511242822224e-14, 0.0, 7.254044971288896e-14, 0.0), Duration(1, 102562489))
points.append(aPoint)
aPoint = JointTrajectoryPoint((-0.019903704200994854, -0.0786659108395838, 1.0004124485664612, -0.4111337518398381, 1.529145923862485, -1.2916627119406199, 0.09331349928122301), (-0.533872188347821, 0.21703765198655037, 0.0, 0.11266810215918183, 1.3070443284704405, -1.0065169659508066, -1.470735258362112), (0.3779342542440911, -0.15364344675879707, 0.0, -0.07975904363627499, -0.9252716929742283, 0.7125249211573006, 1.0411503824159534), Duration(1, 157660064))
points.append(aPoint)
aPoint = JointTrajectoryPoint((-0.04990406924117324, -0.06646971682691194, 1.0004124485664612, -0.4048024907872256, 1.6025938535985462, -1.3482228344881242, 0.010667136366400598), (-0.5056465694810834, 0.20556295414985992, 0.0, 0.1067113825933525, 1.2379413936059942, -0.9533027980583908, -1.392978046500466), (0.5926536863783151, -0.24093437968887688, 0.0, -0.12507331818225356, -1.4509552219327135, 1.1173385752105525, 1.6326691885792808), Duration(1, 214994822))
points.append(aPoint)
aPoint = JointTrajectoryPoint((-0.07990443428135174, -0.054273522814240055, 1.0004124485664612, -0.398471229734613, 1.6760417833346075, -1.4047829570356285, -0.07197922654842204), (-0.4693721826200346, 0.1908161515941057, 0.0, 0.09905605531872848, 1.1491331869784809, -0.8849141713383879, -1.2930477244188552), (0.5833357813624516, -0.23714632653641227, 0.0, -0.12310687247269494, -1.4281428050844136, 1.099771393981981, 1.6069998022729344), Duration(1, 276465442))
points.append(aPoint)
aPoint = JointTrajectoryPoint((-0.10990479932153008, -0.0420773288015682, 1.0004124485664612, -0.3921399686820005, 1.7494897130706688, -1.4613430795831324, -0.15462558946324423), (-0.4293568030989694, 0.17454850513460446, 0.0, 0.09061123094649409, 1.0511661529277676, -0.8094725969953693, -1.1828115465042477), (0.6094176473284412, -0.2477495141012329, 0.0, -0.12861117556862145, -1.4919973300641747, 1.148943913493855, 1.6788513066540751), Duration(1, 343029316))
points.append(aPoint)
aPoint = JointTrajectoryPoint((-0.13990516436170852, -0.029881134788896313, 1.0004124485664612, -0.38580870762938796, 1.82293764280673, -1.5179032021306367, -0.23727195237806686), (-0.38358943985440314, 0.1559424768135622, 0.0, 0.08095251099415271, 0.939116913683852, -0.7231867244627543, -1.0567295435919777), (0.6220400789731343, -0.2528809725032963, 0.0, -0.13127500681716633, -1.5229000031247977, 1.1727411666178345, 1.7136241524237752), Duration(1, 416557244))
points.append(aPoint)
aPoint = JointTrajectoryPoint((-0.16990552940188697, -0.017684940776224456, 1.0004124485664612, -0.37947744657677546, 1.8963855725427914, -1.574463324678141, -0.3199183152928893), (-0.34175135639103427, 0.13893383767353085, 0.0, 0.07212302415314824, 0.8366874728955987, -0.64430877998869, -0.9414720983927828), (0.39572292144968146, -0.16087515997881496, 0.0, -0.08351315448478334, -0.9688225223479209, 0.7460621529796324, 1.0901554076439481), Duration(1, 500085172))
points.append(aPoint)
aPoint = JointTrajectoryPoint((-0.1999058944420653, -0.005488746763552627, 1.0004124485664612, -0.37314618552416295, 1.9698335022788527, -1.631023447225645, -0.4025646782077117), (-0.28581742898323226, 0.11619474667771668, 0.0, 0.060318757916974414, 0.6997481001710263, -0.5388557368503288, -0.7873827845592509), (0.7206317437817781, -0.29296192053269826, 0.0, -0.15208173922455254, -1.7642755217136517, 1.358617459665321, 1.9852289312069493), Duration(1, 592582673))
points.append(aPoint)
aPoint = JointTrajectoryPoint((-0.22990625948224386, 0.006707447249119258, 1.0004124485664612, -0.3668149244715504, 2.043281432014914, -1.6875835697731494, -0.4852110411225341), (-0.1747157427808418, 0.07102803893117376, 0.0, 0.03687191726051188, 0.42774511517992775, -0.32939411935248397, -0.481314832851524), (0.6995037352471314, -0.28437264867400674, 0.0, -0.14762289556130062, -1.712549201575904, 1.3187845192892753, 1.927024537404225), Duration(1, 713895365))
points.append(aPoint)
aPoint = JointTrajectoryPoint((-0.2599066245224222, 0.018903641261791115, 1.0004124485664612, -0.3604836634189379, 2.1167293617509753, -1.7441436923206535, -0.5678574040373565), (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0), (0.6954105015201669, -0.282708606499642, 0.0, -0.1467590616966102, -1.7025280054081884, 1.311067486483095, 1.9157483119436316), Duration(2, 7631682))
points.append(aPoint)
joint_trajectory = JointTrajectory(header, joint_names, points)
newHeader = Header(0, Time(0, 0), '')
multi_dof_joint_trajectory = MultiDOFJointTrajectory(newHeader, [], [])
plan = RobotTrajectory(joint_trajectory, multi_dof_joint_trajectory)
return plan