Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Added dynamic reconfigure for PID gains #1

Merged
merged 13 commits into from
Jul 29, 2013
Merged

Added dynamic reconfigure for PID gains #1

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
13 commits
Select commit Hold shift + click to select a range
d5f9923
Added dynamic reconfigure for PID gains
davetcoleman Jul 11, 2013
e2020db
Merged hydro-devel
davetcoleman Jul 11, 2013
1ec720c
Updated CMakeLists.txt and made fixes per Adolfo
davetcoleman Jul 15, 2013
3ed065a
Fixes per Austin review
davetcoleman Jul 15, 2013
8f83f74
Added realtime buffer to PID, re-ordered functions to more logical or…
davetcoleman Jul 18, 2013
712f2aa
Added realtime_tools as a dependency in package.xml and CMakeLists
davetcoleman Jul 18, 2013
04f11ae
Added new function getGainsConst that allows one to get the PID gains…
davetcoleman Jul 18, 2013
90ab218
Removed const read, added copy constructor and print values function
davetcoleman Jul 19, 2013
88d6bc8
Added test for getting/settings gains, copying/assigning pid class
davetcoleman Jul 22, 2013
c17668d
Made realtime_buffer copiable
davetcoleman Jul 22, 2013
d30b816
Compatibility changes for realtime_tools, tweaked getests
davetcoleman Jul 22, 2013
76761cd
Small fixes
davetcoleman Jul 23, 2013
17e41b5
Removed const getGains function
davetcoleman Jul 26, 2013
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
54 changes: 36 additions & 18 deletions CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,30 +6,46 @@ if(USE_ROSBUILD)
rosbuild_init()
#rosbuild_genmsg()
rosbuild_gensrv()
rosbuild_add_library(control_toolbox
rosbuild_add_library(${PROJECT_NAME}
src/pid.cpp
src/pid_gains_setter.cpp
src/sine_sweep.cpp
src/dither.cpp
src/sinusoid.cpp
src/limited_proxy.cpp
)
target_link_libraries(control_toolbox tinyxml)
target_link_libraries(${PROJECT_NAME} tinyxml)

# rosbuild_add_executable(test_linear test/linear.cpp)

# Tests
rosbuild_add_gtest(test/pid_tests test/pid_tests.cpp)
target_link_libraries(test/pid_tests control_toolbox)
rosbuild_add_gtest(test/pid_tests test/pid_tests.cpp)
target_link_libraries(test/pid_tests ${PROJECT_NAME})

else()

# Load catkin and all dependencies required for this package
find_package(catkin REQUIRED COMPONENTS rosconsole message_generation tf roscpp angles)
find_package(catkin REQUIRED COMPONENTS
rosconsole
message_generation
tf
roscpp
angles
dynamic_reconfigure
)

include_directories(
include
${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS})
include
${catkin_INCLUDE_DIRS}
${Boost_INCLUDE_DIR}
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Should this be ${Boost_INCLUDE_DIR} or ${Boost_INCLUDE_DIRS} ?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I just did some googling but I couldn't find anything conclusive about which one is right... I think they both work? I searched through the my ROS workspace and lots of code uses the singular _DIR include:

./ros_control/controller_manager_tests/CMakeLists.txt:34:  include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS})
./ros_control/controller_manager/CMakeLists.txt:21:  include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS})
./gazebo_tutorials/CMakeLists.txt:18:include_directories(${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS})
./control_toolbox/CMakeLists.txt:40:    ${Boost_INCLUDE_DIR}
./ros_controllers/joint_state_controller/CMakeLists.txt:19:  include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS})
./ros_controllers/position_controllers/CMakeLists.txt:20:  include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS})
./ros_controllers/effort_controllers/CMakeLists.txt:21:  include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS})
./ros_controllers/imu_sensor_controller/CMakeLists.txt:18:  include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS})
./ros_controllers/force_torque_sensor_controller/CMakeLists.txt:18:  include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS})
./ros_controllers/velocity_controllers/CMakeLists.txt:19:  include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS})
./realtime_tools/CMakeLists.txt:16:  include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS})
./gazebo_ros_pkgs/gazebo_ros_control/CMakeLists.txt:29:  ${Boost_INCLUDE_DIR}

)

