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 <cstring>
28 #include <cstdlib>
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  */
49 NaoQiMotionStandupTask::NaoQiMotionStandupTask(AL::ALPtr<AL::ALMotionProxy> almotion,
51  float accel_x, float accel_y,
52  float accel_z)
53 {
54  __almotion = almotion;
55  __from_pos = from_pos;
56  __accel_x = accel_x;
57  __accel_y = accel_y;
58  __accel_z = accel_z;
59 
60  // ALTask variable to cause auto-destruct when done
61  fAutoDelete = true;
62 }
63 
64 
65 /** Destructor. */
67 {
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"); joints.arrayPush("HeadYaw");
85  joints.arrayPush("LAnklePitch"); joints.arrayPush("LAnkleRoll");
86  joints.arrayPush("LElbowRoll"); joints.arrayPush("LElbowYaw");
87  joints.arrayPush("LHipPitch"); joints.arrayPush("LHipRoll");
88  joints.arrayPush("LHipYawPitch"); joints.arrayPush("LKneePitch");
89  joints.arrayPush("LShoulderPitch"); joints.arrayPush("LShoulderRoll");
90  joints.arrayPush("RAnklePitch"); joints.arrayPush("RAnkleRoll");
91  joints.arrayPush("RElbowRoll"); joints.arrayPush("RElbowYaw");
92  joints.arrayPush("RHipPitch"); joints.arrayPush("RHipRoll");
93  joints.arrayPush("RKneePitch"); joints.arrayPush("RShoulderPitch");
94  joints.arrayPush("RShoulderRoll");
95 
96  times = ALValue::array(1.9, 2.9);
97 
98  for (unsigned int i = 0; i < joints.getSize(); ++i) {
99  all_times.arrayPush(times);
100  }
101 
102  _ac(); _a(0); _a(0); _ap();
103  _ac(); _a(3.37175e-007); _a(3.37175e-007); _ap();
104  _ac(); _a(0.523599); _a(0.523599); _ap();
105  _ac(); _a(0); _a(0); _ap();
106  _ac(); _a(0); _a(0); _ap();
107  _ac(); _a(0); _a(0); _ap();
108  _ac(); _a(0); _a(0); _ap();
109  _ac(); _a(0); _a(0); _ap();
110  _ac(); _a(0); _a(0); _ap();
111  _ac(); _a(0); _a(0); _ap();
112  _ac(); _a(0); _a(0); _ap();
113  _ac(); _a(1.5708); _a(1.5708); _ap();
114  _ac(); _a(0.523599); _a(0.523599); _ap();
115  _ac(); _a(0); _a(0); _ap();
116  _ac(); _a(0); _a(0); _ap();
117  _ac(); _a(0); _a(0); _ap();
118  _ac(); _a(0); _a(0); _ap();
119  _ac(); _a(0); _a(0); _ap();
120  _ac(); _a(0); _a(0); _ap();
121  _ac(); _a(0); _a(0); _ap();
122  _ac(); _a(-1.5708); _a(-1.5708); _ap();
123 
124 
125  bool is_absolute = true;
126  __almotion->angleInterpolation(joints, all_angles, all_times, is_absolute);
127 }
128 
129 void
130 NaoQiMotionStandupTask::standup_from_back()
131 {
132 
133  ALValue joints, angles, times, all_angles, all_times;
134 
135  goto_start_pos();
136 
137  joints.arrayPush("HeadPitch"); joints.arrayPush("HeadYaw");
138  joints.arrayPush("LAnklePitch"); joints.arrayPush("LAnkleRoll");
139  joints.arrayPush("LElbowRoll"); joints.arrayPush("LElbowYaw");
140  joints.arrayPush("LHipPitch"); joints.arrayPush("LHipRoll");
141  joints.arrayPush("LHipYawPitch"); joints.arrayPush("LKneePitch");
142  joints.arrayPush("LShoulderPitch"); joints.arrayPush("LShoulderRoll");
143  joints.arrayPush("RAnklePitch"); joints.arrayPush("RAnkleRoll");
144  joints.arrayPush("RElbowRoll"); joints.arrayPush("RElbowYaw");
145  joints.arrayPush("RHipPitch"); joints.arrayPush("RHipRoll");
146  joints.arrayPush("RHipYawPitch"); joints.arrayPush("RKneePitch");
147  joints.arrayPush("RShoulderPitch"); joints.arrayPush("RShoulderRoll");
148 
149  _ac(); _a(0); _a(0); _a(-0.785398); _a(0); _a(0.349066); _a(0.328232); _a(0.331288); _a(0.378859); _a(0.378859); _a(0.378859); _a(0.279253); _ap();
150  _ac(); _a(3.37175e-007); _a(-4.76838e-007); _a(6.7435e-007); _a(3.37177e-007); _a(3.37175e-007); _a(-0.00743961); _a(-0.0107031); _a(-0.00940132); _a(-0.00940132); _a(-0.00940132); _ap();
151  _ac(); _a(0); _a(0.244346); _a(0.244346); _a(0.244346); _a(0.785398); _a(-0.570651); _a(-1.22173); _a(-1.22173); _a(-1.22173); _a(-1.22173); _a(-0.174533); _ap();
152  _ac(); _a(0); _a(0); _a(0); _a(0); _a(0); _a(-0.395731); _a(-0.103788); _a(0.118105); _a(0.0872665); _a(0); _a(0); _ap();
153  _ac(); _a(0); _a(0); _a(-1.65806); _a(-0.698132); _a(0); _a(-0.488692); _a(-0.823719); _a(-0.805354); _a(-0.805354); _a(-1.13446); _a(-1.25664); _ap();
154  _ac(); _a(0); _a(0.15708); _a(0.0872665); _a(0.0872665); _a(0.0872665); _a(0.0829527); _a(0.0944466); _a(0.0830765); _a(0.0830765); _a(-1.25664); _a(-1.23918); _ap();
155  _ac(); _a(0); _a(-0.174533); _a(-0.174533); _a(-1.5708); _a(-1.5708); _a(-0.857056); _a(0.385512); _a(-0.855211); _a(-0.835988); _a(-0.872665); _a(-0.174533); _ap();
156  _ac(); _a(0); _a(1.56923e-007); _a(1.56923e-007); _a(1.56923e-007); _a(0.541052); _a(0.154976); _a(-0.291418); _a(0.191986); _a(0.366519); _a(0); _a(-0.010472); _ap();
157  _ac(); _a(0); _a(0); _a(-4.76838e-007); _a(-0.663225); _a(-0.499093); _a(-0.858972); _a(-0.402255); _a(-0.402255); _a(0); _a(0); _ap();
158  _ac(); _a(0); _a(1.67552); _a(1.67552); _a(1.67552); _a(1.67552); _a(2.20124); _a(1.77479); _a(2.20585); _a(2.20585); _a(2.0944); _a(0.349066); _ap();
159  _ac(); _a(0); _a(2.0944); _a(2.0944); _a(2.0944); _a(2.0944); _a(0.698132); _a(0.740735); _a(0.733209); _a(0.733209); _a(1.71042); _a(1.8326); _ap();
160  _ac(); _a(1.5708); _a(0.802851); _a(0.471239); _a(0.366519); _a(0); _a(1.0472); _a(0.498508); _a(0.498508); _a(0.498508); _a(0.0349066); _a(0.191986); _ap();
161  _ac(); _a(0); _a(0.244346); _a(0.244346); _a(0.244346); _a(0.785398); _a(0.785398); _a(0.69115); _a(0.403171); _a(-0.579449); _a(-1.22173); _a(-0.174533); _ap();
162  _ac(); _a(0); _a(8.63852e-008); _a(8.63852e-008); _a(8.63852e-008); _a(8.63852e-008); _a(0.00928971); _a(-0.129154); _a(0.679603); _a(0.277507); _a(0); _a(0); _ap();
163  _ac(); _a(0); _a(0); _a(1.65806); _a(0.698132); _a(0); _a(0.0720474); _a(0.0581865); _a(0.453786); _a(0.559952); _a(1.13446); _a(1.25664); _ap();
164  _ac(); _a(0); _a(-0.15708); _a(-0.0872665); _a(-0.0872665); _a(-0.0872665); _a(-0.0807962); _a(-0.0824083); _a(0.000615569); _a(0.000615569); _a(1.25664); _a(1.23918); _ap();
165  _ac(); _a(0); _a(-0.174533); _a(-0.174533); _a(-1.5708); _a(-1.5708); _a(-1.52484); _a(-1.55965); _a(-0.905826); _a(-0.905826); _a(-0.872665); _a(-0.174533); _ap();
166  _ac(); _a(0); _a(0); _a(0); _a(0); _a(-0.541052); _a(-0.558422); _a(-0.566003); _a(-0.296706); _a(-0.0174533); _a(0); _a(0.010472); _ap();
167  _ac(); _a(-0.499093); _a(-0.858972); _a(-0.402255); _a(-0.402255); _a(-0.402255); _ap();
168  _ac(); _a(0); _a(1.67552); _a(1.67552); _a(1.67552); _a(1.67552); _a(1.22173); _a(1.08036); _a(0.876155); _a(1.76278); _a(2.0944); _a(0.349066); _ap();
169  _ac(); _a(0); _a(2.0944); _a(2.0944); _a(2.0944); _a(2.0944); _a(2.0944); _a(1.77434); _a(0.891306); _a(0.891306); _a(1.71042); _a(1.8326); _ap();
170  _ac(); _a(-1.5708); _a(-0.802851); _a(-0.471239); _a(-0.366519); _a(0); _a(-0.575959); _a(-0.277696); _a(-0.872665); _a(-0.680678); _a(-0.0349066); _a(-0.191986); _ap();
171 
172  _tc(); _t(0.9); _t(1.9); _t(2.7); _t(3.4); _t(3.9); _t(4.9); _t(5.8); _t(6.8); _t(7.3); _t(8.4); _t(9.4); _tp();
173  _tc(); _t(0.9); _t(1.9); _t(2.7); _t(3.4); _t(3.9); _t(4.9); _t(5.8); _t(6.8); _t(7.3); _t(8.4); _tp();
174  _tc(); _t(0.9); _t(1.9); _t(2.7); _t(3.4); _t(3.9); _t(4.9); _t(5.8); _t(6.8); _t(7.3); _t(8.4); _t(9.4); _tp();
175  _tc(); _t(0.9); _t(1.9); _t(2.7); _t(3.4); _t(3.9); _t(4.9); _t(5.8); _t(6.8); _t(7.3); _t(8.4); _t(9.4); _tp();
176  _tc(); _t(0.9); _t(1.9); _t(2.7); _t(3.4); _t(3.9); _t(4.9); _t(5.8); _t(6.8); _t(7.3); _t(8.4); _t(9.4); _tp();
177  _tc(); _t(0.9); _t(1.9); _t(2.7); _t(3.4); _t(3.9); _t(4.9); _t(5.8); _t(6.8); _t(7.3); _t(8.4); _t(9.4); _tp();
178  _tc(); _t(0.9); _t(1.9); _t(2.7); _t(3.4); _t(3.9); _t(4.9); _t(5.8); _t(6.8); _t(7.3); _t(8.4); _t(9.4); _tp();
179  _tc(); _t(0.9); _t(1.9); _t(2.7); _t(3.4); _t(3.9); _t(4.9); _t(5.8); _t(6.8); _t(7.3); _t(8.4); _t(9.4); _tp();
180  _tc(); _t(0.9); _t(1.9); _t(2.7); _t(3.4); _t(4.9); _t(5.8); _t(6.8); _t(7.3); _t(8.4); _t(9.4); _tp();
181  _tc(); _t(0.9); _t(1.9); _t(2.7); _t(3.4); _t(3.9); _t(4.9); _t(5.8); _t(6.8); _t(7.3); _t(8.4); _t(9.4); _tp();
182  _tc(); _t(0.9); _t(1.9); _t(2.7); _t(3.4); _t(3.9); _t(4.9); _t(5.8); _t(6.8); _t(7.3); _t(8.4); _t(9.4); _tp();
183  _tc(); _t(0.9); _t(1.9); _t(2.7); _t(3.4); _t(3.9); _t(4.9); _t(5.8); _t(6.8); _t(7.3); _t(8.4); _t(9.4); _tp();
184  _tc(); _t(0.9); _t(1.9); _t(2.7); _t(3.4); _t(3.9); _t(4.9); _t(5.8); _t(6.8); _t(7.3); _t(8.4); _t(9.4); _tp();
185  _tc(); _t(0.9); _t(1.9); _t(2.7); _t(3.4); _t(3.9); _t(4.9); _t(5.8); _t(6.8); _t(7.3); _t(8.4); _t(9.4); _tp();
186  _tc(); _t(0.9); _t(1.9); _t(2.7); _t(3.4); _t(3.9); _t(4.9); _t(5.8); _t(6.8); _t(7.3); _t(8.4); _t(9.4); _tp();
187  _tc(); _t(0.9); _t(1.9); _t(2.7); _t(3.4); _t(3.9); _t(4.9); _t(5.8); _t(6.8); _t(7.3); _t(8.4); _t(9.4); _tp();
188  _tc(); _t(0.9); _t(1.9); _t(2.7); _t(3.4); _t(3.9); _t(4.9); _t(5.8); _t(6.8); _t(7.3); _t(8.4); _t(9.4); _tp();
189  _tc(); _t(0.9); _t(1.9); _t(2.7); _t(3.4); _t(3.9); _t(4.9); _t(5.8); _t(6.8); _t(7.3); _t(8.4); _t(9.4); _tp();
190  _tc(); _t(4.9); _t(5.8); _t(6.8); _t(7.3); _t(8.4); _tp();
191  _tc(); _t(0.9); _t(1.9); _t(2.7); _t(3.4); _t(3.9); _t(4.9); _t(5.8); _t(6.8); _t(7.3); _t(8.4); _t(9.4); _tp();
192  _tc(); _t(0.9); _t(1.9); _t(2.7); _t(3.4); _t(3.9); _t(4.9); _t(5.8); _t(6.8); _t(7.3); _t(8.4); _t(9.4); _tp();
193  _tc(); _t(0.9); _t(1.9); _t(2.7); _t(3.4); _t(3.9); _t(4.9); _t(5.8); _t(6.8); _t(7.3); _t(8.4); _t(9.4); _tp();
194 
195  bool is_absolute = true;
196  __almotion->angleInterpolation(joints, all_angles, all_times, is_absolute);
197 
198 }
199 
200 void
201 NaoQiMotionStandupTask::standup_from_front()
202 {
203  ALValue joints, angles, times, all_angles, all_times;
204 
205  goto_start_pos();
206 
207  joints.arrayPush("HeadPitch");
208  joints.arrayPush("LAnklePitch"); joints.arrayPush("LAnkleRoll");
209  joints.arrayPush("LElbowRoll"); joints.arrayPush("LElbowYaw");
210  joints.arrayPush("LHipPitch"); joints.arrayPush("LHipRoll");
211  joints.arrayPush("LHipYawPitch"); joints.arrayPush("LKneePitch");
212  joints.arrayPush("LShoulderPitch"); joints.arrayPush("LShoulderRoll");
213  joints.arrayPush("RAnklePitch"); joints.arrayPush("RAnkleRoll");
214  joints.arrayPush("RElbowRoll"); joints.arrayPush("RElbowYaw");
215  joints.arrayPush("RHipPitch"); joints.arrayPush("RHipRoll");
216  joints.arrayPush("RHipYawPitch"); joints.arrayPush("RKneePitch");
217  joints.arrayPush("RShoulderPitch"); joints.arrayPush("RShoulderRoll");
218 
219  _ac(); _a(-0.575959); _a(0); _a(-0.349066); _a(-0.488692); _a(0); _a(0.279253); _ap();
220  _ac(); _a(-1.13446); _a(-1.13446); _a(-0.783653); _a(0.0872665); _a(-0.312414); _a(-0.715585); _a(-1.0472); _a(-0.174533); _ap();
221  _ac(); _a(0); _a(0); _a(-0.680678); _a(-0.555015); _a(-0.296706); _a(-0.10472); _a(0); _a(0); _ap();
222  _ac(); _a(0); _a(-0.610865); _a(-1.65806); _a(-0.139626); _a(-0.715585); _a(-1.29154); _a(-1.39626); _a(-1.25664); _ap();
223  _ac(); _a(-1.5708); _a(-1.5708); _a(-1.5708); _a(-1.5708); _a(-0.244346); _a(-0.925025); _a(-1.5708); _a(-1.23918); _ap();
224  _ac(); _a(0); _a(-1.5708); _a(-1.5708); _a(-1.5708); _a(-1.5708); _a(-1.06989); _a(-1.0472); _a(-0.174533); _ap();
225  _ac(); _a(1.56923e-007); _a(1.56923e-007); _a(1.56923e-007); _a(1.56923e-007); _a(0.0872665); _a(0.10472); _a(-0.010472); _a(-0.010472); _ap();
226  _ac(); _a(0); _a(0); _a(-0.872665); _a(-0.872665); _a(-0.965167); _a(-0.785398); _a(0); _a(0); _ap();
227  _ac(); _a(2.0944); _a(2.0944); _a(1.0472); _a(1.01229); _a(2.15548); _a(2.16421); _a(2.0944); _a(0.349066); _ap();
228  _ac(); _a(-1.5708); _a(-0.872665); _a(-0.174533); _a(0); _a(0.610865); _a(1.11701); _a(1.62316); _a(1.8326); _ap();
229  _ac(); _a(0); _a(0); _a(0); _a(0); _a(0.0349066); _a(0.1309); _a(0.174533); _a(0.191986); _ap();
230  _ac(); _a(-1.13446); _a(-1.13446); _a(-0.783653); _a(0.0872665); _a(-0.312414); _a(-0.715585); _a(-1.0472); _a(-0.174533); _ap();
231  _ac(); _a(8.63852e-008); _a(8.63852e-008); _a(0.680678); _a(0.555015); _a(0.296706); _a(0.10472); _a(0); _a(0); _ap();
232  _ac(); _a(0); _a(0.610865); _a(1.65806); _a(0.139626); _a(0.715585); _a(1.29154); _a(1.39626); _a(1.25664); _ap();
233  _ac(); _a(1.5708); _a(1.5708); _a(1.5708); _a(1.5708); _a(0.244346); _a(0.925025); _a(1.5708); _a(1.23918); _ap();
234  _ac(); _a(1.44878e-007); _a(-1.5708); _a(-1.5708); _a(-1.5708); _a(-1.5708); _a(-1.06989); _a(-1.0472); _a(-0.174533); _ap();
235  _ac(); _a(0); _a(0); _a(0); _a(0); _a(-0.0872665); _a(-0.10472); _a(0.010472); _a(0.010472); _ap();
236  _ac(); _a(8.54618e-008); _a(9.7389e-008); _a(-0.872665); _ap();
237  _ac(); _a(2.0944); _a(2.0944); _a(1.0472); _a(1.01229); _a(2.15548); _a(2.16421); _a(2.0944); _a(0.349066); _ap();
238  _ac(); _a(-1.5708); _a(-0.872665); _a(-0.174533); _a(0); _a(0.610865); _a(1.11701); _a(1.62316); _a(1.8326); _ap();
239  _ac(); _a(0); _a(0); _a(0); _a(0); _a(-0.0349066); _a(-0.1309); _a(-0.174533); _a(-0.191986); _ap();
240 
241  _tc(); _t(1.4); _t(2.4); _t(3.7); _t(5.2); _t(6.2); _t(8.4); _tp();
242  _tc(); _t(1.4); _t(2.4); _t(3.7); _t(4.4); _t(5.2); _t(6.2); _t(7.4); _t(8.4); _tp();
243  _tc(); _t(1.4); _t(2.4); _t(3.7); _t(4.4); _t(5.2); _t(6.2); _t(7.4); _t(8.4); _tp();
244  _tc(); _t(1.4); _t(2.4); _t(3.7); _t(4.4); _t(5.2); _t(6.2); _t(7.4); _t(8.4); _tp();
245  _tc(); _t(1.4); _t(2.4); _t(3.7); _t(4.4); _t(5.2); _t(6.2); _t(7.4); _t(8.4); _tp();
246  _tc(); _t(1.4); _t(2.4); _t(3.7); _t(4.4); _t(5.2); _t(6.2); _t(7.4); _t(8.4); _tp();
247  _tc(); _t(1.4); _t(2.4); _t(3.7); _t(4.4); _t(5.2); _t(6.2); _t(7.4); _t(8.4); _tp();
248  _tc(); _t(1.4); _t(2.4); _t(3.7); _t(4.4); _t(5.2); _t(6.2); _t(7.4); _t(8.4); _tp();
249  _tc(); _t(1.4); _t(2.4); _t(3.7); _t(4.4); _t(5.2); _t(6.2); _t(7.4); _t(8.4); _tp();
250  _tc(); _t(1.4); _t(2.4); _t(3.7); _t(4.4); _t(5.2); _t(6.2); _t(7.4); _t(8.4); _tp();
251  _tc(); _t(1.4); _t(2.4); _t(3.7); _t(4.4); _t(5.2); _t(6.2); _t(7.4); _t(8.4); _tp();
252  _tc(); _t(1.4); _t(2.4); _t(3.7); _t(4.4); _t(5.2); _t(6.2); _t(7.4); _t(8.4); _tp();
253  _tc(); _t(1.4); _t(2.4); _t(3.7); _t(4.4); _t(5.2); _t(6.2); _t(7.4); _t(8.4); _tp();
254  _tc(); _t(1.4); _t(2.4); _t(3.7); _t(4.4); _t(5.2); _t(6.2); _t(7.4); _t(8.4); _tp();
255  _tc(); _t(1.4); _t(2.4); _t(3.7); _t(4.4); _t(5.2); _t(6.2); _t(7.4); _t(8.4); _tp();
256  _tc(); _t(1.4); _t(2.4); _t(3.7); _t(4.4); _t(5.2); _t(6.2); _t(7.4); _t(8.4); _tp();
257  _tc(); _t(1.4); _t(2.4); _t(3.7); _t(4.4); _t(5.2); _t(6.2); _t(7.4); _t(8.4); _tp();
258  _tc(); _t(1.4); _t(2.4); _t(3.7); _tp();
259  _tc(); _t(1.4); _t(2.4); _t(3.7); _t(4.4); _t(5.2); _t(6.2); _t(7.4); _t(8.4); _tp();
260  _tc(); _t(1.4); _t(2.4); _t(3.7); _t(4.4); _t(5.2); _t(6.2); _t(7.4); _t(8.4); _tp();
261  _tc(); _t(1.4); _t(2.4); _t(3.7); _t(4.4); _t(5.2); _t(6.2); _t(7.4); _t(8.4); _tp();
262 
263  bool is_absolute = true;
264  __almotion->angleInterpolation(joints, all_angles, all_times, is_absolute);
265 }
266 
267 /** Run the standup. */
268 void
270 {
271  if ( __from_pos == fawkes::HumanoidMotionInterface::STANDUP_BACK ) {
272  //__allogger->info("NaoQiMotionStandupTask", "Explicit standup back");
273  standup_from_back();
274  } else if (__from_pos == fawkes::HumanoidMotionInterface::STANDUP_FRONT ) {
275  //__allogger->info("NaoQiMotionStandupTask", "Explicit standup front");
276  standup_from_front();
277  } else {
278  if ( __accel_x > 0.8 ) {
279  //__allogger->info("NaoQiMotionStandupTask", "Standup from front (detected)");
280  standup_from_front();
281  } else if ( __accel_x < -0.8 ) {
282  //__allogger->info("NaoQiMotionStandupTask", "Standup from back (detected)");
283  standup_from_back();
284  } else {
285  //__allogger->error("NaoQiMotionStandupTask",
286  // "NaoQiMotionStandupTask: Does not seem that I'm lying on the ground, "
287  // "not standing up until you tell me from where");
288  }
289  }
290 }
NaoQiMotionStandupTask(AL::ALPtr< AL::ALMotionProxy > almotion, fawkes::HumanoidMotionInterface::StandupEnum from_pos, float accel_x, float accel_y, float accel_z)
Constructor.
Fawkes library namespace.
StandupEnum
From which position to standup.
virtual void run()
Run the standup.
virtual ~NaoQiMotionStandupTask()
Destructor.