iCub-main
tactileFingers.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2011 Department of Robotics Brain and Cognitive Sciences - Istituto Italiano di Tecnologia
3  * Author: Ugo Pattacini
4  * email: ugo.pattacini@iit.it
5  * Permission is granted to copy, distribute, and/or modify this program
6  * under the terms of the GNU General Public License, version 2 or any
7  * later version published by the Free Software Foundation.
8  *
9  * A copy of the license can be found at
10  * http://www.robotcub.org/icub/license/gpl.txt
11  *
12  * This program is distributed in the hope that it will be useful, but
13  * WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
15  * Public License for more details
16 */
17 
18 #include <sstream>
19 #include <iomanip>
20 #include <algorithm>
21 
22 #include <iCub/ctrl/math.h>
26 
27 using namespace std;
28 using namespace yarp::os;
29 using namespace yarp::sig;
30 using namespace iCub::perception;
31 
32 
33 /************************************************************************/
34 bool TactileFinger::fromProperty(const Property &options)
35 {
36  if (!options.check("name"))
37  return false;
38 
39  sensors.clear();
40  callbacks.clear();
41  neighbors.clear();
42 
43  name=options.find("name").asString();
44  directLogic=(options.check("logic",Value("direct")).asString()=="direct");
45  outputGain=options.check("output_gain",Value(1.0)).asFloat64();
46 
47  if ((name=="thumb") || (name=="index") || (name=="middle") ||
48  (name=="ring") || (name=="little"))
49  return true;
50  else
51  return false;
52 }
53 
54 
55 /************************************************************************/
56 void TactileFinger::toProperty(Property &options) const
57 {
58  options.clear();
59  options.put("name",name);
60  options.put("logic",directLogic?"direct":"inverse");
61  options.put("output_gain",outputGain);
62 }
63 
64 
65 /************************************************************************/
66 bool TactileFinger::toStream(ostream &str) const
67 {
68  str<<"name "<<name<<endl;
69  str<<"logic "<<(directLogic?"direct":"inverse")<<endl;
70  str<<"output_gain "<<outputGain<<endl;
71 
72  if (str.fail())
73  return false;
74  else
75  return true;
76 }
77 
78 
79 /************************************************************************/
80 bool TactileFinger::calibrate(const Property &options)
81 {
82  yWarning("TactileFinger: calibration not available");
83  return true;
84 }
85 
86 
87 /************************************************************************/
88 bool TactileFinger::isCalibrated() const
89 {
90  yWarning("TactileFinger: calibration not available");
91  return true;
92 }
93 
94 
95 /************************************************************************/
96 bool TactileFinger::getSensorsData(Value &data) const
97 {
98  Vector in(12);
99  for (int j=0; j<12; j++)
100  {
101  ostringstream tag;
102  tag<<"In_"<<j;
103 
104  map<string,Sensor*>::const_iterator In=sensors.find(tag.str());
105  if (In==sensors.end())
106  return false;
107 
108  Value val_in; In->second->getOutput(val_in);
109  in[j]=val_in.asFloat64();
110  }
111 
112  Property prop;
113  Bottle b;
114 
115  b.addList().read(in);
116  prop.put("in",b.get(0));
117 
118  b.addList().read(prop);
119  data=b.get(1);
120 
121  return true;
122 }
123 
124 
125 /************************************************************************/
126 bool TactileFinger::extractSensorsData(Vector &in) const
127 {
128  bool ret=false;
129 
130  Value data;
131  if (getSensorsData(data))
132  {
133  if (Bottle *b1=data.asList())
134  {
135  if (Bottle *b2=b1->find("in").asList())
136  {
137  in.resize(b2->size());
138  for (size_t i=0; i<in.length(); i++)
139  in[i]=b2->get(i).asFloat64();
140 
141  ret=true;
142  }
143  }
144  }
145 
146  return ret;
147 }
148 
149 
150 /************************************************************************/
151 bool TactileFinger::getOutput(Value &out) const
152 {
153  Vector i;
154  if (!extractSensorsData(i))
155  return false;
156 
157  double ret=0.0;
158  for (size_t j=0; j<i.length(); j++)
159  ret=std::max(ret,directLogic?(255.0-i[j]):i[j]);
160 
161  out=Value(outputGain*ret);
162 
163  return true;
164 }
165 
166 
167 /************************************************************************/
168 TactileFingersModel::TactileFingersModel()
169 {
170  port=new iCub::perception::Port;
171  configured=false;
172 }
173 
174 
175 /************************************************************************/
176 bool TactileFingersModel::fromProperty(const Property &options)
177 {
178  if (!options.check("name") || !options.check("type"))
179  {
180  printMessage(log::error,1,"missing mandatory options \"name\" and/or \"type\"");
181  return false;
182  }
183 
184  if (configured)
185  close();
186 
187  name=options.find("name").asString();
188  type=options.find("type").asString();
189  robot=options.check("robot",Value("icub")).asString();
190  carrier=options.check("carrier",Value("udp")).asString();
191  compensation=(options.check("compensation",Value("false")).asString()=="true");
192  verbosity=options.check("verbosity",Value(0)).asInt32();
193 
194  port->open("/"+name+"/"+type+"_hand:i");
195  string skinPortName("/"+robot+"/skin/"+type+"_hand");
196  if (compensation)
197  skinPortName+="_comp";
198 
199  if (!Network::connect(skinPortName,port->getName(),carrier))
200  {
201  printMessage(log::error,1,"unable to connect to %s",skinPortName.c_str());
202  close();
203  return false;
204  }
205 
206  printMessage(log::info,1,"configuring port-based sensors ...");
207  void *pPort=static_cast<void*>(port);
208  for (int j=0; j<60; j++)
209  {
210  ostringstream tag;
211  tag<<"In_"<<(j%12);
212 
213  Property prop;
214  prop.put("name",tag.str());
215  prop.put("index",j);
216 
217  if (!sensPort[j].configure(pPort,prop))
218  {
219  printMessage(log::error,1,"some errors occured");
220  close();
221  return false;
222  }
223  }
224 
225  printMessage(log::info,1,"configuring fingers ...");
226  Property thumb(options.findGroup("thumb").toString().c_str());
227  Property index(options.findGroup("index").toString().c_str());
228  Property middle(options.findGroup("middle").toString().c_str());
229  Property ring(options.findGroup("ring").toString().c_str());
230  Property little(options.findGroup("little").toString().c_str());
231 
232  bool fingers_ok=true;
233  fingers_ok&=fingers[0].fromProperty(thumb);
234  fingers_ok&=fingers[1].fromProperty(index);
235  fingers_ok&=fingers[2].fromProperty(middle);
236  fingers_ok&=fingers[3].fromProperty(ring);
237  fingers_ok&=fingers[4].fromProperty(little);
238 
239  if (!fingers_ok)
240  {
241  printMessage(log::error,1,"some errors occured");
242  close();
243  return false;
244  }
245 
246  printMessage(log::info,1,"attaching sensors to fingers ...");
247  for (int j=0; j<12; j++)
248  {
249  fingers[0].attachSensor(sensPort[j+4*12]);
250  fingers[1].attachSensor(sensPort[j]);
251  fingers[2].attachSensor(sensPort[j+12]);
252  fingers[3].attachSensor(sensPort[j+2*12]);
253  fingers[4].attachSensor(sensPort[j+3*12]);
254  }
255 
256  attachNode(fingers[0]);
257  attachNode(fingers[1]);
258  attachNode(fingers[2]);
259  attachNode(fingers[3]);
260  attachNode(fingers[4]);
261 
262  printMessage(log::info,1,"configuration complete");
263  return configured=true;
264 }
265 
266 
267 /************************************************************************/
268 void TactileFingersModel::toProperty(Property &options) const
269 {
270  options.clear();
271 
272  if (configured)
273  {
274  Property prop[5];
275  fingers[0].toProperty(prop[0]);
276  fingers[1].toProperty(prop[1]);
277  fingers[2].toProperty(prop[2]);
278  fingers[3].toProperty(prop[3]);
279  fingers[4].toProperty(prop[4]);
280 
281  string thumb="(thumb ";
282  thumb+=prop[0].toString();
283  thumb+=")";
284 
285  string index="(index ";
286  index+=prop[1].toString();
287  index+=")";
288 
289  string middle="(middle ";
290  middle+=prop[2].toString();
291  middle+=")";
292 
293  string ring="(ring ";
294  ring+=prop[3].toString();
295  ring+=")";
296 
297  string little="(little ";
298  little+=prop[4].toString();
299  little+=")";
300 
301  options.fromString(thumb+index+middle+ring+little);
302  options.put("name",name);
303  options.put("type",type);
304  options.put("robot",robot);
305  options.put("compensation",compensation?"true":"false");
306  options.put("verbosity",verbosity);
307  }
308 }
309 
310 
311 /************************************************************************/
312 bool TactileFingersModel::toStream(ostream &str) const
313 {
314  if (configured)
315  {
316  str<<"name "<<name<<endl;
317  str<<"type "<<type<<endl;
318  str<<"robot "<<robot<<endl;
319  str<<"compensation "<<(compensation?"true":"false")<<endl;
320  str<<"verbosity "<<verbosity<<endl;
321 
322  str<<endl;
323  str<<"[thumb]"<<endl;
324  fingers[0].toStream(str);
325 
326  str<<endl;
327  str<<"[index]"<<endl;
328  fingers[1].toStream(str);
329 
330  str<<endl;
331  str<<"[middle]"<<endl;
332  fingers[2].toStream(str);
333 
334  str<<endl;
335  str<<"[ring]"<<endl;
336  fingers[3].toStream(str);
337 
338  str<<endl;
339  str<<"[little]"<<endl;
340  fingers[4].toStream(str);
341 
342  return true;
343  }
344  else
345  return false;
346 }
347 
348 
349 /************************************************************************/
350 bool TactileFingersModel::calibrate(const Property &options)
351 {
352  printMessage(log::warning,1,"calibration not available");
353  return true;
354 }
355 
356 
357 /************************************************************************/
358 bool TactileFingersModel::isCalibrated() const
359 {
360  printMessage(log::warning,1,"calibration not available");
361  return true;
362 }
363 
364 
365 /************************************************************************/
366 bool TactileFingersModel::getOutput(Value &out) const
367 {
368  if (configured)
369  {
370  Value val[5];
371  fingers[0].getOutput(val[0]);
372  fingers[1].getOutput(val[1]);
373  fingers[2].getOutput(val[2]);
374  fingers[3].getOutput(val[3]);
375  fingers[4].getOutput(val[4]);
376 
377  Bottle bOut; Bottle &ins=bOut.addList();
378  ins.addFloat64(val[0].asFloat64());
379  ins.addFloat64(val[1].asFloat64());
380  ins.addFloat64(val[2].asFloat64());
381  ins.addFloat64(val[3].asFloat64());
382  ins.addFloat64(val[4].asFloat64());
383 
384  out=bOut.get(0);
385  return true;
386  }
387  else
388  return false;
389 }
390 
391 
392 /************************************************************************/
393 void TactileFingersModel::close()
394 {
395  printMessage(log::info,1,"closing ...");
396 
397  if (!port->isClosed())
398  port->close();
399 
400  nodes.clear();
401 
402  configured=false;
403 }
404 
405 
406 /************************************************************************/
407 TactileFingersModel::~TactileFingersModel()
408 {
409  close();
410  delete port;
411 }
412 
413 
iCub::action::log::info
@ info
Definition: actionPrimitives.cpp:64
models.h
out
out
Definition: sine.m:8
verbosity
int verbosity
Definition: main.cpp:21
data
@ data
Definition: ST_M1_dataType.h:64
math.h
strain::dsp::fsc::max
const FSC max
Definition: strain.h:48
iCub::perception::Port
Definition: ports.h:34
iCub::action::log::error
@ error
Definition: actionPrimitives.cpp:64
tactileFingers.h
ports.h
iCub::skinManager::calibrate
@ calibrate
Definition: rpcSkinManager.h:13
iCub::action::log::warning
@ warning
Definition: actionPrimitives.cpp:64
iCub::perception
Definition: models.h:52