iCub-main
Loading...
Searching...
No Matches
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
118using namespace yarp::os;
119using namespace yarp::sig;
120using namespace yarp::math;
121using namespace yarp::dev;
122using namespace iCub::ctrl;
123using namespace iCub::iDyn;
124using namespace std;
125
126
127class dataFilter : public PeriodicThread
128{
129private:
130 BufferedPort<Vector> &port_filtered_output;
131 Vector g;
132 IThreeAxisGyroscopes* m_iGyro{nullptr};
133 IThreeAxisLinearAccelerometers* m_iAcc{nullptr};
134
135public:
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
188class wholeBodyDynamics: public RFModule
189{
190private:
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
235public:
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
870int 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
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
@ STATUS_OK
@ STATUS_DISCONNECTED
int main()
Definition main.cpp:67
Copyright (C) 2008 RobotCub Consortium.
double lpf_ord1_3hz(double input, int j)
calib_enum
@ CALIB_LEGS
@ CALIB_ARMS
@ CALIB_FEET
@ CALIB_ALL