23 #include "dcm_thread.h" 24 #include "motion_utils.h" 26 #include <alproxies/allauncherproxy.h> 27 #include <alproxies/dcmproxy.h> 28 #include <alproxies/almotionproxy.h> 29 #include <alproxies/almemoryproxy.h> 30 #include <alcore/alerror.h> 31 #include <almemoryfastaccess/almemoryfastaccess.h> 33 #include <boost/bind.hpp> 35 #include <interfaces/NaoJointStiffnessInterface.h> 36 #include <interfaces/NaoSensorInterface.h> 40 enum SensorType { HEAD_PITCH = 0, HEAD_YAW,
41 L_SHOULDER_PITCH, L_SHOULDER_ROLL, L_ELBOW_YAW, L_ELBOW_ROLL,
43 L_HIP_YAW_PITCH, L_HIP_ROLL, L_HIP_PITCH, L_KNEE_PITCH,
44 L_ANKLE_PITCH, L_ANKLE_ROLL,
45 R_SHOULDER_PITCH, R_SHOULDER_ROLL, R_ELBOW_YAW, R_ELBOW_ROLL,
47 R_HIP_YAW_PITCH, R_HIP_ROLL, R_HIP_PITCH, R_KNEE_PITCH,
48 R_ANKLE_PITCH, R_ANKLE_ROLL,
49 STIFF_HEAD_PITCH, STIFF_HEAD_YAW,
50 STIFF_L_SHOULDER_PITCH, STIFF_L_SHOULDER_ROLL,
51 STIFF_L_ELBOW_YAW, STIFF_L_ELBOW_ROLL,
52 STIFF_L_WRIST_YAW, STIFF_L_HAND,
53 STIFF_L_HIP_YAW_PITCH, STIFF_L_HIP_ROLL, STIFF_L_HIP_PITCH,
54 STIFF_L_KNEE_PITCH, STIFF_L_ANKLE_PITCH, STIFF_L_ANKLE_ROLL,
55 STIFF_R_SHOULDER_PITCH, STIFF_R_SHOULDER_ROLL,
56 STIFF_R_ELBOW_YAW, STIFF_R_ELBOW_ROLL,
57 STIFF_R_WRIST_YAW, STIFF_R_HAND,
58 STIFF_R_HIP_YAW_PITCH, STIFF_R_HIP_ROLL, STIFF_R_HIP_PITCH,
59 STIFF_R_KNEE_PITCH, STIFF_R_ANKLE_PITCH, STIFF_R_ANKLE_ROLL,
60 ACC_X, ACC_Y, ACC_Z, GYR_X, GYR_Y, GYR_REF, ANGLE_X, ANGLE_Y,
61 L_FSR_FL, L_FSR_FR, L_FSR_RL, L_FSR_RR,
62 R_FSR_FL, R_FSR_FR, R_FSR_RL, R_FSR_RR,
63 L_COP_X, L_COP_Y, L_TOTAL_WEIGHT, R_COP_X, R_COP_Y,
65 ULTRASONIC_DIRECTION, ULTRASONIC_DISTANCE,
66 ULTRASONIC_DISTANCE_LEFT_0, ULTRASONIC_DISTANCE_LEFT_1,
67 ULTRASONIC_DISTANCE_LEFT_2, ULTRASONIC_DISTANCE_LEFT_3,
68 ULTRASONIC_DISTANCE_RIGHT_0, ULTRASONIC_DISTANCE_RIGHT_1,
69 ULTRASONIC_DISTANCE_RIGHT_2, ULTRASONIC_DISTANCE_RIGHT_3,
70 L_FOOT_BUMPER_L, L_FOOT_BUMPER_R,
71 R_FOOT_BUMPER_L, R_FOOT_BUMPER_R,
72 HEAD_TOUCH_FRONT, HEAD_TOUCH_MIDDLE, HEAD_TOUCH_REAR,
73 CHEST_BUTTON, BATTERY_CHARGE,
76 enum StiffnessJoint { STIFFJ_HEAD_PITCH = 0, STIFFJ_HEAD_YAW,
77 STIFFJ_L_SHOULDER_PITCH, STIFFJ_L_SHOULDER_ROLL,
78 STIFFJ_L_ELBOW_YAW, STIFFJ_L_ELBOW_ROLL,
79 STIFFJ_L_HIP_YAW_PITCH, STIFFJ_L_HIP_ROLL,
80 STIFFJ_L_HIP_PITCH, STIFFJ_L_KNEE_PITCH,
81 STIFFJ_L_ANKLE_PITCH, STIFFJ_L_ANKLE_ROLL,
82 STIFFJ_R_SHOULDER_PITCH, STIFFJ_R_SHOULDER_ROLL,
83 STIFFJ_R_ELBOW_YAW, STIFFJ_R_ELBOW_ROLL,
84 STIFFJ_R_HIP_YAW_PITCH, STIFFJ_R_HIP_ROLL,
85 STIFFJ_R_HIP_PITCH, STIFFJ_R_KNEE_PITCH,
86 STIFFJ_R_ANKLE_PITCH, STIFFJ_R_ANKLE_ROLL,
87 STIFFJ_L_WRIST_YAW, STIFFJ_L_HAND,
88 STIFFJ_R_WRIST_YAW, STIFFJ_R_HAND,
91 #define ACCELEROMETER_G_FACTOR 56. 95 #define HEAD_YAW_MIN -1.2 96 #define HEAD_YAW_MAX 1.2 98 #define L_SHOULDER_PITCH_MIN -1.7 99 #define L_SHOULDER_PITCH_MAX 1.7 100 #define R_SHOULDER_PITCH_MIN -1.7 101 #define R_SHOULDER_PITCH_MAX 1.7 103 #define CLIP_VALUE(V, value) clip_value(value, V ## _MIN, V ## _MAX) 112 clip_value(
float value,
float min,
float max)
114 if (value < min) value = min;
115 if (value > max) value = max;
132 :
Thread(
"NaoQiDCMThread::HighFreqThread",
Thread::OPMODE_WAITFORWAKEUP),
135 set_coalesce_wakeups(
true);
140 __parent->read_values();
141 if ( (__parent->__joint_pos_highfreq_if->num_readers() > 0) ||
142 (__parent->__joint_stiffness_highfreq_if->num_readers() > 0) ||
143 (__parent->__sensor_if->num_readers() > 0) )
145 __parent->update_interfaces(__parent->__joint_pos_highfreq_if,
146 __parent->__joint_stiffness_highfreq_if,
147 __parent->__sensor_highfreq_if);
150 __parent->process_messages();
179 :
Thread(
"NaoQiDCMThread",
Thread::OPMODE_WAITFORWAKEUP),
196 AL::ALPtr<AL::ALLauncherProxy> launcher(
new AL::ALLauncherProxy(
naoqi_broker));
197 bool is_dcm_available = launcher->isModulePresent(
"DCM");
198 bool is_almotion_available = launcher->isModulePresent(
"ALMotion");
200 if (! is_dcm_available) {
201 throw Exception(
"DCMThread: NaoQi DCM is not available");
203 if (! is_almotion_available) {
204 throw Exception(
"DCMThread: ALMotion is not available");
206 }
catch (AL::ALError& e) {
207 throw Exception(
"Checking module availability failed: %s",
208 e.toString().c_str());
215 AL::ALPtr<AL::ALMemoryProxy> almemory =
naoqi_broker->getMemoryProxy();
216 std::string version = almemory->getData(
"RobotConfig/Body/BaseVersion", 0);
217 unsigned int num_joints = __almotion->getJointNames(
"Body").size();
218 if (num_joints == 26) {
219 __robot_type = NaoJointPositionInterface::ROBOTYPE_ACADEMIC;
221 __robot_type = NaoJointPositionInterface::ROBOTYPE_ROBOCUP;
223 __robot_version[2] = __robot_version[3] = 0;
224 if (version[0] ==
'V') version = version.substr(1);
225 std::string::size_type pos;
226 if ((pos = version.find_first_of(
".")) != std::string::npos) {
227 std::string version_major = version.substr(0, pos);
228 std::string version_minor = version.substr(pos+1);
229 __robot_version[0] = atoi(version_major.c_str());
230 __robot_version[1] = atoi(version_minor.c_str());
233 almemory->getData(
"Device/DeviceList/USBoard/ProgVersion", 0);
234 }
catch (AL::ALError &e) {
235 throw Exception(
"Retrieving robot info failed: %s", e.toString().c_str());
238 __memfa.reset(
new AL::ALMemoryFastAccess());
240 std::string prefix =
"Device/SubDeviceList/";
243 std::vector<std::string> keys;
244 keys.resize(SensorTypeN);
245 __values.resize(SensorTypeN);
247 keys[HEAD_PITCH] = prefix +
"HeadPitch/Position/Sensor/Value";
248 keys[HEAD_YAW] = prefix +
"HeadYaw/Position/Sensor/Value";
249 keys[L_SHOULDER_PITCH] = prefix +
"LShoulderPitch/Position/Sensor/Value";
250 keys[L_SHOULDER_ROLL] = prefix +
"LShoulderRoll/Position/Sensor/Value";
251 keys[L_ELBOW_YAW] = prefix +
"LElbowYaw/Position/Sensor/Value";
252 keys[L_ELBOW_ROLL] = prefix +
"LElbowRoll/Position/Sensor/Value";
253 keys[L_WRIST_YAW] = prefix +
"LWristYaw/Position/Sensor/Value";
254 keys[L_HAND] = prefix +
"LHand/Position/Sensor/Value";
255 keys[L_HIP_YAW_PITCH] = prefix +
"LHipYawPitch/Position/Sensor/Value";
256 keys[L_HIP_ROLL] = prefix +
"LHipRoll/Position/Sensor/Value";
257 keys[L_HIP_PITCH] = prefix +
"LHipPitch/Position/Sensor/Value";
258 keys[L_KNEE_PITCH] = prefix +
"LKneePitch/Position/Sensor/Value";
259 keys[L_ANKLE_PITCH] = prefix +
"LAnklePitch/Position/Sensor/Value";
260 keys[L_ANKLE_ROLL] = prefix +
"LAnkleRoll/Position/Sensor/Value";
262 keys[R_SHOULDER_PITCH] = prefix +
"RShoulderPitch/Position/Sensor/Value";
263 keys[R_SHOULDER_ROLL] = prefix +
"RShoulderRoll/Position/Sensor/Value";
264 keys[R_ELBOW_YAW] = prefix +
"RElbowYaw/Position/Sensor/Value";
265 keys[R_ELBOW_ROLL] = prefix +
"RElbowRoll/Position/Sensor/Value";
266 keys[R_WRIST_YAW] = prefix +
"RWristYaw/Position/Sensor/Value";
267 keys[R_HAND] = prefix +
"RHand/Position/Sensor/Value";
268 keys[R_HIP_YAW_PITCH] = prefix +
"RHipYawPitch/Position/Sensor/Value";
269 keys[R_HIP_ROLL] = prefix +
"RHipRoll/Position/Sensor/Value";
270 keys[R_HIP_PITCH] = prefix +
"RHipPitch/Position/Sensor/Value";
271 keys[R_KNEE_PITCH] = prefix +
"RKneePitch/Position/Sensor/Value";
272 keys[R_ANKLE_PITCH] = prefix +
"RAnklePitch/Position/Sensor/Value";
273 keys[R_ANKLE_ROLL] = prefix +
"RAnkleRoll/Position/Sensor/Value";
275 keys[STIFF_HEAD_PITCH] = prefix +
"HeadPitch/Hardness/Actuator/Value";
276 keys[STIFF_HEAD_YAW] = prefix +
"HeadYaw/Hardness/Actuator/Value";
277 keys[STIFF_L_SHOULDER_PITCH] = prefix +
"LShoulderPitch/Hardness/Actuator/Value";
278 keys[STIFF_L_SHOULDER_ROLL] = prefix +
"LShoulderRoll/Hardness/Actuator/Value";
279 keys[STIFF_L_ELBOW_YAW] = prefix +
"LElbowYaw/Hardness/Actuator/Value";
280 keys[STIFF_L_ELBOW_ROLL] = prefix +
"LElbowRoll/Hardness/Actuator/Value";
281 keys[STIFF_L_WRIST_YAW] = prefix +
"LWristYaw/Hardness/Actuator/Value";
282 keys[STIFF_L_HAND] = prefix +
"LHand/Hardness/Actuator/Value";
283 keys[STIFF_L_HIP_YAW_PITCH] = prefix +
"LHipYawPitch/Hardness/Actuator/Value";
284 keys[STIFF_L_HIP_ROLL] = prefix +
"LHipRoll/Hardness/Actuator/Value";
285 keys[STIFF_L_HIP_PITCH] = prefix +
"LHipPitch/Hardness/Actuator/Value";
286 keys[STIFF_L_KNEE_PITCH] = prefix +
"LKneePitch/Hardness/Actuator/Value";
287 keys[STIFF_L_ANKLE_PITCH] = prefix +
"LAnklePitch/Hardness/Actuator/Value";
288 keys[STIFF_L_ANKLE_ROLL] = prefix +
"LAnkleRoll/Hardness/Actuator/Value";
290 keys[STIFF_R_SHOULDER_PITCH] = prefix +
"RShoulderPitch/Hardness/Actuator/Value";
291 keys[STIFF_R_SHOULDER_ROLL] = prefix +
"RShoulderRoll/Hardness/Actuator/Value";
292 keys[STIFF_R_ELBOW_YAW] = prefix +
"RElbowYaw/Hardness/Actuator/Value";
293 keys[STIFF_R_ELBOW_ROLL] = prefix +
"RElbowRoll/Hardness/Actuator/Value";
294 keys[STIFF_R_WRIST_YAW] = prefix +
"RWristYaw/Hardness/Actuator/Value";
295 keys[STIFF_R_HAND] = prefix +
"RHand/Hardness/Actuator/Value";
296 keys[STIFF_R_HIP_YAW_PITCH] = prefix +
"RHipYawPitch/Hardness/Actuator/Value";
297 keys[STIFF_R_HIP_ROLL] = prefix +
"RHipRoll/Hardness/Actuator/Value";
298 keys[STIFF_R_HIP_PITCH] = prefix +
"RHipPitch/Hardness/Actuator/Value";
299 keys[STIFF_R_KNEE_PITCH] = prefix +
"RKneePitch/Hardness/Actuator/Value";
300 keys[STIFF_R_ANKLE_PITCH] = prefix +
"RAnklePitch/Hardness/Actuator/Value";
301 keys[STIFF_R_ANKLE_ROLL] = prefix +
"RAnkleRoll/Hardness/Actuator/Value";
304 keys[ACC_X] = prefix +
"InertialSensor/AccX/Sensor/Value";
305 keys[ACC_Y] = prefix +
"InertialSensor/AccY/Sensor/Value";
306 keys[ACC_Z] = prefix +
"InertialSensor/AccZ/Sensor/Value";
307 keys[GYR_X] = prefix +
"InertialSensor/GyrX/Sensor/Value";
308 keys[GYR_Y] = prefix +
"InertialSensor/GyrY/Sensor/Value";
309 keys[GYR_REF] = prefix +
"InertialSensor/GyrRef/Sensor/Value";
310 keys[ANGLE_X] = prefix +
"InertialSensor/AngleX/Sensor/Value";
311 keys[ANGLE_Y] = prefix +
"InertialSensor/AngleY/Sensor/Value";
314 keys[L_FSR_FL] = prefix +
"LFoot/FSR/FrontLeft/Sensor/Value";
315 keys[L_FSR_FR] = prefix +
"LFoot/FSR/FrontRight/Sensor/Value";
316 keys[L_FSR_RL] = prefix +
"LFoot/FSR/RearLeft/Sensor/Value";
317 keys[L_FSR_RR] = prefix +
"LFoot/FSR/RearRight/Sensor/Value";
318 keys[R_FSR_FL] = prefix +
"RFoot/FSR/FrontLeft/Sensor/Value";
319 keys[R_FSR_FR] = prefix +
"RFoot/FSR/FrontRight/Sensor/Value";
320 keys[R_FSR_RL] = prefix +
"RFoot/FSR/RearLeft/Sensor/Value";
321 keys[R_FSR_RR] = prefix +
"RFoot/FSR/RearRight/Sensor/Value";
322 keys[L_COP_X] = prefix +
"LFoot/FSR/CenterOfPressure/X/Sensor/Value";
323 keys[L_COP_Y] = prefix +
"LFoot/FSR/CenterOfPressure/Y/Sensor/Value";
324 keys[L_TOTAL_WEIGHT] = prefix +
"LFoot/FSR/TotalWeight/Sensor/Value";
325 keys[R_COP_X] = prefix +
"RFoot/FSR/CenterOfPressure/X/Sensor/Value";
326 keys[R_COP_Y] = prefix +
"RFoot/FSR/CenterOfPressure/Y/Sensor/Value";
327 keys[R_TOTAL_WEIGHT] = prefix +
"RFoot/FSR/TotalWeight/Sensor/Value";
330 keys[ULTRASONIC_DIRECTION] = prefix +
"US/Actuator/Value";
331 keys[ULTRASONIC_DISTANCE] = prefix +
"US/Sensor/Value";
333 keys[ULTRASONIC_DISTANCE_LEFT_0] = prefix +
"US/Left/Sensor/Value";
334 keys[ULTRASONIC_DISTANCE_LEFT_1] = prefix +
"US/Left/Sensor/Value1";
335 keys[ULTRASONIC_DISTANCE_LEFT_2] = prefix +
"US/Left/Sensor/Value2";
336 keys[ULTRASONIC_DISTANCE_LEFT_3] = prefix +
"US/Left/Sensor/Value3";
338 keys[ULTRASONIC_DISTANCE_RIGHT_0] = prefix +
"US/Right/Sensor/Value";
339 keys[ULTRASONIC_DISTANCE_RIGHT_1] = prefix +
"US/Right/Sensor/Value1";
340 keys[ULTRASONIC_DISTANCE_RIGHT_2] = prefix +
"US/Right/Sensor/Value2";
341 keys[ULTRASONIC_DISTANCE_RIGHT_3] = prefix +
"US/Right/Sensor/Value3";
344 keys[L_FOOT_BUMPER_L] = prefix +
"LFoot/Bumper/Left/Sensor/Value";
345 keys[L_FOOT_BUMPER_R] = prefix +
"LFoot/Bumper/Right/Sensor/Value";
346 keys[R_FOOT_BUMPER_L] = prefix +
"RFoot/Bumper/Left/Sensor/Value";
347 keys[R_FOOT_BUMPER_R] = prefix +
"RFoot/Bumper/Right/Sensor/Value";
349 keys[HEAD_TOUCH_FRONT] = prefix +
"Head/Touch/Front/Sensor/Value";
350 keys[HEAD_TOUCH_MIDDLE] = prefix +
"Head/Touch/Middle/Sensor/Value";
351 keys[HEAD_TOUCH_REAR] = prefix +
"Head/Touch/Rear/Sensor/Value";
353 keys[CHEST_BUTTON] = prefix +
"ChestBoard/Button/Sensor/Value";
356 keys[BATTERY_CHARGE] = prefix +
"Battery/Charge/Sensor/Value";
360 }
catch (AL::ALError &e) {
361 throw Exception(
"Failed to setup fast memory access: %s",
362 e.toString().c_str());
367 if (__robot_type == NaoJointPositionInterface::ROBOTYPE_ROBOCUP) {
368 __alljoint_names.arraySetSize(22);
370 __alljoint_names.arraySetSize(26);
371 __alljoint_names[STIFFJ_L_WRIST_YAW] =
"LWristYaw";
372 __alljoint_names[STIFFJ_L_HAND] =
"LHand";
373 __alljoint_names[STIFFJ_R_WRIST_YAW] =
"RWristYaw";
374 __alljoint_names[STIFFJ_R_HAND] =
"RHand";
376 __alljoint_names[STIFFJ_HEAD_PITCH] =
"HeadPitch";
377 __alljoint_names[STIFFJ_HEAD_YAW] =
"HeadYaw";
378 __alljoint_names[STIFFJ_L_SHOULDER_PITCH] =
"LShoulderPitch";
379 __alljoint_names[STIFFJ_L_SHOULDER_ROLL] =
"LShoulderRoll";
380 __alljoint_names[STIFFJ_L_ELBOW_YAW] =
"LElbowYaw";
381 __alljoint_names[STIFFJ_L_ELBOW_ROLL] =
"LElbowRoll";
382 __alljoint_names[STIFFJ_L_HIP_YAW_PITCH] =
"LHipYawPitch";
383 __alljoint_names[STIFFJ_L_HIP_ROLL] =
"LHipRoll";
384 __alljoint_names[STIFFJ_L_HIP_PITCH] =
"LHipPitch";
385 __alljoint_names[STIFFJ_L_KNEE_PITCH] =
"LKneePitch";
386 __alljoint_names[STIFFJ_L_ANKLE_PITCH] =
"LAnklePitch";
387 __alljoint_names[STIFFJ_L_ANKLE_ROLL] =
"LAnkleRoll";
389 __alljoint_names[STIFFJ_R_SHOULDER_PITCH] =
"RShoulderPitch";
390 __alljoint_names[STIFFJ_R_SHOULDER_ROLL] =
"RShoulderRoll";
391 __alljoint_names[STIFFJ_R_ELBOW_YAW] =
"RElbowYaw";
392 __alljoint_names[STIFFJ_R_ELBOW_ROLL] =
"RElbowRoll";
393 __alljoint_names[STIFFJ_R_HIP_YAW_PITCH] =
"RHipYawPitch";
394 __alljoint_names[STIFFJ_R_HIP_ROLL] =
"RHipRoll";
395 __alljoint_names[STIFFJ_R_HIP_PITCH] =
"RHipPitch";
396 __alljoint_names[STIFFJ_R_KNEE_PITCH] =
"RKneePitch";
397 __alljoint_names[STIFFJ_R_ANKLE_PITCH] =
"RAnklePitch";
398 __alljoint_names[STIFFJ_R_ANKLE_ROLL] =
"RAnkleRoll";
401 AL::ALValue setJointStiffnessAlias;
403 setJointStiffnessAlias.clear();
404 setJointStiffnessAlias.arraySetSize(2);
405 setJointStiffnessAlias[0] = std::string(
"setJointStiffness");
406 setJointStiffnessAlias[1].arraySetSize(26);
409 int offset = STIFF_HEAD_PITCH - HEAD_PITCH;
410 for (
int i = HEAD_PITCH; i <= R_ANKLE_ROLL; ++i) {
411 setJointStiffnessAlias[1][i] = keys[i + offset];
414 __dcm->createAlias(setJointStiffnessAlias);
415 }
catch (AL::ALError &e) {
417 throw Exception(
"Failed to create SetJointStiffness alias: %s",
418 e.toString().c_str());
423 __joint_stiffness_if =
425 (
"Nao Joint Stiffness");
429 __joint_pos_highfreq_if =
431 (
"Nao Joint Positions HF");
432 __joint_stiffness_highfreq_if =
434 (
"Nao Joint Stiffness HF");
435 __sensor_highfreq_if =
440 __dcm_time = __dcm->getTime(0);
441 __memfa->GetValues(__values);
443 __joint_pos_if->set_robot_type(__robot_type);
444 __joint_pos_if->set_robot_version(__robot_version);
445 __joint_pos_highfreq_if->set_robot_type(__robot_type);
446 __joint_pos_highfreq_if->set_robot_version(__robot_version);
448 __sensor_if->set_ultrasonic_direction(NaoSensorInterface::USD_NONE);
449 __sensor_highfreq_if->set_ultrasonic_direction(NaoSensorInterface::USD_NONE);
452 update_interfaces(__joint_pos_if, __joint_stiffness_if, __sensor_if);
453 update_interfaces(__joint_pos_highfreq_if, __joint_stiffness_highfreq_if,
454 __sensor_highfreq_if);
457 __highfreq_thread->
start();
460 __dcm->getGenericProxy()->getModule()->
461 atPostProcess(boost::bind(&NaoQiDCMThread::dcm_callback,
this));
480 NaoQiDCMThread::dcm_callback()
482 __highfreq_thread->
wakeup();
488 __dcm_sigconn.disconnect();
490 __highfreq_thread->
cancel();
491 __highfreq_thread->
join();
492 delete __highfreq_thread;
500 __joint_pos_if = NULL;
501 __joint_stiffness_if = NULL;
503 __joint_pos_highfreq_if = NULL;
504 __joint_stiffness_highfreq_if = NULL;
505 __sensor_highfreq_if = NULL;
513 NaoQiDCMThread::read_values()
516 __dcm_time = __dcm->getTime(0);
517 __memfa->GetValues(__values);
525 update_interfaces(__joint_pos_if, __joint_stiffness_if, __sensor_if);
570 joint_stiffness_if->
set_head_yaw(__values[STIFF_HEAD_YAW]);
578 joint_stiffness_if->
set_l_hand(__values[STIFF_L_HAND]);
584 joint_stiffness_if->
set_r_hand(__values[STIFF_R_HAND]);
602 float min_stiffness = 1.;
603 for (
int i = STIFF_HEAD_YAW; i <= STIFF_R_ANKLE_ROLL; ++i) {
605 if ( (__robot_type == NaoJointPositionInterface::ROBOTYPE_ROBOCUP) &&
606 ( (i == STIFF_L_WRIST_YAW) || (i == STIFF_L_HAND) ||
607 (i == STIFF_R_WRIST_YAW) || (i == STIFF_R_HAND) ) )
611 if (i == STIFF_R_HIP_YAW_PITCH)
continue;
613 if (__values[i] < min_stiffness) min_stiffness = __values[i];
648 sensor_if->
set_accel_x(__values[ACC_X] / ACCELEROMETER_G_FACTOR);
649 sensor_if->
set_accel_y(__values[ACC_Y] / ACCELEROMETER_G_FACTOR);
650 sensor_if->
set_accel_z(__values[ACC_Z] / ACCELEROMETER_G_FACTOR);
663 case NaoSensorInterface::USD_LEFT_LEFT:
664 case NaoSensorInterface::USD_RIGHT_LEFT:
666 float us_left[4] = {__values[ULTRASONIC_DISTANCE], 0, 0, 0};
669 float us_right[4] = {0, 0, 0, 0};
673 case NaoSensorInterface::USD_LEFT_RIGHT:
674 case NaoSensorInterface::USD_RIGHT_RIGHT:
676 float us_left[4] = {0, 0, 0, 0};
679 float us_right[4] = {__values[ULTRASONIC_DISTANCE], 0, 0, 0};
686 float us_left[4] = {__values[ULTRASONIC_DISTANCE_LEFT_0],
687 __values[ULTRASONIC_DISTANCE_LEFT_1],
688 __values[ULTRASONIC_DISTANCE_LEFT_2],
689 __values[ULTRASONIC_DISTANCE_LEFT_3]};
692 float us_right[4] = {__values[ULTRASONIC_DISTANCE_RIGHT_0],
693 __values[ULTRASONIC_DISTANCE_RIGHT_1],
694 __values[ULTRASONIC_DISTANCE_RIGHT_2],
695 __values[ULTRASONIC_DISTANCE_RIGHT_3]};
705 joint_pos_if->
write();
706 joint_stiffness_if->
write();
712 NaoQiDCMThread::process_messages()
719 send_commands(msg->servo(),
"Position", msg->value(), msg->time());
730 std::vector<std::string> servos = parse_servo_bitfield(msg->servo());
731 std::vector<float> values(servos.size(), msg->value());
732 __almotion->setAngles(servos, values, msg->speed());
740 msg->head_yaw(), msg->head_pitch(),
741 msg->l_shoulder_pitch(), msg->l_shoulder_roll(),
742 msg->l_elbow_yaw(), msg->l_elbow_roll(),
743 msg->l_wrist_yaw(), msg->l_hand(),
744 msg->l_hip_yaw_pitch(), msg->l_hip_roll(), msg->l_hip_pitch(),
746 msg->l_ankle_pitch(), msg->l_ankle_roll(),
747 msg->r_shoulder_pitch(), msg->r_shoulder_roll(),
748 msg->r_elbow_yaw(), msg->r_elbow_roll(),
749 msg->r_wrist_yaw(), msg->r_hand(),
750 msg->r_hip_yaw_pitch(), msg->r_hip_roll(), msg->r_hip_pitch(),
752 msg->r_ankle_pitch(), msg->r_ankle_roll(),
760 while (! __joint_stiffness_if->
msgq_empty()) {
769 std::vector<std::string> servos = parse_servo_bitfield(msg->servo());
770 std::vector<float> values(servos.size(), msg->value());
772 __almotion->post.stiffnessInterpolation(servos, values,
799 __almotion->post.stiffnessInterpolation(
"Body", msg->value(),
806 std::vector<float> values(__alljoint_names.getSize());
807 values[STIFFJ_HEAD_PITCH] = msg->head_pitch();
808 values[STIFFJ_HEAD_YAW] = msg->head_yaw();
809 values[STIFFJ_L_SHOULDER_PITCH] = msg->l_shoulder_pitch();
810 values[STIFFJ_L_SHOULDER_ROLL] = msg->l_shoulder_roll();
811 values[STIFFJ_L_ELBOW_YAW] = msg->l_elbow_yaw();
812 values[STIFFJ_L_ELBOW_ROLL] = msg->l_elbow_roll();
813 values[STIFFJ_L_HIP_YAW_PITCH] = msg->l_hip_yaw_pitch();
814 values[STIFFJ_L_HIP_ROLL] = msg->l_hip_roll();
815 values[STIFFJ_L_HIP_PITCH] = msg->l_hip_pitch();
816 values[STIFFJ_L_KNEE_PITCH] = msg->l_knee_pitch();
817 values[STIFFJ_L_ANKLE_PITCH] = msg->l_ankle_pitch();
818 values[STIFFJ_L_ANKLE_ROLL] = msg->l_ankle_roll();
820 values[STIFFJ_R_SHOULDER_PITCH] = msg->r_shoulder_pitch();
821 values[STIFFJ_R_SHOULDER_ROLL] = msg->r_shoulder_roll();
822 values[STIFFJ_R_ELBOW_YAW] = msg->r_elbow_yaw();
823 values[STIFFJ_R_ELBOW_ROLL] = msg->r_elbow_roll();
824 values[STIFFJ_R_HIP_YAW_PITCH] = msg->r_hip_yaw_pitch();
825 values[STIFFJ_R_HIP_ROLL] = msg->r_hip_roll();
826 values[STIFFJ_R_HIP_PITCH] = msg->r_hip_pitch();
827 values[STIFFJ_R_KNEE_PITCH] = msg->r_knee_pitch();
828 values[STIFFJ_R_ANKLE_PITCH] = msg->r_ankle_pitch();
829 values[STIFFJ_R_ANKLE_ROLL] = msg->r_ankle_roll();
831 if (__robot_type != NaoJointPositionInterface::ROBOTYPE_ROBOCUP) {
832 values[STIFFJ_L_WRIST_YAW] = msg->l_wrist_yaw();
833 values[STIFFJ_L_HAND] = msg->l_hand();
834 values[STIFFJ_R_WRIST_YAW] = msg->r_wrist_yaw();
835 values[STIFFJ_R_HAND] = msg->r_hand();
838 __almotion->post.stiffnessInterpolation(__alljoint_names, values,
850 int value = ultrasonic_value(msg->ultrasonic_direction());
851 send_command(
"US/Actuator/Value", value,
"ClearAll", 0);
859 int value = ultrasonic_value(msg->ultrasonic_direction());
861 send_command(
"US/Actuator/Value", value,
"ClearAll", 0);
869 send_command(
"US/Actuator/Value", 0,
"ClearAll", 0);
882 NaoQiDCMThread::send_commands(
unsigned int servos, std::string what,
883 float value,
int time_offset)
893 std::vector<std::string> servonames = parse_servo_bitfield(servos);
895 std::vector<std::string>::iterator s;
896 for (s = servonames.begin(); s != servonames.end(); ++s) {
898 if (*s ==
"HeadYaw") {
899 v = CLIP_VALUE(HEAD_YAW, value);
900 }
else if (*s ==
"LShoulderPitch") {
901 v = CLIP_VALUE(L_SHOULDER_PITCH, value);
902 }
else if (*s ==
"RShoulderPitch") {
903 v = CLIP_VALUE(R_SHOULDER_PITCH, value);
905 send_command(*s +
"/" + what +
"/Actuator/Value",
906 v,
"Merge", time_offset);
911 NaoQiDCMThread::send_command(std::string
name,
float value,
912 std::string kind,
int time_offset)
925 cmd[2].arraySetSize(1);
926 cmd[2][0].arraySetSize(2);
927 cmd[2][0][0] = value;
928 cmd[2][0][1] = __dcm_time + time_offset;
934 std::vector<std::string>
935 NaoQiDCMThread::parse_servo_bitfield(
unsigned int servos)
937 std::vector<std::string> servonames;
939 if ( servos & NaoJointPositionInterface::SERVO_head_yaw )
940 servonames.push_back(
"HeadYaw");
942 if ( servos & NaoJointPositionInterface::SERVO_head_pitch )
943 servonames.push_back(
"HeadPitch");
945 if ( servos & NaoJointPositionInterface::SERVO_l_shoulder_pitch )
946 servonames.push_back(
"LShoulderPitch");
948 if ( servos & NaoJointPositionInterface::SERVO_l_shoulder_roll )
949 servonames.push_back(
"LShoulderRoll");
951 if ( servos & NaoJointPositionInterface::SERVO_l_elbow_yaw )
952 servonames.push_back(
"LElbowYaw");
954 if ( servos & NaoJointPositionInterface::SERVO_l_elbow_roll )
955 servonames.push_back(
"LElbowRoll");
957 if ( servos & NaoJointPositionInterface::SERVO_r_shoulder_pitch )
958 servonames.push_back(
"RShoulderPitch");
960 if ( servos & NaoJointPositionInterface::SERVO_r_shoulder_roll )
961 servonames.push_back(
"RShoulderRoll");
963 if ( servos & NaoJointPositionInterface::SERVO_r_elbow_yaw )
964 servonames.push_back(
"RElbowYaw");
966 if ( servos & NaoJointPositionInterface::SERVO_r_elbow_roll )
967 servonames.push_back(
"RElbowRoll");
969 if ( servos & NaoJointPositionInterface::SERVO_l_hip_yaw_pitch )
970 servonames.push_back(
"LHipYawPitch");
972 if ( servos & NaoJointPositionInterface::SERVO_l_hip_pitch )
973 servonames.push_back(
"LHipPitch");
975 if ( servos & NaoJointPositionInterface::SERVO_l_hip_roll )
976 servonames.push_back(
"LHipRoll");
978 if ( servos & NaoJointPositionInterface::SERVO_r_hip_yaw_pitch )
979 servonames.push_back(
"RHipYawPitch");
981 if ( servos & NaoJointPositionInterface::SERVO_r_hip_pitch )
982 servonames.push_back(
"RHipPitch");
984 if ( servos & NaoJointPositionInterface::SERVO_r_hip_roll )
985 servonames.push_back(
"RHipRoll");
987 if ( servos & NaoJointPositionInterface::SERVO_l_knee_pitch )
988 servonames.push_back(
"LKneePitch");
990 if ( servos & NaoJointPositionInterface::SERVO_r_knee_pitch )
991 servonames.push_back(
"RKneePitch");
993 if ( servos & NaoJointPositionInterface::SERVO_l_ankle_pitch )
994 servonames.push_back(
"LAnklePitch");
996 if ( servos & NaoJointPositionInterface::SERVO_l_ankle_roll )
997 servonames.push_back(
"LAnkleRoll");
999 if ( servos & NaoJointPositionInterface::SERVO_r_ankle_pitch )
1000 servonames.push_back(
"RAnklePitch");
1002 if ( servos & NaoJointPositionInterface::SERVO_r_ankle_roll )
1003 servonames.push_back(
"RAnkleRoll");
1013 switch (direction) {
1014 case NaoSensorInterface::USD_LEFT_LEFT: value = 0;
break;
1015 case NaoSensorInterface::USD_LEFT_RIGHT: value = 1;
break;
1016 case NaoSensorInterface::USD_RIGHT_LEFT: value = 2;
break;
1017 case NaoSensorInterface::USD_RIGHT_RIGHT: value = 3;
break;
1018 case NaoSensorInterface::USD_BOTH_BOTH: value = 12;
break;
1021 "using left-right");
NaoJointPositionInterface Fawkes BlackBoard Interface.
void set_r_elbow_roll(const float new_r_elbow_roll)
Set r_elbow_roll value.
void set_l_ankle_roll(const float new_l_ankle_roll)
Set l_ankle_roll value.
SetServoMessage Fawkes BlackBoard Interface Message.
virtual void finalize()
Finalize the thread.
void set_l_fsr_rl(const float new_l_fsr_rl)
Set l_fsr_rl value.
void set_r_hip_pitch(const float new_r_hip_pitch)
Set r_hip_pitch value.
virtual void init()
Initialize the thread.
void set_r_shoulder_pitch(const float new_r_shoulder_pitch)
Set r_shoulder_pitch value.
bool msgq_empty()
Check if queue is empty.
void set_r_foot_bumper_r(const uint8_t new_r_foot_bumper_r)
Set r_foot_bumper_r value.
void set_l_shoulder_roll(const float new_l_shoulder_roll)
Set l_shoulder_roll value.
void set_l_hip_roll(const float new_l_hip_roll)
Set l_hip_roll value.
SetServosMessage Fawkes BlackBoard Interface Message.
void set_accel_x(const float new_accel_x)
Set accel_x value.
StopUltrasonicMessage Fawkes BlackBoard Interface Message.
void set_l_fsr_fl(const float new_l_fsr_fl)
Set l_fsr_fl value.
void set_l_ankle_pitch(const float new_l_ankle_pitch)
Set l_ankle_pitch value.
void set_l_elbow_yaw(const float new_l_elbow_yaw)
Set l_elbow_yaw value.
void set_head_touch_front(const uint8_t new_head_touch_front)
Set head_touch_front value.
void set_chest_button(const uint8_t new_chest_button)
Set chest_button value.
HighFreqThread(NaoQiDCMThread *parent)
Constructor.
void set_accel_z(const float new_accel_z)
Set accel_z value.
Fawkes library namespace.
void set_r_hip_yaw_pitch(const float new_r_hip_yaw_pitch)
Set r_hip_yaw_pitch value.
AL::ALPtr< AL::ALBroker > naoqi_broker
NaoQi broker.
void set_l_fsr_rr(const float new_l_fsr_rr)
Set l_fsr_rr value.
void set_r_hip_yaw_pitch(const float new_r_hip_yaw_pitch)
Set r_hip_yaw_pitch value.
void set_r_ankle_roll(const float new_r_ankle_roll)
Set r_ankle_roll value.
MoveServosMessage Fawkes BlackBoard Interface Message.
void set_l_total_weight(const float new_l_total_weight)
Set l_total_weight value.
StartUltrasonicMessage Fawkes BlackBoard Interface Message.
void set_r_total_weight(const float new_r_total_weight)
Set r_total_weight value.
UltrasonicDirection ultrasonic_direction() const
Get ultrasonic_direction value.
void set_r_shoulder_pitch(const float new_r_shoulder_pitch)
Set r_shoulder_pitch value.
SetBodyStiffnessMessage Fawkes BlackBoard Interface Message.
void set_r_elbow_yaw(const float new_r_elbow_yaw)
Set r_elbow_yaw value.
Thread class encapsulation of pthreads.
void set_head_yaw(const float new_head_yaw)
Set head_yaw value.
NaoSensorInterface Fawkes BlackBoard Interface.
void set_l_cop_y(const float new_l_cop_y)
Set l_cop_y value.
virtual void lock() const
Lock vector.
void set_head_pitch(const float new_head_pitch)
Set head_pitch value.
void write()
Write from local copy into BlackBoard memory.
NaoJointStiffnessInterface Fawkes BlackBoard Interface.
Logger * logger
This is the Logger member used to access the logger.
void set_l_wrist_yaw(const float new_l_wrist_yaw)
Set l_wrist_yaw value.
void set_gyro_y(const float new_gyro_y)
Set gyro_y value.
void set_battery_charge(const float new_battery_charge)
Set battery_charge value.
void set_l_hip_yaw_pitch(const float new_l_hip_yaw_pitch)
Set l_hip_yaw_pitch value.
void set_l_hip_pitch(const float new_l_hip_pitch)
Set l_hip_pitch value.
void set_l_ankle_pitch(const float new_l_ankle_pitch)
Set l_ankle_pitch value.
void set_r_fsr_fr(const float new_r_fsr_fr)
Set r_fsr_fr value.
void set_r_hand(const float new_r_hand)
Set r_hand value.
Thread aspect to use blocked timing.
void msgq_pop()
Erase first message from queue.
void set_ultrasonic_distance_right(unsigned int index, const float new_ultrasonic_distance_right)
Set ultrasonic_distance_right value at given index.
void set_l_hip_pitch(const float new_l_hip_pitch)
Set l_hip_pitch value.
virtual void unlock() const
Unlock vector.
void set_l_elbow_roll(const float new_l_elbow_roll)
Set l_elbow_roll value.
void set_r_fsr_fl(const float new_r_fsr_fl)
Set r_fsr_fl value.
UltrasonicDirection
This determines the chosen sender/receiver.
void set_r_elbow_yaw(const float new_r_elbow_yaw)
Set r_elbow_yaw value.
void wakeup()
Wake up thread.
void set_r_cop_y(const float new_r_cop_y)
Set r_cop_y value.
virtual void loop()
Code to execute in the thread.
void set_r_shoulder_roll(const float new_r_shoulder_roll)
Set r_shoulder_roll value.
void set_r_hip_roll(const float new_r_hip_roll)
Set r_hip_roll value.
Thread to provide DCM to Fawkes.
Base class for exceptions in Fawkes.
void set_gyro_x(const float new_gyro_x)
Set gyro_x value.
void set_angle_y(const float new_angle_y)
Set angle_y value.
void set_accel_y(const float new_accel_y)
Set accel_y value.
void set_l_foot_bumper_r(const uint8_t new_l_foot_bumper_r)
Set l_foot_bumper_r value.
void set_r_ankle_pitch(const float new_r_ankle_pitch)
Set r_ankle_pitch value.
void set_l_shoulder_pitch(const float new_l_shoulder_pitch)
Set l_shoulder_pitch value.
void set_r_hip_pitch(const float new_r_hip_pitch)
Set r_hip_pitch value.
void set_r_ankle_pitch(const float new_r_ankle_pitch)
Set r_ankle_pitch value.
void set_l_hip_yaw_pitch(const float new_l_hip_yaw_pitch)
Set l_hip_yaw_pitch value.
void set_l_cop_x(const float new_l_cop_x)
Set l_cop_x value.
const char * name() const
Get name of thread.
SetStiffnessesMessage Fawkes BlackBoard Interface Message.
void set_r_foot_bumper_l(const uint8_t new_r_foot_bumper_l)
Set r_foot_bumper_l value.
void set_r_wrist_yaw(const float new_r_wrist_yaw)
Set r_wrist_yaw value.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
void set_l_fsr_fr(const float new_l_fsr_fr)
Set l_fsr_fr value.
void set_head_touch_rear(const uint8_t new_head_touch_rear)
Set head_touch_rear value.
void set_l_knee_pitch(const float new_l_knee_pitch)
Set l_knee_pitch value.
virtual ~NaoQiDCMThread()
Destructor.
void cancel()
Cancel a thread.
virtual void loop()
Code to execute in the thread.
void set_l_foot_bumper_l(const uint8_t new_l_foot_bumper_l)
Set l_foot_bumper_l value.
void set_l_wrist_yaw(const float new_l_wrist_yaw)
Set l_wrist_yaw value.
void set_head_pitch(const float new_head_pitch)
Set head_pitch value.
void set_r_elbow_roll(const float new_r_elbow_roll)
Set r_elbow_roll value.
void set_r_knee_pitch(const float new_r_knee_pitch)
Set r_knee_pitch value.
MessageType * msgq_first_safe(MessageType *&msg)
Get first message casted to the desired type without exceptions.
void set_r_knee_pitch(const float new_r_knee_pitch)
Set r_knee_pitch value.
void set_l_hand(const float new_l_hand)
Set l_hand value.
MoveServoMessage Fawkes BlackBoard Interface Message.
void set_l_shoulder_pitch(const float new_l_shoulder_pitch)
Set l_shoulder_pitch value.
void join()
Join the thread.
void set_r_fsr_rl(const float new_r_fsr_rl)
Set r_fsr_rl value.
void set_l_elbow_roll(const float new_l_elbow_roll)
Set l_elbow_roll value.
void set_r_shoulder_roll(const float new_r_shoulder_roll)
Set r_shoulder_roll value.
EmitUltrasonicWaveMessage Fawkes BlackBoard Interface Message.
void set_r_hip_roll(const float new_r_hip_roll)
Set r_hip_roll value.
void set_l_elbow_yaw(const float new_l_elbow_yaw)
Set l_elbow_yaw value.
void set_ultrasonic_distance_left(unsigned int index, const float new_ultrasonic_distance_left)
Set ultrasonic_distance_left value at given index.
void set_r_fsr_rr(const float new_r_fsr_rr)
Set r_fsr_rr value.
void set_l_hip_roll(const float new_l_hip_roll)
Set l_hip_roll value.
void set_r_ankle_roll(const float new_r_ankle_roll)
Set r_ankle_roll value.
Thread to write data at full DCM frequency.
void set_l_knee_pitch(const float new_l_knee_pitch)
Set l_knee_pitch value.
void set_r_wrist_yaw(const float new_r_wrist_yaw)
Set r_wrist_yaw value.
void set_l_hand(const float new_l_hand)
Set l_hand value.
void set_time(const int32_t new_time)
Set time value.
void set_minimum(const float new_minimum)
Set minimum value.
SetStiffnessMessage Fawkes BlackBoard Interface Message.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
void set_r_cop_x(const float new_r_cop_x)
Set r_cop_x value.
void set_ultrasonic_direction(const UltrasonicDirection new_ultrasonic_direction)
Set ultrasonic_direction value.
void set_r_hand(const float new_r_hand)
Set r_hand value.
void set_head_yaw(const float new_head_yaw)
Set head_yaw value.
void set_head_touch_middle(const uint8_t new_head_touch_middle)
Set head_touch_middle value.
void start(bool wait=true)
Call this method to start the thread.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
void set_l_shoulder_roll(const float new_l_shoulder_roll)
Set l_shoulder_roll value.
void set_l_ankle_roll(const float new_l_ankle_roll)
Set l_ankle_roll value.
void set_angle_x(const float new_angle_x)
Set angle_x value.
NaoQiDCMThread()
Constructor.
virtual void close(Interface *interface)=0
Close interface.
void set_gyro_ref(const float new_gyro_ref)
Set gyro_ref value.