From dfbae6964bd630b3a2786615f459a1466e49a923 Mon Sep 17 00:00:00 2001 From: Walter Fetter Lages Date: Fri, 18 May 2018 20:51:00 -0300 Subject: [PATCH 1/1] Initial commit. --- .gitignore | 51 ++++ CMakeLists.txt | 210 ++++++++++++++++ .../voltage_controllers/joint_command_interface.h | 11 + .../voltage_controllers/joint_effort_controller.h | 208 ++++++++++++++++ .../joint_group_effort_controller.h | 67 ++++++ .../joint_group_voltage_controller.h | 76 ++++++ .../joint_position_controller.h | 208 ++++++++++++++++ .../joint_velocity_controller.h | 178 ++++++++++++++ .../voltage_controllers/joint_voltage_controller.h | 74 ++++++ package.xml | 66 +++++ src/joint_effort_controller.cpp | 267 +++++++++++++++++++++ src/joint_group_effort_controller.cpp | 57 +++++ src/joint_group_voltage_controller.cpp | 57 +++++ src/joint_position_controller.cpp | 267 +++++++++++++++++++++ src/joint_velocity_controller.cpp | 177 ++++++++++++++ src/joint_voltage_controller.cpp | 56 +++++ voltage_controllers_plugins.xml | 67 ++++++ 17 files changed, 2097 insertions(+) create mode 100644 .gitignore create mode 100644 CMakeLists.txt create mode 100644 include/voltage_controllers/joint_command_interface.h create mode 100644 include/voltage_controllers/joint_effort_controller.h create mode 100644 include/voltage_controllers/joint_group_effort_controller.h create mode 100644 include/voltage_controllers/joint_group_voltage_controller.h create mode 100644 include/voltage_controllers/joint_position_controller.h create mode 100644 include/voltage_controllers/joint_velocity_controller.h create mode 100644 include/voltage_controllers/joint_voltage_controller.h create mode 100644 package.xml create mode 100644 src/joint_effort_controller.cpp create mode 100644 src/joint_group_effort_controller.cpp create mode 100644 src/joint_group_voltage_controller.cpp create mode 100644 src/joint_position_controller.cpp create mode 100644 src/joint_velocity_controller.cpp create mode 100644 src/joint_voltage_controller.cpp create mode 100644 voltage_controllers_plugins.xml diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..c1edc29 --- /dev/null +++ b/.gitignore @@ -0,0 +1,51 @@ +devel/ +logs/ +build/ +bin/ +lib/ +msg_gen/ +srv_gen/ +msg/*Action.msg +msg/*ActionFeedback.msg +msg/*ActionGoal.msg +msg/*ActionResult.msg +msg/*Feedback.msg +msg/*Goal.msg +msg/*Result.msg +msg/_*.py +build_isolated/ +devel_isolated/ + +# Generated by dynamic reconfigure +*.cfgc +/cfg/cpp/ +/cfg/*.py + +# Ignore generated docs +#*.dox +*.wikidoc + +# eclipse stuff +.project +.cproject + +# qcreator stuff +CMakeLists.txt.user + +srv/_*.py +*.pcd +*.pyc +qtcreator-* +*.user + +/planning/cfg +/planning/docs +/planning/src + +*~ + +# Emacs +.#* + +# Catkin custom files +CATKIN_IGNORE diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..5742c94 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,210 @@ +cmake_minimum_required(VERSION 2.8.3) +project(voltage_controllers) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + angles + control_msgs + control_toolbox + controller_interface + dynamic_reconfigure + forward_command_controller + realtime_tools + urdf +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a run_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# control_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a run_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include +# LIBRARIES voltage_controllers +# CATKIN_DEPENDS angles control_msgs control_toolbox controller_interface dynamic_reconfigure forward_command_controller realtime_tools urdf +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +add_library(${PROJECT_NAME} + src/joint_voltage_controller.cpp + src/joint_effort_controller.cpp + src/joint_velocity_controller.cpp + src/joint_position_controller.cpp + src/joint_group_voltage_controller.cpp + src/joint_group_effort_controller.cpp +) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/voltage_controllers_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} +) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +install(FILES +# # myfile1 +# # myfile2 + voltage_controllers_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_voltage_controllers.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/include/voltage_controllers/joint_command_interface.h b/include/voltage_controllers/joint_command_interface.h new file mode 100644 index 0000000..670afb0 --- /dev/null +++ b/include/voltage_controllers/joint_command_interface.h @@ -0,0 +1,11 @@ +#ifndef VOLTAGE_CONTROLLERS_JOINT_COMMAND_INTERFACE_H +#define VOLTAGE_CONTROLLERS_JOINT_COMMAND_INTERFACE_H + +#include + +namespace hardware_interface +{ + class VoltageJointInterface: public hardware_interface::JointCommandInterface {}; +} + +#endif diff --git a/include/voltage_controllers/joint_effort_controller.h b/include/voltage_controllers/joint_effort_controller.h new file mode 100644 index 0000000..6ffd59d --- /dev/null +++ b/include/voltage_controllers/joint_effort_controller.h @@ -0,0 +1,208 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2012, hiDOF, Inc. + * Copyright (c) 2013, PAL Robotics, S.L. + * Copyright (c) 2018. Walter Fetter Lages + * 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. + *********************************************************************/ + +/* + joint_effort_controller.h + Author: Walter Fetter Lages + Desc: Voltage output controller to track effort commands, + developed based on effort_controllers/joint_effort_contoller.h +*/ + +#ifndef VOLTAGE_CONTROLLERS_JOINT_EFFORT_CONTROLLER_H +#define VOLTAGE_CONTROLLERS_JOINT_EFFORT_CONTROLLER_H + + +/** + @class voltage_controllers::JointEffortController + @brief Joint Effort Controller + + This class controls positon using a pid loop. + + @section ROS ROS interface + + @param type Must be "voltage_controllers::JointEffortController" + @param joint Name of the joint to control. + @param pid Contains the gains for the PID loop around position. See: control_toolbox::Pid + + Subscribes to: + + - @b command (std_msgs::Float64) : The joint position to achieve. + + Publishes: + + - @b state (control_msgs::JointControllerState) : + Current state of the controller, including pid error and gains. + +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// fix this! +#include + +namespace voltage_controllers +{ + +class JointEffortController: public controller_interface::Controller +{ +public: + + /** + * \brief Store position and velocity command in struct to allow easier realtime buffer usage + */ + struct Commands + { + double position_; // Last commanded position + double velocity_; // Last commanded velocity + bool has_velocity_; // false if no velocity command has been specified + }; + + JointEffortController(); + ~JointEffortController(); + + /** \brief The init function is called to initialize the controller from a + * non-realtime thread with a pointer to the hardware interface, itself, + * instead of a pointer to a RobotHW. + * + * \param robot The specific hardware interface used by this controller. + * + * \param n A NodeHandle in the namespace from which the controller + * should read its configuration, and where it should set up its ROS + * interface. + * + * \returns True if initialization was successful and the controller + * is ready to be started. + */ + bool init(hardware_interface::VoltageJointInterface *robot, ros::NodeHandle &n); + + /*! + * \brief Give set position of the joint for next update: revolute (angle) and prismatic (position) + * + * \param command + */ + void setCommand(double pos_target); + + /*! + * \brief Give set position of the joint for next update: revolute (angle) and prismatic (position) + * Also supports a target velocity + * + * \param pos_target - position setpoint + * \param vel_target - velocity setpoint + */ + void setCommand(double pos_target, double vel_target); + + /** \brief This is called from within the realtime thread just before the + * first call to \ref update + * + * \param time The current time + */ + void starting(const ros::Time& time); + + /*! + * \brief Issues commands to the joint. Should be called at regular intervals + */ + void update(const ros::Time& time, const ros::Duration& period); + + /** + * \brief Get the PID parameters + */ + void getGains(double &p, double &i, double &d, double &i_max, double &i_min); + + /** + * \brief Print debug info to console + */ + void printDebug(); + + /** + * \brief Get the PID parameters + */ + void setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min); + + /** + * \brief Get the name of the joint this controller uses + */ + std::string getJointName(); + + /** + * \brief Get the current position of the joint + * \return current position + */ + double getEffort(); + + hardware_interface::JointHandle joint_; + boost::shared_ptr joint_urdf_; + realtime_tools::RealtimeBuffer command_; + Commands command_struct_; // pre-allocated memory that is re-used to set the realtime buffer + +private: + int loop_count_; + control_toolbox::Pid pid_controller_; /**< Internal PID controller. */ + + boost::scoped_ptr< + realtime_tools::RealtimePublisher< + control_msgs::JointControllerState> > controller_state_publisher_ ; + + ros::Subscriber sub_command_; + + /** + * \brief Callback from /command subscriber for setpoint + */ + void setCommandCB(const std_msgs::Float64ConstPtr& msg); + + /** + * \brief Check that the command is within the hard limits of the joint. Checks for joint + * type first. Sets command to limit if out of bounds. + * \param command - the input to test + */ + void enforceJointLimits(double &command); + +}; + +} // namespace + +#endif diff --git a/include/voltage_controllers/joint_group_effort_controller.h b/include/voltage_controllers/joint_group_effort_controller.h new file mode 100644 index 0000000..ebb3ad3 --- /dev/null +++ b/include/voltage_controllers/joint_group_effort_controller.h @@ -0,0 +1,67 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2012, hiDOF, Inc. + * Copyright (c) 2013, PAL Robotics, S.L. + * Copyright (c) 2014, Fraunhofer IPA + * Copyright (c) 2018. Walter Fetter Lages + * 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. + *********************************************************************/ + +#ifndef VOLTAGE_CONTROLLERS_JOINT_GROUP_EFFORT_CONTROLLER_H +#define VOLTAGE_CONTROLLERS_JOINT_GROUP_EFFORT_CONTROLLER_H + +#include + +// fix this! +#include + +namespace voltage_controllers +{ + +/** + * \brief Forward command controller for a set of voltage controlled joints. + * + * This class forwards the commanded efforts down to a set of joints. + * + * \section ROS interface + * + * \param type Must be "JointGroupEffortController". + * \param joints List of names of the joints to control. + * + * Subscribes to: + * - \b command (std_msgs::Float64MultiArray) : The joint efforts to apply + */ +typedef forward_command_controller::ForwardJointGroupCommandController + JointGroupEffortController; +} + +#endif diff --git a/include/voltage_controllers/joint_group_voltage_controller.h b/include/voltage_controllers/joint_group_voltage_controller.h new file mode 100644 index 0000000..2d6e8d1 --- /dev/null +++ b/include/voltage_controllers/joint_group_voltage_controller.h @@ -0,0 +1,76 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2012, hiDOF, Inc. + * Copyright (c) 2013, PAL Robotics, S.L. + * Copyright (c) 2014, Fraunhofer IPA + * Copyright (c) 2018. Walter Fetter Lages + * 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. + *********************************************************************/ + +/* + joint_group_voltage_controller.h + Author: Walter Fetter Lages + Desc: Voltage output group controller to track voltage commands, + developed based on joint_group_effort_contoller.h +*/ + + +#ifndef VOLTAGE_CONTROLLERS_JOINT_GROUP_VOLTAGE_CONTROLLER_H +#define VOLTAGE_CONTROLLERS_JOINT_GROUP_VOLTAGE_CONTROLLER_H + +#include + +// fix this! +#include + + +namespace voltage_controllers +{ + +/** + * \brief Forward command controller for a set of voltage controlled joints. + * + * This class forwards the commanded voltages down to a set of joints. + * + * \section ROS interface + * + * \param type Must be "JointGroupVoltagteController". + * \param joints List of names of the joints to control. + * + * Subscribes to: + * - \b command (std_msgs::Float64MultiArray) : The joint voltagess to apply + */ +typedef forward_command_controller::ForwardJointGroupCommandController + JointGroupVoltageController; +} + +#endif diff --git a/include/voltage_controllers/joint_position_controller.h b/include/voltage_controllers/joint_position_controller.h new file mode 100644 index 0000000..1adce92 --- /dev/null +++ b/include/voltage_controllers/joint_position_controller.h @@ -0,0 +1,208 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2012, hiDOF, Inc. + * Copyright (c) 2018. Walter Fetter Lages + * 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. + *********************************************************************/ + +/* + joint_position_controller.h + Author: Walter Fetter Lages + Desc: Voltage output controller to track position commands, + developed based on effort_controllers/joint_position_contoller.h +*/ + + +#ifndef VOLTAGE_CONTROLLERS_JOINT_POSITION_CONTROLLER_H +#define VOLTAGE_CONTROLLERS_JOINT_POSITION_CONTROLLER_H + +/** + @class voltage_controllers::JointPositionController + @brief Joint Position Controller + + This class controls positon using a pid loop. + + @section ROS ROS interface + + @param type Must be "voltage_controllers::JointPositionController" + @param joint Name of the joint to control. + @param pid Contains the gains for the PID loop around position. See: control_toolbox::Pid + + Subscribes to: + + - @b command (std_msgs::Float64) : The joint position to achieve. + + Publishes: + + - @b state (control_msgs::JointControllerState) : + Current state of the controller, including pid error and gains. + +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// fix this! +#include + + +namespace voltage_controllers +{ + +class JointPositionController: public controller_interface::Controller +{ +public: + + /** + * \brief Store position and velocity command in struct to allow easier realtime buffer usage + */ + struct Commands + { + double position_; // Last commanded position + double velocity_; // Last commanded velocity + bool has_velocity_; // false if no velocity command has been specified + }; + + JointPositionController(); + ~JointPositionController(); + + /** \brief The init function is called to initialize the controller from a + * non-realtime thread with a pointer to the hardware interface, itself, + * instead of a pointer to a RobotHW. + * + * \param robot The specific hardware interface used by this controller. + * + * \param n A NodeHandle in the namespace from which the controller + * should read its configuration, and where it should set up its ROS + * interface. + * + * \returns True if initialization was successful and the controller + * is ready to be started. + */ + bool init(hardware_interface::VoltageJointInterface *robot, ros::NodeHandle &n); + + /*! + * \brief Give set position of the joint for next update: revolute (angle) and prismatic (position) + * + * \param command + */ + void setCommand(double pos_target); + + /*! + * \brief Give set position of the joint for next update: revolute (angle) and prismatic (position) + * Also supports a target velocity + * + * \param pos_target - position setpoint + * \param vel_target - velocity setpoint + */ + void setCommand(double pos_target, double vel_target); + + /** \brief This is called from within the realtime thread just before the + * first call to \ref update + * + * \param time The current time + */ + void starting(const ros::Time& time); + + /*! + * \brief Issues commands to the joint. Should be called at regular intervals + */ + void update(const ros::Time& time, const ros::Duration& period); + + /** + * \brief Get the PID parameters + */ + void getGains(double &p, double &i, double &d, double &i_max, double &i_min); + + /** + * \brief Print debug info to console + */ + void printDebug(); + + /** + * \brief Get the PID parameters + */ + void setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min); + + /** + * \brief Get the name of the joint this controller uses + */ + std::string getJointName(); + + /** + * \brief Get the current position of the joint + * \return current position + */ + double getPosition(); + + hardware_interface::JointHandle joint_; + boost::shared_ptr joint_urdf_; + realtime_tools::RealtimeBuffer command_; + Commands command_struct_; // pre-allocated memory that is re-used to set the realtime buffer + +private: + int loop_count_; + control_toolbox::Pid pid_controller_; /**< Internal PID controller. */ + + boost::scoped_ptr< + realtime_tools::RealtimePublisher< + control_msgs::JointControllerState> > controller_state_publisher_ ; + + ros::Subscriber sub_command_; + + /** + * \brief Callback from /command subscriber for setpoint + */ + void setCommandCB(const std_msgs::Float64ConstPtr& msg); + + /** + * \brief Check that the command is within the hard limits of the joint. Checks for joint + * type first. Sets command to limit if out of bounds. + * \param command - the input to test + */ + void enforceJointLimits(double &command); + +}; + +} // namespace + +#endif diff --git a/include/voltage_controllers/joint_velocity_controller.h b/include/voltage_controllers/joint_velocity_controller.h new file mode 100644 index 0000000..728a7bd --- /dev/null +++ b/include/voltage_controllers/joint_velocity_controller.h @@ -0,0 +1,178 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2012, hiDOF, Inc. + * Copyright (c) 2018. Walter Fetter Lages + * 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. + *********************************************************************/ + +/* + joint_velocity_controller.h + Author: Walter Fetter Lages + Desc: Voltage output controller to track velocity commands, + developed based on effort_controllers/joint_effort_contoller.h +*/ + +// fix this! +#include + + +#ifndef VOLTAGE_CONTROLLERS_JOINT_VELOCITY_CONTROLLER_H +#define VOLTAGE_CONTROLLERS_JOINT_VELOCITY_CONTROLLER_H + +/** + @class voltage_controllers::JointVelocityController + @brief Joint Velocity Controller + + This class controls velocity using a pid loop. + + @section ROS ROS interface + + @param type Must be "voltage_controllers::JointVelocityController" + @param joint Name of the joint to control. + @param pid Contains the gains for the PID loop around velocity. See: control_toolbox::Pid + + Subscribes to: + + - @b command (std_msgs::Float64) : The joint velocity to achieve. + + Publishes: + + - @b state (control_msgs::JointControllerState) : + Current state of the controller, including pid error and gains. + +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace voltage_controllers +{ + +class JointVelocityController: public controller_interface::Controller +{ +public: + + JointVelocityController(); + ~JointVelocityController(); + + bool init(hardware_interface::VoltageJointInterface *robot, const std::string &joint_name, const control_toolbox::Pid &pid); + + /** \brief The init function is called to initialize the controller from a + * non-realtime thread with a pointer to the hardware interface, itself, + * instead of a pointer to a RobotHW. + * + * \param robot The specific hardware interface used by this controller. + * + * \param n A NodeHandle in the namespace from which the controller + * should read its configuration, and where it should set up its ROS + * interface. + * + * \returns True if initialization was successful and the controller + * is ready to be started. + */ + bool init(hardware_interface::VoltageJointInterface *robot, ros::NodeHandle &n); + + /*! + * \brief Give set velocity of the joint for next update: revolute (angle) and prismatic (velocity) + * + * \param double pos Velocity command to issue + */ + void setCommand(double cmd); + + /*! + * \brief Get latest velocity command to the joint: revolute (angle) and prismatic (velocity). + */ + void getCommand(double & cmd); + + /** \brief This is called from within the realtime thread just before the + * first call to \ref update + * + * \param time The current time + */ + void starting(const ros::Time& time); + + /*! + * \brief Issues commands to the joint. Should be called at regular intervals + */ + void update(const ros::Time& time, const ros::Duration& period); + + /** + * \brief Get the PID parameters + */ + void getGains(double &p, double &i, double &d, double &i_max, double &i_min); + + /** + * \brief Print debug info to console + */ + void printDebug(); + + /** + * \brief Get the PID parameters + */ + void setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min); + + /** + * \brief Get the name of the joint this controller uses + */ + std::string getJointName(); + + hardware_interface::JointHandle joint_; + double command_; /**< Last commanded velocity. */ + +private: + int loop_count_; + control_toolbox::Pid pid_controller_; /**< Internal PID controller. */ + + //friend class JointVelocityControllerNode; // what is this for?? + + boost::scoped_ptr< + realtime_tools::RealtimePublisher< + control_msgs::JointControllerState> > controller_state_publisher_ ; + + ros::Subscriber sub_command_; + + /** + * \brief Callback from /command subscriber for setpoint + */ + void setCommandCB(const std_msgs::Float64ConstPtr& msg); +}; + +} // namespace + +#endif diff --git a/include/voltage_controllers/joint_voltage_controller.h b/include/voltage_controllers/joint_voltage_controller.h new file mode 100644 index 0000000..44dca7a --- /dev/null +++ b/include/voltage_controllers/joint_voltage_controller.h @@ -0,0 +1,74 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2012, hiDOF, Inc. + * Copyright (c) 2013, PAL Robotics, S.L. + * Copyright (c) 2018. Walter Fetter Lages. + * 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. + *********************************************************************/ + +/* + joint_voltage_controller.h + Author: Walter Fetter Lages + Desc: Voltage output controller to track voltage commands, + developed based on effort_controllers/joint_effort_contoller.h +*/ + + +#ifndef VOLTAGE_CONTROLLERS_JOINT_VOLTAGE_CONTROLLER_H +#define VOLTAGE_CONTROLLERS_JOINT_VOLTAGE_CONTROLLER_H + +#include + +// fix this! +#include + +namespace voltage_controllers +{ + +/** + * \brief Joint Voltate Controller + * + * This class passes the commanded voltate down to the joint. + * + * \section ROS interface + * + * \param type Must be "JointVoltateController". + * \param joint Name of the joint to control. + * + * Subscribes to: + * - \b command (std_msgs::Float64) : The joint voltate to apply. + */ +typedef forward_command_controller::ForwardCommandController + JointVoltageController; +} + +#endif diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..43607ee --- /dev/null +++ b/package.xml @@ -0,0 +1,66 @@ + + + voltage_controllers + 0.0.0 + The voltage_controllers package + + + + + Walter Fetter Lages + + + + + + GPLv3 + + + + + + + + + + + + + Walter Fetter Lages + + + + + + + + + + + + + catkin + angles + control_msgs + control_toolbox + controller_interface + dynamic_reconfigure + forward_command_controller + realtime_tools + urdf + angles + control_msgs + control_toolbox + controller_interface + dynamic_reconfigure + forward_command_controller + realtime_tools + urdf + + + + + + + + diff --git a/src/joint_effort_controller.cpp b/src/joint_effort_controller.cpp new file mode 100644 index 0000000..ecc8567 --- /dev/null +++ b/src/joint_effort_controller.cpp @@ -0,0 +1,267 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2012, hiDOF, Inc. + * Copyright (c) 2018, Walter Fetter Lages + * 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: Vijay Pradeep + Contributors: Jonathan Bohren, Wim Meeussen, Dave Coleman + Desc: Effort(force)-based position controller using basic PID loop + + Author: Walter Fetter Lages + Desc: Voltage output controller to track effort commands, + developed based on effort_controllers/joint_position_controller.cpp +*/ + +#include +#include +#include + +namespace voltage_controllers { + +JointEffortController::JointEffortController() + : loop_count_(0) +{} + +JointEffortController::~JointEffortController() +{ + sub_command_.shutdown(); +} + +bool JointEffortController::init(hardware_interface::VoltageJointInterface *robot, ros::NodeHandle &n) +{ + // Get joint name from parameter server + std::string joint_name; + if (!n.getParam("joint", joint_name)) + { + ROS_ERROR("No joint given (namespace: %s)", n.getNamespace().c_str()); + return false; + } + + // Load PID Controller using gains set on parameter server + if (!pid_controller_.init(ros::NodeHandle(n, "pid"))) + return false; + + // Start realtime state publisher + controller_state_publisher_.reset( + new realtime_tools::RealtimePublisher(n, "state", 1)); + + // Start command subscriber + sub_command_ = n.subscribe("command", 1, &JointEffortController::setCommandCB, this); + + // Get joint handle from hardware interface + joint_ = robot->getHandle(joint_name); + + // Get URDF info about joint + urdf::Model urdf; + if (!urdf.initParam("robot_description")) + { + ROS_ERROR("Failed to parse urdf file"); + return false; + } + joint_urdf_ = urdf.getJoint(joint_name); + if (!joint_urdf_) + { + ROS_ERROR("Could not find joint '%s' in urdf", joint_name.c_str()); + return false; + } + + return true; +} + +void JointEffortController::setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min) +{ + pid_controller_.setGains(p,i,d,i_max,i_min); +} + +void JointEffortController::getGains(double &p, double &i, double &d, double &i_max, double &i_min) +{ + pid_controller_.getGains(p,i,d,i_max,i_min); +} + +void JointEffortController::printDebug() +{ + pid_controller_.printValues(); +} + +std::string JointEffortController::getJointName() +{ + return joint_.getName(); +} + +double JointEffortController::getEffort() +{ + return joint_.getEffort(); +} + +// Set the joint position command +void JointEffortController::setCommand(double pos_command) +{ + command_struct_.position_ = pos_command; + command_struct_.has_velocity_ = false; // Flag to ignore the velocity command since our setCommand method did not include it + + // the writeFromNonRT can be used in RT, if you have the guarantee that + // * no non-rt thread is calling the same function (we're not subscribing to ros callbacks) + // * there is only one single rt thread + command_.writeFromNonRT(command_struct_); +} + +// Set the joint position command with a velocity command as well +void JointEffortController::setCommand(double pos_command, double vel_command) +{ + command_struct_.position_ = pos_command; + command_struct_.velocity_ = vel_command; + command_struct_.has_velocity_ = true; + + command_.writeFromNonRT(command_struct_); +} + +void JointEffortController::starting(const ros::Time& time) +{ + double pos_command = joint_.getEffort(); + + // Make sure joint is within limits if applicable + enforceJointLimits(pos_command); + + command_struct_.position_ = pos_command; + command_struct_.has_velocity_ = false; + + command_.initRT(command_struct_); + + pid_controller_.reset(); +} + +void JointEffortController::update(const ros::Time& time, const ros::Duration& period) +{ + command_struct_ = *(command_.readFromRT()); + double command_position = command_struct_.position_; + double command_velocity = command_struct_.velocity_; + bool has_velocity_ = command_struct_.has_velocity_; + + double error, vel_error; + double commanded_effort; + + double current_position = joint_.getEffort(); + + // Make sure joint is within limits if applicable + enforceJointLimits(command_position); + + // Compute position error + if (joint_urdf_->type == urdf::Joint::REVOLUTE) + { + angles::shortest_angular_distance_with_limits( + current_position, + command_position, + joint_urdf_->limits->lower, + joint_urdf_->limits->upper, + error); + } + else if (joint_urdf_->type == urdf::Joint::CONTINUOUS) + { + error = angles::shortest_angular_distance(current_position, command_position); + } + else //prismatic + { + error = command_position - current_position; + } + + // Decide which of the two PID computeCommand() methods to call + if (has_velocity_) + { + // Compute velocity error if a non-zero velocity command was given + vel_error = command_velocity - joint_.getVelocity(); + + // 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. + commanded_effort = pid_controller_.computeCommand(error, vel_error, period); + } + else + { + // Set the PID error and compute the PID command with nonuniform + // time step size. + commanded_effort = pid_controller_.computeCommand(error, period); + } + + joint_.setCommand(commanded_effort); + + // publish state + if (loop_count_ % 10 == 0) + { + if(controller_state_publisher_ && controller_state_publisher_->trylock()) + { + controller_state_publisher_->msg_.header.stamp = time; + controller_state_publisher_->msg_.set_point = command_position; + controller_state_publisher_->msg_.process_value = current_position; + controller_state_publisher_->msg_.process_value_dot = joint_.getVelocity(); + controller_state_publisher_->msg_.error = error; + controller_state_publisher_->msg_.time_step = period.toSec(); + controller_state_publisher_->msg_.command = commanded_effort; + + double dummy; + getGains(controller_state_publisher_->msg_.p, + controller_state_publisher_->msg_.i, + controller_state_publisher_->msg_.d, + controller_state_publisher_->msg_.i_clamp, + dummy); + controller_state_publisher_->unlockAndPublish(); + } + } + loop_count_++; +} + +void JointEffortController::setCommandCB(const std_msgs::Float64ConstPtr& msg) +{ + setCommand(msg->data); +} + +// Note: we may want to remove this function once issue https://github.com/ros/angles/issues/2 is resolved +void JointEffortController::enforceJointLimits(double &command) +{ + // Check that this joint has applicable limits + if (joint_urdf_->type == urdf::Joint::REVOLUTE || joint_urdf_->type == urdf::Joint::PRISMATIC) + { + if( command > joint_urdf_->limits->upper ) // above upper limnit + { + command = joint_urdf_->limits->upper; + } + else if( command < joint_urdf_->limits->lower ) // below lower limit + { + command = joint_urdf_->limits->lower; + } + } +} + +} // namespace + +PLUGINLIB_EXPORT_CLASS( voltage_controllers::JointEffortController, controller_interface::ControllerBase) diff --git a/src/joint_group_effort_controller.cpp b/src/joint_group_effort_controller.cpp new file mode 100644 index 0000000..ff6b440 --- /dev/null +++ b/src/joint_group_effort_controller.cpp @@ -0,0 +1,57 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2012, hiDOF, Inc. + * Copyright (c) 2013, PAL Robotics, S.L. + * Copyright (c) 2014, Fraunhofer IPA + * Copyright (c) 2018, Walter Fetter Lages + * 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: Walter Fetter Lages + Desc: Voltage output group controller to track effort commands, + developed based on effort_controllers/joint_group_effort_contoller.cpp +*/ + + +#include +#include + +template +void forward_command_controller::ForwardJointGroupCommandController::starting(const ros::Time& time) +{ + // Start controller with 0.0 efforts + commands_buffer_.readFromRT()->assign(n_joints_, 0.0); +} + + +PLUGINLIB_EXPORT_CLASS(voltage_controllers::JointGroupEffortController,controller_interface::ControllerBase) diff --git a/src/joint_group_voltage_controller.cpp b/src/joint_group_voltage_controller.cpp new file mode 100644 index 0000000..e5c37a3 --- /dev/null +++ b/src/joint_group_voltage_controller.cpp @@ -0,0 +1,57 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2012, hiDOF, Inc. + * Copyright (c) 2013, PAL Robotics, S.L. + * Copyright (c) 2014, Fraunhofer IPA + * Copyright (c) 2018, Walter Fetter Lages + * 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: Walter Fetter Lages + Desc: Voltage output group controller to track voltage commands, + developed based on effort_controllers/joint_group_effort_contoller.cpp +*/ + + +#include +#include + +template +void forward_command_controller::ForwardJointGroupCommandController::starting(const ros::Time& time) +{ + // Start controller with 0.0 efforts + commands_buffer_.readFromRT()->assign(n_joints_, 0.0); +} + + +PLUGINLIB_EXPORT_CLASS(voltage_controllers::JointGroupVoltageController,controller_interface::ControllerBase) diff --git a/src/joint_position_controller.cpp b/src/joint_position_controller.cpp new file mode 100644 index 0000000..8b1e752 --- /dev/null +++ b/src/joint_position_controller.cpp @@ -0,0 +1,267 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2012, hiDOF, Inc. + * Copyright (c) 2018, Walter Fetter Lages + * 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: Vijay Pradeep + Contributors: Jonathan Bohren, Wim Meeussen, Dave Coleman + Desc: Effort(force)-based position controller using basic PID loop + + Author: Walter Fetter Lages + Desc: Voltage output controller to track position commands, + developed based on effort_controllers/joint_position_controller.cpp +*/ + +#include +#include +#include + +namespace voltage_controllers { + +JointPositionController::JointPositionController() + : loop_count_(0) +{} + +JointPositionController::~JointPositionController() +{ + sub_command_.shutdown(); +} + +bool JointPositionController::init(hardware_interface::VoltageJointInterface *robot, ros::NodeHandle &n) +{ + // Get joint name from parameter server + std::string joint_name; + if (!n.getParam("joint", joint_name)) + { + ROS_ERROR("No joint given (namespace: %s)", n.getNamespace().c_str()); + return false; + } + + // Load PID Controller using gains set on parameter server + if (!pid_controller_.init(ros::NodeHandle(n, "pid"))) + return false; + + // Start realtime state publisher + controller_state_publisher_.reset( + new realtime_tools::RealtimePublisher(n, "state", 1)); + + // Start command subscriber + sub_command_ = n.subscribe("command", 1, &JointPositionController::setCommandCB, this); + + // Get joint handle from hardware interface + joint_ = robot->getHandle(joint_name); + + // Get URDF info about joint + urdf::Model urdf; + if (!urdf.initParam("robot_description")) + { + ROS_ERROR("Failed to parse urdf file"); + return false; + } + joint_urdf_ = urdf.getJoint(joint_name); + if (!joint_urdf_) + { + ROS_ERROR("Could not find joint '%s' in urdf", joint_name.c_str()); + return false; + } + + return true; +} + +void JointPositionController::setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min) +{ + pid_controller_.setGains(p,i,d,i_max,i_min); +} + +void JointPositionController::getGains(double &p, double &i, double &d, double &i_max, double &i_min) +{ + pid_controller_.getGains(p,i,d,i_max,i_min); +} + +void JointPositionController::printDebug() +{ + pid_controller_.printValues(); +} + +std::string JointPositionController::getJointName() +{ + return joint_.getName(); +} + +double JointPositionController::getPosition() +{ + return joint_.getPosition(); +} + +// Set the joint position command +void JointPositionController::setCommand(double pos_command) +{ + command_struct_.position_ = pos_command; + command_struct_.has_velocity_ = false; // Flag to ignore the velocity command since our setCommand method did not include it + + // the writeFromNonRT can be used in RT, if you have the guarantee that + // * no non-rt thread is calling the same function (we're not subscribing to ros callbacks) + // * there is only one single rt thread + command_.writeFromNonRT(command_struct_); +} + +// Set the joint position command with a velocity command as well +void JointPositionController::setCommand(double pos_command, double vel_command) +{ + command_struct_.position_ = pos_command; + command_struct_.velocity_ = vel_command; + command_struct_.has_velocity_ = true; + + command_.writeFromNonRT(command_struct_); +} + +void JointPositionController::starting(const ros::Time& time) +{ + double pos_command = joint_.getPosition(); + + // Make sure joint is within limits if applicable + enforceJointLimits(pos_command); + + command_struct_.position_ = pos_command; + command_struct_.has_velocity_ = false; + + command_.initRT(command_struct_); + + pid_controller_.reset(); +} + +void JointPositionController::update(const ros::Time& time, const ros::Duration& period) +{ + command_struct_ = *(command_.readFromRT()); + double command_position = command_struct_.position_; + double command_velocity = command_struct_.velocity_; + bool has_velocity_ = command_struct_.has_velocity_; + + double error, vel_error; + double commanded_effort; + + double current_position = joint_.getPosition(); + + // Make sure joint is within limits if applicable + enforceJointLimits(command_position); + + // Compute position error + if (joint_urdf_->type == urdf::Joint::REVOLUTE) + { + angles::shortest_angular_distance_with_limits( + current_position, + command_position, + joint_urdf_->limits->lower, + joint_urdf_->limits->upper, + error); + } + else if (joint_urdf_->type == urdf::Joint::CONTINUOUS) + { + error = angles::shortest_angular_distance(current_position, command_position); + } + else //prismatic + { + error = command_position - current_position; + } + + // Decide which of the two PID computeCommand() methods to call + if (has_velocity_) + { + // Compute velocity error if a non-zero velocity command was given + vel_error = command_velocity - joint_.getVelocity(); + + // 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. + commanded_effort = pid_controller_.computeCommand(error, vel_error, period); + } + else + { + // Set the PID error and compute the PID command with nonuniform + // time step size. + commanded_effort = pid_controller_.computeCommand(error, period); + } + + joint_.setCommand(commanded_effort); + + // publish state + if (loop_count_ % 10 == 0) + { + if(controller_state_publisher_ && controller_state_publisher_->trylock()) + { + controller_state_publisher_->msg_.header.stamp = time; + controller_state_publisher_->msg_.set_point = command_position; + controller_state_publisher_->msg_.process_value = current_position; + controller_state_publisher_->msg_.process_value_dot = joint_.getVelocity(); + controller_state_publisher_->msg_.error = error; + controller_state_publisher_->msg_.time_step = period.toSec(); + controller_state_publisher_->msg_.command = commanded_effort; + + double dummy; + getGains(controller_state_publisher_->msg_.p, + controller_state_publisher_->msg_.i, + controller_state_publisher_->msg_.d, + controller_state_publisher_->msg_.i_clamp, + dummy); + controller_state_publisher_->unlockAndPublish(); + } + } + loop_count_++; +} + +void JointPositionController::setCommandCB(const std_msgs::Float64ConstPtr& msg) +{ + setCommand(msg->data); +} + +// Note: we may want to remove this function once issue https://github.com/ros/angles/issues/2 is resolved +void JointPositionController::enforceJointLimits(double &command) +{ + // Check that this joint has applicable limits + if (joint_urdf_->type == urdf::Joint::REVOLUTE || joint_urdf_->type == urdf::Joint::PRISMATIC) + { + if( command > joint_urdf_->limits->upper ) // above upper limnit + { + command = joint_urdf_->limits->upper; + } + else if( command < joint_urdf_->limits->lower ) // below lower limit + { + command = joint_urdf_->limits->lower; + } + } +} + +} // namespace + +PLUGINLIB_EXPORT_CLASS( voltage_controllers::JointPositionController, controller_interface::ControllerBase) diff --git a/src/joint_velocity_controller.cpp b/src/joint_velocity_controller.cpp new file mode 100644 index 0000000..e3d7aaf --- /dev/null +++ b/src/joint_velocity_controller.cpp @@ -0,0 +1,177 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2012, hiDOF, Inc. + * Copyright (c) 2018, Walter Fetter Lages + * 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: Walter Fetter Lages + Desc: Voltage output controller to track position commands, + developed based on effort_controllers/joint_velocity_controller.cpp +*/ + +#include +#include + + +namespace voltage_controllers { + +JointVelocityController::JointVelocityController() +: command_(0), loop_count_(0) +{} + +JointVelocityController::~JointVelocityController() +{ + sub_command_.shutdown(); +} + +bool JointVelocityController::init(hardware_interface::VoltageJointInterface *robot, + const std::string &joint_name, const control_toolbox::Pid &pid) +{ + pid_controller_ = pid; + + // Get joint handle from hardware interface + joint_ = robot->getHandle(joint_name); + + return true; +} + +bool JointVelocityController::init(hardware_interface::VoltageJointInterface *robot, ros::NodeHandle &n) +{ + // Get joint name from parameter server + std::string joint_name; + if (!n.getParam("joint", joint_name)) { + ROS_ERROR("No joint given (namespace: %s)", n.getNamespace().c_str()); + return false; + } + + // Get joint handle from hardware interface + joint_ = robot->getHandle(joint_name); + + // Load PID Controller using gains set on parameter server + if (!pid_controller_.init(ros::NodeHandle(n, "pid"))) + return false; + + // Start realtime state publisher + controller_state_publisher_.reset( + new realtime_tools::RealtimePublisher + (n, "state", 1)); + + // Start command subscriber + sub_command_ = n.subscribe("command", 1, &JointVelocityController::setCommandCB, this); + + return true; +} + + +void JointVelocityController::setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min) +{ + pid_controller_.setGains(p,i,d,i_max,i_min); + +} + +void JointVelocityController::getGains(double &p, double &i, double &d, double &i_max, double &i_min) +{ + pid_controller_.getGains(p,i,d,i_max,i_min); +} + +void JointVelocityController::printDebug() +{ + pid_controller_.printValues(); +} + +std::string JointVelocityController::getJointName() +{ + return joint_.getName(); +} + +// Set the joint velocity command +void JointVelocityController::setCommand(double cmd) +{ + command_ = cmd; +} + +// Return the current velocity command +void JointVelocityController::getCommand(double& cmd) +{ + cmd = command_; +} + +void JointVelocityController::starting(const ros::Time& time) +{ + command_ = 0.0; + pid_controller_.reset(); +} + +void JointVelocityController::update(const ros::Time& time, const ros::Duration& period) +{ + double error = command_ - joint_.getVelocity(); + + // Set the PID error and compute the PID command with nonuniform time + // step size. The derivative error is computed from the change in the error + // and the timestep dt. + double commanded_effort = pid_controller_.computeCommand(error, period); + + joint_.setCommand(commanded_effort); + + if(loop_count_ % 10 == 0) + { + if(controller_state_publisher_ && controller_state_publisher_->trylock()) + { + controller_state_publisher_->msg_.header.stamp = time; + controller_state_publisher_->msg_.set_point = command_; + controller_state_publisher_->msg_.process_value = joint_.getVelocity(); + controller_state_publisher_->msg_.error = error; + controller_state_publisher_->msg_.time_step = period.toSec(); + controller_state_publisher_->msg_.command = commanded_effort; + + double dummy; + getGains(controller_state_publisher_->msg_.p, + controller_state_publisher_->msg_.i, + controller_state_publisher_->msg_.d, + controller_state_publisher_->msg_.i_clamp, + dummy); + controller_state_publisher_->unlockAndPublish(); + } + } + loop_count_++; +} + +void JointVelocityController::setCommandCB(const std_msgs::Float64ConstPtr& msg) +{ + command_ = msg->data; +} + +} // namespace + +PLUGINLIB_EXPORT_CLASS( voltage_controllers::JointVelocityController, controller_interface::ControllerBase) diff --git a/src/joint_voltage_controller.cpp b/src/joint_voltage_controller.cpp new file mode 100644 index 0000000..a8bfeb2 --- /dev/null +++ b/src/joint_voltage_controller.cpp @@ -0,0 +1,56 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2012, hiDOF, Inc. + * Copyright (c) 2013, PAL Robotics, S.L. + * Copyright (c) 2018, Walter Fetter Lages + * 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: Walter Fetter Lages + Desc: Voltage output controller to track voltage commands, + developed based on effort_controllers/joint_effort_controller.cpp +*/ + + +#include +#include + +template +void forward_command_controller::ForwardCommandController::starting(const ros::Time& time) +{ + // Start controller with 0.0 effort + command_buffer_.writeFromNonRT(0.0); +} + + +PLUGINLIB_EXPORT_CLASS(voltage_controllers::JointVoltageController,controller_interface::ControllerBase) diff --git a/voltage_controllers_plugins.xml b/voltage_controllers_plugins.xml new file mode 100644 index 0000000..6373a3a --- /dev/null +++ b/voltage_controllers_plugins.xml @@ -0,0 +1,67 @@ + + + + + The JointPositionController tracks position commands. It expects a + VoltageJointInterface type of hardware interface. + + + + + + The JointVelocityController tracks velocity commands. It expects a + VoltageJointInterface type of hardware interface. + + + + + + The JointEffortController tracks voltage commands. It expects a + VoltageJointInterface type of hardware interface. + + + + + + The JointGroupEffortController tracks voltage commands for a set of joints. It expects a + VoltageJointInterface type of hardware interface. + + + + + + The JointVoltagetController tracks voltage commands. It expects a + VoltageJointInterface type of hardware interface. + + + + + + The JointGroupVoltateController tracks voltage commands for a set of joints. It expects a + VoltageJointInterface type of hardware interface. + + + + + + + The JointGroupPositionController tracks position commands for a set of joints. It expects a + VoltageJointInterface type of hardware interface. + + + + -- 2.12.0