iCub-main
compensator.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2009 RobotCub Consortium, European Commission FP6 Project IST-004370
3  * Authors: Andrea Del Prete, Alexander Schmitz
4  * email: andrea.delprete@iit.it, alexander.schmitz@iit.it
5  * website: www.robotcub.org
6  * Permission is granted to copy, distribute, and/or modify this program
7  * under the terms of the GNU General Public License, version 2 or any
8  * later version published by the Free Software Foundation.
9  *
10  * A copy of the license can be found at
11  * http://www.robotcub.org/icub/license/gpl.txt
12  *
13  * This program is distributed in the hope that it will be useful, but
14  * WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
16  * Public License for more details
17  */
18 #include <yarp/os/Time.h>
19 #include <yarp/math/Math.h>
20 #include <yarp/math/Rand.h> // TEMP
21 #include "math.h"
22 #include <algorithm>
24 
25 
26 using namespace std;
27 using namespace yarp::os;
28 using namespace yarp::sig;
29 using namespace yarp::math;
30 using namespace iCub::skinManager;
31 using namespace iCub::skinDynLib;
32 
33 const double Compensator::BIN_TOUCH = 100.0;
34 const double Compensator::BIN_NO_TOUCH = 0.0;
35 
36 Compensator::Compensator(string _name, string _robotName, string outputPortName, string inputPortName, BufferedPort<Bottle>* _infoPort,
37  double _compensationGain, double _contactCompensationGain, int addThreshold, float _minBaseline, bool _zeroUpRawData,
38  bool _binarization, bool _smoothFilter, float _smoothFactor, unsigned int _linkNum)
39  :
40  compensationGain(_compensationGain), contactCompensationGain(_contactCompensationGain),
41  addThreshold(addThreshold), infoPort(_infoPort),
42  minBaseline(_minBaseline), binarization(_binarization), smoothFilter(_smoothFilter),
43  smoothFactor(_smoothFactor), robotName(_robotName), name(_name), linkNum(_linkNum)
44 {
45  this->zeroUpRawData = _zeroUpRawData;
46  _isWorking = init(_name, _robotName, outputPortName, inputPortName);
47 }
48 
50  if(tactileSensorDevice){
51  tactileSensorDevice->close();
52  delete tactileSensorDevice;
53  }
54 
55  compensatedTactileDataPort.interrupt();
56  compensatedTactileDataPort.close();
57 }
58 
59 bool Compensator::init(string name, string robotName, string outputPortName, string inputPortName){
61  bodyPart = BODY_PART_UNKNOWN;
62 
63  if (!compensatedTactileDataPort.open(outputPortName.c_str())) {
64  stringstream msg; msg<< "Unable to open output port "<< outputPortName;
65  sendInfoMsg(msg.str());
66  return false; // unable to open
67  }
68 
69  Property options;
70  stringstream localPortName;
71  localPortName<< "/"<< name<< "/input";
72  options.put("robot", robotName.c_str());
73  options.put("local", localPortName.str().c_str());
74  options.put("remote", inputPortName.c_str());
75  options.put("device", "analogsensorclient");
76 
77  // create a new device driver
78  tactileSensorDevice = new PolyDriver(options);
79  if (!tactileSensorDevice->isValid()){
80  sendInfoMsg("Device not available.\n");
81  //printf("%s", Drivers::factory().toString().c_str());
82  return false;
83  }
84  // open the sensor interface
85  bool ok = tactileSensorDevice->view(tactileSensor);
86  if (!ok) {
87  sendInfoMsg("Problems acquiring interfaces");
88  return false;
89  }
90 
91  // (temp) open port to read skin data with timestamp (because device driver doesn't provide timestamps)
92  localPortName<< "_temp";
93  if(!inputPort.open(localPortName.str().c_str()))
94  {
95  stringstream msg; msg<< "Unable to open input data port "<< localPortName.str();
96  sendInfoMsg(msg.str());
97  return false;
98  }
99 
100  if( !Network::connect(inputPortName.c_str(), localPortName.str().c_str()))
101  {
102  stringstream msg;
103  msg<< "Problems trying to connect ports %s and %s."<< inputPortName.c_str()<< localPortName.str().c_str();
104  sendInfoMsg(msg.str());
105  return false;
106  }
107 
108  int getChannelsCounter = 0;
109  skinDim = tactileSensor->getChannels();
110  while(skinDim<=0){
111  // getChannels() returns 0 if it hasn't performed the first reading yet
112  // so wait for 1/50 sec, then try again
113  if(++getChannelsCounter==50){
114  // after 50 failed trials give up and use the default value
115  skinDim = 192;
116  sendInfoMsg("Error reading the number of channels of the port. Using 192 as default value.");
117  break;
118  }
119  Time::delay(0.02);
120  skinDim = tactileSensor->getChannels();
121  }
122  readErrorCounter = 0;
123  rawData.resize(skinDim);
124  baselines.resize(skinDim);
125  touchThresholds.resize(skinDim);
126  touchDetected.resize(skinDim);
127  subTouchDetected.resize(skinDim);
128  touchDetectedFilt.resize(skinDim);
129  compensatedData.resize(skinDim);
130  compensatedDataOld.resize(skinDim);
131  compensatedDataFilt.resize(skinDim);
132  taxelPos.resize(skinDim, zeros(3));
133  taxelOri.resize(skinDim, zeros(3));
134  taxelPoseConfidence.resize(skinDim,0.0);
135  maxNeighDist = MAX_NEIGHBOR_DISTANCE;
136  // by default every taxel is neighbor with all the other taxels
137  list<int> defaultNeighbors(skinDim);
138  int i=0;
139  for(list<int>::iterator it=defaultNeighbors.begin();it!=defaultNeighbors.end();it++, i++)
140  *it = i;
141  neighborsXtaxel.resize(skinDim, defaultNeighbors);
142 
143  // test read to check if the skin is broken (all taxel output is 0)
144  if(robotName!="icubSim" && readInputData(compensatedData)){
145  bool skinBroken = true;
146  for(unsigned int i=0; i<skinDim; i++){
147  if(compensatedData[i]!=0.0){
148  skinBroken = false;
149  break;
150  }
151  }
152  if(skinBroken)
153  sendInfoMsg("The output of all the taxels is 0. Probably there is a hardware problem.");
154  return !skinBroken;
155  }
156 
157  return true;
158 }
159 
161  // take the semaphore so that the touchThreshold can't be read during the calibration phase
162  lock_guard<mutex> lck(touchThresholdSem);
163 
164  // send a command to the microcontroller for calibrating the skin sensors
165  if(robotName!="icubSim"){ // this feature isn't implemented in the simulator and causes a runtime error
166  tactileSensor->calibrateSensor();
167  }
168 
169  // initialize
170  calibrationRead = 0;
171  saturatedTaxels.resize(0);
172  start_sum.assign(skinDim, 0);
173  skin_empty.assign(skinDim, vector<int>(MAX_SKIN+1, 0));
174 }
175 
177  Vector skin_values(skinDim);
178  if(!readInputData(skin_values))
179  return;
180  calibrationRead++;
181 
182  for (unsigned int j=0; j<skinDim; j++) {
183  if (zeroUpRawData==false)
184  skin_values[j] = MAX_SKIN - skin_values[j];
185 
186  if(skin_values[j]<0 || skin_values[j]>MAX_SKIN)
187  sendInfoMsg("Error while reading the tactile data! Data out of range: "+toString(skin_values[j]));
188  else{
189  skin_empty[j][int(skin_values[j])]++;
190  start_sum[j] += int(skin_values[j]);
191  }
192  }
193 }
194 
196 
197  //vector<float> standard_dev(skinDim, 0);
198  //get percentile
199  for (unsigned int i=0; i<skinDim; i++) {
200  //avg start value
201  baselines[i] = start_sum[i]/ (calibrationRead!=0.0 ? calibrationRead : 1.0);
202 
203  //cumulative values
204  for (int j=1; j<=MAX_SKIN; j++) {
205  //standard_dev[i] += fabs(j-baselines[i]) * skin_empty[i][j];
206  skin_empty[i][j] += skin_empty[i][j-1] ;
207  }
208  //standard_dev[i] /= (CAL_TIME*PERIOD);
209 
210  //when do we cross the threshold? Compute 95 percentile
211  if( skin_empty[i][MAX_SKIN] < 0.95*calibrationRead )
212  {
213  sendInfoMsg("Error during calibration. Most readings were out of range.");
214  _isWorking = false;
215  }
216  else
217  {
218  for (int j=0; j<=MAX_SKIN; j++)
219  {
220  if (skin_empty[i][j] > (calibrationRead*0.95))
221  {
222  // the threshold can not be less than MIN_TOUCH_THR
223  touchThresholds[i] = (double)j - baselines[i];
224  j = MAX_SKIN; // break
225  }
226  }
227  }
228 
229 
230  }
231  // store the initial baseline to compute the drift compensated later
232  initialBaselines = baselines;
233 
234  // set the "old output value" for the smoothing filter to the baseline value to get a smooth start
235  compensatedDataOld = baselines;
236 
237  // test to check if the skin is broken (all taxel baselines are 255 OR thresholds are 0)
238  bool baseline255 = true, thresholdZero = true;
239  for(unsigned int i=0; i<skinDim; i++){
240  if(baselines[i]!=255){
241  baseline255 = false;
242  }
243  if(robotName=="icubSim" || touchThresholds[i]>1e-5){
244  thresholdZero = false;
245  }
246  }
247  if(baseline255 || thresholdZero){
248  _isWorking = false;
249 // if(baseline255)
250 // sendInfoMsg("The baselines of all the taxels are 255. Probably there is a hardware problem.");
251 // if(thresholdZero)
252 // sendInfoMsg("The noise of all taxels is 0. Probably there is a hardware problem.");
253  }
254  for (unsigned int i=0; i<skinDim; i++)
255  touchThresholds[i] = max<double>(MIN_TOUCH_THR, touchThresholds[i]);
256 
257  // print to console
258  /*if(_isWorking){
259  printf("\n[%s (%s)] Baselines:\n", name.c_str(), getSkinPartName().c_str());
260  for (unsigned int i=0; i<skinDim; i++) {
261  if(!(i%12)) fprintf(stderr, "\n");
262  fprintf(stderr,"%5.1f ", baselines[i]);
263  }
264  printf("\n[%s (%s)] Thresholds (95 percentile):\n", name.c_str(), getSkinPartName().c_str());
265  for (unsigned int i=0; i<skinDim; i++) {
266  if(!(i%12)) fprintf(stderr, "\n");
267  touchThresholds[i] = max<double>(MIN_TOUCH_THR, touchThresholds[i]);
268  fprintf(stderr,"%3.1f ", touchThresholds[i]);
269  }
270  printf("\n");
271  }*/
272  sendInfoMsg("Calibration finished");
273 }
274 
275 bool Compensator::readInputData(Vector& skin_values){
276  Vector *tmp=0;
277  if((tmp=inputPort.read(false))==0){
278  readErrorCounter++;
279  if(readErrorCounter>MAX_READ_ERROR){
280  _isWorking = false;
281  sendInfoMsg("Too many errors in a row. Stopping the compensator.");
282  }
283  return false;
284  }
285  //try to read envelope of input data port
286  inputPort.getEnvelope(timestamp);
287 
288  skin_values = *tmp; // copy data
289 
290  if(skin_values.size() != skinDim){
291  readErrorCounter++;
292  sendInfoMsg("Unexpected size of the input array (raw tactile data): "+toString(skin_values.size()));
293  if(readErrorCounter>MAX_READ_ERROR){
294  _isWorking = false;
295  sendInfoMsg("Too many errors in a row. Stopping the compensator.");
296  }
297  return false;
298  }
299 
300  readErrorCounter = 0;
301  return true;
302 /*
303  int err;
304  if((err=tactileSensor->read(skin_values))!=IAnalogSensor::AS_OK){
305  readErrorCounter++;
306 
307  stringstream msg;
308  if(err == IAnalogSensor::AS_TIMEOUT)
309  msg<< "Timeout error reading tactile sensor.";
310  else if(err == IAnalogSensor::AS_OVF)
311  msg<< "Ovf error reading tactile sensor.";
312  else if(err == IAnalogSensor::AS_ERROR)
313  msg<< "Generic error reading tactile sensor.";
314  sendInfoMsg(msg.str());
315 
316  if(readErrorCounter>MAX_READ_ERROR){
317  _isWorking = false;
318  sendInfoMsg("Too many errors in a row. Stopping the compensator.");
319  }
320  return false;
321  }
322 
323  if(skin_values.size() != skinDim){
324  readErrorCounter++;
325  sendInfoMsg("Unexpected size of the input array (raw tactile data): "+toString(skin_values.size()));
326  if(readErrorCounter>MAX_READ_ERROR){
327  _isWorking = false;
328  sendInfoMsg("Too many errors in a row. Stopping the compensator.");
329  }
330  return false;
331  }
332 
333  readErrorCounter = 0;
334  return true;*/
335 }
336 
338  if(!readInputData(rawData))
339  return false;
340 
341  Vector& compensatedData2Send = compensatedTactileDataPort.prepare();
342  compensatedData2Send.resize(skinDim); // local variable with data to send
343  compensatedData.resize(skinDim); // global variable with data to store
344 
345  double d;
346  for(unsigned int i=0; i<skinDim; i++){
347  // baseline compensation
348  d = (double)( zeroUpRawData ? rawData(i)-baselines[i] : MAX_SKIN-rawData(i)-baselines[i] );
349  d = min<double>( MAX_SKIN, d);
350  compensatedData[i] = d; // save the data before applying filtering
351 
352  // detect touch (before applying filtering, so the compensation algorithm is not affected by the filters)
353  touchDetected[i] = (d > touchThresholds[i] + addThreshold);
354 
355  // detect subtouch
356  subTouchDetected[i] = (d < -touchThresholds[i] - addThreshold);
357 
358  // smooth filter
359  if(smoothFilter){
360  lock_guard<mutex> lck(smoothFactorSem);
361  d = (1-smoothFactor)*d + smoothFactor*compensatedDataOld(i);
362  compensatedDataOld(i) = d; // update old value
363  }
364  compensatedDataFilt[i] = d;
365 
366  // binarization filter
367  // here we don't use the touchDetected array because, if the smooth filter is on,
368  // we want to use the filtered values
369  touchDetectedFilt[i] = (d > touchThresholds[i] + addThreshold);
370  if(binarization)
371  d = ( touchDetectedFilt[i] ? BIN_TOUCH : BIN_NO_TOUCH );
372 
373  compensatedData2Send[i] = max<double>(0.0, d); // trim only data to send because you need negative values for update baseline
374  }
375 
376  compensatedTactileDataPort.write();
377  return true;
378 }
379 
381  double mean_change = 0, change, gain;
382  unsigned int non_touching_taxels = 0;
383  double d;
384 
385  for(unsigned int j=0; j<skinDim; j++) {
386  // *** Algorithm 1
387  /*if(!touchDetected[j]){
388  if(d > 0.5) {
389  baselines[j] += compensationGain;
390  mean_change += compensationGain; //for changing the taxels where we detected touch
391  }else if(d < -0.5) {
392  baselines[j] -= compensationGain;
393  mean_change -= compensationGain; //for changing the taxels where we detected touch
394  }
395  }*/
396 
397  // *** Algorithm 2
398  /*if(!(touchDetected[j] || subTouchDetected[j])){
399  non_touching_taxels++; //for changing the taxels where we detected touch
400  d = compensatedData(j);
401  if(fabs(d)>0.5){
402  change = (compensationGain/50)*d/touchThresholds[j];
403  baselines[j] += change;
404  mean_change += change;
405  }
406  }*/
407 
408  d = compensatedData(j);
409  if(touchDetected[j]){
410  gain = contactCompensationGain*0.02;
411  }else{
412  gain = compensationGain*0.02;
413  non_touching_taxels++;
414  }
415  change = gain*d/touchThresholds[j];
416  baselines[j] += change;
417  mean_change += change;
418 
419  // if(j<5)
420  // printf("%d touch detected %s, change %.6f, gain: %.3f\n", j, touchDetected[j]?"yes":"no", change, touchDetected[j]?contactCompensationGain:compensationGain);
421 
422  if(baselines[j]<0){
423  char* temp = new char[300];
424  sprintf(temp, "ERROR-Negative baseline. Port %s; tax %d; baseline %.2f; gain: %.4f; d: %.2f; raw: %.2f; change: %f; touchThr: %.2f",
425  SkinPart_s[skinPart].c_str(), j, baselines[j], gain, d, rawData[j], change, touchThresholds[j]);
426  sendInfoMsg(temp);
427  }
428  }
429 
430  //for compensating the taxels where we detected touch
431  /*if (non_touching_taxels>0 && non_touching_taxels<skinDim && mean_change!=0){
432  mean_change /= non_touching_taxels;
433  for(unsigned int j=0; j<skinDim; j++) {
434  if (touchDetected[j]) {
435  baselines[j] += mean_change;
436  if(baselines[j]<0){
437  char* temp = new char[300];
438  sprintf(temp, "ERROR-Negative baseline. Taxel %d; baseline %.2f; meanchange: %f", j, baselines[j], mean_change);
439  sendInfoMsg(temp);
440  }
441  }
442  }
443  }*/
444 }
445 
446 bool Compensator::doesBaselineExceed(unsigned int &taxelIndex, double &baseline, double &initialBaseline){
447  vector<unsigned int>::iterator it;
448  for(unsigned int i=0; i<skinDim; i++){
449  if(baselines[i]<minBaseline || baselines[i]>MAX_SKIN-minBaseline){
450  it = find(saturatedTaxels.begin(), saturatedTaxels.end(), i);
451  if(it==saturatedTaxels.end()){ // if the taxel hasn't been already signalled
452  //fprintf(stderr, "Baseline %d exceeds: %f\n", i, baselines[i]);
453  saturatedTaxels.push_back(i);
454  baseline = baselines[i];
455  initialBaseline = initialBaselines[i];
456  taxelIndex = i;
457  return true;
458  }
459  }
460  }
461  return false;
462 }
463 
465  vector<int> contactXtaxel(skinDim, -1); // contact for each taxel (-1 means no contact)
466  deque<deque<int> > taxelsXcontact; // taxels for each contact
467  int contactId = 0; // id of the next contact to create
468  int neighCont; // id of the contact of the current neighbor
469 
470  poseSem.lock();
471  {
472  for(unsigned int i=0; i<skinDim; i++){
473  if(touchDetectedFilt[i] ){ // && contactXtaxel[i]<0 (second condition should always be true)
474  list<int> *neighbors = &(neighborsXtaxel[i]);
475  //printf("Taxel %d active. Going to check its %d neighbors\n", i, neighbors->size());
476  for(list<int>::iterator it=neighbors->begin(); it!=neighbors->end(); it++){
477  neighCont = contactXtaxel[(*it)];
478  if(neighCont >= 0){ // ** if neighbor belongs to a contact
479  if(contactXtaxel[i]<0){ // ** add taxel to pre-existing contact
480  contactXtaxel[i] = neighCont;
481  taxelsXcontact[neighCont].push_back(i);
482  //printf("Add taxel to pre existing contact %d (neighbor %d)\n", neighCont, (*neighbors)[n]);
483  }else if(contactXtaxel[i]!=neighCont){ // ** merge 2 contacts
484  //mergeContacts(contactXtaxel[i], neighCont);
485  int newId = min(contactXtaxel[i], neighCont);
486  int oldId = max(contactXtaxel[i], neighCont);
487  deque<int> tax2move = taxelsXcontact[oldId];
488  for(deque<int>::iterator it=tax2move.begin(); it!=tax2move.end(); it++){
489  contactXtaxel[(*it)] = newId; // assign new contact id
490  taxelsXcontact[newId].push_back((*it)); // add taxel ids to contact
491  }
492  taxelsXcontact[oldId].clear(); // clear the list of taxels belonging to the merged contact
493  //printf("Merge two contacts: %d and %d\n", oldId, newId);
494  }
495  }
496 
497  }
498  if(contactXtaxel[i]<0){ // ** if no neighbor belongs to a contact -> create new contact
499  contactXtaxel[i] = contactId;
500  taxelsXcontact.resize(contactId+1);
501  taxelsXcontact[contactId].push_back(i);
502  contactId++;
503  //printf("New contact created: %d\n", contactId-1);
504  }
505  }
506  }
507  }
508  poseSem.unlock();
509 
510  skinContactList contactList;
511  Vector CoP(3), geoCenter(3), normal(3);
512  double pressure, pressureCoP, pressureNormal, out;
513  int activeTaxels, activeTaxelsGeo;
514  vector<unsigned int> taxelList;
515  for( deque<deque<int> >::iterator it=taxelsXcontact.begin(); it!=taxelsXcontact.end(); it++){
516  activeTaxels = it->size();
517  if(activeTaxels==0) continue;
518 
519  taxelList.resize(activeTaxels);
520  CoP.zero();
521  geoCenter.zero();
522  normal.zero();
523  pressure = pressureCoP = pressureNormal = 0.0;
524  activeTaxelsGeo = 0;
525  int i=0;
526  for( deque<int>::iterator tax=it->begin(); tax!=it->end(); tax++, i++){
527  out = max(compensatedDataFilt[(*tax)], 0.0);
528  if(norm(taxelPos[(*tax)])!=0.0){ // if the taxel position estimate exists
529  CoP += taxelPos[(*tax)] * out;
530  pressureCoP += out;
531  activeTaxelsGeo++;
532  geoCenter += taxelPos[(*tax)];
533  }
534  if(norm(taxelOri[(*tax)])!=0.0){ // if the taxel orientation estimate exists
535  normal += taxelOri[(*tax)] * out;
536  pressureNormal += out;
537  }
538  pressure += out;
539  taxelList[i] = *tax;
540  }
541  // if this is not the only contact and no taxel in this contact has a position => discard it
542  if(taxelsXcontact.size()>1 && activeTaxelsGeo==0)
543  continue;
544  if(pressureCoP!=0.0) CoP /= pressureCoP;
545  if(pressureNormal!=0.0) normal /= pressureNormal;
546  if(activeTaxelsGeo!=0) geoCenter /= activeTaxelsGeo;
547  pressure /= activeTaxels;
548  skinContact c(bodyPart, skinPart, linkNum, CoP, geoCenter, taxelList, pressure, normal);
549  // set an estimate of the force that is with normal direction and intensity equal to the pressure
550  c.setForce(-0.05*activeTaxels*pressure*normal);
551  contactList.push_back(c);
552  }
553  //printf("ContactList: %s\n", contactList.toString().c_str());
554 
555  return contactList;
556 }
557 
559  if(smoothFilter != value){
560  smoothFilter = value;
561  if(value){
562  // set the old output value of the smooth filter to the last read, to get a smooth start
563  compensatedDataOld = compensatedData;
564  }
565  }
566 }
568  if(value<0.0 || value>1.0)
569  return false;
570  if(value==1.0)
571  value = 0.99f; // otherwise with 1 the values don't update
572  lock_guard<mutex> lck(smoothFactorSem);
573  smoothFactor = value;
574  return true;
575 }
576 
578  this->skinPart = _skinPart;
579  this->bodyPart = skinDynLib::getBodyPart(skinPart);
580  this->linkNum = skinDynLib::getLinkNum(skinPart);
581 }
582 
583 bool Compensator::setAddThreshold(unsigned int thr){
584  if(thr>=MAX_SKIN)
585  return false;
586  addThreshold = thr;
587  return true;
588 }
589 
591  if(gain<=0.0)
592  return false;
593  compensationGain = gain;
594  return true;
595 }
596 
598  if(gain<0.0)
599  return false;
600  contactCompensationGain = gain;
601  return true;
602 }
603 
605  if(_isWorking)
606  return skinDim;
607  return 0;
608 }
609 
610 Vector Compensator::getCompensation(){ return baselines-initialBaselines; }
611 
613  lock_guard<mutex> lck(touchThresholdSem);
614  return touchThresholds;
615 }
616 
618  lock_guard<mutex> lck(smoothFactorSem);
619  return smoothFactor;
620 }
621 Vector Compensator::getTaxelPosition(unsigned int taxelId){
622  if(taxelId>=skinDim)
623  return zeros(0);
624  lock_guard<mutex> lck(poseSem);
625  Vector res = taxelPos[taxelId];
626  res.push_back(taxelPoseConfidence[taxelId]);
627  return res;
628 }
630  lock_guard<mutex> lck(poseSem);
631  vector<Vector> res = taxelPos;
632  for(unsigned int i=0; i<res.size(); i++)
633  res[i].push_back(taxelPoseConfidence[i]);
634  return res;
635 }
636 Vector Compensator::getTaxelOrientation(unsigned int taxelId){
637  if(taxelId>=skinDim)
638  return zeros(0);
639  lock_guard<mutex> lck(poseSem);
640  Vector res = taxelOri[taxelId];
641  res.push_back(taxelPoseConfidence[taxelId]);
642  return res;
643 }
645  lock_guard<mutex> lck(poseSem);
646  vector<Vector> res = taxelOri;
647  for(unsigned int i=0; i<res.size(); i++)
648  res[i].push_back(taxelPoseConfidence[i]);
649  return res;
650 }
651 Vector Compensator::getTaxelPose(unsigned int taxelId){
652  if(taxelId>=skinDim)
653  return zeros(0);
654  lock_guard<mutex> lck(poseSem);
655  Vector res = cat(taxelPos[taxelId], taxelOri[taxelId]);
656  res.push_back(taxelPoseConfidence[taxelId]);
657  return res;
658 }
659 vector<Vector> Compensator::getTaxelPoses(){
660  vector<Vector> res(skinDim);
661  lock_guard<mutex> lck(poseSem);
662  for(unsigned int i=0; i<skinDim; i++){
663  res[i] = cat(taxelPos[i], taxelOri[i]);
664  res[i].push_back(taxelPoseConfidence[i]);
665  }
666  return res;
667 }
668 
669 double Compensator::getPoseConfidence(unsigned int taxelId){
670  if(taxelId>=skinDim)
671  return -1.0;
672  lock_guard<mutex> lck(poseSem);
673  return taxelPoseConfidence[taxelId];
674 }
675 
677  lock_guard<mutex> lck(poseSem);
678  return taxelPoseConfidence;
679 }
680 
682  if(d<0.0)
683  return false;
684  maxNeighDist = d;
685  lock_guard<mutex> lck(poseSem);
686  computeNeighbors();
687  return true;
688 }
689 
690 bool Compensator::setTaxelPosesFromFile(const char *filePath){
691  yarp::os::ResourceFinder rf;
692  rf.setDefaultContext("skinGui"); //overridden by --context parameter
693  rf.setDefaultConfigFile(filePath); //overridden by --from parameter
694  rf.configure(0,NULL);
695 
696  yarp::os::Bottle &calibration = rf.findGroup("calibration");
697  if (calibration.isNull())
698  {
699  return setTaxelPosesFromFileOld(filePath);
700  }
701  else
702  {
703  lock_guard<mutex> lck(poseSem);
704  int size=calibration.size()-1;
705  skinDim=size;
706  taxelPos.resize(skinDim, zeros(3));
707  taxelOri.resize(skinDim, zeros(3));
708  taxelPoseConfidence.resize(skinDim, 0.0);
709  for (int i = 0; i < size; ++i)
710  {
711  if (i<(int)taxelPos.size())
712  {
713  Vector taxelPosNrm(6,0.0);
714  taxelPosNrm = iCub::skinDynLib::vectorFromBottle(*(calibration.get(i+1).asList()),0,6);
715  taxelPos[i] = taxelPosNrm.subVector(0,2);
716  taxelOri[i] = taxelPosNrm.subVector(3,5);
717  }
718  if(norm(taxelPos[i])>0.0)
719  taxelPoseConfidence[i] = 1.0;
720  }
721  computeNeighbors();
722  }
723 
724  return true;
725 }
726 
727 bool Compensator::setTaxelPosesFromFileOld(const char *filePath){
728  ifstream posFile;
729  posFile.open(filePath);
730  if (!posFile.is_open())
731  return false;
732 
733  string posLine;
734  int totalLines = 0;
735  while (getline(posFile,posLine)){
736  posLine.erase(posLine.find_last_not_of(" \n\r\t")+1);
737  if(!posLine.empty())totalLines++;
738  }
739  posFile.clear();
740  posFile.seekg(0, std::ios::beg);//rewind iterator
741  if(totalLines!=skinDim){
742  char* temp = new char[200];
743  sprintf(temp, "Error while reading taxel position file %s: num of lines %d is not equal to num of taxels %d.\n",
744  filePath, totalLines, skinDim);
745  sendInfoMsg(temp);
746  }
747  lock_guard<mutex> lck(poseSem);
748  {
749  for(unsigned int i= 0; getline(posFile,posLine); i++) {
750  posLine.erase(posLine.find_last_not_of(" \n\r\t")+1);
751  if(posLine.empty())
752  continue;
753  string number;
754  istringstream iss(posLine, istringstream::in);
755  for(unsigned int j = 0; iss >> number; j++ ){
756  if(i<taxelPos.size()){
757  if(j<3)
758  taxelPos[i][j] = strtod(number.c_str(),NULL);
759  else
760  taxelOri[i][j-3] = strtod(number.c_str(),NULL);
761  }
762  }
763  if(norm(taxelPos[i])>0.0)
764  taxelPoseConfidence[i] = 1.0;
765  }
766  computeNeighbors();
767  }
768  return true;
769 }
770 bool Compensator::setTaxelPoses(const vector<Vector> &poses){
771  if(poses.size()!=skinDim)
772  return false;
773  lock_guard<mutex> lck(poseSem);
774  {
775  for(unsigned int i=0; i<skinDim; i++){
776  taxelPos[i] = poses[i].subVector(0,2);
777  taxelOri[i] = poses[i].subVector(3,5);
778  if(poses[i].size() == 7)
779  taxelPoseConfidence[i] = poses[i][6];
780  }
781  }
782  computeNeighbors();
783  return true;
784 }
785 bool Compensator::setTaxelPose(unsigned int taxelId, const Vector &pose){
786  if(taxelId>=skinDim || pose.size()!=6)
787  return false;
788  lock_guard<mutex> lck(poseSem);
789  {
790  taxelPos[taxelId] = pose.subVector(0,2);
791  taxelOri[taxelId] = pose.subVector(3,5);
792  if(pose.size() == 7)
793  taxelPoseConfidence[taxelId] = pose[6];
794  }
795  updateNeighbors(taxelId);
796  return true;
797 }
798 bool Compensator::setTaxelPositions(const Vector &positions){
799  lock_guard<mutex> lck(poseSem);
800  if(skinDim*3 == positions.size()){
801  for(unsigned int j=0; j<skinDim; j++){
802  taxelPos[j] = positions.subVector(3*j, 3*j+2);
803  }
804  }
805  else if(skinDim*4 == positions.size()){
806  for(unsigned int j=0; j<skinDim; j++){
807  taxelPos[j] = positions.subVector(4*j, 4*j+2);
808  taxelPoseConfidence[j] = positions[4*j+3];
809  }
810  }
811  else
812  return false;
813 
814  computeNeighbors();
815  return true;
816 }
817 
818 bool Compensator::setTaxelPosition(unsigned int taxelId, const Vector &position){
819  if(taxelId>=skinDim || (position.size()!=3 && position.size()!=4))
820  return false;
821  lock_guard<mutex> lck(poseSem);
822  taxelPos[taxelId] = position.subVector(0,2);
823  if(position.size()==4)
824  taxelPoseConfidence[taxelId] = position[3];
825  updateNeighbors(taxelId);
826  return true;
827 }
828 bool Compensator::setTaxelOrientations(const vector<Vector> &orientations){
829  if(orientations.size()!=skinDim)
830  return false;
831  lock_guard<mutex> lck(poseSem);
832  taxelOri = orientations;
833  return true;
834 }
835 bool Compensator::setTaxelOrientation(unsigned int taxelId, const Vector &orientation){
836  if(taxelId>=skinDim || (orientation.size()!=3 && orientation.size()!=4))
837  return false;
838  lock_guard<mutex> lck(poseSem);
839  taxelOri[taxelId] = orientation.subVector(0,2);
840  if(orientation.size()==4)
841  taxelPoseConfidence[taxelId] = orientation[3];
842  return true;
843 }
844 void Compensator::computeNeighbors(){
845  neighborsXtaxel.clear();
846  neighborsXtaxel.resize(skinDim, list<int>(0));
847  Vector v;
848  double d2 = maxNeighDist*maxNeighDist;
849  for(unsigned int i=0; i<skinDim; i++){
850  for(unsigned int j=i+1; j<skinDim; j++){
851  //if(taxelPos[i][0]!=0.0 || taxelPos[i][1]!=0.0 || taxelPos[i][2]!=0.0){ // if the taxel exists
852  v = taxelPos[i]-taxelPos[j];
853  if( dot(v,v) <= d2){
854  neighborsXtaxel[i].push_back(j);
855  neighborsXtaxel[j].push_back(i);
856  //printf("Taxels %d (%s) and %d (%s) are neighbors\n", i, taxelPos[i].toString().c_str(), j, taxelPos[j].toString().c_str());
857  }
858  //}
859  }
860  }
861 
862  int minNeighbors=skinDim, maxNeighbors=0, ns;
863  for(unsigned int i=0; i<skinDim; i++){
864  //if(taxelPos[i][0]!=0.0 || taxelPos[i][1]!=0.0 || taxelPos[i][2]!=0.0){ // if the taxel exists
865  ns = neighborsXtaxel[i].size();
866  if(ns>maxNeighbors) maxNeighbors = ns;
867  if(ns<minNeighbors) minNeighbors = ns;
868  }
869  stringstream ss;
870  ss<<"Neighbors computed. Min neighbors: "<<minNeighbors<<"; max neighbors: "<<maxNeighbors;
871  sendInfoMsg(ss.str());
872 }
873 void Compensator::updateNeighbors(unsigned int taxelId){
874  Vector v;
875  double d2 = maxNeighDist*maxNeighDist;
876  // remove all the neighbors of the taxel with id=taxelId
877  neighborsXtaxel[taxelId].clear();
878 
879  for(unsigned int i=0; i<skinDim; i++){
880  //if(taxelPos[i][0]!=0.0 || taxelPos[i][1]!=0.0 || taxelPos[i][2]!=0.0){ // if the taxel exists
881  // remove taxelId from the neighbor list (if there is)
882  neighborsXtaxel[i].remove(taxelId);
883  // check whether they are neighbors
884  v = taxelPos[i]-taxelPos[taxelId];
885  if( dot(v,v) <= d2){
886  neighborsXtaxel[i].push_back(taxelId);
887  neighborsXtaxel[taxelId].push_back(i);
888  }
889  }
890 }
891 
892 void Compensator::sendInfoMsg(string msg){
893  yInfo("[%s]: %s", getInputPortName().c_str(), msg.c_str());
894  Bottle& b = infoPort->prepare();
895  b.clear();
896  b.addString(getInputPortName().c_str());
897  b.addString((": " + msg).c_str());
898  infoPort->write(true);
899 }
virtual bool setForce(const yarp::sig::Vector &_F)
Set the contact force.
Definition: dynContact.cpp:101
Class representing a list of external contacts acting on the iCub' skin.
Class representing an external contact acting on the iCub' skin.
Definition: skinContact.h:50
Class that encloses everything relate to a skinPart.
Definition: skinPart.h:146
bool setTaxelPoses(const vector< Vector > &poses)
bool setTaxelPose(unsigned int taxelId, const Vector &pose)
bool setTaxelPosesFromFile(const char *filePath)
vector< Vector > getTaxelOrientations()
vector< Vector > getTaxelPoses()
bool setSmoothFactor(float value)
bool setTaxelOrientations(const vector< Vector > &orientations)
bool setTaxelPosition(unsigned int taxelId, const Vector &position)
double getPoseConfidence(unsigned int taxelId)
bool setAddThreshold(unsigned int thr)
bool setContactCompensationGain(double gain)
bool doesBaselineExceed(unsigned int &taxelIndex, double &baseline, double &initialBaseline)
bool setTaxelPosesFromFileOld(const char *filePath)
void setSkinPart(SkinPart _skinPart)
vector< Vector > getTaxelPositions()
Vector getTaxelPosition(unsigned int taxelId)
bool setTaxelOrientation(unsigned int taxelId, const Vector &orientation)
void setSmoothFilter(bool value)
bool setTaxelPositions(const Vector &positions)
Vector getTaxelPose(unsigned int taxelId)
bool setCompensationGain(double gain)
bool setMaxNeighborDistance(double d)
Vector getTaxelOrientation(unsigned int taxelId)
zeros(2, 2) eye(2
double dot(const yarp::sig::Matrix &A, int colA, const yarp::sig::Matrix &B, int colB)
Returns the dot product between two vectors given in the form: matrix(:,col).
double norm(const yarp::sig::Matrix &M, int col)
Returns the norm of the vector given in the form: matrix(:,col).
int getLinkNum(SkinPart s)
Get the link number associated to the specified skin part.
Definition: common.cpp:34
yarp::sig::Vector vectorFromBottle(const yarp::os::Bottle b, int in, const int size)
Retrieves a vector from a bottle.
Definition: common.cpp:89
BodyPart getBodyPart(SkinPart s)
Get the body part associated to the specified skin part.
Definition: common.cpp:26
static int v
Definition: iCub_Sim.cpp:42
const std::string SkinPart_s[]
Definition: common.h:64
@ SKIN_PART_UNKNOWN
Definition: common.h:57
@ BODY_PART_UNKNOWN
Definition: common.h:45
std::string toString(const T &t)
Definition: compensator.h:199
static const double MAX_NEIGHBOR_DISTANCE
const FSC max
Definition: strain.h:48
const FSC min
Definition: strain.h:49
out
Definition: sine.m:8