114 #include <yarp/os/all.h>
115 #include <yarp/sig/all.h>
116 #include <yarp/dev/all.h>
128 using namespace yarp::os;
129 using namespace yarp::sig;
130 using namespace yarp::math;
140 BufferedPort<Vector> &port_filtered_output;
142 IThreeAxisGyroscopes* m_iGyro{
nullptr};
143 IThreeAxisLinearAccelerometers* m_iAcc{
nullptr};
147 IThreeAxisGyroscopes* iGyro,
148 IThreeAxisLinearAccelerometers* iAcc): PeriodicThread(0.01),
149 port_filtered_output(_port_filtered_output),
158 if (!m_iGyro || !m_iAcc || port_filtered_output.isClosed()) {
159 yError()<<
"dataFilter: something went wrong during configuration closing";
164 double acc_ts{0.0}, gyro_ts{0.0};
167 ok &= m_iAcc->getThreeAxisLinearAccelerometerMeasure(0,
acc, acc_ts);
168 ok &= m_iGyro->getThreeAxisGyroscopeMeasure(0, gyro, gyro_ts);
170 yError()<<
"dataFilter: error while reading from inertial sensor";
174 info.update(gyro_ts);
176 temp.setSubvector(0,
acc);
177 temp.setSubvector(3,gyro);
178 for(
size_t i=0;i<temp.size();i++)
190 port_filtered_output.prepare() = g;
191 port_filtered_output.setEnvelope(
info);
192 port_filtered_output.write();
201 Property OptionsLeftArm;
202 Property OptionsRightArm;
203 Property OptionsHead;
204 Property OptionsLeftLeg;
205 Property OptionsRightLeg;
206 Property OptionsTorso;
211 bool com_vel_enabled;
212 bool left_arm_enabled;
213 bool right_arm_enabled;
216 bool dump_vel_enabled;
217 bool auto_drift_comp;
218 bool default_ee_cont;
222 BufferedPort<Vector> port_filtered_output;
226 IThreeAxisLinearAccelerometers* m_iAcc{
nullptr};
227 IThreeAxisGyroscopes* m_iGyro{
nullptr};
229 PolyDriver *dd_left_arm;
230 PolyDriver *dd_right_arm;
232 PolyDriver *dd_left_leg;
233 PolyDriver *dd_right_leg;
234 PolyDriver *dd_torso;
235 PolyDriver dd_MASClient;
242 dd_right_arm=
nullptr;
245 dd_right_leg=
nullptr;
247 com_vel_enabled=
false;
250 torso_enabled =
true;
251 left_arm_enabled =
true;
252 right_arm_enabled =
true;
254 w0_dw0_enabled =
false;
256 dump_vel_enabled =
false;
257 auto_drift_comp =
false;
258 default_ee_cont =
false;
264 double start_time = yarp::os::Time::now();
268 double current_time = yarp::os::Time::now();
278 _dd =
new PolyDriver(options);
279 bool connected =_dd->isValid();
282 if (connected)
break;
285 if (current_time-start_time > 60.0)
287 yError(
"It is not possible to instantiate the device driver. I tried %d times!\n", trials);
291 yarp::os::Time::delay(5);
293 yWarning(
"\nUnable to connect the device driver, trying again...\n");
303 yError(
"one or more devices has not been viewed\nreturning...");
310 bool respond(
const Bottle& command, Bottle& reply)
override
314 if (command.get(0).isInt32())
316 if (command.get(0).asInt32()==0)
318 yInfo(
"Asking recalibration...\n");
325 yInfo(
"Recalibration complete.\n");
326 reply.addString(
"Recalibrated");
331 if (command.get(0).isString())
333 if (command.get(0).asString()==
"help")
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");
343 else if (command.get(0).asString()==
"calib")
345 yInfo(
"Asking recalibration...\n");
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;
358 yInfo(
"Recalibration complete.\n");
359 reply.addString(
"Recalibrated");
364 reply.addString(
"Unknown command");
371 string local_name =
"wholeBodyDynamics";
372 if (rf.check(
"local"))
374 local_name=rf.find(
"local").asString();
380 if (rf.check(
"robot"))
381 robot_name = rf.find(
"robot").asString();
382 else robot_name =
"icub";
390 if (rf.check(
"headV2"))
392 yInfo(
"'headV2' option found. Using icubV2 head kinematics.\n");
396 if (rf.check(
"headV2.6"))
398 yInfo(
"'headV2.6' option found. Using icubV2.6 head kinematics.\n");
403 if (rf.check(
"headV2.7"))
405 yInfo(
"'headV2.7' option found. Using icubV2.7 head kinematics.\n");
411 if(rf.check(
"legsV2"))
413 yInfo(
"'legsV2' option found. Using legsV2 kinematics. \n");
419 if (rf.check(
"autoconnect"))
421 yInfo(
"'autoconnect' option enabled.\n");
430 if (rf.check(
"no_com"))
433 yInfo(
"'no_com' option found. COM computation will be disabled.\n");
437 if (rf.check(
"disable_w0_dw0"))
439 w0_dw0_enabled=
false;
440 yInfo(
"'disable_w0_dw0' option found. w0 and dw0 will be set to zero.\n");
443 if (rf.check(
"enable_w0_dw0"))
445 w0_dw0_enabled=
true;
446 yInfo(
"'enable_w0_dw0' option found. w0 and dw0 will be used.\n");
450 if (rf.check(
"experimental_com_vel"))
452 com_vel_enabled=
true;
453 yInfo(
"'enable_com_vel' option found. Extra COM velocity computation will be enabled.\n");
457 if (rf.check(
"no_legs"))
460 yInfo(
"'no_legs' option found. Legs will be disabled.\n");
464 if (rf.check(
"no_torso_legs"))
466 torso_enabled=
false;
468 yInfo(
"no_torso_legs' option found. Torso and legs will be disabled.\n");
470 if (rf.check(
"no_torso"))
472 torso_enabled=
false;
473 yInfo(
"'no_torso' option found. Torso will be disabled.\n");
478 if (rf.check(
"no_head"))
481 yInfo(
"'no_head' option found. Head will be disabled.\n");
485 if (rf.check(
"no_left_arm"))
487 left_arm_enabled=
false;
488 yInfo(
"'no_left_arm' option found. Left arm will be disabled.\n");
490 if (rf.check(
"no_right_arm"))
492 right_arm_enabled=
false;
493 yInfo(
"'no_right_arm' option found. Right arm will be disabled.\n");
498 if (rf.check(
"period"))
500 rate = rf.find(
"period").asInt32();
501 yInfo(
"rateThread working at %d ms\n", rate);
505 yInfo(
"Could not find period in the config file\nusing 10ms as default");
509 if (rf.check(
"rate"))
511 yError (
"'rate' parameter is deprecated. Use 'period' instead");
514 std::string remoteInertialName{
"/"+robot_name+
"/head/inertials"};
515 if (rf.check(
"imuPortName"))
517 remoteInertialName = rf.find(
"imuPortName").asString();
521 if (rf.check(
"dummy_ft"))
524 yInfo(
"Using dummy FT sensors (debug mode)\n");
528 if (rf.check(
"dumpvel"))
530 dump_vel_enabled =
true;
531 yInfo(
"Dumping joint velocities and accelerations (debug mode)\n");
534 if (rf.check(
"auto_drift_comp"))
536 auto_drift_comp =
true;
537 yInfo(
"Enabling automatic drift compensation (experimental)\n");
540 if (rf.check(
"default_ee_cont"))
542 default_ee_cont =
true;
543 yInfo(
"Default contact at the end effector\n");
549 OptionsHead.put(
"device",
"remote_controlboard");
550 OptionsHead.put(
"local",
"/"+local_name+
"/head/client");
551 OptionsHead.put(
"remote",
"/"+robot_name+
"/head");
553 if (!createDriver(dd_head, OptionsHead))
555 yError(
"unable to create head device driver...quitting\n");
559 yInfo(
"device driver created\n");
562 if (left_arm_enabled)
564 OptionsLeftArm.put(
"device",
"remote_controlboard");
565 OptionsLeftArm.put(
"local",
"/"+local_name+
"/left_arm/client");
566 OptionsLeftArm.put(
"remote",
"/"+robot_name+
"/left_arm");
568 if (!createDriver(dd_left_arm, OptionsLeftArm))
570 yError(
"unable to create left arm device driver...quitting\n");
575 if (right_arm_enabled)
577 OptionsRightArm.put(
"device",
"remote_controlboard");
578 OptionsRightArm.put(
"local",
"/"+local_name+
"/right_arm/client");
579 OptionsRightArm.put(
"remote",
"/"+robot_name+
"/right_arm");
581 if (!createDriver(dd_right_arm, OptionsRightArm))
583 yError(
"unable to create right arm device driver...quitting\n");
590 OptionsLeftLeg.put(
"device",
"remote_controlboard");
591 OptionsLeftLeg.put(
"local",
"/"+local_name+
"/left_leg/client");
592 OptionsLeftLeg.put(
"remote",
"/"+robot_name+
"/left_leg");
594 if (!createDriver(dd_left_leg, OptionsLeftLeg))
596 yError(
"unable to create left leg device driver...quitting\n");
600 OptionsRightLeg.put(
"device",
"remote_controlboard");
601 OptionsRightLeg.put(
"local",
"/"+local_name+
"/right_leg/client");
602 OptionsRightLeg.put(
"remote",
"/"+robot_name+
"/right_leg");
604 if (!createDriver(dd_right_leg, OptionsRightLeg))
606 yError(
"unable to create right leg device driver...quitting\n");
613 OptionsTorso.put(
"device",
"remote_controlboard");
614 OptionsTorso.put(
"local",
"/"+local_name+
"/torso/client");
615 OptionsTorso.put(
"remote",
"/"+robot_name+
"/torso");
617 if (!createDriver(dd_torso, OptionsTorso))
619 yError(
"unable to create head device driver...quitting\n");
623 yInfo(
"device driver created\n");
626 Property masConf {{
"device",Value(
"multipleanalogsensorsclient")},
627 {
"local", Value(
"/"+local_name+
"/inertials")},
628 {
"remote",Value(remoteInertialName)},
629 {
"timeout",Value(0.1)}};
631 if (!dd_MASClient.open(masConf))
633 yError(
"unable to open the MAS client...quitting\n");
637 if(!dd_MASClient.view(m_iAcc) || !dd_MASClient.view(m_iGyro))
639 yError(
"view of one of the MAS interfaces required failed...quitting\n");
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")) )
651 yError(
"Unable to detect the presence of F/T sensors in your iCub...quitting\n");
657 string rpcPortName =
"/"+local_name+
"/rpc:i";
658 rpcPort.open(rpcPortName);
662 port_filtered_output.open(
"/"+local_name+
"/filtered/inertial:o");
663 inertialFilter =
new dataFilter(port_filtered_output, m_iGyro, m_iAcc);
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);
675 yInfo(
"ft thread istantiated...\n");
678 inertialFilter->start();
680 yInfo(
"thread started\n");
687 yInfo(
"Closing wholeBodyDynamics module... \n");
694 yInfo(
"Setting the icub in stiff mode\n");
697 yInfo(
"Stopping the inv_dyn thread...");
699 yInfo(
"inv_dyn thread stopped\n");
706 yInfo(
"Stopping the inertial filter thread \n");
707 inertialFilter->stop();
708 delete inertialFilter;
709 inertialFilter=
nullptr;
710 dd_MASClient.close();
713 yInfo(
"Closing the filtered inertial output port \n");
714 port_filtered_output.interrupt();
715 port_filtered_output.close();
717 yInfo(
"Closing the rpc port \n");
722 yInfo(
"Closing dd_left_arm \n");
723 dd_left_arm->close();
729 yInfo(
"Closing dd_right_arm \n");
730 dd_right_arm->close();
732 dd_right_arm=
nullptr;
736 yInfo(
"Closing dd_head \n");
744 yInfo(
"Closing dd_left_leg \n");
745 dd_left_leg->close();
751 yInfo(
"Closing dd_right_leg \n");
752 dd_right_leg->close();
754 dd_right_leg=
nullptr;
758 yInfo(
"Closing dd_torso \n");
764 yInfo(
"wholeBodyDynamics module was closed successfully! \n");
774 double avgTime, stdDev, period;
775 period = inv_dyn->getPeriod();
776 inv_dyn->getEstimatedPeriod(avgTime, stdDev);
777 if(avgTime > 1.3 * period){
781 static unsigned long int alive_counter = 0;
782 static double curr_time = Time::now();
783 if (Time::now() - curr_time > 60)
785 yInfo (
"wholeBodyDynamics is alive! running for %ld mins.\n",++alive_counter);
786 curr_time = Time::now();
789 if (inv_dyn==
nullptr)
796 yError (
"wholeBodyDynamics module lost connection with iCubInterface, now closing...\n");
801 yInfo(
"wholeBodyDynamics module was closed successfully! \n");
812 rf.setDefaultContext(
"wholeBodyDynamics");
813 rf.setDefaultConfigFile(
"wholeBodyDynamics.ini");
814 rf.configure(
argc,argv);
816 if (rf.check(
"help"))
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;
842 if (!
yarp.checkNetwork())
844 yError(
"Sorry YARP network does not seem to be available, is the yarp server available?\n");
849 return obs.runModule(rf);
dataFilter(BufferedPort< Vector > &_port_filtered_output, IThreeAxisGyroscopes *iGyro, IThreeAxisLinearAccelerometers *iAcc)
void calibrateOffset(calib_enum calib_code=CALIB_ALL)
thread_status_enum getThreadStatus()
virtual bool createDriver(PolyDriver *&_dd, Property options)
bool respond(const Bottle &command, Bottle &reply) override
double getPeriod() override
bool updateModule() override
bool configure(ResourceFinder &rf) override
int main(int argc, char *argv[])
Copyright (C) 2008 RobotCub Consortium.
double lpf_ord1_3hz(double input, int j)