iCub-main
compensator.h
Go to the documentation of this file.
1 
2 /*
3  * Copyright (C) 2009 RobotCub Consortium, European Commission FP6 Project IST-004370
4  * Authors: Andrea Del Prete, Alexander Schmitz
5  * email: andrea.delprete@iit.it, alexander.schmitz@iit.it
6  * website: www.robotcub.org
7  * Permission is granted to copy, distribute, and/or modify this program
8  * under the terms of the GNU General Public License, version 2 or any
9  * later version published by the Free Software Foundation.
10  *
11  * A copy of the license can be found at
12  * http://www.robotcub.org/icub/license/gpl.txt
13  *
14  * This program is distributed in the hope that it will be useful, but
15  * WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
17  * Public License for more details
18  */
19 #ifndef __COMP_H__
20 #define __COMP_H__
21 
22 #include <mutex>
23 #include <iostream>
24 #include <string>
25 #include <sstream>
26 #include <vector>
27 #include <list>
28 #include <fstream>
29 #include <deque>
30 
31 #include <yarp/sig/Vector.h>
32 #include <yarp/os/BufferedPort.h>
33 #include <yarp/os/PeriodicThread.h>
34 #include <yarp/os/ResourceFinder.h>
35 #include <yarp/os/Stamp.h>
36 #include <yarp/os/Log.h>
37 #include <yarp/dev/IAnalogSensor.h>
38 #include <yarp/dev/PolyDriver.h>
39 
43 #include "iCub/skinDynLib/common.h"
44 
45 using namespace std;
46 using namespace yarp::os;
47 using namespace yarp::sig;
48 using namespace yarp::dev;
49 using namespace iCub::skinDynLib;
50 
51 namespace iCub{
52 
53 namespace skinManager{
54 
56 {
57 private:
58  /* class constants */
59  static const int MAX_READ_ERROR = 100; // max number of read errors before suspending the compensator
60  static const int MAX_SKIN = 255; // max value you can read from the skin sensors
61  static const int MIN_TOUCH_THR = 1; // min value assigned to the touch thresholds (i.e. the 95% percentile)
62  static const double BIN_TOUCH; // output value of the binarization filter when touch is detected
63  static const double BIN_NO_TOUCH; // output value of the binarization filter when no touch is detected
64 
65  // INIT
66  unsigned int skinDim; // number of taxels (for the hand it is 192)
67  string robotName;
68  string name; // name of the compensator
69  SkinPart skinPart; // id of the part of the skin (e.g. left_hand, right_forearm, left_upper_arm)
70  BodyPart bodyPart; // id of the body part
71  unsigned int linkNum; // number of the link
72 
73  // SKIN CONTACTS
74  vector< list<int> > neighborsXtaxel; // list of neighbors for each taxel
75  vector<Vector> taxelPos; // taxel positions {xPos, yPos, zPos}
76  vector<Vector> taxelOri; // taxel normals {xOri, yOri, zOri}
77  Vector taxelPoseConfidence;// taxels pose estimation confidence
78  double maxNeighDist; // max distance between two neighbor taxels
79  mutex poseSem; // mutex to access taxel poses
80 
81  // COMPENSATION
82  vector<bool> touchDetected; // true if touch has been detected in the last read of the taxel
83  vector<bool> touchDetectedFilt; // true if touch has been detected after applying the filtering
84  vector<bool> subTouchDetected; // true if the taxel value has gone under the baseline (because of touch in neighbouring taxels)
85  Vector rawData; // data read from the skin
86  Vector touchThresholds; // thresholds for discriminating between "touch" and "no touch"
87  mutex touchThresholdSem; // semaphore for controlling the access to the touchThreshold
88  Vector initialBaselines; // mean of the raw tactile data computed during calibration
89  Vector baselines; // mean of the raw tactile data
90  Vector compensatedData; // compensated tactile data (that is rawData-touchThreshold)
91  Vector compensatedDataOld; // compensated tactile data of the previous step (used for smoothing filter)
92  Vector compensatedDataFilt; // compensated tactile data after smooth filter
93 
94  // CALIBRATION
95  int calibrationRead; // count the calibration reads
96  vector<float> start_sum; // sum of the values read during the calibration
97  vector< vector<int> > skin_empty; // distribution of the values read during the calibration
98 
99  // DEVICE
100  IAnalogSensor* tactileSensor; // interface for executing the tactile sensor calibration
101  PolyDriver* tactileSensorDevice;
102  std::string tactileSensorDevice_inputPortName;
103 
104  // ERROR MANAGEMENT
105  bool _isWorking; // true if the compensator is working fine
106  int readErrorCounter; // it counts the number of successive errors
107  vector<unsigned int> saturatedTaxels; // list of all the taxels whose baseline exceeded
108 
109  // input parameters
110  unsigned int addThreshold; // value added to the touch threshold of every taxel
111  double compensationGain; // proportional gain of the compensation algorithm
112  double contactCompensationGain; // proportional gain of the compensation algorithm during contact
113  bool zeroUpRawData; // if true the raw data are considered from zero up, otherwise from 255 down
114  float minBaseline; // min baseline value regarded as "safe"
115  bool binarization; // if true binarize the compensated output value (0: no touch, 255: touch)
116  bool smoothFilter; // if true the smooth filter is on, otherwise it is off
117  float smoothFactor; // intensity of the smooth filter action
118  mutex smoothFactorSem;
119 
120  /* ports */
121  BufferedPort<Vector> compensatedTactileDataPort; // output port
122  BufferedPort<Bottle>* infoPort; // info output port
123  BufferedPort<Vector> inputPort;
124  Stamp timestamp; // timestamp of last data read from inputPort
125 
126 
127  /* class private methods */
128  bool init(string name, string robotName, string outputPortName, string inputPortName);
129  bool readInputData(Vector& skin_values);
130  void sendInfoMsg(string msg);
131  void computeNeighbors();
132  void updateNeighbors(unsigned int taxelId);
133 
134  /* class methods */
135 public:
136  Compensator(string name, string robotName, string outputPortName, string inputPortName, BufferedPort<Bottle>* _infoPort,
137  double _compensationGain, double _contactCompensationGain, int addThreshold, float _minBaseline, bool _zeroUpRawData,
138  bool _binarization, bool _smoothFilter, float _smoothFactor, unsigned int _linkId = 0);
139  ~Compensator();
140 
141  void calibrationInit();
142  void calibrationDataCollection();
143  void calibrationFinish();
144  bool readRawAndWriteCompensatedData();
145  void updateBaseline();
146  bool doesBaselineExceed(unsigned int &taxelIndex, double &baseline, double &initialBaseline);
147  skinContactList getContacts();
148  bool isWorking(){ return _isWorking; }
149 
150  void setBinarization(bool value){ binarization = value; }
151  void setSmoothFilter(bool value);
152  bool setSmoothFactor(float value);
153  bool setAddThreshold(unsigned int thr);
154  bool setCompensationGain(double gain);
155  bool setContactCompensationGain(double gain);
156  bool setMaxNeighborDistance(double d);
157  bool setTaxelPosesFromFile(const char *filePath);
158  bool setTaxelPosesFromFileOld(const char *filePath);
159  bool setTaxelPoses(const vector<Vector> &poses);
160  bool setTaxelPose(unsigned int taxelId, const Vector &pose);
161  bool setTaxelPositions(const Vector &positions);
162  bool setTaxelPosition(unsigned int taxelId, const Vector &position);
163  bool setTaxelOrientations(const vector<Vector> &orientations);
164  bool setTaxelOrientation(unsigned int taxelId, const Vector &orientation);
165  void setSkinPart(SkinPart _skinPart);
166 
167  Vector getTouchThreshold();
168  bool getBinarization(){ return binarization; }
169  bool getSmoothFilter(){ return smoothFilter; }
170  float getSmoothFactor();
171  unsigned int getAddThreshold(){ return addThreshold; }
172  double getCompensationGain(){ return compensationGain; }
173  double getContactCompensationGain(){ return contactCompensationGain; }
174  Vector getTaxelPosition(unsigned int taxelId);
175  vector<Vector> getTaxelPositions();
176  Vector getTaxelOrientation(unsigned int taxelId);
177  vector<Vector> getTaxelOrientations();
178  Vector getTaxelPose(unsigned int taxelId);
179  vector<Vector> getTaxelPoses();
180  double getPoseConfidence(unsigned int taxelId);
181  Vector getPoseConfidences();
182  unsigned int getNumTaxels();
183  Vector getCompensation();
184  Vector getBaselines(){ return baselines; }
185  Vector getRawData(){ return rawData; }
186  Vector getCompData(){ return compensatedData; }
187  Stamp getTimestamp(){ return timestamp; }
188 
189  string getName(){ return name; }
190  string getInputPortName(){ return tactileSensorDevice_inputPortName; }
191  string getSkinPartName(){ return SkinPart_s[skinPart]; }
193  string getBodyPartName(){ return BodyPart_s[bodyPart]; }
194  BodyPart getBodyPart(){ return bodyPart; }
195  unsigned int getLinkNum(){ return linkNum; }
196 
197 };
198 
199 template <class T>
200 inline std::string toString(const T& t){
201  std::stringstream ss;
202  ss << t;
203  return ss.str();
204 }
205 
206 } //namespace iCub
207 
208 } //namespace skinManager
209 
210 #endif
211 
Class representing a list of external contacts acting on the iCub' skin.
Class that encloses everything relate to a skinPart.
Definition: skinPart.h:146
void setBinarization(bool value)
Definition: compensator.h:150
const std::string SkinPart_s[]
Definition: common.h:64
const std::string BodyPart_s[]
Definition: common.h:49
std::string toString(const T &t)
Definition: compensator.h:200
This file contains the definition of unique IDs for the body parts and the skin parts of the robot.