Added dynamic reconfigure for PID gains #1

davetcoleman · 2013-07-11T01:20:10Z

You can now set the P, I, D and i_clam_min/max using dynamic reconfigure

davetcoleman · 2013-07-11T01:22:10Z

I added this so that Gazebo users can easily tune their robot controllers to work in simulation
@jbohren @adolfo-rt

davetcoleman · 2013-07-13T23:44:00Z

@adolfo-rt can you review this PR and its real-time controller implications?

adolfo-rt · 2013-07-15T07:32:34Z

src/pid.cpp

    ROS_ERROR("No p gain specified for pid.  Namespace: %s", n.getNamespace().c_str());
    return false;
  }
-  n.param("i", i_gain_, 0.0);
-  n.param("d", d_gain_, 0.0);
+  if (!n.getParam("i", i_gain_)) 


init should not fail when the derivative or integral gains are not specified. The previous behavior of defaulting them to zero without raising an error should be preserved. We should allow specifying P, PD or PI controllers without requiring the explicit setting of the unused gains. The proportional gain is error-checked because it is unlikely you'll find a controller without it.

Ah, that makes a lot of sense. I thought it had just been coding laziness!

davetcoleman · 2013-07-15T18:01:31Z

@adolfo-rt i have made your suggested fixes, can you double check the mutex usage here: https://github.com/ros-controls/control_toolbox/pull/1/files#L4R168 please?

Thanks!

davetcoleman · 2013-07-15T18:02:00Z

src/pid.cpp

+
+  getGains(config.p_gain, config.i_gain, config.d_gain, config.i_clamp_max, config.i_clamp_min);
+
+  // Set starting values, using a shared mutex with dynamic reconfig


Does this look right?

I think so, yes.

davetcoleman · 2013-07-15T20:50:09Z

Who has commit access to control_toolbox? I seem to only have access to ros_controls and ros_controllers

ahendrix · 2013-07-15T21:04:11Z

CMakeLists.txt

+    ${Boost_INCLUDE_DIR}
+    )
+
+  find_package(Boost REQUIRED COMPONENTS system thread)


Doing find_package(Boost ...) after setting the include dirs to include boost seems like a bug.

I've moved it above the include_directories line.

ahendrix · 2013-07-15T21:58:51Z

I think the assignment operator needs an update; probably just an added call to updateDynamicReconfig().

ahendrix · 2013-07-15T22:05:57Z

I'm worried about locking around the p_gain_, i_gain_, d_gain_, i_max_ and i_min_ terms.

Without locking, it's possible that the dynamic_reconfigure callback will update a few of the terms and get preempted while the realtime part of the controller runs, at which point it will see an inconsistent set of gains.

With locking around these parameters, it's possible that the dynamic_reconfigure callback could block execution of the realtime part of the controller, which could cause the entire realtime loop to miss its deadline.

I know some parts of this code operate in realtime and some parts do not; is there a clear delineation of which methods run as part of the realtime controller and which do not?

davetcoleman · 2013-07-15T22:15:47Z

From what I can tell, the only risk of breaking the realtime run loop is if you:

Call Pid.setGains() within your real time controller. I don't see why someone would do this but we can document proper warnings against doing so
Use the Pid X = Pid Y assignment operator (which also triggers dynamic reconfigure). You really wouldn't want to do this in a realtime loop.

adolfo-rt · 2013-07-16T12:07:50Z

