Fawkes API
Fawkes Development Version
|
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