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