diff --git a/pr2_controllers_msgs/CMakeLists.txt b/pr2_controllers_msgs/CMakeLists.txt index 4633e46..1d1f0c0 100755 --- a/pr2_controllers_msgs/CMakeLists.txt +++ b/pr2_controllers_msgs/CMakeLists.txt @@ -9,6 +9,7 @@ add_message_files( DIRECTORY msg FILES JointControllerState.msg + JointControllerStateArray.msg JointTrajectoryControllerState.msg Pr2GripperCommand.msg ) diff --git a/pr2_controllers_msgs/msg/JointControllerStateArray.msg b/pr2_controllers_msgs/msg/JointControllerStateArray.msg new file mode 100644 index 0000000..5c7ad28 --- /dev/null +++ b/pr2_controllers_msgs/msg/JointControllerStateArray.msg @@ -0,0 +1,2 @@ +Header header +JointControllerState[] controllestate diff --git a/robot_mechanism_controllers/CMakeLists.txt b/robot_mechanism_controllers/CMakeLists.txt index a50a65b..4f18e5c 100755 --- a/robot_mechanism_controllers/CMakeLists.txt +++ b/robot_mechanism_controllers/CMakeLists.txt @@ -32,13 +32,14 @@ add_library(robot_mechanism_controllers src/joint_effort_controller.cpp src/joint_position_controller.cpp src/joint_velocity_controller.cpp + src/joint_group_velocity_controller.cpp src/joint_spline_trajectory_controller.cpp src/joint_trajectory_action_controller.cpp src/jt_cartesian_controller.cpp ) target_link_libraries(robot_mechanism_controllers ltdl ${Boost_LIBRARIES} ${catkin_LIBRARIES}) -add_dependencies(robot_mechanism_controllers +add_dependencies(robot_mechanism_controllers ${robot_mechanism_controllers_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) pr2_enable_rpath(robot_mechanism_controllers) diff --git a/robot_mechanism_controllers/controller_plugins.xml b/robot_mechanism_controllers/controller_plugins.xml index b9ea4e5..2ee4e37 100755 --- a/robot_mechanism_controllers/controller_plugins.xml +++ b/robot_mechanism_controllers/controller_plugins.xml @@ -1,6 +1,7 @@ + diff --git a/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_group_velocity_controller.h b/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_group_velocity_controller.h new file mode 100644 index 0000000..ca30d97 --- /dev/null +++ b/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_group_velocity_controller.h @@ -0,0 +1,139 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * 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 JOINT_GROUP_VELOCITY_CONTROLLER_H +#define JOINT_GROUP_VELOCITY_CONTROLLER_H + +/** + @class pr2_controller_interface::JointGroupVelocityController + @author Stuart Glaser + @brief Joint Group Velocity Controller + + This controller controls velocity using a pid loop. + + @section ROS ROS interface + + @param type Must be "JointGroupVelocityController" + @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 (robot_mechanism_controllers::JointControllerState) : + Current state of the controller, including pid error and gains. + +*/ + +#include +#include +#include + +#include "pr2_controller_interface/controller.h" +#include "control_toolbox/pid.h" +#include "control_toolbox/pid_gains_setter.h" + +// Services +#include +#include + +//Realtime publisher +#include +#include +#include + +namespace controller +{ + +class JointGroupVelocityController : public pr2_controller_interface::Controller +{ +public: + + JointGroupVelocityController(); + ~JointGroupVelocityController(); + + bool init(pr2_mechanism_model::RobotState *robot, const std::vector< std::string> &joint_names, const control_toolbox::Pid &pid); + bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n); + + /*! + * \brief Give set position of the joint for next update: revolute (angle) and prismatic (position) + * + * \param double pos Velocity command to issue + */ + void setCommand(std::vector cmd); + + /*! + * \brief Get latest position command to the joint: revolute (angle) and prismatic (position). + */ + void getCommand(std::vector & cmd); + + /*! + * \brief Issues commands to the joint. Should be called at regular intervals + */ + + virtual void starting(); + virtual void update(); + + void getGains(control_toolbox::Pid &pid, double &p, double &i, double &d, double &i_max, double &i_min); + //void setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min); + + std::vector< std::string > getJointName(); + unsigned int n_joints_; + +private: + ros::NodeHandle node_; + pr2_mechanism_model::RobotState *robot_; /**< Pointer to robot structure. */ + std::vector pid_controllers_; /**< Internal PID controller. */ + std::vector joints_; /**< Joint we're controlling. */ + ros::Time last_time_; /**< Last time stamp of update. */ + int loop_count_; + + realtime_tools::RealtimeBuffer > commands_buffer_; /**< Last commanded position. */ + + friend class JointGroupVelocityControllerNode; + + boost::scoped_ptr< + realtime_tools::RealtimePublisher< + pr2_controllers_msgs::JointControllerStateArray> > controller_state_publisher_ ; + + ros::Subscriber sub_command_; + void setCommandCB(const std_msgs::Float64MultiArrayConstPtr& msg); +}; + +} // namespace + +#endif diff --git a/robot_mechanism_controllers/src/joint_group_velocity_controller.cpp b/robot_mechanism_controllers/src/joint_group_velocity_controller.cpp new file mode 100644 index 0000000..9f9e785 --- /dev/null +++ b/robot_mechanism_controllers/src/joint_group_velocity_controller.cpp @@ -0,0 +1,214 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * 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. + *********************************************************************/ + +#include "robot_mechanism_controllers/joint_group_velocity_controller.h" +#include "pluginlib/class_list_macros.h" + +PLUGINLIB_EXPORT_CLASS( controller::JointGroupVelocityController, pr2_controller_interface::Controller) + +using namespace std; + +namespace controller { + +JointGroupVelocityController::JointGroupVelocityController() +: robot_(NULL), loop_count_(0) +{ +} + +JointGroupVelocityController::~JointGroupVelocityController() +{ + sub_command_.shutdown(); +} + +bool JointGroupVelocityController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n) +{ + using namespace XmlRpc; + assert(robot); + node_ = n; + robot_ = robot; + + // Gets all of the joints + XmlRpc::XmlRpcValue joint_names; + if (!node_.getParam("joints", joint_names)) + { + ROS_ERROR("No joints given. (namespace: %s)", node_.getNamespace().c_str()); + return false; + } + if (joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray) + { + ROS_ERROR("Malformed joint specification. (namespace: %s)", node_.getNamespace().c_str()); + return false; + } + for (int i = 0; i < joint_names.size(); ++i) + { + XmlRpcValue &name_value = joint_names[i]; + if (name_value.getType() != XmlRpcValue::TypeString) + { + ROS_ERROR("Array of joint names should contain all strings. (namespace: %s)", + node_.getNamespace().c_str()); + return false; + } + + pr2_mechanism_model::JointState *j = robot->getJointState((std::string)name_value); + if (!j) { + ROS_ERROR("Joint not found: %s. (namespace: %s)", + ((std::string)name_value).c_str(), node_.getNamespace().c_str()); + return false; + } + joints_.push_back(j); + } + + // Sets up pid controllers for all of the joints + std::string gains_ns; + if (!node_.getParam("gains", gains_ns)) + gains_ns = node_.getNamespace() + "/gains"; + pid_controllers_.resize(joints_.size()); + for (size_t i = 0; i < joints_.size(); ++i) + { + ros::NodeHandle joint_nh(gains_ns + "/" + joints_[i]->joint_->name); + if (!pid_controllers_[i].init(joint_nh)) + return false; + } + + n_joints_ = joint_names.size(); + + commands_buffer_.writeFromNonRT(std::vector(n_joints_, 0.0)); + + controller_state_publisher_.reset( + new realtime_tools::RealtimePublisher + (node_, "statearray", 1)); + controller_state_publisher_->lock(); + controller_state_publisher_->msg_.controllestate.resize(joints_.size()); + controller_state_publisher_->unlock(); + + sub_command_ = node_.subscribe("command", 1, &JointGroupVelocityController::setCommandCB, this); + + return true; +} + +void JointGroupVelocityController::getGains(control_toolbox::Pid &pid, double &p, double &i, double &d, double &i_max, double &i_min) +{ + pid.getGains(p,i,d,i_max,i_min); +} + +std::vector< std::string > JointGroupVelocityController::getJointName() +{ + std::vector< std::string > joint_names; + for(unsigned int i=0; ijoint_->name); + } + return joint_names; +} + +// Set the joint velocity commands +void JointGroupVelocityController::setCommand(std::vector cmd) +{ + commands_buffer_.writeFromNonRT(cmd); +} + +// Return the current velocity commands +void JointGroupVelocityController::getCommand(std::vector & cmd) +{ + cmd = *commands_buffer_.readFromRT(); +} + +void JointGroupVelocityController::starting() +{ + for (size_t i = 0; i < pid_controllers_.size(); ++i) + pid_controllers_[i].reset(); + +} +void JointGroupVelocityController::update() +{ + assert(robot_ != NULL); + ros::Time time = robot_->getTime(); + ros::Duration dt_ = time - last_time_; + std::vector & commands = *commands_buffer_.readFromRT(); + std::vector compute_command(n_joints_); + std::vector compute_error(n_joints_); + + for(unsigned int i=0; ivelocity_; + double command = pid_controllers_[i].computeCommand(compute_error[i], dt_); + joints_[i]->commanded_effort_ += command; + compute_command[i] = command; + } + + if(loop_count_ % 10 == 0){ + if(controller_state_publisher_ && controller_state_publisher_->trylock()) + { + controller_state_publisher_->msg_.header.stamp = time; + for (unsigned int i = 0; i < n_joints_; ++i) + { + pr2_controllers_msgs::JointControllerState singlejointcontrollerstate; + singlejointcontrollerstate.header.stamp = time; + singlejointcontrollerstate.set_point = commands[i]; + singlejointcontrollerstate.process_value = joints_[i]->velocity_; + singlejointcontrollerstate.error = compute_error[i]; + singlejointcontrollerstate.time_step = dt_.toSec(); + singlejointcontrollerstate.command = compute_command[i]; + + double dummy; + getGains(pid_controllers_[i], + singlejointcontrollerstate.p, + singlejointcontrollerstate.i, + singlejointcontrollerstate.d, + singlejointcontrollerstate.i_clamp, + dummy); + controller_state_publisher_->msg_.controllestate.push_back(singlejointcontrollerstate); + } + controller_state_publisher_->unlockAndPublish(); + } +} + + loop_count_++; + + last_time_ = time; +} + +void JointGroupVelocityController::setCommandCB(const std_msgs::Float64MultiArrayConstPtr& msg) +{ + // command_ = msg->data; + if(msg->data.size()!=n_joints_) + { + ROS_ERROR_STREAM("Dimension of command (" << msg->data.size() << ") does not match number of joints (" << n_joints_ << ")! Not executing!"); + return; + } + // command_ = msg->data; + commands_buffer_.writeFromNonRT(msg->data); +} + +} // namespace