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");