@ahendrix, @davetcoleman
A solution would be to use the RealtimeBuffer from realtime_tools. The class is undocumented :( but should be easy to use.

The recursive lock passed to dynamic_reconfigure would not be used inside the PID class. Instead, when new values arrive via a reconfigure update, one would safely make them effective by doing a writeFromNonRT(), and reading them would be done with readFromRT().

Lastly, I would advise to put the gains inside of a struct, so that only one RealtimeBuffer insance is needed instead of one for every gain/parameter.

davetcoleman · 2013-07-16T15:49:12Z

I've seen the RealtimeBuffer used in ros_control[lers] in lots of places, and I'll implement it here, but is this really needed you think?

adolfo-rt · 2013-07-16T16:51:29Z

I've seen the RealtimeBuffer used in ros_control[lers] in lots of places, and I'll implement it here, but is this really needed you think?

If the alternative is using the recursive_mutex shared with dynamic_reconfigure, then yes; otherwise there is a risk of a priority inversion. In the end what you want is to read data using a fast, constant-time operation (ie. deterministic). The RealtimeBuffer provides that.

Notice that the RealtimeBuffer uses internally a lock, but it only attempts a try_lock when reading from realtime, and if that fails it returns a pointer to the last (cached) valid data. For completeness, Orocos offers alternative implementations of data structures not subject to priority inversions, summarized here. I don't think it's worth the additional dependency when you can get away with realtime_tools.

ahendrix · 2013-07-16T17:41:55Z

+1; RealtimeBuffer sounds like the right solution.

@davetcoleman see my reasoning above for why I think this is required. Realtime constraints don't seem to be a big problem in Gazebo, but they're a very real problem on a real robot where we're running the controllers at 1kHz, and a delay of more than a few hundred microseconds could halt the system.

…der and to match header file

davetcoleman · 2013-07-18T00:51:24Z

I've added the realtime buffer... it required a lot of changes and in order for me to thoroughly make sure I implemented it right I re-ordered some of the functions such that they are grouped by function (initialization, etc) and their ordering matches the header file. My apologies for the diff.

I'd like to do some more runtime testing before merging this.

ahendrix · 2013-07-18T04:59:13Z

I think this looks good overall. More testing is better.

ahendrix · 2013-07-18T05:11:57Z

Is it still possible to set the gains through the old parameters?

adolfo-rt · 2013-07-18T08:47:37Z

Thanks for taking on this Dave. It's a great usability improvement for the PID class.

davetcoleman · 2013-07-18T22:54:27Z

Thoughts on putting this inside of Pid so that it's control_toolbox::Pid::Gains instead of control_toolbox::Gains ?

Done

Since you're no longer trying to return multiple things, I think it would make more sense for this to be Gains getGains();

Done

Is it still possible to set the gains through the old parameters?

Yes, I haven't changed any of the old api

There is one issue though... the ability to do
Pid pid2 = pid1;

In the old implementation (without a realtime buffer) it had an opertor= overload that did some custom stuff:

  Pid &operator =(const Pid& p)
  {
    if (this == &p)
      return *this;

    p_gain_ = p.p_gain_;
    i_gain_ = p.i_gain_;
    d_gain_ = p.d_gain_;
    i_max_ = p.i_max_;
    i_min_ = p.i_min_;

    p_error_last_ = p_error_ = i_term_ = d_error_ = cmd_ = 0.0;
    return *this;
  }

Now with the real time buffer, we can't do

    p_gain_ = p.p_gain_;
    i_gain_ = p.i_gain_;
    d_gain_ = p.d_gain_;
    i_max_ = p.i_max_;
    i_min_ = p.i_min_;

But we also can't do any kind of accessing or copying of the gains_buffer_ because the gains buffer has a non-const readFromRT() and assignment functions pass in a const copy of the original element.

The only solution I can think of is adding a new function to realtime_buffer.h like this:

T* readFromRTConst() const
  {                                     
    return realtime_data_; 
  }

But I'm not entirely sure if this will return the latest data/if its a good addition. I'll add a pull request do discuss this idea on the realtime_tools repo...

… from a const PID class

davetcoleman · 2013-07-19T20:41:12Z

I think I have resolved the overloaded assignment operator issue as well as removed the need for adding a new readFromRT() function in the realtime_tools package.

It should be pretty stable now.

ahendrix · 2013-07-22T19:08:50Z

include/control_toolbox/pid.h

   */
-  void getCurrentPIDErrors(double *pe, double *ie, double *de);
+  Gains getGainsConst() const;


getGains() should probably be const, which would make this redundant.

having const and non-const methods that do exactly the same thing is redundant and confusing.

As of today's fixes, getGainsConst() calls a different function in the runtime_buffer class - it calls a blocking readFromRTConst(). I think providing this optional functionality is beneficial for certain applications.

davetcoleman · 2013-07-22T20:47:49Z

I upgraded the PID tests to use the new computeCommand() function and I added a bunch of tests for getting/setting PID gains as well as coping/assigning the Pid class.

davetcoleman · 2013-07-26T18:27:47Z

@ahendrix can we merge and release this too? thanks!

ahendrix · 2013-07-26T21:02:43Z

src/pid.cpp

+
+Pid::Gains Pid::getGains() const
+{
+  return *gains_buffer_.readFromNonRT();


Having different versions of getGains that differ only in const-ness, one of which is realtime-safe and one of which is not seems like a good way to introduce bugs.

Is the non-const version useful for anything?

I suggest we either eliminate the non-const version or rename them to indicate realtime safety or lack thereof; perhaps getGainsRT and getGainsNonRT.

I was using it for the printValues() helper function, but I have removed the const getGains() function.

ahendrix · 2013-07-29T17:41:37Z

include/control_toolbox/pid.h


  /*!
   * \brief Get PID gains for the controller.
+   * \return gains A struct of the PID gain values


Having const and non-const methods with the same name, one of which is realtime-safe and the other of which is not sounds like a recipe for bugs.

I suggest you rename them to 'getGainsNonRT' and 'getGainsRT' or something similar that reflects the realtime-safety of each method, or just remove one of them.

Did you mean to post this comment again? I fixed this issue last Friday. There are no const getters anymore.

Added dynamic reconfigure for PID gains

davetcoleman · 2013-07-29T20:37:05Z

Release to build farm?

ahendrix · 2013-07-29T20:49:05Z

Doesn't build on my machine; copy constructors are broken:

In file included from /usr/include/c++/4.6/x86_64-linux-gnu/./bits/c++allocator.h:34:0,
                 from /usr/include/c++/4.6/bits/allocator.h:48,
                 from /usr/include/c++/4.6/vector:62,
                 from /wg/stor2a/ahendrix/hydro/src/pr2_controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_pose_controller.h:72,
                 from /wg/stor2a/ahendrix/hydro/src/pr2_controllers/robot_mechanism_controllers/src/cartesian_pose_controller.cpp:36:
/usr/include/c++/4.6/ext/new_allocator.h: In member function ‘void __gnu_cxx::new_allocator<_Tp>::construct(__gnu_cxx::new_allocator<_Tp>::pointer, const _Tp&) [with _Tp = control_toolbox::Pid, __gnu_cxx::new_allocator<_Tp>::pointer = control_toolbox::Pid*]’:
/usr/include/c++/4.6/bits/stl_vector.h:830:6:   instantiated from ‘void std::vector<_Tp, _Alloc>::push_back(const value_type&) [with _Tp = control_toolbox::Pid, _Alloc = std::allocator<control_toolbox::Pid>, std::vector<_Tp, _Alloc>::value_type = control_toolbox::Pid]’
/wg/stor2a/ahendrix/hydro/src/pr2_controllers/robot_mechanism_controllers/src/cartesian_pose_controller.cpp:103:45:   instantiated from here
/usr/include/c++/4.6/ext/new_allocator.h:108:9: error: no matching function for call to ‘control_toolbox::Pid::Pid(const control_toolbox::Pid&)’
/usr/include/c++/4.6/ext/new_allocator.h:108:9: note: candidates are:
/wg/stor2a/ahendrix/hydro/src/control_toolbox/include/control_toolbox/pid.h:156:3: note: control_toolbox::Pid::Pid(control_toolbox::Pid&)

The main problem here is that the copy constructor for control_toolbox::Pid does not take a const argument. I'll work on it...

ahendrix · 2013-07-29T20:59:38Z

Fixed by making the copy constructor parameter const.

Released control_toolbox 1.10.2 to Hydro: ros/rosdistro#1578

davetcoleman · 2013-07-29T21:24:25Z

Thanks!

* Update gh actions versions * Cleanup doc and destructor

davetcoleman added 2 commits July 10, 2013 18:09

Added dynamic reconfigure for PID gains

d5f9923

Merged hydro-devel

e2020db

davetcoleman mentioned this pull request Jul 11, 2013

Fixed PID destructor bug, cleaned up code ros-controls/ros_controllers#29

Merged

adolfo-rt reviewed Jul 15, 2013
View reviewed changes

Updated CMakeLists.txt and made fixes per Adolfo

1ec720c

davetcoleman reviewed Jul 15, 2013
View reviewed changes

ahendrix reviewed Jul 15, 2013
View reviewed changes

Fixes per Austin review

3ed065a

davetcoleman added 2 commits July 17, 2013 17:43

Added realtime buffer to PID, re-ordered functions to more logical or…

8f83f74

…der and to match header file

Added realtime_tools as a dependency in package.xml and CMakeLists

712f2aa

Added new function getGainsConst that allows one to get the PID gains…

04f11ae

… from a const PID class

davetcoleman mentioned this pull request Jul 18, 2013

Made member vars mutable in realtime buffer to allow const read ros-controls/realtime_tools#2

Merged

Removed const read, added copy constructor and print values function

90ab218

ahendrix reviewed Jul 22, 2013
View reviewed changes

davetcoleman added 2 commits July 22, 2013 13:44

Added test for getting/settings gains, copying/assigning pid class

88d6bc8

Made realtime_buffer copiable

c17668d

davetcoleman added 2 commits July 22, 2013 15:50

Compatibility changes for realtime_tools, tweaked getests

d30b816

Small fixes

76761cd

ahendrix reviewed Jul 26, 2013
View reviewed changes

Removed const getGains function

17e41b5

ahendrix reviewed Jul 29, 2013
View reviewed changes

ahendrix pushed a commit that referenced this pull request Jul 29, 2013

Merge pull request #1 from davetcoleman/hydro-devel

81f02fd

Added dynamic reconfigure for PID gains

ahendrix merged commit 81f02fd into ros-controls:hydro-devel Jul 29, 2013

davetcoleman mentioned this pull request Oct 8, 2014

i_clamp use between ros parameters and dynamic_reconfigure in pid class is inconsistent #28

Closed

bmagyar added a commit that referenced this pull request Jul 1, 2020

Small cleanup (#1)

dffa9ca

* Update gh actions versions * Cleanup doc and destructor


		getGains(config.p_gain, config.i_gain, config.d_gain, config.i_clamp_max, config.i_clamp_min);

		// Set starting values, using a shared mutex with dynamic reconfig

Added dynamic reconfigure for PID gains #1

Added dynamic reconfigure for PID gains #1

Conversation

davetcoleman commented Jul 11, 2013

davetcoleman commented Jul 11, 2013

davetcoleman commented Jul 13, 2013

Choose a reason for hiding this comment

Choose a reason for hiding this comment

davetcoleman commented Jul 15, 2013

Choose a reason for hiding this comment

Choose a reason for hiding this comment

davetcoleman commented Jul 15, 2013

Choose a reason for hiding this comment

Choose a reason for hiding this comment

ahendrix commented Jul 15, 2013

ahendrix commented Jul 15, 2013

davetcoleman commented Jul 15, 2013

adolfo-rt commented Jul 16, 2013

davetcoleman commented Jul 16, 2013

adolfo-rt commented Jul 16, 2013

ahendrix commented Jul 16, 2013

davetcoleman commented Jul 18, 2013

ahendrix commented Jul 18, 2013

ahendrix commented Jul 18, 2013

adolfo-rt commented Jul 18, 2013

davetcoleman commented Jul 18, 2013

davetcoleman commented Jul 19, 2013

Choose a reason for hiding this comment

Choose a reason for hiding this comment

davetcoleman commented Jul 22, 2013

davetcoleman commented Jul 26, 2013

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

davetcoleman commented Jul 29, 2013

ahendrix commented Jul 29, 2013

ahendrix commented Jul 29, 2013

davetcoleman commented Jul 29, 2013