Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * dcm_thread.cpp - Provide NaoQi DCM to Fawkes 00004 * 00005 * Created: Tue May 31 15:00:54 2011 00006 * Copyright 2006-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 "dcm_thread.h" 00024 #include "motion_utils.h" 00025 00026 #include <alproxies/allauncherproxy.h> 00027 #include <alproxies/dcmproxy.h> 00028 #include <alproxies/almotionproxy.h> 00029 #include <alproxies/almemoryproxy.h> 00030 #include <alcore/alerror.h> 00031 #include <almemoryfastaccess/almemoryfastaccess.h> 00032 00033 #include <boost/bind.hpp> 00034 00035 #include <interfaces/NaoJointStiffnessInterface.h> 00036 #include <interfaces/NaoSensorInterface.h> 00037 00038 using namespace fawkes; 00039 00040 enum SensorType { HEAD_PITCH = 0, HEAD_YAW, 00041 L_SHOULDER_PITCH, L_SHOULDER_ROLL, L_ELBOW_YAW, L_ELBOW_ROLL, 00042 L_WRIST_YAW, L_HAND, 00043 L_HIP_YAW_PITCH, L_HIP_ROLL, L_HIP_PITCH, L_KNEE_PITCH, 00044 L_ANKLE_PITCH, L_ANKLE_ROLL, 00045 R_SHOULDER_PITCH, R_SHOULDER_ROLL, R_ELBOW_YAW, R_ELBOW_ROLL, 00046 R_WRIST_YAW, R_HAND, 00047 R_HIP_YAW_PITCH, R_HIP_ROLL, R_HIP_PITCH, R_KNEE_PITCH, 00048 R_ANKLE_PITCH, R_ANKLE_ROLL, 00049 STIFF_HEAD_PITCH, STIFF_HEAD_YAW, 00050 STIFF_L_SHOULDER_PITCH, STIFF_L_SHOULDER_ROLL, 00051 STIFF_L_ELBOW_YAW, STIFF_L_ELBOW_ROLL, 00052 STIFF_L_WRIST_YAW, STIFF_L_HAND, 00053 STIFF_L_HIP_YAW_PITCH, STIFF_L_HIP_ROLL, STIFF_L_HIP_PITCH, 00054 STIFF_L_KNEE_PITCH, STIFF_L_ANKLE_PITCH, STIFF_L_ANKLE_ROLL, 00055 STIFF_R_SHOULDER_PITCH, STIFF_R_SHOULDER_ROLL, 00056 STIFF_R_ELBOW_YAW, STIFF_R_ELBOW_ROLL, 00057 STIFF_R_WRIST_YAW, STIFF_R_HAND, 00058 STIFF_R_HIP_YAW_PITCH, STIFF_R_HIP_ROLL, STIFF_R_HIP_PITCH, 00059 STIFF_R_KNEE_PITCH, STIFF_R_ANKLE_PITCH, STIFF_R_ANKLE_ROLL, 00060 ACC_X, ACC_Y, ACC_Z, GYR_X, GYR_Y, GYR_REF, ANGLE_X, ANGLE_Y, 00061 L_FSR_FL, L_FSR_FR, L_FSR_RL, L_FSR_RR, 00062 R_FSR_FL, R_FSR_FR, R_FSR_RL, R_FSR_RR, 00063 L_COP_X, L_COP_Y, L_TOTAL_WEIGHT, R_COP_X, R_COP_Y, 00064 R_TOTAL_WEIGHT, 00065 ULTRASONIC_DIRECTION, ULTRASONIC_DISTANCE, 00066 ULTRASONIC_DISTANCE_LEFT_0, ULTRASONIC_DISTANCE_LEFT_1, 00067 ULTRASONIC_DISTANCE_LEFT_2, ULTRASONIC_DISTANCE_LEFT_3, 00068 ULTRASONIC_DISTANCE_RIGHT_0, ULTRASONIC_DISTANCE_RIGHT_1, 00069 ULTRASONIC_DISTANCE_RIGHT_2, ULTRASONIC_DISTANCE_RIGHT_3, 00070 L_FOOT_BUMPER_L, L_FOOT_BUMPER_R, 00071 R_FOOT_BUMPER_L, R_FOOT_BUMPER_R, 00072 HEAD_TOUCH_FRONT, HEAD_TOUCH_MIDDLE, HEAD_TOUCH_REAR, 00073 CHEST_BUTTON, BATTERY_CHARGE, 00074 SensorTypeN}; 00075 00076 enum StiffnessJoint { STIFFJ_HEAD_PITCH = 0, STIFFJ_HEAD_YAW, 00077 STIFFJ_L_SHOULDER_PITCH, STIFFJ_L_SHOULDER_ROLL, 00078 STIFFJ_L_ELBOW_YAW, STIFFJ_L_ELBOW_ROLL, 00079 STIFFJ_L_HIP_YAW_PITCH, STIFFJ_L_HIP_ROLL, 00080 STIFFJ_L_HIP_PITCH, STIFFJ_L_KNEE_PITCH, 00081 STIFFJ_L_ANKLE_PITCH, STIFFJ_L_ANKLE_ROLL, 00082 STIFFJ_R_SHOULDER_PITCH, STIFFJ_R_SHOULDER_ROLL, 00083 STIFFJ_R_ELBOW_YAW, STIFFJ_R_ELBOW_ROLL, 00084 STIFFJ_R_HIP_YAW_PITCH, STIFFJ_R_HIP_ROLL, 00085 STIFFJ_R_HIP_PITCH, STIFFJ_R_KNEE_PITCH, 00086 STIFFJ_R_ANKLE_PITCH, STIFFJ_R_ANKLE_ROLL, 00087 STIFFJ_L_WRIST_YAW, STIFFJ_L_HAND, 00088 STIFFJ_R_WRIST_YAW, STIFFJ_R_HAND, 00089 StiffnessJointN }; 00090 00091 #define ACCELEROMETER_G_FACTOR 56. 00092 00093 // Clipping as suggested by Aldebaran at RoboCup 2008, Suzhou 00094 // Changed head yaw to +/- 1.2 according to Nao v3 red book 00095 #define HEAD_YAW_MIN -1.2 00096 #define HEAD_YAW_MAX 1.2 00097 // New values after RoboCup Workshop 2009 at Aldebaran, Paris 00098 #define L_SHOULDER_PITCH_MIN -1.7 00099 #define L_SHOULDER_PITCH_MAX 1.7 00100 #define R_SHOULDER_PITCH_MIN -1.7 00101 #define R_SHOULDER_PITCH_MAX 1.7 00102 00103 #define CLIP_VALUE(V, value) clip_value(value, V ## _MIN, V ## _MAX) 00104 00105 /** Clip value to given constraints. 00106 * @param value value to clop 00107 * @param min minimum value 00108 * @param max maximum value 00109 * @return clipped value 00110 */ 00111 static inline float 00112 clip_value(float value, float min, float max) 00113 { 00114 if (value < min) value = min; 00115 if (value > max) value = max; 00116 return value; 00117 } 00118 00119 00120 /** Thread to write data at full DCM frequency. 00121 * This thread is woken up by the DCM callback and publishes new data 00122 * if there is a reader for any of the high frequency interfaces. It 00123 * is also responsible for processing incoming commands. 00124 */ 00125 class NaoQiDCMThread::HighFreqThread : public Thread 00126 { 00127 public: 00128 /** Constructor. 00129 * @param parent parent NaoQiDCMThread to call. 00130 */ 00131 HighFreqThread(NaoQiDCMThread *parent) 00132 : Thread("NaoQiDCMThread::HighFreqThread", Thread::OPMODE_WAITFORWAKEUP), 00133 __parent(parent) 00134 { 00135 set_coalesce_wakeups(true); 00136 } 00137 00138 virtual void loop() 00139 { 00140 __parent->read_values(); 00141 if ( (__parent->__joint_pos_highfreq_if->num_readers() > 0) || 00142 (__parent->__joint_stiffness_highfreq_if->num_readers() > 0) || 00143 (__parent->__sensor_if->num_readers() > 0) ) 00144 { 00145 __parent->update_interfaces(__parent->__joint_pos_highfreq_if, 00146 __parent->__joint_stiffness_highfreq_if, 00147 __parent->__sensor_highfreq_if); 00148 } 00149 00150 __parent->process_messages(); 00151 } 00152 00153 00154 private: 00155 NaoQiDCMThread *__parent; 00156 }; 00157 00158 00159 00160 /** @class NaoQiDCMThread "dcm_thread.h" 00161 * Thread to provide DCM to Fawkes. 00162 * This thread opens a DCM proxy and updates information about hardware 00163 * in the blackboard and provides basic setting functionality to move 00164 * specific servos. 00165 * 00166 * The DCM thread writes to two sets of interfaces, at a high and a lower 00167 * frequency. The high frequency data is written if there is a reader at 00168 * each DCM callback, which results in a frequency of about 100Hz. This 00169 * should only be used if necessary, for example for custom motion pattern 00170 * generating plugins. Otherwise, and especially if in a thread hooked 00171 * into the main loop, use the lower frequency interfaces. These are 00172 * written during the sensor hook. 00173 * 00174 * @author Tim Niemueller 00175 */ 00176 00177 /** Constructor. */ 00178 NaoQiDCMThread::NaoQiDCMThread() 00179 : Thread("NaoQiDCMThread", Thread::OPMODE_WAITFORWAKEUP), 00180 BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_ACQUIRE) 00181 { 00182 } 00183 00184 00185 /** Destructor. */ 00186 NaoQiDCMThread::~NaoQiDCMThread() 00187 { 00188 } 00189 00190 00191 void 00192 NaoQiDCMThread::init() 00193 { 00194 // Is the DCM running ? 00195 try { 00196 AL::ALPtr<AL::ALLauncherProxy> launcher(new AL::ALLauncherProxy(naoqi_broker)); 00197 bool is_dcm_available = launcher->isModulePresent("DCM"); 00198 bool is_almotion_available = launcher->isModulePresent("ALMotion"); 00199 00200 if (! is_dcm_available) { 00201 throw Exception("DCMThread: NaoQi DCM is not available"); 00202 } 00203 if (! is_almotion_available) { 00204 throw Exception("DCMThread: ALMotion is not available"); 00205 } 00206 } catch (AL::ALError& e) { 00207 throw Exception("Checking module availability failed: %s", 00208 e.toString().c_str()); 00209 } 00210 00211 __dcm = naoqi_broker->getDcmProxy(); 00212 __almotion = naoqi_broker->getMotionProxy(); 00213 00214 try { 00215 AL::ALPtr<AL::ALMemoryProxy> almemory = naoqi_broker->getMemoryProxy(); 00216 std::string version = almemory->getData("RobotConfig/Body/BaseVersion", 0); 00217 unsigned int num_joints = __almotion->getJointNames("Body").size(); 00218 if (num_joints == 26) { 00219 __robot_type = NaoJointPositionInterface::ROBOTYPE_ACADEMIC; 00220 } else { 00221 __robot_type = NaoJointPositionInterface::ROBOTYPE_ROBOCUP; 00222 } 00223 __robot_version[2] = __robot_version[3] = 0; 00224 if (version[0] == 'V') version = version.substr(1); 00225 std::string::size_type pos; 00226 if ((pos = version.find_first_of(".")) != std::string::npos) { 00227 std::string version_major = version.substr(0, pos); 00228 std::string version_minor = version.substr(pos+1); 00229 __robot_version[0] = atoi(version_major.c_str()); 00230 __robot_version[1] = atoi(version_minor.c_str()); 00231 } 00232 __usboard_version = 00233 almemory->getData("Device/DeviceList/USBoard/ProgVersion", 0); 00234 } catch (AL::ALError &e) { 00235 throw Exception("Retrieving robot info failed: %s", e.toString().c_str()); 00236 } 00237 00238 __memfa.reset(new AL::ALMemoryFastAccess()); 00239 00240 std::string prefix = "Device/SubDeviceList/"; 00241 00242 // Initialize fast memory access 00243 std::vector<std::string> keys; 00244 keys.resize(SensorTypeN); 00245 __values.resize(SensorTypeN); 00246 00247 keys[HEAD_PITCH] = prefix + "HeadPitch/Position/Sensor/Value"; 00248 keys[HEAD_YAW] = prefix + "HeadYaw/Position/Sensor/Value"; 00249 keys[L_SHOULDER_PITCH] = prefix + "LShoulderPitch/Position/Sensor/Value"; 00250 keys[L_SHOULDER_ROLL] = prefix + "LShoulderRoll/Position/Sensor/Value"; 00251 keys[L_ELBOW_YAW] = prefix + "LElbowYaw/Position/Sensor/Value"; 00252 keys[L_ELBOW_ROLL] = prefix + "LElbowRoll/Position/Sensor/Value"; 00253 keys[L_WRIST_YAW] = prefix + "LWristYaw/Position/Sensor/Value"; 00254 keys[L_HAND] = prefix + "LHand/Position/Sensor/Value"; 00255 keys[L_HIP_YAW_PITCH] = prefix + "LHipYawPitch/Position/Sensor/Value"; 00256 keys[L_HIP_ROLL] = prefix + "LHipRoll/Position/Sensor/Value"; 00257 keys[L_HIP_PITCH] = prefix + "LHipPitch/Position/Sensor/Value"; 00258 keys[L_KNEE_PITCH] = prefix + "LKneePitch/Position/Sensor/Value"; 00259 keys[L_ANKLE_PITCH] = prefix + "LAnklePitch/Position/Sensor/Value"; 00260 keys[L_ANKLE_ROLL] = prefix + "LAnkleRoll/Position/Sensor/Value"; 00261 00262 keys[R_SHOULDER_PITCH] = prefix + "RShoulderPitch/Position/Sensor/Value"; 00263 keys[R_SHOULDER_ROLL] = prefix + "RShoulderRoll/Position/Sensor/Value"; 00264 keys[R_ELBOW_YAW] = prefix + "RElbowYaw/Position/Sensor/Value"; 00265 keys[R_ELBOW_ROLL] = prefix + "RElbowRoll/Position/Sensor/Value"; 00266 keys[R_WRIST_YAW] = prefix + "RWristYaw/Position/Sensor/Value"; 00267 keys[R_HAND] = prefix + "RHand/Position/Sensor/Value"; 00268 keys[R_HIP_YAW_PITCH] = prefix + "RHipYawPitch/Position/Sensor/Value"; 00269 keys[R_HIP_ROLL] = prefix + "RHipRoll/Position/Sensor/Value"; 00270 keys[R_HIP_PITCH] = prefix + "RHipPitch/Position/Sensor/Value"; 00271 keys[R_KNEE_PITCH] = prefix + "RKneePitch/Position/Sensor/Value"; 00272 keys[R_ANKLE_PITCH] = prefix + "RAnklePitch/Position/Sensor/Value"; 00273 keys[R_ANKLE_ROLL] = prefix + "RAnkleRoll/Position/Sensor/Value"; 00274 00275 keys[STIFF_HEAD_PITCH] = prefix + "HeadPitch/Hardness/Actuator/Value"; 00276 keys[STIFF_HEAD_YAW] = prefix + "HeadYaw/Hardness/Actuator/Value"; 00277 keys[STIFF_L_SHOULDER_PITCH] = prefix + "LShoulderPitch/Hardness/Actuator/Value"; 00278 keys[STIFF_L_SHOULDER_ROLL] = prefix + "LShoulderRoll/Hardness/Actuator/Value"; 00279 keys[STIFF_L_ELBOW_YAW] = prefix + "LElbowYaw/Hardness/Actuator/Value"; 00280 keys[STIFF_L_ELBOW_ROLL] = prefix + "LElbowRoll/Hardness/Actuator/Value"; 00281 keys[STIFF_L_WRIST_YAW] = prefix + "LWristYaw/Hardness/Actuator/Value"; 00282 keys[STIFF_L_HAND] = prefix + "LHand/Hardness/Actuator/Value"; 00283 keys[STIFF_L_HIP_YAW_PITCH] = prefix + "LHipYawPitch/Hardness/Actuator/Value"; 00284 keys[STIFF_L_HIP_ROLL] = prefix + "LHipRoll/Hardness/Actuator/Value"; 00285 keys[STIFF_L_HIP_PITCH] = prefix + "LHipPitch/Hardness/Actuator/Value"; 00286 keys[STIFF_L_KNEE_PITCH] = prefix + "LKneePitch/Hardness/Actuator/Value"; 00287 keys[STIFF_L_ANKLE_PITCH] = prefix + "LAnklePitch/Hardness/Actuator/Value"; 00288 keys[STIFF_L_ANKLE_ROLL] = prefix + "LAnkleRoll/Hardness/Actuator/Value"; 00289 00290 keys[STIFF_R_SHOULDER_PITCH] = prefix + "RShoulderPitch/Hardness/Actuator/Value"; 00291 keys[STIFF_R_SHOULDER_ROLL] = prefix + "RShoulderRoll/Hardness/Actuator/Value"; 00292 keys[STIFF_R_ELBOW_YAW] = prefix + "RElbowYaw/Hardness/Actuator/Value"; 00293 keys[STIFF_R_ELBOW_ROLL] = prefix + "RElbowRoll/Hardness/Actuator/Value"; 00294 keys[STIFF_R_WRIST_YAW] = prefix + "RWristYaw/Hardness/Actuator/Value"; 00295 keys[STIFF_R_HAND] = prefix + "RHand/Hardness/Actuator/Value"; 00296 keys[STIFF_R_HIP_YAW_PITCH] = prefix + "RHipYawPitch/Hardness/Actuator/Value"; 00297 keys[STIFF_R_HIP_ROLL] = prefix + "RHipRoll/Hardness/Actuator/Value"; 00298 keys[STIFF_R_HIP_PITCH] = prefix + "RHipPitch/Hardness/Actuator/Value"; 00299 keys[STIFF_R_KNEE_PITCH] = prefix + "RKneePitch/Hardness/Actuator/Value"; 00300 keys[STIFF_R_ANKLE_PITCH] = prefix + "RAnklePitch/Hardness/Actuator/Value"; 00301 keys[STIFF_R_ANKLE_ROLL] = prefix + "RAnkleRoll/Hardness/Actuator/Value"; 00302 00303 // Inertial sensors 00304 keys[ACC_X] = prefix + "InertialSensor/AccX/Sensor/Value"; 00305 keys[ACC_Y] = prefix + "InertialSensor/AccY/Sensor/Value"; 00306 keys[ACC_Z] = prefix + "InertialSensor/AccZ/Sensor/Value"; 00307 keys[GYR_X] = prefix + "InertialSensor/GyrX/Sensor/Value"; 00308 keys[GYR_Y] = prefix + "InertialSensor/GyrY/Sensor/Value"; 00309 keys[GYR_REF] = prefix + "InertialSensor/GyrRef/Sensor/Value"; 00310 keys[ANGLE_X] = prefix + "InertialSensor/AngleX/Sensor/Value"; 00311 keys[ANGLE_Y] = prefix + "InertialSensor/AngleY/Sensor/Value"; 00312 00313 // FSR sensors 00314 keys[L_FSR_FL] = prefix + "LFoot/FSR/FrontLeft/Sensor/Value"; 00315 keys[L_FSR_FR] = prefix + "LFoot/FSR/FrontRight/Sensor/Value"; 00316 keys[L_FSR_RL] = prefix + "LFoot/FSR/RearLeft/Sensor/Value"; 00317 keys[L_FSR_RR] = prefix + "LFoot/FSR/RearRight/Sensor/Value"; 00318 keys[R_FSR_FL] = prefix + "RFoot/FSR/FrontLeft/Sensor/Value"; 00319 keys[R_FSR_FR] = prefix + "RFoot/FSR/FrontRight/Sensor/Value"; 00320 keys[R_FSR_RL] = prefix + "RFoot/FSR/RearLeft/Sensor/Value"; 00321 keys[R_FSR_RR] = prefix + "RFoot/FSR/RearRight/Sensor/Value"; 00322 keys[L_COP_X] = prefix + "LFoot/FSR/CenterOfPressure/X/Sensor/Value"; 00323 keys[L_COP_Y] = prefix + "LFoot/FSR/CenterOfPressure/Y/Sensor/Value"; 00324 keys[L_TOTAL_WEIGHT] = prefix + "LFoot/FSR/TotalWeight/Sensor/Value"; 00325 keys[R_COP_X] = prefix + "RFoot/FSR/CenterOfPressure/X/Sensor/Value"; 00326 keys[R_COP_Y] = prefix + "RFoot/FSR/CenterOfPressure/Y/Sensor/Value"; 00327 keys[R_TOTAL_WEIGHT] = prefix + "RFoot/FSR/TotalWeight/Sensor/Value"; 00328 00329 // Ultrasonic 00330 keys[ULTRASONIC_DIRECTION] = prefix + "US/Actuator/Value"; 00331 keys[ULTRASONIC_DISTANCE] = prefix + "US/Sensor/Value"; 00332 00333 keys[ULTRASONIC_DISTANCE_LEFT_0] = prefix + "US/Left/Sensor/Value"; 00334 keys[ULTRASONIC_DISTANCE_LEFT_1] = prefix + "US/Left/Sensor/Value1"; 00335 keys[ULTRASONIC_DISTANCE_LEFT_2] = prefix + "US/Left/Sensor/Value2"; 00336 keys[ULTRASONIC_DISTANCE_LEFT_3] = prefix + "US/Left/Sensor/Value3"; 00337 00338 keys[ULTRASONIC_DISTANCE_RIGHT_0] = prefix + "US/Right/Sensor/Value"; 00339 keys[ULTRASONIC_DISTANCE_RIGHT_1] = prefix + "US/Right/Sensor/Value1"; 00340 keys[ULTRASONIC_DISTANCE_RIGHT_2] = prefix + "US/Right/Sensor/Value2"; 00341 keys[ULTRASONIC_DISTANCE_RIGHT_3] = prefix + "US/Right/Sensor/Value3"; 00342 00343 // Bumpers and Buttons 00344 keys[L_FOOT_BUMPER_L] = prefix + "LFoot/Bumper/Left/Sensor/Value"; 00345 keys[L_FOOT_BUMPER_R] = prefix + "LFoot/Bumper/Right/Sensor/Value"; 00346 keys[R_FOOT_BUMPER_L] = prefix + "RFoot/Bumper/Left/Sensor/Value"; 00347 keys[R_FOOT_BUMPER_R] = prefix + "RFoot/Bumper/Right/Sensor/Value"; 00348 00349 keys[HEAD_TOUCH_FRONT] = prefix + "Head/Touch/Front/Sensor/Value"; 00350 keys[HEAD_TOUCH_MIDDLE] = prefix + "Head/Touch/Middle/Sensor/Value"; 00351 keys[HEAD_TOUCH_REAR] = prefix + "Head/Touch/Rear/Sensor/Value"; 00352 00353 keys[CHEST_BUTTON] = prefix + "ChestBoard/Button/Sensor/Value"; 00354 00355 // Battery 00356 keys[BATTERY_CHARGE] = prefix + "Battery/Charge/Sensor/Value"; 00357 00358 try { 00359 __memfa->ConnectToVariables(naoqi_broker, keys, false); 00360 } catch (AL::ALError &e) { 00361 throw Exception("Failed to setup fast memory access: %s", 00362 e.toString().c_str()); 00363 } 00364 00365 00366 // Setup alias for setting stiffness, reuse fastmem keys vector 00367 if (__robot_type == NaoJointPositionInterface::ROBOTYPE_ROBOCUP) { 00368 __alljoint_names.arraySetSize(22); 00369 } else { 00370 __alljoint_names.arraySetSize(26); 00371 __alljoint_names[STIFFJ_L_WRIST_YAW] = "LWristYaw"; 00372 __alljoint_names[STIFFJ_L_HAND] = "LHand"; 00373 __alljoint_names[STIFFJ_R_WRIST_YAW] = "RWristYaw"; 00374 __alljoint_names[STIFFJ_R_HAND] = "RHand"; 00375 } 00376 __alljoint_names[STIFFJ_HEAD_PITCH] = "HeadPitch"; 00377 __alljoint_names[STIFFJ_HEAD_YAW] = "HeadYaw"; 00378 __alljoint_names[STIFFJ_L_SHOULDER_PITCH] = "LShoulderPitch"; 00379 __alljoint_names[STIFFJ_L_SHOULDER_ROLL] = "LShoulderRoll"; 00380 __alljoint_names[STIFFJ_L_ELBOW_YAW] = "LElbowYaw"; 00381 __alljoint_names[STIFFJ_L_ELBOW_ROLL] = "LElbowRoll"; 00382 __alljoint_names[STIFFJ_L_HIP_YAW_PITCH] = "LHipYawPitch"; 00383 __alljoint_names[STIFFJ_L_HIP_ROLL] = "LHipRoll"; 00384 __alljoint_names[STIFFJ_L_HIP_PITCH] = "LHipPitch"; 00385 __alljoint_names[STIFFJ_L_KNEE_PITCH] = "LKneePitch"; 00386 __alljoint_names[STIFFJ_L_ANKLE_PITCH] = "LAnklePitch"; 00387 __alljoint_names[STIFFJ_L_ANKLE_ROLL] = "LAnkleRoll"; 00388 00389 __alljoint_names[STIFFJ_R_SHOULDER_PITCH] = "RShoulderPitch"; 00390 __alljoint_names[STIFFJ_R_SHOULDER_ROLL] = "RShoulderRoll"; 00391 __alljoint_names[STIFFJ_R_ELBOW_YAW] = "RElbowYaw"; 00392 __alljoint_names[STIFFJ_R_ELBOW_ROLL] = "RElbowRoll"; 00393 __alljoint_names[STIFFJ_R_HIP_YAW_PITCH] = "RHipYawPitch"; 00394 __alljoint_names[STIFFJ_R_HIP_ROLL] = "RHipRoll"; 00395 __alljoint_names[STIFFJ_R_HIP_PITCH] = "RHipPitch"; 00396 __alljoint_names[STIFFJ_R_KNEE_PITCH] = "RKneePitch"; 00397 __alljoint_names[STIFFJ_R_ANKLE_PITCH] = "RAnklePitch"; 00398 __alljoint_names[STIFFJ_R_ANKLE_ROLL] = "RAnkleRoll"; 00399 00400 try { 00401 AL::ALValue setJointStiffnessAlias; 00402 // Alias for all joint stiffness 00403 setJointStiffnessAlias.clear(); 00404 setJointStiffnessAlias.arraySetSize(2); 00405 setJointStiffnessAlias[0] = std::string("setJointStiffness"); 00406 setJointStiffnessAlias[1].arraySetSize(26); 00407 00408 // stiffness list 00409 int offset = STIFF_HEAD_PITCH - HEAD_PITCH; 00410 for (int i = HEAD_PITCH; i <= R_ANKLE_ROLL; ++i) { 00411 setJointStiffnessAlias[1][i] = keys[i + offset]; 00412 } 00413 00414 __dcm->createAlias(setJointStiffnessAlias); 00415 } catch (AL::ALError &e) { 00416 __memfa.reset(); 00417 throw Exception("Failed to create SetJointStiffness alias: %s", 00418 e.toString().c_str()); 00419 } 00420 00421 __joint_pos_if = 00422 blackboard->open_for_writing<NaoJointPositionInterface>("Nao Joint Positions"); 00423 __joint_stiffness_if = 00424 blackboard->open_for_writing<NaoJointStiffnessInterface> 00425 ("Nao Joint Stiffness"); 00426 __sensor_if = 00427 blackboard->open_for_writing<NaoSensorInterface>("Nao Sensors"); 00428 00429 __joint_pos_highfreq_if = 00430 blackboard->open_for_writing<NaoJointPositionInterface> 00431 ("Nao Joint Positions HF"); 00432 __joint_stiffness_highfreq_if = 00433 blackboard->open_for_writing<NaoJointStiffnessInterface> 00434 ("Nao Joint Stiffness HF"); 00435 __sensor_highfreq_if = 00436 blackboard->open_for_writing<NaoSensorInterface>("Nao Sensors HF"); 00437 00438 00439 // Get all values from ALMemory using fastaccess 00440 __dcm_time = __dcm->getTime(0); 00441 __memfa->GetValues(__values); 00442 00443 __joint_pos_if->set_robot_type(__robot_type); 00444 __joint_pos_if->set_robot_version(__robot_version); 00445 __joint_pos_highfreq_if->set_robot_type(__robot_type); 00446 __joint_pos_highfreq_if->set_robot_version(__robot_version); 00447 00448 __sensor_if->set_ultrasonic_direction(NaoSensorInterface::USD_NONE); 00449 __sensor_highfreq_if->set_ultrasonic_direction(NaoSensorInterface::USD_NONE); 00450 00451 // Write once the current data on startup 00452 update_interfaces(__joint_pos_if, __joint_stiffness_if, __sensor_if); 00453 update_interfaces(__joint_pos_highfreq_if, __joint_stiffness_highfreq_if, 00454 __sensor_highfreq_if); 00455 00456 __highfreq_thread = new NaoQiDCMThread::HighFreqThread(this); 00457 __highfreq_thread->start(); 00458 00459 __dcm_sigconn = 00460 __dcm->getGenericProxy()->getModule()-> 00461 atPostProcess(boost::bind(&NaoQiDCMThread::dcm_callback, this)); 00462 00463 /* 00464 AL::ALValue cmd; 00465 cmd.arraySetSize(3); 00466 cmd[0] = std::string("setJointStiffness"); 00467 cmd[1] = std::string("Merge"); 00468 cmd[2].arraySetSize(1); 00469 cmd[2][0].arraySetSize(2); 00470 cmd[2][0][0] = 1.0; 00471 cmd[2][0][1] = __dcm_time + 500; 00472 __dcm->set(cmd); 00473 00474 send_command("HeadPitch/Position/Sensor/Value", 0.4, 1000); 00475 */ 00476 } 00477 00478 00479 void 00480 NaoQiDCMThread::dcm_callback() 00481 { 00482 __highfreq_thread->wakeup(); 00483 } 00484 00485 void 00486 NaoQiDCMThread::finalize() 00487 { 00488 __dcm_sigconn.disconnect(); 00489 00490 __highfreq_thread->cancel(); 00491 __highfreq_thread->join(); 00492 delete __highfreq_thread; 00493 00494 blackboard->close(__joint_pos_if); 00495 blackboard->close(__joint_stiffness_if); 00496 blackboard->close(__sensor_if); 00497 blackboard->close(__joint_pos_highfreq_if); 00498 blackboard->close(__joint_stiffness_highfreq_if); 00499 blackboard->close(__sensor_highfreq_if); 00500 __joint_pos_if = NULL; 00501 __joint_stiffness_if = NULL; 00502 __sensor_if = NULL; 00503 __joint_pos_highfreq_if = NULL; 00504 __joint_stiffness_highfreq_if = NULL; 00505 __sensor_highfreq_if = NULL; 00506 00507 __memfa.reset(); 00508 __dcm.reset(); 00509 } 00510 00511 00512 void 00513 NaoQiDCMThread::read_values() 00514 { 00515 __values.lock(); 00516 __dcm_time = __dcm->getTime(0); 00517 __memfa->GetValues(__values); 00518 __values.unlock(); 00519 } 00520 00521 00522 void 00523 NaoQiDCMThread::loop() 00524 { 00525 update_interfaces(__joint_pos_if, __joint_stiffness_if, __sensor_if); 00526 } 00527 00528 void 00529 NaoQiDCMThread::update_interfaces(NaoJointPositionInterface *joint_pos_if, 00530 NaoJointStiffnessInterface *joint_stiffness_if, 00531 NaoSensorInterface *sensor_if) 00532 { 00533 // Joint Position 00534 // Head 00535 joint_pos_if->set_head_yaw(__values[HEAD_YAW]); 00536 joint_pos_if->set_head_pitch(__values[HEAD_PITCH]); 00537 // Arms 00538 joint_pos_if->set_l_shoulder_pitch(__values[L_SHOULDER_PITCH]); 00539 joint_pos_if->set_l_shoulder_roll(__values[L_SHOULDER_ROLL]); 00540 joint_pos_if->set_l_elbow_yaw(__values[L_ELBOW_YAW]); 00541 joint_pos_if->set_l_elbow_roll(__values[L_ELBOW_ROLL]); 00542 joint_pos_if->set_l_wrist_yaw(__values[L_WRIST_YAW]); 00543 joint_pos_if->set_l_hand(__values[L_HAND]); 00544 joint_pos_if->set_r_shoulder_pitch(__values[R_SHOULDER_PITCH]); 00545 joint_pos_if->set_r_shoulder_roll(__values[R_SHOULDER_ROLL]); 00546 joint_pos_if->set_r_elbow_yaw(__values[R_ELBOW_YAW]); 00547 joint_pos_if->set_r_elbow_roll(__values[R_ELBOW_ROLL]); 00548 joint_pos_if->set_r_wrist_yaw(__values[R_WRIST_YAW]); 00549 joint_pos_if->set_r_hand(__values[R_HAND]); 00550 // Hip 00551 joint_pos_if->set_l_hip_yaw_pitch(__values[L_HIP_YAW_PITCH]); 00552 joint_pos_if->set_l_hip_pitch(__values[L_HIP_PITCH]); 00553 joint_pos_if->set_l_hip_roll(__values[L_HIP_ROLL]); 00554 joint_pos_if->set_r_hip_yaw_pitch(__values[R_HIP_YAW_PITCH]); 00555 joint_pos_if->set_r_hip_pitch(__values[R_HIP_PITCH]); 00556 joint_pos_if->set_r_hip_roll(__values[R_HIP_ROLL]); 00557 // Knees 00558 joint_pos_if->set_l_knee_pitch(__values[L_KNEE_PITCH]); 00559 joint_pos_if->set_r_knee_pitch(__values[R_KNEE_PITCH]); 00560 // Feet 00561 joint_pos_if->set_l_ankle_pitch(__values[L_ANKLE_PITCH]); 00562 joint_pos_if->set_l_ankle_roll(__values[L_ANKLE_ROLL]); 00563 joint_pos_if->set_r_ankle_pitch(__values[R_ANKLE_PITCH]); 00564 joint_pos_if->set_r_ankle_roll(__values[R_ANKLE_ROLL]); 00565 00566 joint_pos_if->set_time(__dcm_time); 00567 00568 // Joint Stiffness 00569 // Head 00570 joint_stiffness_if->set_head_yaw(__values[STIFF_HEAD_YAW]); 00571 joint_stiffness_if->set_head_pitch(__values[STIFF_HEAD_PITCH]); 00572 // Arms 00573 joint_stiffness_if->set_l_shoulder_pitch(__values[STIFF_L_SHOULDER_PITCH]); 00574 joint_stiffness_if->set_l_shoulder_roll(__values[STIFF_L_SHOULDER_ROLL]); 00575 joint_stiffness_if->set_l_elbow_yaw(__values[STIFF_L_ELBOW_YAW]); 00576 joint_stiffness_if->set_l_elbow_roll(__values[STIFF_L_ELBOW_ROLL]); 00577 joint_stiffness_if->set_l_wrist_yaw(__values[STIFF_L_WRIST_YAW]); 00578 joint_stiffness_if->set_l_hand(__values[STIFF_L_HAND]); 00579 joint_stiffness_if->set_r_shoulder_pitch(__values[STIFF_R_SHOULDER_PITCH]); 00580 joint_stiffness_if->set_r_shoulder_roll(__values[STIFF_R_SHOULDER_ROLL]); 00581 joint_stiffness_if->set_r_elbow_yaw(__values[STIFF_R_ELBOW_YAW]); 00582 joint_stiffness_if->set_r_elbow_roll(__values[STIFF_R_ELBOW_ROLL]); 00583 joint_stiffness_if->set_r_wrist_yaw(__values[STIFF_R_WRIST_YAW]); 00584 joint_stiffness_if->set_r_hand(__values[STIFF_R_HAND]); 00585 // Hip 00586 joint_stiffness_if->set_l_hip_yaw_pitch(__values[STIFF_L_HIP_YAW_PITCH]); 00587 joint_stiffness_if->set_l_hip_pitch(__values[STIFF_L_HIP_PITCH]); 00588 joint_stiffness_if->set_l_hip_roll(__values[STIFF_L_HIP_ROLL]); 00589 // RHipYawPitch stiffness is always 0, copy from RYawPitch 00590 joint_stiffness_if->set_r_hip_yaw_pitch(__values[STIFF_L_HIP_YAW_PITCH]); 00591 joint_stiffness_if->set_r_hip_pitch(__values[STIFF_R_HIP_PITCH]); 00592 joint_stiffness_if->set_r_hip_roll(__values[STIFF_R_HIP_ROLL]); 00593 // Knees 00594 joint_stiffness_if->set_l_knee_pitch(__values[STIFF_L_KNEE_PITCH]); 00595 joint_stiffness_if->set_r_knee_pitch(__values[STIFF_R_KNEE_PITCH]); 00596 // Feet 00597 joint_stiffness_if->set_l_ankle_pitch(__values[STIFF_L_ANKLE_PITCH]); 00598 joint_stiffness_if->set_l_ankle_roll(__values[STIFF_L_ANKLE_ROLL]); 00599 joint_stiffness_if->set_r_ankle_pitch(__values[STIFF_R_ANKLE_PITCH]); 00600 joint_stiffness_if->set_r_ankle_roll(__values[STIFF_R_ANKLE_ROLL]); 00601 00602 float min_stiffness = 1.; 00603 for (int i = STIFF_HEAD_YAW; i <= STIFF_R_ANKLE_ROLL; ++i) { 00604 // ignore wrist and hand on RoboCup version 00605 if ( (__robot_type == NaoJointPositionInterface::ROBOTYPE_ROBOCUP) && 00606 ( (i == STIFF_L_WRIST_YAW) || (i == STIFF_L_HAND) || 00607 (i == STIFF_R_WRIST_YAW) || (i == STIFF_R_HAND) ) ) 00608 continue; 00609 00610 // ignore RHipYawPitch stiffness, it's always 0 00611 if (i == STIFF_R_HIP_YAW_PITCH) continue; 00612 00613 if (__values[i] < min_stiffness) min_stiffness = __values[i]; 00614 } 00615 joint_stiffness_if->set_minimum(min_stiffness); 00616 00617 // Sensors 00618 // FSRs 00619 sensor_if->set_l_fsr_fl(__values[L_FSR_FL]); 00620 sensor_if->set_l_fsr_fr(__values[L_FSR_FR]); 00621 sensor_if->set_l_fsr_rl(__values[L_FSR_RL]); 00622 sensor_if->set_l_fsr_rr(__values[L_FSR_RR]); 00623 sensor_if->set_r_fsr_fl(__values[R_FSR_FL]); 00624 sensor_if->set_r_fsr_fr(__values[R_FSR_FR]); 00625 sensor_if->set_r_fsr_rl(__values[R_FSR_RL]); 00626 sensor_if->set_r_fsr_rr(__values[R_FSR_RR]); 00627 00628 sensor_if->set_l_cop_x(__values[L_COP_X]); 00629 sensor_if->set_l_cop_y(__values[L_COP_Y]); 00630 sensor_if->set_l_total_weight(__values[L_TOTAL_WEIGHT]); 00631 00632 sensor_if->set_r_cop_x(__values[R_COP_X]); 00633 sensor_if->set_r_cop_y(__values[R_COP_Y]); 00634 sensor_if->set_r_total_weight(__values[R_TOTAL_WEIGHT]); 00635 00636 // Buttons and bumpers 00637 sensor_if->set_chest_button((__values[CHEST_BUTTON] >= 0.5) ? 1 : 0); 00638 sensor_if->set_head_touch_front((__values[HEAD_TOUCH_FRONT] >= 0.5) ? 1 : 0); 00639 sensor_if->set_head_touch_middle((__values[HEAD_TOUCH_MIDDLE] >= 0.5) ? 1 : 0); 00640 sensor_if->set_head_touch_rear((__values[HEAD_TOUCH_REAR] >= 0.5) ? 1 : 0); 00641 00642 sensor_if->set_l_foot_bumper_l((__values[L_FOOT_BUMPER_L] >= 0.5) ? 1 : 0); 00643 sensor_if->set_l_foot_bumper_r((__values[L_FOOT_BUMPER_R] >= 0.5) ? 1 : 0); 00644 sensor_if->set_r_foot_bumper_l((__values[R_FOOT_BUMPER_L] >= 0.5) ? 1 : 0); 00645 sensor_if->set_r_foot_bumper_r((__values[R_FOOT_BUMPER_R] >= 0.5) ? 1 : 0); 00646 00647 // Inertial measurement unit 00648 sensor_if->set_accel_x(__values[ACC_X] / ACCELEROMETER_G_FACTOR); 00649 sensor_if->set_accel_y(__values[ACC_Y] / ACCELEROMETER_G_FACTOR); 00650 sensor_if->set_accel_z(__values[ACC_Z] / ACCELEROMETER_G_FACTOR); 00651 00652 sensor_if->set_gyro_x(__values[GYR_X]); 00653 sensor_if->set_gyro_y(__values[GYR_Y]); 00654 sensor_if->set_gyro_ref(__values[GYR_REF]); 00655 00656 sensor_if->set_angle_x(__values[ANGLE_X]); 00657 sensor_if->set_angle_y(__values[ANGLE_Y]); 00658 00659 // Ultrasonic sound 00660 NaoSensorInterface::UltrasonicDirection us_dir = 00661 sensor_if->ultrasonic_direction(); 00662 switch (us_dir) { 00663 case NaoSensorInterface::USD_LEFT_LEFT: 00664 case NaoSensorInterface::USD_RIGHT_LEFT: 00665 { 00666 float us_left[4] = {__values[ULTRASONIC_DISTANCE], 0, 0, 0}; 00667 sensor_if->set_ultrasonic_distance_left(us_left); 00668 00669 float us_right[4] = {0, 0, 0, 0}; 00670 sensor_if->set_ultrasonic_distance_right(us_right); 00671 } 00672 break; 00673 case NaoSensorInterface::USD_LEFT_RIGHT: 00674 case NaoSensorInterface::USD_RIGHT_RIGHT: 00675 { 00676 float us_left[4] = {0, 0, 0, 0}; 00677 sensor_if->set_ultrasonic_distance_left(us_left); 00678 00679 float us_right[4] = {__values[ULTRASONIC_DISTANCE], 0, 0, 0}; 00680 sensor_if->set_ultrasonic_distance_right(us_right); 00681 } 00682 break; 00683 00684 default: 00685 { 00686 float us_left[4] = {__values[ULTRASONIC_DISTANCE_LEFT_0], 00687 __values[ULTRASONIC_DISTANCE_LEFT_1], 00688 __values[ULTRASONIC_DISTANCE_LEFT_2], 00689 __values[ULTRASONIC_DISTANCE_LEFT_3]}; 00690 sensor_if->set_ultrasonic_distance_left(us_left); 00691 00692 float us_right[4] = {__values[ULTRASONIC_DISTANCE_RIGHT_0], 00693 __values[ULTRASONIC_DISTANCE_RIGHT_1], 00694 __values[ULTRASONIC_DISTANCE_RIGHT_2], 00695 __values[ULTRASONIC_DISTANCE_RIGHT_3]}; 00696 sensor_if->set_ultrasonic_distance_right(us_right); 00697 } 00698 break; 00699 } 00700 00701 // Battery 00702 sensor_if->set_battery_charge(__values[BATTERY_CHARGE]); 00703 00704 // Write to blackboard 00705 joint_pos_if->write(); 00706 joint_stiffness_if->write(); 00707 sensor_if->write(); 00708 } 00709 00710 00711 void 00712 NaoQiDCMThread::process_messages() 00713 { 00714 // *** Joint position messages 00715 while (! __joint_pos_if->msgq_empty()) { 00716 if (NaoJointPositionInterface::SetServoMessage *msg = 00717 __joint_pos_if->msgq_first_safe(msg)) 00718 { 00719 send_commands(msg->servo(), "Position", msg->value(), msg->time()); 00720 } 00721 00722 else if (NaoJointPositionInterface::SetServosMessage *msg = 00723 __joint_pos_if->msgq_first_safe(msg)) 00724 { 00725 } 00726 00727 else if (NaoJointPositionInterface::MoveServoMessage *msg = 00728 __joint_pos_if->msgq_first_safe(msg)) 00729 { 00730 std::vector<std::string> servos = parse_servo_bitfield(msg->servo()); 00731 std::vector<float> values(servos.size(), msg->value()); 00732 __almotion->setAngles(servos, values, msg->speed()); 00733 } 00734 00735 else if (NaoJointPositionInterface::MoveServosMessage *msg = 00736 __joint_pos_if->msgq_first_safe(msg)) 00737 { 00738 motion::move_joints( 00739 __almotion, 00740 /* head */ msg->head_yaw(), msg->head_pitch(), 00741 /* l shoulder */ msg->l_shoulder_pitch(), msg->l_shoulder_roll(), 00742 /* l elbow */ msg->l_elbow_yaw(), msg->l_elbow_roll(), 00743 /* l wrist/hand */ msg->l_wrist_yaw(), msg->l_hand(), 00744 /* l hip */ msg->l_hip_yaw_pitch(), msg->l_hip_roll(), msg->l_hip_pitch(), 00745 /* l knee */ msg->l_knee_pitch(), 00746 /* l ankle */ msg->l_ankle_pitch(), msg->l_ankle_roll(), 00747 /* r shoulder */ msg->r_shoulder_pitch(), msg->r_shoulder_roll(), 00748 /* r elbow */ msg->r_elbow_yaw(), msg->r_elbow_roll(), 00749 /* r wrist/hand */ msg->r_wrist_yaw(), msg->r_hand(), 00750 /* r hip */ msg->r_hip_yaw_pitch(), msg->r_hip_roll(), msg->r_hip_pitch(), 00751 /* r knee */ msg->r_knee_pitch(), 00752 /* r ankle */ msg->r_ankle_pitch(), msg->r_ankle_roll(), 00753 /* speed */ msg->speed()); 00754 } 00755 00756 __joint_pos_if->msgq_pop(); 00757 } 00758 00759 // *** Joint stiffness messages 00760 while (! __joint_stiffness_if->msgq_empty()) { 00761 if (NaoJointStiffnessInterface::SetStiffnessMessage *msg = 00762 __joint_stiffness_if->msgq_first_safe(msg)) 00763 { 00764 /* DCM version, disfunctional due to ALMotion deficiencies 00765 send_commands(msg->servo(), "Hardness", msg->value(), 00766 "Merge", (int)roundf(1000. * msg->time_sec())); 00767 */ 00768 00769 std::vector<std::string> servos = parse_servo_bitfield(msg->servo()); 00770 std::vector<float> values(servos.size(), msg->value()); 00771 00772 __almotion->post.stiffnessInterpolation(servos, values, 00773 msg->time_sec()); 00774 00775 } 00776 else if (NaoJointStiffnessInterface::SetBodyStiffnessMessage *msg = 00777 __joint_stiffness_if->msgq_first_safe(msg)) 00778 { 00779 /* Cannot be used atm because ALMotion will not update its internal 00780 * belief of stiffness values causing any further motion via DCM 00781 * or ALMotion to fail 00782 // use setJointStiffness alias setup in init() 00783 AL::ALValue cmd; 00784 cmd.arraySetSize(3); 00785 cmd[0] = std::string("setJointStiffness"); 00786 cmd[1] = std::string("Merge"); 00787 cmd[2].arraySetSize(1); 00788 cmd[2][0].arraySetSize(2); 00789 cmd[2][0][0] = msg->value(); 00790 cmd[2][0][1] = __dcm_time + (int)roundf(1000. * msg->time_sec()); 00791 try { 00792 __dcm->set(cmd); 00793 } catch (const AL::ALError &e) { 00794 logger->log_warn(name(), "Failed to call setJointStiffness: %s", 00795 e.toString().c_str()); 00796 } 00797 */ 00798 00799 __almotion->post.stiffnessInterpolation("Body", msg->value(), 00800 msg->time_sec()); 00801 } 00802 00803 else if (NaoJointStiffnessInterface::SetStiffnessesMessage *msg = 00804 __joint_stiffness_if->msgq_first_safe(msg)) 00805 { 00806 std::vector<float> values(__alljoint_names.getSize()); 00807 values[STIFFJ_HEAD_PITCH] = msg->head_pitch(); 00808 values[STIFFJ_HEAD_YAW] = msg->head_yaw(); 00809 values[STIFFJ_L_SHOULDER_PITCH] = msg->l_shoulder_pitch(); 00810 values[STIFFJ_L_SHOULDER_ROLL] = msg->l_shoulder_roll(); 00811 values[STIFFJ_L_ELBOW_YAW] = msg->l_elbow_yaw(); 00812 values[STIFFJ_L_ELBOW_ROLL] = msg->l_elbow_roll(); 00813 values[STIFFJ_L_HIP_YAW_PITCH] = msg->l_hip_yaw_pitch(); 00814 values[STIFFJ_L_HIP_ROLL] = msg->l_hip_roll(); 00815 values[STIFFJ_L_HIP_PITCH] = msg->l_hip_pitch(); 00816 values[STIFFJ_L_KNEE_PITCH] = msg->l_knee_pitch(); 00817 values[STIFFJ_L_ANKLE_PITCH] = msg->l_ankle_pitch(); 00818 values[STIFFJ_L_ANKLE_ROLL] = msg->l_ankle_roll(); 00819 00820 values[STIFFJ_R_SHOULDER_PITCH] = msg->r_shoulder_pitch(); 00821 values[STIFFJ_R_SHOULDER_ROLL] = msg->r_shoulder_roll(); 00822 values[STIFFJ_R_ELBOW_YAW] = msg->r_elbow_yaw(); 00823 values[STIFFJ_R_ELBOW_ROLL] = msg->r_elbow_roll(); 00824 values[STIFFJ_R_HIP_YAW_PITCH] = msg->r_hip_yaw_pitch(); 00825 values[STIFFJ_R_HIP_ROLL] = msg->r_hip_roll(); 00826 values[STIFFJ_R_HIP_PITCH] = msg->r_hip_pitch(); 00827 values[STIFFJ_R_KNEE_PITCH] = msg->r_knee_pitch(); 00828 values[STIFFJ_R_ANKLE_PITCH] = msg->r_ankle_pitch(); 00829 values[STIFFJ_R_ANKLE_ROLL] = msg->r_ankle_roll(); 00830 00831 if (__robot_type != NaoJointPositionInterface::ROBOTYPE_ROBOCUP) { 00832 values[STIFFJ_L_WRIST_YAW] = msg->l_wrist_yaw(); 00833 values[STIFFJ_L_HAND] = msg->l_hand(); 00834 values[STIFFJ_R_WRIST_YAW] = msg->r_wrist_yaw(); 00835 values[STIFFJ_R_HAND] = msg->r_hand(); 00836 } 00837 00838 __almotion->post.stiffnessInterpolation(__alljoint_names, values, 00839 msg->time_sec()); 00840 } 00841 00842 __joint_stiffness_if->msgq_pop(); 00843 } 00844 00845 // *** Sensor messages 00846 while (! __sensor_if->msgq_empty()) { 00847 if (NaoSensorInterface::EmitUltrasonicWaveMessage *msg = 00848 __sensor_if->msgq_first_safe(msg)) 00849 { 00850 int value = ultrasonic_value(msg->ultrasonic_direction()); 00851 send_command("US/Actuator/Value", value, "ClearAll", 0); 00852 00853 __sensor_if->set_ultrasonic_direction(msg->ultrasonic_direction()); 00854 __sensor_highfreq_if->set_ultrasonic_direction(msg->ultrasonic_direction()); 00855 00856 } else if (NaoSensorInterface::StartUltrasonicMessage *msg = 00857 __sensor_if->msgq_first_safe(msg)) 00858 { 00859 int value = ultrasonic_value(msg->ultrasonic_direction()); 00860 value += 64; 00861 send_command("US/Actuator/Value", value, "ClearAll", 0); 00862 00863 __sensor_if->set_ultrasonic_direction(msg->ultrasonic_direction()); 00864 __sensor_highfreq_if->set_ultrasonic_direction(msg->ultrasonic_direction()); 00865 00866 } else if (NaoSensorInterface::StopUltrasonicMessage *msg = 00867 __sensor_if->msgq_first_safe(msg)) 00868 { 00869 send_command("US/Actuator/Value", 0, "ClearAll", 0); 00870 00871 __sensor_if->set_ultrasonic_direction(NaoSensorInterface::USD_NONE); 00872 __sensor_highfreq_if->set_ultrasonic_direction(NaoSensorInterface::USD_NONE); 00873 } 00874 00875 __sensor_if->msgq_pop(); 00876 } 00877 00878 } 00879 00880 00881 void 00882 NaoQiDCMThread::send_commands(unsigned int servos, std::string what, 00883 float value, int time_offset) 00884 { 00885 /* 00886 __almotion->wbEnable(false); 00887 __almotion->wbEnableEffectorControl("LArm", false); 00888 __almotion->wbEnableEffectorControl("RArm", false); 00889 __almotion->setWalkArmsEnable(false, false); 00890 __almotion->killAll(); 00891 */ 00892 00893 std::vector<std::string> servonames = parse_servo_bitfield(servos); 00894 00895 std::vector<std::string>::iterator s; 00896 for (s = servonames.begin(); s != servonames.end(); ++s) { 00897 float v = value; 00898 if (*s == "HeadYaw") { 00899 v = CLIP_VALUE(HEAD_YAW, value); 00900 } else if (*s == "LShoulderPitch") { 00901 v = CLIP_VALUE(L_SHOULDER_PITCH, value); 00902 } else if (*s == "RShoulderPitch") { 00903 v = CLIP_VALUE(R_SHOULDER_PITCH, value); 00904 } 00905 send_command(*s + "/" + what + "/Actuator/Value", 00906 v, "Merge", time_offset); 00907 } 00908 } 00909 00910 void 00911 NaoQiDCMThread::send_command(std::string name, float value, 00912 std::string kind, int time_offset) 00913 { 00914 AL::ALValue cmd; 00915 00916 /* 00917 printf("Command for %s to %f in %i ms (%i vs. %i)\n", 00918 name.c_str(), value, time_offset, __dcm->getTime(time_offset), 00919 __dcm_time + time_offset); 00920 */ 00921 00922 cmd.arraySetSize(3); 00923 cmd[0] = name; 00924 cmd[1] = kind; 00925 cmd[2].arraySetSize(1); 00926 cmd[2][0].arraySetSize(2); 00927 cmd[2][0][0] = value; 00928 cmd[2][0][1] = __dcm_time + time_offset; 00929 00930 __dcm->set(cmd); 00931 } 00932 00933 00934 std::vector<std::string> 00935 NaoQiDCMThread::parse_servo_bitfield(unsigned int servos) 00936 { 00937 std::vector<std::string> servonames; 00938 00939 if ( servos & NaoJointPositionInterface::SERVO_head_yaw ) 00940 servonames.push_back("HeadYaw"); 00941 00942 if ( servos & NaoJointPositionInterface::SERVO_head_pitch ) 00943 servonames.push_back("HeadPitch"); 00944 00945 if ( servos & NaoJointPositionInterface::SERVO_l_shoulder_pitch ) 00946 servonames.push_back("LShoulderPitch"); 00947 00948 if ( servos & NaoJointPositionInterface::SERVO_l_shoulder_roll ) 00949 servonames.push_back("LShoulderRoll"); 00950 00951 if ( servos & NaoJointPositionInterface::SERVO_l_elbow_yaw ) 00952 servonames.push_back("LElbowYaw"); 00953 00954 if ( servos & NaoJointPositionInterface::SERVO_l_elbow_roll ) 00955 servonames.push_back("LElbowRoll"); 00956 00957 if ( servos & NaoJointPositionInterface::SERVO_r_shoulder_pitch ) 00958 servonames.push_back("RShoulderPitch"); 00959 00960 if ( servos & NaoJointPositionInterface::SERVO_r_shoulder_roll ) 00961 servonames.push_back("RShoulderRoll"); 00962 00963 if ( servos & NaoJointPositionInterface::SERVO_r_elbow_yaw ) 00964 servonames.push_back("RElbowYaw"); 00965 00966 if ( servos & NaoJointPositionInterface::SERVO_r_elbow_roll ) 00967 servonames.push_back("RElbowRoll"); 00968 00969 if ( servos & NaoJointPositionInterface::SERVO_l_hip_yaw_pitch ) 00970 servonames.push_back("LHipYawPitch"); 00971 00972 if ( servos & NaoJointPositionInterface::SERVO_l_hip_pitch ) 00973 servonames.push_back("LHipPitch"); 00974 00975 if ( servos & NaoJointPositionInterface::SERVO_l_hip_roll ) 00976 servonames.push_back("LHipRoll"); 00977 00978 if ( servos & NaoJointPositionInterface::SERVO_r_hip_yaw_pitch ) 00979 servonames.push_back("RHipYawPitch"); 00980 00981 if ( servos & NaoJointPositionInterface::SERVO_r_hip_pitch ) 00982 servonames.push_back("RHipPitch"); 00983 00984 if ( servos & NaoJointPositionInterface::SERVO_r_hip_roll ) 00985 servonames.push_back("RHipRoll"); 00986 00987 if ( servos & NaoJointPositionInterface::SERVO_l_knee_pitch ) 00988 servonames.push_back("LKneePitch"); 00989 00990 if ( servos & NaoJointPositionInterface::SERVO_r_knee_pitch ) 00991 servonames.push_back("RKneePitch"); 00992 00993 if ( servos & NaoJointPositionInterface::SERVO_l_ankle_pitch ) 00994 servonames.push_back("LAnklePitch"); 00995 00996 if ( servos & NaoJointPositionInterface::SERVO_l_ankle_roll ) 00997 servonames.push_back("LAnkleRoll"); 00998 00999 if ( servos & NaoJointPositionInterface::SERVO_r_ankle_pitch ) 01000 servonames.push_back("RAnklePitch"); 01001 01002 if ( servos & NaoJointPositionInterface::SERVO_r_ankle_roll ) 01003 servonames.push_back("RAnkleRoll"); 01004 01005 return servonames; 01006 } 01007 01008 01009 int 01010 NaoQiDCMThread::ultrasonic_value(fawkes::NaoSensorInterface::UltrasonicDirection direction) 01011 { 01012 int value = 0; 01013 switch (direction) { 01014 case NaoSensorInterface::USD_LEFT_LEFT: value = 0; break; 01015 case NaoSensorInterface::USD_LEFT_RIGHT: value = 1; break; 01016 case NaoSensorInterface::USD_RIGHT_LEFT: value = 2; break; 01017 case NaoSensorInterface::USD_RIGHT_RIGHT: value = 3; break; 01018 case NaoSensorInterface::USD_BOTH_BOTH: value = 12; break; 01019 default: 01020 logger->log_warn(name(), "Illegal ultrasonic direction, " 01021 "using left-right"); 01022 } 01023 return value; 01024 }