Fawkes API  Fawkes Development Version
motion_standup_task.cpp
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 }