Allow PD gains to be set through datastore call

[rohanpsingh]

~[this is currently a draft which allows pd gains to be set only when the OutPorts are available (i.e. when using ModifiedServo)]~

This PR will create datastore functions to modify the servo PD gains, either directly on the iob if REAL_ROBOT = ON or through RTC ports if REAL_ROBOT = OFF.REAL_ROBOT is a cmake option.

The datastores can be called from an mc-rtc controller as follows:

• Example for reading pd gains:

  std::vector<double> kp;
  std::vector<double> kd;
  if(ctl.datastore().has(ctl.robot().name() + "::GetPDGains"))
  {
  bool get_servo_success = ctl.datastore().call<bool, std::vector<double> &, std::vector<double> &>(
      ctl.robot().name() + "::GetPDGains", kp, kd);
  }

• Example for writing pd gains:

  if(ctl.datastore().has(ctl.robot().name() + "::SetPDGains"))
  {
  bool set_servo_success = ctl.datastore().call<bool, const std::vector<double> &, const std::vector<double> &>(
      ctl.robot().name() + "::SetPDGains", kp, kd);
  }

[rohanpsingh]

Thanks for the review @gergondet I made most of the changes you requested with 2 remaining questions. I also added functionality to read/write gains by joint name.

I'd prefer this to be a run-time switch. We can easily detect if mc_openrtm is being run on a real robot (RobotHardware component is available) or in simulation

Is there an example for this? I'm not sure I know how to check for the RobotHardware component inside mc_openrtm

possible failure of IOB calls.

Should I check if each call returns a TRUE? And what to do if open_iob is successful but close_iob is not?

[gergondet]

Is there an example for this? I'm not sure I know how to check for the RobotHardware component inside mc_openrtm

I have implemented the member m_is_simulation in #25 you can rely on this to detect whether we are running in simulation or on hardware

Should I check if each call returns a TRUE? And what to do if open_iob is successful but close_iob is not?

To make the output readable:

• if open_iob fails, a critical error message should be displayed (mc_rtc::log::critical) should be displayed and the feature disabled
• if setting the gains fail, same deal
• I don't think there's a scenario where close_iob can fail when open_iob didn't but it might be worth reporting if that happens

[rohanpsingh]

Thanks @gergondet ! I implemented checks for possible IOB failure calls here https://github.com/jrl-umi3218/mc_openrtm/pull/23/commits/f531cc5e8ce46ed2d01a5882ac10983b4f3044ed. It looks dirty to me, so if you have a better suggestion, please let me know.

I am also using the m_is_simulation member now here https://github.com/jrl-umi3218/mc_openrtm/pull/23/commits/4d0cba07a7bdd9a9628f558dce9c75020afc7f0e.

[gergondet]

Thanks @rohanpsingh

Once the failed_iob issue is fixed and when this is tested on a robot we can merge.

[rohanpsingh]

Okay, I made the fix! If the CI passes, I'll find a way to test this safely on the real robot. :smiley:

[rohanpsingh]

I confirmed that I could change the gains on the real robot (with servo ON)