diff --git a/CMakeLists.txt b/CMakeLists.txt index 40b2491d..93b4c07a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,12 +18,14 @@ find_package(TinyXML REQUIRED) # Dynamics reconfigure generate_dynamic_reconfigure_options( cfg/Parameters.cfg + cfg/LeadLag.cfg ) # Add services and generate them add_service_files( FILES SetPidGains.srv + SetLeadLagGains.srv ) generate_messages( @@ -52,6 +54,8 @@ include_directories( ) add_library(${PROJECT_NAME} + src/lead_lag.cpp + src/lead_lag_gains_setter.cpp src/pid.cpp src/pid_gains_setter.cpp src/sine_sweep.cpp @@ -68,6 +72,9 @@ if(CATKIN_ENABLE_TESTING) catkin_add_gtest(pid_tests test/pid_tests.cpp) target_link_libraries(pid_tests ${catkin_LIBRARIES} ${PROJECT_NAME}) + catkin_add_gtest(lead_lag_tests test/lead_lag_tests.cpp) + target_link_libraries(lead_lag_tests ${catkin_LIBRARIES} ${PROJECT_NAME}) + # add_executable(test_linear test/linear.cpp) endif() diff --git a/cfg/LeadLag.cfg b/cfg/LeadLag.cfg new file mode 100755 index 00000000..f5f3c428 --- /dev/null +++ b/cfg/LeadLag.cfg @@ -0,0 +1,62 @@ +#! /usr/bin/env python +#********************************************************************* +#* Software License Agreement (BSD License) +#* +#* Copyright (c) 2017, 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: Aris Synodinos +# Desc: Allows Lead Lag parameters, etc to be tuned in realtime using dynamic reconfigure + +PACKAGE='control_toolbox' + +def generate(gen): + # Name Type Level Description Default Min Max + gen.add( "k" , double_t, 1,"Gain.", 10.0 , -100000 , 100000) + gen.add( "a" , double_t, 1,"Lead gain.", 0.1 , -1000 , 1000) + gen.add( "b" , double_t, 1,"Lag gain.", 1.0 , -1000 , 1000) + # PkgName #NodeName #Prefix for generated .h include file, e.g. ParametersConfig.py + exit(gen.generate(PACKAGE, "control_toolbox", "LeadLag")) + +# try catkin generator first +try: + from dynamic_reconfigure.parameter_generator_catkin import * + gen = ParameterGenerator() + generate(gen) +# reason for catching IndexError +# parameter_generator_catkin expects 4 arguments while rosbuild only passes in 2 +# not thrilled with this solution +except IndexError: + print 'ERROR', PACKAGE, 'Parameters.cfg failed using parameter_generator_catkin, using rosbuild instead' + from dynamic_reconfigure.parameter_generator import * + gen = ParameterGenerator() + generate(gen) diff --git a/doc.h b/doc.h index 04a04731..5670b639 100644 --- a/doc.h +++ b/doc.h @@ -4,9 +4,11 @@ The control_toolbox package contains the following C++ classes which may be useful when implementing controllers. \li \ref control_toolbox::Pid "Pid" for implementing position/integral/derivative control loops. + \li \ref control_toolbox::LeadLag "Lead Lag" for implementing lead or lag compensator control loops. \li \ref control_toolbox::Ramp "Ramp Output" \li \ref control_toolbox::SineSweep "Sine Sweep" \li \ref control_toolbox::Dither "Dither" for giving white noise at specified amplitude \li \ref control_toolbox::PidGainsSetter "Pid gains setter" for advertising a service for automatically changing PID gains. + \li \ref control_toolbox::LeadLagGainsSetter "Lead Lag gains setter" for advertising a service for automatically changing LeadLag gains. **/ diff --git a/include/control_toolbox/lead_lag.h b/include/control_toolbox/lead_lag.h new file mode 100644 index 00000000..cb1ff6e7 --- /dev/null +++ b/include/control_toolbox/lead_lag.h @@ -0,0 +1,321 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, 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 Willow Garage 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: Aris Synodinos + +#ifndef CONTROL_TOOLBOX__LEAD_LAG_H +#define CONTROL_TOOLBOX__LEAD_LAG_H + +#include +#include + +// Dynamic reconfigure +#include +#include +#include + +// Realtime buffer +#include + +class TiXmlElement; + +namespace control_toolbox { +/***************************************************/ +/*! \class LeadLag + @brief A basic Lead/Lag class. + + This class implements a generic structure that + can be used to create a wide range of lead / lag + 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 + lead-lag equation: + + \f$ G(s) = gain * \frac{1 + \frac{w}{w_z}}{1 + \frac{w}{w_p}} \f$ + + where: -$w_z$ is the zero, -$w_p$ is the pole + and gain is the proportional gain of the controller. + + Discretized using the Bilinear trasformation + + \f$ s = \frac{2}{T} \frac{z - 1}{z + 1} \f$ + + Which results in the discrete form of + + \f$ G(z) = k * \frac{z - a}{z - b} \f$ + + where:
+
    +
  • \f$ k = gain * \frac{ w_p ( w_z + \frac{2}{dt} ) }{ w_z ( w_p + \frac{2}{dt} ) } \f$ +
  • \f$ a = - \frac{ w_z - \frac{2}{dt} }{ w_z + \frac{2}{dt} } \f$ +
  • \f$ b = - \frac{ w_p - \frac{2}{dt} }{ w_p + \frac{2}{dt} } \f$ +