find_package(Boost REQUIRED COMPONENTS system thread)
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I've moved it above the include_directories line.


# Dynamics reconfigure
generate_dynamic_reconfigure_options(
cfg/Parameters.cfg
)

# Add services and generate them
add_service_files(
Expand All @@ -41,6 +57,14 @@ else()
DEPENDENCIES std_msgs
)

# Declare catkin package
catkin_package(
DEPENDS tinyxml
CATKIN_DEPENDS rosconsole tf roscpp angles dynamic_reconfigure
INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME}
)

add_library(${PROJECT_NAME}
src/pid.cpp
src/pid_gains_setter.cpp
Expand All @@ -49,20 +73,14 @@ else()
src/sinusoid.cpp
src/limited_proxy.cpp
)
add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg) # wait for dynamic reconfigure

target_link_libraries(control_toolbox tinyxml ${catkin_LIBRARIES})
add_dependencies(control_toolbox ${PROJECT_NAME}_gencpp)
target_link_libraries(${PROJECT_NAME} tinyxml ${catkin_LIBRARIES} ${Boost_LIBRARIES})

# Declare catkin package
catkin_package(
DEPENDS tinyxml
CATKIN_DEPENDS rosconsole tf roscpp angles
INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME}
)
add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencpp) # wait for msgs

# Tests
catkin_add_gtest(pid_tests test/pid_tests.cpp)
catkin_add_gtest(pid_tests test/pid_tests.cpp)
target_link_libraries(pid_tests ${catkin_LIBRARIES} ${PROJECT_NAME})

# add_executable(test_linear test/linear.cpp)
Expand All @@ -78,6 +96,6 @@ else()
)

