multi_joint_velocity_controller.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 /*
00031  * Author: Stuart Glaser
00032  */
00033 
00034 #include "ias_mechanism_controllers/multi_joint_velocity_controller.h"
00035 #include <sstream>
00036 #include "angles/angles.h"
00037 #include "pluginlib/class_list_macros.h"
00038 
00039 PLUGINLIB_DECLARE_CLASS(ias_mechanism_controllers, MultiJointVelocityController, controller::MultiJointVelocityController, pr2_controller_interface::Controller)
00040 
00041 namespace controller {
00042 
00043 
00044 MultiJointVelocityController::MultiJointVelocityController()
00045   : loop_count_(0), robot_(NULL)
00046 {
00047 }
00048 
00049 MultiJointVelocityController::~MultiJointVelocityController()
00050 {
00051   sub_command_.shutdown();
00052 }
00053 
00054 bool MultiJointVelocityController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
00055 {
00056   using namespace XmlRpc;
00057   node_ = n;
00058   robot_ = robot;
00059 
00060   // Gets all of the joints
00061   XmlRpc::XmlRpcValue joint_names;
00062   if (!node_.getParam("joints", joint_names))
00063   {
00064     ROS_ERROR("No joints given. (namespace: %s)", node_.getNamespace().c_str());
00065     return false;
00066   }
00067   if (joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray)
00068   {
00069     ROS_ERROR("Malformed joint specification.  (namespace: %s)", node_.getNamespace().c_str());
00070     return false;
00071   }
00072   for (int i = 0; i < joint_names.size(); ++i)
00073   {
00074     XmlRpcValue &name_value = joint_names[i];
00075     if (name_value.getType() != XmlRpcValue::TypeString)
00076     {
00077       ROS_ERROR("Array of joint names should contain all strings.  (namespace: %s)",
00078                 node_.getNamespace().c_str());
00079       return false;
00080     }
00081 
00082     pr2_mechanism_model::JointState *j = robot->getJointState((std::string)name_value);
00083     if (!j) {
00084       ROS_ERROR("Joint not found: %s. (namespace: %s)",
00085                 ((std::string)name_value).c_str(), node_.getNamespace().c_str());
00086       return false;
00087     }
00088     joints_.push_back(j);
00089   }
00090 
00091   // Ensures that all the joints are calibrated.
00092   for (size_t i = 0; i < joints_.size(); ++i)
00093   {
00094     if (!joints_[i]->calibrated_)
00095     {
00096       ROS_ERROR("Joint %s was not calibrated (namespace: %s)",
00097                 joints_[i]->joint_->name.c_str(), node_.getNamespace().c_str());
00098       return false;
00099     }
00100   }
00101 
00102   // Sets up pid controllers for all of the joints
00103   std::string gains_ns;
00104   if (!node_.getParam("gains", gains_ns))
00105     gains_ns = node_.getNamespace() + "/gains";
00106   pids_.resize(joints_.size());
00107   for (size_t i = 0; i < joints_.size(); ++i)
00108     if (!pids_[i].init(ros::NodeHandle(gains_ns + "/" + joints_[i]->joint_->name)))
00109       return false;
00110 
00111 
00112   output_filters_.resize(joints_.size());
00113   for (size_t i = 0; i < joints_.size(); ++i)
00114   {
00115     std::string p = "output_filters/" + joints_[i]->joint_->name;
00116     if (node_.hasParam(p))
00117     {
00118       output_filters_[i].reset(new filters::FilterChain<double>("double"));
00119       if (!output_filters_[i]->configure(node_.resolveName(p)))
00120         output_filters_[i].reset();
00121     }
00122   }
00123 
00124   sub_command_ = node_.subscribe("command", 1, &MultiJointVelocityController::commandCB, this);
00125 
00126   command_.resize(joints_.size());
00127 
00128   controller_state_publisher_.reset(
00129     new realtime_tools::RealtimePublisher<pr2_controllers_msgs::JointTrajectoryControllerState>
00130     (node_, "state", 1));
00131   controller_state_publisher_->lock();
00132   for (size_t j = 0; j < joints_.size(); ++j)
00133     controller_state_publisher_->msg_.joint_names.push_back(joints_[j]->joint_->name);
00134   controller_state_publisher_->msg_.desired.velocities.resize(joints_.size());
00135   controller_state_publisher_->msg_.actual.positions.resize(joints_.size());
00136   controller_state_publisher_->msg_.actual.velocities.resize(joints_.size());
00137   controller_state_publisher_->msg_.error.velocities.resize(joints_.size());
00138   controller_state_publisher_->unlock();
00139 
00140 
00141   return true;
00142 }
00143 
00144 void MultiJointVelocityController::starting()
00145 {
00146   last_time_ = robot_->getTime();
00147 
00148   for (size_t i = 0; i < pids_.size(); ++i)
00149     pids_[i].reset();
00150 
00151   // Creates a "hold current position" trajectory.
00152 }
00153 
00154 void MultiJointVelocityController::update()
00155 {
00156   ros::Time time = robot_->getTime();
00157   ros::Duration dt = time - last_time_;
00158   last_time_ = time;
00159 
00160   double command[joints_.size()];
00161 
00162   boost::mutex::scoped_lock guard(command_mutex_);
00163   for(unsigned int i=0; i < command_.size(); ++i)
00164     command[i] = command_[i];
00165   guard.unlock();
00166 
00167   double error[joints_.size()];
00168   for (size_t i = 0; i < joints_.size(); ++i)
00169   {
00170     error[i] = joints_[i]->velocity_ - command[i];
00171     double effort = pids_[i].updatePid(error[i], dt);
00172     double effort_filtered = effort;
00173     if (output_filters_[i])
00174       output_filters_[i]->update(effort, effort_filtered);
00175     joints_[i]->commanded_effort_ += effort_filtered;
00176   }
00177 
00178 
00179   // ------ State publishing
00180 
00181   if (loop_count_ % 10 == 0)
00182   {
00183     if (controller_state_publisher_ && controller_state_publisher_->trylock())
00184     {
00185       controller_state_publisher_->msg_.header.stamp = time;
00186       for (size_t j = 0; j < joints_.size(); ++j)
00187       {
00188         controller_state_publisher_->msg_.desired.velocities[j] = command[j];
00189         controller_state_publisher_->msg_.actual.positions[j] = joints_[j]->position_;
00190         controller_state_publisher_->msg_.actual.velocities[j] = joints_[j]->velocity_;
00191         controller_state_publisher_->msg_.error.velocities[j] = error[j];
00192       }
00193       controller_state_publisher_->unlockAndPublish();
00194     }
00195   }
00196 
00197   ++loop_count_;
00198 }
00199 
00200 void MultiJointVelocityController::commandCB(const std_msgs::Float64MultiArray::ConstPtr &msg)
00201 {
00202   boost::mutex::scoped_lock guard(command_mutex_);
00203   for(unsigned int i=0; i < command_.size(); ++i)
00204     command_[i] = msg->data[i];
00205 }
00206 
00207 
00208 
00209 
00210 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


ias_mechanism_controllers
Author(s): Lorenz Moesenlechner, Ingo Kresse
autogenerated on Thu May 23 2013 10:34:13