Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * motion_standup_task.cpp - Task for making the robot stand up 00004 * 00005 * Created: Mon Jan 19 14:18:40 2009 00006 * Copyright 2009-2011 Tim Niemueller [www.niemueller.de] 00007 * 00008 ****************************************************************************/ 00009 00010 /* This program is free software; you can redistribute it and/or modify 00011 * it under the terms of the GNU General Public License as published by 00012 * the Free Software Foundation; either version 2 of the License, or 00013 * (at your option) any later version. 00014 * 00015 * This program is distributed in the hope that it will be useful, 00016 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00017 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00018 * GNU Library General Public License for more details. 00019 * 00020 * Read the full text in the LICENSE.GPL file in the doc directory. 00021 */ 00022 00023 #include "motion_standup_task.h" 00024 00025 #include <core/exceptions/system.h> 00026 00027 #include <cstring> 00028 #include <cstdlib> 00029 #include <string> 00030 00031 using namespace AL; 00032 using namespace fawkes; 00033 00034 /** @class NaoQiMotionStandupTask "motion_standup_task.h" 00035 * NaoQi standup task. 00036 * This task can be used to make the robot standup ina non-blocking way. It will 00037 * use (blocking) ALMotion calls to execute the move. Note that ALMotion should 00038 * not be used otherwise. 00039 * @author Tim Niemueller 00040 */ 00041 00042 /** Constructor. 00043 * @param almotion ALMotion proxy 00044 * @param from_pos position from which to standup 00045 * @param accel_x current accelerometer value 00046 * @param accel_y current accelerometer value 00047 * @param accel_z current accelerometer value 00048 */ 00049 NaoQiMotionStandupTask::NaoQiMotionStandupTask(AL::ALPtr<AL::ALMotionProxy> almotion, 00050 fawkes::HumanoidMotionInterface::StandupEnum from_pos, 00051 float accel_x, float accel_y, 00052 float accel_z) 00053 { 00054 __almotion = almotion; 00055 __from_pos = from_pos; 00056 __accel_x = accel_x; 00057 __accel_y = accel_y; 00058 __accel_z = accel_z; 00059 00060 // ALTask variable to cause auto-destruct when done 00061 fAutoDelete = true; 00062 } 00063 00064 00065 /** Destructor. */ 00066 NaoQiMotionStandupTask::~NaoQiMotionStandupTask() 00067 { 00068 } 00069 00070 00071 #define _t(x) times.arrayPush(x) 00072 #define _tc() times.clear() 00073 #define _tp() all_times.arrayPush(times) 00074 00075 #define _a(x) angles.arrayPush(x) 00076 #define _ac() angles.clear() 00077 #define _ap() all_angles.arrayPush(angles) 00078 00079 void 00080 NaoQiMotionStandupTask::goto_start_pos() 00081 { 00082 ALValue joints, angles, times, all_angles, all_times; 00083 00084 joints.arrayPush("HeadPitch"); joints.arrayPush("HeadYaw"); 00085 joints.arrayPush("LAnklePitch"); joints.arrayPush("LAnkleRoll"); 00086 joints.arrayPush("LElbowRoll"); joints.arrayPush("LElbowYaw"); 00087 joints.arrayPush("LHipPitch"); joints.arrayPush("LHipRoll"); 00088 joints.arrayPush("LHipYawPitch"); joints.arrayPush("LKneePitch"); 00089 joints.arrayPush("LShoulderPitch"); joints.arrayPush("LShoulderRoll"); 00090 joints.arrayPush("RAnklePitch"); joints.arrayPush("RAnkleRoll"); 00091 joints.arrayPush("RElbowRoll"); joints.arrayPush("RElbowYaw"); 00092 joints.arrayPush("RHipPitch"); joints.arrayPush("RHipRoll"); 00093 joints.arrayPush("RKneePitch"); joints.arrayPush("RShoulderPitch"); 00094 joints.arrayPush("RShoulderRoll"); 00095 00096 times = ALValue::array(1.9, 2.9); 00097 00098 for (unsigned int i = 0; i < joints.getSize(); ++i) { 00099 all_times.arrayPush(times); 00100 } 00101 00102 _ac(); _a(0); _a(0); _ap(); 00103 _ac(); _a(3.37175e-007); _a(3.37175e-007); _ap(); 00104 _ac(); _a(0.523599); _a(0.523599); _ap(); 00105 _ac(); _a(0); _a(0); _ap(); 00106 _ac(); _a(0); _a(0); _ap(); 00107 _ac(); _a(0); _a(0); _ap(); 00108 _ac(); _a(0); _a(0); _ap(); 00109 _ac(); _a(0); _a(0); _ap(); 00110 _ac(); _a(0); _a(0); _ap(); 00111 _ac(); _a(0); _a(0); _ap(); 00112 _ac(); _a(0); _a(0); _ap(); 00113 _ac(); _a(1.5708); _a(1.5708); _ap(); 00114 _ac(); _a(0.523599); _a(0.523599); _ap(); 00115 _ac(); _a(0); _a(0); _ap(); 00116 _ac(); _a(0); _a(0); _ap(); 00117 _ac(); _a(0); _a(0); _ap(); 00118 _ac(); _a(0); _a(0); _ap(); 00119 _ac(); _a(0); _a(0); _ap(); 00120 _ac(); _a(0); _a(0); _ap(); 00121 _ac(); _a(0); _a(0); _ap(); 00122 _ac(); _a(-1.5708); _a(-1.5708); _ap(); 00123 00124 00125 bool is_absolute = true; 00126 __almotion->angleInterpolation(joints, all_angles, all_times, is_absolute); 00127 } 00128 00129 void 00130 NaoQiMotionStandupTask::standup_from_back() 00131 { 00132 00133 ALValue joints, angles, times, all_angles, all_times; 00134 00135 goto_start_pos(); 00136 00137 joints.arrayPush("HeadPitch"); joints.arrayPush("HeadYaw"); 00138 joints.arrayPush("LAnklePitch"); joints.arrayPush("LAnkleRoll"); 00139 joints.arrayPush("LElbowRoll"); joints.arrayPush("LElbowYaw"); 00140 joints.arrayPush("LHipPitch"); joints.arrayPush("LHipRoll"); 00141 joints.arrayPush("LHipYawPitch"); joints.arrayPush("LKneePitch"); 00142 joints.arrayPush("LShoulderPitch"); joints.arrayPush("LShoulderRoll"); 00143 joints.arrayPush("RAnklePitch"); joints.arrayPush("RAnkleRoll"); 00144 joints.arrayPush("RElbowRoll"); joints.arrayPush("RElbowYaw"); 00145 joints.arrayPush("RHipPitch"); joints.arrayPush("RHipRoll"); 00146 joints.arrayPush("RHipYawPitch"); joints.arrayPush("RKneePitch"); 00147 joints.arrayPush("RShoulderPitch"); joints.arrayPush("RShoulderRoll"); 00148 00149 _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(); 00150 _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(); 00151 _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(); 00152 _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(); 00153 _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(); 00154 _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(); 00155 _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(); 00156 _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(); 00157 _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(); 00158 _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(); 00159 _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(); 00160 _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(); 00161 _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(); 00162 _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(); 00163 _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(); 00164 _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(); 00165 _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(); 00166 _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(); 00167 _ac(); _a(-0.499093); _a(-0.858972); _a(-0.402255); _a(-0.402255); _a(-0.402255); _ap(); 00168 _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(); 00169 _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(); 00170 _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(); 00171 00172 _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(); 00173 _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(); 00174 _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(); 00175 _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(); 00176 _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(); 00177 _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(); 00178 _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(); 00179 _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(); 00180 _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(); 00181 _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(); 00182 _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(); 00183 _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(); 00184 _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(); 00185 _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(); 00186 _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(); 00187 _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(); 00188 _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(); 00189 _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(); 00190 _tc(); _t(4.9); _t(5.8); _t(6.8); _t(7.3); _t(8.4); _tp(); 00191 _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(); 00192 _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(); 00193 _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(); 00194 00195 bool is_absolute = true; 00196 __almotion->angleInterpolation(joints, all_angles, all_times, is_absolute); 00197 00198 } 00199 00200 void 00201 NaoQiMotionStandupTask::standup_from_front() 00202 { 00203 ALValue joints, angles, times, all_angles, all_times; 00204 00205 goto_start_pos(); 00206 00207 joints.arrayPush("HeadPitch"); 00208 joints.arrayPush("LAnklePitch"); joints.arrayPush("LAnkleRoll"); 00209 joints.arrayPush("LElbowRoll"); joints.arrayPush("LElbowYaw"); 00210 joints.arrayPush("LHipPitch"); joints.arrayPush("LHipRoll"); 00211 joints.arrayPush("LHipYawPitch"); joints.arrayPush("LKneePitch"); 00212 joints.arrayPush("LShoulderPitch"); joints.arrayPush("LShoulderRoll"); 00213 joints.arrayPush("RAnklePitch"); joints.arrayPush("RAnkleRoll"); 00214 joints.arrayPush("RElbowRoll"); joints.arrayPush("RElbowYaw"); 00215 joints.arrayPush("RHipPitch"); joints.arrayPush("RHipRoll"); 00216 joints.arrayPush("RHipYawPitch"); joints.arrayPush("RKneePitch"); 00217 joints.arrayPush("RShoulderPitch"); joints.arrayPush("RShoulderRoll"); 00218 00219 _ac(); _a(-0.575959); _a(0); _a(-0.349066); _a(-0.488692); _a(0); _a(0.279253); _ap(); 00220 _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(); 00221 _ac(); _a(0); _a(0); _a(-0.680678); _a(-0.555015); _a(-0.296706); _a(-0.10472); _a(0); _a(0); _ap(); 00222 _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(); 00223 _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(); 00224 _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(); 00225 _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(); 00226 _ac(); _a(0); _a(0); _a(-0.872665); _a(-0.872665); _a(-0.965167); _a(-0.785398); _a(0); _a(0); _ap(); 00227 _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(); 00228 _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(); 00229 _ac(); _a(0); _a(0); _a(0); _a(0); _a(0.0349066); _a(0.1309); _a(0.174533); _a(0.191986); _ap(); 00230 _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(); 00231 _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(); 00232 _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(); 00233 _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(); 00234 _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(); 00235 _ac(); _a(0); _a(0); _a(0); _a(0); _a(-0.0872665); _a(-0.10472); _a(0.010472); _a(0.010472); _ap(); 00236 _ac(); _a(8.54618e-008); _a(9.7389e-008); _a(-0.872665); _ap(); 00237 _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(); 00238 _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(); 00239 _ac(); _a(0); _a(0); _a(0); _a(0); _a(-0.0349066); _a(-0.1309); _a(-0.174533); _a(-0.191986); _ap(); 00240 00241 _tc(); _t(1.4); _t(2.4); _t(3.7); _t(5.2); _t(6.2); _t(8.4); _tp(); 00242 _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(); 00243 _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(); 00244 _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(); 00245 _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(); 00246 _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(); 00247 _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(); 00248 _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(); 00249 _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(); 00250 _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(); 00251 _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(); 00252 _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(); 00253 _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(); 00254 _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(); 00255 _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(); 00256 _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(); 00257 _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(); 00258 _tc(); _t(1.4); _t(2.4); _t(3.7); _tp(); 00259 _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(); 00260 _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(); 00261 _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(); 00262 00263 bool is_absolute = true; 00264 __almotion->angleInterpolation(joints, all_angles, all_times, is_absolute); 00265 } 00266 00267 /** Run the standup. */ 00268 void 00269 NaoQiMotionStandupTask::run() 00270 { 00271 if ( __from_pos == fawkes::HumanoidMotionInterface::STANDUP_BACK ) { 00272 //__allogger->info("NaoQiMotionStandupTask", "Explicit standup back"); 00273 standup_from_back(); 00274 } else if (__from_pos == fawkes::HumanoidMotionInterface::STANDUP_FRONT ) { 00275 //__allogger->info("NaoQiMotionStandupTask", "Explicit standup front"); 00276 standup_from_front(); 00277 } else { 00278 if ( __accel_x > 0.8 ) { 00279 //__allogger->info("NaoQiMotionStandupTask", "Standup from front (detected)"); 00280 standup_from_front(); 00281 } else if ( __accel_x < -0.8 ) { 00282 //__allogger->info("NaoQiMotionStandupTask", "Standup from back (detected)"); 00283 standup_from_back(); 00284 } else { 00285 //__allogger->error("NaoQiMotionStandupTask", 00286 // "NaoQiMotionStandupTask: Does not seem that I'm lying on the ground, " 00287 // "not standing up until you tell me from where"); 00288 } 00289 } 00290 }