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 
31 using namespace AL;
32 using 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 
79 void
80 NaoQiMotionStandupTask::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 
201 void
202 NaoQiMotionStandupTask::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 
793 void
794 NaoQiMotionStandupTask::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. */
1235 void
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.