trajectory.cpp
Go to the documentation of this file.
1 #include "ros_marty/trajectory.hpp"
2 
3 // linear interpolation of trajectory with desired dt
4 // this function ensures that the original tIn points are explicitly stated in tOut
5 // it does not (necessarily) result in a uniform dt in tOut
6 
7 // TODO: Re-Write using iterators rather than popping.
8 // Also then don't need to copy tIn, maybe rewrite to produce uniform dt on tOut
9 
10 float Trajectory::gettime() {
11  struct timespec monotime;
12  clock_gettime(CLOCK_MONOTONIC, &monotime);
13  return (monotime.tv_sec + ((float)monotime.tv_nsec) / 1000000000);
14 }
15 
16 bool Trajectory::interpTrajectory(data_t tIn, data_t& tOut, float dt) {
17  if (tIn.empty())
18  return false;
19 
20  deque <float> tstart, tend;
21  tend = tIn.front();
22  tIn.pop_front();
23 
24  int numjoints = tend.size() - 1;
25  if (DEBUG_MODE) { printf("numjoints: %d\n", numjoints); }
26 
27  if (numjoints < 0) return false;
28 
29  while (!tIn.empty()) {
30  tstart = tend;
31  tend = tIn.front();
32  tIn.pop_front();
33 
34  for (float t = tstart[0]; t < tend[0]; t += dt) {
35  deque <float> thisline;
36  thisline.push_back(t);
37  for (int j = 1; j <= numjoints; j++) {
38  thisline.push_back(tstart[j] + (tend[j] - tstart[j]) * (t - tstart[0]) /
39  (tend[0] - tstart[0]));
40  }
41  tOut.push_back(thisline);
42  }
43  }
44  tOut.push_back(tend);
45 
46  return true;
47 }
48 
49 
50 void Trajectory::printTrajectory(data_t& traj) {
51  if (DEBUG_MODE) {
52  printf("trajectory size: %d x %d\n", (int)traj.size(), (int)traj[0].size());
53 
54  for (data_t::record_iterator ri = traj.begin(); ri != traj.end(); ri++) {
55  for (data_t::field_iterator fi = (*ri).begin(); fi != (*ri).end(); fi++)
56  { printf("%2.2f\t", *fi); }
57  printf("\n");
58  }
59  }
60  return;
61 }
62 
63 bool Trajectory::runTrajectory(MartyCore* robot, data_t& traj) {
64  float startTime = gettime();
65 
66  data_t::record_iterator ri = traj.begin();
67  while (ri != traj.end()) {
68  if (gettime() - startTime > (*ri)[0]) {
69  deque<float> thisline = *ri;
70  thisline.pop_front(); // get rid of time from angles
71  std::map<int, float> angles;
72  for (int i = 0; i < NUMJOINTS; ++i) {
73  angles[i] = thisline[i];
74  }
75  // robot->setServos(thisline);
76  robot->setServos(angles);
77  ri++;
78  }
79  }
80  return true;
81 }
82 
83 data_t Trajectory::genStepLeft(MartyCore* robot, int stepLength, int turn,
84  float period) {
85  int arm_move1 = stepLength / 5;
86  int arm_move2 = stepLength / 1.6;
87  int arm_move3 = stepLength * 1.6;
88  int arm_move4 = stepLength / 1.25;
89  data_t tSetpoints, tInterp;
90  deque<float> tline(robot->jangles_);
91  tline.push_front(0.0);
92  tSetpoints.push_back(tline);
93 
94  tline[0] = 0.25 * period;
95  tline[1 + RKNEE] = WKSMALL; tline[1 + LKNEE] = WKSMALL;
96  tline[1 + RARM] = arm_move1; tline[1 + LARM] = -arm_move1;
97  tSetpoints.push_back(tline);
98 
99  tline[0] = 0.375 * period;
100  tline[1 + LKNEE] = WKLARGE;
101  tline[1 + RARM] = arm_move2; tline[1 + LARM] = -arm_move2;
102  tSetpoints.push_back(tline);
103 
104  tline[0] = 0.625 * period;
105  tline[1 + RHIP] = stepLength / 2; tline[1 + LHIP] = 0 - stepLength / 2;
106  tline[1 + LTWIST] = 0; tline[1 + RTWIST] = turn;
107  tline[1 + RARM] = arm_move3; tline[1 + LARM] = -arm_move3;
108  tSetpoints.push_back(tline);
109 
110  tline[0] = 0.75 * period;
111  tline[1 + LKNEE] = WKSMALL;
112  tline[1 + RARM] = arm_move4; tline[1 + LARM] = -arm_move4;
113  tSetpoints.push_back(tline);
114 
115  tline[0] = period;
116  tline[1 + RKNEE] = 0; tline[1 + LKNEE] = 0;
117  tline[1 + RARM] = 0; tline[1 + LARM] = 0;
118  tSetpoints.push_back(tline);
119 
120  interpTrajectory(tSetpoints, tInterp, INTERP_DT);
121 
122  return tInterp;
123 }
124 
125 data_t Trajectory::genSitBack(MartyCore* robot, float period) {
126  data_t tSetpoints, tInterp;
127  deque<float> tline(robot->jangles_);
128  tline.push_front(0.0);
129  tSetpoints.push_back(tline);
130 
131  tline[0] = period;
132  tline[1 + RHIP] = -80; tline[1 + LHIP] = -80;
133  tline[1 + RARM] = 100; tline[1 + LARM] = 100;
134  tSetpoints.push_back(tline);
135 
136  interpTrajectory(tSetpoints, tInterp, INTERP_DT);
137 
138  return tInterp;
139 }
140 
141 data_t Trajectory::genStepRight(MartyCore* robot, int stepLength, int turn,
142  float period) {
143  int arm_move1 = stepLength / 5;
144  int arm_move2 = stepLength / 1.6;
145  int arm_move3 = stepLength * 1.6;
146  int arm_move4 = stepLength / 1.25;
147  data_t tSetpoints, tInterp;
148  deque<float> tline(robot->jangles_);
149  tline.push_front(0.0);
150  tSetpoints.push_back(tline);
151 
152  tline[0] = 0.25 * period;
153  tline[1 + RKNEE] = 0 - WKSMALL; tline[1 + LKNEE] = 0 - WKSMALL;
154  tline[1 + RARM] = -arm_move1; tline[1 + LARM] = arm_move1;
155  tSetpoints.push_back(tline);
156 
157  tline[0] = 0.375 * period;
158  tline[1 + RKNEE] = 0 - WKLARGE;
159  tline[1 + RARM] = -arm_move2; tline[1 + LARM] = arm_move2;
160  tSetpoints.push_back(tline);
161 
162  tline[0] = 0.625 * period;
163  tline[1 + RHIP] = 0 - stepLength / 2; tline[1 + LHIP] = stepLength / 2;
164  tline[1 + LTWIST] = turn; tline[1 + RTWIST] = 0;
165  tline[1 + RARM] = -arm_move3; tline[1 + LARM] = arm_move3;
166  tSetpoints.push_back(tline);
167 
168  tline[0] = 0.75 * period;
169  tline[1 + RKNEE] = 0 - WKSMALL;
170  tline[1 + RARM] = -arm_move4; tline[1 + LARM] = arm_move4;
171  tSetpoints.push_back(tline);
172 
173  tline[0] = period;
174  tline[1 + RKNEE] = 0; tline[1 + LKNEE] = 0;
175  tline[1 + RARM] = 0; tline[1 + LARM] = 0;
176  tSetpoints.push_back(tline);
177 
178  interpTrajectory(tSetpoints, tInterp, INTERP_DT);
179 
180  return tInterp;
181 
182 }
183 
184 data_t Trajectory::genCelebration(MartyCore* robot, float move_time) {
185  data_t tSetpoints, tInterp;
186  deque<float> tline(robot->jangles_);
187  tline.push_front(0.0);
188  tSetpoints.push_back(tline);
189  int lean_amount = 40;
190 
191  tline[0] = 0.1 * move_time;
192  tline[1 + RARM] = 60; tline[1 + LARM] = 60;
193  tline[1 + LTWIST] = 0; tline[1 + RTWIST] = 0;
194  tline[1 + EYES] = -20;
195  tSetpoints.push_back(tline);
196 
197 
198  for (float t = 0.2; t < 1.0; t += 0.2) {
199  tline[0] = t * move_time;
200  tline[1 + LKNEE] = lean_amount; tline[1 + RKNEE] = lean_amount;
201  tline[1 + RARM] = 60; tline[1 + LARM] = 10;
202  tline[1 + EYES] = -0;
203  tSetpoints.push_back(tline);
204 
205  tline[0] = (t + 0.1) * move_time;
206  tline[1 + LKNEE] = -lean_amount; tline[1 + RKNEE] = -lean_amount;
207  tline[1 + RARM] = 10; tline[1 + LARM] = 60;
208  tline[1 + EYES] = -40;
209  tSetpoints.push_back(tline);
210  }
211 
212 
213  tline[0] = move_time;
214  tline[1 + LKNEE] = 0; tline[1 + RKNEE] = 0;
215  tline[1 + RARM] = 0; tline[1 + LARM] = 0;
216  tline[1 + EYES] = 0;
217  tSetpoints.push_back(tline);
218 
219  interpTrajectory(tSetpoints, tInterp, INTERP_DT);
220 
221  return tInterp;
222 }
223 
224 data_t Trajectory::genRaisedFootTwistLeft(MartyCore* robot, float period) {
225  data_t tSetpoints, tInterp;
226  deque<float> tline(robot->jangles_);
227  tline.push_front(0.0);
228  tSetpoints.push_back(tline);
229 
230  tline[0] = 0.15 * period;
231  tline[1 + RKNEE] = WKSMALL; tline[1 + LKNEE] = WKSMALL;
232  tSetpoints.push_back(tline);
233 
234  tline[0] = 0.3 * period;
235  tline[1 + LKNEE] = WKLARGE;
236  tline[1 + LHIP] = -110;
237  tSetpoints.push_back(tline);
238 
239  tline[0] = 0.4 * period;
240  tline[1 + LTWIST] = -70;
241  tSetpoints.push_back(tline);
242 
243  tline[0] = 0.5 * period;
244  tline[1 + LTWIST] = 70;
245  tSetpoints.push_back(tline);
246 
247  tline[0] = 0.6 * period;
248  tline[1 + LTWIST] = -70;
249  tSetpoints.push_back(tline);
250 
251  tline[0] = 0.7 * period;
252  tline[1 + LTWIST] = 70;
253  tSetpoints.push_back(tline);
254 
255  tline[0] = 0.85 * period;
256  tline[1 + LKNEE] = WKSMALL;
257  tline[1 + LTWIST] = 0;
258  tline[1 + LHIP] = 0;
259  tSetpoints.push_back(tline);
260 
261  tline[0] = period;
262  tline[1 + LKNEE] = 0; tline[1 + RKNEE] = 0;
263  tSetpoints.push_back(tline);
264 
265  interpTrajectory(tSetpoints, tInterp, INTERP_DT);
266 
267  return tInterp;
268 
269 }
270 
271 data_t Trajectory::genRaisedFootTwistRight(MartyCore* robot, float period) {
272  data_t tSetpoints, tInterp;
273  deque<float> tline(robot->jangles_);
274  tline.push_front(0.0);
275  tSetpoints.push_back(tline);
276 
277  tline[0] = 0.15 * period;
278  tline[1 + RKNEE] = -WKSMALL; tline[1 + LKNEE] = -WKSMALL;
279  tSetpoints.push_back(tline);
280 
281  tline[0] = 0.3 * period;
282  tline[1 + RKNEE] = -WKLARGE;
283  tline[1 + RHIP] = -110;
284  tSetpoints.push_back(tline);
285 
286  tline[0] = 0.4 * period;
287  tline[1 + RTWIST] = -70;
288  tSetpoints.push_back(tline);
289 
290  tline[0] = 0.5 * period;
291  tline[1 + RTWIST] = 70;
292  tSetpoints.push_back(tline);
293 
294  tline[0] = 0.6 * period;
295  tline[1 + RTWIST] = -70;
296  tSetpoints.push_back(tline);
297 
298  tline[0] = 0.7 * period;
299  tline[1 + RTWIST] = 70;
300  tSetpoints.push_back(tline);
301 
302  tline[0] = 0.85 * period;
303  tline[1 + RKNEE] = -WKSMALL;
304  tline[1 + RTWIST] = 0;
305  tline[1 + RHIP] = 0;
306  tSetpoints.push_back(tline);
307 
308  tline[0] = period;
309  tline[1 + LKNEE] = 0; tline[1 + RKNEE] = 0;
310  tSetpoints.push_back(tline);
311 
312  interpTrajectory(tSetpoints, tInterp, INTERP_DT);
313 
314  return tInterp;
315 
316 }
317 
318 data_t Trajectory::genKickLeft(MartyCore* robot, float period) {
319  data_t tSetpoints, tInterp;
320  deque<float> tline(robot->jangles_);
321  tline.push_front(0.0);
322  tSetpoints.push_back(tline);
323 
324  tline[0] = 0.2 * period;
325  tline[1 + RKNEE] = WKSMALL; tline[1 + LKNEE] = WKSMALL;
326  tline[1 + RARM] = 20; tline[1 + LARM] = 20;
327  tSetpoints.push_back(tline);
328 
329  tline[0] = 0.375 * period;
330  tline[1 + LKNEE] = WKLARGE;
331  tline[1 + LHIP] = 10;
332  tSetpoints.push_back(tline);
333 
334  tline[0] = 0.425 * period;
335  tline[1 + LHIP] = -120;
336  tline[1 + EYES] = 30;
337  tline[1 + RARM] = -20; tline[1 + LARM] = -20;
338  tSetpoints.push_back(tline);
339 
340  tline[0] = 0.575 * period;
341  tline[1 + LHIP] = 0; tline[1 + RHIP] = 0;
342  tline[1 + EYES] = 0;
343  tline[1 + RARM] = 0; tline[1 + LARM] = 0;
344  tSetpoints.push_back(tline);
345 
346  tline[0] = 0.7 * period;
347  tline[1 + LKNEE] = WKSMALL;
348  tSetpoints.push_back(tline);
349 
350  tline[0] = period;
351  tline[1 + LKNEE] = 0; tline[1 + RKNEE] = 0;
352  tSetpoints.push_back(tline);
353 
354  interpTrajectory(tSetpoints, tInterp, INTERP_DT);
355 
356  return tInterp;
357 }
358 
359 data_t Trajectory::genKickRight(MartyCore* robot, float period) {
360  data_t tSetpoints, tInterp;
361  deque<float> tline(robot->jangles_);
362  tline.push_front(0.0);
363  tSetpoints.push_back(tline);
364 
365  tline[0] = 0.2 * period;
366  tline[1 + RKNEE] = -WKSMALL; tline[1 + LKNEE] = -WKSMALL;
367  tline[1 + RARM] = 20; tline[1 + LARM] = 20;
368  tSetpoints.push_back(tline);
369 
370  tline[0] = 0.375 * period;
371  tline[1 + RKNEE] = -WKLARGE;
372  tline[1 + RHIP] = 10;
373  tSetpoints.push_back(tline);
374 
375  tline[0] = 0.425 * period;
376  tline[1 + RHIP] = -120;
377  tline[1 + EYES] = 30;
378  tline[1 + RARM] = -20; tline[1 + LARM] = -20;
379  tSetpoints.push_back(tline);
380 
381  tline[0] = 0.575 * period;
382  tline[1 + LHIP] = 0; tline[1 + RHIP] = 0;
383  tline[1 + EYES] = 0;
384  tline[1 + RARM] = 0; tline[1 + LARM] = 0;
385  tSetpoints.push_back(tline);
386 
387  tline[0] = 0.7 * period;
388  tline[1 + RKNEE] = -WKSMALL;
389  tSetpoints.push_back(tline);
390 
391  tline[0] = period;
392  tline[1 + LKNEE] = 0; tline[1 + RKNEE] = 0;
393  tSetpoints.push_back(tline);
394 
395  interpTrajectory(tSetpoints, tInterp, INTERP_DT);
396  return tInterp;
397 
398 }
399 
400 data_t Trajectory::genGetUp(MartyCore* robot) {
401  data_t tSetpoints, tInterp;
402  deque<float> tline(robot->jangles_);
403  tline.push_front(0.0);
404  tSetpoints.push_back(tline);
405 
406  for (std::deque<float>::iterator di = tline.begin(); di != tline.end(); di++)
407  *di = 0;
408  tline[0] = 1.0;
409  tSetpoints.push_back(tline);
410 
411  tline[0] = 2.5;
412  tline[1 + LARM] = -200;
413  tline[1 + RARM] = 0;
414  tSetpoints.push_back(tline);
415 
416 
417  tline[0] = 4.0;
418  tline[1 + LHIP] = -150; tline[1 + RHIP] = -150;
419  tSetpoints.push_back(tline);
420 
421  tline[0] = 6.0;
422  tline[1 + LKNEE] = 125; tline[1 + RKNEE] = -125;
423  tSetpoints.push_back(tline);
424 
425  tline[0] = 8.0;
426  tline[1 + RTWIST] = -85; tline[1 + LTWIST] = 85;
427  tSetpoints.push_back(tline);
428 
429  tline[0] = 10.0;
430  tline[1 + RARM] = 250;
431  tSetpoints.push_back(tline);
432 
433  tline[0] += 2.0;
434  tline[1 + RTWIST] = -170; tline[1 + LTWIST] = 0;
435  tSetpoints.push_back(tline);
436 
437  tline[0] += 2.0;
438  tline[1 + LHIP] = 150;
439  tSetpoints.push_back(tline);
440 
441  tline[0] += 2.0;
442  tline[1 + RARM] = 80;
443  tSetpoints.push_back(tline);
444 
445  tline[0] += 2.0;
446  tline[1 + RTWIST] = 0;
447  tSetpoints.push_back(tline);
448 
449  tline[0] += 2.0;
450  tline[1 + RHIP] = 0; tline[1 + LHIP] = 0;
451  tSetpoints.push_back(tline);
452 
453  tline[0] += 2.0;
454  tline[1 + RKNEE] = 0; tline[1 + LKNEE] = 0;
455  tSetpoints.push_back(tline);
456 
457  interpTrajectory(tSetpoints, tInterp, INTERP_DT);
458 
459  return tInterp;
460 
461 }
462 
463 data_t Trajectory::genReturnToZero(MartyCore* robot, float period) {
464  data_t tSetpoints, tInterp;
465  deque<float> tline(robot->jangles_);
466  tline.push_front(0.0);
467  tSetpoints.push_back(tline);
468 
469  // set all joint angles to zero
470  for (std::deque<float>::iterator di = tline.begin(); di != tline.end(); di++)
471  *di = 0;
472  tline[0] = period;
473  tSetpoints.push_back(tline);
474  interpTrajectory(tSetpoints, tInterp, INTERP_DT);
475  return tInterp;
476 }
477 
478 bool Trajectory::setPointsLeanLeft(data_t& tSetpoints, int leanAmount,
479  float period) {
480  deque <float> tline(tSetpoints.back());
481 
482  tline[0] += period;
483  //tline[1+RHIP] = 0; tline[1+LHIP] = 0;
484  tline[1 + LKNEE] = -leanAmount; tline[1 + RKNEE] = -leanAmount;
485  tline[1 + LTWIST] = 0; tline[1 + RTWIST] = 0;
486  tSetpoints.push_back(tline);
487 
488  return 1;
489 }
490 
491 bool Trajectory::setPointsLeanRight(data_t& tSetpoints, int leanAmount,
492  float period) {
493  deque <float> tline(tSetpoints.back());
494 
495  tline[0] += period;
496  //tline[1+RHIP] = 0; tline[1+LHIP] = 0;
497  tline[1 + LKNEE] = leanAmount; tline[1 + RKNEE] = leanAmount;
498  tline[1 + LTWIST] = 0; tline[1 + RTWIST] = 0;
499  tSetpoints.push_back(tline);
500 
501  return 1;
502 }
503 
504 bool Trajectory::setPointsLeanForward(data_t& tSetpoints, int leanAmount,
505  float period) {
506  deque <float> tline(tSetpoints.back());
507 
508  tline[0] += period;
509  tline[1 + RHIP] = leanAmount; tline[1 + LHIP] = leanAmount;
510  tSetpoints.push_back(tline);
511 
512  return 1;
513 }
514 
515 bool Trajectory::setPointsLeanBackward(data_t& tSetpoints, int leanAmount,
516  float period) {
517  deque <float> tline(tSetpoints.back());
518 
519  tline[0] += period;
520  tline[1 + RHIP] = -leanAmount; tline[1 + LHIP] = -leanAmount;
521  tSetpoints.push_back(tline);
522 
523  return 1;
524 }
525 
526 bool Trajectory::setPointsLegsZero(data_t& tSetpoints, float period) {
527  deque <float> tline(tSetpoints.back());
528 
529  tline[0] += period;
530  tline[1 + RHIP] = 0; tline[1 + LHIP] = 0;
531  tline[1 + LKNEE] = 0; tline[1 + RKNEE] = 0;
532  tline[1 + LTWIST] = 0; tline[1 + RTWIST] = 0;
533  tSetpoints.push_back(tline);
534 
535  return 1;
536 }
537 
538 bool Trajectory::setPointsCrossLeftLeg(data_t& tSetpoints, float period) {
539  deque <float> tline(tSetpoints.back());
540 
541  tline[0] += period;
542  tline[1 + LHIP] = -75; tline[1 + RHIP] = 75;
543  tline[1 + LKNEE] = 20; tline[1 + RKNEE] = -20;
544  tline[1 + RTWIST] = -40; tline[1 + LTWIST] = 80;
545  tSetpoints.push_back(tline);
546 
547  return 1;
548 }
549 
550 bool Trajectory::setPointsCrossRightLeg(data_t& tSetpoints, float period) {
551  deque <float> tline(tSetpoints.back());
552 
553  tline[0] += period;
554  tline[1 + LHIP] = 75; tline[1 + RHIP] = -75;
555  tline[1 + LKNEE] = 20; tline[1 + RKNEE] = -20;
556  tline[1 + LTWIST] = 40; tline[1 + RTWIST] = -80;
557  tSetpoints.push_back(tline);
558 
559  return 1;
560 }
561 
562 bool Trajectory::setPointsLegsApart(data_t& tSetpoints, float period) {
563  deque <float> tline(tSetpoints.back());
564 
565  tline[0] += period;
566  tline[1 + LKNEE] = 60; tline[1 + RKNEE] = -60;
567  tline[1 + LTWIST] = 0; tline[1 + RTWIST] = 0;
568  tSetpoints.push_back(tline);
569 
570  return 1;
571 }
572 
573 bool Trajectory::setPointsKickOutLeft(data_t& tSetpoints, float period) {
574  deque <float> tline(tSetpoints.back());
575  float startTime = tline[0];
576 
577  tline[0] = startTime + 0.2 * period;
578  tline[1 + RKNEE] = WKSMALL; tline[1 + LKNEE] = WKSMALL;
579  tline[1 + LTWIST] = 0; tline[1 + RTWIST] = 0;
580  tline[1 + RHIP] = 0; tline[1 + LHIP] = 0;
581  tSetpoints.push_back(tline);
582 
583  tline[0] = startTime + 0.275 * period;
584  tline[1 + LKNEE] = WKLARGE;
585  tSetpoints.push_back(tline);
586 
587  tline[0] = startTime + 0.5 * period;
588  tline[1 + RHIP] = 0;
589  tline[1 + LHIP] = -150;
590  tline[1 + LTWIST] = 80;
591  tSetpoints.push_back(tline);
592 
593  tline[0] = startTime + 0.675 * period;
594  tline[1 + LHIP] = 0;
595  tline[1 + RHIP] = 0;
596  tline[1 + LTWIST] = 0;
597  tSetpoints.push_back(tline);
598 
599  tline[0] = startTime + 0.725 * period;
600  tline[1 + LKNEE] = WKSMALL;
601  tSetpoints.push_back(tline);
602 
603  tline[0] = startTime + period;
604  tline[1 + LKNEE] = 0; tline[1 + RKNEE] = 0;
605  tSetpoints.push_back(tline);
606 
607  return 1;
608 }
609 
610 bool Trajectory::setPointsKickOutRight(data_t& tSetpoints, float period) {
611  deque <float> tline(tSetpoints.back());
612  float startTime = tline[0];
613 
614  tline[0] = startTime + 0.2 * period;
615  tline[1 + RKNEE] = -WKSMALL; tline[1 + LKNEE] = -WKSMALL;
616  tline[1 + LTWIST] = 0; tline[1 + RTWIST] = 0;
617  tline[1 + RHIP] = 0; tline[1 + LHIP] = 0;
618  tSetpoints.push_back(tline);
619 
620  tline[0] = startTime + 0.275 * period;
621  tline[1 + RKNEE] = -WKLARGE;
622  tSetpoints.push_back(tline);
623 
624  tline[0] = startTime + 0.5 * period;
625  tline[1 + LHIP] = 0;
626  tline[1 + RHIP] = -150;
627  tline[1 + RTWIST] = -80;
628  tSetpoints.push_back(tline);
629 
630  tline[0] = startTime + 0.675 * period;
631  tline[1 + LHIP] = 0;
632  tline[1 + RHIP] = 0;
633  tline[1 + RTWIST] = 0;
634  tSetpoints.push_back(tline);
635 
636  tline[0] = startTime + 0.725 * period;
637  tline[1 + RKNEE] = -WKSMALL;
638  tSetpoints.push_back(tline);
639 
640  tline[0] = startTime + period;
641  tline[1 + LKNEE] = 0; tline[1 + RKNEE] = 0;
642  tSetpoints.push_back(tline);
643 
644  return 1;
645 }
646 
647 bool Trajectory::setPointsFlickRight(data_t& tSetpoints, float period) {
648  deque <float> tline(tSetpoints.back());
649  float startTime = tline[0];
650 
651  tline[0] = startTime + 0.5 * period;
652  tline[1 + RKNEE] = -WKSMALL; tline[1 + LKNEE] = 0;
653  tline[1 + LTWIST] = 0; tline[1 + RTWIST] = 0;
654  tline[1 + RHIP] = -50; tline[1 + LHIP] = 0;
655  tSetpoints.push_back(tline);
656 
657  tline[0] = startTime + period;
658  tline[1 + RKNEE] = 0;
659  tline[1 + RHIP] = 0;
660  tSetpoints.push_back(tline);
661 
662  return 1;
663 }
664 
665 bool Trajectory::setPointsCircleACW(data_t& tSetpoints, float period) {
666  deque <float> tline(tSetpoints.back());
667  float startTime = tline[0];
668 
669  tline[0] = startTime + 0.25 * period;
670  tline[1 + RKNEE] = -WKSMALL; tline[1 + LKNEE] = -WKSMALL;
671  tline[1 + LTWIST] = 0; tline[1 + RTWIST] = 0;
672  tline[1 + RHIP] = 0; tline[1 + LHIP] = 0;
673  tSetpoints.push_back(tline);
674 
675  tline[0] = startTime + 0.5 * period;
676  tline[1 + RKNEE] = 0; tline[1 + LKNEE] = 0;
677  tline[1 + RHIP] = 60; tline[1 + LHIP] = 60;
678  tSetpoints.push_back(tline);
679 
680  tline[0] = startTime + 0.75 * period;
681  tline[1 + RKNEE] = WKSMALL; tline[1 + LKNEE] = WKSMALL;
682  tline[1 + RHIP] = 0; tline[1 + LHIP] = 0;
683  tSetpoints.push_back(tline);
684 
685  tline[0] = startTime + period;
686  tline[1 + RKNEE] = 0; tline[1 + LKNEE] = 0;
687  tline[1 + RHIP] = -60; tline[1 + LHIP] = -60;
688  tSetpoints.push_back(tline);
689 
690  return 1;
691 }
692 
693 bool Trajectory::setPointsCircleCW(data_t& tSetpoints, float period) {
694  deque <float> tline(tSetpoints.back());
695  float startTime = tline[0];
696 
697  tline[0] = startTime + 0.25 * period;
698  tline[1 + RKNEE] = WKSMALL; tline[1 + LKNEE] = WKSMALL;
699  tline[1 + LTWIST] = 0; tline[1 + RTWIST] = 0;
700  tline[1 + RHIP] = 0; tline[1 + LHIP] = 0;
701  tSetpoints.push_back(tline);
702 
703  tline[0] = startTime + 0.5 * period;
704  tline[1 + RKNEE] = 0; tline[1 + LKNEE] = 0;
705  tline[1 + RHIP] = 60; tline[1 + LHIP] = 60;
706  tSetpoints.push_back(tline);
707 
708  tline[0] = startTime + 0.75 * period;
709  tline[1 + RKNEE] = -WKSMALL; tline[1 + LKNEE] = -WKSMALL;
710  tline[1 + RHIP] = 0; tline[1 + LHIP] = 0;
711  tSetpoints.push_back(tline);
712 
713  tline[0] = startTime + period;
714  tline[1 + RKNEE] = 0; tline[1 + LKNEE] = 0;
715  tline[1 + RHIP] = -60; tline[1 + LHIP] = -60;
716  tSetpoints.push_back(tline);
717 
718  return 1;
719 }
720 
721 bool Trajectory::setPointsTapFR(data_t& tSetpoints, float period) {
722  deque <float> tline(tSetpoints.back());
723  float startTime = tline[0];
724 
725  tline[0] = startTime + 0.25 * period;
726  tline[1 + LKNEE] = -WKSMALL; tline[1 + RKNEE] = -WKLARGE;
727  tline[1 + LHIP] = 0; tline[1 + RHIP] = 0;
728  tSetpoints.push_back(tline);
729 
730  tline[0] = startTime + 0.75 * period;
731  tline[1 + RHIP] = -WKSMALL;
732  tSetpoints.push_back(tline);
733 
734  tline[0] = startTime + period;
735  tline[1 + RKNEE] = 0;
736  tSetpoints.push_back(tline);
737 
738  return 1;
739 }
740 
741 bool Trajectory::setPointsTapMR(data_t& tSetpoints, float period) {
742  deque <float> tline(tSetpoints.back());
743  float startTime = tline[0];
744 
745  tline[0] = startTime + 0.25 * period;
746  tline[1 + LKNEE] = -WKSMALL; tline[1 + RKNEE] = -WKLARGE;
747  tline[1 + LHIP] = 0;
748  tSetpoints.push_back(tline);
749 
750  tline[0] = startTime + 0.75 * period;
751  tline[1 + RHIP] = 0;
752  tSetpoints.push_back(tline);
753 
754  tline[0] = startTime + period;
755  tline[1 + RKNEE] = -WKSMALL;
756  tSetpoints.push_back(tline);
757 
758  return 1;
759 }
760 
761 bool Trajectory::setPointsTapBR(data_t& tSetpoints, float period) {
762  deque <float> tline(tSetpoints.back());
763  float startTime = tline[0];
764 
765  tline[0] = startTime + 0.25 * period;
766  tline[1 + LKNEE] = -WKSMALL; tline[1 + RKNEE] = -WKLARGE;
767  tline[1 + LHIP] = 0; tline[1 + RHIP] = 0;
768  tSetpoints.push_back(tline);
769 
770  tline[0] = startTime + 0.75 * period;
771  tline[1 + RHIP] = WKSMALL;
772  tSetpoints.push_back(tline);
773 
774  tline[0] = startTime + period;
775  tline[1 + RKNEE] = 0;
776  tSetpoints.push_back(tline);
777 
778  return 1;
779 }
780 
781 bool Trajectory::setPointsTapFL(data_t& tSetpoints, float period) {
782  deque <float> tline(tSetpoints.back());
783  float startTime = tline[0];
784 
785  tline[0] = startTime + 0.25 * period;
786  tline[1 + RKNEE] = WKSMALL; tline[1 + LKNEE] = WKLARGE;
787  tline[1 + LHIP] = 0; tline[1 + RHIP] = 0;
788  tSetpoints.push_back(tline);
789 
790  tline[0] = startTime + 0.75 * period;
791  tline[1 + LHIP] = -WKSMALL;
792  tSetpoints.push_back(tline);
793 
794  tline[0] = startTime + period;
795  tline[1 + LKNEE] = 0;
796  tSetpoints.push_back(tline);
797 
798  return 1;
799 }
800 
801 bool Trajectory::setPointsTapML(data_t& tSetpoints, float period) {
802  deque <float> tline(tSetpoints.back());
803  float startTime = tline[0];
804 
805  tline[0] = startTime + 0.25 * period;
806  tline[1 + RKNEE] = WKSMALL; tline[1 + LKNEE] = WKLARGE;
807  tline[1 + RHIP] = 0;
808  tSetpoints.push_back(tline);
809 
810  tline[0] = startTime + 0.75 * period;
811  tline[1 + LHIP] = 0;
812  tSetpoints.push_back(tline);
813 
814  tline[0] = startTime + period;
815  tline[1 + LKNEE] = WKSMALL;
816  tSetpoints.push_back(tline);
817 
818  return 1;
819 }
820 
821 bool Trajectory::setPointsTapBL(data_t& tSetpoints, float period) {
822  deque <float> tline(tSetpoints.back());
823  float startTime = tline[0];
824 
825  tline[0] = startTime + 0.25 * period;
826  tline[1 + RKNEE] = WKSMALL; tline[1 + LKNEE] = WKLARGE;
827  tline[1 + LHIP] = 0; tline[1 + RHIP] = 0;
828  tSetpoints.push_back(tline);
829 
830  tline[0] = startTime + 0.75 * period;
831  tline[1 + LHIP] = WKSMALL;
832  tSetpoints.push_back(tline);
833 
834  tline[0] = startTime + period;
835  tline[1 + LKNEE] = 0;
836  tSetpoints.push_back(tline);
837 
838  return 1;
839 }
840 
841 
842 bool Trajectory::setPointsArmsUp(data_t& tSetpoints, float amountRight,
843  float amountLeft,
844  float period) {
845  deque <float> tline(tSetpoints.back());
846  float startTime = tline[0];
847 
848  tline[0] = startTime + period;
849  tline[1 + RARM] = amountRight; tline[1 + LARM] = amountLeft;
850  tSetpoints.push_back(tline);
851 
852  return 1;
853 }
854 
855 bool Trajectory::setPointsLeftArmUp(data_t& tSetpoints, float amount,
856  float period) {
857  deque <float> tline(tSetpoints.back());
858  float startTime = tline[0];
859 
860  tline[0] = startTime + period;
861  tline[1 + LARM] = amount;
862  tSetpoints.push_back(tline);
863 
864  return 1;
865 }
866 
867 bool Trajectory::setPointsRightArmUp(data_t& tSetpoints, float amount,
868  float period) {
869  deque <float> tline(tSetpoints.back());
870  float startTime = tline[0];
871 
872  tline[0] = startTime + period;
873  tline[1 + RARM] = amount;
874  tSetpoints.push_back(tline);
875 
876  return 1;
877 }
878 
879 bool Trajectory::setPointsEyes(data_t& tSetpoints, float targetPos,
880  float period) {
881  deque <float> tline(tSetpoints.back());
882  float startTime = tline[0];
883 
884  tline[0] = startTime + period;
885  tline[1 + EYES] = targetPos;
886  tSetpoints.push_back(tline);
887 
888  return 1;
889 }
890 
891 // function to combine some joints from one trajectory with some joints from another
892 // e.g. for combining leg and arm movements
893 // treats the time code in the same way as a joint
894 // i.e. THE dt FOR BOTH TRAJECTORIES MUST BE THE SAME!
895 data_t Trajectory::combineTrajectories(data_t& t1, data_t& t2,
896  vector<bool> which) {
897  data_t traj;
898  data_t::record_iterator ni2 = t2.begin();
899  for (data_t::record_iterator ni1 = t1.begin(); ni1 != t1.end(); ni1++) {
900  deque<float> tline;
901  for (unsigned int ji = 0; ji < which.size(); ji++) {
902  float tfield = !which[ji] ? (*ni1)[ji] : (*ni2)[ji];
903  tline.push_back(tfield);
904  }
905  traj.push_back(tline);
906  if (ni2 != t2.end())
907  ni2++;
908  if (ni2 == t2.end())
909  ni2--;
910  }
911  return traj;
912 }
913 
914 data_t Trajectory::combineLegsArmsEyes(data_t& legs, data_t& arms,
915  data_t& eyes) {
916  data_t tlegsArms, genTraj;
917  vector<bool> ji(1 + NUMJOINTS, 0);
918  ji[1 + RARM] = 1; ji[1 + LARM] = 1;
919  tlegsArms = combineTrajectories(legs, arms, ji);
920  ji[1 + RARM] = 0; ji[1 + LARM] = 0;
921  ji[1 + EYES] = 1;
922  genTraj = combineTrajectories(tlegsArms, eyes, ji);
923  return genTraj;
924 }
925 
926 
927 int Trajectory::hipToBeSquare(MartyCore* robot, int robotID) {
928 #define BEAT 0.4255
929 
930  printf("starting\n");
931 
932  data_t tSetpoints, genTraj;
933 
934  data_t tLegs, tArms, tEyes;
935  data_t tiLegs, tiArms, tiEyes;
936  deque<float> tline(robot->jangles_);
937  tline.push_front(0);
938  tLegs.push_back(tline);
939  tArms.push_back(tline);
940  tEyes.push_back(tline);
941 
942  int polarity = 0;
943  if (robotID == 0) {
944  polarity = -1;
945  } else if (robotID == 2) {
946  polarity = 1;
947  }
948 
949  // leaning forward and backward
950  for (int i = 0; i < 3; i++) {
951  setPointsLeanForward(tLegs, 0, BEAT / 2);
952  setPointsArmsUp(tArms, 0, 0, BEAT / 2);
953  setPointsEyes(tEyes, 0, BEAT / 2);
954 
955  setPointsLeanForward(tLegs, 45 * polarity, BEAT / 2);
956  setPointsArmsUp(tArms, -200, 200, BEAT / 2);
957  setPointsEyes(tEyes, 30 * polarity, BEAT / 2);
958  setPointsLeanForward(tLegs, 45 * polarity, BEAT);
959  setPointsArmsUp(tArms, -200, 200, BEAT);
960  setPointsEyes(tEyes, 30 * polarity, BEAT);
961 
962  setPointsLeanForward(tLegs, 0, BEAT / 2);
963  setPointsArmsUp(tArms, 0, 0, BEAT / 2);
964  setPointsEyes(tEyes, 0, BEAT / 2);
965  setPointsLeanForward(tLegs, -45 * polarity, BEAT / 2);
966  setPointsArmsUp(tArms, 200, -200, BEAT / 2);
967  setPointsEyes(tEyes, -30 * polarity, BEAT / 2);
968  setPointsLeanForward(tLegs, -45 * polarity, BEAT);
969  setPointsArmsUp(tArms, 200, -200, BEAT);
970  setPointsEyes(tEyes, -30 * polarity, BEAT);
971  }
972 
973  // staggered center, with eyes wide, then eyes together all at the same time
974  if (robotID == 0) {
975  setPointsLeanForward(tLegs, 0, BEAT);
976  setPointsEyes(tEyes, 30, BEAT);
977  setPointsLeanForward(tLegs, 0, BEAT * 2);
978  setPointsEyes(tEyes, 30, BEAT * 2);
979  setPointsArmsUp(tArms, 0, 0, BEAT * 3);
980  } else if (robotID == 1) {
981  setPointsLeanForward(tLegs, -45 * polarity, BEAT);
982  setPointsEyes(tEyes, 0, BEAT);
983  setPointsArmsUp(tArms, -200 * polarity, 200 * polarity, BEAT);
984  setPointsLeanForward(tLegs, 0, BEAT);
985  setPointsEyes(tEyes, 30, BEAT);
986  setPointsArmsUp(tArms, 0, 0, BEAT);
987  setPointsLeanForward(tLegs, 0, BEAT);
988  setPointsEyes(tEyes, 30, BEAT);
989  setPointsArmsUp(tArms, 0, 0, BEAT);
990  } else if (robotID == 2) {
991  setPointsLeanForward(tLegs, -45 * polarity, BEAT * 2);
992  setPointsEyes(tEyes, 30, BEAT * 2);
993  setPointsLeanForward(tLegs, 0, BEAT);
994  setPointsEyes(tEyes, 30, BEAT);
995  setPointsArmsUp(tArms, 0, 0, BEAT * 3);
996  }
997  setPointsLeanForward(tLegs, 0, BEAT);
998  setPointsEyes(tEyes, 0, BEAT);
999  setPointsArmsUp(tArms, 0, 0, BEAT);
1000 
1001  // lean left, pause, lean right, pause
1002  for (int i = 0; i < 3; i++) {
1003  // center
1004  setPointsLeanLeft(tLegs, 0, BEAT / 2);
1005  setPointsArmsUp(tArms, 0, 0, BEAT / 2);
1006  // lean left and pause
1007  setPointsLeanLeft(tLegs, 45, BEAT / 2);
1008  setPointsArmsUp(tArms, 0, 200, BEAT / 2);
1009  setPointsLeanLeft(tLegs, 45, BEAT);
1010  setPointsArmsUp(tArms, 0, 200, BEAT);
1011 
1012  //lean right and pause
1013  setPointsLeanLeft(tLegs, 0, BEAT / 2);
1014  setPointsArmsUp(tArms, 0, 0, BEAT / 2);
1015  setPointsLeanRight(tLegs, 45, BEAT / 2);
1016  setPointsArmsUp(tArms, 200, 0, BEAT / 2);
1017  setPointsLeanRight(tLegs, 45, BEAT);
1018  setPointsArmsUp(tArms, 200, 0, BEAT);
1019  }
1020  setPointsEyes(tEyes, 0, BEAT * 12);
1021 
1022  // staggered centering, then all lean left at same time
1023  if (robotID == 0) {
1024  setPointsLeanLeft(tLegs, 0, BEAT);
1025  setPointsLeanLeft(tLegs, 0, BEAT * 2);
1026  } else if (robotID == 1) {
1027  setPointsLeanRight(tLegs, 45, BEAT);
1028  setPointsLeanLeft(tLegs, 0, BEAT);
1029  setPointsLeanLeft(tLegs, 0, BEAT);
1030  } else if (robotID == 2) {
1031  setPointsLeanRight(tLegs, 45, BEAT * 2);
1032  setPointsLeanLeft(tLegs, 0, BEAT);
1033  }
1034  setPointsLeanLeft(tLegs, 45, BEAT);
1035  setPointsArmsUp(tArms, 0, 0, BEAT * 4);
1036  setPointsEyes(tEyes, 0, BEAT * 4);
1037 
1038  //
1039 
1040 
1041  interpTrajectory(tLegs, tiLegs, INTERP_DT);
1042  interpTrajectory(tArms, tiArms, INTERP_DT);
1043  interpTrajectory(tEyes, tiEyes, INTERP_DT);
1044  genTraj = combineLegsArmsEyes(tiLegs, tiArms, tiEyes);
1045 
1046  runTrajectory(robot, genTraj);
1047 
1048  tline = genTraj.back();
1049  tline[0] = 0;
1050 
1051  tLegs.clear();
1052  tArms.clear();
1053  tLegs.push_back(tline);
1054  tArms.push_back(tline);
1055  tiArms.clear();
1056  tiLegs.clear();
1057  genTraj.clear();
1058 
1059  setPointsTapFR(tLegs, BEAT * 2);
1060  setPointsTapMR(tLegs, BEAT * 2);
1061  setPointsTapBR(tLegs, BEAT * 2);
1062  setPointsTapMR(tLegs, BEAT * 2);
1063  setPointsTapFR(tLegs, BEAT * 2);
1064  setPointsTapMR(tLegs, BEAT * 2);
1065  setPointsTapMR(tLegs, BEAT * 2);
1066  setPointsLeanLeft(tLegs, 0, BEAT);
1067  setPointsLeanRight(tLegs, 45, BEAT);
1068  setPointsTapFL(tLegs, BEAT * 2);
1069  setPointsTapML(tLegs, BEAT * 2);
1070  setPointsTapBL(tLegs, BEAT * 2);
1071  setPointsTapML(tLegs, BEAT * 2);
1072  setPointsTapFL(tLegs, BEAT * 2);
1073  setPointsTapML(tLegs, BEAT * 2);
1074  setPointsTapML(tLegs, BEAT * 2);
1075  setPointsLeanLeft(tLegs, 0, BEAT);
1076  setPointsLeanLeft(tLegs, 0, BEAT);
1077 
1078  interpTrajectory(tLegs, tiLegs, INTERP_DT);
1079  runTrajectory(robot, tiLegs);
1080 
1081  tline = tLegs.back();
1082  tline[0] = 0;
1083 
1084  tLegs.clear();
1085  tArms.clear();
1086  tLegs.push_back(tline);
1087  tArms.push_back(tline);
1088  tiArms.clear();
1089  tiLegs.clear();
1090  genTraj.clear();
1091 
1092  for (int i = 0; i < 2; i++) {
1093  setPointsLeanLeft(tLegs, 45, BEAT * 4);
1094  setPointsArmsUp(tArms, 0, 200, BEAT * 4);
1095  setPointsLeanRight(tLegs, 45, BEAT * 4);
1096  setPointsArmsUp(tArms, 200, 0, BEAT * 4);
1097  setPointsLeanLeft(tLegs, 45, BEAT * 2);
1098  setPointsArmsUp(tArms, 0, 200, BEAT * 2);
1099  setPointsLeanRight(tLegs, 45, BEAT * 2);
1100  setPointsArmsUp(tArms, 200, 0, BEAT * 2);
1101  if (robotID == 0) {
1102  setPointsKickOutLeft(tLegs, BEAT * 4);
1103  } else if (robotID == 1) {
1104  setPointsLeanBackward(tLegs, 45, BEAT * 3);
1105  setPointsLeanBackward(tLegs, 0, BEAT);
1106  } else {
1107  setPointsKickOutRight(tLegs, BEAT * 4);
1108  }
1109 
1110  setPointsArmsUp(tArms, 0, 0, BEAT * 4);
1111 
1112  //for (int j=0; j<2; j++){
1113  // setPointsLeanLeft(tLegs, 45, BEAT);
1114  // setPointsArmsUp(tArms, 0, 200, BEAT);
1115  // setPointsLeanRight(tLegs, 45, BEAT);
1116  // setPointsArmsUp(tArms, 200, 0, BEAT);
1117  //}
1118  }
1119 
1120  for (int i = 0; i < 4; i++) {
1121  setPointsCircleCW(tLegs, BEAT * 4);
1122  setPointsArmsUp(tArms, 0, 0, BEAT * 4);
1123  }
1124 
1125  setPointsLegsZero(tLegs, BEAT);
1126  setPointsArmsUp(tArms, 0, 0, BEAT);
1127 
1128  vector<bool> ji(1 + NUMJOINTS, 0);
1129  ji[1 + RARM] = 1; ji[1 + LARM] = 1;
1130  interpTrajectory(tLegs, tiLegs, INTERP_DT);
1131  interpTrajectory(tArms, tiArms, INTERP_DT);
1132  genTraj = combineTrajectories(tiLegs, tiArms, ji);
1133 
1134  runTrajectory(robot, genTraj);
1135 
1136  return 1;
1137 }
1138 
1139 bool Trajectory::setPointsSkateLeft(data_t& tSetpoints, float amount,
1140  float period) {
1141  deque <float> tline(tSetpoints.back());
1142  float startTime = tline[0];
1143 
1144  // lean right
1145  tline[0] = startTime + 0.2 * period;
1146  tline[1 + RKNEE] = 55; tline[1 + LKNEE] = 55;
1147  tline[1 + LTWIST] = 0; tline[1 + RTWIST] = 0;
1148  tline[1 + RHIP] = 0; tline[1 + LHIP] = 0;
1149  tSetpoints.push_back(tline);
1150 
1151  // lift left foot
1152  tline[0] = startTime + 0.275 * period;
1153  tline[1 + LKNEE] = 75;
1154  tSetpoints.push_back(tline);
1155 
1156  // right leg backward, left forward and twist
1157  tline[0] = startTime + 0.5 * period;
1158  tline[1 + RHIP] = -30;
1159  tline[1 + LHIP] = -50;
1160  tline[1 + LTWIST] = 30;
1161  tSetpoints.push_back(tline);
1162 
1163  // left leg swing back
1164  tline[0] = startTime + 0.675 * period;
1165  tline[1 + LHIP] = 0;
1166  //tline[1+RHIP] = 0;
1167  //tline[1+LTWIST] = 0;
1168  tSetpoints.push_back(tline);
1169 
1170  // left leg swing back, pt 2
1171  tline[0] = startTime + 0.725 * period;
1172  tline[1 + LHIP] = 50;
1173  tline[1 + RHIP] = 0;
1174  tline[1 + LTWIST] = 0;
1175  //tline[1+LKNEE] = 75;
1176  tSetpoints.push_back(tline);
1177 
1178  tline[0] = startTime + period;
1179  tline[1 + LHIP] = 0;
1180  tline[1 + LKNEE] = 0; tline[1 + RKNEE] = 0;
1181  tSetpoints.push_back(tline);
1182 
1183  return 1;
1184 }
1185 
1186 int Trajectory::rollerSkate(MartyCore* robot) {
1187  data_t tSetpoints, genTraj;
1188 
1189  data_t tLegs, tArms, tEyes;
1190  data_t tiLegs, tiArms, tiEyes;
1191  deque<float> tline(robot->jangles_);
1192  tline.push_front(0);
1193  tLegs.push_back(tline);
1194  tArms.push_back(tline);
1195  tEyes.push_back(tline);
1196 
1197  setPointsLegsZero(tLegs, 1.0);
1198  setPointsSkateLeft(tLegs, 0, 3.0);
1199 
1200  interpTrajectory(tLegs, tiLegs, INTERP_DT);
1201 
1202  runTrajectory(robot, tiLegs);
1203 
1204  return 1;
1205 }
MartyCore::setServos
void setServos(std::map< int, float > angles)
Definition: marty_core.cpp:604
Trajectory::printTrajectory
void printTrajectory(data_t &traj)
Definition: trajectory.cpp:50
Trajectory::setPointsKickOutLeft
bool setPointsKickOutLeft(data_t &tSetpoints, float period)
Definition: trajectory.cpp:573
Trajectory::setPointsLegsApart
bool setPointsLegsApart(data_t &tSetpoints, float period)
Definition: trajectory.cpp:562
Trajectory::setPointsLeanBackward
bool setPointsLeanBackward(data_t &tSetpoints, int leanAmount, float period)
Definition: trajectory.cpp:515
Trajectory::setPointsEyes
bool setPointsEyes(data_t &tSetpoints, float targetPos, float period)
Definition: trajectory.cpp:879
Trajectory::genGetUp
data_t genGetUp(MartyCore *robot)
Definition: trajectory.cpp:400
Trajectory::genStepLeft
data_t genStepLeft(MartyCore *robot, int stepLength, int turn, float period)
Definition: trajectory.cpp:83
MartyCore::jangles_
std::deque< float > jangles_
Definition: marty_core.hpp:94
Trajectory::setPointsTapFR
bool setPointsTapFR(data_t &tSetpoints, float period)
Definition: trajectory.cpp:721
data_t::record_iterator
deque< deque< float > >::iterator record_iterator
Definition: data_t.hpp:16
Trajectory::combineLegsArmsEyes
data_t combineLegsArmsEyes(data_t &legs, data_t &arms, data_t &eyes)
Definition: trajectory.cpp:914
Trajectory::setPointsCircleCW
bool setPointsCircleCW(data_t &tSetpoints, float period)
Definition: trajectory.cpp:693
Trajectory::setPointsLeanForward
bool setPointsLeanForward(data_t &tSetpoints, int leanAmount, float period)
Definition: trajectory.cpp:504
Trajectory::setPointsTapBL
bool setPointsTapBL(data_t &tSetpoints, float period)
Definition: trajectory.cpp:821
data_t
Definition: data_t.hpp:15
BEAT
#define BEAT
Trajectory::setPointsLeanLeft
bool setPointsLeanLeft(data_t &tSetpoints, int leanAmount, float period)
Definition: trajectory.cpp:478
Trajectory::setPointsTapML
bool setPointsTapML(data_t &tSetpoints, float period)
Definition: trajectory.cpp:801
Trajectory::setPointsRightArmUp
bool setPointsRightArmUp(data_t &tSetpoints, float amount, float period)
Definition: trajectory.cpp:867
WKSMALL
#define WKSMALL
Definition: trajectory.hpp:15
Trajectory::genStepRight
data_t genStepRight(MartyCore *robot, int stepLength, int turn, float period)
Definition: trajectory.cpp:141
Trajectory::genKickLeft
data_t genKickLeft(MartyCore *robot, float period)
Definition: trajectory.cpp:318
Trajectory::setPointsKickOutRight
bool setPointsKickOutRight(data_t &tSetpoints, float period)
Definition: trajectory.cpp:610
Trajectory::runTrajectory
bool runTrajectory(MartyCore *robot, data_t &traj)
Definition: trajectory.cpp:63
Trajectory::gettime
float gettime()
Definition: trajectory.cpp:10
Trajectory::setPointsFlickRight
bool setPointsFlickRight(data_t &tSetpoints, float period)
Definition: trajectory.cpp:647
Trajectory::setPointsCrossLeftLeg
bool setPointsCrossLeftLeg(data_t &tSetpoints, float period)
Definition: trajectory.cpp:538
Trajectory::setPointsLegsZero
bool setPointsLegsZero(data_t &tSetpoints, float period)
Definition: trajectory.cpp:526
Trajectory::setPointsCrossRightLeg
bool setPointsCrossRightLeg(data_t &tSetpoints, float period)
Definition: trajectory.cpp:550
Trajectory::genSitBack
data_t genSitBack(MartyCore *robot, float period)
Definition: trajectory.cpp:125
Trajectory::genCelebration
data_t genCelebration(MartyCore *robot, float move_time)
Definition: trajectory.cpp:184
Trajectory::setPointsCircleACW
bool setPointsCircleACW(data_t &tSetpoints, float period)
Definition: trajectory.cpp:665
Trajectory::setPointsTapBR
bool setPointsTapBR(data_t &tSetpoints, float period)
Definition: trajectory.cpp:761
Trajectory::interpTrajectory
bool interpTrajectory(data_t tIn, data_t &tOut, float dt)
Definition: trajectory.cpp:16
Trajectory::setPointsLeftArmUp
bool setPointsLeftArmUp(data_t &tSetpoints, float amount, float period)
Definition: trajectory.cpp:855
Trajectory::setPointsLeanRight
bool setPointsLeanRight(data_t &tSetpoints, int leanAmount, float period)
Definition: trajectory.cpp:491
Trajectory::hipToBeSquare
int hipToBeSquare(MartyCore *robot, int robotID)
Definition: trajectory.cpp:927
Trajectory::genKickRight
data_t genKickRight(MartyCore *robot, float period)
Definition: trajectory.cpp:359
Trajectory::rollerSkate
int rollerSkate(MartyCore *robot)
Definition: trajectory.cpp:1186
DEBUG_MODE
#define DEBUG_MODE
Definition: definitions.hpp:18
Trajectory::combineTrajectories
data_t combineTrajectories(data_t &t1, data_t &t2, vector< bool > ti)
Definition: trajectory.cpp:895
Trajectory::genRaisedFootTwistRight
data_t genRaisedFootTwistRight(MartyCore *robot, float period)
Definition: trajectory.cpp:271
Trajectory::setPointsTapFL
bool setPointsTapFL(data_t &tSetpoints, float period)
Definition: trajectory.cpp:781
Trajectory::genRaisedFootTwistLeft
data_t genRaisedFootTwistLeft(MartyCore *robot, float period)
Definition: trajectory.cpp:224
data_t::field_iterator
deque< float >::iterator field_iterator
Definition: data_t.hpp:17
Trajectory::setPointsArmsUp
bool setPointsArmsUp(data_t &tSetpoints, float amountRight, float amountLeft, float period)
Definition: trajectory.cpp:842
Trajectory::setPointsSkateLeft
bool setPointsSkateLeft(data_t &tSetpoints, float amount, float period)
Definition: trajectory.cpp:1139
INTERP_DT
#define INTERP_DT
Definition: trajectory.hpp:18
WKLARGE
#define WKLARGE
Definition: trajectory.hpp:16
Trajectory::setPointsTapMR
bool setPointsTapMR(data_t &tSetpoints, float period)
Definition: trajectory.cpp:741
Trajectory::genReturnToZero
data_t genReturnToZero(MartyCore *robot, float period)
Definition: trajectory.cpp:463

ros_marty
Author(s): Alejandro Bordallo
autogenerated on Fri Jul 21 2017 14:51:45