-
Notifications
You must be signed in to change notification settings - Fork 2
/
arm_kinematics.yaml
190 lines (184 loc) · 12.1 KB
/
arm_kinematics.yaml
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
base_link_name: dh_base_link # fixed frame (default: base_link)
last_link: link6 # frame to get pose in relation to fixed frame (default: link6)
time_to_complete_movement: 3.0 # time in seconds to the joints complete the movement (default: 2.0)
plot_charts: true
write_poses_list_file: false
# this map each joint state passed in joints sets with ur5 joints
joints_remap:
- shoulder_pan_joint: 0
- shoulder_lift_joint: 1
- elbow_joint: 2
- wrist_1_joint: 3
- wrist_2_joint: 4
- wrist_3_joint: 5
set_to_pick: 1
datasets:
set_1:
shoulder_position: 'left'
elbow_position: 'up'
wrist_position: 'up'
joints_set_list:
- [0.00, -1.57, 0.00, 0.00, 0.00, 0.00] # 0
- [0.95, -0.47, 0.73, 0.00, -0.31, 1.32] # 1
- [1.09, -0.47, 0.73, 0.00, -0.20, 1.32] # 2
- [0.95, -0.47, 0.73, 0.00, -0.31, 1.32] # 3
- [0.00, -1.57, 0.00, 0.00, 0.00, 0.00] # 4
- [1.94, -0.45, 0.00, 0.00, 3.27, 1.57] # 5
- [1.94, -0.45, 0.45, 0.00, 3.27, 1.57] # 6
- [1.76, -0.45, 0.45, 0.00, 3.22, 1.57] # 7
- [1.94, -0.45, 0.45, 0.00, 3.27, 1.57] # 8
- [1.94, -0.45, 0.00, 0.00, 3.27, 1.57] # 9
- [0.00, -1.57, 0.00, 0.00, 0.00, 0.00] # 10
- [1.00, -1.25, 2.07, -0.75, 1.33, 1.57] # 11
- [1.33, -1.25, 2.07, -0.75, 1.33, 1.57] # 12
- [1.33, -1.20, 1.88, -0.60, 1.31, 1.57] # 13
- [1.33, -1.25, 2.07, -0.75, 1.33, 1.57] # 14
- [1.00, -1.25, 2.07, -0.75, 1.33, 1.57] # 15
- [0.00, -1.57, 0.00, 0.00, 0.00, 0.00] # 16
- [1.44, -0.96, 0.75, -1.49, -1.57, 0.00] # 17
- [1.44, -0.84, 0.75, -1.46, -1.57, 0.00] # 18
- [1.44, -0.96, 0.75, -1.49, -1.57, 0.00] # 19
- [0.00, -1.57, 0.00, 0.00, 0.00, 0.00] # 20
poses_set_list:
- [[-0.00010600655476245002, 0.999998533908079, 0.0017090770324088012, -0.09496206406691732], # 0
[0.000816550868354804, 0.00170916303186186, -0.9999982060015957, -0.19161254180082915],
[-0.9999996610035873, -0.00010461081625190438, -0.000816730853700709, 0.9063316422450596],
[0.0, 0.0, 0.0, 1.0]]
- [[-0.07375987346105783, -0.3153417840937208, 0.9461073090678452, -0.26011538845336646], # 1
[0.027021535536452895, -0.94897515474207, -0.3141910124423005, -0.6860809361616739],
[0.9969098844350287, 0.0023905829529289047, 0.0785173065564797, 0.09575253048626876],
[0.0, 0.0, 0.0, 1.0]]
- [[-0.05033092088318869, -0.2832138647525295, 0.9577351957691579, -0.16371901239929187], # 2
[0.010133501031128506, -0.9590460248910684, -0.2830689567887895, -0.7242216999518585],
[0.9986811856442993, -0.004541910674748489, 0.05113961759262853, 0.0935026556212737],
[0.0, 0.0, 0.0, 1.0]]
- [[-0.07375987346105645, -0.31534178409372016, 0.9461073090678446, -0.2601153884533656], # 3
[0.027021535536454033, -0.9489751547420693, -0.31419101244229986, -0.6860809361616746],
[0.9969098844350278, 0.0023905829529303757, 0.07851730655647876, 0.09575253048626882],
[0.0, 0.0, 0.0, 1.0]]
- [[0.000458631099084561, 0.9999985571599903, 0.0016356452710300506, -0.09497734141278762], # 4
[0.0005315057276296664, 0.0016354014472636513, -0.9999985214807909, -0.19160557782106502],
[-0.999999753579558, 0.0004594997758196051, -0.0005307549147887669, 0.9063348374973433],
[0.0, 0.0, 0.0, 1.0]]
- [[-0.15656995259080747, -0.202971589462558, -0.9665859422817793, 0.3026151012407853], # 5
[0.40445902652513255, 0.8796570012378352, -0.25023280367610024, -0.7059728621729371],
[0.9010542413240474, -0.43012334747639824, -0.05563416349369461, 0.35461705212197375],
[0.0, 0.0, 0.0, 1.0]]
- [[0.0003188154916986532, -0.23840734587207826, -0.9711651949029655, 0.3014686528955668], # 6
[-0.0011073880250771218, 0.9711645646146746, -0.23840755468011648, -0.7029505933448066],
[0.9999993360240024, 0.0011514647289772445, 4.561286827875575e-05, 0.1793513818578941],
[0.0, 0.0, 0.0, 1.0]]
- [[0.00015622553972493907, -0.11050194259559283, -0.9938758958120381, 0.17110141722147454], # 7
[-0.0011490854634128578, 0.9938752319477759, -0.11050204940792885, -0.7496640236001009],
[0.9999993275978633, 0.0011593115866235168, 2.8292518614860766e-05, 0.17935164472969234],
[0.0, 0.0, 0.0, 1.0]]
- [[0.00031880580486760325, -0.23840734928350477, -0.9711651940686878, 0.30146864185013217], # 8
[-0.0011073615612708504, 0.9711645638104263, -0.23840755807917985, -0.7029505984900541],
[0.9999993360563955, 0.0011514367189957714, 4.560976590350062e-05, 0.17935139654011745],
[0.0, 0.0, 0.0, 1.0]]
- [[-0.1568635885199231, -0.2028310751803568, -0.9665678297656266, 0.3025886188959229], # 9
[0.4052182849162508, 0.8792941932867293, -0.2502795701246362, -0.7059048249373385],
[0.9006619544347285, -0.4309307097358239, -0.05573838211138393, 0.354964896311215],
[0.0, 0.0, 0.0, 1.0]]
- [[0.0010683681130561618, 0.9999980866802359, 0.0016386657509261937, -0.09492973085401157], # 10
[0.000709338307322338, 0.0016379084400212074, -0.9999984070462856, -0.19160571031453275],
[-0.9999991777140327, 0.001069528779585327, -0.0007075870609818402, 0.9062319120184383],
[0.0, 0.0, 0.0, 1.0]]
- [[-0.037035231143762865, -0.9457490810651363, -0.32278021519073785, -0.14814750304167354], # 11
[-0.059080919814843924, 0.32450902364319956, -0.9440356658982594, -0.4690369223614448],
[0.9975659559988806, -0.015892427082996097, -0.06789399232223431, 0.10569669900006345],
[0.0, 0.0, 0.0, 1.0]]
- [[-0.01588460589044874, -0.999873689663892, 0.0005329292824656773, 0.011832276348997105], # 12
[-0.06791371603698138, 0.000547152950399532, -0.9976910482698025, -0.4917360771247945],
[0.9975647379843342, -0.015884122310151627, -0.06791382914166141, 0.10568820644838044],
[0.0, 0.0, 0.0, 1.0]]
- [[-0.018253549075266634, -0.9996184390428243, 0.020731239030644166, 4.420278578242793e-05], # 13
[-0.07755014516269088, -0.019256732394706377, -0.9968024645047412, -0.546481874033239],
[0.9968213395244927, -0.01980289330041296, -0.07716905134527749, 0.13794891140563734],
[0.0, 0.0, 0.0, 1.0]]
- [[-0.015872682323419873, -0.9998738768337792, 0.0005370114042506091, 0.011844896283290683], # 14
[-0.06788422147648071, 0.0005418004655169673, -0.9976930584737912, -0.49173688885801214],
[0.9975669353133112, -0.01587251957453717, -0.0678842595302035, 0.10569203423359805],
[0.0, 0.0, 0.0, 1.0]]
- [[-0.03700437759047534, -0.9457491858539444, -0.32278344674989656, -0.14813992457736178], # 15
[-0.05907337001590124, 0.32451003336017503, -0.9440357912725252, -0.46904157490691023],
[0.997567548085896, -0.015865550894307023, -0.06787688340454504, 0.10569593180668135],
[0.0, 0.0, 0.0, 1.0]]
- [[0.0013135889811695112, 0.9999977727465015, -0.0016519642924510691, -0.09555376159633609], # 16
[-0.0005750810261364037, -0.0016512100248101402, -0.9999984713924648, -0.19129245966387667],
[-0.9999989718823719, 0.0013145369865280032, 0.0005729107339909889, 0.906345645929318],
[0.0, 0.0, 0.0, 1.0]]
- [[-0.9914554927279243, 0.12946738462651808, -0.01594372157303977, 0.012836245950038763], # 17
[0.13044245425632572, 0.9831763176423612, -0.12786357790549255, -0.7398523232072589],
[-0.0008786735547168497, -0.12885078480621223, -0.9916636038433638, 0.4496456919444819],
[0.0, 0.0, 0.0, 1.0]]
- [[-0.9914400982537864, 0.13051338311503055, 0.003576087581546202, 0.008220083331652344], # 18
[0.13055947584607044, 0.9912284570569639, 0.020502906800483767, -0.7749576385749449],
[-0.0008688160455333092, 0.02079429605298617, -0.9997833977469016, 0.35661089612774244],
[0.0, 0.0, 0.0, 1.0]]
- [[-0.991457583953001, 0.12945142706559348, -0.015943251009705135, 0.012836747022145292], # 19
[0.13042657205207292, 0.9831788268559539, -0.1278604853928495, -0.7398534440169728],
[-0.0008766554754119464, -0.1288476715072142, -0.9916640101477646, 0.4496433982926582],
[0.0, 0.0, 0.0, 1.0]]
- [[-0.0007454634505658086, 0.9999984528394098, -0.0015932994160481884, -0.09555363466697968], # 20
[-0.0013670635165582423, -0.0015943174648042069, -0.9999977946421491, -0.1912978648091873],
[-0.9999987877100573, -0.0007432836650494989, 0.0013682499068901066, 0.9065615327928863],
[0.0, 0.0, 0.0, 1.0]]
set_2:
shoulder_position: 'left'
elbow_position: 'up'
wrist_position: 'down'
joints_set_list:
- [0.00, -1.57, 0.00, 0.00, 0.00, 0.00] # 10
- [1.00, -1.25, 2.07, -0.75, 1.33, 1.57] # 11
- [1.33, -1.25, 2.07, -0.75, 1.33, 1.57] # 12
- [1.33, -1.20, 1.88, -0.60, 1.31, 1.57] # 13
- [1.33, -1.25, 2.07, -0.75, 1.33, 1.57] # 14
- [1.00, -1.25, 2.07, -0.75, 1.33, 1.57] # 15
- [0.00, -1.57, 0.00, 0.00, 0.00, 0.00] # 16
poses_set_list:
- [[0.0014354179450615234, 0.9999978154863796, -0.0015194070523245062, -0.09553419071482575],
[-0.0007330709204442232, -0.0015183559463545493, -0.9999985786001122, -0.19130513074192323],
[-0.9999987010903302, 0.0014365297378850506, 0.000730889843668453, 0.9063502890220441],
[0.0, 0.0, 0.0, 1.0]]
- [[-0.037036685416555276, -0.9457489809904792, -0.3227803415464222, -0.14814765683747758],
[-0.059083068504053426, 0.3245092828016922, -0.9440354423387272, -0.4690369415778026],
[0.9975657747484624, -0.01589309066862904, -0.0678965000533775, 0.10569548235652114],
[0.0, 0.0, 0.0, 1.0]]
- [[-0.015884605890447795, -0.9998736896638919, 0.0005329292824657883, 0.011832276348997105],
[-0.06791371603698126, 0.00054715295039931, -0.9976910482698023, -0.4917360771247944],
[0.9975647379843342, -0.015884122310150683, -0.06791382914166125, 0.10568820644838071],
[0.0, 0.0, 0.0, 1.0]]
- [[-0.018253549075266468, -0.999618439042824, 0.020731239030644166, 4.420278578231691e-05],
[-0.07755014516269004, -0.019256732394706294, -0.996802464504741, -0.5464818740332396],
[0.9968213395244925, -0.01980289330041274, -0.0771690513452766, 0.13794891140563756],
[0.0, 0.0, 0.0, 1.0]]
- [[-0.015875560963345525, -0.9998738321151766, 0.0005351779254427202, 0.011842842840904803],
[-0.06789196850514806, 0.0005439486952618644, -0.9976925301576203, -0.4917374469442858],
[0.9975663622920512, -0.015875262868050577, -0.06789203820975775, 0.10568828688236145],
[0.0, 0.0, 0.0, 1.0]]
- [[-0.03701058328806858, -0.9457512735050129, -0.3227766184040066, -0.14813827383727332],
[-0.05908233864976459, 0.32450378155942083, -0.944037379033962, -0.46904311466227844],
[0.9975667867287652, -0.015868976567015025, -0.06788727125436736, 0.10569019470894718],
[0.0, 0.0, 0.0, 1.0]]
- [[0.00028884819520949567, 0.999998704992728, 0.0015832181111943289, -0.09496862162387869],
[0.0004060217801843402, 0.0015831007682247378, -0.9999986644682441, -0.19160057361602362],
[-0.9999998758565096, 0.0002894906304795053, -0.0004055639785834342, 0.9063366783049188],
[0.0, 0.0, 0.0, 1.0]]
set_3:
shoulder_position: 'left'
elbow_position: 'up'
wrist_position: 'down'
joints_set_list:
- [0.00, -1.57, 0.00, 0.00, 0.00, 0.00] # 10
- [1.00, -1.25, 2.07, -0.75, 1.33, 1.57] # 11
poses_set_list:
- [[0.0013957910618540015, 0.9999979141490123, -0.0014911287456609301, -0.09556265839409475],
[-0.0006348717167568019, -0.0014902437495405707, -0.9999986880548744, -0.19130777444031707],
[-0.9999988243519162, 0.0013967359061193285, 0.0006327903236034438, 0.9063438970137548],
[0.0, 0.0, 0.0, 1.0]]
- [[-0.037035231143762726, -0.9457490810651378, -0.3227802151907346, -0.14814750304167174],
[-0.05908091981484731, 0.3245090236431963, -0.9440356658982607, -0.4690369223614447],
[0.9975659559988808, -0.015892427082995153, -0.06789399232223745, 0.10569669900006234],
[0.0, 0.0, 0.0, 1.0]]
time_step: 5.0 # time between consecutive joints sets (default: 10.0)