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