23 #include "dcm_thread.h"
25 #include "motion_utils.h"
27 #include <alcore/alerror.h>
28 #include <almemoryfastaccess/almemoryfastaccess.h>
29 #include <alproxies/allauncherproxy.h>
30 #include <alproxies/almemoryproxy.h>
31 #include <alproxies/almotionproxy.h>
32 #include <alproxies/dcmproxy.h>
33 #include <interfaces/NaoJointStiffnessInterface.h>
34 #include <interfaces/NaoSensorInterface.h>
36 #include <boost/bind.hpp>
69 STIFF_L_SHOULDER_PITCH,
70 STIFF_L_SHOULDER_ROLL,
75 STIFF_L_HIP_YAW_PITCH,
81 STIFF_R_SHOULDER_PITCH,
82 STIFF_R_SHOULDER_ROLL,
87 STIFF_R_HIP_YAW_PITCH,
115 ULTRASONIC_DIRECTION,
117 ULTRASONIC_DISTANCE_LEFT_0,
118 ULTRASONIC_DISTANCE_LEFT_1,
119 ULTRASONIC_DISTANCE_LEFT_2,
120 ULTRASONIC_DISTANCE_LEFT_3,
121 ULTRASONIC_DISTANCE_RIGHT_0,
122 ULTRASONIC_DISTANCE_RIGHT_1,
123 ULTRASONIC_DISTANCE_RIGHT_2,
124 ULTRASONIC_DISTANCE_RIGHT_3,
137 enum StiffnessJoint {
138 STIFFJ_HEAD_PITCH = 0,
140 STIFFJ_L_SHOULDER_PITCH,
141 STIFFJ_L_SHOULDER_ROLL,
144 STIFFJ_L_HIP_YAW_PITCH,
148 STIFFJ_L_ANKLE_PITCH,
150 STIFFJ_R_SHOULDER_PITCH,
151 STIFFJ_R_SHOULDER_ROLL,
154 STIFFJ_R_HIP_YAW_PITCH,
158 STIFFJ_R_ANKLE_PITCH,
167 #define ACCELEROMETER_G_FACTOR 56.
171 #define HEAD_YAW_MIN -1.2
172 #define HEAD_YAW_MAX 1.2
174 #define L_SHOULDER_PITCH_MIN -1.7
175 #define L_SHOULDER_PITCH_MAX 1.7
176 #define R_SHOULDER_PITCH_MIN -1.7
177 #define R_SHOULDER_PITCH_MAX 1.7
179 #define CLIP_VALUE(V, value) clip_value(value, V##_MIN, V##_MAX)
188 clip_value(
float value,
float min,
float max)
209 :
Thread(
"NaoQiDCMThread::HighFreqThread",
Thread::OPMODE_WAITFORWAKEUP), parent_(parent)
211 set_coalesce_wakeups(
true);
217 parent_->read_values();
218 if ((parent_->joint_pos_highfreq_if_->num_readers() > 0)
219 || (parent_->joint_stiffness_highfreq_if_->num_readers() > 0)
220 || (parent_->sensor_if_->num_readers() > 0)) {
221 parent_->update_interfaces(parent_->joint_pos_highfreq_if_,
222 parent_->joint_stiffness_highfreq_if_,
223 parent_->sensor_highfreq_if_);
226 parent_->process_messages();
252 :
Thread(
"NaoQiDCMThread",
Thread::OPMODE_WAITFORWAKEUP),
267 AL::ALPtr<AL::ALLauncherProxy> launcher(
new AL::ALLauncherProxy(
naoqi_broker));
268 bool is_dcm_available = launcher->isModulePresent(
"DCM");
269 bool is_almotion_available = launcher->isModulePresent(
"ALMotion");
271 if (!is_dcm_available) {
272 throw Exception(
"DCMThread: NaoQi DCM is not available");
274 if (!is_almotion_available) {
275 throw Exception(
"DCMThread: ALMotion is not available");
277 }
catch (AL::ALError &e) {
278 throw Exception(
"Checking module availability failed: %s", e.toString().c_str());
285 AL::ALPtr<AL::ALMemoryProxy> almemory =
naoqi_broker->getMemoryProxy();
286 std::string version = almemory->getData(
"RobotConfig/Body/BaseVersion", 0);
287 unsigned int num_joints = almotion_->getJointNames(
"Body").size();
288 if (num_joints == 26) {
289 robot_type_ = NaoJointPositionInterface::ROBOTYPE_ACADEMIC;
291 robot_type_ = NaoJointPositionInterface::ROBOTYPE_ROBOCUP;
293 robot_version_[2] = robot_version_[3] = 0;
294 if (version[0] ==
'V')
295 version = version.substr(1);
296 std::string::size_type pos;
297 if ((pos = version.find_first_of(
".")) != std::string::npos) {
298 std::string version_major = version.substr(0, pos);
299 std::string version_minor = version.substr(pos + 1);
300 robot_version_[0] = atoi(version_major.c_str());
301 robot_version_[1] = atoi(version_minor.c_str());
303 usboard_version_ = almemory->getData(
"Device/DeviceList/USBoard/ProgVersion", 0);
304 }
catch (AL::ALError &e) {
305 throw Exception(
"Retrieving robot info failed: %s", e.toString().c_str());
308 memfa_.reset(
new AL::ALMemoryFastAccess());
310 std::string prefix =
"Device/SubDeviceList/";
313 std::vector<std::string> keys;
314 keys.resize(SensorTypeN);
315 values_.resize(SensorTypeN);
317 keys[HEAD_PITCH] = prefix +
"HeadPitch/Position/Sensor/Value";
318 keys[HEAD_YAW] = prefix +
"HeadYaw/Position/Sensor/Value";
319 keys[L_SHOULDER_PITCH] = prefix +
"LShoulderPitch/Position/Sensor/Value";
320 keys[L_SHOULDER_ROLL] = prefix +
"LShoulderRoll/Position/Sensor/Value";
321 keys[L_ELBOW_YAW] = prefix +
"LElbowYaw/Position/Sensor/Value";
322 keys[L_ELBOW_ROLL] = prefix +
"LElbowRoll/Position/Sensor/Value";
323 keys[L_WRIST_YAW] = prefix +
"LWristYaw/Position/Sensor/Value";
324 keys[L_HAND] = prefix +
"LHand/Position/Sensor/Value";
325 keys[L_HIP_YAW_PITCH] = prefix +
"LHipYawPitch/Position/Sensor/Value";
326 keys[L_HIP_ROLL] = prefix +
"LHipRoll/Position/Sensor/Value";
327 keys[L_HIP_PITCH] = prefix +
"LHipPitch/Position/Sensor/Value";
328 keys[L_KNEE_PITCH] = prefix +
"LKneePitch/Position/Sensor/Value";
329 keys[L_ANKLE_PITCH] = prefix +
"LAnklePitch/Position/Sensor/Value";
330 keys[L_ANKLE_ROLL] = prefix +
"LAnkleRoll/Position/Sensor/Value";
332 keys[R_SHOULDER_PITCH] = prefix +
"RShoulderPitch/Position/Sensor/Value";
333 keys[R_SHOULDER_ROLL] = prefix +
"RShoulderRoll/Position/Sensor/Value";
334 keys[R_ELBOW_YAW] = prefix +
"RElbowYaw/Position/Sensor/Value";
335 keys[R_ELBOW_ROLL] = prefix +
"RElbowRoll/Position/Sensor/Value";
336 keys[R_WRIST_YAW] = prefix +
"RWristYaw/Position/Sensor/Value";
337 keys[R_HAND] = prefix +
"RHand/Position/Sensor/Value";
338 keys[R_HIP_YAW_PITCH] = prefix +
"RHipYawPitch/Position/Sensor/Value";
339 keys[R_HIP_ROLL] = prefix +
"RHipRoll/Position/Sensor/Value";
340 keys[R_HIP_PITCH] = prefix +
"RHipPitch/Position/Sensor/Value";
341 keys[R_KNEE_PITCH] = prefix +
"RKneePitch/Position/Sensor/Value";
342 keys[R_ANKLE_PITCH] = prefix +
"RAnklePitch/Position/Sensor/Value";
343 keys[R_ANKLE_ROLL] = prefix +
"RAnkleRoll/Position/Sensor/Value";
345 keys[STIFF_HEAD_PITCH] = prefix +
"HeadPitch/Hardness/Actuator/Value";
346 keys[STIFF_HEAD_YAW] = prefix +
"HeadYaw/Hardness/Actuator/Value";
347 keys[STIFF_L_SHOULDER_PITCH] = prefix +
"LShoulderPitch/Hardness/Actuator/Value";
348 keys[STIFF_L_SHOULDER_ROLL] = prefix +
"LShoulderRoll/Hardness/Actuator/Value";
349 keys[STIFF_L_ELBOW_YAW] = prefix +
"LElbowYaw/Hardness/Actuator/Value";
350 keys[STIFF_L_ELBOW_ROLL] = prefix +
"LElbowRoll/Hardness/Actuator/Value";
351 keys[STIFF_L_WRIST_YAW] = prefix +
"LWristYaw/Hardness/Actuator/Value";
352 keys[STIFF_L_HAND] = prefix +
"LHand/Hardness/Actuator/Value";
353 keys[STIFF_L_HIP_YAW_PITCH] = prefix +
"LHipYawPitch/Hardness/Actuator/Value";
354 keys[STIFF_L_HIP_ROLL] = prefix +
"LHipRoll/Hardness/Actuator/Value";
355 keys[STIFF_L_HIP_PITCH] = prefix +
"LHipPitch/Hardness/Actuator/Value";
356 keys[STIFF_L_KNEE_PITCH] = prefix +
"LKneePitch/Hardness/Actuator/Value";
357 keys[STIFF_L_ANKLE_PITCH] = prefix +
"LAnklePitch/Hardness/Actuator/Value";
358 keys[STIFF_L_ANKLE_ROLL] = prefix +
"LAnkleRoll/Hardness/Actuator/Value";
360 keys[STIFF_R_SHOULDER_PITCH] = prefix +
"RShoulderPitch/Hardness/Actuator/Value";
361 keys[STIFF_R_SHOULDER_ROLL] = prefix +
"RShoulderRoll/Hardness/Actuator/Value";
362 keys[STIFF_R_ELBOW_YAW] = prefix +
"RElbowYaw/Hardness/Actuator/Value";
363 keys[STIFF_R_ELBOW_ROLL] = prefix +
"RElbowRoll/Hardness/Actuator/Value";
364 keys[STIFF_R_WRIST_YAW] = prefix +
"RWristYaw/Hardness/Actuator/Value";
365 keys[STIFF_R_HAND] = prefix +
"RHand/Hardness/Actuator/Value";
366 keys[STIFF_R_HIP_YAW_PITCH] = prefix +
"RHipYawPitch/Hardness/Actuator/Value";
367 keys[STIFF_R_HIP_ROLL] = prefix +
"RHipRoll/Hardness/Actuator/Value";
368 keys[STIFF_R_HIP_PITCH] = prefix +
"RHipPitch/Hardness/Actuator/Value";
369 keys[STIFF_R_KNEE_PITCH] = prefix +
"RKneePitch/Hardness/Actuator/Value";
370 keys[STIFF_R_ANKLE_PITCH] = prefix +
"RAnklePitch/Hardness/Actuator/Value";
371 keys[STIFF_R_ANKLE_ROLL] = prefix +
"RAnkleRoll/Hardness/Actuator/Value";
374 keys[ACC_X] = prefix +
"InertialSensor/AccX/Sensor/Value";
375 keys[ACC_Y] = prefix +
"InertialSensor/AccY/Sensor/Value";
376 keys[ACC_Z] = prefix +
"InertialSensor/AccZ/Sensor/Value";
377 keys[GYR_X] = prefix +
"InertialSensor/GyrX/Sensor/Value";
378 keys[GYR_Y] = prefix +
"InertialSensor/GyrY/Sensor/Value";
379 keys[GYR_REF] = prefix +
"InertialSensor/GyrRef/Sensor/Value";
380 keys[ANGLE_X] = prefix +
"InertialSensor/AngleX/Sensor/Value";
381 keys[ANGLE_Y] = prefix +
"InertialSensor/AngleY/Sensor/Value";
384 keys[L_FSR_FL] = prefix +
"LFoot/FSR/FrontLeft/Sensor/Value";
385 keys[L_FSR_FR] = prefix +
"LFoot/FSR/FrontRight/Sensor/Value";
386 keys[L_FSR_RL] = prefix +
"LFoot/FSR/RearLeft/Sensor/Value";
387 keys[L_FSR_RR] = prefix +
"LFoot/FSR/RearRight/Sensor/Value";
388 keys[R_FSR_FL] = prefix +
"RFoot/FSR/FrontLeft/Sensor/Value";
389 keys[R_FSR_FR] = prefix +
"RFoot/FSR/FrontRight/Sensor/Value";
390 keys[R_FSR_RL] = prefix +
"RFoot/FSR/RearLeft/Sensor/Value";
391 keys[R_FSR_RR] = prefix +
"RFoot/FSR/RearRight/Sensor/Value";
392 keys[L_COP_X] = prefix +
"LFoot/FSR/CenterOfPressure/X/Sensor/Value";
393 keys[L_COP_Y] = prefix +
"LFoot/FSR/CenterOfPressure/Y/Sensor/Value";
394 keys[L_TOTAL_WEIGHT] = prefix +
"LFoot/FSR/TotalWeight/Sensor/Value";
395 keys[R_COP_X] = prefix +
"RFoot/FSR/CenterOfPressure/X/Sensor/Value";
396 keys[R_COP_Y] = prefix +
"RFoot/FSR/CenterOfPressure/Y/Sensor/Value";
397 keys[R_TOTAL_WEIGHT] = prefix +
"RFoot/FSR/TotalWeight/Sensor/Value";
400 keys[ULTRASONIC_DIRECTION] = prefix +
"US/Actuator/Value";
401 keys[ULTRASONIC_DISTANCE] = prefix +
"US/Sensor/Value";
403 keys[ULTRASONIC_DISTANCE_LEFT_0] = prefix +
"US/Left/Sensor/Value";
404 keys[ULTRASONIC_DISTANCE_LEFT_1] = prefix +
"US/Left/Sensor/Value1";
405 keys[ULTRASONIC_DISTANCE_LEFT_2] = prefix +
"US/Left/Sensor/Value2";
406 keys[ULTRASONIC_DISTANCE_LEFT_3] = prefix +
"US/Left/Sensor/Value3";
408 keys[ULTRASONIC_DISTANCE_RIGHT_0] = prefix +
"US/Right/Sensor/Value";
409 keys[ULTRASONIC_DISTANCE_RIGHT_1] = prefix +
"US/Right/Sensor/Value1";
410 keys[ULTRASONIC_DISTANCE_RIGHT_2] = prefix +
"US/Right/Sensor/Value2";
411 keys[ULTRASONIC_DISTANCE_RIGHT_3] = prefix +
"US/Right/Sensor/Value3";
414 keys[L_FOOT_BUMPER_L] = prefix +
"LFoot/Bumper/Left/Sensor/Value";
415 keys[L_FOOT_BUMPER_R] = prefix +
"LFoot/Bumper/Right/Sensor/Value";
416 keys[R_FOOT_BUMPER_L] = prefix +
"RFoot/Bumper/Left/Sensor/Value";
417 keys[R_FOOT_BUMPER_R] = prefix +
"RFoot/Bumper/Right/Sensor/Value";
419 keys[HEAD_TOUCH_FRONT] = prefix +
"Head/Touch/Front/Sensor/Value";
420 keys[HEAD_TOUCH_MIDDLE] = prefix +
"Head/Touch/Middle/Sensor/Value";
421 keys[HEAD_TOUCH_REAR] = prefix +
"Head/Touch/Rear/Sensor/Value";
423 keys[CHEST_BUTTON] = prefix +
"ChestBoard/Button/Sensor/Value";
426 keys[BATTERY_CHARGE] = prefix +
"Battery/Charge/Sensor/Value";
430 }
catch (AL::ALError &e) {
431 throw Exception(
"Failed to setup fast memory access: %s", e.toString().c_str());
435 if (robot_type_ == NaoJointPositionInterface::ROBOTYPE_ROBOCUP) {
436 alljoint_names_.arraySetSize(22);
438 alljoint_names_.arraySetSize(26);
439 alljoint_names_[STIFFJ_L_WRIST_YAW] =
"LWristYaw";
440 alljoint_names_[STIFFJ_L_HAND] =
"LHand";
441 alljoint_names_[STIFFJ_R_WRIST_YAW] =
"RWristYaw";
442 alljoint_names_[STIFFJ_R_HAND] =
"RHand";
444 alljoint_names_[STIFFJ_HEAD_PITCH] =
"HeadPitch";
445 alljoint_names_[STIFFJ_HEAD_YAW] =
"HeadYaw";
446 alljoint_names_[STIFFJ_L_SHOULDER_PITCH] =
"LShoulderPitch";
447 alljoint_names_[STIFFJ_L_SHOULDER_ROLL] =
"LShoulderRoll";
448 alljoint_names_[STIFFJ_L_ELBOW_YAW] =
"LElbowYaw";
449 alljoint_names_[STIFFJ_L_ELBOW_ROLL] =
"LElbowRoll";
450 alljoint_names_[STIFFJ_L_HIP_YAW_PITCH] =
"LHipYawPitch";
451 alljoint_names_[STIFFJ_L_HIP_ROLL] =
"LHipRoll";
452 alljoint_names_[STIFFJ_L_HIP_PITCH] =
"LHipPitch";
453 alljoint_names_[STIFFJ_L_KNEE_PITCH] =
"LKneePitch";
454 alljoint_names_[STIFFJ_L_ANKLE_PITCH] =
"LAnklePitch";
455 alljoint_names_[STIFFJ_L_ANKLE_ROLL] =
"LAnkleRoll";
457 alljoint_names_[STIFFJ_R_SHOULDER_PITCH] =
"RShoulderPitch";
458 alljoint_names_[STIFFJ_R_SHOULDER_ROLL] =
"RShoulderRoll";
459 alljoint_names_[STIFFJ_R_ELBOW_YAW] =
"RElbowYaw";
460 alljoint_names_[STIFFJ_R_ELBOW_ROLL] =
"RElbowRoll";
461 alljoint_names_[STIFFJ_R_HIP_YAW_PITCH] =
"RHipYawPitch";
462 alljoint_names_[STIFFJ_R_HIP_ROLL] =
"RHipRoll";
463 alljoint_names_[STIFFJ_R_HIP_PITCH] =
"RHipPitch";
464 alljoint_names_[STIFFJ_R_KNEE_PITCH] =
"RKneePitch";
465 alljoint_names_[STIFFJ_R_ANKLE_PITCH] =
"RAnklePitch";
466 alljoint_names_[STIFFJ_R_ANKLE_ROLL] =
"RAnkleRoll";
469 AL::ALValue setJointStiffnessAlias;
471 setJointStiffnessAlias.clear();
472 setJointStiffnessAlias.arraySetSize(2);
473 setJointStiffnessAlias[0] = std::string(
"setJointStiffness");
474 setJointStiffnessAlias[1].arraySetSize(26);
477 int offset = STIFF_HEAD_PITCH - HEAD_PITCH;
478 for (
int i = HEAD_PITCH; i <= R_ANKLE_ROLL; ++i) {
479 setJointStiffnessAlias[1][i] = keys[i + offset];
482 dcm_->createAlias(setJointStiffnessAlias);
483 }
catch (AL::ALError &e) {
485 throw Exception(
"Failed to create SetJointStiffness alias: %s", e.toString().c_str());
489 joint_stiffness_if_ =
493 joint_pos_highfreq_if_ =
495 joint_stiffness_highfreq_if_ =
500 dcm_time_ = dcm_->getTime(0);
501 memfa_->GetValues(values_);
505 joint_pos_highfreq_if_->set_robot_type(robot_type_);
506 joint_pos_highfreq_if_->set_robot_version(robot_version_);
508 sensor_if_->set_ultrasonic_direction(NaoSensorInterface::USD_NONE);
509 sensor_highfreq_if_->set_ultrasonic_direction(NaoSensorInterface::USD_NONE);
512 update_interfaces(joint_pos_if_, joint_stiffness_if_, sensor_if_);
513 update_interfaces(joint_pos_highfreq_if_, joint_stiffness_highfreq_if_, sensor_highfreq_if_);
516 highfreq_thread_->
start();
518 dcm_sigconn_ = dcm_->getGenericProxy()->getModule()->atPostProcess(
519 boost::bind(&NaoQiDCMThread::dcm_callback,
this));
537 NaoQiDCMThread::dcm_callback()
539 highfreq_thread_->
wakeup();
545 dcm_sigconn_.disconnect();
547 highfreq_thread_->
cancel();
548 highfreq_thread_->
join();
549 delete highfreq_thread_;
557 joint_pos_if_ = NULL;
558 joint_stiffness_if_ = NULL;
560 joint_pos_highfreq_if_ = NULL;
561 joint_stiffness_highfreq_if_ = NULL;
562 sensor_highfreq_if_ = NULL;
569 NaoQiDCMThread::read_values()
572 dcm_time_ = dcm_->getTime(0);
573 memfa_->GetValues(values_);
580 update_interfaces(joint_pos_if_, joint_stiffness_if_, sensor_if_);
625 joint_stiffness_if->
set_head_yaw(values_[STIFF_HEAD_YAW]);
633 joint_stiffness_if->
set_l_hand(values_[STIFF_L_HAND]);
639 joint_stiffness_if->
set_r_hand(values_[STIFF_R_HAND]);
657 float min_stiffness = 1.;
658 for (
int i = STIFF_HEAD_YAW; i <= STIFF_R_ANKLE_ROLL; ++i) {
660 if ((robot_type_ == NaoJointPositionInterface::ROBOTYPE_ROBOCUP)
661 && ((i == STIFF_L_WRIST_YAW) || (i == STIFF_L_HAND) || (i == STIFF_R_WRIST_YAW)
662 || (i == STIFF_R_HAND)))
666 if (i == STIFF_R_HIP_YAW_PITCH)
669 if (values_[i] < min_stiffness)
670 min_stiffness = values_[i];
705 sensor_if->
set_accel_x(values_[ACC_X] / ACCELEROMETER_G_FACTOR);
706 sensor_if->
set_accel_y(values_[ACC_Y] / ACCELEROMETER_G_FACTOR);
707 sensor_if->
set_accel_z(values_[ACC_Z] / ACCELEROMETER_G_FACTOR);
719 case NaoSensorInterface::USD_LEFT_LEFT:
720 case NaoSensorInterface::USD_RIGHT_LEFT: {
721 float us_left[4] = {values_[ULTRASONIC_DISTANCE], 0, 0, 0};
724 float us_right[4] = {0, 0, 0, 0};
727 case NaoSensorInterface::USD_LEFT_RIGHT:
728 case NaoSensorInterface::USD_RIGHT_RIGHT: {
729 float us_left[4] = {0, 0, 0, 0};
732 float us_right[4] = {values_[ULTRASONIC_DISTANCE], 0, 0, 0};
737 float us_left[4] = {values_[ULTRASONIC_DISTANCE_LEFT_0],
738 values_[ULTRASONIC_DISTANCE_LEFT_1],
739 values_[ULTRASONIC_DISTANCE_LEFT_2],
740 values_[ULTRASONIC_DISTANCE_LEFT_3]};
743 float us_right[4] = {values_[ULTRASONIC_DISTANCE_RIGHT_0],
744 values_[ULTRASONIC_DISTANCE_RIGHT_1],
745 values_[ULTRASONIC_DISTANCE_RIGHT_2],
746 values_[ULTRASONIC_DISTANCE_RIGHT_3]};
755 joint_pos_if->
write();
756 joint_stiffness_if->
write();
761 NaoQiDCMThread::process_messages()
766 send_commands(msg->servo(),
"Position", msg->value(), msg->time());
775 std::vector<std::string> servos = parse_servo_bitfield(msg->servo());
776 std::vector<float> values(servos.size(), msg->value());
777 almotion_->setAngles(servos, values, msg->speed());
782 motion::move_joints(almotion_,
785 msg->l_shoulder_pitch(),
786 msg->l_shoulder_roll(),
791 msg->l_hip_yaw_pitch(),
795 msg->l_ankle_pitch(),
797 msg->r_shoulder_pitch(),
798 msg->r_shoulder_roll(),
803 msg->r_hip_yaw_pitch(),
807 msg->r_ankle_pitch(),
824 std::vector<std::string> servos = parse_servo_bitfield(msg->servo());
825 std::vector<float> values(servos.size(), msg->value());
827 almotion_->post.stiffnessInterpolation(servos, values, msg->time_sec());
851 almotion_->post.stiffnessInterpolation(
"Body", msg->value(), msg->time_sec());
856 std::vector<float> values(alljoint_names_.getSize());
857 values[STIFFJ_HEAD_PITCH] = msg->head_pitch();
858 values[STIFFJ_HEAD_YAW] = msg->head_yaw();
859 values[STIFFJ_L_SHOULDER_PITCH] = msg->l_shoulder_pitch();
860 values[STIFFJ_L_SHOULDER_ROLL] = msg->l_shoulder_roll();
861 values[STIFFJ_L_ELBOW_YAW] = msg->l_elbow_yaw();
862 values[STIFFJ_L_ELBOW_ROLL] = msg->l_elbow_roll();
863 values[STIFFJ_L_HIP_YAW_PITCH] = msg->l_hip_yaw_pitch();
864 values[STIFFJ_L_HIP_ROLL] = msg->l_hip_roll();
865 values[STIFFJ_L_HIP_PITCH] = msg->l_hip_pitch();
866 values[STIFFJ_L_KNEE_PITCH] = msg->l_knee_pitch();
867 values[STIFFJ_L_ANKLE_PITCH] = msg->l_ankle_pitch();
868 values[STIFFJ_L_ANKLE_ROLL] = msg->l_ankle_roll();
870 values[STIFFJ_R_SHOULDER_PITCH] = msg->r_shoulder_pitch();
871 values[STIFFJ_R_SHOULDER_ROLL] = msg->r_shoulder_roll();
872 values[STIFFJ_R_ELBOW_YAW] = msg->r_elbow_yaw();
873 values[STIFFJ_R_ELBOW_ROLL] = msg->r_elbow_roll();
874 values[STIFFJ_R_HIP_YAW_PITCH] = msg->r_hip_yaw_pitch();
875 values[STIFFJ_R_HIP_ROLL] = msg->r_hip_roll();
876 values[STIFFJ_R_HIP_PITCH] = msg->r_hip_pitch();
877 values[STIFFJ_R_KNEE_PITCH] = msg->r_knee_pitch();
878 values[STIFFJ_R_ANKLE_PITCH] = msg->r_ankle_pitch();
879 values[STIFFJ_R_ANKLE_ROLL] = msg->r_ankle_roll();
881 if (robot_type_ != NaoJointPositionInterface::ROBOTYPE_ROBOCUP) {
882 values[STIFFJ_L_WRIST_YAW] = msg->l_wrist_yaw();
883 values[STIFFJ_L_HAND] = msg->l_hand();
884 values[STIFFJ_R_WRIST_YAW] = msg->r_wrist_yaw();
885 values[STIFFJ_R_HAND] = msg->r_hand();
888 almotion_->post.stiffnessInterpolation(alljoint_names_, values, msg->time_sec());
897 int value = ultrasonic_value(msg->ultrasonic_direction());
898 send_command(
"US/Actuator/Value", value,
"ClearAll", 0);
904 int value = ultrasonic_value(msg->ultrasonic_direction());
906 send_command(
"US/Actuator/Value", value,
"ClearAll", 0);
912 send_command(
"US/Actuator/Value", 0,
"ClearAll", 0);
923 NaoQiDCMThread::send_commands(
unsigned int servos,
924 const std::string &what,
936 std::vector<std::string> servonames = parse_servo_bitfield(servos);
938 std::vector<std::string>::iterator s;
939 for (s = servonames.begin(); s != servonames.end(); ++s) {
941 if (*s ==
"HeadYaw") {
942 v = CLIP_VALUE(HEAD_YAW, value);
943 }
else if (*s ==
"LShoulderPitch") {
944 v = CLIP_VALUE(L_SHOULDER_PITCH, value);
945 }
else if (*s ==
"RShoulderPitch") {
946 v = CLIP_VALUE(R_SHOULDER_PITCH, value);
948 send_command(*s +
"/" + what +
"/Actuator/Value", v,
"Merge", time_offset);
953 NaoQiDCMThread::send_command(
const std::string &name,
955 const std::string &kind,
969 cmd[2].arraySetSize(1);
970 cmd[2][0].arraySetSize(2);
971 cmd[2][0][0] = value;
972 cmd[2][0][1] = dcm_time_ + time_offset;
977 std::vector<std::string>
978 NaoQiDCMThread::parse_servo_bitfield(
unsigned int servos)
980 std::vector<std::string> servonames;
982 if (servos & NaoJointPositionInterface::SERVO_head_yaw)
983 servonames.push_back(
"HeadYaw");
985 if (servos & NaoJointPositionInterface::SERVO_head_pitch)
986 servonames.push_back(
"HeadPitch");
988 if (servos & NaoJointPositionInterface::SERVO_l_shoulder_pitch)
989 servonames.push_back(
"LShoulderPitch");
991 if (servos & NaoJointPositionInterface::SERVO_l_shoulder_roll)
992 servonames.push_back(
"LShoulderRoll");
994 if (servos & NaoJointPositionInterface::SERVO_l_elbow_yaw)
995 servonames.push_back(
"LElbowYaw");
997 if (servos & NaoJointPositionInterface::SERVO_l_elbow_roll)
998 servonames.push_back(
"LElbowRoll");
1000 if (servos & NaoJointPositionInterface::SERVO_r_shoulder_pitch)
1001 servonames.push_back(
"RShoulderPitch");
1003 if (servos & NaoJointPositionInterface::SERVO_r_shoulder_roll)
1004 servonames.push_back(
"RShoulderRoll");
1006 if (servos & NaoJointPositionInterface::SERVO_r_elbow_yaw)
1007 servonames.push_back(
"RElbowYaw");
1009 if (servos & NaoJointPositionInterface::SERVO_r_elbow_roll)
1010 servonames.push_back(
"RElbowRoll");
1012 if (servos & NaoJointPositionInterface::SERVO_l_hip_yaw_pitch)
1013 servonames.push_back(
"LHipYawPitch");
1015 if (servos & NaoJointPositionInterface::SERVO_l_hip_pitch)
1016 servonames.push_back(
"LHipPitch");
1018 if (servos & NaoJointPositionInterface::SERVO_l_hip_roll)
1019 servonames.push_back(
"LHipRoll");
1021 if (servos & NaoJointPositionInterface::SERVO_r_hip_yaw_pitch)
1022 servonames.push_back(
"RHipYawPitch");
1024 if (servos & NaoJointPositionInterface::SERVO_r_hip_pitch)
1025 servonames.push_back(
"RHipPitch");
1027 if (servos & NaoJointPositionInterface::SERVO_r_hip_roll)
1028 servonames.push_back(
"RHipRoll");
1030 if (servos & NaoJointPositionInterface::SERVO_l_knee_pitch)
1031 servonames.push_back(
"LKneePitch");
1033 if (servos & NaoJointPositionInterface::SERVO_r_knee_pitch)
1034 servonames.push_back(
"RKneePitch");
1036 if (servos & NaoJointPositionInterface::SERVO_l_ankle_pitch)
1037 servonames.push_back(
"LAnklePitch");
1039 if (servos & NaoJointPositionInterface::SERVO_l_ankle_roll)
1040 servonames.push_back(
"LAnkleRoll");
1042 if (servos & NaoJointPositionInterface::SERVO_r_ankle_pitch)
1043 servonames.push_back(
"RAnklePitch");
1045 if (servos & NaoJointPositionInterface::SERVO_r_ankle_roll)
1046 servonames.push_back(
"RAnkleRoll");
1055 switch (direction) {
1056 case NaoSensorInterface::USD_LEFT_LEFT: value = 0;
break;
1057 case NaoSensorInterface::USD_LEFT_RIGHT: value = 1;
break;
1058 case NaoSensorInterface::USD_RIGHT_LEFT: value = 2;
break;
1059 case NaoSensorInterface::USD_RIGHT_RIGHT: value = 3;
break;
1060 case NaoSensorInterface::USD_BOTH_BOTH: value = 12;
break;
1063 "Illegal ultrasonic direction, "
1064 "using left-right");
Thread to write data at full DCM frequency.
virtual void loop()
Code to execute in the thread.
HighFreqThread(NaoQiDCMThread *parent)
Constructor.
Thread to provide DCM to Fawkes.
virtual void finalize()
Finalize the thread.
virtual void init()
Initialize the thread.
virtual ~NaoQiDCMThread()
Destructor.
NaoQiDCMThread()
Constructor.
virtual void loop()
Code to execute in the thread.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
Base class for exceptions in Fawkes.
MessageType * msgq_first_safe(MessageType *&msg)
Get first message casted to the desired type without exceptions.
void msgq_pop()
Erase first message from queue.
void write()
Write from local copy into BlackBoard memory.
bool msgq_empty()
Check if queue is empty.
virtual void lock() const
Lock vector.
virtual void unlock() const
Unlock vector.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
Logger * logger
This is the Logger member used to access the logger.
MoveServoMessage Fawkes BlackBoard Interface Message.
MoveServosMessage Fawkes BlackBoard Interface Message.
SetServoMessage Fawkes BlackBoard Interface Message.
SetServosMessage Fawkes BlackBoard Interface Message.
NaoJointPositionInterface Fawkes BlackBoard Interface.
void set_l_wrist_yaw(const float new_l_wrist_yaw)
Set l_wrist_yaw value.
void set_r_ankle_pitch(const float new_r_ankle_pitch)
Set r_ankle_pitch value.
void set_r_hip_pitch(const float new_r_hip_pitch)
Set r_hip_pitch value.
void set_r_elbow_yaw(const float new_r_elbow_yaw)
Set r_elbow_yaw value.
void set_l_ankle_roll(const float new_l_ankle_roll)
Set l_ankle_roll value.
void set_r_shoulder_roll(const float new_r_shoulder_roll)
Set r_shoulder_roll value.
void set_l_shoulder_pitch(const float new_l_shoulder_pitch)
Set l_shoulder_pitch value.
void set_r_knee_pitch(const float new_r_knee_pitch)
Set r_knee_pitch value.
void set_r_hip_yaw_pitch(const float new_r_hip_yaw_pitch)
Set r_hip_yaw_pitch value.
void set_l_ankle_pitch(const float new_l_ankle_pitch)
Set l_ankle_pitch value.
void set_r_wrist_yaw(const float new_r_wrist_yaw)
Set r_wrist_yaw value.
void set_l_elbow_yaw(const float new_l_elbow_yaw)
Set l_elbow_yaw value.
void set_l_hip_yaw_pitch(const float new_l_hip_yaw_pitch)
Set l_hip_yaw_pitch value.
void set_r_elbow_roll(const float new_r_elbow_roll)
Set r_elbow_roll 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_l_shoulder_roll(const float new_l_shoulder_roll)
Set l_shoulder_roll value.
void set_robot_version(unsigned int index, const uint8_t new_robot_version)
Set robot_version value at given index.
void set_r_ankle_roll(const float new_r_ankle_roll)
Set r_ankle_roll value.
void set_l_knee_pitch(const float new_l_knee_pitch)
Set l_knee_pitch value.
void set_r_shoulder_pitch(const float new_r_shoulder_pitch)
Set r_shoulder_pitch value.
void set_l_elbow_roll(const float new_l_elbow_roll)
Set l_elbow_roll value.
void set_r_hip_roll(const float new_r_hip_roll)
Set r_hip_roll value.
void set_robot_type(const RobotType new_robot_type)
Set robot_type value.
void set_l_hip_pitch(const float new_l_hip_pitch)
Set l_hip_pitch value.
void set_head_yaw(const float new_head_yaw)
Set head_yaw value.
void set_head_pitch(const float new_head_pitch)
Set head_pitch value.
void set_l_hip_roll(const float new_l_hip_roll)
Set l_hip_roll value.
void set_r_hand(const float new_r_hand)
Set r_hand value.
SetBodyStiffnessMessage Fawkes BlackBoard Interface Message.
SetStiffnessMessage Fawkes BlackBoard Interface Message.
SetStiffnessesMessage Fawkes BlackBoard Interface Message.
NaoJointStiffnessInterface Fawkes BlackBoard Interface.
void set_r_hip_roll(const float new_r_hip_roll)
Set r_hip_roll 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_l_ankle_roll(const float new_l_ankle_roll)
Set l_ankle_roll value.
void set_r_hip_pitch(const float new_r_hip_pitch)
Set r_hip_pitch value.
void set_l_elbow_yaw(const float new_l_elbow_yaw)
Set l_elbow_yaw value.
void set_r_hip_yaw_pitch(const float new_r_hip_yaw_pitch)
Set r_hip_yaw_pitch value.
void set_r_wrist_yaw(const float new_r_wrist_yaw)
Set r_wrist_yaw value.
void set_l_hip_roll(const float new_l_hip_roll)
Set l_hip_roll value.
void set_minimum(const float new_minimum)
Set minimum value.
void set_r_shoulder_pitch(const float new_r_shoulder_pitch)
Set r_shoulder_pitch value.
void set_head_pitch(const float new_head_pitch)
Set head_pitch value.
void set_l_knee_pitch(const float new_l_knee_pitch)
Set l_knee_pitch value.
void set_l_hip_yaw_pitch(const float new_l_hip_yaw_pitch)
Set l_hip_yaw_pitch value.
void set_r_hand(const float new_r_hand)
Set r_hand value.
void set_l_shoulder_roll(const float new_l_shoulder_roll)
Set l_shoulder_roll value.
void set_l_ankle_pitch(const float new_l_ankle_pitch)
Set l_ankle_pitch value.
void set_r_ankle_roll(const float new_r_ankle_roll)
Set r_ankle_roll value.
void set_l_elbow_roll(const float new_l_elbow_roll)
Set l_elbow_roll value.
void set_r_elbow_yaw(const float new_r_elbow_yaw)
Set r_elbow_yaw value.
void set_l_wrist_yaw(const float new_l_wrist_yaw)
Set l_wrist_yaw value.
void set_l_hip_pitch(const float new_l_hip_pitch)
Set l_hip_pitch value.
void set_l_hand(const float new_l_hand)
Set l_hand value.
void set_r_shoulder_roll(const float new_r_shoulder_roll)
Set r_shoulder_roll value.
void set_r_knee_pitch(const float new_r_knee_pitch)
Set r_knee_pitch value.
void set_r_elbow_roll(const float new_r_elbow_roll)
Set r_elbow_roll value.
void set_head_yaw(const float new_head_yaw)
Set head_yaw value.
AL::ALPtr< AL::ALBroker > naoqi_broker
NaoQi broker.
EmitUltrasonicWaveMessage Fawkes BlackBoard Interface Message.
StartUltrasonicMessage Fawkes BlackBoard Interface Message.
StopUltrasonicMessage Fawkes BlackBoard Interface Message.
NaoSensorInterface Fawkes BlackBoard Interface.
void set_r_total_weight(const float new_r_total_weight)
Set r_total_weight value.
void set_l_fsr_fr(const float new_l_fsr_fr)
Set l_fsr_fr value.
void set_gyro_ref(const float new_gyro_ref)
Set gyro_ref value.
void set_accel_z(const float new_accel_z)
Set accel_z value.
void set_angle_x(const float new_angle_x)
Set angle_x value.
void set_accel_x(const float new_accel_x)
Set accel_x value.
void set_head_touch_rear(const uint8_t new_head_touch_rear)
Set head_touch_rear value.
void set_l_fsr_rr(const float new_l_fsr_rr)
Set l_fsr_rr value.
void set_head_touch_front(const uint8_t new_head_touch_front)
Set head_touch_front value.
UltrasonicDirection
This determines the chosen sender/receiver.
void set_ultrasonic_direction(const UltrasonicDirection new_ultrasonic_direction)
Set ultrasonic_direction value.
void set_battery_charge(const float new_battery_charge)
Set battery_charge value.
void set_accel_y(const float new_accel_y)
Set accel_y value.
void set_r_foot_bumper_l(const uint8_t new_r_foot_bumper_l)
Set r_foot_bumper_l value.
void set_r_cop_x(const float new_r_cop_x)
Set r_cop_x value.
void set_r_fsr_fl(const float new_r_fsr_fl)
Set r_fsr_fl value.
void set_r_fsr_fr(const float new_r_fsr_fr)
Set r_fsr_fr value.
void set_gyro_x(const float new_gyro_x)
Set gyro_x value.
void set_head_touch_middle(const uint8_t new_head_touch_middle)
Set head_touch_middle value.
void set_gyro_y(const float new_gyro_y)
Set gyro_y value.
void set_l_total_weight(const float new_l_total_weight)
Set l_total_weight value.
void set_r_foot_bumper_r(const uint8_t new_r_foot_bumper_r)
Set r_foot_bumper_r value.
void set_l_foot_bumper_r(const uint8_t new_l_foot_bumper_r)
Set l_foot_bumper_r value.
void set_l_fsr_fl(const float new_l_fsr_fl)
Set l_fsr_fl 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_cop_y(const float new_r_cop_y)
Set r_cop_y value.
void set_r_fsr_rr(const float new_r_fsr_rr)
Set r_fsr_rr value.
void set_l_cop_x(const float new_l_cop_x)
Set l_cop_x value.
void set_l_cop_y(const float new_l_cop_y)
Set l_cop_y value.
void set_r_fsr_rl(const float new_r_fsr_rl)
Set r_fsr_rl value.
void set_chest_button(const uint8_t new_chest_button)
Set chest_button value.
UltrasonicDirection ultrasonic_direction() const
Get ultrasonic_direction value.
void set_ultrasonic_distance_right(unsigned int index, const float new_ultrasonic_distance_right)
Set ultrasonic_distance_right value at given index.
void set_angle_y(const float new_angle_y)
Set angle_y value.
void set_l_foot_bumper_l(const uint8_t new_l_foot_bumper_l)
Set l_foot_bumper_l value.
void set_l_fsr_rl(const float new_l_fsr_rl)
Set l_fsr_rl value.
Thread class encapsulation of pthreads.
const char * name() const
Get name of thread.
void start(bool wait=true)
Call this method to start the thread.
void join()
Join the thread.
void wakeup()
Wake up thread.
void cancel()
Cancel a thread.
Fawkes library namespace.