iCub-main
tutorial_actionPrimitives.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010 RobotCub Consortium, European Commission FP6 Project IST-004370
3  * Author: Ugo Pattacini
4  * email: ugo.pattacini@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 
141 #include <iostream>
142 #include <fstream>
143 #include <iomanip>
144 #include <string>
145 #include <deque>
146 
147 #include <yarp/os/Network.h>
148 #include <yarp/os/RFModule.h>
149 #include <yarp/os/Bottle.h>
150 #include <yarp/os/BufferedPort.h>
151 #include <yarp/sig/Vector.h>
152 #include <yarp/math/Math.h>
153 #include <yarp/dev/Drivers.h>
154 
155 #include <iCub/perception/models.h>
157 
158 #define USE_LEFT 0
159 #define USE_RIGHT 1
160 
161 #define AFFACTIONPRIMITIVESLAYER ActionPrimitivesLayer1
162 
163 using namespace std;
164 using namespace yarp::os;
165 using namespace yarp::sig;
166 using namespace yarp::dev;
167 using namespace yarp::math;
168 using namespace iCub::perception;
169 using namespace iCub::action;
170 
171 
172 /************************************************************************/
173 class ExampleModule: public RFModule
174 {
175 protected:
177  BufferedPort<Bottle> inPort;
179 
180  Vector graspOrien;
181  Vector graspDisp;
182  Vector dOffs;
183  Vector dLift;
184  Vector home_x;
185 
186  bool firstRun;
187 
188 public:
189  /************************************************************************/
191  {
192  graspOrien.resize(4);
193  graspDisp.resize(3);
194  dOffs.resize(3);
195  dLift.resize(3);
196  home_x.resize(3);
197 
198  // default values for arm-dependent quantities
199  graspOrien[0]=-0.171542;
200  graspOrien[1]= 0.124396;
201  graspOrien[2]=-0.977292;
202  graspOrien[3]= 3.058211;
203 
204  graspDisp[0]=0.0;
205  graspDisp[1]=0.0;
206  graspDisp[2]=0.05;
207 
208  dOffs[0]=-0.03;
209  dOffs[1]=-0.07;
210  dOffs[2]=-0.02;
211 
212  dLift[0]=0.0;
213  dLift[1]=0.0;
214  dLift[2]=0.15;
215 
216  home_x[0]=-0.29;
217  home_x[1]=-0.21;
218  home_x[2]= 0.11;
219 
220  action=NULL;
221  firstRun=true;
222  }
223 
224  /************************************************************************/
225  void getArmDependentOptions(Bottle &b, Vector &_gOrien, Vector &_gDisp,
226  Vector &_dOffs, Vector &_dLift, Vector &_home_x)
227  {
228  if (Bottle *pB=b.find("grasp_orientation").asList())
229  {
230  int sz=(int)pB->size();
231  int len=(int)_gOrien.length();
232  int l=len<sz?len:sz;
233 
234  for (int i=0; i<l; i++)
235  _gOrien[i]=pB->get(i).asFloat64();
236  }
237 
238  if (Bottle *pB=b.find("grasp_displacement").asList())
239  {
240  int sz=(int)pB->size();
241  int len=(int)_gDisp.length();
242  int l=len<sz?len:sz;
243 
244  for (int i=0; i<l; i++)
245  _gDisp[i]=pB->get(i).asFloat64();
246  }
247 
248  if (Bottle *pB=b.find("systematic_error_displacement").asList())
249  {
250  int sz=(int)pB->size();
251  int len=(int)_dOffs.length();
252  int l=len<sz?len:sz;
253 
254  for (int i=0; i<l; i++)
255  _dOffs[i]=pB->get(i).asFloat64();
256  }
257 
258  if (Bottle *pB=b.find("lifting_displacement").asList())
259  {
260  int sz=(int)pB->size();
261  int len=(int)_dLift.length();
262  int l=len<sz?len:sz;
263 
264  for (int i=0; i<l; i++)
265  _dLift[i]=pB->get(i).asFloat64();
266  }
267 
268  if (Bottle *pB=b.find("home_position").asList())
269  {
270  int sz=(int)pB->size();
271  int len=(int)_home_x.length();
272  int l=len<sz?len:sz;
273 
274  for (int i=0; i<l; i++)
275  _home_x[i]=pB->get(i).asFloat64();
276  }
277  }
278 
279  /************************************************************************/
280  bool configure(ResourceFinder &rf)
281  {
282  string name=rf.find("name").asString();
283  setName(name.c_str());
284 
285  Property config; config.fromConfigFile(rf.findFile("from"));
286  Bottle &bGeneral=config.findGroup("general");
287  if (bGeneral.isNull())
288  {
289  cout<<"Error: group general is missing!"<<endl;
290  return false;
291  }
292 
293  // parsing general config options
294  Property option(bGeneral.toString().c_str());
295  option.put("local",name);
296  option.put("part","left_arm");
297  option.put("grasp_model_type",rf.find("grasp_model_type").asString());
298  option.put("grasp_model_file",rf.findFile("grasp_model_file"));
299  option.put("hand_sequences_file",rf.findFile("hand_sequences_file"));
300 
301  // parsing arm dependent config options
302  Bottle &bArm=config.findGroup("arm_dependent");
303  getArmDependentOptions(bArm,graspOrien,graspDisp,dOffs,dLift,home_x);
304 
305  cout<<"***** Instantiating primitives for left arm"<<endl;
306  action=new AFFACTIONPRIMITIVESLAYER(option);
307  if (!action->isValid())
308  {
309  delete action;
310  return false;
311  }
312 
313  deque<string> q=action->getHandSeqList();
314  cout<<"***** List of available hand sequence keys:"<<endl;
315  for (size_t i=0; i<q.size(); i++)
316  cout<<q[i]<<endl;
317 
318  string fwslash="/";
319  inPort.open(fwslash+name+"/in");
320  rpcPort.open(fwslash+name+"/rpc");
321  attach(rpcPort);
322 
323  // check whether the grasp model is calibrated,
324  // otherwise calibrate it
325  Model *model; action->getGraspModel(model);
326  if (model!=NULL)
327  {
328  if (!model->isCalibrated())
329  {
330  Property prop;
331  prop.put("finger","all");
332  model->calibrate(prop);
333  }
334  }
335 
336  return true;
337  }
338 
339  /************************************************************************/
340  bool close()
341  {
342  if (action!=NULL)
343  delete action;
344 
345  inPort.close();
346  rpcPort.close();
347 
348  return true;
349  }
350 
351  /************************************************************************/
352  double getPeriod()
353  {
354  return 0.1;
355  }
356 
357  /************************************************************************/
358  void init()
359  {
360  bool f;
361 
362  action->pushAction(home_x,"open_hand");
363  action->checkActionsDone(f,true);
364  action->enableArmWaving(home_x);
365  }
366 
367  // we don't need a thread since the actions library already
368  // incapsulates one inside dealing with all the tight time constraints
369  /************************************************************************/
371  {
372  // do it only once
373  if (firstRun)
374  {
375  init();
376  firstRun=false;
377  }
378 
379  // get a target object position from a YARP port
380  Bottle *b=inPort.read(); // blocking call
381 
382  if (b!=NULL)
383  {
384  Vector xd(3);
385  bool f;
386 
387  xd[0]=b->get(0).asFloat64();
388  xd[1]=b->get(1).asFloat64();
389  xd[2]=b->get(2).asFloat64();
390 
391  // apply systematic offset
392  // due to uncalibrated kinematic
393  xd=xd+dOffs;
394 
395  // safe thresholding
396  xd[0]=xd[0]>-0.1?-0.1:xd[0];
397 
398  // grasp (wait until it's done)
399  action->grasp(xd,graspOrien,graspDisp);
400  action->checkActionsDone(f,true);
401  action->areFingersInPosition(f);
402 
403  // if fingers are not in position,
404  // it's likely that we've just grasped
405  // something, so lift it up!
406  if (!f)
407  {
408  cout<<"Wow, got something!"<<endl;
409 
410  // lift the object (wait until it's done)
411  action->pushAction(xd+dLift,graspOrien);
412  action->checkActionsDone(f,true);
413  }
414  else
415  cout<<"Sorry :( ... nothing to grasp"<<endl;
416 
417  // release the object or just open the
418  // hand (wait until it's done)
419  action->pushAction("open_hand");
420  action->checkActionsDone(f,true);
421 
422  // go home :) (wait until it's done, since
423  // we may use two arms that share the torso)
424  action->pushAction(home_x);
425  action->checkActionsDone(f,true);
426 
427  // let the hand wave a bit around home position
428  // the waving will be disabled before commencing
429  // a new action
430  action->enableArmWaving(home_x);
431  }
432 
433  return true;
434  }
435 
436  /************************************************************************/
438  {
439  // since a call to checkActionsDone() blocks
440  // the execution until it's done, we need to
441  // take control and exit from the waiting state
442  action->syncCheckInterrupt(true);
443 
444  inPort.interrupt();
445  rpcPort.interrupt();
446 
447  return true;
448  }
449 };
450 
451 
452 /************************************************************************/
453 int main(int argc, char *argv[])
454 {
455  Network yarp;
456  if (!yarp.checkNetwork())
457  {
458  cout<<"YARP server not available!"<<endl;
459  return 1;
460  }
461 
462  ResourceFinder rf;
463  rf.setDefaultConfigFile("config.ini");
464  rf.setDefault("grasp_model_type","tactile");
465  rf.setDefault("grasp_model_file","grasp_model.ini");
466  rf.setDefault("hand_sequences_file","hand_sequences.ini");
467  rf.setDefault("name","actionPrimitivesMod");
468  rf.configure(argc,argv);
469 
470  ExampleModule mod;
471  return mod.runModule(rf);
472 }
ExampleModule::configure
bool configure(ResourceFinder &rf)
Definition: tutorial_actionPrimitives.cpp:280
ExampleModule::graspDisp
Vector graspDisp
Definition: tutorial_actionPrimitives.cpp:181
main
int main(int argc, char *argv[])
Definition: tutorial_actionPrimitives.cpp:453
iCub::action
Definition: actionPrimitives.h:102
ExampleModule::firstRun
bool firstRun
Definition: tutorial_actionPrimitives.cpp:186
actionPrimitives.h
ExampleModule::inPort
BufferedPort< Bottle > inPort
Definition: tutorial_actionPrimitives.cpp:177
ExampleModule::dOffs
Vector dOffs
Definition: tutorial_actionPrimitives.cpp:182
ExampleModule::action
AFFACTIONPRIMITIVESLAYER * action
Definition: tutorial_actionPrimitives.cpp:176
yarp::dev
Definition: DebugInterfaces.h:52
ExampleModule::interruptModule
bool interruptModule()
Definition: tutorial_actionPrimitives.cpp:437
ExampleModule::init
void init()
Definition: tutorial_actionPrimitives.cpp:358
ExampleModule::getArmDependentOptions
void getArmDependentOptions(Bottle &b, Vector &_gOrien, Vector &_gDisp, Vector &_dOffs, Vector &_dLift, Vector &_home_x)
Definition: tutorial_actionPrimitives.cpp:225
iCub::perception::Model::isCalibrated
virtual bool isCalibrated() const =0
Return the internal status of the calibration.
ExampleModule::graspOrien
Vector graspOrien
Definition: tutorial_actionPrimitives.cpp:180
iCub::perception::Port
Definition: ports.h:34
ExampleModule::ExampleModule
ExampleModule()
Definition: tutorial_actionPrimitives.cpp:190
inPort
Definition: main.cpp:52
models.h
f
f
Definition: compute_ekf_sym.m:22
ExampleModule::close
bool close()
Definition: tutorial_actionPrimitives.cpp:340
iCub::perception::Model
Definition: models.h:61
yarp
Copyright (C) 2008 RobotCub Consortium.
Definition: DebugInterfaces.h:51
ExampleModule::dLift
Vector dLift
Definition: tutorial_actionPrimitives.cpp:183
ExampleModule
Definition: tutorial_actionPrimitives.cpp:173
ExampleModule::updateModule
bool updateModule()
Definition: tutorial_actionPrimitives.cpp:370
scripting.argc
argc
Definition: scripting.py:184
ExampleModule::home_x
Vector home_x
Definition: tutorial_actionPrimitives.cpp:184
AFFACTIONPRIMITIVESLAYER
#define AFFACTIONPRIMITIVESLAYER
Definition: tutorial_actionPrimitives.cpp:161
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_actionPrimitives.cpp:352
iCub::perception
Definition: models.h:52
ExampleModule::rpcPort
Port rpcPort
Definition: tutorial_actionPrimitives.cpp:178