23 #include "motion_standup_task.h" 25 #include <core/exceptions/system.h> 51 float accel_x,
float accel_y,
54 __almotion = almotion;
55 __from_pos = from_pos;
71 #define _t(x) times.arrayPush(x) 72 #define _tc() times.clear() 73 #define _tp() all_times.arrayPush(times) 75 #define _a(x) angles.arrayPush(x) 76 #define _ac() angles.clear() 77 #define _ap() all_angles.arrayPush(angles) 80 NaoQiMotionStandupTask::goto_start_pos()
82 ALValue joints, angles, times, all_angles, all_times;
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");
96 times = ALValue::array(1.9, 2.9);
98 for (
unsigned int i = 0; i < joints.getSize(); ++i) {
99 all_times.arrayPush(times);
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();
125 bool is_absolute =
true;
126 __almotion->angleInterpolation(joints, all_angles, all_times, is_absolute);
130 NaoQiMotionStandupTask::standup_from_back()
133 ALValue joints, angles, times, all_angles, all_times;
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");
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();
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();
195 bool is_absolute =
true;
196 __almotion->angleInterpolation(joints, all_angles, all_times, is_absolute);
201 NaoQiMotionStandupTask::standup_from_front()
203 ALValue joints, angles, times, all_angles, all_times;
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");
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();
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();
263 bool is_absolute =
true;
264 __almotion->angleInterpolation(joints, all_angles, all_times, is_absolute);
276 standup_from_front();
278 if ( __accel_x > 0.8 ) {
280 standup_from_front();
281 }
else if ( __accel_x < -0.8 ) {
NaoQiMotionStandupTask(AL::ALPtr< AL::ALMotionProxy > almotion, fawkes::HumanoidMotionInterface::StandupEnum from_pos, float accel_x, float accel_y, float accel_z)
Constructor.
Standup from lying on the back.
Fawkes library namespace.
StandupEnum
From which position to standup.
virtual void run()
Run the standup.
virtual ~NaoQiMotionStandupTask()
Destructor.
Standup from lying on the tummy.