Fawkes API  Fawkes Development Version
dcm_thread.cpp
1 
2 /***************************************************************************
3  * dcm_thread.cpp - Provide NaoQi DCM to Fawkes
4  *
5  * Created: Tue May 31 15:00:54 2011
6  * Copyright 2006-2011 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "dcm_thread.h"
24 #include "motion_utils.h"
25 
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>
32 
33 #include <boost/bind.hpp>
34 
35 #include <interfaces/NaoJointStiffnessInterface.h>
36 #include <interfaces/NaoSensorInterface.h>
37 
38 using namespace fawkes;
39 
40 enum SensorType { HEAD_PITCH = 0, HEAD_YAW,
41  L_SHOULDER_PITCH, L_SHOULDER_ROLL, L_ELBOW_YAW, L_ELBOW_ROLL,
42  L_WRIST_YAW, L_HAND,
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,
46  R_WRIST_YAW, R_HAND,
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,
64  R_TOTAL_WEIGHT,
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,
74  SensorTypeN};
75 
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,
89  StiffnessJointN };
90 
91 #define ACCELEROMETER_G_FACTOR 56.
92 
93 // Clipping as suggested by Aldebaran at RoboCup 2008, Suzhou
94 // Changed head yaw to +/- 1.2 according to Nao v3 red book
95 #define HEAD_YAW_MIN -1.2
96 #define HEAD_YAW_MAX 1.2
97 // New values after RoboCup Workshop 2009 at Aldebaran, Paris
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
102 
103 #define CLIP_VALUE(V, value) clip_value(value, V ## _MIN, V ## _MAX)
104 
105 /** Clip value to given constraints.
106  * @param value value to clop
107  * @param min minimum value
108  * @param max maximum value
109  * @return clipped value
110  */
111 static inline float
112 clip_value(float value, float min, float max)
113 {
114  if (value < min) value = min;
115  if (value > max) value = max;
116  return value;
117 }
118 
119 
120 /** Thread to write data at full DCM frequency.
121  * This thread is woken up by the DCM callback and publishes new data
122  * if there is a reader for any of the high frequency interfaces. It
123  * is also responsible for processing incoming commands.
124  */
126 {
127  public:
128  /** Constructor.
129  * @param parent parent NaoQiDCMThread to call.
130  */
132  : Thread("NaoQiDCMThread::HighFreqThread", Thread::OPMODE_WAITFORWAKEUP),
133  __parent(parent)
134  {
135  set_coalesce_wakeups(true);
136  }
137 
138  virtual void loop()
139  {
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) )
144  {
145  __parent->update_interfaces(__parent->__joint_pos_highfreq_if,
146  __parent->__joint_stiffness_highfreq_if,
147  __parent->__sensor_highfreq_if);
148  }
149 
150  __parent->process_messages();
151  }
152 
153 
154  private:
155  NaoQiDCMThread *__parent;
156 };
157 
158 
159 
160 /** @class NaoQiDCMThread "dcm_thread.h"
161  * Thread to provide DCM to Fawkes.
162  * This thread opens a DCM proxy and updates information about hardware
163  * in the blackboard and provides basic setting functionality to move
164  * specific servos.
165  *
166  * The DCM thread writes to two sets of interfaces, at a high and a lower
167  * frequency. The high frequency data is written if there is a reader at
168  * each DCM callback, which results in a frequency of about 100Hz. This
169  * should only be used if necessary, for example for custom motion pattern
170  * generating plugins. Otherwise, and especially if in a thread hooked
171  * into the main loop, use the lower frequency interfaces. These are
172  * written during the sensor hook.
173  *
174  * @author Tim Niemueller
175  */
176 
177 /** Constructor. */
179  : Thread("NaoQiDCMThread", Thread::OPMODE_WAITFORWAKEUP),
180  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_ACQUIRE)
181 {
182 }
183 
184 
185 /** Destructor. */
187 {
188 }
189 
190 
191 void
193 {
194  // Is the DCM running ?
195  try {
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");
199 
200  if (! is_dcm_available) {
201  throw Exception("DCMThread: NaoQi DCM is not available");
202  }
203  if (! is_almotion_available) {
204  throw Exception("DCMThread: ALMotion is not available");
205  }
206  } catch (AL::ALError& e) {
207  throw Exception("Checking module availability failed: %s",
208  e.toString().c_str());
209  }
210 
211  __dcm = naoqi_broker->getDcmProxy();
212  __almotion = naoqi_broker->getMotionProxy();
213 
214  try {
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;
220  } else {
221  __robot_type = NaoJointPositionInterface::ROBOTYPE_ROBOCUP;
222  }
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());
231  }
232  __usboard_version =
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());
236  }
237 
238  __memfa.reset(new AL::ALMemoryFastAccess());
239 
240  std::string prefix = "Device/SubDeviceList/";
241 
242  // Initialize fast memory access
243  std::vector<std::string> keys;
244  keys.resize(SensorTypeN);
245  __values.resize(SensorTypeN);
246 
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";
261 
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";
274 
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";
289 
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";
302 
303  // Inertial sensors
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";
312 
313  // FSR sensors
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";
328 
329  // Ultrasonic
330  keys[ULTRASONIC_DIRECTION] = prefix + "US/Actuator/Value";
331  keys[ULTRASONIC_DISTANCE] = prefix + "US/Sensor/Value";
332 
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";
337 
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";
342 
343  // Bumpers and Buttons
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";
348 
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";
352 
353  keys[CHEST_BUTTON] = prefix + "ChestBoard/Button/Sensor/Value";
354 
355  // Battery
356  keys[BATTERY_CHARGE] = prefix + "Battery/Charge/Sensor/Value";
357 
358  try {
359  __memfa->ConnectToVariables(naoqi_broker, keys, false);
360  } catch (AL::ALError &e) {
361  throw Exception("Failed to setup fast memory access: %s",
362  e.toString().c_str());
363  }
364 
365 
366  // Setup alias for setting stiffness, reuse fastmem keys vector
367  if (__robot_type == NaoJointPositionInterface::ROBOTYPE_ROBOCUP) {
368  __alljoint_names.arraySetSize(22);
369  } else {
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";
375  }
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";
388 
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";
399 
400  try {
401  AL::ALValue setJointStiffnessAlias;
402  // Alias for all joint stiffness
403  setJointStiffnessAlias.clear();
404  setJointStiffnessAlias.arraySetSize(2);
405  setJointStiffnessAlias[0] = std::string("setJointStiffness");
406  setJointStiffnessAlias[1].arraySetSize(26);
407 
408  // stiffness list
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];
412  }
413 
414  __dcm->createAlias(setJointStiffnessAlias);
415  } catch (AL::ALError &e) {
416  __memfa.reset();
417  throw Exception("Failed to create SetJointStiffness alias: %s",
418  e.toString().c_str());
419  }
420 
421  __joint_pos_if =
422  blackboard->open_for_writing<NaoJointPositionInterface>("Nao Joint Positions");
423  __joint_stiffness_if =
425  ("Nao Joint Stiffness");
426  __sensor_if =
428 
429  __joint_pos_highfreq_if =
431  ("Nao Joint Positions HF");
432  __joint_stiffness_highfreq_if =
434  ("Nao Joint Stiffness HF");
435  __sensor_highfreq_if =
436  blackboard->open_for_writing<NaoSensorInterface>("Nao Sensors HF");
437 
438 
439  // Get all values from ALMemory using fastaccess
440  __dcm_time = __dcm->getTime(0);
441  __memfa->GetValues(__values);
442 
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);
447 
448  __sensor_if->set_ultrasonic_direction(NaoSensorInterface::USD_NONE);
449  __sensor_highfreq_if->set_ultrasonic_direction(NaoSensorInterface::USD_NONE);
450 
451  // Write once the current data on startup
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);
455 
456  __highfreq_thread = new NaoQiDCMThread::HighFreqThread(this);
457  __highfreq_thread->start();
458 
459  __dcm_sigconn =
460  __dcm->getGenericProxy()->getModule()->
461  atPostProcess(boost::bind(&NaoQiDCMThread::dcm_callback, this));
462 
463  /*
464  AL::ALValue cmd;
465  cmd.arraySetSize(3);
466  cmd[0] = std::string("setJointStiffness");
467  cmd[1] = std::string("Merge");
468  cmd[2].arraySetSize(1);
469  cmd[2][0].arraySetSize(2);
470  cmd[2][0][0] = 1.0;
471  cmd[2][0][1] = __dcm_time + 500;
472  __dcm->set(cmd);
473 
474  send_command("HeadPitch/Position/Sensor/Value", 0.4, 1000);
475  */
476 }
477 
478 
479 void
480 NaoQiDCMThread::dcm_callback()
481 {
482  __highfreq_thread->wakeup();
483 }
484 
485 void
487 {
488  __dcm_sigconn.disconnect();
489 
490  __highfreq_thread->cancel();
491  __highfreq_thread->join();
492  delete __highfreq_thread;
493 
494  blackboard->close(__joint_pos_if);
495  blackboard->close(__joint_stiffness_if);
496  blackboard->close(__sensor_if);
497  blackboard->close(__joint_pos_highfreq_if);
498  blackboard->close(__joint_stiffness_highfreq_if);
499  blackboard->close(__sensor_highfreq_if);
500  __joint_pos_if = NULL;
501  __joint_stiffness_if = NULL;
502  __sensor_if = NULL;
503  __joint_pos_highfreq_if = NULL;
504  __joint_stiffness_highfreq_if = NULL;
505  __sensor_highfreq_if = NULL;
506 
507  __memfa.reset();
508  __dcm.reset();
509 }
510 
511 
512 void
513 NaoQiDCMThread::read_values()
514 {
515  __values.lock();
516  __dcm_time = __dcm->getTime(0);
517  __memfa->GetValues(__values);
518  __values.unlock();
519 }
520 
521 
522 void
524 {
525  update_interfaces(__joint_pos_if, __joint_stiffness_if, __sensor_if);
526 }
527 
528 void
529 NaoQiDCMThread::update_interfaces(NaoJointPositionInterface *joint_pos_if,
530  NaoJointStiffnessInterface *joint_stiffness_if,
531  NaoSensorInterface *sensor_if)
532 {
533  // Joint Position
534  // Head
535  joint_pos_if->set_head_yaw(__values[HEAD_YAW]);
536  joint_pos_if->set_head_pitch(__values[HEAD_PITCH]);
537  // Arms
538  joint_pos_if->set_l_shoulder_pitch(__values[L_SHOULDER_PITCH]);
539  joint_pos_if->set_l_shoulder_roll(__values[L_SHOULDER_ROLL]);
540  joint_pos_if->set_l_elbow_yaw(__values[L_ELBOW_YAW]);
541  joint_pos_if->set_l_elbow_roll(__values[L_ELBOW_ROLL]);
542  joint_pos_if->set_l_wrist_yaw(__values[L_WRIST_YAW]);
543  joint_pos_if->set_l_hand(__values[L_HAND]);
544  joint_pos_if->set_r_shoulder_pitch(__values[R_SHOULDER_PITCH]);
545  joint_pos_if->set_r_shoulder_roll(__values[R_SHOULDER_ROLL]);
546  joint_pos_if->set_r_elbow_yaw(__values[R_ELBOW_YAW]);
547  joint_pos_if->set_r_elbow_roll(__values[R_ELBOW_ROLL]);
548  joint_pos_if->set_r_wrist_yaw(__values[R_WRIST_YAW]);
549  joint_pos_if->set_r_hand(__values[R_HAND]);
550  // Hip
551  joint_pos_if->set_l_hip_yaw_pitch(__values[L_HIP_YAW_PITCH]);
552  joint_pos_if->set_l_hip_pitch(__values[L_HIP_PITCH]);
553  joint_pos_if->set_l_hip_roll(__values[L_HIP_ROLL]);
554  joint_pos_if->set_r_hip_yaw_pitch(__values[R_HIP_YAW_PITCH]);
555  joint_pos_if->set_r_hip_pitch(__values[R_HIP_PITCH]);
556  joint_pos_if->set_r_hip_roll(__values[R_HIP_ROLL]);
557  // Knees
558  joint_pos_if->set_l_knee_pitch(__values[L_KNEE_PITCH]);
559  joint_pos_if->set_r_knee_pitch(__values[R_KNEE_PITCH]);
560  // Feet
561  joint_pos_if->set_l_ankle_pitch(__values[L_ANKLE_PITCH]);
562  joint_pos_if->set_l_ankle_roll(__values[L_ANKLE_ROLL]);
563  joint_pos_if->set_r_ankle_pitch(__values[R_ANKLE_PITCH]);
564  joint_pos_if->set_r_ankle_roll(__values[R_ANKLE_ROLL]);
565 
566  joint_pos_if->set_time(__dcm_time);
567 
568  // Joint Stiffness
569  // Head
570  joint_stiffness_if->set_head_yaw(__values[STIFF_HEAD_YAW]);
571  joint_stiffness_if->set_head_pitch(__values[STIFF_HEAD_PITCH]);
572  // Arms
573  joint_stiffness_if->set_l_shoulder_pitch(__values[STIFF_L_SHOULDER_PITCH]);
574  joint_stiffness_if->set_l_shoulder_roll(__values[STIFF_L_SHOULDER_ROLL]);
575  joint_stiffness_if->set_l_elbow_yaw(__values[STIFF_L_ELBOW_YAW]);
576  joint_stiffness_if->set_l_elbow_roll(__values[STIFF_L_ELBOW_ROLL]);
577  joint_stiffness_if->set_l_wrist_yaw(__values[STIFF_L_WRIST_YAW]);
578  joint_stiffness_if->set_l_hand(__values[STIFF_L_HAND]);
579  joint_stiffness_if->set_r_shoulder_pitch(__values[STIFF_R_SHOULDER_PITCH]);
580  joint_stiffness_if->set_r_shoulder_roll(__values[STIFF_R_SHOULDER_ROLL]);
581  joint_stiffness_if->set_r_elbow_yaw(__values[STIFF_R_ELBOW_YAW]);
582  joint_stiffness_if->set_r_elbow_roll(__values[STIFF_R_ELBOW_ROLL]);
583  joint_stiffness_if->set_r_wrist_yaw(__values[STIFF_R_WRIST_YAW]);
584  joint_stiffness_if->set_r_hand(__values[STIFF_R_HAND]);
585  // Hip
586  joint_stiffness_if->set_l_hip_yaw_pitch(__values[STIFF_L_HIP_YAW_PITCH]);
587  joint_stiffness_if->set_l_hip_pitch(__values[STIFF_L_HIP_PITCH]);
588  joint_stiffness_if->set_l_hip_roll(__values[STIFF_L_HIP_ROLL]);
589  // RHipYawPitch stiffness is always 0, copy from RYawPitch
590  joint_stiffness_if->set_r_hip_yaw_pitch(__values[STIFF_L_HIP_YAW_PITCH]);
591  joint_stiffness_if->set_r_hip_pitch(__values[STIFF_R_HIP_PITCH]);
592  joint_stiffness_if->set_r_hip_roll(__values[STIFF_R_HIP_ROLL]);
593  // Knees
594  joint_stiffness_if->set_l_knee_pitch(__values[STIFF_L_KNEE_PITCH]);
595  joint_stiffness_if->set_r_knee_pitch(__values[STIFF_R_KNEE_PITCH]);
596  // Feet
597  joint_stiffness_if->set_l_ankle_pitch(__values[STIFF_L_ANKLE_PITCH]);
598  joint_stiffness_if->set_l_ankle_roll(__values[STIFF_L_ANKLE_ROLL]);
599  joint_stiffness_if->set_r_ankle_pitch(__values[STIFF_R_ANKLE_PITCH]);
600  joint_stiffness_if->set_r_ankle_roll(__values[STIFF_R_ANKLE_ROLL]);
601 
602  float min_stiffness = 1.;
603  for (int i = STIFF_HEAD_YAW; i <= STIFF_R_ANKLE_ROLL; ++i) {
604  // ignore wrist and hand on RoboCup version
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) ) )
608  continue;
609 
610  // ignore RHipYawPitch stiffness, it's always 0
611  if (i == STIFF_R_HIP_YAW_PITCH) continue;
612 
613  if (__values[i] < min_stiffness) min_stiffness = __values[i];
614  }
615  joint_stiffness_if->set_minimum(min_stiffness);
616 
617  // Sensors
618  // FSRs
619  sensor_if->set_l_fsr_fl(__values[L_FSR_FL]);
620  sensor_if->set_l_fsr_fr(__values[L_FSR_FR]);
621  sensor_if->set_l_fsr_rl(__values[L_FSR_RL]);
622  sensor_if->set_l_fsr_rr(__values[L_FSR_RR]);
623  sensor_if->set_r_fsr_fl(__values[R_FSR_FL]);
624  sensor_if->set_r_fsr_fr(__values[R_FSR_FR]);
625  sensor_if->set_r_fsr_rl(__values[R_FSR_RL]);
626  sensor_if->set_r_fsr_rr(__values[R_FSR_RR]);
627 
628  sensor_if->set_l_cop_x(__values[L_COP_X]);
629  sensor_if->set_l_cop_y(__values[L_COP_Y]);
630  sensor_if->set_l_total_weight(__values[L_TOTAL_WEIGHT]);
631 
632  sensor_if->set_r_cop_x(__values[R_COP_X]);
633  sensor_if->set_r_cop_y(__values[R_COP_Y]);
634  sensor_if->set_r_total_weight(__values[R_TOTAL_WEIGHT]);
635 
636  // Buttons and bumpers
637  sensor_if->set_chest_button((__values[CHEST_BUTTON] >= 0.5) ? 1 : 0);
638  sensor_if->set_head_touch_front((__values[HEAD_TOUCH_FRONT] >= 0.5) ? 1 : 0);
639  sensor_if->set_head_touch_middle((__values[HEAD_TOUCH_MIDDLE] >= 0.5) ? 1 : 0);
640  sensor_if->set_head_touch_rear((__values[HEAD_TOUCH_REAR] >= 0.5) ? 1 : 0);
641 
642  sensor_if->set_l_foot_bumper_l((__values[L_FOOT_BUMPER_L] >= 0.5) ? 1 : 0);
643  sensor_if->set_l_foot_bumper_r((__values[L_FOOT_BUMPER_R] >= 0.5) ? 1 : 0);
644  sensor_if->set_r_foot_bumper_l((__values[R_FOOT_BUMPER_L] >= 0.5) ? 1 : 0);
645  sensor_if->set_r_foot_bumper_r((__values[R_FOOT_BUMPER_R] >= 0.5) ? 1 : 0);
646 
647  // Inertial measurement unit
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);
651 
652  sensor_if->set_gyro_x(__values[GYR_X]);
653  sensor_if->set_gyro_y(__values[GYR_Y]);
654  sensor_if->set_gyro_ref(__values[GYR_REF]);
655 
656  sensor_if->set_angle_x(__values[ANGLE_X]);
657  sensor_if->set_angle_y(__values[ANGLE_Y]);
658 
659  // Ultrasonic sound
661  sensor_if->ultrasonic_direction();
662  switch (us_dir) {
663  case NaoSensorInterface::USD_LEFT_LEFT:
664  case NaoSensorInterface::USD_RIGHT_LEFT:
665  {
666  float us_left[4] = {__values[ULTRASONIC_DISTANCE], 0, 0, 0};
667  sensor_if->set_ultrasonic_distance_left(us_left);
668 
669  float us_right[4] = {0, 0, 0, 0};
670  sensor_if->set_ultrasonic_distance_right(us_right);
671  }
672  break;
673  case NaoSensorInterface::USD_LEFT_RIGHT:
674  case NaoSensorInterface::USD_RIGHT_RIGHT:
675  {
676  float us_left[4] = {0, 0, 0, 0};
677  sensor_if->set_ultrasonic_distance_left(us_left);
678 
679  float us_right[4] = {__values[ULTRASONIC_DISTANCE], 0, 0, 0};
680  sensor_if->set_ultrasonic_distance_right(us_right);
681  }
682  break;
683 
684  default:
685  {
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]};
690  sensor_if->set_ultrasonic_distance_left(us_left);
691 
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]};
696  sensor_if->set_ultrasonic_distance_right(us_right);
697  }
698  break;
699  }
700 
701  // Battery
702  sensor_if->set_battery_charge(__values[BATTERY_CHARGE]);
703 
704  // Write to blackboard
705  joint_pos_if->write();
706  joint_stiffness_if->write();
707  sensor_if->write();
708 }
709 
710 
711 void
712 NaoQiDCMThread::process_messages()
713 {
714  // *** Joint position messages
715  while (! __joint_pos_if->msgq_empty()) {
717  __joint_pos_if->msgq_first_safe(msg))
718  {
719  send_commands(msg->servo(), "Position", msg->value(), msg->time());
720  }
721 
723  __joint_pos_if->msgq_first_safe(msg))
724  {
725  }
726 
728  __joint_pos_if->msgq_first_safe(msg))
729  {
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());
733  }
734 
736  __joint_pos_if->msgq_first_safe(msg))
737  {
738  motion::move_joints(
739  __almotion,
740  /* head */ msg->head_yaw(), msg->head_pitch(),
741  /* l shoulder */ msg->l_shoulder_pitch(), msg->l_shoulder_roll(),
742  /* l elbow */ msg->l_elbow_yaw(), msg->l_elbow_roll(),
743  /* l wrist/hand */ msg->l_wrist_yaw(), msg->l_hand(),
744  /* l hip */ msg->l_hip_yaw_pitch(), msg->l_hip_roll(), msg->l_hip_pitch(),
745  /* l knee */ msg->l_knee_pitch(),
746  /* l ankle */ msg->l_ankle_pitch(), msg->l_ankle_roll(),
747  /* r shoulder */ msg->r_shoulder_pitch(), msg->r_shoulder_roll(),
748  /* r elbow */ msg->r_elbow_yaw(), msg->r_elbow_roll(),
749  /* r wrist/hand */ msg->r_wrist_yaw(), msg->r_hand(),
750  /* r hip */ msg->r_hip_yaw_pitch(), msg->r_hip_roll(), msg->r_hip_pitch(),
751  /* r knee */ msg->r_knee_pitch(),
752  /* r ankle */ msg->r_ankle_pitch(), msg->r_ankle_roll(),
753  /* speed */ msg->speed());
754  }
755 
756  __joint_pos_if->msgq_pop();
757  }
758 
759  // *** Joint stiffness messages
760  while (! __joint_stiffness_if->msgq_empty()) {
762  __joint_stiffness_if->msgq_first_safe(msg))
763  {
764  /* DCM version, disfunctional due to ALMotion deficiencies
765  send_commands(msg->servo(), "Hardness", msg->value(),
766  "Merge", (int)roundf(1000. * msg->time_sec()));
767  */
768 
769  std::vector<std::string> servos = parse_servo_bitfield(msg->servo());
770  std::vector<float> values(servos.size(), msg->value());
771 
772  __almotion->post.stiffnessInterpolation(servos, values,
773  msg->time_sec());
774 
775  }
777  __joint_stiffness_if->msgq_first_safe(msg))
778  {
779  /* Cannot be used atm because ALMotion will not update its internal
780  * belief of stiffness values causing any further motion via DCM
781  * or ALMotion to fail
782  // use setJointStiffness alias setup in init()
783  AL::ALValue cmd;
784  cmd.arraySetSize(3);
785  cmd[0] = std::string("setJointStiffness");
786  cmd[1] = std::string("Merge");
787  cmd[2].arraySetSize(1);
788  cmd[2][0].arraySetSize(2);
789  cmd[2][0][0] = msg->value();
790  cmd[2][0][1] = __dcm_time + (int)roundf(1000. * msg->time_sec());
791  try {
792  __dcm->set(cmd);
793  } catch (const AL::ALError &e) {
794  logger->log_warn(name(), "Failed to call setJointStiffness: %s",
795  e.toString().c_str());
796  }
797  */
798 
799  __almotion->post.stiffnessInterpolation("Body", msg->value(),
800  msg->time_sec());
801  }
802 
804  __joint_stiffness_if->msgq_first_safe(msg))
805  {
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();
819 
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();
830 
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();
836  }
837 
838  __almotion->post.stiffnessInterpolation(__alljoint_names, values,
839  msg->time_sec());
840  }
841 
842  __joint_stiffness_if->msgq_pop();
843  }
844 
845  // *** Sensor messages
846  while (! __sensor_if->msgq_empty()) {
848  __sensor_if->msgq_first_safe(msg))
849  {
850  int value = ultrasonic_value(msg->ultrasonic_direction());
851  send_command("US/Actuator/Value", value, "ClearAll", 0);
852 
853  __sensor_if->set_ultrasonic_direction(msg->ultrasonic_direction());
854  __sensor_highfreq_if->set_ultrasonic_direction(msg->ultrasonic_direction());
855 
857  __sensor_if->msgq_first_safe(msg))
858  {
859  int value = ultrasonic_value(msg->ultrasonic_direction());
860  value += 64;
861  send_command("US/Actuator/Value", value, "ClearAll", 0);
862 
863  __sensor_if->set_ultrasonic_direction(msg->ultrasonic_direction());
864  __sensor_highfreq_if->set_ultrasonic_direction(msg->ultrasonic_direction());
865 
867  __sensor_if->msgq_first_safe(msg))
868  {
869  send_command("US/Actuator/Value", 0, "ClearAll", 0);
870 
871  __sensor_if->set_ultrasonic_direction(NaoSensorInterface::USD_NONE);
872  __sensor_highfreq_if->set_ultrasonic_direction(NaoSensorInterface::USD_NONE);
873  }
874 
875  __sensor_if->msgq_pop();
876  }
877 
878 }
879 
880 
881 void
882 NaoQiDCMThread::send_commands(unsigned int servos, std::string what,
883  float value, int time_offset)
884 {
885  /*
886  __almotion->wbEnable(false);
887  __almotion->wbEnableEffectorControl("LArm", false);
888  __almotion->wbEnableEffectorControl("RArm", false);
889  __almotion->setWalkArmsEnable(false, false);
890  __almotion->killAll();
891  */
892 
893  std::vector<std::string> servonames = parse_servo_bitfield(servos);
894 
895  std::vector<std::string>::iterator s;
896  for (s = servonames.begin(); s != servonames.end(); ++s) {
897  float v = value;
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);
904  }
905  send_command(*s + "/" + what + "/Actuator/Value",
906  v, "Merge", time_offset);
907  }
908 }
909 
910 void
911 NaoQiDCMThread::send_command(std::string name, float value,
912  std::string kind, int time_offset)
913 {
914  AL::ALValue cmd;
915 
916  /*
917  printf("Command for %s to %f in %i ms (%i vs. %i)\n",
918  name.c_str(), value, time_offset, __dcm->getTime(time_offset),
919  __dcm_time + time_offset);
920  */
921 
922  cmd.arraySetSize(3);
923  cmd[0] = name;
924  cmd[1] = kind;
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;
929 
930  __dcm->set(cmd);
931 }
932 
933 
934 std::vector<std::string>
935 NaoQiDCMThread::parse_servo_bitfield(unsigned int servos)
936 {
937  std::vector<std::string> servonames;
938 
939  if ( servos & NaoJointPositionInterface::SERVO_head_yaw )
940  servonames.push_back("HeadYaw");
941 
942  if ( servos & NaoJointPositionInterface::SERVO_head_pitch )
943  servonames.push_back("HeadPitch");
944 
945  if ( servos & NaoJointPositionInterface::SERVO_l_shoulder_pitch )
946  servonames.push_back("LShoulderPitch");
947 
948  if ( servos & NaoJointPositionInterface::SERVO_l_shoulder_roll )
949  servonames.push_back("LShoulderRoll");
950 
951  if ( servos & NaoJointPositionInterface::SERVO_l_elbow_yaw )
952  servonames.push_back("LElbowYaw");
953 
954  if ( servos & NaoJointPositionInterface::SERVO_l_elbow_roll )
955  servonames.push_back("LElbowRoll");
956 
957  if ( servos & NaoJointPositionInterface::SERVO_r_shoulder_pitch )
958  servonames.push_back("RShoulderPitch");
959 
960  if ( servos & NaoJointPositionInterface::SERVO_r_shoulder_roll )
961  servonames.push_back("RShoulderRoll");
962 
963  if ( servos & NaoJointPositionInterface::SERVO_r_elbow_yaw )
964  servonames.push_back("RElbowYaw");
965 
966  if ( servos & NaoJointPositionInterface::SERVO_r_elbow_roll )
967  servonames.push_back("RElbowRoll");
968 
969  if ( servos & NaoJointPositionInterface::SERVO_l_hip_yaw_pitch )
970  servonames.push_back("LHipYawPitch");
971 
972  if ( servos & NaoJointPositionInterface::SERVO_l_hip_pitch )
973  servonames.push_back("LHipPitch");
974 
975  if ( servos & NaoJointPositionInterface::SERVO_l_hip_roll )
976  servonames.push_back("LHipRoll");
977 
978  if ( servos & NaoJointPositionInterface::SERVO_r_hip_yaw_pitch )
979  servonames.push_back("RHipYawPitch");
980 
981  if ( servos & NaoJointPositionInterface::SERVO_r_hip_pitch )
982  servonames.push_back("RHipPitch");
983 
984  if ( servos & NaoJointPositionInterface::SERVO_r_hip_roll )
985  servonames.push_back("RHipRoll");
986 
987  if ( servos & NaoJointPositionInterface::SERVO_l_knee_pitch )
988  servonames.push_back("LKneePitch");
989 
990  if ( servos & NaoJointPositionInterface::SERVO_r_knee_pitch )
991  servonames.push_back("RKneePitch");
992 
993  if ( servos & NaoJointPositionInterface::SERVO_l_ankle_pitch )
994  servonames.push_back("LAnklePitch");
995 
996  if ( servos & NaoJointPositionInterface::SERVO_l_ankle_roll )
997  servonames.push_back("LAnkleRoll");
998 
999  if ( servos & NaoJointPositionInterface::SERVO_r_ankle_pitch )
1000  servonames.push_back("RAnklePitch");
1001 
1002  if ( servos & NaoJointPositionInterface::SERVO_r_ankle_roll )
1003  servonames.push_back("RAnkleRoll");
1004 
1005  return servonames;
1006 }
1007 
1008 
1009 int
1010 NaoQiDCMThread::ultrasonic_value(fawkes::NaoSensorInterface::UltrasonicDirection direction)
1011 {
1012  int value = 0;
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;
1019  default:
1020  logger->log_warn(name(), "Illegal ultrasonic direction, "
1021  "using left-right");
1022  }
1023  return value;
1024 }
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.
Definition: dcm_thread.cpp:486
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.
Definition: dcm_thread.cpp:192
void set_r_shoulder_pitch(const float new_r_shoulder_pitch)
Set r_shoulder_pitch value.
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1048
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.
Definition: dcm_thread.cpp:131
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.
Definition: naoqi.h:45
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.
Definition: thread.h:42
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.
Definition: lock_vector.h:98
void set_head_pitch(const float new_head_pitch)
Set head_pitch value.
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:500
NaoJointStiffnessInterface Fawkes BlackBoard Interface.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
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.
Definition: interface.cpp:1193
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.
Definition: lock_vector.h:118
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.
Definition: thread.cpp:1000
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.
Definition: dcm_thread.cpp:138
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.
Definition: dcm_thread.h:51
Base class for exceptions in Fawkes.
Definition: exception.h:36
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.
Definition: thread.h:95
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.
Definition: dcm_thread.cpp:186
void cancel()
Cancel a thread.
Definition: thread.cpp:651
virtual void loop()
Code to execute in the thread.
Definition: dcm_thread.cpp:523
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.
Definition: interface.h:302
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.
Definition: thread.cpp:610
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.
Definition: dcm_thread.cpp:125
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.
Definition: thread.cpp:511
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
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.
Definition: dcm_thread.cpp:178
virtual void close(Interface *interface)=0
Close interface.
void set_gyro_ref(const float new_gyro_ref)
Set gyro_ref value.