--- /dev/null
+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
--- /dev/null
+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)
--- /dev/null
+#ifndef VOLTAGE_CONTROLLERS_JOINT_COMMAND_INTERFACE_H
+#define VOLTAGE_CONTROLLERS_JOINT_COMMAND_INTERFACE_H
+
+#include <hardware_interface/joint_command_interface.h>
+
+namespace hardware_interface
+{
+ class VoltageJointInterface: public hardware_interface::JointCommandInterface {};
+}
+
+#endif
--- /dev/null
+/*********************************************************************
+ * 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 <ros/node_handle.h>
+#include <urdf/model.h>
+#include <control_toolbox/pid.h>
+#include <boost/scoped_ptr.hpp>
+#include <boost/thread/condition.hpp>
+#include <realtime_tools/realtime_publisher.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <controller_interface/controller.h>
+#include <control_msgs/JointControllerState.h>
+#include <std_msgs/Float64.h>
+#include <control_msgs/JointControllerState.h>
+#include <realtime_tools/realtime_buffer.h>
+
+// fix this!
+#include <voltage_controllers/joint_command_interface.h>
+
+namespace voltage_controllers
+{
+
+class JointEffortController: public controller_interface::Controller<hardware_interface::VoltageJointInterface>
+{
+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<const urdf::Joint> joint_urdf_;
+ realtime_tools::RealtimeBuffer<Commands> 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
--- /dev/null
+/*********************************************************************
+ * 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 <forward_command_controller/forward_joint_group_command_controller.h>
+
+// fix this!
+#include <voltage_controllers/joint_command_interface.h>
+
+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<hardware_interface::VoltageJointInterface>
+ JointGroupEffortController;
+}
+
+#endif
--- /dev/null
+/*********************************************************************
+ * 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 <forward_command_controller/forward_joint_group_command_controller.h>
+
+// fix this!
+#include <voltage_controllers/joint_command_interface.h>
+
+
+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<hardware_interface::VoltageJointInterface>
+ JointGroupVoltageController;
+}
+
+#endif
--- /dev/null
+/*********************************************************************
+ * 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 <ros/node_handle.h>
+#include <urdf/model.h>
+#include <control_toolbox/pid.h>
+#include <boost/scoped_ptr.hpp>
+#include <boost/thread/condition.hpp>
+#include <realtime_tools/realtime_publisher.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <controller_interface/controller.h>
+#include <control_msgs/JointControllerState.h>
+#include <std_msgs/Float64.h>
+#include <control_msgs/JointControllerState.h>
+#include <realtime_tools/realtime_buffer.h>
+
+// fix this!
+#include <voltage_controllers/joint_command_interface.h>
+
+
+namespace voltage_controllers
+{
+
+class JointPositionController: public controller_interface::Controller<hardware_interface::VoltageJointInterface>
+{
+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<const urdf::Joint> joint_urdf_;
+ realtime_tools::RealtimeBuffer<Commands> 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
--- /dev/null
+/*********************************************************************
+ * 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 <voltage_controllers/joint_command_interface.h>
+
+
+#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 <ros/node_handle.h>
+#include <boost/thread/condition.hpp>
+#include <boost/scoped_ptr.hpp>
+#include <hardware_interface/joint_command_interface.h>
+#include <controller_interface/controller.h>
+#include <control_msgs/JointControllerState.h>
+#include <std_msgs/Float64.h>
+#include <control_toolbox/pid.h>
+#include <realtime_tools/realtime_publisher.h>
+
+namespace voltage_controllers
+{
+
+class JointVelocityController: public controller_interface::Controller<hardware_interface::VoltageJointInterface>
+{
+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
--- /dev/null
+/*********************************************************************
+ * 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 <forward_command_controller/forward_command_controller.h>
+
+// fix this!
+#include <voltage_controllers/joint_command_interface.h>
+
+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<hardware_interface::VoltageJointInterface>
+ JointVoltageController;
+}
+
+#endif
--- /dev/null
+<?xml version="1.0"?>
+<package>
+ <name>voltage_controllers</name>
+ <version>0.0.0</version>
+ <description>The voltage_controllers package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="fetter@ece.ufrgs.br">Walter Fetter Lages</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>GPLv3</license>
+
+
+ <!-- Url tags are optional, but multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/voltage_controllers</url> -->
+
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+ <author email="fetter@ece.ufrgs.br">Walter Fetter Lages</author>
+
+ <!-- The *_depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use run_depend for packages you need at runtime: -->
+ <!-- <run_depend>message_runtime</run_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>angles</build_depend>
+ <build_depend>control_msgs</build_depend>
+ <build_depend>control_toolbox</build_depend>
+ <build_depend>controller_interface</build_depend>
+ <build_depend>dynamic_reconfigure</build_depend>
+ <build_depend>forward_command_controller</build_depend>
+ <build_depend>realtime_tools</build_depend>
+ <build_depend>urdf</build_depend>
+ <run_depend>angles</run_depend>
+ <run_depend>control_msgs</run_depend>
+ <run_depend>control_toolbox</run_depend>
+ <run_depend>controller_interface</run_depend>
+ <run_depend>dynamic_reconfigure</run_depend>
+ <run_depend>forward_command_controller</run_depend>
+ <run_depend>realtime_tools</run_depend>
+ <run_depend>urdf</run_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+ <controller_interface plugin="${prefix}/voltage_controllers_plugins.xml" />
+ </export>
+</package>
--- /dev/null
+/*********************************************************************
+ * 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 <voltage_controllers/joint_effort_controller.h>
+#include <angles/angles.h>
+#include <pluginlib/class_list_macros.h>
+
+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<control_msgs::JointControllerState>(n, "state", 1));
+
+ // Start command subscriber
+ sub_command_ = n.subscribe<std_msgs::Float64>("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)
--- /dev/null
+/*********************************************************************
+ * 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 <voltage_controllers/joint_group_effort_controller.h>
+#include <pluginlib/class_list_macros.h>
+
+template <class T>
+void forward_command_controller::ForwardJointGroupCommandController<T>::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)
--- /dev/null
+/*********************************************************************
+ * 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 <voltage_controllers/joint_group_voltage_controller.h>
+#include <pluginlib/class_list_macros.h>
+
+template <class T>
+void forward_command_controller::ForwardJointGroupCommandController<T>::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)
--- /dev/null
+/*********************************************************************
+ * 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 <voltage_controllers/joint_position_controller.h>
+#include <angles/angles.h>
+#include <pluginlib/class_list_macros.h>
+
+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<control_msgs::JointControllerState>(n, "state", 1));
+
+ // Start command subscriber
+ sub_command_ = n.subscribe<std_msgs::Float64>("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)
--- /dev/null
+/*********************************************************************
+ * 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 <voltage_controllers/joint_velocity_controller.h>
+#include <pluginlib/class_list_macros.h>
+
+
+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<control_msgs::JointControllerState>
+ (n, "state", 1));
+
+ // Start command subscriber
+ sub_command_ = n.subscribe<std_msgs::Float64>("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)
--- /dev/null
+/*********************************************************************
+ * 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 <voltage_controllers/joint_voltage_controller.h>
+#include <pluginlib/class_list_macros.h>
+
+template <class T>
+void forward_command_controller::ForwardCommandController<T>::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)
--- /dev/null
+<library path="lib/libvoltage_controllers">
+
+ <class name="voltage_controllers/JointPositionController"
+ type="voltage_controllers::JointPositionController"
+ base_class_type="controller_interface::ControllerBase">
+ <description>
+ The JointPositionController tracks position commands. It expects a
+ VoltageJointInterface type of hardware interface.
+ </description>
+ </class>
+
+ <class name="voltage_controllers/JointVelocityController"
+ type="voltage_controllers::JointVelocityController"
+ base_class_type="controller_interface::ControllerBase">
+ <description>
+ The JointVelocityController tracks velocity commands. It expects a
+ VoltageJointInterface type of hardware interface.
+ </description>
+ </class>
+
+ <class name="voltage_controllers/JointEffortController"
+ type="voltage_controllers::JointEffortController"
+ base_class_type="controller_interface::ControllerBase">
+ <description>
+ The JointEffortController tracks voltage commands. It expects a
+ VoltageJointInterface type of hardware interface.
+ </description>
+ </class>
+
+ <class name="voltage_controllers/JointGroupEffortController"
+ type="voltage_controllers::JointGroupEffortController"
+ base_class_type="controller_interface::ControllerBase">
+ <description>
+ The JointGroupEffortController tracks voltage commands for a set of joints. It expects a
+ VoltageJointInterface type of hardware interface.
+ </description>
+ </class>
+
+ <class name="voltage_controllers/JointVoltageController"
+ type="voltage_controllers::JointVoltageController"
+ base_class_type="controller_interface::ControllerBase">
+ <description>
+ The JointVoltagetController tracks voltage commands. It expects a
+ VoltageJointInterface type of hardware interface.
+ </description>
+ </class>
+
+ <class name="voltage_controllers/JointGroupVoltageController"
+ type="voltage_controllers::JointGroupVoltateController"
+ base_class_type="controller_interface::ControllerBase">
+ <description>
+ The JointGroupVoltateController tracks voltage commands for a set of joints. It expects a
+ VoltageJointInterface type of hardware interface.
+ </description>
+ </class>
+
+
+ <class name="voltage_controllers/JointGroupPositionController"
+ type="voltage_controllers::JointGroupPositionController"
+ base_class_type="controller_interface::ControllerBase">
+ <description>
+ The JointGroupPositionController tracks position commands for a set of joints. It expects a
+ VoltageJointInterface type of hardware interface.
+ </description>
+ </class>
+
+</library>
projects / voltage_controllers.git / commitdiff