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 
103  // ERROR MANAGEMENT
104  bool _isWorking; // true if the compensator is working fine
105  int readErrorCounter; // it counts the number of successive errors
106  vector<unsigned int> saturatedTaxels; // list of all the taxels whose baseline exceeded
107 
108  // input parameters
109  unsigned int addThreshold; // value added to the touch threshold of every taxel
110  double compensationGain; // proportional gain of the compensation algorithm
111  double contactCompensationGain; // proportional gain of the compensation algorithm during contact
112  bool zeroUpRawData; // if true the raw data are considered from zero up, otherwise from 255 down
113  float minBaseline; // min baseline value regarded as "safe"
114  bool binarization; // if true binarize the compensated output value (0: no touch, 255: touch)
115  bool smoothFilter; // if true the smooth filter is on, otherwise it is off
116  float smoothFactor; // intensity of the smooth filter action
117  mutex smoothFactorSem;
118 
119  /* ports */
120  BufferedPort<Vector> compensatedTactileDataPort; // output port
121  BufferedPort<Bottle>* infoPort; // info output port
122  BufferedPort<Vector> inputPort;
123  Stamp timestamp; // timestamp of last data read from inputPort
124 
125 
126  /* class private methods */
127  bool init(string name, string robotName, string outputPortName, string inputPortName);
128  bool readInputData(Vector& skin_values);
129  void sendInfoMsg(string msg);
130  void computeNeighbors();
131  void updateNeighbors(unsigned int taxelId);
132 
133  /* class methods */
134 public:
135  Compensator(string name, string robotName, string outputPortName, string inputPortName, BufferedPort<Bottle>* _infoPort,
136  double _compensationGain, double _contactCompensationGain, int addThreshold, float _minBaseline, bool _zeroUpRawData,
137  bool _binarization, bool _smoothFilter, float _smoothFactor, unsigned int _linkId = 0);
138  ~Compensator();
139 
140  void calibrationInit();
141  void calibrationDataCollection();
142  void calibrationFinish();
143  bool readRawAndWriteCompensatedData();
144  void updateBaseline();
145  bool doesBaselineExceed(unsigned int &taxelIndex, double &baseline, double &initialBaseline);
146  skinContactList getContacts();
147  bool isWorking(){ return _isWorking; }
148 
149  void setBinarization(bool value){ binarization = value; }
150  void setSmoothFilter(bool value);
151  bool setSmoothFactor(float value);
152  bool setAddThreshold(unsigned int thr);
153  bool setCompensationGain(double gain);
154  bool setContactCompensationGain(double gain);
155  bool setMaxNeighborDistance(double d);
156  bool setTaxelPosesFromFile(const char *filePath);
157  bool setTaxelPosesFromFileOld(const char *filePath);
158  bool setTaxelPoses(const vector<Vector> &poses);
159  bool setTaxelPose(unsigned int taxelId, const Vector &pose);
160  bool setTaxelPositions(const Vector &positions);
161  bool setTaxelPosition(unsigned int taxelId, const Vector &position);
162  bool setTaxelOrientations(const vector<Vector> &orientations);
163  bool setTaxelOrientation(unsigned int taxelId, const Vector &orientation);
164  void setSkinPart(SkinPart _skinPart);
165 
166  Vector getTouchThreshold();
167  bool getBinarization(){ return binarization; }
168  bool getSmoothFilter(){ return smoothFilter; }
169  float getSmoothFactor();
170  unsigned int getAddThreshold(){ return addThreshold; }
171  double getCompensationGain(){ return compensationGain; }
172  double getContactCompensationGain(){ return contactCompensationGain; }
173  Vector getTaxelPosition(unsigned int taxelId);
174  vector<Vector> getTaxelPositions();
175  Vector getTaxelOrientation(unsigned int taxelId);
176  vector<Vector> getTaxelOrientations();
177  Vector getTaxelPose(unsigned int taxelId);
178  vector<Vector> getTaxelPoses();
179  double getPoseConfidence(unsigned int taxelId);
180  Vector getPoseConfidences();
181  unsigned int getNumTaxels();
182  Vector getCompensation();
183  Vector getBaselines(){ return baselines; }
184  Vector getRawData(){ return rawData; }
185  Vector getCompData(){ return compensatedData; }
186  Stamp getTimestamp(){ return timestamp; }
187 
188  string getName(){ return name; }
189  string getInputPortName(){ return tactileSensorDevice->getValue("remote").asString().c_str(); }
190  string getSkinPartName(){ return SkinPart_s[skinPart]; }
192  string getBodyPartName(){ return BodyPart_s[bodyPart]; }
193  BodyPart getBodyPart(){ return bodyPart; }
194  unsigned int getLinkNum(){ return linkNum; }
195 
196 };
197 
198 template <class T>
199 inline std::string toString(const T& t){
200  std::stringstream ss;
201  ss << t;
202  return ss.str();
203 }
204 
205 } //namespace iCub
206 
207 } //namespace skinManager
208 
209 #endif
210 
iCub::skinDynLib::skinContactList
Definition: skinContactList.h:44
iCub
This file contains the definition of unique IDs for the body parts and the skin parts of the robot.
Definition: emotionInterface.h:17
T
T
Definition: compute_ekf_sym.m:12
iCub::skinDynLib::skinPart
Definition: skinPart.h:145
iCub::skinDynLib
Definition: common.h:35
iCub::skinManager::Compensator::getBodyPartName
string getBodyPartName()
Definition: compensator.h:192
iCub::skinManager::Compensator::getSkinPartName
string getSkinPartName()
Definition: compensator.h:190
iCub::skinManager::Compensator::getBinarization
bool getBinarization()
Definition: compensator.h:167
iCub::skinManager::Compensator
Definition: compensator.h:55
iCub::skinManager::Compensator::getName
string getName()
Definition: compensator.h:188
yarp::dev
Definition: DebugInterfaces.h:52
rpcSkinManager.h
iCub::skinManager::Compensator::getTimestamp
Stamp getTimestamp()
Definition: compensator.h:186
iCub::skinManager::toString
std::string toString(const T &t)
Definition: compensator.h:199
iCub::skinManager::Compensator::getInputPortName
string getInputPortName()
Definition: compensator.h:189
iCub::skinManager::Compensator::getBodyPart
BodyPart getBodyPart()
Definition: compensator.h:193
iCub::skinManager::Compensator::getContactCompensationGain
double getContactCompensationGain()
Definition: compensator.h:172
iCub::skinManager::Compensator::getLinkNum
unsigned int getLinkNum()
Definition: compensator.h:194
iCub::skinManager::Compensator::isWorking
bool isWorking()
Definition: compensator.h:147
skinContactList.h
string
string(REPLACE "-rdynamic" "" CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS}") include_directories($
Definition: CMakeLists.txt:9
iCub::skinManager::Compensator::getSkinPart
SkinPart getSkinPart()
Definition: compensator.h:191
iCub::skinManager::Compensator::getCompensationGain
double getCompensationGain()
Definition: compensator.h:171
iCub::skinDynLib::SkinPart
SkinPart
Definition: common.h:56
iCub::skinManager::Compensator::getRawData
Vector getRawData()
Definition: compensator.h:184
iCub::skinDynLib::SkinPart_s
const std::string SkinPart_s[]
Definition: common.h:64
iCub::skinManager::Compensator::getCompData
Vector getCompData()
Definition: compensator.h:185
iCub::skinManager::Compensator::getBaselines
Vector getBaselines()
Definition: compensator.h:183
common.h
iCub::skinDynLib::BodyPart
BodyPart
Definition: common.h:44
iCub::skinManager::Compensator::setBinarization
void setBinarization(bool value)
Definition: compensator.h:149
iCub::skinDynLib::BodyPart_s
const std::string BodyPart_s[]
Definition: common.h:49
skinContact.h
iCub::skinManager::Compensator::getAddThreshold
unsigned int getAddThreshold()
Definition: compensator.h:170
iCub::skinManager::Compensator::getSmoothFilter
bool getSmoothFilter()
Definition: compensator.h:168