Fawkes API  Fawkes Development Version
motion_utils.cpp
00001 
00002 /***************************************************************************
00003  *  motion_utils.cpp - Motion utility functions
00004  *
00005  *  Created: Wed Aug 17 21:54:59 2011
00006  *  Copyright  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_utils.h"
00024 
00025 namespace motion {
00026 #if 0 /* just to make Emacs auto-indent happy */
00027 }
00028 #endif
00029 
00030 /** Fix ALMotion's belief of body angles.
00031  * If body angles have been set via the DCM, ALMotions model is out of
00032  * date which can cause a quick snapping back to the last posture known
00033  * to ALMotion causing very fast and potentially dangerous movements.
00034  *
00035  * Seems not to work as expected atm.
00036  */
00037 void
00038 fix_angles(AL::ALPtr<AL::ALMotionProxy> &almotion)
00039 {
00040 
00041   almotion->setAngles("Body", almotion->getAngles("Body", true), 1.);
00042   //almotion->setStiffnesses("Body", 0.0);
00043   //almotion->setStiffnesses("Body", 1.0);
00044 }
00045 
00046 void
00047 move_joints(AL::ALPtr<AL::ALMotionProxy> &almotion,
00048             float head_yaw, float head_pitch,
00049             float l_shoulder_pitch, float l_shoulder_roll,
00050             float l_elbow_yaw, float l_elbow_roll,
00051             float l_wrist_yaw, float l_hand,
00052             float l_hip_yaw_pitch, float l_hip_roll, float l_hip_pitch,
00053             float l_knee_pitch, float l_ankle_pitch, float l_ankle_roll,
00054             float r_shoulder_pitch, float r_shoulder_roll,
00055             float r_elbow_yaw, float r_elbow_roll,
00056             float r_wrist_yaw, float r_hand,
00057             float r_hip_yaw_pitch, float r_hip_roll, float r_hip_pitch,
00058             float r_knee_pitch, float r_ankle_pitch, float r_ankle_roll,
00059             float speed)
00060 {
00061   int num_joints = almotion->getJointNames("Body").size();
00062 
00063   std::vector<float> angles;
00064   angles.push_back(head_yaw);
00065   angles.push_back(head_pitch);
00066 
00067   angles.push_back(l_shoulder_pitch);
00068   angles.push_back(l_shoulder_roll);
00069   angles.push_back(l_elbow_yaw);
00070   angles.push_back(l_elbow_roll);
00071   if (num_joints == 26) { //academic version
00072     angles.push_back(l_wrist_yaw);
00073     angles.push_back(l_hand);
00074   }
00075 
00076   angles.push_back(l_hip_yaw_pitch);
00077   angles.push_back(l_hip_roll);
00078   angles.push_back(l_hip_pitch);
00079   angles.push_back(l_knee_pitch);
00080   angles.push_back(l_ankle_pitch);
00081   angles.push_back(l_ankle_roll);
00082 
00083   angles.push_back(r_hip_yaw_pitch);
00084   angles.push_back(r_hip_roll);
00085   angles.push_back(r_hip_pitch);
00086   angles.push_back(r_knee_pitch);
00087   angles.push_back(r_ankle_pitch);
00088   angles.push_back(r_ankle_roll);
00089 
00090   angles.push_back(r_shoulder_pitch);
00091   angles.push_back(r_shoulder_roll);
00092   angles.push_back(r_elbow_yaw);
00093   angles.push_back(r_elbow_roll);
00094   if (num_joints == 26) {
00095     angles.push_back(r_wrist_yaw);
00096     angles.push_back(r_hand);
00097   }
00098 
00099   std::vector<std::string> joint_names = almotion->getJointNames("Body");
00100   almotion->killTasksUsingResources(joint_names);
00101 
00102   fix_angles(almotion);
00103 
00104   almotion->setAngles(joint_names, angles, speed);
00105 }
00106 
00107 
00108 int
00109 timed_move_joints(AL::ALPtr<AL::ALMotionProxy> &almotion,
00110                   float head_yaw, float head_pitch,
00111                   float l_shoulder_pitch, float l_shoulder_roll,
00112                   float l_elbow_yaw, float l_elbow_roll,
00113                   float l_wrist_yaw, float l_hand,
00114                   float l_hip_yaw_pitch, float l_hip_roll, float l_hip_pitch,
00115                   float l_knee_pitch, float l_ankle_pitch, float l_ankle_roll,
00116                   float r_shoulder_pitch, float r_shoulder_roll,
00117                   float r_elbow_yaw, float r_elbow_roll,
00118                   float r_wrist_yaw, float r_hand,
00119                   float r_hip_yaw_pitch, float r_hip_roll, float r_hip_pitch,
00120                   float r_knee_pitch, float r_ankle_pitch, float r_ankle_roll,
00121                   float time_sec)
00122 {
00123   int num_joints = almotion->getJointNames("Body").size();
00124 
00125   std::vector<float> angles;
00126   angles.push_back(head_yaw);
00127   angles.push_back(head_pitch);
00128 
00129   angles.push_back(l_shoulder_pitch);
00130   angles.push_back(l_shoulder_roll);
00131   angles.push_back(l_elbow_yaw);
00132   angles.push_back(l_elbow_roll);
00133   if (num_joints == 26) { //academic version
00134     angles.push_back(l_wrist_yaw);
00135     angles.push_back(l_hand);
00136   }
00137 
00138   angles.push_back(l_hip_yaw_pitch);
00139   angles.push_back(l_hip_roll);
00140   angles.push_back(l_hip_pitch);
00141   angles.push_back(l_knee_pitch);
00142   angles.push_back(l_ankle_pitch);
00143   angles.push_back(l_ankle_roll);
00144 
00145   angles.push_back(r_hip_yaw_pitch);
00146   angles.push_back(r_hip_roll);
00147   angles.push_back(r_hip_pitch);
00148   angles.push_back(r_knee_pitch);
00149   angles.push_back(r_ankle_pitch);
00150   angles.push_back(r_ankle_roll);
00151 
00152   angles.push_back(r_shoulder_pitch);
00153   angles.push_back(r_shoulder_roll);
00154   angles.push_back(r_elbow_yaw);
00155   angles.push_back(r_elbow_roll);
00156   if (num_joints == 26) {
00157     angles.push_back(r_wrist_yaw);
00158     angles.push_back(r_hand);
00159   }
00160 
00161   std::vector<std::string> joint_names = almotion->getJointNames("Body");
00162   almotion->killTasksUsingResources(joint_names);
00163 
00164   fix_angles(almotion);
00165 
00166   return almotion->post.angleInterpolation("Body", angles, time_sec, true);
00167 }
00168 
00169 
00170 } // end of namespace motion