install(DIRECTORY scripts/
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

endif()
54 changes: 54 additions & 0 deletions cfg/Parameters.cfg
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,54 @@
#! /usr/bin/env python
#*********************************************************************
#* Software License Agreement (BSD License)
#*
#* Copyright (c) 2013, Open Source Robotics Foundation
#* All rights reserved.
#*
#* Redistribution and use in source and binary forms, with or without
#* modification, are permitted provided that the following conditions
#* are met:
#*
#* * Redistributions of source code must retain the above copyright
#* notice, this list of conditions and the following disclaimer.
#* * Redistributions in binary form must reproduce the above
#* copyright notice, this list of conditions and the following
#* disclaimer in the documentation and/or other materials provided
#* with the distribution.
#* * Neither the name of the Open Source Robotics Foundation
#* nor the names of its contributors may be
#* used to endorse or promote products derived
#* from this software without specific prior written permission.
#*
#* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
#* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
#* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
#* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
#* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
#* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
#* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
#* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
#* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
#* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
#* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
#* POSSIBILITY OF SUCH DAMAGE.
#*********************************************************************/

# Author: Dave Coleman
# Desc: Allows PID parameters, etc to be tuned in realtime using dynamic reconfigure


PACKAGE='control_toolbox'

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

# Name Type Level Description Default Min Max
gen.add( "p_gain" , double_t, 1,"Proportional gain.", 10.0 , 0 , 1000)
gen.add( "i_gain" , double_t, 1,"Integral gain.", 10.0 , 0 , 100)
gen.add( "d_gain" , double_t, 1,"Derivative gain.", 10.0 , 0 , 100)
gen.add( "i_clamp_min" , double_t, 1,"Min bounds for the integral windup", 10.0 , 0 , 100)
gen.add( "i_clamp_max" , double_t, 1,"Max bounds for the integral windup", 10.0 , 0 , 100)
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I believe dynamic_reconfigure interacts with the parameter server; it may be useful to give these the same names and defaults and their matching parameters.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If you mean the full path of the parameters on the parameter server, the Pid class doesn't know those before hand (it is passed a nodehandle at initialization).

If you mean we should name the gains "p", "i", and "d" - I tried that and wasted a lot of time debugging why it wasn't working. There is a bug that disallows you to name a dynamic reconfigure parameter "i" ros/dynamic_reconfigure#6

I documented the issue on the tutorial in the red box.

# PkgName #NodeName #Prefix for generated .h include file, e.g. ParametersConfig.py
exit(gen.generate(PACKAGE, "control_toolbox", "Parameters"))
130 changes: 81 additions & 49 deletions include/control_toolbox/pid.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,68 +36,73 @@


#include <string>
#include "ros/node_handle.h"
#include <ros/ros.h>

// Dynamic reconfigure
#include <dynamic_reconfigure/server.h>
#include <control_toolbox/ParametersConfig.h>
#include <boost/thread/mutex.hpp>

class TiXmlElement;

namespace control_toolbox {

/***************************************************/
/*! \class Pid
\brief A basic pid class.
\brief A basic pid class.

This class implements a generic structure that
can be used to create a wide range of pid
controllers. It can function independently or
be subclassed to provide more specific controls
based on a particular control loop.
This class implements a generic structure that
can be used to create a wide range of pid
controllers. It can function independently or
be subclassed to provide more specific controls
based on a particular control loop.

In particular, this class implements the standard
pid equation:
In particular, this class implements the standard
pid equation:

\f$command = -p_{term} - i_{term} - d_{term} \f$
\f$command = -p_{term} - i_{term} - d_{term} \f$

where: <br>
<UL TYPE="none">
<LI> \f$ p_{term} = p_{gain} * p_{error} \f$
<LI> \f$ i_{term} = i_{term} + \int{i_{gain} * p_{error} * dt} \f$
<LI> \f$ d_{term} = d_{gain} * d_{error} \f$
<LI> \f$ d_{error} = (p_{error} - p_{error last}) / dt \f$
</UL>
where: <br>
<UL TYPE="none">
<LI> \f$ p_{term} = p_{gain} * p_{error} \f$
<LI> \f$ i_{term} = i_{term} + \int{i_{gain} * p_{error} * dt} \f$
<LI> \f$ d_{term} = d_{gain} * d_{error} \f$
<LI> \f$ d_{error} = (p_{error} - p_{error last}) / dt \f$
</UL>

given:<br>
<UL TYPE="none">
<LI> \f$ p_{error} = p_{state} - p_{target} \f$.
</UL>
given:<br>
<UL TYPE="none">
<LI> \f$ p_{error} = p_{state} - p_{target} \f$.
</UL>

\section ROS ROS interface
\section ROS ROS interface

\param p Proportional gain
\param p Proportional gain

\param d Derivative gain
\param d Derivative gain

\param i Integral gain
\param i Integral gain

\param i_clamp Min/max bounds for the integral windup, the clamp is applied to the \f$i_{term}\f$
\param i_clamp Min/max bounds for the integral windup, the clamp is applied to the \f$i_{term}\f$

\section Usage
\section Usage

To use the Pid class, you should first call some version of init()
(in non-realtime) and then call updatePid() at every update step.
For example:
To use the Pid class, you should first call some version of init()
(in non-realtime) and then call updatePid() at every update step.
For example:

\verbatim
control_toolbox::Pid pid;
pid.initPid(6.0, 1.0, 2.0, 0.3, -0.3);
double position_desi_ = 0.5;
...
ros::Time last_time = ros::Time::now();
while (true) {
\verbatim
control_toolbox::Pid pid;
pid.initPid(6.0, 1.0, 2.0, 0.3, -0.3);
double position_desi_ = 0.5;
...
ros::Time last_time = ros::Time::now();
while (true) {
ros::Time time = ros::Time::now();
double effort = pid.updatePid(currentPosition() - position_desi_, time - last_time);
last_time = time;
}
\endverbatim
}
\endverbatim

*/
/***************************************************/
Expand Down Expand Up @@ -133,18 +138,18 @@ class Pid
* \param i_min The min integral windup.
*/
void initPid(double p, double i, double d, double i_max, double i_min);
/*!
* \brief Initialize PID with the parameters in a namespace
*

/*!
* \brief Initialize PID with the parameters in a namespace
*
* \param prefix The namespace prefix.
*/
bool initParam(const std::string& prefix);
bool initXml(TiXmlElement *config);
/*!
/*!
* \brief Initialize PID with the parameters in a NodeHandle namespace
*
* \param n The NodeHandle which should be used to query parameters.
*
* \param n The NodeHandle which should be used to query parameters.
*/
bool init(const ros::NodeHandle &n);

Expand Down Expand Up @@ -206,7 +211,7 @@ class Pid
/*!
* \brief Set the PID error and compute the PID command with nonuniform
* time step size. This also allows the user to pass in a precomputed
* derivative error.
* derivative error.
*
* \param error Error since last call (error = target - state)
* \param error_dot d(Error)/dt since last call
Expand All @@ -217,7 +222,7 @@ class Pid
double computeCommand(double error, double error_dot, ros::Duration dt);

/*!
* \brief Update the Pid loop with nonuniform time step size.
* \brief Update the Pid loop with nonuniform time step size.
*
* \deprecated This function assumes <tt> p_error = (state - target) </tt>
* which is an unconventional definition of the error. Please use \ref
Expand All @@ -232,7 +237,7 @@ class Pid

/*!
* \brief Update the Pid loop with nonuniform time step size. This update
* call allows the user to pass in a precomputed derivative error.
* call allows the user to pass in a precomputed derivative error.
*
* \deprecated This function assumes <tt> p_error = (state - target) </tt>
* which is an unconventional definition of the error. Please use \ref
Expand All @@ -246,7 +251,25 @@ class Pid
*/
ROS_DEPRECATED double updatePid(double error, double error_dot, ros::Duration dt);

/**
* \brief Update the PID parameters from dynamics reconfigure
*/
void dynamicReconfigCallback(control_toolbox::ParametersConfig &config, uint32_t level);

/**
* @brief Start the dynamic reconfigure node and load the default values
* @param node - a node handle where dynamic reconfigure services will be published
*/
void initDynamicReconfig(ros::NodeHandle &node);

/**
* @brief Set Dynamic Reconfigure's gains to Pid's values
*/
void updateDynamicReconfig();

/**
* @brief Custom assignment operator
*/
Pid &operator =(const Pid& p)
{
if (this == &p)
Expand All @@ -273,6 +296,15 @@ class Pid
double i_max_; /**< Maximum allowable integral term. */
double i_min_; /**< Minimum allowable integral term. */
double cmd_; /**< Command to send. */

// Dynamics reconfigure
typedef dynamic_reconfigure::Server<control_toolbox::ParametersConfig> DynamicReconfigServer;

boost::shared_ptr<DynamicReconfigServer> param_reconfig_server_;
DynamicReconfigServer::CallbackType param_reconfig_callback_;

boost::recursive_mutex param_reconfig_mutex_;

};

}
Expand Down
6 changes: 5 additions & 1 deletion package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,8 @@
<license>BSD</license>

<url type="website">http://ros.org/wiki/control_toolbox</url>
<!-- <url type="bugtracker"></url> -->
<url type="bugtracker">https://github.com/ros-controls/control_toolbox/issues</url>
<url type="repository">https://github.com/ros-controls/control_toolbox/</url>

<author>Melonee Wise</author>
<author>Sachin Chitta</author>
Expand All @@ -20,12 +21,15 @@
<build_depend>roscpp</build_depend>
<build_depend>angles</build_depend>
<build_depend>message_generation</build_depend>
<build_depend>dynamic_reconfigure</build_depend>
<build_depend>tinyxml</build_depend>

<run_depend>rosconsole</run_depend>
<run_depend>tf</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>angles</run_depend>
<run_depend>message_runtime</run_depend>
<run_depend>dynamic_reconfigure</run_depend>
<run_depend>tinyxml</run_depend>

<export>
Expand Down
Loading