iCub-main
main.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2011 RobotCub Consortium, European Commission FP6 Project IST-004370
3  * Author: Marco Randazzo, Matteo Fumagalli
4  * email: marco.randazzo@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 
104 #include <yarp/os/all.h>
105 #include <yarp/sig/all.h>
106 #include <yarp/dev/all.h>
107 #include <iCub/ctrl/math.h>
109 #include <iCub/iDyn/iDyn.h>
110 #include <iCub/iDyn/iDynBody.h>
111 
112 #include <iostream>
113 #include <iomanip>
114 #include <cstring>
115 
116 #include "observerThread.h"
117 
118 using namespace yarp::os;
119 using namespace yarp::sig;
120 using namespace yarp::math;
121 using namespace yarp::dev;
122 using namespace iCub::ctrl;
123 using namespace iCub::iDyn;
124 using namespace std;
125 
126 
127 class dataFilter : public PeriodicThread
128 {
129 private:
130  BufferedPort<Vector> &port_filtered_output;
131  Vector g;
132  IThreeAxisGyroscopes* m_iGyro{nullptr};
133  IThreeAxisLinearAccelerometers* m_iAcc{nullptr};
134 
135 public:
136  dataFilter(BufferedPort<Vector> &_port_filtered_output,
137  IThreeAxisGyroscopes* iGyro,
138  IThreeAxisLinearAccelerometers* iAcc): PeriodicThread(0.01),
139  port_filtered_output(_port_filtered_output),
140  m_iGyro(iGyro),
141  m_iAcc(iAcc)
142  {
143  g.resize(6);
144  }
145 
146  void run() override
147  {
148  if (!m_iGyro || !m_iAcc || port_filtered_output.isClosed()) {
149  yError()<<"dataFilter: something went wrong during configuration closing";
150  this->askToStop();
151  return;
152  }
153 
154  double acc_ts{0.0}, gyro_ts{0.0};
155  Vector acc, gyro;
156  bool ok = true;
157  ok &= m_iAcc->getThreeAxisLinearAccelerometerMeasure(0, acc, acc_ts);
158  ok &= m_iGyro->getThreeAxisGyroscopeMeasure(0, gyro, gyro_ts);
159  if (!ok) {
160  yError()<<"dataFilter: error while reading from inertial sensor";
161  return;
162  }
163  static Stamp info;
164  info.update(gyro_ts);
165  Vector temp(6,0.0);
166  temp.setSubvector(0,acc);
167  temp.setSubvector(3,gyro);
168  for(size_t i=0;i<temp.size();i++)
169  {
170  temp(i) = lpf_ord1_3hz(temp(i), i);
171  }
172  g[0] = temp[0]; // acc
173  g[1] = temp[1]; // acc
174  g[2] = temp[2]; // acc
175  g[3] = temp[3]; // gyro
176  g[4] = temp[4]; // gyro
177  g[5] = temp[5]; // gyro
178  //g = (9.81/norm(g))*g;
179 
180  port_filtered_output.prepare() = g;
181  port_filtered_output.setEnvelope(info);
182  port_filtered_output.write();
183  }
184 
185 };
186 
187 // The main module
188 class wholeBodyDynamics: public RFModule
189 {
190 private:
191  Property OptionsLeftArm;
192  Property OptionsRightArm;
193  Property OptionsHead;
194  Property OptionsLeftLeg;
195  Property OptionsRightLeg;
196  Property OptionsTorso;
197  bool legs_enabled;
198  bool torso_enabled;
199  bool com_enabled;
200  bool w0_dw0_enabled;
201  bool com_vel_enabled;
202  bool left_arm_enabled;
203  bool right_arm_enabled;
204  bool head_enabled;
205  bool dummy_ft;
206  bool dump_vel_enabled;
207  bool auto_drift_comp;
208  bool default_ee_cont; // true: when skin detects no contact, the ext contact is supposed at the end effector
209  // false: ext contact is supposed at the last location where skin detected a contact
210 
211  dataFilter *inertialFilter{};
212  BufferedPort<Vector> port_filtered_output;
213  Port rpcPort;
214 
215  inverseDynamics *inv_dyn;
216  IThreeAxisLinearAccelerometers* m_iAcc{nullptr};
217  IThreeAxisGyroscopes* m_iGyro{nullptr};
218  ISixAxisForceTorqueSensors* m_left_arm_FT{nullptr};
219  ISixAxisForceTorqueSensors* m_right_arm_FT{nullptr};
220  ISixAxisForceTorqueSensors* m_left_leg_FT{nullptr};
221  ISixAxisForceTorqueSensors* m_right_leg_FT{nullptr};
222 
223  PolyDriver *dd_left_arm;
224  PolyDriver *dd_right_arm;
225  PolyDriver *dd_head;
226  PolyDriver *dd_left_leg;
227  PolyDriver *dd_right_leg;
228  PolyDriver *dd_torso;
229  PolyDriver dd_head_inertial_MASClient;
230  PolyDriver dd_left_arm_FT_MASClient;
231  PolyDriver dd_right_arm_FT_MASClient;
232  PolyDriver dd_left_leg_FT_MASClient;
233  PolyDriver dd_right_leg_FT_MASClient;
234 
235 public:
237  {
238  inv_dyn=nullptr;
239  dd_left_arm=nullptr;
240  dd_right_arm=nullptr;
241  dd_head=nullptr;
242  dd_left_leg=nullptr;
243  dd_right_leg=nullptr;
244  dd_torso=nullptr;
245  com_vel_enabled=false;
246  com_enabled=true;
247  legs_enabled = true;
248  torso_enabled = true;
249  left_arm_enabled = true;
250  right_arm_enabled = true;
251  head_enabled = true;
252  w0_dw0_enabled = false;
253  dummy_ft = false;
254  dump_vel_enabled = false;
255  auto_drift_comp = false;
256  default_ee_cont = false;
257  }
258 
259  virtual bool createDriver(PolyDriver *&_dd, Property options)
260  {
261  int trials=0;
262  double start_time = yarp::os::Time::now();
263 
264  do
265  {
266  double current_time = yarp::os::Time::now();
267 
268  //remove previously existing drivers
269  if (_dd)
270  {
271  delete _dd;
272  _dd=0;
273  }
274 
275  //creates the new device driver
276  _dd = new PolyDriver(options);
277  bool connected =_dd->isValid();
278 
279  //check if the driver is connected
280  if (connected) break;
281 
282  //check if the timeout (60s) is expired
283  if (current_time-start_time > 60.0)
284  {
285  yError("It is not possible to instantiate the device driver. I tried %d times!\n", trials);
286  return false;
287  }
288 
289  yarp::os::Time::delay(5);
290  trials++;
291  yWarning("\nUnable to connect the device driver, trying again...\n");
292  }
293  while (true);
294 
295  IEncoders *encs;
296 
297  bool ok = true;
298  ok = ok & _dd->view(encs);
299  if (!ok)
300  {
301  yError("one or more devices has not been viewed\nreturning...");
302  return false;
303  }
304 
305  return true;
306  }
307 
308  bool respond(const Bottle& command, Bottle& reply) override
309  {
310  reply.clear();
311 
312  if (command.get(0).isInt32())
313  {
314  if (command.get(0).asInt32()==0)
315  {
316  yInfo("Asking recalibration...\n");
317  if (inv_dyn)
318  {
319  inv_dyn->suspend();
320  inv_dyn->calibrateOffset();
321  inv_dyn->resume();
322  }
323  yInfo("Recalibration complete.\n");
324  reply.addString("Recalibrated");
325  return true;
326  }
327  }
328 
329  if (command.get(0).isString())
330  {
331  if (command.get(0).asString()=="help")
332  {
333  reply.addVocab32("many");
334  reply.addString("Available commands:");
335  reply.addString("calib all");
336  reply.addString("calib arms");
337  reply.addString("calib legs");
338  reply.addString("calib feet");
339  return true;
340  }
341  else if (command.get(0).asString()=="calib")
342  {
343  yInfo("Asking recalibration...\n");
344  if (inv_dyn)
345  {
346  calib_enum calib_code=CALIB_ALL;
347  if (command.get(1).asString()=="all") calib_code=CALIB_ALL;
348  else if (command.get(1).asString()=="arms") calib_code=CALIB_ARMS;
349  else if (command.get(1).asString()=="legs") calib_code=CALIB_LEGS;
350  else if (command.get(1).asString()=="feet") calib_code=CALIB_FEET;
351 
352  inv_dyn->suspend();
353  inv_dyn->calibrateOffset(calib_code);
354  inv_dyn->resume();
355  }
356  yInfo("Recalibration complete.\n");
357  reply.addString("Recalibrated");
358  return true;
359  }
360  }
361 
362  reply.addString("Unknown command");
363  return true;
364  }
365 
366  bool configure(ResourceFinder &rf) override
367  {
368  //---------------------LOCAL NAME-----------------------//
369  string local_name = "wholeBodyDynamics";
370  if (rf.check("local"))
371  {
372  local_name=rf.find("local").asString();
373  }
374 
375 
376  //-----------------GET THE ROBOT NAME-------------------//
377  string robot_name;
378  if (rf.check("robot"))
379  robot_name = rf.find("robot").asString();
380  else robot_name = "icub";
381 
382  //------------SPECIAL PARAM TP DEFINE THE HEAD TYPE-----//
383  version_tag icub_type;
384 
385  icub_type.head_version = 1;
386  icub_type.legs_version = 1;
387 
388  if (rf.check("headV2"))
389  {
390  yInfo("'headV2' option found. Using icubV2 head kinematics.\n");
391  icub_type.head_version = 2;
392  }
393 
394  if (rf.check("headV2.6"))
395  {
396  yInfo("'headV2.6' option found. Using icubV2.6 head kinematics.\n");
397  icub_type.head_version = 2;
398  icub_type.head_subversion = 6;
399  }
400 
401  if (rf.check("headV2.7"))
402  {
403  yInfo("'headV2.7' option found. Using icubV2.7 head kinematics.\n");
404  icub_type.head_version = 2;
405  icub_type.head_subversion = 7;
406  }
407 
408  //----------SPECIAL PARAM TO DEFINE LEGS VERSION--------//
409  if(rf.check("legsV2"))
410  {
411  yInfo("'legsV2' option found. Using legsV2 kinematics. \n");
412  icub_type.legs_version = 2;
413  }
414 
415  //-----------------CHECK IF AUTOCONNECT IS ON-----------//
416  bool autoconnect;
417  if (rf.check("autoconnect"))
418  {
419  yInfo("'autoconnect' option enabled.\n");
420  autoconnect = true;
421  }
422  else
423  {
424  autoconnect = false;
425  }
426 
427  //------------CHECK IF COM COMPUTATION IS ENABLED-----------//
428  if (rf.check("no_com"))
429  {
430  com_enabled= false;
431  yInfo("'no_com' option found. COM computation will be disabled.\n");
432  }
433 
434  //------------DEBUG ONLY-----------//
435  if (rf.check("disable_w0_dw0"))
436  {
437  w0_dw0_enabled= false;
438  yInfo("'disable_w0_dw0' option found. w0 and dw0 will be set to zero.\n");
439  }
440  //------------DEBUG ONLY-----------//
441  if (rf.check("enable_w0_dw0"))
442  {
443  w0_dw0_enabled= true;
444  yInfo("'enable_w0_dw0' option found. w0 and dw0 will be used.\n");
445  }
446 
447  //------------CHECK IF COM VELOCITY COMPUTATION IS ENABLED-----------//
448  if (rf.check("experimental_com_vel"))
449  {
450  com_vel_enabled= true;
451  yInfo("'enable_com_vel' option found. Extra COM velocity computation will be enabled.\n");
452  }
453 
454  //------------------CHECK IF LEGS ARE ENABLED-----------//
455  if (rf.check("no_legs"))
456  {
457  legs_enabled= false;
458  yInfo("'no_legs' option found. Legs will be disabled.\n");
459  }
460 
461  //------------------CHECK IF TORSO IS ENABLED-----------//
462  if (rf.check("no_torso_legs"))
463  {
464  torso_enabled= false;
465  legs_enabled= false;
466  yInfo("no_torso_legs' option found. Torso and legs will be disabled.\n");
467  }
468  if (rf.check("no_torso"))
469  {
470  torso_enabled= false;
471  yInfo("'no_torso' option found. Torso will be disabled.\n");
472  }
473 
474 
475  //------------------CHECK IF HEAD IS ENABLED-----------//
476  if (rf.check("no_head"))
477  {
478  head_enabled= false;
479  yInfo("'no_head' option found. Head will be disabled.\n");
480  }
481 
482  //------------------CHECK IF ARMS ARE ENABLED-----------//
483  if (rf.check("no_left_arm"))
484  {
485  left_arm_enabled= false;
486  yInfo("'no_left_arm' option found. Left arm will be disabled.\n");
487  }
488  if (rf.check("no_right_arm"))
489  {
490  right_arm_enabled= false;
491  yInfo("'no_right_arm' option found. Right arm will be disabled.\n");
492  }
493 
494  //---------------------RATE/PERIOD-----------------------------//
495  int rate = 10;
496  if (rf.check("period"))
497  {
498  rate = rf.find("period").asInt32();
499  yInfo("rateThread working at %d ms\n", rate);
500  }
501  else
502  {
503  yInfo("Could not find period in the config file\nusing 10ms as default");
504  rate = 10;
505  }
506 
507  if (rf.check("rate"))
508  {
509  yError ("'rate' parameter is deprecated. Use 'period' instead");
510  return false;
511  }
512  std::string remoteInertialName{"/"+robot_name+"/head/inertials"};
513  if (rf.check("imuPortName"))
514  {
515  remoteInertialName = rf.find("imuPortName").asString();
516  }
517 
518  //---------------------DUMMY_FT-------------------------//
519  if (rf.check("dummy_ft"))
520  {
521  dummy_ft = true;
522  yInfo("Using dummy FT sensors (debug mode)\n");
523  }
524 
525  //---------------------DUMP-VEL-------------------------//
526  if (rf.check("dumpvel"))
527  {
528  dump_vel_enabled = true;
529  yInfo("Dumping joint velocities and accelerations (debug mode)\n");
530  }
531 
532  if (rf.check("auto_drift_comp"))
533  {
534  auto_drift_comp = true;
535  yInfo("Enabling automatic drift compensation (experimental)\n");
536  }
537 
538  if (rf.check("default_ee_cont"))
539  {
540  default_ee_cont = true;
541  yInfo("Default contact at the end effector\n");
542  }
543 
544  //---------------------DEVICES--------------------------//
545  if(head_enabled)
546  {
547  OptionsHead.put("device","remote_controlboard");
548  OptionsHead.put("local","/"+local_name+"/head/client");
549  OptionsHead.put("remote","/"+robot_name+"/head");
550 
551  if (!createDriver(dd_head, OptionsHead))
552  {
553  yError("unable to create head device driver...quitting\n");
554  return false;
555  }
556  else
557  yInfo("device driver created\n");
558  }
559 
560  if (left_arm_enabled)
561  {
562  if (!dummy_ft) {
563  Property mas_left_arm_FT_conf {{"device",Value("multipleanalogsensorsclient")},
564  {"local", Value("/"+local_name+"/left_arm/FT")},
565  {"remote",Value("/"+robot_name+"/left_arm/FT")},
566  {"timeout",Value(0.1)}};
567 
568  if (!dd_left_arm_FT_MASClient.open(mas_left_arm_FT_conf))
569  {
570  yError("Unable to open the left_arm FT MAS client...quitting\n");
571  return false;
572  }
573 
574 
575  if (!dd_left_arm_FT_MASClient.view(m_left_arm_FT))
576  {
577  yError("View of one of the MAS interfaces for left_arm required failed...quitting\n");
578  return false;
579  }
580  }
581 
582  OptionsLeftArm.put("device","remote_controlboard");
583  OptionsLeftArm.put("local","/"+local_name+"/left_arm/client");
584  OptionsLeftArm.put("remote","/"+robot_name+"/left_arm");
585 
586  if (!createDriver(dd_left_arm, OptionsLeftArm))
587  {
588  yError("unable to create left arm device driver...quitting\n");
589  return false;
590  }
591  }
592 
593  if (right_arm_enabled)
594  {
595  if (!dummy_ft) {
596  Property mas_right_arm_FT_conf {{"device",Value("multipleanalogsensorsclient")},
597  {"local", Value("/"+local_name+"/right_arm/FT")},
598  {"remote",Value("/"+robot_name+"/right_arm/FT")},
599  {"timeout",Value(0.1)}};
600 
601  if (!dd_right_arm_FT_MASClient.open(mas_right_arm_FT_conf))
602  {
603  yError("Unable to open the right_arm FT MAS client...quitting\n");
604  return false;
605  }
606 
607  if (!dd_right_arm_FT_MASClient.view(m_right_arm_FT))
608  {
609  yError("View of one of the MAS interfaces for right_arm required failed...quitting\n");
610  return false;
611  }
612  }
613  OptionsRightArm.put("device","remote_controlboard");
614  OptionsRightArm.put("local","/"+local_name+"/right_arm/client");
615  OptionsRightArm.put("remote","/"+robot_name+"/right_arm");
616 
617  if (!createDriver(dd_right_arm, OptionsRightArm))
618  {
619  yError("unable to create right arm device driver...quitting\n");
620  return false;
621  }
622  }
623 
624  if (legs_enabled)
625  {
626  if (!dummy_ft) {
627  Property mas_left_leg_FT_conf {{"device",Value("multipleanalogsensorsclient")},
628  {"local", Value("/"+local_name+"/left_leg/FT")},
629  {"remote",Value("/"+robot_name+"/left_leg/FT")},
630  {"timeout",Value(0.1)}};
631 
632  if (!dd_left_leg_FT_MASClient.open(mas_left_leg_FT_conf))
633  {
634  yError("Unable to open the left_leg FT MAS client...quitting\n");
635  return false;
636  }
637 
638  if (!dd_left_leg_FT_MASClient.view(m_left_leg_FT))
639  {
640  yError("View of one of the MAS interfaces for left_leg required failed...quitting\n");
641  return false;
642  }
643  Property mas_right_leg_FT_conf {{"device",Value("multipleanalogsensorsclient")},
644  {"local", Value("/"+local_name+"/right_leg/FT")},
645  {"remote",Value("/"+robot_name+"/right_leg/FT")},
646  {"timeout",Value(0.1)}};
647 
648  if (!dd_right_leg_FT_MASClient.open(mas_right_leg_FT_conf))
649  {
650  yError("Unable to open the right_leg FT MAS client...quitting\n");
651  return false;
652  }
653 
654  if (!dd_right_leg_FT_MASClient.view(m_right_leg_FT))
655  {
656  yError("View of one of the MAS interfaces for right_leg required failed...quitting\n");
657  return false;
658  }
659  }
660  OptionsLeftLeg.put("device","remote_controlboard");
661  OptionsLeftLeg.put("local","/"+local_name+"/left_leg/client");
662  OptionsLeftLeg.put("remote","/"+robot_name+"/left_leg");
663 
664  if (!createDriver(dd_left_leg, OptionsLeftLeg))
665  {
666  yError("unable to create left leg device driver...quitting\n");
667  return false;
668  }
669 
670  OptionsRightLeg.put("device","remote_controlboard");
671  OptionsRightLeg.put("local","/"+local_name+"/right_leg/client");
672  OptionsRightLeg.put("remote","/"+robot_name+"/right_leg");
673 
674  if (!createDriver(dd_right_leg, OptionsRightLeg))
675  {
676  yError("unable to create right leg device driver...quitting\n");
677  return false;
678  }
679  }
680 
681  if (torso_enabled)
682  {
683  OptionsTorso.put("device","remote_controlboard");
684  OptionsTorso.put("local","/"+local_name+"/torso/client");
685  OptionsTorso.put("remote","/"+robot_name+"/torso");
686 
687  if (!createDriver(dd_torso, OptionsTorso))
688  {
689  yError("unable to create head device driver...quitting\n");
690  return false;
691  }
692  else
693  yInfo("device driver created\n");
694  }
695 
696  Property mas_head_inertial_conf {{"device",Value("multipleanalogsensorsclient")},
697  {"local", Value("/"+local_name+"/inertials")},
698  {"remote",Value(remoteInertialName)},
699  {"timeout",Value(0.1)}};
700 
701  if (!dd_head_inertial_MASClient.open(mas_head_inertial_conf))
702  {
703  yError("unable to open the head inertial MAS client...quitting\n");
704  return false;
705  }
706 
707  if(!dd_head_inertial_MASClient.view(m_iAcc) || !dd_head_inertial_MASClient.view(m_iGyro))
708  {
709  yError("view of one of the MAS interfaces required failed...quitting\n");
710  return false;
711  }
712 
713  //---------------OPEN RPC PORT--------------------//
714  string rpcPortName = "/"+local_name+"/rpc:i";
715  rpcPort.open(rpcPortName);
716  attach(rpcPort);
717 
718  //---------------OPEN INERTIAL PORTS--------------------//
719  port_filtered_output.open("/"+local_name+"/filtered/inertial:o");
720  inertialFilter = new dataFilter(port_filtered_output, m_iGyro, m_iAcc);
721 
722  //--------------------------THREAD--------------------------
723  inv_dyn = new inverseDynamics(rate, dd_left_arm, dd_right_arm, dd_head, dd_left_leg, dd_right_leg, dd_torso, robot_name, local_name, icub_type, autoconnect, m_left_arm_FT, m_right_arm_FT, m_left_leg_FT, m_right_leg_FT);
724  inv_dyn->com_enabled=com_enabled;
725  inv_dyn->auto_drift_comp=auto_drift_comp;
726  inv_dyn->com_vel_enabled=com_vel_enabled;
727  inv_dyn->dummy_ft=dummy_ft;
728  inv_dyn->w0_dw0_enabled=w0_dw0_enabled;
729  inv_dyn->dumpvel_enabled=dump_vel_enabled;
730  inv_dyn->default_ee_cont=default_ee_cont;
731 
732  yInfo("ft thread istantiated...\n");
733  Time::delay(5.0);
734 
735  inertialFilter->start();
736  inv_dyn->start();
737  yInfo("thread started\n");
738  return true;
739  }
740 
741  bool close() override
742  {
743  //The order of execution of the following closures is important, do not change it.
744  yInfo("Closing wholeBodyDynamics module... \n");
745 
746  if (inv_dyn)
747  {
748  thread_status_enum thread_status = inv_dyn->getThreadStatus();
749  if (thread_status!=STATUS_DISCONNECTED)
750  {
751  yInfo("Setting the icub in stiff mode\n");
752  inv_dyn->setStiffMode();
753  }
754  yInfo("Stopping the inv_dyn thread...");
755  inv_dyn->stop();
756  yInfo("inv_dyn thread stopped\n");
757  delete inv_dyn;
758  inv_dyn=nullptr;
759  }
760 
761  if(inertialFilter)
762  {
763  yInfo("Stopping the inertial filter thread \n");
764  inertialFilter->stop();
765  delete inertialFilter;
766  inertialFilter=nullptr;
767  dd_head_inertial_MASClient.close();
768  }
769 
770  yInfo("Closing the filtered inertial output port \n");
771  port_filtered_output.interrupt();
772  port_filtered_output.close();
773 
774  yInfo("Closing the rpc port \n");
775  rpcPort.close();
776 
777  if (dd_left_arm)
778  {
779  yInfo("Closing dd_left_arm \n");
780  dd_left_arm_FT_MASClient.close();
781  dd_left_arm->close();
782  delete dd_left_arm;
783  dd_left_arm=nullptr;
784  }
785  if (dd_right_arm)
786  {
787  yInfo("Closing dd_right_arm \n");
788  dd_right_arm_FT_MASClient.close();
789  dd_right_arm->close();
790  delete dd_right_arm;
791  dd_right_arm=nullptr;
792  }
793  if (dd_head)
794  {
795  yInfo("Closing dd_head \n");
796  dd_head->close();
797  delete dd_head;
798  dd_head=nullptr;
799  }
800 
801  if (dd_left_leg)
802  {
803  yInfo("Closing dd_left_leg \n");
804  dd_left_leg_FT_MASClient.close();
805  dd_left_leg->close();
806  delete dd_left_leg;
807  dd_left_leg=nullptr;
808  }
809  if (dd_right_leg)
810  {
811  yInfo("Closing dd_right_leg \n");
812  dd_right_leg_FT_MASClient.close();
813  dd_right_leg->close();
814  delete dd_right_leg;
815  dd_right_leg=nullptr;
816  }
817  if (dd_torso)
818  {
819  yInfo("Closing dd_torso \n");
820  dd_torso->close();
821  delete dd_torso;
822  dd_torso=nullptr;
823  }
824 
825  yInfo("wholeBodyDynamics module was closed successfully! \n");
826  return true;
827  }
828 
829  double getPeriod() override
830  {
831  return 1.0;
832  }
833  bool updateModule() override
834  {
835  double avgTime, stdDev, period;
836  period = inv_dyn->getPeriod();
837  inv_dyn->getEstimatedPeriod(avgTime, stdDev);
838  if(avgTime > 1.3 * period){
839  // yDebug("(real period: %3.3f +/- %3.3f; expected period %3.3f)\n", avgTime, stdDev, period);
840  }
841 
842  static unsigned long int alive_counter = 0;
843  static double curr_time = Time::now();
844  if (Time::now() - curr_time > 60)
845  {
846  yInfo ("wholeBodyDynamics is alive! running for %ld mins.\n",++alive_counter);
847  curr_time = Time::now();
848  }
849 
850  if (inv_dyn==nullptr)
851  return false;
852  thread_status_enum thread_status = inv_dyn->getThreadStatus();
853  if (thread_status==STATUS_OK)
854  return true;
855  else if (thread_status==STATUS_DISCONNECTED)
856  {
857  yError ("wholeBodyDynamics module lost connection with iCubInterface, now closing...\n");
858  return false;
859  }
860  else
861  {
862  yInfo("wholeBodyDynamics module was closed successfully! \n");
863  return true;
864  }
865 
866  }
867 };
868 
869 
870 int main(int argc, char * argv[])
871 {
872  ResourceFinder rf;
873  rf.setDefaultContext("wholeBodyDynamics");
874  rf.setDefaultConfigFile("wholeBodyDynamics.ini");
875  rf.configure(argc,argv);
876 
877  if (rf.check("help"))
878  {
879  cout << "Options:" << endl << endl;
880  cout << "\t--context context: where to find the called resource (referred to $ICUB_ROOT/app: default wholeBodyDynamics)" << endl;
881  cout << "\t--from from: the name of the file.ini to be used for calibration" << endl;
882  cout << "\t--period period: the period used by the module. default: 10ms" << endl;
883  cout << "\t--robot robot: the robot name. default: iCub" << endl;
884  cout << "\t--local name: the prefix of the ports opened by the module. defualt: wholeBodyDynamics" << endl;
885  cout << "\t--autoconnect automatically connects the module ports to iCubInterface" << endl;
886  cout << "\t--no_legs this option disables the dynamics computation for the legs joints" << endl;
887  cout << "\t--headV2 use the model of the headV2" << endl;
888  cout << "\t--headV2.6 use the model of the headV2.6" << endl;
889  cout << "\t--headV2.7 use the model of the headV2.7" << endl;
890  cout << "\t--legsV2 use the model of legsV2" << endl;
891  cout << "\t--no_left_arm disables the left arm" << endl;
892  cout << "\t--no_right_arm disables the right arm" << endl;
893  cout << "\t--no_com disables the com computation" << endl;
894  cout << "\t--dummy_ft uses fake FT sensors (debug use only)" << endl;
895  cout << "\t--dumpvel dumps joint velocities and accelerations (debug use only)" << endl;
896  cout << "\t--experimental_com_vel enables com velocity computation (experimental)" << endl;
897  cout << "\t--auto_drift_comp enables automatic drift compensation (experimental, under debug)" << endl;
898  return 0;
899  }
900 
901  Network yarp;
902 
903  if (!yarp.checkNetwork())
904  {
905  yError("Sorry YARP network does not seem to be available, is the yarp server available?\n");
906  return 1;
907  }
908 
909  wholeBodyDynamics obs;
910  return obs.runModule(rf);
911 }
912 
dataFilter(BufferedPort< Vector > &_port_filtered_output, IThreeAxisGyroscopes *iGyro, IThreeAxisLinearAccelerometers *iAcc)
Definition: main.cpp:136
void run() override
Definition: main.cpp:146
void calibrateOffset(calib_enum calib_code=CALIB_ALL)
thread_status_enum getThreadStatus()
virtual bool createDriver(PolyDriver *&_dd, Property options)
Definition: main.cpp:259
bool respond(const Bottle &command, Bottle &reply) override
Definition: main.cpp:308
double getPeriod() override
Definition: main.cpp:829
bool updateModule() override
Definition: main.cpp:833
bool close() override
Definition: main.cpp:741
bool configure(ResourceFinder &rf) override
Definition: main.cpp:366
_3f_vect_t acc
Definition: dataTypes.h:1
thread_status_enum
Definition: gravityThread.h:41
@ STATUS_OK
Definition: gravityThread.h:41
@ STATUS_DISCONNECTED
Definition: gravityThread.h:41
int main(int argc, char *argv[])
Definition: main.cpp:31
Copyright (C) 2008 RobotCub Consortium.
double lpf_ord1_3hz(double input, int j)
calib_enum
@ CALIB_LEGS
@ CALIB_ARMS
@ CALIB_FEET
@ CALIB_ALL