Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * motion_kick_task.cpp - Make the robot kick asses... ehm soccer balls 00004 * 00005 * Created: Fri Jan 23 18:36:01 2009 00006 * Copyright 2006-2011 Tim Niemueller [www.niemueller.de] 00007 * 2010 Patrick Podbregar [www.podbregar.com] 00008 * 00009 ****************************************************************************/ 00010 00011 /* This program is free software; you can redistribute it and/or modify 00012 * it under the terms of the GNU General Public License as published by 00013 * the Free Software Foundation; either version 2 of the License, or 00014 * (at your option) any later version. 00015 * 00016 * This program is distributed in the hope that it will be useful, 00017 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00018 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00019 * GNU Library General Public License for more details. 00020 * 00021 * Read the full text in the LICENSE.GPL file in the doc directory. 00022 */ 00023 00024 #include "motion_kick_task.h" 00025 00026 #include <core/exceptions/system.h> 00027 #include <utils/math/angle.h> 00028 00029 #include <cstring> 00030 #include <cstdlib> 00031 #include <string> 00032 #include <unistd.h> 00033 00034 using namespace AL; 00035 using namespace fawkes; 00036 00037 /** @class NaoQiMotionKickTask "motion_kick_task.h" 00038 * NaoQi kick task. 00039 * This task can be used to make the robot kick in a non-blocking way. It will 00040 * use (blocking) ALMotion calls to execute the move. Note that ALMotion should 00041 * not be used otherwise while kicking. 00042 * @author Tim Niemueller 00043 */ 00044 00045 /** Constructor. 00046 * @param almotion ALMotion proxy 00047 * @param leg leg to kick with 00048 */ 00049 NaoQiMotionKickTask::NaoQiMotionKickTask(AL::ALPtr<AL::ALMotionProxy> almotion, 00050 fawkes::HumanoidMotionInterface::LegEnum leg) 00051 { 00052 __quit = false; 00053 __almotion = almotion; 00054 __leg = leg; 00055 00056 // ALTask variable to cause auto-destruct when done 00057 fAutoDelete = true; 00058 } 00059 00060 00061 /** Destructor. */ 00062 NaoQiMotionKickTask::~NaoQiMotionKickTask() 00063 { 00064 } 00065 00066 00067 /* 00068 static void 00069 print_angles(std::vector<float> angles) 00070 { 00071 for (std::vector<float>::iterator i = angles.begin(); i != angles.end(); ++i) { 00072 printf("%f, ", *i); 00073 } 00074 printf("\n"); 00075 for (std::vector<float>::iterator i = angles.begin(); i != angles.end(); ++i) { 00076 printf("%f, ", rad2deg(*i)); 00077 } 00078 printf("\n\n\n"); 00079 } 00080 */ 00081 00082 void 00083 NaoQiMotionKickTask::goto_start_pos(AL::ALValue speed, bool concurrent) 00084 { 00085 float shoulder_pitch = 120; 00086 float shoulder_roll = 15; 00087 float elbow_yaw = -90; 00088 float elbow_roll = -80; 00089 float hip_yaw_pitch = 0; 00090 float hip_roll = 0; 00091 float hip_pitch = -25; 00092 float knee_pitch = 40; 00093 float ankle_pitch = -20; 00094 float ankle_roll = 0; 00095 00096 ALValue target_angles; 00097 target_angles.arraySetSize(20); 00098 00099 // left arm 00100 target_angles[0] = deg2rad(shoulder_pitch); 00101 target_angles[1] = deg2rad(shoulder_roll); 00102 target_angles[2] = deg2rad(elbow_yaw); 00103 target_angles[3] = deg2rad(elbow_roll); 00104 // right arm 00105 target_angles[4] = deg2rad(shoulder_pitch); 00106 target_angles[5] = -deg2rad(shoulder_roll); 00107 target_angles[6] = -deg2rad(elbow_yaw); 00108 target_angles[7] = -deg2rad(elbow_roll); 00109 // left leg 00110 target_angles[8] = deg2rad(hip_yaw_pitch); 00111 target_angles[9] = deg2rad(hip_roll); 00112 target_angles[10] = deg2rad(hip_pitch); 00113 target_angles[11] = deg2rad(knee_pitch); 00114 target_angles[12] = deg2rad(ankle_pitch); 00115 target_angles[13] = deg2rad(ankle_roll); 00116 // right leg 00117 target_angles[14] = deg2rad(hip_yaw_pitch); 00118 target_angles[15] = deg2rad(hip_roll); 00119 target_angles[16] = deg2rad(hip_pitch); 00120 target_angles[17] = deg2rad(knee_pitch); 00121 target_angles[18] = deg2rad(ankle_pitch); 00122 target_angles[19] = -deg2rad(ankle_roll); 00123 00124 ALValue names = ALValue::array("LArm", "RArm", "LLeg", "RLeg"); 00125 00126 if (concurrent) { 00127 __almotion->post.angleInterpolationWithSpeed(names, target_angles, speed); 00128 } else { 00129 __almotion->angleInterpolationWithSpeed(names, target_angles, speed); 00130 } 00131 } 00132 00133 00134 /** Stop the current kick task. 00135 * Stops the current motion and posts a goto for the start position. 00136 * This is not stable from all configurations but seems to suffices 00137 * most of the time. 00138 */ 00139 void 00140 NaoQiMotionKickTask::exitTask() 00141 { 00142 __quit = true; 00143 std::vector<std::string> joint_names = __almotion->getJointNames("Body"); 00144 __almotion->killTasksUsingResources(joint_names); 00145 goto_start_pos(0.2, true); 00146 } 00147 00148 /** Run the kick. */ 00149 void 00150 NaoQiMotionKickTask::run() 00151 { 00152 const char *shoot_hip_roll_name = NULL; 00153 const char *support_hip_roll_name = NULL; 00154 const char *shoot_hip_pitch_name = NULL; 00155 const char *support_hip_pitch_name = NULL; 00156 const char *shoot_knee_pitch_name = NULL; 00157 const char *shoot_ankle_pitch_name = NULL; 00158 const char *shoot_ankle_roll_name = NULL; 00159 const char *support_ankle_roll_name = NULL; 00160 00161 float shoot_hip_roll = 0; 00162 float support_hip_roll = 0; 00163 float shoot_hip_pitch = 0; 00164 float support_hip_pitch = 0; 00165 float shoot_knee_pitch = 0; 00166 float shoot_ankle_pitch = 0; 00167 float shoot_ankle_roll = 0; 00168 float support_ankle_roll = 0; 00169 00170 float BALANCE_HIP_ROLL = 0; 00171 float BALANCE_ANKLE_ROLL = 0; 00172 float STRIKE_OUT_HIP_ROLL = 0; 00173 00174 if ( __leg == fawkes::HumanoidMotionInterface::LEG_LEFT ) { 00175 shoot_hip_roll_name = "LHipRoll"; 00176 support_hip_roll_name = "RHipRoll"; 00177 shoot_hip_pitch_name = "LHipPitch"; 00178 support_hip_pitch_name = "RHipPitch"; 00179 shoot_knee_pitch_name = "LKneePitch"; 00180 shoot_ankle_pitch_name = "LAnklePitch"; 00181 shoot_ankle_roll_name = "LAnkleRoll"; 00182 support_ankle_roll_name = "RAnkleRoll"; 00183 00184 BALANCE_HIP_ROLL = 20; 00185 BALANCE_ANKLE_ROLL = -25; 00186 STRIKE_OUT_HIP_ROLL = 30; 00187 } else if (__leg == fawkes::HumanoidMotionInterface::LEG_RIGHT ) { 00188 shoot_hip_roll_name = "RHipRoll"; 00189 support_hip_roll_name = "LHipRoll"; 00190 shoot_hip_pitch_name = "RHipPitch"; 00191 support_hip_pitch_name = "LHipPitch"; 00192 shoot_knee_pitch_name = "RKneePitch"; 00193 shoot_ankle_pitch_name = "RAnklePitch"; 00194 shoot_ankle_roll_name = "RAnkleRoll"; 00195 support_ankle_roll_name = "LAnkleRoll"; 00196 00197 BALANCE_HIP_ROLL = -20; 00198 BALANCE_ANKLE_ROLL = 25; 00199 STRIKE_OUT_HIP_ROLL = -30; 00200 } 00201 00202 if (__quit) return; 00203 goto_start_pos(0.2); 00204 00205 ALValue names; 00206 ALValue target_angles; 00207 float speed = 0; 00208 00209 // Balance on supporting leg 00210 names.arraySetSize(4); 00211 target_angles.arraySetSize(4); 00212 00213 support_hip_roll = BALANCE_HIP_ROLL; 00214 shoot_hip_roll = BALANCE_HIP_ROLL; 00215 shoot_ankle_roll = BALANCE_ANKLE_ROLL; 00216 support_ankle_roll = BALANCE_ANKLE_ROLL; 00217 00218 names = ALValue::array(support_hip_roll_name, shoot_hip_roll_name, 00219 support_ankle_roll_name, shoot_ankle_roll_name); 00220 target_angles = ALValue::array(deg2rad(support_hip_roll), deg2rad(shoot_hip_roll), 00221 deg2rad(support_ankle_roll), deg2rad(shoot_ankle_roll)); 00222 speed = 0.15; 00223 00224 //if (__quit) return; 00225 __almotion->angleInterpolationWithSpeed(names, target_angles, speed); 00226 00227 names.clear(); 00228 target_angles.clear(); 00229 00230 // Raise shooting leg 00231 names.arraySetSize(3); 00232 target_angles.arraySetSize(3); 00233 00234 shoot_hip_roll = STRIKE_OUT_HIP_ROLL; 00235 shoot_knee_pitch = 90; 00236 shoot_ankle_pitch = -50; 00237 00238 names = ALValue::array(shoot_hip_roll_name, shoot_knee_pitch_name, shoot_ankle_pitch_name); 00239 target_angles = ALValue::array(deg2rad(shoot_hip_roll), deg2rad(shoot_knee_pitch), 00240 deg2rad(shoot_ankle_pitch)); 00241 speed = 0.2; 00242 00243 if (__quit) return; 00244 __almotion->angleInterpolationWithSpeed(names, target_angles, speed); 00245 00246 names.clear(); 00247 target_angles.clear(); 00248 00249 // Strike out 00250 names.arraySetSize(2); 00251 target_angles.arraySetSize(2); 00252 00253 shoot_hip_pitch = 20; 00254 support_hip_pitch = -50; 00255 00256 names = ALValue::array(shoot_hip_pitch_name, support_hip_pitch_name); 00257 target_angles = ALValue::array(deg2rad(shoot_hip_pitch), deg2rad(support_hip_pitch)); 00258 speed = 0.1; 00259 00260 if (__quit) return; 00261 __almotion->angleInterpolationWithSpeed(names, target_angles, speed); 00262 00263 names.clear(); 00264 target_angles.clear(); 00265 00266 // Shoot 00267 names.arraySetSize(4); 00268 target_angles.arraySetSize(4); 00269 00270 shoot_hip_pitch = -80; 00271 support_hip_pitch = -40; 00272 shoot_knee_pitch = 50; 00273 shoot_ankle_pitch = -30; 00274 00275 names = ALValue::array(shoot_hip_pitch_name, support_hip_pitch_name, 00276 shoot_knee_pitch_name, shoot_ankle_pitch_name); 00277 target_angles = ALValue::array(deg2rad(shoot_hip_pitch), deg2rad(support_hip_pitch), 00278 deg2rad(shoot_knee_pitch), deg2rad(shoot_ankle_pitch)); 00279 speed = 1.0; 00280 if (__quit) return; 00281 __almotion->angleInterpolationWithSpeed(names, target_angles, speed); 00282 00283 names.clear(); 00284 target_angles.clear(); 00285 00286 // Move to upright position 00287 names.arraySetSize(2); 00288 target_angles.arraySetSize(2); 00289 00290 shoot_hip_pitch = -25; 00291 support_hip_pitch = -25; 00292 00293 names = ALValue::array(shoot_hip_pitch_name, support_hip_pitch_name); 00294 target_angles = ALValue::array(deg2rad(shoot_hip_pitch), deg2rad(support_hip_pitch)); 00295 speed = 0.1; 00296 00297 if (__quit) return; 00298 __almotion->angleInterpolationWithSpeed(names, target_angles, speed); 00299 00300 //names.clear(); 00301 //target_angles.clear(); 00302 00303 if (__quit) return; 00304 goto_start_pos(0.1); 00305 }