Fawkes API Fawkes Development Version
motion_standup_task.cpp
1
2/***************************************************************************
3 * motion_standup_task.cpp - Task for making the robot stand up
4 *
5 * Created: Mon Jan 19 14:18:40 2009
6 * Copyright 2009-2011 Tim Niemueller [www.niemueller.de]
7 *
8 ****************************************************************************/
9
10/* This program is free software; you can redistribute it and/or modify
11 * it under the terms of the GNU General Public License as published by
12 * the Free Software Foundation; either version 2 of the License, or
13 * (at your option) any later version.
14 *
15 * This program is distributed in the hope that it will be useful,
16 * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 * GNU Library General Public License for more details.
19 *
20 * Read the full text in the LICENSE.GPL file in the doc directory.
21 */
22
23#include "motion_standup_task.h"
24
25#include <core/exceptions/system.h>
26
27#include <cstdlib>
28#include <cstring>
29#include <string>
30
31using namespace AL;
32using namespace fawkes;
33
34/** @class NaoQiMotionStandupTask "motion_standup_task.h"
35 * NaoQi standup task.
36 * This task can be used to make the robot standup ina non-blocking way. It will
37 * use (blocking) ALMotion calls to execute the move. Note that ALMotion should
38 * not be used otherwise.
39 * @author Tim Niemueller
40 */
41
42/** Constructor.
43 * @param almotion ALMotion proxy
44 * @param from_pos position from which to standup
45 * @param accel_x current accelerometer value
46 * @param accel_y current accelerometer value
47 * @param accel_z current accelerometer value
48 */
50 AL::ALPtr<AL::ALMotionProxy> almotion,
52 float accel_x,
53 float accel_y,
54 float accel_z)
55{
56 almotion_ = almotion;
57 from_pos_ = from_pos;
58 accel_x_ = accel_x;
59 accel_y_ = accel_y;
60 accel_z_ = accel_z;
61
62 // ALTask variable to cause auto-destruct when done
63 fAutoDelete = true;
64}
65
66/** Destructor. */
68{
69}
70
71#define _t(x) times.arrayPush(x)
72#define _tc() times.clear()
73#define _tp() all_times.arrayPush(times)
74
75#define _a(x) angles.arrayPush(x)
76#define _ac() angles.clear()
77#define _ap() all_angles.arrayPush(angles)
78
79void
80NaoQiMotionStandupTask::goto_start_pos()
81{
82 ALValue joints, angles, times, all_angles, all_times;
83
84 joints.arrayPush("HeadPitch");
85 joints.arrayPush("HeadYaw");
86 joints.arrayPush("LAnklePitch");
87 joints.arrayPush("LAnkleRoll");
88 joints.arrayPush("LElbowRoll");
89 joints.arrayPush("LElbowYaw");
90 joints.arrayPush("LHipPitch");
91 joints.arrayPush("LHipRoll");
92 joints.arrayPush("LHipYawPitch");
93 joints.arrayPush("LKneePitch");
94 joints.arrayPush("LShoulderPitch");
95 joints.arrayPush("LShoulderRoll");
96 joints.arrayPush("RAnklePitch");
97 joints.arrayPush("RAnkleRoll");
98 joints.arrayPush("RElbowRoll");
99 joints.arrayPush("RElbowYaw");
100 joints.arrayPush("RHipPitch");
101 joints.arrayPush("RHipRoll");
102 joints.arrayPush("RKneePitch");
103 joints.arrayPush("RShoulderPitch");
104 joints.arrayPush("RShoulderRoll");
105
106 times = ALValue::array(1.9, 2.9);
107
108 for (unsigned int i = 0; i < joints.getSize(); ++i) {
109 all_times.arrayPush(times);
110 }
111
112 _ac();
113 _a(0);
114 _a(0);
115 _ap();
116 _ac();
117 _a(3.37175e-007);
118 _a(3.37175e-007);
119 _ap();
120 _ac();
121 _a(0.523599);
122 _a(0.523599);
123 _ap();
124 _ac();
125 _a(0);
126 _a(0);
127 _ap();
128 _ac();
129 _a(0);
130 _a(0);
131 _ap();
132 _ac();
133 _a(0);
134 _a(0);
135 _ap();
136 _ac();
137 _a(0);
138 _a(0);
139 _ap();
140 _ac();
141 _a(0);
142 _a(0);
143 _ap();
144 _ac();
145 _a(0);
146 _a(0);
147 _ap();
148 _ac();
149 _a(0);
150 _a(0);
151 _ap();
152 _ac();
153 _a(0);
154 _a(0);
155 _ap();
156 _ac();
157 _a(1.5708);
158 _a(1.5708);
159 _ap();
160 _ac();
161 _a(0.523599);
162 _a(0.523599);
163 _ap();
164 _ac();
165 _a(0);
166 _a(0);
167 _ap();
168 _ac();
169 _a(0);
170 _a(0);
171 _ap();
172 _ac();
173 _a(0);
174 _a(0);
175 _ap();
176 _ac();
177 _a(0);
178 _a(0);
179 _ap();
180 _ac();
181 _a(0);
182 _a(0);
183 _ap();
184 _ac();
185 _a(0);
186 _a(0);
187 _ap();
188 _ac();
189 _a(0);
190 _a(0);
191 _ap();
192 _ac();
193 _a(-1.5708);
194 _a(-1.5708);
195 _ap();
196
197 bool is_absolute = true;
198 almotion_->angleInterpolation(joints, all_angles, all_times, is_absolute);
199}
200
201void
202NaoQiMotionStandupTask::standup_from_back()
203{
204 ALValue joints, angles, times, all_angles, all_times;
205
206 goto_start_pos();
207
208 joints.arrayPush("HeadPitch");
209 joints.arrayPush("HeadYaw");
210 joints.arrayPush("LAnklePitch");
211 joints.arrayPush("LAnkleRoll");
212 joints.arrayPush("LElbowRoll");
213 joints.arrayPush("LElbowYaw");
214 joints.arrayPush("LHipPitch");
215 joints.arrayPush("LHipRoll");
216 joints.arrayPush("LHipYawPitch");
217 joints.arrayPush("LKneePitch");
218 joints.arrayPush("LShoulderPitch");
219 joints.arrayPush("LShoulderRoll");
220 joints.arrayPush("RAnklePitch");
221 joints.arrayPush("RAnkleRoll");
222 joints.arrayPush("RElbowRoll");
223 joints.arrayPush("RElbowYaw");
224 joints.arrayPush("RHipPitch");
225 joints.arrayPush("RHipRoll");
226 joints.arrayPush("RHipYawPitch");
227 joints.arrayPush("RKneePitch");
228 joints.arrayPush("RShoulderPitch");
229 joints.arrayPush("RShoulderRoll");
230
231 _ac();
232 _a(0);
233 _a(0);
234 _a(-0.785398);
235 _a(0);
236 _a(0.349066);
237 _a(0.328232);
238 _a(0.331288);
239 _a(0.378859);
240 _a(0.378859);
241 _a(0.378859);
242 _a(0.279253);
243 _ap();
244 _ac();
245 _a(3.37175e-007);
246 _a(-4.76838e-007);
247 _a(6.7435e-007);
248 _a(3.37177e-007);
249 _a(3.37175e-007);
250 _a(-0.00743961);
251 _a(-0.0107031);
252 _a(-0.00940132);
253 _a(-0.00940132);
254 _a(-0.00940132);
255 _ap();
256 _ac();
257 _a(0);
258 _a(0.244346);
259 _a(0.244346);
260 _a(0.244346);
261 _a(0.785398);
262 _a(-0.570651);
263 _a(-1.22173);
264 _a(-1.22173);
265 _a(-1.22173);
266 _a(-1.22173);
267 _a(-0.174533);
268 _ap();
269 _ac();
270 _a(0);
271 _a(0);
272 _a(0);
273 _a(0);
274 _a(0);
275 _a(-0.395731);
276 _a(-0.103788);
277 _a(0.118105);
278 _a(0.0872665);
279 _a(0);
280 _a(0);
281 _ap();
282 _ac();
283 _a(0);
284 _a(0);
285 _a(-1.65806);
286 _a(-0.698132);
287 _a(0);
288 _a(-0.488692);
289 _a(-0.823719);
290 _a(-0.805354);
291 _a(-0.805354);
292 _a(-1.13446);
293 _a(-1.25664);
294 _ap();
295 _ac();
296 _a(0);
297 _a(0.15708);
298 _a(0.0872665);
299 _a(0.0872665);
300 _a(0.0872665);
301 _a(0.0829527);
302 _a(0.0944466);
303 _a(0.0830765);
304 _a(0.0830765);
305 _a(-1.25664);
306 _a(-1.23918);
307 _ap();
308 _ac();
309 _a(0);
310 _a(-0.174533);
311 _a(-0.174533);
312 _a(-1.5708);
313 _a(-1.5708);
314 _a(-0.857056);
315 _a(0.385512);
316 _a(-0.855211);
317 _a(-0.835988);
318 _a(-0.872665);
319 _a(-0.174533);
320 _ap();
321 _ac();
322 _a(0);
323 _a(1.56923e-007);
324 _a(1.56923e-007);
325 _a(1.56923e-007);
326 _a(0.541052);
327 _a(0.154976);
328 _a(-0.291418);
329 _a(0.191986);
330 _a(0.366519);
331 _a(0);
332 _a(-0.010472);
333 _ap();
334 _ac();
335 _a(0);
336 _a(0);
337 _a(-4.76838e-007);
338 _a(-0.663225);
339 _a(-0.499093);
340 _a(-0.858972);
341 _a(-0.402255);
342 _a(-0.402255);
343 _a(0);
344 _a(0);
345 _ap();
346 _ac();
347 _a(0);
348 _a(1.67552);
349 _a(1.67552);
350 _a(1.67552);
351 _a(1.67552);
352 _a(2.20124);
353 _a(1.77479);
354 _a(2.20585);
355 _a(2.20585);
356 _a(2.0944);
357 _a(0.349066);
358 _ap();
359 _ac();
360 _a(0);
361 _a(2.0944);
362 _a(2.0944);
363 _a(2.0944);
364 _a(2.0944);
365 _a(0.698132);
366 _a(0.740735);
367 _a(0.733209);
368 _a(0.733209);
369 _a(1.71042);
370 _a(1.8326);
371 _ap();
372 _ac();
373 _a(1.5708);
374 _a(0.802851);
375 _a(0.471239);
376 _a(0.366519);
377 _a(0);
378 _a(1.0472);
379 _a(0.498508);
380 _a(0.498508);
381 _a(0.498508);
382 _a(0.0349066);
383 _a(0.191986);
384 _ap();
385 _ac();
386 _a(0);
387 _a(0.244346);
388 _a(0.244346);
389 _a(0.244346);
390 _a(0.785398);
391 _a(0.785398);
392 _a(0.69115);
393 _a(0.403171);
394 _a(-0.579449);
395 _a(-1.22173);
396 _a(-0.174533);
397 _ap();
398 _ac();
399 _a(0);
400 _a(8.63852e-008);
401 _a(8.63852e-008);
402 _a(8.63852e-008);
403 _a(8.63852e-008);
404 _a(0.00928971);
405 _a(-0.129154);
406 _a(0.679603);
407 _a(0.277507);
408 _a(0);
409 _a(0);
410 _ap();
411 _ac();
412 _a(0);
413 _a(0);
414 _a(1.65806);
415 _a(0.698132);
416 _a(0);
417 _a(0.0720474);
418 _a(0.0581865);
419 _a(0.453786);
420 _a(0.559952);
421 _a(1.13446);
422 _a(1.25664);
423 _ap();
424 _ac();
425 _a(0);
426 _a(-0.15708);
427 _a(-0.0872665);
428 _a(-0.0872665);
429 _a(-0.0872665);
430 _a(-0.0807962);
431 _a(-0.0824083);
432 _a(0.000615569);
433 _a(0.000615569);
434 _a(1.25664);
435 _a(1.23918);
436 _ap();
437 _ac();
438 _a(0);
439 _a(-0.174533);
440 _a(-0.174533);
441 _a(-1.5708);
442 _a(-1.5708);
443 _a(-1.52484);
444 _a(-1.55965);
445 _a(-0.905826);
446 _a(-0.905826);
447 _a(-0.872665);
448 _a(-0.174533);
449 _ap();
450 _ac();
451 _a(0);
452 _a(0);
453 _a(0);
454 _a(0);
455 _a(-0.541052);
456 _a(-0.558422);
457 _a(-0.566003);
458 _a(-0.296706);
459 _a(-0.0174533);
460 _a(0);
461 _a(0.010472);
462 _ap();
463 _ac();
464 _a(-0.499093);
465 _a(-0.858972);
466 _a(-0.402255);
467 _a(-0.402255);
468 _a(-0.402255);
469 _ap();
470 _ac();
471 _a(0);
472 _a(1.67552);
473 _a(1.67552);
474 _a(1.67552);
475 _a(1.67552);
476 _a(1.22173);
477 _a(1.08036);
478 _a(0.876155);
479 _a(1.76278);
480 _a(2.0944);
481 _a(0.349066);
482 _ap();
483 _ac();
484 _a(0);
485 _a(2.0944);
486 _a(2.0944);
487 _a(2.0944);
488 _a(2.0944);
489 _a(2.0944);
490 _a(1.77434);
491 _a(0.891306);
492 _a(0.891306);
493 _a(1.71042);
494 _a(1.8326);
495 _ap();
496 _ac();
497 _a(-1.5708);
498 _a(-0.802851);
499 _a(-0.471239);
500 _a(-0.366519);
501 _a(0);
502 _a(-0.575959);
503 _a(-0.277696);
504 _a(-0.872665);
505 _a(-0.680678);
506 _a(-0.0349066);
507 _a(-0.191986);
508 _ap();
509
510 _tc();
511 _t(0.9);
512 _t(1.9);
513 _t(2.7);
514 _t(3.4);
515 _t(3.9);
516 _t(4.9);
517 _t(5.8);
518 _t(6.8);
519 _t(7.3);
520 _t(8.4);
521 _t(9.4);
522 _tp();
523 _tc();
524 _t(0.9);
525 _t(1.9);
526 _t(2.7);
527 _t(3.4);
528 _t(3.9);
529 _t(4.9);
530 _t(5.8);
531 _t(6.8);
532 _t(7.3);
533 _t(8.4);
534 _tp();
535 _tc();
536 _t(0.9);
537 _t(1.9);
538 _t(2.7);
539 _t(3.4);
540 _t(3.9);
541 _t(4.9);
542 _t(5.8);
543 _t(6.8);
544 _t(7.3);
545 _t(8.4);
546 _t(9.4);
547 _tp();
548 _tc();
549 _t(0.9);
550 _t(1.9);
551 _t(2.7);
552 _t(3.4);
553 _t(3.9);
554 _t(4.9);
555 _t(5.8);
556 _t(6.8);
557 _t(7.3);
558 _t(8.4);
559 _t(9.4);
560 _tp();
561 _tc();
562 _t(0.9);
563 _t(1.9);
564 _t(2.7);
565 _t(3.4);
566 _t(3.9);
567 _t(4.9);
568 _t(5.8);
569 _t(6.8);
570 _t(7.3);
571 _t(8.4);
572 _t(9.4);
573 _tp();
574 _tc();
575 _t(0.9);
576 _t(1.9);
577 _t(2.7);
578 _t(3.4);
579 _t(3.9);
580 _t(4.9);
581 _t(5.8);
582 _t(6.8);
583 _t(7.3);
584 _t(8.4);
585 _t(9.4);
586 _tp();
587 _tc();
588 _t(0.9);
589 _t(1.9);
590 _t(2.7);
591 _t(3.4);
592 _t(3.9);
593 _t(4.9);
594 _t(5.8);
595 _t(6.8);
596 _t(7.3);
597 _t(8.4);
598 _t(9.4);
599 _tp();
600 _tc();
601 _t(0.9);
602 _t(1.9);
603 _t(2.7);
604 _t(3.4);
605 _t(3.9);
606 _t(4.9);
607 _t(5.8);
608 _t(6.8);
609 _t(7.3);
610 _t(8.4);
611 _t(9.4);
612 _tp();
613 _tc();
614 _t(0.9);
615 _t(1.9);
616 _t(2.7);
617 _t(3.4);
618 _t(4.9);
619 _t(5.8);
620 _t(6.8);
621 _t(7.3);
622 _t(8.4);
623 _t(9.4);
624 _tp();
625 _tc();
626 _t(0.9);
627 _t(1.9);
628 _t(2.7);
629 _t(3.4);
630 _t(3.9);
631 _t(4.9);
632 _t(5.8);
633 _t(6.8);
634 _t(7.3);
635 _t(8.4);
636 _t(9.4);
637 _tp();
638 _tc();
639 _t(0.9);
640 _t(1.9);
641 _t(2.7);
642 _t(3.4);
643 _t(3.9);
644 _t(4.9);
645 _t(5.8);
646 _t(6.8);
647 _t(7.3);
648 _t(8.4);
649 _t(9.4);
650 _tp();
651 _tc();
652 _t(0.9);
653 _t(1.9);
654 _t(2.7);
655 _t(3.4);
656 _t(3.9);
657 _t(4.9);
658 _t(5.8);
659 _t(6.8);
660 _t(7.3);
661 _t(8.4);
662 _t(9.4);
663 _tp();
664 _tc();
665 _t(0.9);
666 _t(1.9);
667 _t(2.7);
668 _t(3.4);
669 _t(3.9);
670 _t(4.9);
671 _t(5.8);
672 _t(6.8);
673 _t(7.3);
674 _t(8.4);
675 _t(9.4);
676 _tp();
677 _tc();
678 _t(0.9);
679 _t(1.9);
680 _t(2.7);
681 _t(3.4);
682 _t(3.9);
683 _t(4.9);
684 _t(5.8);
685 _t(6.8);
686 _t(7.3);
687 _t(8.4);
688 _t(9.4);
689 _tp();
690 _tc();
691 _t(0.9);
692 _t(1.9);
693 _t(2.7);
694 _t(3.4);
695 _t(3.9);
696 _t(4.9);
697 _t(5.8);
698 _t(6.8);
699 _t(7.3);
700 _t(8.4);
701 _t(9.4);
702 _tp();
703 _tc();
704 _t(0.9);
705 _t(1.9);
706 _t(2.7);
707 _t(3.4);
708 _t(3.9);
709 _t(4.9);
710 _t(5.8);
711 _t(6.8);
712 _t(7.3);
713 _t(8.4);
714 _t(9.4);
715 _tp();
716 _tc();
717 _t(0.9);
718 _t(1.9);
719 _t(2.7);
720 _t(3.4);
721 _t(3.9);
722 _t(4.9);
723 _t(5.8);
724 _t(6.8);
725 _t(7.3);
726 _t(8.4);
727 _t(9.4);
728 _tp();
729 _tc();
730 _t(0.9);
731 _t(1.9);
732 _t(2.7);
733 _t(3.4);
734 _t(3.9);
735 _t(4.9);
736 _t(5.8);
737 _t(6.8);
738 _t(7.3);
739 _t(8.4);
740 _t(9.4);
741 _tp();
742 _tc();
743 _t(4.9);
744 _t(5.8);
745 _t(6.8);
746 _t(7.3);
747 _t(8.4);
748 _tp();
749 _tc();
750 _t(0.9);
751 _t(1.9);
752 _t(2.7);
753 _t(3.4);
754 _t(3.9);
755 _t(4.9);
756 _t(5.8);
757 _t(6.8);
758 _t(7.3);
759 _t(8.4);
760 _t(9.4);
761 _tp();
762 _tc();
763 _t(0.9);
764 _t(1.9);
765 _t(2.7);
766 _t(3.4);
767 _t(3.9);
768 _t(4.9);
769 _t(5.8);
770 _t(6.8);
771 _t(7.3);
772 _t(8.4);
773 _t(9.4);
774 _tp();
775 _tc();
776 _t(0.9);
777 _t(1.9);
778 _t(2.7);
779 _t(3.4);
780 _t(3.9);
781 _t(4.9);
782 _t(5.8);
783 _t(6.8);
784 _t(7.3);
785 _t(8.4);
786 _t(9.4);
787 _tp();
788
789 bool is_absolute = true;
790 almotion_->angleInterpolation(joints, all_angles, all_times, is_absolute);
791}
792
793void
794NaoQiMotionStandupTask::standup_from_front()
795{
796 ALValue joints, angles, times, all_angles, all_times;
797
798 goto_start_pos();
799
800 joints.arrayPush("HeadPitch");
801 joints.arrayPush("LAnklePitch");
802 joints.arrayPush("LAnkleRoll");
803 joints.arrayPush("LElbowRoll");
804 joints.arrayPush("LElbowYaw");
805 joints.arrayPush("LHipPitch");
806 joints.arrayPush("LHipRoll");
807 joints.arrayPush("LHipYawPitch");
808 joints.arrayPush("LKneePitch");
809 joints.arrayPush("LShoulderPitch");
810 joints.arrayPush("LShoulderRoll");
811 joints.arrayPush("RAnklePitch");
812 joints.arrayPush("RAnkleRoll");
813 joints.arrayPush("RElbowRoll");
814 joints.arrayPush("RElbowYaw");
815 joints.arrayPush("RHipPitch");
816 joints.arrayPush("RHipRoll");
817 joints.arrayPush("RHipYawPitch");
818 joints.arrayPush("RKneePitch");
819 joints.arrayPush("RShoulderPitch");
820 joints.arrayPush("RShoulderRoll");
821
822 _ac();
823 _a(-0.575959);
824 _a(0);
825 _a(-0.349066);
826 _a(-0.488692);
827 _a(0);
828 _a(0.279253);
829 _ap();
830 _ac();
831 _a(-1.13446);
832 _a(-1.13446);
833 _a(-0.783653);
834 _a(0.0872665);
835 _a(-0.312414);
836 _a(-0.715585);
837 _a(-1.0472);
838 _a(-0.174533);
839 _ap();
840 _ac();
841 _a(0);
842 _a(0);
843 _a(-0.680678);
844 _a(-0.555015);
845 _a(-0.296706);
846 _a(-0.10472);
847 _a(0);
848 _a(0);
849 _ap();
850 _ac();
851 _a(0);
852 _a(-0.610865);
853 _a(-1.65806);
854 _a(-0.139626);
855 _a(-0.715585);
856 _a(-1.29154);
857 _a(-1.39626);
858 _a(-1.25664);
859 _ap();
860 _ac();
861 _a(-1.5708);
862 _a(-1.5708);
863 _a(-1.5708);
864 _a(-1.5708);
865 _a(-0.244346);
866 _a(-0.925025);
867 _a(-1.5708);
868 _a(-1.23918);
869 _ap();
870 _ac();
871 _a(0);
872 _a(-1.5708);
873 _a(-1.5708);
874 _a(-1.5708);
875 _a(-1.5708);
876 _a(-1.06989);
877 _a(-1.0472);
878 _a(-0.174533);
879 _ap();
880 _ac();
881 _a(1.56923e-007);
882 _a(1.56923e-007);
883 _a(1.56923e-007);
884 _a(1.56923e-007);
885 _a(0.0872665);
886 _a(0.10472);
887 _a(-0.010472);
888 _a(-0.010472);
889 _ap();
890 _ac();
891 _a(0);
892 _a(0);
893 _a(-0.872665);
894 _a(-0.872665);
895 _a(-0.965167);
896 _a(-0.785398);
897 _a(0);
898 _a(0);
899 _ap();
900 _ac();
901 _a(2.0944);
902 _a(2.0944);
903 _a(1.0472);
904 _a(1.01229);
905 _a(2.15548);
906 _a(2.16421);
907 _a(2.0944);
908 _a(0.349066);
909 _ap();
910 _ac();
911 _a(-1.5708);
912 _a(-0.872665);
913 _a(-0.174533);
914 _a(0);
915 _a(0.610865);
916 _a(1.11701);
917 _a(1.62316);
918 _a(1.8326);
919 _ap();
920 _ac();
921 _a(0);
922 _a(0);
923 _a(0);
924 _a(0);
925 _a(0.0349066);
926 _a(0.1309);
927 _a(0.174533);
928 _a(0.191986);
929 _ap();
930 _ac();
931 _a(-1.13446);
932 _a(-1.13446);
933 _a(-0.783653);
934 _a(0.0872665);
935 _a(-0.312414);
936 _a(-0.715585);
937 _a(-1.0472);
938 _a(-0.174533);
939 _ap();
940 _ac();
941 _a(8.63852e-008);
942 _a(8.63852e-008);
943 _a(0.680678);
944 _a(0.555015);
945 _a(0.296706);
946 _a(0.10472);
947 _a(0);
948 _a(0);
949 _ap();
950 _ac();
951 _a(0);
952 _a(0.610865);
953 _a(1.65806);
954 _a(0.139626);
955 _a(0.715585);
956 _a(1.29154);
957 _a(1.39626);
958 _a(1.25664);
959 _ap();
960 _ac();
961 _a(1.5708);
962 _a(1.5708);
963 _a(1.5708);
964 _a(1.5708);
965 _a(0.244346);
966 _a(0.925025);
967 _a(1.5708);
968 _a(1.23918);
969 _ap();
970 _ac();
971 _a(1.44878e-007);
972 _a(-1.5708);
973 _a(-1.5708);
974 _a(-1.5708);
975 _a(-1.5708);
976 _a(-1.06989);
977 _a(-1.0472);
978 _a(-0.174533);
979 _ap();
980 _ac();
981 _a(0);
982 _a(0);
983 _a(0);
984 _a(0);
985 _a(-0.0872665);
986 _a(-0.10472);
987 _a(0.010472);
988 _a(0.010472);
989 _ap();
990 _ac();
991 _a(8.54618e-008);
992 _a(9.7389e-008);
993 _a(-0.872665);
994 _ap();
995 _ac();
996 _a(2.0944);
997 _a(2.0944);
998 _a(1.0472);
999 _a(1.01229);
1000 _a(2.15548);
1001 _a(2.16421);
1002 _a(2.0944);
1003 _a(0.349066);
1004 _ap();
1005 _ac();
1006 _a(-1.5708);
1007 _a(-0.872665);
1008 _a(-0.174533);
1009 _a(0);
1010 _a(0.610865);
1011 _a(1.11701);
1012 _a(1.62316);
1013 _a(1.8326);
1014 _ap();
1015 _ac();
1016 _a(0);
1017 _a(0);
1018 _a(0);
1019 _a(0);
1020 _a(-0.0349066);
1021 _a(-0.1309);
1022 _a(-0.174533);
1023 _a(-0.191986);
1024 _ap();
1025
1026 _tc();
1027 _t(1.4);
1028 _t(2.4);
1029 _t(3.7);
1030 _t(5.2);
1031 _t(6.2);
1032 _t(8.4);
1033 _tp();
1034 _tc();
1035 _t(1.4);
1036 _t(2.4);
1037 _t(3.7);
1038 _t(4.4);
1039 _t(5.2);
1040 _t(6.2);
1041 _t(7.4);
1042 _t(8.4);
1043 _tp();
1044 _tc();
1045 _t(1.4);
1046 _t(2.4);
1047 _t(3.7);
1048 _t(4.4);
1049 _t(5.2);
1050 _t(6.2);
1051 _t(7.4);
1052 _t(8.4);
1053 _tp();
1054 _tc();
1055 _t(1.4);
1056 _t(2.4);
1057 _t(3.7);
1058 _t(4.4);
1059 _t(5.2);
1060 _t(6.2);
1061 _t(7.4);
1062 _t(8.4);
1063 _tp();
1064 _tc();
1065 _t(1.4);
1066 _t(2.4);
1067 _t(3.7);
1068 _t(4.4);
1069 _t(5.2);
1070 _t(6.2);
1071 _t(7.4);
1072 _t(8.4);
1073 _tp();
1074 _tc();
1075 _t(1.4);
1076 _t(2.4);
1077 _t(3.7);
1078 _t(4.4);
1079 _t(5.2);
1080 _t(6.2);
1081 _t(7.4);
1082 _t(8.4);
1083 _tp();
1084 _tc();
1085 _t(1.4);
1086 _t(2.4);
1087 _t(3.7);
1088 _t(4.4);
1089 _t(5.2);
1090 _t(6.2);
1091 _t(7.4);
1092 _t(8.4);
1093 _tp();
1094 _tc();
1095 _t(1.4);
1096 _t(2.4);
1097 _t(3.7);
1098 _t(4.4);
1099 _t(5.2);
1100 _t(6.2);
1101 _t(7.4);
1102 _t(8.4);
1103 _tp();
1104 _tc();
1105 _t(1.4);
1106 _t(2.4);
1107 _t(3.7);
1108 _t(4.4);
1109 _t(5.2);
1110 _t(6.2);
1111 _t(7.4);
1112 _t(8.4);
1113 _tp();
1114 _tc();
1115 _t(1.4);
1116 _t(2.4);
1117 _t(3.7);
1118 _t(4.4);
1119 _t(5.2);
1120 _t(6.2);
1121 _t(7.4);
1122 _t(8.4);
1123 _tp();
1124 _tc();
1125 _t(1.4);
1126 _t(2.4);
1127 _t(3.7);
1128 _t(4.4);
1129 _t(5.2);
1130 _t(6.2);
1131 _t(7.4);
1132 _t(8.4);
1133 _tp();
1134 _tc();
1135 _t(1.4);
1136 _t(2.4);
1137 _t(3.7);
1138 _t(4.4);
1139 _t(5.2);
1140 _t(6.2);
1141 _t(7.4);
1142 _t(8.4);
1143 _tp();
1144 _tc();
1145 _t(1.4);
1146 _t(2.4);
1147 _t(3.7);
1148 _t(4.4);
1149 _t(5.2);
1150 _t(6.2);
1151 _t(7.4);
1152 _t(8.4);
1153 _tp();
1154 _tc();
1155 _t(1.4);
1156 _t(2.4);
1157 _t(3.7);
1158 _t(4.4);
1159 _t(5.2);
1160 _t(6.2);
1161 _t(7.4);
1162 _t(8.4);
1163 _tp();
1164 _tc();
1165 _t(1.4);
1166 _t(2.4);
1167 _t(3.7);
1168 _t(4.4);
1169 _t(5.2);
1170 _t(6.2);
1171 _t(7.4);
1172 _t(8.4);
1173 _tp();
1174 _tc();
1175 _t(1.4);
1176 _t(2.4);
1177 _t(3.7);
1178 _t(4.4);
1179 _t(5.2);
1180 _t(6.2);
1181 _t(7.4);
1182 _t(8.4);
1183 _tp();
1184 _tc();
1185 _t(1.4);
1186 _t(2.4);
1187 _t(3.7);
1188 _t(4.4);
1189 _t(5.2);
1190 _t(6.2);
1191 _t(7.4);
1192 _t(8.4);
1193 _tp();
1194 _tc();
1195 _t(1.4);
1196 _t(2.4);
1197 _t(3.7);
1198 _tp();
1199 _tc();
1200 _t(1.4);
1201 _t(2.4);
1202 _t(3.7);
1203 _t(4.4);
1204 _t(5.2);
1205 _t(6.2);
1206 _t(7.4);
1207 _t(8.4);
1208 _tp();
1209 _tc();
1210 _t(1.4);
1211 _t(2.4);
1212 _t(3.7);
1213 _t(4.4);
1214 _t(5.2);
1215 _t(6.2);
1216 _t(7.4);
1217 _t(8.4);
1218 _tp();
1219 _tc();
1220 _t(1.4);
1221 _t(2.4);
1222 _t(3.7);
1223 _t(4.4);
1224 _t(5.2);
1225 _t(6.2);
1226 _t(7.4);
1227 _t(8.4);
1228 _tp();
1229
1230 bool is_absolute = true;
1231 almotion_->angleInterpolation(joints, all_angles, all_times, is_absolute);
1232}
1233
1234/** Run the standup. */
1235void
1237{
1239 //allogger_->info("NaoQiMotionStandupTask", "Explicit standup back");
1240 standup_from_back();
1241 } else if (from_pos_ == fawkes::HumanoidMotionInterface::STANDUP_FRONT) {
1242 //allogger_->info("NaoQiMotionStandupTask", "Explicit standup front");
1243 standup_from_front();
1244 } else {
1245 if (accel_x_ > 0.8) {
1246 //allogger_->info("NaoQiMotionStandupTask", "Standup from front (detected)");
1247 standup_from_front();
1248 } else if (accel_x_ < -0.8) {
1249 //allogger_->info("NaoQiMotionStandupTask", "Standup from back (detected)");
1250 standup_from_back();
1251 } else {
1252 //allogger_->error("NaoQiMotionStandupTask",
1253 // "NaoQiMotionStandupTask: Does not seem that I'm lying on the ground, "
1254 // "not standing up until you tell me from where");
1255 }
1256 }
1257}
NaoQiMotionStandupTask(AL::ALPtr< AL::ALMotionProxy > almotion, fawkes::HumanoidMotionInterface::StandupEnum from_pos, float accel_x, float accel_y, float accel_z)
Constructor.
virtual ~NaoQiMotionStandupTask()
Destructor.
virtual void run()
Run the standup.
StandupEnum
From which position to standup.
@ STANDUP_BACK
Standup from lying on the back.
@ STANDUP_FRONT
Standup from lying on the tummy.
Fawkes library namespace.