humotion / examples / yarp_icub / src / icub_jointinterface.cpp @ 21444915
History | View | Annotate | Download (22.928 KB)
1 | 8c6c1163 | Simon Schulz | #include "icub_jointinterface.h" |
---|---|---|---|
2 | #include <yarp/os/Property.h> |
||
3 | using namespace yarp::dev; |
||
4 | using namespace yarp::sig; |
||
5 | using namespace yarp::os; |
||
6 | using namespace std; |
||
7 | /*running:
|
||
8 | /media/local_data/sschulz/iros15/icub-nightly/share/iCub/contexts/simConfig:> iCub_SIM
|
||
9 | /media/local_data/sschulz/iros15/icub-nightly/share/iCub/contexts/simFaceExpressions:> ../../../../bin/simFaceExpressions
|
||
10 | yarp connect /face/eyelids /icubSim/face/eyelids
|
||
11 | yarp connect /face/image/out /icubSim/texture/face
|
||
12 | |||
13 | TEST: yarp write /writer /icubSim/face/raw/in
|
||
14 | |||
15 | http://wiki.icub.org/wiki/Motor_control
|
||
16 | */
|
||
17 | |||
18 | //WARNING: DO NOT CHANGE THIS; VELOCITYMODE IS NOT YET IMPLEMENTED
|
||
19 | 7adf90be | Simon Schulz | #define POSITION_CONTROL 0 |
20 | 8c6c1163 | Simon Schulz | |
21 | |||
22 | //! constructor
|
||
23 | iCubJointInterface::iCubJointInterface(string _scope) : humotion::server::JointInterface(){
|
||
24 | scope = _scope; |
||
25 | |||
26 | |||
27 | //add mapping from ids to enums:
|
||
28 | //this might look strange at the first sight but we need to have a generic
|
||
29 | //way to acces joints from libhumotion. therefore the lib uses its enum with ID_* enum ids
|
||
30 | //to access the joints. now we need to define a mapping to map those to our motor ids.
|
||
31 | //this is what we use the enum bimap for (convertion fro/to motorid is handled
|
||
32 | //by \sa convert_enum_to_motorid() and \sa convert_motorid_to_enum() lateron
|
||
33 | |||
34 | //MOUTH
|
||
35 | enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_LEFT_UPPER, ID_LIP_LEFT_UPPER)); |
||
36 | enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_LEFT_LOWER, ID_LIP_LEFT_LOWER)); |
||
37 | enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_CENTER_UPPER, ID_LIP_CENTER_UPPER)); |
||
38 | enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_CENTER_LOWER, ID_LIP_CENTER_LOWER)); |
||
39 | enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_RIGHT_UPPER, ID_LIP_RIGHT_UPPER)); |
||
40 | enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_RIGHT_LOWER, ID_LIP_RIGHT_LOWER)); |
||
41 | |||
42 | //NECK
|
||
43 | enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_NECK_PAN, ID_NECK_PAN)); |
||
44 | enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_NECK_TILT, ID_NECK_TILT)); |
||
45 | enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_NECK_ROLL, ID_NECK_ROLL)); |
||
46 | |||
47 | //EYES
|
||
48 | 7adf90be | Simon Schulz | enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_PAN, ID_EYES_LEFT_LR)); |
49 | enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_VERGENCE, ID_EYES_RIGHT_LR)); |
||
50 | 8c6c1163 | Simon Schulz | enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_BOTH_UD, ID_EYES_BOTH_UD)); |
51 | |||
52 | //EYELIDS
|
||
53 | enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_LEFT_LID_LOWER, ID_EYES_LEFT_LID_LOWER)); |
||
54 | enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_LEFT_LID_UPPER, ID_EYES_LEFT_LID_UPPER)); |
||
55 | enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_LEFT_BROW, ID_EYES_LEFT_BROW)); |
||
56 | |||
57 | enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_RIGHT_LID_LOWER, ID_EYES_RIGHT_LID_LOWER)); |
||
58 | enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_RIGHT_LID_UPPER,ID_EYES_RIGHT_LID_UPPER)); |
||
59 | enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_RIGHT_BROW, ID_EYES_RIGHT_BROW)); |
||
60 | |||
61 | Property options; |
||
62 | options.put("device", "remote_controlboard"); |
||
63 | options.put("local", "/local/head"); |
||
64 | options.put("remote", scope+"/head"); |
||
65 | dd.open(options); |
||
66 | |||
67 | //fetch views:
|
||
68 | dd.view(iencs); |
||
69 | dd.view(ipos); |
||
70 | dd.view(ivel); |
||
71 | dd.view(ilimits); |
||
72 | 7adf90be | Simon Schulz | dd.view(pid); |
73 | dd.view(amp); |
||
74 | 8c6c1163 | Simon Schulz | |
75 | 7adf90be | Simon Schulz | |
76 | if ( (!iencs) || (!ipos) || (!ilimits) || (!ivel) || (!amp) || (!pid)){
|
||
77 | 8c6c1163 | Simon Schulz | printf("> ERROR: failed to open icub views\n");
|
78 | exit(EXIT_FAILURE); |
||
79 | } |
||
80 | |||
81 | int joints;
|
||
82 | |||
83 | //tell humotion about min/max joint values:
|
||
84 | init_joints(); |
||
85 | |||
86 | iencs->getAxes(&joints); |
||
87 | positions.resize(joints); |
||
88 | velocities.resize(joints); |
||
89 | commands.resize(joints); |
||
90 | |||
91 | //set position mode:
|
||
92 | if (POSITION_CONTROL){
|
||
93 | commands=200000.0; |
||
94 | ipos->setRefAccelerations(commands.data()); |
||
95 | ipos->setPositionMode(); |
||
96 | }else{
|
||
97 | ivel->setVelocityMode(); |
||
98 | 7adf90be | Simon Schulz | commands=100.0; |
99 | 8c6c1163 | Simon Schulz | ivel->setRefAccelerations(commands.data()); |
100 | } |
||
101 | |||
102 | //attach to facial expressions:
|
||
103 | string emotion_scope = scope + "/face/raw/in"; |
||
104 | printf("> opening connection to %s\n", emotion_scope.c_str());
|
||
105 | |||
106 | for(int i=0; i<4; i++){ |
||
107 | //strange, if we use one output port only the first command is executed?! flushing issues?
|
||
108 | string emotion_port_out = "/emotionwriter" + to_string(i); |
||
109 | if (!emotion_port[i].open(emotion_port_out.c_str())){
|
||
110 | printf("> ERROR: failed to open to %s\n",emotion_port_out.c_str());
|
||
111 | exit(EXIT_FAILURE); |
||
112 | } |
||
113 | if (!Network::connect(emotion_port_out.c_str(), emotion_scope.c_str())){
|
||
114 | printf("> ERROR: failed to connect emotion ports\n");
|
||
115 | exit(EXIT_FAILURE); |
||
116 | } |
||
117 | } |
||
118 | } |
||
119 | |||
120 | //! destructor
|
||
121 | iCubJointInterface::~iCubJointInterface(){ |
||
122 | } |
||
123 | |||
124 | |||
125 | |||
126 | //! conversion table for humotion motor ids to our ids:
|
||
127 | //! \param enum from JointInterface::JOINT_ID_ENUM
|
||
128 | //! \return int value of motor id
|
||
129 | int iCubJointInterface::convert_enum_to_motorid(int e){ |
||
130 | enum_id_bimap_t::right_const_iterator it = enum_id_bimap.right.find(e); |
||
131 | if(it == enum_id_bimap.right.end()) {
|
||
132 | //key does not exists, we are not interested in that dataset, return -1
|
||
133 | return -1; |
||
134 | } |
||
135 | return it->second;
|
||
136 | } |
||
137 | |||
138 | |||
139 | //! conversion table for our ids to humotion motor ids:
|
||
140 | //! \param int value of motor id
|
||
141 | //! \return enum from JointInterface::JOINT_ID_ENUM
|
||
142 | int iCubJointInterface::convert_motorid_to_enum(int id){ |
||
143 | enum_id_bimap_t::left_const_iterator it = enum_id_bimap.left.find(id); |
||
144 | if(it == enum_id_bimap.left.end()) {
|
||
145 | //key does not exists, we are not interested in that dataset, return -1
|
||
146 | return -1; |
||
147 | } |
||
148 | return it->second;
|
||
149 | } |
||
150 | |||
151 | //! special command to set eyelid angle
|
||
152 | //! \param angle in degrees
|
||
153 | void iCubJointInterface::set_eyelid_angle(double angle){ |
||
154 | if (emotion_port[0].getOutputCount()>0){ |
||
155 | //try to set the value based on the upper one
|
||
156 | //some guesses from the sim: S30 = 0° / S40 = 10°
|
||
157 | int opening = (25.0 + 0.8*angle); |
||
158 | opening = min(48, max(24, opening)); |
||
159 | |||
160 | if (opening == lid_opening_previous){
|
||
161 | //no update necessary
|
||
162 | return;
|
||
163 | } |
||
164 | |||
165 | lid_angle = angle; |
||
166 | lid_opening_previous = opening; |
||
167 | |||
168 | char buf[20]; |
||
169 | sprintf(buf, "S%2d", opening);
|
||
170 | |||
171 | //printf("> SETTING EYELID '%s' (%f -> %d)\n",buf,angle,opening);
|
||
172 | Bottle &cmd = emotion_port[0].prepare();
|
||
173 | cmd.clear(); |
||
174 | cmd.addString(buf); |
||
175 | 7adf90be | Simon Schulz | emotion_port[0].writeStrict();
|
176 | 8c6c1163 | Simon Schulz | }else{
|
177 | printf("> ERROR: no icub emotion output\n");
|
||
178 | exit(EXIT_FAILURE); |
||
179 | } |
||
180 | } |
||
181 | |||
182 | //! special command to set the eyebrow angle
|
||
183 | //! \param id {0=left, 1=right)
|
||
184 | //! \param angle in degrees
|
||
185 | void iCubJointInterface::set_eyebrow_angle(int id){ |
||
186 | int port_id;
|
||
187 | if (id == ICUB_ID_EYES_LEFT_BROW){
|
||
188 | port_id = 1;
|
||
189 | }else{
|
||
190 | port_id = 2;
|
||
191 | } |
||
192 | |||
193 | if (emotion_port[port_id].getOutputCount()>0){ |
||
194 | double angle = target_angle[id];
|
||
195 | int icub_val = 0; |
||
196 | |||
197 | //swap rotation direction:
|
||
198 | if (id==ICUB_ID_EYES_LEFT_BROW) angle = -angle;
|
||
199 | |||
200 | //convert to icub representation
|
||
201 | if (angle < -20){ |
||
202 | icub_val = 1;
|
||
203 | }else if (angle<10){ |
||
204 | icub_val = 2;
|
||
205 | }else if (angle<20){ |
||
206 | icub_val = 4;
|
||
207 | }else{
|
||
208 | icub_val = 8;
|
||
209 | } |
||
210 | |||
211 | //make sure to update only on new values:
|
||
212 | if (icub_val == target_angle_previous[id]){
|
||
213 | //no updata necessary
|
||
214 | return;
|
||
215 | } |
||
216 | |||
217 | //store actual value:
|
||
218 | target_angle_previous[id] = icub_val; |
||
219 | |||
220 | |||
221 | string cmd_s;
|
||
222 | if (id==ICUB_ID_EYES_LEFT_BROW){
|
||
223 | cmd_s = "L0" + to_string(icub_val);
|
||
224 | }else{
|
||
225 | cmd_s = "R0" + to_string(icub_val);
|
||
226 | } |
||
227 | |||
228 | printf("> SETTING EYEBROW %d (%f -> %s)\n",id,angle,cmd_s.c_str());
|
||
229 | |||
230 | Bottle &cmd = emotion_port[port_id].prepare(); |
||
231 | cmd.clear(); |
||
232 | cmd.addString(cmd_s); |
||
233 | 7adf90be | Simon Schulz | emotion_port[port_id].writeStrict(); |
234 | 8c6c1163 | Simon Schulz | }else{
|
235 | printf("> ERROR: no icub emotion output\n");
|
||
236 | exit(EXIT_FAILURE); |
||
237 | } |
||
238 | } |
||
239 | |||
240 | void iCubJointInterface::run(){
|
||
241 | iCubDataReceiver *data_receiver = new iCubDataReceiver(10/*1000.0 / MAIN_LOOP_FREQUENCY*/, iencs, this); |
||
242 | data_receiver->start(); |
||
243 | } |
||
244 | |||
245 | //! set the target position of a joint
|
||
246 | //! \param enum id of joint
|
||
247 | //! \param float value
|
||
248 | void iCubJointInterface::publish_target_position(int e){ |
||
249 | //first: convert humotion enum to our enum:
|
||
250 | int id = convert_enum_to_motorid(e);
|
||
251 | if (id == -1){ |
||
252 | return; //we are not interested in that data, so we just return here |
||
253 | } |
||
254 | |||
255 | if (id == ICUB_ID_NECK_PAN){
|
||
256 | //PAN seems to be swapped
|
||
257 | store_joint(ICUB_ID_NECK_PAN, -joint_target[e]); |
||
258 | 7adf90be | Simon Schulz | }else if ((id == ICUB_ID_EYES_PAN) || ( id == ICUB_ID_EYES_VERGENCE)){ |
259 | 8c6c1163 | Simon Schulz | //icub handles eyes differently, we have to set pan angle + vergence
|
260 | float pan = (joint_target[ID_EYES_LEFT_LR] + joint_target[ID_EYES_RIGHT_LR]) / 2; |
||
261 | float vergence = (joint_target[ID_EYES_LEFT_LR] - joint_target[ID_EYES_RIGHT_LR]);
|
||
262 | //printf("EYEDBG %3.2f %3.2f --_> pan %3.2f verg=%3.2f\n",joint_target[ID_EYES_LEFT_LR], joint_target[ID_EYES_RIGHT_LR],pan,vergence);
|
||
263 | |||
264 | store_joint(ICUB_ID_EYES_PAN, pan); |
||
265 | store_joint(ICUB_ID_EYES_VERGENCE, vergence); |
||
266 | }else{
|
||
267 | store_joint(id, joint_target[e]); |
||
268 | } |
||
269 | } |
||
270 | |||
271 | |||
272 | //! set the target position of a joint
|
||
273 | //! \param id of joint
|
||
274 | //! \param float value of position
|
||
275 | void iCubJointInterface::store_joint(int id, float value){ |
||
276 | //printf("> set joint %d = %f\n",id,value);
|
||
277 | target_angle[id] = value; |
||
278 | //ipos->positionMove(id, value);
|
||
279 | } |
||
280 | |||
281 | //! execute a move in position mode
|
||
282 | //! \param id of joint
|
||
283 | //! \param angle
|
||
284 | void iCubJointInterface::set_target_in_positionmode(int id, double value){ |
||
285 | if (id>ICUB_ID_EYES_VERGENCE){
|
||
286 | printf("> ERROR: set_target_positionmode(id=%d, %3.2f) not supported for this id\n",id,value);
|
||
287 | return;
|
||
288 | } |
||
289 | |||
290 | 7adf90be | Simon Schulz | // execute motion as position control cmd
|
291 | 8c6c1163 | Simon Schulz | ipos->positionMove(id, value); |
292 | 7adf90be | Simon Schulz | |
293 | 8c6c1163 | Simon Schulz | } |
294 | |||
295 | //! execute a move in velocity mode
|
||
296 | //! \param id of joint
|
||
297 | //! \param angle
|
||
298 | void iCubJointInterface::set_target_in_velocitymode(int id, double value){ |
||
299 | 7adf90be | Simon Schulz | // set speed cacluated as in velocity + set position -> replicates smoothmotion from flobi?!
|
300 | 8c6c1163 | Simon Schulz | //first: calculate necessary speed to reach the given target within the next clock tick:
|
301 | double distance = value - target_angle_previous[id];
|
||
302 | 7adf90be | Simon Schulz | |
303 | 8c6c1163 | Simon Schulz | //make the motion smooth: we want to reach 85% of the target in the next iteration:
|
304 | distance = 0.85 * distance; |
||
305 | 7adf90be | Simon Schulz | |
306 | //distance = -5.0 / 50.0;
|
||
307 | |||
308 | 8c6c1163 | Simon Schulz | //calculate speed
|
309 | 7adf90be | Simon Schulz | //double speed = distance * ((double)MAIN_LOOP_FREQUENCY);
|
310 | |||
311 | |||
312 | |||
313 | int e = convert_motorid_to_enum(id);
|
||
314 | double speed = joint_target_speed[e];
|
||
315 | |||
316 | double max = 150.0; |
||
317 | if (speed > max) speed = max;
|
||
318 | if (speed < -max) speed = -max;
|
||
319 | |||
320 | //speed = -speed;
|
||
321 | |||
322 | |||
323 | 8c6c1163 | Simon Schulz | //execute:
|
324 | 7adf90be | Simon Schulz | //ivel->velocityMove(id, speed);
|
325 | if ((id == ICUB_ID_NECK_PAN) || (id == ICUB_ID_EYES_BOTH_UD) || (id == ICUB_ID_NECK_TILT) || (id == ICUB_ID_EYES_BOTH_UD) || (id == ICUB_ID_NECK_TILT) ){
|
||
326 | if (id == ICUB_ID_NECK_PAN) speed = -speed;
|
||
327 | ivel->velocityMove(id, speed); |
||
328 | printf("> VEL now=%3.2f target=%3.2f --> dist=%3.2f speed=%3.2f\n",target_angle_previous[id],value,distance,speed);
|
||
329 | } |
||
330 | 8c6c1163 | Simon Schulz | |
331 | target_angle_previous[id] = get_ts_position(convert_motorid_to_enum(id)).get_newest_value(); |
||
332 | } |
||
333 | |||
334 | //! actually execute the scheduled motion commands
|
||
335 | void iCubJointInterface::execute_motion(){
|
||
336 | |||
337 | // set up neck and eye motion commands:
|
||
338 | if (POSITION_CONTROL){
|
||
339 | //position control
|
||
340 | for(int i=ICUB_ID_NECK_TILT; i<=ICUB_ID_EYES_VERGENCE; i++){ |
||
341 | set_target_in_positionmode(i, target_angle[i]); |
||
342 | } |
||
343 | }else{
|
||
344 | //velocity control
|
||
345 | for(int i=ICUB_ID_NECK_TILT; i<=ICUB_ID_EYES_VERGENCE; i++){ |
||
346 | set_target_in_velocitymode(i, target_angle[i]); |
||
347 | } |
||
348 | } |
||
349 | //printf("> TARGET PAN = %3.2f\n",target_angle[ICUB_ID_NECK_PAN]);
|
||
350 | |||
351 | |||
352 | //eyelids: unfortuantely the icub has only 1dof for eyelids, so we use only one dof here:
|
||
353 | set_eyelid_angle(target_angle[ICUB_ID_EYES_RIGHT_LID_UPPER]); |
||
354 | |||
355 | //eyebrows are set using a special command as well:
|
||
356 | set_eyebrow_angle(ICUB_ID_EYES_LEFT_BROW); |
||
357 | set_eyebrow_angle(ICUB_ID_EYES_RIGHT_BROW); |
||
358 | |||
359 | //mouth
|
||
360 | set_mouth(); |
||
361 | |||
362 | |||
363 | } |
||
364 | |||
365 | void iCubJointInterface::set_mouth(){
|
||
366 | //convert from 6DOF mouth displacement to icub leds:
|
||
367 | int led_value = 0; |
||
368 | |||
369 | //fetch center opening:
|
||
370 | double center_opening = target_angle[ICUB_ID_LIP_CENTER_LOWER] - target_angle[ICUB_ID_LIP_CENTER_UPPER];
|
||
371 | bool mouth_open = (center_opening>15.0)?true:false; |
||
372 | |||
373 | //side of mouth high or low?
|
||
374 | double center_avg = (target_angle[ICUB_ID_LIP_CENTER_LOWER] + target_angle[ICUB_ID_LIP_CENTER_UPPER])/2.0; |
||
375 | double left_avg = (target_angle[ICUB_ID_LIP_LEFT_LOWER] + target_angle[ICUB_ID_LIP_LEFT_UPPER])/2.0; |
||
376 | double right_avg = (target_angle[ICUB_ID_LIP_RIGHT_LOWER] + target_angle[ICUB_ID_LIP_RIGHT_UPPER])/2.0; |
||
377 | |||
378 | //happy, neutral or sad?
|
||
379 | double diff_l = center_avg - left_avg;
|
||
380 | double diff_r = center_avg - right_avg;
|
||
381 | double diff = (diff_l+diff_r)/2.0; |
||
382 | |||
383 | if (diff > 2.0){ |
||
384 | if (mouth_open){
|
||
385 | led_value = 0x14;
|
||
386 | }else{
|
||
387 | if (diff > 2.6){ |
||
388 | led_value = 0x0A;
|
||
389 | }else{
|
||
390 | led_value = 0x0B;
|
||
391 | } |
||
392 | } |
||
393 | }else if (diff < -3.0){ |
||
394 | if (mouth_open){
|
||
395 | led_value = 0x06;
|
||
396 | }else{
|
||
397 | led_value = 0x18;
|
||
398 | } |
||
399 | }else if (diff < -2.0){ |
||
400 | if (mouth_open){
|
||
401 | led_value = 0x04; //0x25; |
||
402 | }else{
|
||
403 | led_value = 0x08;
|
||
404 | } |
||
405 | }else{
|
||
406 | if (mouth_open){
|
||
407 | led_value = 0x16;
|
||
408 | }else{
|
||
409 | led_value = 0x08;
|
||
410 | } |
||
411 | } |
||
412 | |||
413 | |||
414 | if (led_value == previous_mouth_state){
|
||
415 | //no update necessary
|
||
416 | return;
|
||
417 | } |
||
418 | |||
419 | previous_mouth_state = led_value; |
||
420 | |||
421 | //convert to string:
|
||
422 | char buf[10]; |
||
423 | sprintf(buf, "M%02X",led_value);
|
||
424 | |||
425 | /*printf("> sending mouth '%s'\n",buf);
|
||
426 | printf("> mouth angles: %3.2f %3.2f %3.2f\n",target_angle[ICUB_ID_LIP_LEFT_UPPER],target_angle[ICUB_ID_LIP_CENTER_UPPER],target_angle[ICUB_ID_LIP_RIGHT_UPPER]);
|
||
427 | printf(" mouth %3.2f %3.2f %3.2f\n",target_angle[ICUB_ID_LIP_LEFT_LOWER],target_angle[ICUB_ID_LIP_CENTER_LOWER],target_angle[ICUB_ID_LIP_RIGHT_LOWER]);
|
||
428 | printf(" mouth open=%3.2f diff=%3.2f\n", center_opening, diff);*/
|
||
429 | |||
430 | //add mouth:
|
||
431 | Bottle &cmd = emotion_port[3].prepare();
|
||
432 | cmd.clear(); |
||
433 | cmd.addString(buf); |
||
434 | 7adf90be | Simon Schulz | emotion_port[3].writeStrict();
|
435 | 8c6c1163 | Simon Schulz | |
436 | |||
437 | //store joint values which we do not handle on icub here:
|
||
438 | double timestamp = get_timestamp_ms();
|
||
439 | JointInterface::store_incoming_position(ID_LIP_LEFT_UPPER, target_angle[ICUB_ID_LIP_LEFT_UPPER], timestamp); |
||
440 | JointInterface::store_incoming_position(ID_LIP_LEFT_LOWER, target_angle[ICUB_ID_LIP_LEFT_LOWER], timestamp); |
||
441 | JointInterface::store_incoming_position(ID_LIP_CENTER_UPPER, target_angle[ICUB_ID_LIP_CENTER_UPPER], timestamp); |
||
442 | JointInterface::store_incoming_position(ID_LIP_CENTER_LOWER, target_angle[ICUB_ID_LIP_CENTER_LOWER], timestamp); |
||
443 | JointInterface::store_incoming_position(ID_LIP_RIGHT_UPPER, target_angle[ICUB_ID_LIP_RIGHT_UPPER], timestamp); |
||
444 | JointInterface::store_incoming_position(ID_LIP_RIGHT_LOWER, target_angle[ICUB_ID_LIP_RIGHT_LOWER], timestamp); |
||
445 | |||
446 | } |
||
447 | |||
448 | double iCubJointInterface::get_timestamp_ms(){
|
||
449 | struct timespec spec;
|
||
450 | clock_gettime(CLOCK_REALTIME, &spec); |
||
451 | return spec.tv_sec*1000 + spec.tv_nsec / 1.0e6; |
||
452 | } |
||
453 | |||
454 | //! set the current position of a joint
|
||
455 | //! \param id of joint
|
||
456 | //! \param float value of position
|
||
457 | //! \param double timestamp
|
||
458 | void iCubJointInterface::fetch_position(int id, double value, double timestamp){ |
||
459 | //store joint based on id:
|
||
460 | switch(id){
|
||
461 | default:
|
||
462 | printf("> ERROR: unhandled joint id %d\n",id);
|
||
463 | return;
|
||
464 | |||
465 | case(100): |
||
466 | JointInterface::store_incoming_position(ID_EYES_RIGHT_LID_UPPER, lid_angle, timestamp); |
||
467 | break;
|
||
468 | |||
469 | 7adf90be | Simon Schulz | case(ICUB_ID_NECK_PAN):
|
470 | 8c6c1163 | Simon Schulz | //PAN is inverted!
|
471 | JointInterface::store_incoming_position(ID_NECK_PAN, -value, timestamp); |
||
472 | break;
|
||
473 | |||
474 | 7adf90be | Simon Schulz | case(ICUB_ID_NECK_TILT):
|
475 | 8c6c1163 | Simon Schulz | JointInterface::store_incoming_position(ID_NECK_TILT, value, timestamp); |
476 | break;
|
||
477 | |||
478 | 7adf90be | Simon Schulz | case(ICUB_ID_NECK_ROLL):
|
479 | 8c6c1163 | Simon Schulz | JointInterface::store_incoming_position(ID_NECK_ROLL, value, timestamp); |
480 | break;
|
||
481 | |||
482 | 7adf90be | Simon Schulz | case(ICUB_ID_EYES_BOTH_UD):
|
483 | 8c6c1163 | Simon Schulz | JointInterface::store_incoming_position(ID_EYES_BOTH_UD, value, timestamp); |
484 | break;
|
||
485 | |||
486 | //icub handles eyes differently, we have to set pan angle + vergence
|
||
487 | 7adf90be | Simon Schulz | case(ICUB_ID_EYES_PAN): {//pan |
488 | 8c6c1163 | Simon Schulz | last_pos_eye_pan = value; |
489 | float left = last_pos_eye_pan + last_pos_eye_vergence/2.0; |
||
490 | float right = last_pos_eye_pan - last_pos_eye_vergence/2.0; |
||
491 | |||
492 | //printf("> eye: pan=%3.2f vergence=%3.2f --> L=%3.2f R=%3.2f\n", last_pos_eye_pan, last_pos_eye_vergence, left, right);
|
||
493 | JointInterface::store_incoming_position(ID_EYES_LEFT_LR, left, timestamp); |
||
494 | JointInterface::store_incoming_position(ID_EYES_RIGHT_LR, right, timestamp); |
||
495 | break;
|
||
496 | } |
||
497 | |||
498 | 7adf90be | Simon Schulz | case(ICUB_ID_EYES_VERGENCE): { //vergence |
499 | 8c6c1163 | Simon Schulz | last_pos_eye_vergence = value; |
500 | float left = last_pos_eye_pan + last_pos_eye_vergence/2.0; |
||
501 | float right = last_pos_eye_pan - last_pos_eye_vergence/2.0; |
||
502 | |||
503 | //printf("> eye: pan=%3.2f vergence=%3.2f --> L=%3.2f R=%3.2f\n", last_pos_eye_pan, last_pos_eye_vergence, left, right);
|
||
504 | JointInterface::store_incoming_position(ID_EYES_LEFT_LR, left, timestamp); |
||
505 | JointInterface::store_incoming_position(ID_EYES_RIGHT_LR, right, timestamp); |
||
506 | break;
|
||
507 | } |
||
508 | } |
||
509 | |||
510 | |||
511 | } |
||
512 | |||
513 | //! set the current speed of a joint
|
||
514 | //! \param enum id of joint
|
||
515 | //! \param float value of speed
|
||
516 | //! \param double timestamp
|
||
517 | void iCubJointInterface::fetch_speed(int id, double value, double timestamp){ |
||
518 | |||
519 | switch(id){
|
||
520 | default:
|
||
521 | printf("> ERROR: unhandled joint id %d\n",id);
|
||
522 | return;
|
||
523 | |||
524 | 7adf90be | Simon Schulz | case(ICUB_ID_NECK_PAN):
|
525 | //PAN IS INVERTED
|
||
526 | JointInterface::store_incoming_speed(ID_NECK_PAN, -value, timestamp); |
||
527 | 8c6c1163 | Simon Schulz | break;
|
528 | |||
529 | 7adf90be | Simon Schulz | case(ICUB_ID_NECK_TILT):
|
530 | 8c6c1163 | Simon Schulz | JointInterface::store_incoming_speed(ID_NECK_TILT, value, timestamp); |
531 | break;
|
||
532 | |||
533 | 7adf90be | Simon Schulz | case(ICUB_ID_NECK_ROLL):
|
534 | 8c6c1163 | Simon Schulz | JointInterface::store_incoming_speed(ID_NECK_ROLL, value, timestamp); |
535 | break;
|
||
536 | |||
537 | 7adf90be | Simon Schulz | case(ICUB_ID_EYES_BOTH_UD):
|
538 | 8c6c1163 | Simon Schulz | JointInterface::store_incoming_speed(ID_EYES_BOTH_UD, value, timestamp); |
539 | break;
|
||
540 | |||
541 | //icub handles eyes differently, we have to set pan angle + vergence
|
||
542 | 7adf90be | Simon Schulz | case(ICUB_ID_EYES_PAN): {//pan |
543 | 8c6c1163 | Simon Schulz | last_vel_eye_pan = value; |
544 | float left = last_vel_eye_pan + last_vel_eye_vergence/2.0; |
||
545 | float right = last_vel_eye_pan - last_vel_eye_vergence/2.0; |
||
546 | |||
547 | //printf("> eye: velocity pan=%3.2f vergence=%3.2f --> L=%3.2f R=%3.2f\n", last_vel_eye_pan, last_vel_eye_vergence, left, right);
|
||
548 | JointInterface::store_incoming_speed(ID_EYES_LEFT_LR, left, timestamp); |
||
549 | JointInterface::store_incoming_speed(ID_EYES_RIGHT_LR, right, timestamp); |
||
550 | break;
|
||
551 | } |
||
552 | |||
553 | 7adf90be | Simon Schulz | case(ICUB_ID_EYES_VERGENCE): { //vergence |
554 | 8c6c1163 | Simon Schulz | last_vel_eye_pan = value; |
555 | float left = last_vel_eye_pan + last_vel_eye_vergence/2.0; |
||
556 | float right = last_vel_eye_pan - last_vel_eye_vergence/2.0; |
||
557 | |||
558 | //printf("> eye: velocity pan=%3.2f vergence=%3.2f --> L=%3.2f R=%3.2f\n", last_vel_eye_pan, last_vel_eye_vergence, left, right);
|
||
559 | JointInterface::store_incoming_speed(ID_EYES_LEFT_LR, left, timestamp); |
||
560 | JointInterface::store_incoming_speed(ID_EYES_RIGHT_LR, right, timestamp); |
||
561 | break;
|
||
562 | } |
||
563 | } |
||
564 | 7adf90be | Simon Schulz | } |
565 | 8c6c1163 | Simon Schulz | |
566 | 7adf90be | Simon Schulz | void iCubJointInterface::set_joint_enable_state(int e, bool enable) { |
567 | int icub_jointid = -1; |
||
568 | 8c6c1163 | Simon Schulz | |
569 | 7adf90be | Simon Schulz | switch(e){
|
570 | default:
|
||
571 | break;
|
||
572 | 8c6c1163 | Simon Schulz | |
573 | 7adf90be | Simon Schulz | case(ID_NECK_PAN):
|
574 | icub_jointid = ICUB_ID_NECK_PAN; |
||
575 | break;
|
||
576 | 8c6c1163 | Simon Schulz | |
577 | 7adf90be | Simon Schulz | case(ID_NECK_TILT):
|
578 | icub_jointid = ICUB_ID_NECK_TILT; |
||
579 | break;
|
||
580 | 8c6c1163 | Simon Schulz | |
581 | 7adf90be | Simon Schulz | case(ID_NECK_ROLL):
|
582 | icub_jointid = ICUB_ID_NECK_ROLL; |
||
583 | break;
|
||
584 | |||
585 | case(ID_EYES_BOTH_UD):
|
||
586 | icub_jointid = ICUB_ID_EYES_BOTH_UD; |
||
587 | break;
|
||
588 | |||
589 | // icub handles eyes as pan angle + vergence...
|
||
590 | // -> hack: left eye enables pan and right eye enables vergence
|
||
591 | case(ID_EYES_LEFT_LR):
|
||
592 | icub_jointid = ICUB_ID_EYES_PAN; |
||
593 | break;
|
||
594 | |||
595 | case(ID_EYES_RIGHT_LR):
|
||
596 | icub_jointid = ICUB_ID_EYES_VERGENCE; |
||
597 | break;
|
||
598 | 8c6c1163 | Simon Schulz | } |
599 | |||
600 | 7adf90be | Simon Schulz | if (icub_jointid != -1) { |
601 | if (enable) {
|
||
602 | amp->enableAmp(icub_jointid); |
||
603 | pid->enablePid(icub_jointid); |
||
604 | } else {
|
||
605 | pid->disablePid(icub_jointid); |
||
606 | amp->disableAmp(icub_jointid); |
||
607 | } |
||
608 | } |
||
609 | 8c6c1163 | Simon Schulz | } |
610 | |||
611 | //! prepare and enable a joint
|
||
612 | //! NOTE: this should also prefill the min/max positions for this joint
|
||
613 | //! \param the enum id of a joint
|
||
614 | void iCubJointInterface::enable_joint(int e){ |
||
615 | 7adf90be | Simon Schulz | set_joint_enable_state(e, true);
|
616 | } |
||
617 | 8c6c1163 | Simon Schulz | |
618 | 7adf90be | Simon Schulz | //! shutdown and disable a joint
|
619 | //! \param the enum id of a joint
|
||
620 | void iCubJointInterface::disable_joint(int e){ |
||
621 | set_joint_enable_state(e, false);
|
||
622 | 8c6c1163 | Simon Schulz | } |
623 | |||
624 | void iCubJointInterface::store_min_max(IControlLimits *ilimits, int id, int e){ |
||
625 | double min, max;
|
||
626 | ilimits->getLimits(id, &min, &max); |
||
627 | joint_min[e] = min; |
||
628 | joint_max[e] = max; |
||
629 | } |
||
630 | |||
631 | //! initialise a joint (set up controller mode etc)
|
||
632 | //! \param joint enum
|
||
633 | void iCubJointInterface::init_joints(){
|
||
634 | 7adf90be | Simon Schulz | store_min_max(ilimits, ICUB_ID_NECK_TILT, ID_NECK_TILT); |
635 | store_min_max(ilimits, ICUB_ID_NECK_ROLL, ID_NECK_ROLL); |
||
636 | store_min_max(ilimits, ICUB_ID_NECK_PAN, ID_NECK_PAN); |
||
637 | store_min_max(ilimits, ICUB_ID_EYES_BOTH_UD, ID_EYES_BOTH_UD); |
||
638 | 8c6c1163 | Simon Schulz | |
639 | //icub handles eyes differently, we have to set pan angle + vergence
|
||
640 | double pan_min, pan_max, vergence_min, vergence_max;
|
||
641 | 7adf90be | Simon Schulz | ilimits->getLimits(ICUB_ID_EYES_PAN, &pan_min, &pan_max); |
642 | ilimits->getLimits(ICUB_ID_EYES_VERGENCE, &vergence_min, &vergence_max); |
||
643 | 8c6c1163 | Simon Schulz | |
644 | //this is not 100% correct, should be fixed:
|
||
645 | joint_min[ID_EYES_LEFT_LR] = pan_min; // - vergence_max/2;
|
||
646 | joint_max[ID_EYES_LEFT_LR] = pan_max; // - vergence_max/2;
|
||
647 | joint_min[ID_EYES_RIGHT_LR] = joint_min[ID_EYES_LEFT_LR]; |
||
648 | joint_max[ID_EYES_RIGHT_LR] = joint_max[ID_EYES_LEFT_LR]; |
||
649 | |||
650 | //eyelids:
|
||
651 | joint_min[ID_EYES_RIGHT_LID_UPPER] = -50; //24-30; |
||
652 | joint_max[ID_EYES_RIGHT_LID_UPPER] = 50; //48-30; |
||
653 | lid_angle = joint_max[ID_EYES_RIGHT_LID_UPPER]; |
||
654 | |||
655 | //eyebrows:
|
||
656 | joint_min[ID_EYES_LEFT_BROW] = -50;
|
||
657 | joint_max[ID_EYES_LEFT_BROW] = 50;
|
||
658 | joint_min[ID_EYES_RIGHT_BROW] = joint_min[ID_EYES_LEFT_BROW]; |
||
659 | joint_max[ID_EYES_RIGHT_BROW] = joint_max[ID_EYES_LEFT_BROW]; |
||
660 | |||
661 | //mouth:
|
||
662 | joint_min[ID_LIP_CENTER_UPPER] = 5;
|
||
663 | joint_max[ID_LIP_CENTER_UPPER] = 50;
|
||
664 | joint_min[ID_LIP_CENTER_LOWER] = 5;
|
||
665 | joint_max[ID_LIP_CENTER_LOWER] = 50;
|
||
666 | joint_min[ID_LIP_LEFT_UPPER] = 5;
|
||
667 | joint_max[ID_LIP_LEFT_UPPER] = 50;
|
||
668 | joint_min[ID_LIP_LEFT_LOWER] = 5;
|
||
669 | joint_max[ID_LIP_LEFT_LOWER] = 50;
|
||
670 | joint_min[ID_LIP_RIGHT_UPPER] = 5;
|
||
671 | joint_max[ID_LIP_RIGHT_UPPER] = 50;
|
||
672 | joint_min[ID_LIP_RIGHT_LOWER] = 5;
|
||
673 | joint_max[ID_LIP_RIGHT_LOWER] = 50;
|
||
674 | |||
675 | |||
676 | } |