+ + and can be converted in the discrete time-domain as + + \f$ y = b * y_{-1} + k * ( x - a * x_{-1} ) \f$ + + \section ROS ROS interface + + \param k Proportional gain + + \param a Zero parameter + + \param b Pole parameter + + \section Usage + + To use the LeadLag class, you should first call some version of init() + (in non-realtime) and then call computeCommand() at every step. + For example: + + \verbatim + control_toolbox::LeadLag comp; + comp.initLeadLag(2.0, 0.8, 1.2); + double desired_position = 0.5; + ... + ros::Time last_time = ros::Time::now(); + while (true) { + ros::Time time = ros::Time::now(); + double effort = comp.computeCommand(current_position - desired_position, time - last_time); + last_time = time; + } + \endverbatim + */ + /***************************************************/ + +class LeadLag { + public: + + /** + * @brief Stores the gains to allow easier realtime buffer usage + */ + struct Gains { + Gains(double k, double a, double b) : k_(k), a_(a), b_(b) {} + Gains() {} + double k_, a_, b_; + }; + + /** + * @brief Constructor, zeros out LeadLag values when created and + * initializes the controller state. + * Does not initialize dynamic reconfigure. + * @param k The proportional gain. + * @param a The zero parameter. + * @param b The pole parameter. + */ + LeadLag(double k = 0.0, double a = 0.0, double b = 0.0); + + /** + * @brief Copy constructor required for preventing mutexes from being copied + * @param source - LeadLag to copy + */ + LeadLag(const LeadLag &source); + + /** + * @brief Destructor of LeadLag class. + */ + ~LeadLag(); + + /** + * @brief Zeros out LeadLag values and initializes the gains + * Does not initialize dynamic reconfigure. + * @param k The proportional gain. + * @param a The zero parameter. + * @param b The pole parameter. + */ + void initLeadLag(double k, double a, double b); + + /** + * @brief Zeros out LeadLag values and initializes the gains + * Does not initialize dynamic reconfigure. + * @param k The proportional gain. + * @param a The zero parameter. + * @param b The pole parameter. + */ + void initLeadLag(double k, double a, double b, + const ros::NodeHandle & /*node*/); + + /** + * @brief Initialize LeadLag with the parameters in a namespace + * Initializes dynamic reconfigure for LeadLag gains + * + * @param prefix The namespace prefix. + * @param quiet If true, no error messages will be emitted on failure + * @return True on success, false otherwise. + */ + bool initParam(const std::string &prefix, const bool quiet = false); + + /** + * @brief Initialize LeadLag with the parameters in a NodeHandle namespace + * Initializes dynamic reconfigure for LeadLag gains + * + * @param n The NodeHandle which should be used to query parameters. + * @param quiet If true, no error messages will be emitted on failure + * @return True on success, false otherwise. + */ + bool init(const ros::NodeHandle &n, const bool quiet = false); + + /** + * @brief Initialize LeadLag with the parameters in an XML element + * Initializes dynamic reconfigure for LeadLag gains + * + * @param config the XML element + * @return True on success, false otherwise. + */ + bool initXml(TiXmlElement *config); + + /** + * @brief Start the dynamic reconfigure node and lead the default values. + * @param node - a NodeHandle where dynamic reconfigure services will be published. + */ + void initDynamicReconfig(ros::NodeHandle &node); + + /** + * @brief Reset the state of this LeadLag controller. + */ + void reset(); + + /** + * @brief Get LeadLag gains for the controller. + * @param k The proportional gain. + * @param a The zero parameter. + * @param b The pole parameter. + */ + void getGains(double &k, double &a, double &b); + + /** + * @brief Get LeadLag gains for the controller. + * @return gains A struct of the LeadLag gain values + */ + Gains getGains(); + + /** + * @brief Set LeadLag gains for the controller. + * @param k The proportional gain. + * @param a The zero parameter. + * @param b The pole parameter. + */ + void setGains(double k, double a, double b); + + /** + * @brief Set LeadLag gains for the controller. + * @param gains A struct of the LeadLag gain values + */ + void setGains(const Gains &gains); + + /** + * @brief Set Dynamic Reconfigure's gains to LeadLag's values + */ + void updateDynamicReconfig(); + void updateDynamicReconfig(LeadLag::Gains gains); + void updateDynamicReconfig(control_toolbox::LeadLagConfig config); + + /** + * @brief Update the LeadLag parameters from dynamic reconfigure + */ + void dynamicReconfigCallback(control_toolbox::LeadLagConfig &config, + uint32_t /*level*/); + + /** + * @brief Set the LeadLag error and compute the command. + * @param error Error since last call (error = target - state) + * @return LeadLag command + */ + double computeCommand(double error, ros::Duration /*dt*/); + + /** + * @brief Set current command for this LeadLag controller. + */ + void setCurrentCmd(double cmd); + + /** + * @brief Return current command for this LeadLag controller. + */ + double getCurrentCmd(); + + /** + * @brief Return current error. + */ + double getCurrentError(); + + /** + * @brief Print to console the current parameters + */ + void printValues(); + + /** + * @brief Custom assignment operator + * Does not initialize dynamic reconfigure for LeadLag gains + */ + LeadLag &operator=(const LeadLag &source) { + if (this == &source) return *this; + + // Copy the realtime buffer to then new LeadLag class + gains_buffer_ = source.gains_buffer_; + + // Reset the state of this LeadLag controller + reset(); + + return *this; + } + + private: + // Store the LeadLag gains in a realtime buffer to allow dynamic reconfigure + // to update it without + // blocking the realtime update loop + realtime_tools::RealtimeBuffer gains_buffer_; + + double error_last_; + double cmd_last_; + double error_; + double cmd_; + + // Dynamic reconfigure + bool dynamic_reconfig_initialized_; + typedef dynamic_reconfigure::Server + DynamicReconfigServer; + boost::shared_ptr lead_lag_reconfig_server_; + DynamicReconfigServer::CallbackType lead_lag_reconfig_callback_; + boost::recursive_mutex lead_lag_reconfig_mutex_; +}; + +typedef boost::shared_ptr LeadLagPtr; +typedef boost::shared_ptr LeadLagConstPtr; + +} + +#endif // CONTROL_TOOLBOX__LEAD_LAG_H diff --git a/include/control_toolbox/lead_lag_gains_setter.h b/include/control_toolbox/lead_lag_gains_setter.h new file mode 100644 index 00000000..aa47b5af --- /dev/null +++ b/include/control_toolbox/lead_lag_gains_setter.h @@ -0,0 +1,70 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, 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 Willow Garage 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. + *********************************************************************/ + +// Exposes a ROS interface for tuning a set of lead lag controllers. +// +// Author: Aris Synodinos + +#ifndef CONTROL_TOOLBOX__LEAD_LAG_GAINS_SETTER_H +#define CONTROL_TOOLBOX__LEAD_LAG_GAINS_SETTER_H + +#include +#include +#include + +namespace control_toolbox { + +class LeadLagGainsSetter { + public: + LeadLagGainsSetter() {} + + ~LeadLagGainsSetter(); + + LeadLagGainsSetter &add(LeadLag *controller); + + void advertise(const ros::NodeHandle &n); + + void advertise(const std::string &ns) { advertise(ros::NodeHandle(ns)); } + + bool setGains(control_toolbox::SetLeadLagGains::Request& req, + control_toolbox::SetLeadLagGains::Response& /*res*/); + + private: + ros::NodeHandle node_; + ros::ServiceServer serve_set_gains_; + std::vector controllers_; +}; +} + +#endif // CONTROL_TOOLBOX__LEAD_LAG_GAINS_SETTER_H diff --git a/src/lead_lag.cpp b/src/lead_lag.cpp new file mode 100644 index 00000000..b83afa2e --- /dev/null +++ b/src/lead_lag.cpp @@ -0,0 +1,245 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, 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 Willow Garage 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: Aris Synodinos + Desc: Implements a standard lead-lag controller +*/ + +#include +#include + +using namespace control_toolbox; + +static const std::string DEFAULT_NAMESPACE = "lead_lag"; + +LeadLag::LeadLag(double k, double a, double b) + : dynamic_reconfig_initialized_(false) { + setGains(k, a, b); + reset(); +} + +LeadLag::LeadLag(const LeadLag& source) : dynamic_reconfig_initialized_(false) { + // Copy the realtime buffer to then new LeadLag class + gains_buffer_ = source.gains_buffer_; + + // Reset the state of this LeadLag controller + reset(); +} + +LeadLag::~LeadLag() {} + +void LeadLag::initLeadLag(double k, double a, double b) { + setGains(k, a, b); + reset(); +} + +void LeadLag::initLeadLag(double k, double a, double b, + const ros::NodeHandle& /*node*/) { + initLeadLag(k, a, b); + ros::NodeHandle nh(DEFAULT_NAMESPACE); + initDynamicReconfig(nh); +} + +bool LeadLag::initParam(const std::string& prefix, const bool quiet) { + ros::NodeHandle nh(prefix); + return init(nh, quiet); +} + +bool LeadLag::init(const ros::NodeHandle& n, const bool quiet) { + ros::NodeHandle nh(n); + + Gains gains; + + // Load LeadLag gains from parameter server + if (!nh.getParam("k", gains.k_)) { + if (!quiet) { + ROS_ERROR_NAMED("lead-lag", + "No k gain specified for lead-lag. Namespace: %s", + nh.getNamespace().c_str()); + } + return false; + } + + // Only the K gain is required, the A and B gains are optional and default to + // 0: + nh.param("a", gains.a_, 0.0); + nh.param("b", gains.b_, 0.0); + + setGains(gains); + + reset(); + initDynamicReconfig(nh); + + return true; +} + +bool LeadLag::initXml(TiXmlElement* config) { + ros::NodeHandle nh(DEFAULT_NAMESPACE); + + setGains(config->Attribute("k") ? atof(config->Attribute("k")) : 0.0, + config->Attribute("a") ? atof(config->Attribute("a")) : 0.0, + config->Attribute("b") ? atof(config->Attribute("b")) : 0.0); + reset(); + initDynamicReconfig(nh); + + return true; +} + +void LeadLag::initDynamicReconfig(ros::NodeHandle& node) { + ROS_DEBUG_STREAM_NAMED("lead-lag", + "Initializing dynamic reconfigure in namespace " + << node.getNamespace()); + + // Start Dynamic reconfigure server + lead_lag_reconfig_server_.reset( + new DynamicReconfigServer(lead_lag_reconfig_mutex_, node)); + dynamic_reconfig_initialized_ = true; + + // Set Dynamic Reconfigure's gains to LeadLag's values + updateDynamicReconfig(); + + // Set callback + lead_lag_reconfig_callback_ = + boost::bind(&LeadLag::dynamicReconfigCallback, this, _1, _2); + lead_lag_reconfig_server_->setCallback(lead_lag_reconfig_callback_); +} + +void LeadLag::reset() { + error_last_ = 0.0; + error_ = 0.0; + cmd_last_ = 0.0; + cmd_ = 0.0; +} + +void LeadLag::getGains(double& k, double& a, double& b) { + Gains gains = *gains_buffer_.readFromRT(); + + k = gains.k_; + a = gains.a_; + b = gains.b_; +} + +LeadLag::Gains LeadLag::getGains() { return *gains_buffer_.readFromRT(); } + +void LeadLag::setGains(double k, double a, double b) { + Gains gains(k, a, b); + + setGains(gains); +} + +void LeadLag::setGains(const LeadLag::Gains& gains) { + gains_buffer_.writeFromNonRT(gains); + + // Update dynamic reconfigure with the new gains + updateDynamicReconfig(gains); +} + +void LeadLag::updateDynamicReconfig() { + // Make sure dynamic reconfigure is initialized + if (!dynamic_reconfig_initialized_) return; + + // Get starting values + control_toolbox::LeadLagConfig config; + + // Get starting values + getGains(config.k, config.a, config.b); + + updateDynamicReconfig(config); +} + +void LeadLag::updateDynamicReconfig(LeadLag::Gains gains) { + // Make sure dynamic reconfigure is initialized + if (!dynamic_reconfig_initialized_) return; + + control_toolbox::LeadLagConfig config; + + config.k = gains.k_; + config.a = gains.a_; + config.b = gains.b_; + + updateDynamicReconfig(config); +} + +void LeadLag::updateDynamicReconfig(control_toolbox::LeadLagConfig config) { + // Make sure dynamic reconfigure is initialized + if (!dynamic_reconfig_initialized_) return; + + // Set starting values, using a shared mutex with dynamic reconfig + { + boost::recursive_mutex::scoped_lock lock(lead_lag_reconfig_mutex_); + lead_lag_reconfig_server_->updateConfig(config); + } +} + +void LeadLag::dynamicReconfigCallback(control_toolbox::LeadLagConfig& config, + uint32_t /*level*/) { + ROS_DEBUG_STREAM_NAMED("lead-lag", "Dynamics reconfigure callback received."); + + // Set the gains + setGains(config.k, config.a, config.b); +} + +double LeadLag::computeCommand(double error, ros::Duration /*dt*/) { + // Get the gain parameters from the realtime buffer + Gains gains = *gains_buffer_.readFromRT(); + error_ = error; + + if (std::isnan(error) || std::isinf(error)) return 0.0; + + cmd_ = gains.k_ * (error_ - gains.a_ * error_last_) + gains.b_ * cmd_last_; + error_last_ = error_; + cmd_last_ = cmd_; + return cmd_; +} + +void LeadLag::setCurrentCmd(double cmd) { cmd_ = cmd; } + +double LeadLag::getCurrentCmd() { return cmd_; } + +double LeadLag::getCurrentError() { return error_; } + +void LeadLag::printValues() { + Gains gains = getGains(); + + ROS_INFO_STREAM_NAMED("lead-lag", + "Current Values of LeadLag Class:\n" + << " K Gain: " << gains.k_ << "\n" + << " A Gain: " << gains.a_ << "\n" + << " B Gain: " << gains.b_ << "\n" + << " Error_Last: " << error_last_ << "\n" + << " Error: " << error_ << "\n" + << " Command_Last: " << cmd_last_ << "\n" + << " Command: " << cmd_); +} diff --git a/src/lead_lag_gains_setter.cpp b/src/lead_lag_gains_setter.cpp new file mode 100644 index 00000000..68782b5e --- /dev/null +++ b/src/lead_lag_gains_setter.cpp @@ -0,0 +1,66 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, 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 Willow Garage 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. + *********************************************************************/ + +// Exposes a ROS interface for tuning a set of lead lag controllers. +// +// Author: Aris Synodinos + +#include + +using namespace control_toolbox; + +LeadLagGainsSetter::~LeadLagGainsSetter() { serve_set_gains_.shutdown(); } + +LeadLagGainsSetter& LeadLagGainsSetter::add(LeadLag* controller) { + assert(controller); + controllers_.push_back(controller); + return *this; +} + +void LeadLagGainsSetter::advertise(const ros::NodeHandle& n) { + node_ = n; + serve_set_gains_ = + node_.advertiseService("set_gains", &LeadLagGainsSetter::setGains, this); +} + +bool LeadLagGainsSetter::setGains(SetLeadLagGains::Request& req, + SetLeadLagGains::Response& /*res*/) { + for (size_t i = 0; i < controllers_.size(); ++i) { + controllers_[i]->setGains(req.k, req.a, req.b); + } + node_.setParam("k", req.k); + node_.setParam("a", req.a); + node_.setParam("b", req.b); + return true; +} diff --git a/src/pid.cpp b/src/pid.cpp index 741e4dbc..2bad2a3e 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -400,9 +400,6 @@ double Pid::getCurrentCmd() void Pid::getCurrentPIDErrors(double *pe, double *ie, double *de) { - // Get the gain parameters from the realtime buffer - Gains gains = *gains_buffer_.readFromRT(); - *pe = p_error_; *ie = i_error_; *de = d_error_; diff --git a/srv/SetLeadLagGains.srv b/srv/SetLeadLagGains.srv new file mode 100644 index 00000000..0f416e5a --- /dev/null +++ b/srv/SetLeadLagGains.srv @@ -0,0 +1,4 @@ +float64 k +float64 a +float64 b +--- \ No newline at end of file diff --git a/test/lead_lag_tests.cpp b/test/lead_lag_tests.cpp new file mode 100644 index 00000000..ad6af583 --- /dev/null +++ b/test/lead_lag_tests.cpp @@ -0,0 +1,250 @@ +#include +#include +#include +#include + +using namespace control_toolbox; + +TEST(ParameterTest, gainSettingCopyLeadLagTest) { + RecordProperty("description", + "This test succeeds if a LeadLag object has its gain set at " + "different points in time then the values are get-ed and " + "still remain the same, as well as when LeadLag is copied."); + + // Test values + double k = rand() % 100; + double a = rand() % 100; + double b = rand() % 100; + + // Initialize the default way + LeadLag controller1(k, a, b); + + // Test return values ------------------------------------------------- + double k_return, a_return, b_return; + controller1.getGains(k_return, a_return, b_return); + + EXPECT_EQ(k, k_return); + EXPECT_EQ(a, a_return); + EXPECT_EQ(b, b_return); + + // Test return values using struct + // ------------------------------------------------- + + // New values + k = rand() % 100; + a = rand() % 100; + b = rand() % 100; + controller1.setGains(k, a, b); + + LeadLag::Gains g1 = controller1.getGains(); + EXPECT_EQ(k, g1.k_); + EXPECT_EQ(a, g1.a_); + EXPECT_EQ(b, g1.b_); + + // \todo test initParam() ------------------------------------------------- + + // \todo test bool init(const ros::NodeHandle &n); + // ----------------------------------- + + // Send update command to populate errors + // ------------------------------------------------- + controller1.setCurrentCmd(10); + controller1.computeCommand(20, ros::Duration(1.0)); + + // Test copy constructor ------------------------------------------------- + LeadLag controller2(controller1); + + controller2.getGains(k_return, a_return, b_return); + + EXPECT_EQ(k, k_return); + EXPECT_EQ(a, a_return); + EXPECT_EQ(b, b_return); + + // Test that errors are zero + EXPECT_EQ(0.0, controller2.getCurrentError()); + + // Test assignment constructor + // ------------------------------------------------- + LeadLag controller3; + controller3 = controller1; + + controller3.getGains(k_return, a_return, b_return); + + EXPECT_EQ(k, k_return); + EXPECT_EQ(a, a_return); + EXPECT_EQ(b, b_return); + + // Test that errors are zero + EXPECT_EQ(0.0, controller3.getCurrentError()); + + // Test the reset() function, it should clear errors and command + controller1.reset(); + + EXPECT_EQ(0.0, controller3.getCurrentError()); + EXPECT_EQ(0.0, controller3.getCurrentCmd()); +} + +TEST(CommandTest, proportionalOnlyTest) { + RecordProperty("description", + "This test checks that a command is computed correctly using " + "the proportional contribution only."); + + // Set only proportional gain + LeadLag controller(1.0, 0.0, 0.0); + double cmd = 0.0; + + // If initial error = 0, p-gain = 1, dt = 1 + cmd = controller.computeCommand(-0.5, ros::Duration(1.0)); + // Then expect command = error + EXPECT_EQ(-0.5, cmd); + + // If call again + cmd = controller.computeCommand(-0.5, ros::Duration(1.0)); + // Then expect the same as before + EXPECT_EQ(-0.5, cmd); + + // If call again doubling the error + cmd = controller.computeCommand(-1.0, ros::Duration(1.0)); + // Then expect the command doubled + EXPECT_EQ(-1.0, cmd); + + // If call with positive error + cmd = controller.computeCommand(0.5, ros::Duration(1.0)); + // Then expect always command = error + EXPECT_EQ(0.5, cmd); +} + +TEST(CommandTest, zeroOnlyTest) { + RecordProperty("description", + "This test checks that a command is computed correctly using " + "the zero contribution only."); + LeadLag controller(0.0, 1.0, 0.0); + double cmd = 0.0; + + // If initial error = 0 + cmd = controller.computeCommand(-0.5, ros::Duration(1.0)); + // Then expect command to be zero + EXPECT_EQ(0.0, cmd); + + // If call again with same arguments + cmd = controller.computeCommand(-0.5, ros::Duration(1.0)); + // Then expect the command to be zero + EXPECT_EQ(0.0, cmd); + + // Call again with no error + cmd = controller.computeCommand(0.0, ros::Duration(1.0)); + // Then expect the command to be zero + EXPECT_EQ(0.0, cmd); +} + +TEST(CommandTest, poleOnlyTest) { + RecordProperty("description", + "This test checks that a command is computed correctly using " + "the pole contribution only."); + LeadLag controller(0.0, 0.0, 1.0); + double cmd = 0.0; + + // If initial error = 0 + cmd = controller.computeCommand(-0.5, ros::Duration(1.0)); + // Then expect command to be zero + EXPECT_EQ(0.0, cmd); + + // If call again with same arguments + cmd = controller.computeCommand(-0.5, ros::Duration(1.0)); + // Then expect the pole part to be zero + EXPECT_EQ(0.0, cmd); + + // Call again with no error + cmd = controller.computeCommand(0.0, ros::Duration(1.0)); + // Then expect the command to be zero + EXPECT_EQ(0.0, cmd); +} + +TEST(CommandTest, proportionalAndZeroTest) { + RecordProperty("description", + "This test checks that a command is computed correctly using " + "the proportional and the zero contribution."); + LeadLag controller(1.0, 1.0, 0.0); + double cmd = 0.0; + + // If initial error = 0 + cmd = controller.computeCommand(-0.5, ros::Duration(1.0)); + // Then expect command = error + EXPECT_EQ(-0.5, cmd); + + // If call again with same arguments + cmd = controller.computeCommand(-0.5, ros::Duration(1.0)); + // Then expect the zero part to be equal to the proportional part + EXPECT_EQ(0.0, cmd); + + // Call again with no error + cmd = controller.computeCommand(0.0, ros::Duration(1.0)); + // Then expect the command to be 0.5 + EXPECT_EQ(0.5, cmd); + + // Call again with no error + cmd = controller.computeCommand(0.0, ros::Duration(1.0)); + // Then expect the command to be zero + EXPECT_EQ(0.0, cmd); +} + +TEST(CommandTest, proportionalAndPoleTest) { + RecordProperty("description", + "This test checks that a command is computed correctly using " + "the pole contribution only."); + LeadLag controller(1.0, 0.0, 1.0); + double cmd = 0.0; + + // If initial error = 0 + cmd = controller.computeCommand(-0.5, ros::Duration(1.0)); + // Then expect command = error + EXPECT_EQ(-0.5, cmd); + + // If call again with same arguments + cmd = controller.computeCommand(-0.5, ros::Duration(1.0)); + // Then expect the pole part to be equal to the proportional part + EXPECT_EQ(-1.0, cmd); + + // Call again with no error + cmd = controller.computeCommand(0.0, ros::Duration(1.0)); + // Then expect the command to be -1.0 + EXPECT_EQ(-1.0, cmd); + + // Call again with no error + cmd = controller.computeCommand(0.0, ros::Duration(1.0)); + // Then expect the command to be -1.0 + EXPECT_EQ(-1.0, cmd); +} + +TEST(CommandTest, completeLeadLagTest) { + RecordProperty("description", + "This test checks that a command is computed correctly using " + "a complete LeadLag controller."); + LeadLag controller(1.0, 1.0, -2.0); + double cmd = 0.0; + + // If initial error = 0 + cmd = controller.computeCommand(-0.5, ros::Duration(1.0)); + // Then expect command = error + EXPECT_EQ(-0.5, cmd); + + // If call again with same arguments + cmd = controller.computeCommand(-0.5, ros::Duration(1.0)); + // The expected command is from the Pole only + EXPECT_EQ(1.0, cmd); + + // If call with zero error + cmd = controller.computeCommand(0.0, ros::Duration(1.0)); + // The zero causes 0.5 but the pole causes -2.0 + EXPECT_EQ(-1.5, cmd); + + // If call with 2.0 + cmd = controller.computeCommand(2.0, ros::Duration(1.0)); + // The proportional causes 2.0 and the pole causes 3.0 + EXPECT_EQ(5.0, cmd); +} + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}