stroke_to_angle.cpp
Go to the documentation of this file.
1 /*
2  * This file auto-generated from script. Do not Edit!
3  * Original : aero_startup/.templates/aero_hardware_interface/Stroke2Angle.hh
4  * Original : aero_description/{my_robot}/headers/Stroke2Angle.hh
5 */
6 
7 #include <vector>
8 #include <algorithm>
9 #include <stdint.h>
10 #include <math.h> // for M_PI
11 #include "stroke_to_angle.h"
12 
13 namespace noid
14 {
15  namespace common
16  {
17 
18 
20 
21  float ShoulderPitchInvTable (float _stroke)
22  {
23  int roundedStroke = static_cast<int>(_stroke);
24  int roundedStrokeIndex = roundedStroke - ArrayShoulderPitchInvTableOffset;
25  if(static_cast<int>(ShoulderPitchInvTableCandidates.size() - 1) < roundedStrokeIndex) roundedStrokeIndex = static_cast<int>(ShoulderPitchInvTableCandidates.size() - 1);
26  if(roundedStrokeIndex < 0) roundedStrokeIndex = 0;
27  auto ref = ShoulderPitchInvTableCandidates.at(roundedStrokeIndex);
28  std::vector<S2AData> candidates = ref.first;
29  std::vector<S2AData> appendix = ref.second;
30 
31  if (_stroke < 0) {
32  if (candidates.size() >= 2)
33  if (candidates[0].stroke < candidates[1].stroke)
34  std::reverse(candidates.begin(), candidates.end());
35 
36  for (unsigned int i = 0; i < candidates.size(); ++i)
37  if (_stroke >= candidates[i].stroke)
38  if (candidates[i].range == 0)
39  return candidates[i].angle;
40  else
41  return candidates[i].angle
42  - (candidates[i].stroke - _stroke) / candidates[i].range;
43 
44  if (appendix.size() >= 2)
45  if (appendix[0].stroke < appendix[1].stroke)
46  std::reverse(appendix.begin(), appendix.end());
47 
48  if (appendix.size() == 0) {
49  return candidates[candidates.size() - 1].angle;
50  } else {
51  if (appendix[0].range == 0)
52  return appendix[0].angle;
53  else
54  return appendix[0].angle
55  - (appendix[0].stroke - _stroke) / appendix[0].range;
56  }
57  }
58 
59  else {
60  if (candidates.size() >= 2)
61  if (candidates[0].stroke > candidates[1].stroke)
62  std::reverse(candidates.begin(), candidates.end());
63 
64  for (unsigned int i = 0; i < candidates.size(); ++i)
65  if (_stroke <= candidates[i].stroke)
66  if (candidates[i].range == 0)
67  return candidates[i].angle;
68  else
69  return candidates[i].angle
70  - (candidates[i].stroke - _stroke) / candidates[i].range;
71 
72  if (appendix.size() >= 2)
73  if (appendix[0].stroke > appendix[1].stroke)
74  std::reverse(appendix.begin(), appendix.end());
75 
76  if (appendix.size() == 0) {
77  return candidates[candidates.size() - 1].angle;
78  } else {
79  if (appendix[0].range == 0)
80  return appendix[0].angle;
81  else
82  return appendix[0].angle
83  - (appendix[0].stroke - _stroke) / appendix[0].range;
84  }
85 
87  }
88  }
89  float ShoulderRollInvTable (float _stroke)
90  {
91  int roundedStroke = static_cast<int>(_stroke);
92  int roundedStrokeIndex = roundedStroke - ArrayShoulderRollInvTableOffset;
93  if(static_cast<int>(ShoulderRollInvTableCandidates.size() - 1) < roundedStrokeIndex) roundedStrokeIndex = static_cast<int>(ShoulderRollInvTableCandidates.size() - 1);
94  if(roundedStrokeIndex < 0) roundedStrokeIndex = 0;
95  auto ref = ShoulderRollInvTableCandidates.at(roundedStrokeIndex);
96  std::vector<S2AData> candidates = ref.first;
97  std::vector<S2AData> appendix = ref.second;
98 
99  if (_stroke < 0) {
100  if (candidates.size() >= 2)
101  if (candidates[0].stroke < candidates[1].stroke)
102  std::reverse(candidates.begin(), candidates.end());
103 
104  for (unsigned int i = 0; i < candidates.size(); ++i)
105  if (_stroke >= candidates[i].stroke)
106  if (candidates[i].range == 0)
107  return candidates[i].angle;
108  else
109  return candidates[i].angle
110  - (candidates[i].stroke - _stroke) / candidates[i].range;
111 
112  if (appendix.size() >= 2)
113  if (appendix[0].stroke < appendix[1].stroke)
114  std::reverse(appendix.begin(), appendix.end());
115 
116  if (appendix.size() == 0) {
117  return candidates[candidates.size() - 1].angle;
118  } else {
119  if (appendix[0].range == 0)
120  return appendix[0].angle;
121  else
122  return appendix[0].angle
123  - (appendix[0].stroke - _stroke) / appendix[0].range;
124  }
125  }
126 
127  else {
128  if (candidates.size() >= 2)
129  if (candidates[0].stroke > candidates[1].stroke)
130  std::reverse(candidates.begin(), candidates.end());
131 
132  for (unsigned int i = 0; i < candidates.size(); ++i)
133  if (_stroke <= candidates[i].stroke)
134  if (candidates[i].range == 0)
135  return candidates[i].angle;
136  else
137  return candidates[i].angle
138  - (candidates[i].stroke - _stroke) / candidates[i].range;
139 
140  if (appendix.size() >= 2)
141  if (appendix[0].stroke > appendix[1].stroke)
142  std::reverse(appendix.begin(), appendix.end());
143 
144  if (appendix.size() == 0) {
145  return candidates[candidates.size() - 1].angle;
146  } else {
147  if (appendix[0].range == 0)
148  return appendix[0].angle;
149  else
150  return appendix[0].angle
151  - (appendix[0].stroke - _stroke) / appendix[0].range;
152  }
153 
155  }
156  }
157  float ElbowPitchInvTable (float _stroke)
158  {
159  int roundedStroke = static_cast<int>(_stroke);
160  int roundedStrokeIndex = roundedStroke - ArrayElbowPitchInvTableOffset;
161  if(static_cast<int>(ElbowPitchInvTableCandidates.size() - 1) < roundedStrokeIndex) roundedStrokeIndex = static_cast<int>(ElbowPitchInvTableCandidates.size() - 1);
162  if(roundedStrokeIndex < 0) roundedStrokeIndex = 0;
163  auto ref = ElbowPitchInvTableCandidates.at(roundedStrokeIndex);
164  std::vector<S2AData> candidates = ref.first;
165  std::vector<S2AData> appendix = ref.second;
166 
167  if (_stroke < 0) {
168  if (candidates.size() >= 2)
169  if (candidates[0].stroke < candidates[1].stroke)
170  std::reverse(candidates.begin(), candidates.end());
171 
172  for (unsigned int i = 0; i < candidates.size(); ++i)
173  if (_stroke >= candidates[i].stroke)
174  if (candidates[i].range == 0)
175  return candidates[i].angle;
176  else
177  return candidates[i].angle
178  - (candidates[i].stroke - _stroke) / candidates[i].range;
179 
180  if (appendix.size() >= 2)
181  if (appendix[0].stroke < appendix[1].stroke)
182  std::reverse(appendix.begin(), appendix.end());
183 
184  if (appendix.size() == 0) {
185  return candidates[candidates.size() - 1].angle;
186  } else {
187  if (appendix[0].range == 0)
188  return appendix[0].angle;
189  else
190  return appendix[0].angle
191  - (appendix[0].stroke - _stroke) / appendix[0].range;
192  }
193  }
194 
195  else {
196  if (candidates.size() >= 2)
197  if (candidates[0].stroke > candidates[1].stroke)
198  std::reverse(candidates.begin(), candidates.end());
199 
200  for (unsigned int i = 0; i < candidates.size(); ++i)
201  if (_stroke <= candidates[i].stroke)
202  if (candidates[i].range == 0)
203  return candidates[i].angle;
204  else
205  return candidates[i].angle
206  - (candidates[i].stroke - _stroke) / candidates[i].range;
207 
208  if (appendix.size() >= 2)
209  if (appendix[0].stroke > appendix[1].stroke)
210  std::reverse(appendix.begin(), appendix.end());
211 
212  if (appendix.size() == 0) {
213  return candidates[candidates.size() - 1].angle;
214  } else {
215  if (appendix[0].range == 0)
216  return appendix[0].angle;
217  else
218  return appendix[0].angle
219  - (appendix[0].stroke - _stroke) / appendix[0].range;
220  }
221 
223  }
224  }
225  float WristPitchInvTable (float _stroke)
226  {
227  int roundedStroke = static_cast<int>(_stroke);
228  int roundedStrokeIndex = roundedStroke - ArrayWristPitchInvTableOffset;
229  if(static_cast<int>(WristPitchInvTableCandidates.size() - 1) < roundedStrokeIndex) roundedStrokeIndex = static_cast<int>(WristPitchInvTableCandidates.size() - 1);
230  if(roundedStrokeIndex < 0) roundedStrokeIndex = 0;
231  auto ref = WristPitchInvTableCandidates.at(roundedStrokeIndex);
232  std::vector<S2AData> candidates = ref.first;
233  std::vector<S2AData> appendix = ref.second;
234 
235  if (_stroke < 0) {
236  if (candidates.size() >= 2)
237  if (candidates[0].stroke < candidates[1].stroke)
238  std::reverse(candidates.begin(), candidates.end());
239 
240  for (unsigned int i = 0; i < candidates.size(); ++i)
241  if (_stroke >= candidates[i].stroke)
242  if (candidates[i].range == 0)
243  return candidates[i].angle;
244  else
245  return candidates[i].angle
246  - (candidates[i].stroke - _stroke) / candidates[i].range;
247 
248  if (appendix.size() >= 2)
249  if (appendix[0].stroke < appendix[1].stroke)
250  std::reverse(appendix.begin(), appendix.end());
251 
252  if (appendix.size() == 0) {
253  return candidates[candidates.size() - 1].angle;
254  } else {
255  if (appendix[0].range == 0)
256  return appendix[0].angle;
257  else
258  return appendix[0].angle
259  - (appendix[0].stroke - _stroke) / appendix[0].range;
260  }
261  }
262 
263  else {
264  if (candidates.size() >= 2)
265  if (candidates[0].stroke > candidates[1].stroke)
266  std::reverse(candidates.begin(), candidates.end());
267 
268  for (unsigned int i = 0; i < candidates.size(); ++i)
269  if (_stroke <= candidates[i].stroke)
270  if (candidates[i].range == 0)
271  return candidates[i].angle;
272  else
273  return candidates[i].angle
274  - (candidates[i].stroke - _stroke) / candidates[i].range;
275 
276  if (appendix.size() >= 2)
277  if (appendix[0].stroke > appendix[1].stroke)
278  std::reverse(appendix.begin(), appendix.end());
279 
280  if (appendix.size() == 0) {
281  return candidates[candidates.size() - 1].angle;
282  } else {
283  if (appendix[0].range == 0)
284  return appendix[0].angle;
285  else
286  return appendix[0].angle
287  - (appendix[0].stroke - _stroke) / appendix[0].range;
288  }
289 
291  }
292  }
293  float WristRollInvTable (float _stroke)
294  {
295  int roundedStroke = static_cast<int>(_stroke);
296  int roundedStrokeIndex = roundedStroke - ArrayWristRollInvTableOffset;
297  if(static_cast<int>(WristRollInvTableCandidates.size() - 1) < roundedStrokeIndex) roundedStrokeIndex = static_cast<int>(WristRollInvTableCandidates.size() - 1);
298  if(roundedStrokeIndex < 0) roundedStrokeIndex = 0;
299  auto ref = WristRollInvTableCandidates.at(roundedStrokeIndex);
300  std::vector<S2AData> candidates = ref.first;
301  std::vector<S2AData> appendix = ref.second;
302 
303  if (_stroke < 0) {
304  if (candidates.size() >= 2)
305  if (candidates[0].stroke < candidates[1].stroke)
306  std::reverse(candidates.begin(), candidates.end());
307 
308  for (unsigned int i = 0; i < candidates.size(); ++i)
309  if (_stroke >= candidates[i].stroke)
310  if (candidates[i].range == 0)
311  return candidates[i].angle;
312  else
313  return candidates[i].angle
314  - (candidates[i].stroke - _stroke) / candidates[i].range;
315 
316  if (appendix.size() >= 2)
317  if (appendix[0].stroke < appendix[1].stroke)
318  std::reverse(appendix.begin(), appendix.end());
319 
320  if (appendix.size() == 0) {
321  return candidates[candidates.size() - 1].angle;
322  } else {
323  if (appendix[0].range == 0)
324  return appendix[0].angle;
325  else
326  return appendix[0].angle
327  - (appendix[0].stroke - _stroke) / appendix[0].range;
328  }
329  }
330 
331  else {
332  if (candidates.size() >= 2)
333  if (candidates[0].stroke > candidates[1].stroke)
334  std::reverse(candidates.begin(), candidates.end());
335 
336  for (unsigned int i = 0; i < candidates.size(); ++i)
337  if (_stroke <= candidates[i].stroke)
338  if (candidates[i].range == 0)
339  return candidates[i].angle;
340  else
341  return candidates[i].angle
342  - (candidates[i].stroke - _stroke) / candidates[i].range;
343 
344  if (appendix.size() >= 2)
345  if (appendix[0].stroke > appendix[1].stroke)
346  std::reverse(appendix.begin(), appendix.end());
347 
348  if (appendix.size() == 0) {
349  return candidates[candidates.size() - 1].angle;
350  } else {
351  if (appendix[0].range == 0)
352  return appendix[0].angle;
353  else
354  return appendix[0].angle
355  - (appendix[0].stroke - _stroke) / appendix[0].range;
356  }
357 
359  }
360  }
361  float WaistPitchInvTable (float _stroke)
362  {
363  int roundedStroke = static_cast<int>(_stroke);
364  int roundedStrokeIndex = roundedStroke - ArrayWaistPitchInvTableOffset;
365  if(static_cast<int>(WaistPitchInvTableCandidates.size() - 1) < roundedStrokeIndex) roundedStrokeIndex = static_cast<int>(WaistPitchInvTableCandidates.size() - 1);
366  if(roundedStrokeIndex < 0) roundedStrokeIndex = 0;
367  auto ref = WaistPitchInvTableCandidates.at(roundedStrokeIndex);
368  std::vector<S2AData> candidates = ref.first;
369  std::vector<S2AData> appendix = ref.second;
370 
371  if (_stroke < 0) {
372  if (candidates.size() >= 2)
373  if (candidates[0].stroke < candidates[1].stroke)
374  std::reverse(candidates.begin(), candidates.end());
375 
376  for (unsigned int i = 0; i < candidates.size(); ++i)
377  if (_stroke >= candidates[i].stroke)
378  if (candidates[i].range == 0)
379  return candidates[i].angle;
380  else
381  return candidates[i].angle
382  - (candidates[i].stroke - _stroke) / candidates[i].range;
383 
384  if (appendix.size() >= 2)
385  if (appendix[0].stroke < appendix[1].stroke)
386  std::reverse(appendix.begin(), appendix.end());
387 
388  if (appendix.size() == 0) {
389  return candidates[candidates.size() - 1].angle;
390  } else {
391  if (appendix[0].range == 0)
392  return appendix[0].angle;
393  else
394  return appendix[0].angle
395  - (appendix[0].stroke - _stroke) / appendix[0].range;
396  }
397  }
398 
399  else {
400  if (candidates.size() >= 2)
401  if (candidates[0].stroke > candidates[1].stroke)
402  std::reverse(candidates.begin(), candidates.end());
403 
404  for (unsigned int i = 0; i < candidates.size(); ++i)
405  if (_stroke <= candidates[i].stroke)
406  if (candidates[i].range == 0)
407  return candidates[i].angle;
408  else
409  return candidates[i].angle
410  - (candidates[i].stroke - _stroke) / candidates[i].range;
411 
412  if (appendix.size() >= 2)
413  if (appendix[0].stroke > appendix[1].stroke)
414  std::reverse(appendix.begin(), appendix.end());
415 
416  if (appendix.size() == 0) {
417  return candidates[candidates.size() - 1].angle;
418  } else {
419  if (appendix[0].range == 0)
420  return appendix[0].angle;
421  else
422  return appendix[0].angle
423  - (appendix[0].stroke - _stroke) / appendix[0].range;
424  }
425 
427  }
428  }
429  float WaistRollInvTable (float _stroke)
430  {
431  int roundedStroke = static_cast<int>(_stroke);
432  int roundedStrokeIndex = roundedStroke - ArrayWaistRollInvTableOffset;
433  if(static_cast<int>(WaistRollInvTableCandidates.size() - 1) < roundedStrokeIndex) roundedStrokeIndex = static_cast<int>(WaistRollInvTableCandidates.size() - 1);
434  if(roundedStrokeIndex < 0) roundedStrokeIndex = 0;
435  auto ref = WaistRollInvTableCandidates.at(roundedStrokeIndex);
436  std::vector<S2AData> candidates = ref.first;
437  std::vector<S2AData> appendix = ref.second;
438 
439  if (_stroke < 0) {
440  if (candidates.size() >= 2)
441  if (candidates[0].stroke < candidates[1].stroke)
442  std::reverse(candidates.begin(), candidates.end());
443 
444  for (unsigned int i = 0; i < candidates.size(); ++i)
445  if (_stroke >= candidates[i].stroke)
446  if (candidates[i].range == 0)
447  return candidates[i].angle;
448  else
449  return candidates[i].angle
450  - (candidates[i].stroke - _stroke) / candidates[i].range;
451 
452  if (appendix.size() >= 2)
453  if (appendix[0].stroke < appendix[1].stroke)
454  std::reverse(appendix.begin(), appendix.end());
455 
456  if (appendix.size() == 0) {
457  return candidates[candidates.size() - 1].angle;
458  } else {
459  if (appendix[0].range == 0)
460  return appendix[0].angle;
461  else
462  return appendix[0].angle
463  - (appendix[0].stroke - _stroke) / appendix[0].range;
464  }
465  }
466 
467  else {
468  if (candidates.size() >= 2)
469  if (candidates[0].stroke > candidates[1].stroke)
470  std::reverse(candidates.begin(), candidates.end());
471 
472  for (unsigned int i = 0; i < candidates.size(); ++i)
473  if (_stroke <= candidates[i].stroke)
474  if (candidates[i].range == 0)
475  return candidates[i].angle;
476  else
477  return candidates[i].angle
478  - (candidates[i].stroke - _stroke) / candidates[i].range;
479 
480  if (appendix.size() >= 2)
481  if (appendix[0].stroke > appendix[1].stroke)
482  std::reverse(appendix.begin(), appendix.end());
483 
484  if (appendix.size() == 0) {
485  return candidates[candidates.size() - 1].angle;
486  } else {
487  if (appendix[0].range == 0)
488  return appendix[0].angle;
489  else
490  return appendix[0].angle
491  - (appendix[0].stroke - _stroke) / appendix[0].range;
492  }
493 
495  }
496  }
497  float NeckPitchInvTable (float _stroke)
498  {
499  int roundedStroke = static_cast<int>(_stroke);
500  int roundedStrokeIndex = roundedStroke - ArrayNeckPitchInvTableOffset;
501  if(static_cast<int>(NeckPitchInvTableCandidates.size() - 1) < roundedStrokeIndex) roundedStrokeIndex = static_cast<int>(NeckPitchInvTableCandidates.size() - 1);
502  if(roundedStrokeIndex < 0) roundedStrokeIndex = 0;
503  auto ref = NeckPitchInvTableCandidates.at(roundedStrokeIndex);
504  std::vector<S2AData> candidates = ref.first;
505  std::vector<S2AData> appendix = ref.second;
506 
507  if (_stroke < 0) {
508  if (candidates.size() >= 2)
509  if (candidates[0].stroke < candidates[1].stroke)
510  std::reverse(candidates.begin(), candidates.end());
511 
512  for (unsigned int i = 0; i < candidates.size(); ++i)
513  if (_stroke >= candidates[i].stroke)
514  if (candidates[i].range == 0)
515  return candidates[i].angle;
516  else
517  return candidates[i].angle
518  - (candidates[i].stroke - _stroke) / candidates[i].range;
519 
520  if (appendix.size() >= 2)
521  if (appendix[0].stroke < appendix[1].stroke)
522  std::reverse(appendix.begin(), appendix.end());
523 
524  if (appendix.size() == 0) {
525  return candidates[candidates.size() - 1].angle;
526  } else {
527  if (appendix[0].range == 0)
528  return appendix[0].angle;
529  else
530  return appendix[0].angle
531  - (appendix[0].stroke - _stroke) / appendix[0].range;
532  }
533  }
534 
535  else {
536  if (candidates.size() >= 2)
537  if (candidates[0].stroke > candidates[1].stroke)
538  std::reverse(candidates.begin(), candidates.end());
539 
540  for (unsigned int i = 0; i < candidates.size(); ++i)
541  if (_stroke <= candidates[i].stroke)
542  if (candidates[i].range == 0)
543  return candidates[i].angle;
544  else
545  return candidates[i].angle
546  - (candidates[i].stroke - _stroke) / candidates[i].range;
547 
548  if (appendix.size() >= 2)
549  if (appendix[0].stroke > appendix[1].stroke)
550  std::reverse(appendix.begin(), appendix.end());
551 
552  if (appendix.size() == 0) {
553  return candidates[candidates.size() - 1].angle;
554  } else {
555  if (appendix[0].range == 0)
556  return appendix[0].angle;
557  else
558  return appendix[0].angle
559  - (appendix[0].stroke - _stroke) / appendix[0].range;
560  }
561 
563  }
564  }
565  float NeckRollInvTable (float _stroke)
566  {
567  int roundedStroke = static_cast<int>(_stroke);
568  int roundedStrokeIndex = roundedStroke - ArrayNeckRollInvTableOffset;
569  if(static_cast<int>(NeckRollInvTableCandidates.size() - 1) < roundedStrokeIndex) roundedStrokeIndex = static_cast<int>(NeckRollInvTableCandidates.size() - 1);
570  if(roundedStrokeIndex < 0) roundedStrokeIndex = 0;
571  auto ref = NeckRollInvTableCandidates.at(roundedStrokeIndex);
572  std::vector<S2AData> candidates = ref.first;
573  std::vector<S2AData> appendix = ref.second;
574 
575  if (_stroke < 0) {
576  if (candidates.size() >= 2)
577  if (candidates[0].stroke < candidates[1].stroke)
578  std::reverse(candidates.begin(), candidates.end());
579 
580  for (unsigned int i = 0; i < candidates.size(); ++i)
581  if (_stroke >= candidates[i].stroke)
582  if (candidates[i].range == 0)
583  return candidates[i].angle;
584  else
585  return candidates[i].angle
586  - (candidates[i].stroke - _stroke) / candidates[i].range;
587 
588  if (appendix.size() >= 2)
589  if (appendix[0].stroke < appendix[1].stroke)
590  std::reverse(appendix.begin(), appendix.end());
591 
592  if (appendix.size() == 0) {
593  return candidates[candidates.size() - 1].angle;
594  } else {
595  if (appendix[0].range == 0)
596  return appendix[0].angle;
597  else
598  return appendix[0].angle
599  - (appendix[0].stroke - _stroke) / appendix[0].range;
600  }
601  }
602 
603  else {
604  if (candidates.size() >= 2)
605  if (candidates[0].stroke > candidates[1].stroke)
606  std::reverse(candidates.begin(), candidates.end());
607 
608  for (unsigned int i = 0; i < candidates.size(); ++i)
609  if (_stroke <= candidates[i].stroke)
610  if (candidates[i].range == 0)
611  return candidates[i].angle;
612  else
613  return candidates[i].angle
614  - (candidates[i].stroke - _stroke) / candidates[i].range;
615 
616  if (appendix.size() >= 2)
617  if (appendix[0].stroke > appendix[1].stroke)
618  std::reverse(appendix.begin(), appendix.end());
619 
620  if (appendix.size() == 0) {
621  return candidates[candidates.size() - 1].angle;
622  } else {
623  if (appendix[0].range == 0)
624  return appendix[0].angle;
625  else
626  return appendix[0].angle
627  - (appendix[0].stroke - _stroke) / appendix[0].range;
628  }
629 
631  }
632  }
633  float LegInvTable (float _stroke)
634  {
635  int roundedStroke = static_cast<int>(_stroke);
636  int roundedStrokeIndex = roundedStroke - ArrayLegInvTableOffset;
637  if(static_cast<int>(LegInvTableCandidates.size() - 1) < roundedStrokeIndex) roundedStrokeIndex = static_cast<int>(LegInvTableCandidates.size() - 1);
638  if(roundedStrokeIndex < 0) roundedStrokeIndex = 0;
639  auto ref = LegInvTableCandidates.at(roundedStrokeIndex);
640  std::vector<S2AData> candidates = ref.first;
641  std::vector<S2AData> appendix = ref.second;
642 
643  if (_stroke < 0) {
644  if (candidates.size() >= 2)
645  if (candidates[0].stroke < candidates[1].stroke)
646  std::reverse(candidates.begin(), candidates.end());
647 
648  for (unsigned int i = 0; i < candidates.size(); ++i)
649  if (_stroke >= candidates[i].stroke)
650  if (candidates[i].range == 0)
651  return candidates[i].angle;
652  else
653  return candidates[i].angle
654  - (candidates[i].stroke - _stroke) / candidates[i].range;
655 
656  if (appendix.size() >= 2)
657  if (appendix[0].stroke < appendix[1].stroke)
658  std::reverse(appendix.begin(), appendix.end());
659 
660  if (appendix.size() == 0) {
661  return candidates[candidates.size() - 1].angle;
662  } else {
663  if (appendix[0].range == 0)
664  return appendix[0].angle;
665  else
666  return appendix[0].angle
667  - (appendix[0].stroke - _stroke) / appendix[0].range;
668  }
669  }
670 
671  else {
672  if (candidates.size() >= 2)
673  if (candidates[0].stroke > candidates[1].stroke)
674  std::reverse(candidates.begin(), candidates.end());
675 
676  for (unsigned int i = 0; i < candidates.size(); ++i)
677  if (_stroke <= candidates[i].stroke)
678  if (candidates[i].range == 0)
679  return candidates[i].angle;
680  else
681  return candidates[i].angle
682  - (candidates[i].stroke - _stroke) / candidates[i].range;
683 
684  if (appendix.size() >= 2)
685  if (appendix[0].stroke > appendix[1].stroke)
686  std::reverse(appendix.begin(), appendix.end());
687 
688  if (appendix.size() == 0) {
689  return candidates[candidates.size() - 1].angle;
690  } else {
691  if (appendix[0].range == 0)
692  return appendix[0].angle;
693  else
694  return appendix[0].angle
695  - (appendix[0].stroke - _stroke) / appendix[0].range;
696  }
697  }
698  }
699 
700 
701  }
702 }
703 
noid::common::ShoulderRollInvTable
float ShoulderRollInvTable(float _stroke)
Definition: stroke_to_angle.cpp:89
noid::common::WristRollInvTableCandidates
static const std::vector< std::pair< std::vector< S2AData >, std::vector< S2AData > > > WristRollInvTableCandidates
Definition: stroke_to_angle.h:35
noid::common::WristPitchInvTableCandidates
static const std::vector< std::pair< std::vector< S2AData >, std::vector< S2AData > > > WristPitchInvTableCandidates
Definition: stroke_to_angle.h:37
noid::common::ArrayElbowPitchInvTableOffset
static const int ArrayElbowPitchInvTableOffset
Definition: stroke_to_angle.h:40
noid::common::ArrayWristPitchInvTableOffset
static const int ArrayWristPitchInvTableOffset
Definition: stroke_to_angle.h:38
noid::common::ElbowPitchInvTableCandidates
static const std::vector< std::pair< std::vector< S2AData >, std::vector< S2AData > > > ElbowPitchInvTableCandidates
Definition: stroke_to_angle.h:39
noid::common::WaistRollInvTableCandidates
static const std::vector< std::pair< std::vector< S2AData >, std::vector< S2AData > > > WaistRollInvTableCandidates
Definition: stroke_to_angle.h:31
noid::common::NeckRollInvTableCandidates
static const std::vector< std::pair< std::vector< S2AData >, std::vector< S2AData > > > NeckRollInvTableCandidates
Definition: stroke_to_angle.h:27
noid::common::WristPitchInvTable
float WristPitchInvTable(float _stroke)
Definition: stroke_to_angle.cpp:225
noid::common::WristRollInvTable
float WristRollInvTable(float _stroke)
Definition: stroke_to_angle.cpp:293
noid::common::ShoulderRollInvTableCandidates
static const std::vector< std::pair< std::vector< S2AData >, std::vector< S2AData > > > ShoulderRollInvTableCandidates
Definition: stroke_to_angle.h:41
noid::common::WaistPitchInvTable
float WaistPitchInvTable(float _stroke)
Definition: stroke_to_angle.cpp:361
noid::common::LegInvTableCandidates
static const std::vector< std::pair< std::vector< S2AData >, std::vector< S2AData > > > LegInvTableCandidates
Definition: stroke_to_angle.h:25
noid::common::NeckPitchInvTable
float NeckPitchInvTable(float _stroke)
Definition: stroke_to_angle.cpp:497
noid::common::NeckPitchInvTableCandidates
static const std::vector< std::pair< std::vector< S2AData >, std::vector< S2AData > > > NeckPitchInvTableCandidates
Definition: stroke_to_angle.h:29
noid::common::ElbowPitchInvTable
float ElbowPitchInvTable(float _stroke)
Definition: stroke_to_angle.cpp:157
noid::common::ArrayNeckRollInvTableOffset
static const int ArrayNeckRollInvTableOffset
Definition: stroke_to_angle.h:28
noid::common::ArrayWristRollInvTableOffset
static const int ArrayWristRollInvTableOffset
Definition: stroke_to_angle.h:36
noid::common::ShoulderPitchInvTable
float ShoulderPitchInvTable(float _stroke)
Definition: stroke_to_angle.cpp:21
noid::common::ArrayShoulderRollInvTableOffset
static const int ArrayShoulderRollInvTableOffset
Definition: stroke_to_angle.h:42
noid::common::ArrayShoulderPitchInvTableOffset
static const int ArrayShoulderPitchInvTableOffset
Definition: stroke_to_angle.h:45
noid::common::ArrayLegInvTableOffset
static const int ArrayLegInvTableOffset
Definition: stroke_to_angle.h:26
noid::common::NeckRollInvTable
float NeckRollInvTable(float _stroke)
Definition: stroke_to_angle.cpp:565
noid::common::ArrayWaistRollInvTableOffset
static const int ArrayWaistRollInvTableOffset
Definition: stroke_to_angle.h:32
noid::common::WaistRollInvTable
float WaistRollInvTable(float _stroke)
Definition: stroke_to_angle.cpp:429
noid::common::WaistPitchInvTableCandidates
static const std::vector< std::pair< std::vector< S2AData >, std::vector< S2AData > > > WaistPitchInvTableCandidates
Definition: stroke_to_angle.h:33
noid::common::ShoulderPitchInvTableCandidates
static const std::vector< std::pair< std::vector< S2AData >, std::vector< S2AData > > > ShoulderPitchInvTableCandidates
Definition: stroke_to_angle.h:43
stroke_to_angle.h
noid::common::LegInvTable
float LegInvTable(float _stroke)
Definition: stroke_to_angle.cpp:633
noid::common::ArrayNeckPitchInvTableOffset
static const int ArrayNeckPitchInvTableOffset
Definition: stroke_to_angle.h:30
noid::common::ArrayWaistPitchInvTableOffset
static const int ArrayWaistPitchInvTableOffset
Definition: stroke_to_angle.h:34

noid_robot_interface
Author(s):
autogenerated on Sat Jul 20 2019 03:44:26