iCub-main
tutorial_perceptiveModels.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 
75 #include <string>
76 #include <cstdio>
77 #include <cmath>
78 
79 #include <yarp/os/Network.h>
80 #include <yarp/os/Value.h>
81 #include <yarp/os/Property.h>
82 #include <yarp/os/RFModule.h>
83 #include <yarp/dev/PolyDriver.h>
84 #include <yarp/dev/ControlBoardInterfaces.h>
85 
88 
89 using namespace std;
90 using namespace yarp::os;
91 using namespace yarp::dev;
92 using namespace iCub::perception;
93 
94 
95 /************************************************************************/
96 class ExampleModule: public RFModule
97 {
98  Model *model;
99  PolyDriver driver;
100  bool calibrate;
101  string fingerName;
102 
103  IPositionControl *ipos;
104  IEncoders *ienc;
105 
106  double min,max,*val;
107  int joint;
108 
109 public:
110  /************************************************************************/
111  ExampleModule() : calibrate(true) { }
112 
113  /************************************************************************/
114  bool configure(ResourceFinder &rf)
115  {
116  string name=rf.find("name").asString();
117  string robot=rf.find("robot").asString();
118  string hand=rf.find("hand").asString();
119  string modelType=rf.find("modelType").asString();
120  fingerName=rf.find("finger").asString();
121 
122  if (fingerName=="thumb")
123  joint=10;
124  else if (fingerName=="index")
125  joint=12;
126  else if (fingerName=="middle")
127  joint=14;
128  else if (fingerName=="ring")
129  joint=15;
130  else if (fingerName=="little")
131  joint=15;
132  else
133  {
134  printf("unknown finger!\n");
135  return false;
136  }
137 
138  Property driverOpt("(device remote_controlboard)");
139  driverOpt.put("remote","/"+robot+"/"+hand+"_arm");
140  driverOpt.put("local","/"+name);
141  if (!driver.open(driverOpt))
142  return false;
143 
144  driver.view(ipos);
145  driver.view(ienc);
146 
147  IControlLimits *ilim;
148  driver.view(ilim);
149 
150  ilim->getLimits(joint,&min,&max);
151  double margin=0.1*(max-min);
152  min=min+margin;
153  max=max-margin;
154  val=&min;
155 
156  Property genOpt;
157  genOpt.put("name",name+"/"+modelType);
158  genOpt.put("robot",robot);
159  genOpt.put("type",hand);
160  genOpt.put("verbose",1);
161  string general(genOpt.toString());
162  string thumb( "(thumb (name thumb))");
163  string index( "(index (name index))");
164  string middle("(middle (name middle))");
165  string ring( "(ring (name ring))");
166  string little("(little (name little))");
167 
168  Property options((general+" "+thumb+" "+index+" "+middle+" "+ring+" "+little).c_str());
169  printf("configuring options: %s\n",options.toString().c_str());
170 
171  if (modelType=="springy")
172  model=new SpringyFingersModel;
173  else if (modelType=="tactile")
174  model=new TactileFingersModel;
175  else
176  {
177  printf("unknown model type!\n");
178  return false;
179  }
180 
181  if (model->fromProperty(options))
182  return true;
183  else
184  {
185  delete model;
186  return false;
187  }
188  }
189 
190  /************************************************************************/
191  bool close()
192  {
193  driver.close();
194 
195  Property options;
196  model->toProperty(options);
197  printf("model options: %s\n",options.toString().c_str());
198 
199  return true;
200  }
201 
202  /************************************************************************/
203  double getPeriod()
204  {
205  return 0.1;
206  }
207 
208  /************************************************************************/
210  {
211  if (calibrate)
212  {
213  Property options;
214  options.put("finger",fingerName);
215  model->calibrate(options);
216  calibrate=false;
217 
218  ipos->setRefAcceleration(joint,1e9);
219  if ((fingerName=="ring")||(fingerName=="little"))
220  ipos->setRefSpeed(joint,60.0);
221  else
222  ipos->setRefSpeed(joint,30.0);
223 
224  ipos->positionMove(joint,*val);
225  }
226  else
227  {
228  if (iCub::perception::Node *finger=model->getNode(fingerName))
229  {
230  Value data; finger->getSensorsData(data);
231  Value out; finger->getOutput(out);
232  printf("%s sensors data = %s; output = %s\n",
233  finger->getName().c_str(),data.toString().c_str(),out.toString().c_str());
234  }
235 
236  double fb; ienc->getEncoder(joint,&fb);
237  if (fabs(*val-fb)<5.0)
238  {
239  val==&min?val=&max:val=&min;
240  ipos->positionMove(joint,*val);
241  }
242  }
243 
244  return true;
245  }
246 };
247 
248 
249 /************************************************************************/
250 int main(int argc, char *argv[])
251 {
252  Network yarp;
253  if (!yarp.checkNetwork())
254  {
255  printf("YARP server not available!\n");
256  return 1;
257  }
258 
259  ResourceFinder rf;
260  rf.setDefault("name","percex");
261  rf.setDefault("robot","icub");
262  rf.setDefault("hand","right");
263  rf.setDefault("modelType","springy");
264  rf.setDefault("finger","index");
265  rf.configure(argc,argv);
266 
267  ExampleModule mod;
268  return mod.runModule(rf);
269 }
iCub::perception::Model::getNode
Node * getNode(const std::string &name) const
Retrieve an attached node by its name.
Definition: models.cpp:80
ExampleModule::configure
bool configure(ResourceFinder &rf)
Definition: tutorial_perceptiveModels.cpp:114
iCub::perception::SpringyFingersModel
Definition: springyFingers.h:204
out
out
Definition: sine.m:8
strain::dsp::fsc::min
const FSC min
Definition: strain.h:49
yarp::dev
Definition: DebugInterfaces.h:52
data
@ data
Definition: ST_M1_dataType.h:64
strain::dsp::fsc::max
const FSC max
Definition: strain.h:48
iCub::perception::Node
Definition: nodes.h:93
ExampleModule::ExampleModule
ExampleModule()
Definition: tutorial_perceptiveModels.cpp:111
iCub::perception::TactileFingersModel
Definition: tactileFingers.h:151
ExampleModule::close
bool close()
Definition: tutorial_perceptiveModels.cpp:191
tactileFingers.h
iCub::perception::Model
Definition: models.h:61
iCub::perception::Model::fromProperty
virtual bool fromProperty(const yarp::os::Property &options)=0
Configure the model taking its parameters from a Property object.
yarp
Copyright (C) 2008 RobotCub Consortium.
Definition: DebugInterfaces.h:51
ExampleModule
Definition: tutorial_actionPrimitives.cpp:173
ExampleModule::updateModule
bool updateModule()
Definition: tutorial_perceptiveModels.cpp:209
scripting.argc
argc
Definition: scripting.py:184
springyFingers.h
iCub::skinManager::calibrate
@ calibrate
Definition: rpcSkinManager.h:13
main
int main(int argc, char *argv[])
Definition: tutorial_perceptiveModels.cpp:250
iCub::perception::Model::toProperty
virtual void toProperty(yarp::os::Property &options) const =0
Return a Property representation of all the model parameters.
iCub::perception::Model::calibrate
virtual bool calibrate(const yarp::os::Property &options)=0
Some kinds of models need to be calibrated to properly operate.
ExampleModule::getPeriod
double getPeriod()
Definition: tutorial_perceptiveModels.cpp:203
iCub::perception
Definition: models.h:52