iCub-main
main.cpp
Go to the documentation of this file.
1 
69 #include <yarp/os/RFModule.h>
70 #include <yarp/os/Time.h>
71 #include <yarp/os/Network.h>
72 #include <yarp/os/Log.h>
73 #include <yarp/os/LogStream.h>
74 #include <yarp/dev/PolyDriver.h>
75 #include <yarp/dev/ControlBoardInterfaces.h>
76 #include <iCub/iDyn/iDyn.h>
77 #include <iCub/iDyn/iDynBody.h>
78 
79 #include <iostream>
80 
81 #include "gravityThread.h"
82 
83 using namespace yarp::os;
84 using namespace yarp::sig;
85 using namespace yarp::math;
86 using namespace yarp::dev;
87 using namespace iCub::ctrl;
88 using namespace iCub::iDyn;
89 using namespace std;
90 
91 
92 
93 class gravityModuleCompensator: public RFModule
94 {
95 private:
96  int rate{};
97  gravityCompensatorThread *g_comp{};
98 
99  Property OptionsLeftArm;
100  Property OptionsRightArm;
101  Property OptionsHead;
102  Property OptionsLeftLeg;
103  Property OptionsRightLeg;
104  Property OptionsTorso;
105 
106  Port rpcPort;
107 
108  PolyDriver *dd_left_arm;
109  PolyDriver *dd_right_arm;
110  PolyDriver *dd_head;
111  PolyDriver *dd_left_leg;
112  PolyDriver *dd_right_leg;
113  PolyDriver *dd_torso;
114 
115  bool legs_enabled;
116  bool right_arm_enabled;
117  bool left_arm_enabled;
118  bool torso_enabled;
119  bool head_enabled;
120  string m_side;
121  string m_part;
122 
123 public:
125  {
126  legs_enabled = true;
127  left_arm_enabled = true;
128  right_arm_enabled = true;
129  torso_enabled = true;
130  head_enabled = true;
131  dd_left_arm = nullptr;
132  dd_right_arm = nullptr;
133  dd_head = nullptr;
134  dd_left_leg = nullptr;
135  dd_right_leg = nullptr;
136  dd_torso = nullptr;
137  m_side = "right";
138  m_part = "arm";
139  }
140 
141  virtual bool createDriver(PolyDriver *&_dd, Property options)
142  {
143  int trials=0;
144  double start_time = yarp::os::Time::now();
145 
146  do
147  {
148  double current_time = yarp::os::Time::now();
149 
150  //remove previously existing drivers
151  if (_dd)
152  {
153  delete _dd;
154  _dd=nullptr;
155  }
156 
157  //creates the new device driver
158  _dd = new PolyDriver(options);
159  bool connected =_dd->isValid();
160 
161  //check if the driver is connected
162  if (connected) break;
163 
164  //check if the timeout (60s) is expired
165  if (current_time-start_time > 60.0)
166  {
167  yError("It is not possible to instantiate the device driver. I tried %d times!\n", trials);
168  return false;
169  }
170 
171  yarp::os::Time::delay(5);
172  trials++;
173  yWarning("Unable to connect the device driver, trying again...\n");
174  }
175  while (true);
176 
177  IEncoders *encs = nullptr;
178  IControlMode *ctrlMode = nullptr;
179  IImpedanceControl *imp = nullptr;
180  ITorqueControl *tqs = nullptr;
181 
182  bool ok = true;
183  ok = ok & _dd->view(encs);
184  ok = ok & _dd->view(ctrlMode);
185  ok = ok & _dd->view(imp);
186  ok = ok & _dd->view(tqs);
187  if (!ok)
188  {
189  yError("One or more devices has not been viewed\nreturning...");
190  return false;
191  }
192 
193  return true;
194  }
195 
196  bool configure(ResourceFinder &rf) override
197  {
198  string fwdSlash = "/";
199 
200  string name;
201  name = "gravityCompensator";
202 
203  if (rf.check("period"))
204  rate = rf.find("period").asInt32();
205  else rate = 20;
206 
207  if (rf.check("rate"))
208  {
209  yError ("'rate' parameter is deprecated. Use 'period' instead");
210  return false;
211  }
212 
213  //-----------------GET THE ROBOT NAME-------------------//
214  string robot_name;
215  if (rf.check("robot"))
216  robot_name = rf.find("robot").asString();
217  else robot_name = "icub";
218 
219  //------------SPECIAL PARAM TP DEFINE THE HEAD TYPE-----//
220  version_tag icub_type;
221  if (rf.check("headV2"))
222  {
223  yInfo("'headV2' option found. Using icubV2 head kinematics.\n");
224  icub_type.head_version = 2;
225  }
226 
227  if (rf.check("headV2.6"))
228  {
229  yInfo("'headV2.6' option found. Using icubV2.6 head kinematics.\n");
230  icub_type.head_version = 2;
231  icub_type.head_subversion = 6;
232  }
233 
234  if (rf.check("headV2.7"))
235  {
236  yInfo("'headV2.7' option found. Using icubV2.7 head kinematics.\n");
237  icub_type.head_version = 2;
238  icub_type.head_subversion = 7;
239  }
240 
241  //------------------CHECK IF LEGS ARE ENABLED-----------//
242  if (rf.check("no_legs"))
243  {
244  legs_enabled= false;
245  yInfo("'no_legs' option found. Legs will be disabled.\n");
246  }
247  //------------------CHECK IF ARMS ARE ENABLED-----------//
248  if (rf.check("no_left_arm"))
249  {
250  left_arm_enabled= false;
251  yInfo("'no_left_arm' option found. Left arm will be disabled.\n");
252  }
253  //------------------CHECK IF ARMS ARE ENABLED-----------//
254  if (rf.check("no_right_arm"))
255  {
256  right_arm_enabled= false;
257  yInfo("'no_right_arm' option found. Right arm will be disabled.\n");
258  }
259  //------------------CHECK IF TORSO IS ENABLED-----------//
260  if (rf.check("no_torso_legs"))
261  {
262  torso_enabled= false;
263  legs_enabled= false;
264  yInfo("no_torso_legs' option found. Torso and legs will be disabled.\n");
265  }
266  if (rf.check("no_torso"))
267  {
268  torso_enabled= false;
269  yInfo("'no_torso' option found. Torso will be disabled.\n");
270  }
271  //------------------CHECK IF HEAD IS ENABLED-----------//
272  if (rf.check("no_head"))
273  {
274  head_enabled= false;
275  yInfo("'no_head' option found. Head will be disabled.\n");
276  }
277  //---------------------DEVICES--------------------------//
278  if (head_enabled)
279  {
280  OptionsHead.put("device","remote_controlboard");
281  OptionsHead.put("local","/gravityCompensator/head/client");
282  OptionsHead.put("remote","/"+robot_name+"/head");
283 
284  if (!createDriver(dd_head, OptionsHead))
285  {
286  yError("unable to create head device driver...quitting\n");
287  return false;
288  }
289  }
290 
291  if (left_arm_enabled)
292  {
293  OptionsLeftArm.put("device","remote_controlboard");
294  OptionsLeftArm.put("local","/gravityCompensator/left_arm/client");
295  OptionsLeftArm.put("remote","/"+robot_name+"/left_arm");
296  if (!createDriver(dd_left_arm,OptionsLeftArm))
297  {
298  yError("unable to create left arm device driver...quitting\n");
299  return false;
300  }
301  }
302 
303  if (right_arm_enabled)
304  {
305  OptionsRightArm.put("device","remote_controlboard");
306  OptionsRightArm.put("local","/gravityCompensator/right_arm/client");
307  OptionsRightArm.put("remote","/"+robot_name+"/right_arm");
308  if (!createDriver(dd_right_arm,OptionsRightArm))
309  {
310  yError("unable to create right arm device driver...quitting\n");
311  return false;
312  }
313  }
314 
315  if (legs_enabled)
316  {
317  OptionsLeftLeg.put("device","remote_controlboard");
318  OptionsLeftLeg.put("local","/gravityCompensator/left_leg/client");
319  OptionsLeftLeg.put("remote","/"+robot_name+"/left_leg");
320  if (!createDriver(dd_left_leg,OptionsLeftLeg))
321  {
322  yError("unable to create left leg device driver...quitting\n");
323  return false;
324  }
325 
326  OptionsRightLeg.put("device","remote_controlboard");
327  OptionsRightLeg.put("local","/gravityCompensator/right_leg/client");
328  OptionsRightLeg.put("remote","/"+robot_name+"/right_leg");
329  if (!createDriver(dd_right_leg,OptionsRightLeg))
330  {
331  yError("unable to create right leg device driver...quitting\n");
332  return false;
333  }
334  }
335 
336  if (torso_enabled)
337  {
338  OptionsTorso.put("device","remote_controlboard");
339  OptionsTorso.put("local","/gravityCompensator/torso/client");
340  OptionsTorso.put("remote","/"+robot_name+"/torso");
341 
342  if (!createDriver(dd_torso,OptionsTorso))
343  {
344  yError("unable to create torso device driver...quitting\n");
345  return false;
346  }
347  }
348 
349  yInfo("device driver created\n");
350 
351  rpcPort.open("/"+name+"/rpc");
352  attach(rpcPort);
353 
354  //------------------CHECK FOR WHOLEBODYNAME -----------//
355  std::string wholeBodyName = "wholeBodyDynamics";
356  if (rf.check("wholebody_name"))
357  {
358  wholeBodyName = rf.find("wholebody_name").asString();
359  yInfo("'wholeBodyName' option found. Using /%s prefix for connections.\n", wholeBodyName.c_str());
360  }
361 
362  //------------------CHECK FOR INERTIAL -----------//
363  bool inertial_enabled=true;
364  if (rf.check("no_inertial"))
365  {
366  inertial_enabled=false;
367  yInfo("'no_inertial' option found. Disabling inertial measurment.\n");
368  }
369 
370  //--------------------------THREAD--------------------------
371 
372  g_comp = new gravityCompensatorThread(wholeBodyName, rate, dd_left_arm, dd_right_arm, dd_head, dd_left_leg, dd_right_leg, dd_torso, icub_type, inertial_enabled);
373  yInfo("ft thread istantiated...\n");
374  g_comp->start();
375  yInfo("thread started\n");
376 
377  //------------------CHECK CONTROL OPTIONS -----------//
378  if (rf.check("gravity_on"))
379  {
380  g_comp->gravity_mode = GRAVITY_COMPENSATION_ON;
381  }
382  if (rf.check("gravity_off"))
383  {
384  g_comp->gravity_mode = GRAVITY_COMPENSATION_OFF;
385  }
386  if (rf.check("external_on"))
387  {
388  g_comp->external_mode = EXTERNAL_TRQ_ON;
389  }
390  if (rf.check("external_on"))
391  {
392  g_comp->external_mode = EXTERNAL_TRQ_OFF;
393  }
394  if (g_comp->gravity_mode== GRAVITY_COMPENSATION_ON) yInfo("gravity compensation on");
395  else if (g_comp->gravity_mode== GRAVITY_COMPENSATION_OFF) yInfo("gravity compensation off");
396  if (g_comp->external_mode==EXTERNAL_TRQ_ON) yInfo("external input on");
397  else if (g_comp->external_mode==EXTERNAL_TRQ_OFF) yInfo("external input off");
398 
399  return true;
400  }
401 
402  bool respond(const Bottle& command, Bottle& reply) override
403  {
404  reply.addVocab32("many");
405  reply.addString("Available commands:");
406  reply.addString("calib all");
407 
408  Bottle position_bot;
409  string helpMessage = getName() +
410  " commands are: \n" +
411  "help to display this message\n" +
412  "gravity_on to enabl e the gravity compensation \n" +
413  "gravity_off to disbale the gravity compensation \n" +
414  "external_on to enable the external input torque \n" +
415  "external_off to disable the external input torque \n";
416 
417  reply.clear();
418  if (command.get(0).asString()=="help")
419  {
420  cout << helpMessage;
421  reply.addVocab32("many");
422  reply.addString(" commands are: \n" );
423  reply.addString(helpMessage.c_str());
424  }
425  else if (command.get(0).asString()=="gravity_on")
426  {
427  if (g_comp)
428  {
429  g_comp->gravity_mode = GRAVITY_COMPENSATION_ON;
430  reply.addString("gravity compensation on");
431  }
432  }
433  else if (command.get(0).asString()=="gravity_off")
434  {
435  if (g_comp)
436  {
437  g_comp->gravity_mode = GRAVITY_COMPENSATION_OFF;
438  reply.addString("gravity compensation off");
439  }
440  }
441  else if (command.get(0).asString()=="external_on")
442  {
443  if (g_comp)
444  {
445  g_comp->external_mode = EXTERNAL_TRQ_ON;
446  reply.addString("external input on");
447  }
448  }
449  else if (command.get(0).asString()=="external_off")
450  {
451  if (g_comp)
452  {
453  g_comp->external_mode = EXTERNAL_TRQ_OFF;
454  reply.addString("external input off");
455  }
456  }
457  else
458  {
459  reply.addString("unknown command. type help.");
460  }
461 
462  return true;
463  }
464 
465 
466  bool close() override
467  {
468  //stop thread
469  if(g_comp)
470  {
471  g_comp->stop();
472  delete g_comp; g_comp = nullptr;
473  }
474 
475  //closing interfaces
476  if (dd_left_arm) {delete dd_left_arm; dd_left_arm=nullptr; }
477  if (dd_right_arm) {delete dd_right_arm; dd_right_arm=nullptr; }
478  if (dd_left_leg) {delete dd_left_leg; dd_left_leg=nullptr; }
479  if (dd_right_leg) {delete dd_right_leg; dd_right_leg=nullptr; }
480  if (dd_head) {delete dd_head; dd_head=nullptr; }
481  if (dd_torso) {delete dd_torso; dd_torso=nullptr; }
482 
483  //closing ports
484  rpcPort.interrupt();
485  rpcPort.close();
486 
487  return true;
488  }
489 
490  double getPeriod() override { return 1.0; }
491  bool updateModule() override
492  {
493  static unsigned long int alive_counter = 0;
494  static double curr_time = Time::now();
495  if (Time::now() - curr_time > 60)
496  {
497  yInfo ("gravityCompensator is alive! running for %ld mins.\n",++alive_counter);
498  curr_time = Time::now();
499  }
500 
501  if (g_comp==nullptr) return false;
502 
503  thread_status_enum thread_status = g_comp->getThreadStatus();
504 
505  if (thread_status==STATUS_OK)
506  return true;
507  else if (thread_status==STATUS_DISCONNECTED)
508  {
509  yError("gravityCompensator module lost connection with iCubInterface, now closing...\n");
510  return false;
511  }
512  else
513  {
514  yInfo("gravityCompensator module was closed successfully! \n");
515  return true;
516  }
517  }
518 };
519 
520 
521 int main(int argc, char * argv[])
522 {
523  ResourceFinder rf;
524  rf.configure(argc,argv);
525 
526  if (rf.check("help"))
527  {
528  yInfo() << "Options:";
529  yInfo() << "--context context: where to find the called resource (referred to $ICUB_ROOT/app:)";
530  yInfo() << "--from from: the name of the file.ini to be used for calibration";
531  yInfo() << "--rate rate: the period used by the module. default 100ms (not less than 15ms)";
532  yInfo() << "--no_legs this option disables the gravity compensation for the legs joints" ;
533  yInfo() << "--headV2 use the model of the headV2";
534  yInfo() << "--headV2.6 use the model of the headV2.6";
535  yInfo() << "--headV2.7 use the model of the headV2.7";
536  yInfo() << "--no_left_arm disables the left arm";
537  yInfo() << "--no_right_arm disables the right arm";
538  yInfo() << "--no_legs disables the legs";
539  yInfo() << "--no_left_arm disables the left arm";
540  yInfo() << "--no_torso disables the torso";
541  yInfo() << "--no_torso_legs disables the torso and the legs";
542  yInfo() << "--no_head disables the head";
543  yInfo() << "--wholebody_name the wholeBodyDyanmics port prefix (e.g. 'wholeBodyDynamics' / 'wholeBodyDynamicsTree')";
544  yInfo() << "--no_inertial disables the inertial";
545  yInfo() << "--gravity_on enables gravity compensation (default)";
546  yInfo() << "--gravity_off disables gravity compensation";
547  yInfo() << "--external_on enables external torque command (default)";
548  yInfo() << "--external_off disables external torque command";
549  return 0;
550  }
551 
552  Network yarp;
553 
554  if (!yarp.checkNetwork())
555  return 1;
556 
558 
559  return gcomp.runModule(rf);
560 }
561 
562 
gravityModuleCompensator::updateModule
bool updateModule() override
Definition: main.cpp:491
gravityThread.h
iCub::iDyn::version_tag::head_subversion
int head_subversion
Definition: iDynBody.h:111
STATUS_DISCONNECTED
@ STATUS_DISCONNECTED
Definition: gravityThread.h:41
GRAVITY_COMPENSATION_ON
@ GRAVITY_COMPENSATION_ON
Definition: gravityThread.h:42
EXTERNAL_TRQ_ON
@ EXTERNAL_TRQ_ON
Definition: gravityThread.h:43
gravityModuleCompensator::configure
bool configure(ResourceFinder &rf) override
Definition: main.cpp:196
iDyn.h
EXTERNAL_TRQ_OFF
@ EXTERNAL_TRQ_OFF
Definition: gravityThread.h:43
main
int main(int argc, char *argv[])
Definition: main.cpp:31
thread_status_enum
thread_status_enum
Definition: gravityThread.h:41
yarp::dev
Definition: DebugInterfaces.h:52
iCub::iDyn::version_tag::head_version
int head_version
Definition: iDynBody.h:110
gravityModuleCompensator::gravityModuleCompensator
gravityModuleCompensator()
Definition: main.cpp:124
iCub::iDyn
Definition: iDyn.h:81
gravityModuleCompensator::respond
bool respond(const Bottle &command, Bottle &reply) override
Definition: main.cpp:402
iCub::ctrl
Definition: adaptWinPolyEstimator.h:37
state::ok
@ ok
STATUS_OK
@ STATUS_OK
Definition: gravityThread.h:41
string
string(REPLACE "-rdynamic" "" CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS}") include_directories($
Definition: CMakeLists.txt:9
iCub::iDyn::version_tag
Definition: iDynBody.h:108
iDynBody.h
gravityModuleCompensator::close
bool close() override
Definition: main.cpp:466
yarp
Copyright (C) 2008 RobotCub Consortium.
Definition: DebugInterfaces.h:51
python-motor-control.encs
encs
Definition: python-motor-control.py:36
gravityModuleCompensator::getPeriod
double getPeriod() override
Definition: main.cpp:490
scripting.argc
argc
Definition: scripting.py:184
gravityModuleCompensator::createDriver
virtual bool createDriver(PolyDriver *&_dd, Property options)
Definition: main.cpp:141
gravityModuleCompensator
Definition: main.cpp:93
gravityCompensatorThread
Definition: gravityThread.h:45
GRAVITY_COMPENSATION_OFF
@ GRAVITY_COMPENSATION_OFF
Definition: gravityThread.h:42