id_controller.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /*
00036   Author: Daniel Hennes
00037  */
00038 
00039 #include "inverse_dynamics/id_controller.h"
00040 #include <pluginlib/class_list_macros.h>
00041 
00042 namespace controller {
00043 
00045 PLUGINLIB_DECLARE_CLASS(inverse_dynamics,
00046                         IDController, 
00047                         controller::IDController, 
00048                         pr2_controller_interface::Controller)
00049 
00050 
00051 
00052 bool IDController::init(pr2_mechanism_model::RobotState *robot,
00053                             ros::NodeHandle &n)
00054 {
00055   robot_ = robot;
00056   node_ = n;
00057 
00058   // solver
00059   std::string sai_xml_fname;
00060   if (!node_.getParam("sai_xml", sai_xml_fname))
00061   {
00062     ROS_ERROR("No sai_xml filename given. (namespace: %s)", node_.getNamespace().c_str());
00063     return false;
00064   }
00065   ROS_INFO("sai_xml filename: %s", sai_xml_fname.c_str());
00066   solver_ = new InverseDynamicsSolverWBC(sai_xml_fname);
00067 
00068   // get all joints
00069   XmlRpc::XmlRpcValue joint_names;
00070   if (!node_.getParam("joints", joint_names))
00071   {
00072     ROS_ERROR("No joints given. (namespace: %s)", node_.getNamespace().c_str());
00073     return false;
00074   }
00075   if (joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray)
00076   {
00077     ROS_ERROR("Malformed joint specification.  (namespace: %s)", node_.getNamespace().c_str());
00078     return false;
00079   }
00080   for (int i = 0; i < joint_names.size(); ++i)
00081   {
00082     XmlRpc::XmlRpcValue &name_value = joint_names[i];
00083     if (name_value.getType() != XmlRpc::XmlRpcValue::TypeString)
00084     {
00085       ROS_ERROR("Array of joint names should contain all strings.  (namespace: %s)",
00086                 node_.getNamespace().c_str());
00087       return false;
00088     }
00089 
00090     pr2_mechanism_model::JointState *j = robot->getJointState((std::string)name_value);
00091     if (!j) {
00092       ROS_ERROR("Joint not found: %s. (namespace: %s)",
00093                 ((std::string)name_value).c_str(), node_.getNamespace().c_str());
00094       return false;
00095     }
00096     joints_.push_back(j);
00097   }
00098 
00099   // filters
00100   std::string p = "acc_filter";
00101   if (node_.hasParam(p)) {
00102     acc_filter_ = new filters::MultiChannelFilterChain<double>("double");
00103     if (!acc_filter_->configure(joints_.size(), node_.resolveName(p).c_str())) {
00104       ROS_ERROR("Filters not configured, %s. (namespace %s)", p.c_str(),
00105                 node_.getNamespace().c_str());
00106       return false;
00107     }
00108   }
00109   p = "vel_filter";
00110   if (node_.hasParam(p)) {
00111     vel_filter_ = new filters::MultiChannelFilterChain<double>("double");
00112     if (!vel_filter_->configure(joints_.size(), node_.resolveName(p).c_str())) {
00113       ROS_ERROR("Filters not configured, %s. (namespace %s)", p.c_str(),
00114                 node_.getNamespace().c_str());
00115       return false;
00116     }
00117   }
00118   p = "pos_filter";
00119   if (node_.hasParam(p)) {
00120     pos_filter_ = new filters::MultiChannelFilterChain<double>("double");
00121     if (!pos_filter_->configure(joints_.size(), node_.resolveName(p).c_str())) {
00122       ROS_ERROR("Filters not configured, %s. (namespace %s)", p.c_str(),
00123                 node_.getNamespace().c_str());
00124       return false;
00125     }
00126   }
00127   p = "eff_filter";
00128   if (node_.hasParam(p)) {
00129     eff_filter_ = new filters::MultiChannelFilterChain<double>("double");
00130     if (!eff_filter_->configure(joints_.size(), node_.resolveName(p).c_str())) {
00131       ROS_ERROR("Filters not configured, %s. (namespace %s)", p.c_str(),
00132                 node_.getNamespace().c_str());
00133       return false;
00134     }
00135   }
00136 
00137 
00138   controller_state_publisher_.reset(
00139     new realtime_tools::RealtimePublisher<inverse_dynamics::JointState> (node_, "joint_states", 1));
00140 
00141   controller_state_publisher_->lock();
00142   for (size_t j = 0; j < joints_.size(); ++j)
00143     controller_state_publisher_->msg_.name.push_back(joints_[j]->joint_->name);
00144   controller_state_publisher_->msg_.position.resize(joints_.size());
00145   controller_state_publisher_->msg_.velocity.resize(joints_.size());
00146   controller_state_publisher_->msg_.acceleration.resize(joints_.size());
00147   controller_state_publisher_->msg_.effort.resize(joints_.size());
00148   controller_state_publisher_->msg_.computed_effort.resize(joints_.size());
00149   controller_state_publisher_->unlock();
00150 
00151   return true;
00152 }
00153 
00154 
00156 void IDController::starting() {
00157   last_vel_.resize(joints_.size());
00158   last_time_ = robot_->getTime();
00159 }
00160 
00161 
00163 void IDController::update() {
00164   ros::Time time = robot_->getTime();
00165   ros::Duration dt = time - last_time_;
00166   last_time_ = time;
00167 
00168   std::vector<double> pos(joints_.size());
00169   std::vector<double> vel(joints_.size());
00170   std::vector<double> acc(joints_.size());
00171   std::vector<double> eff(joints_.size());
00172   std::vector<double> computed_eff(joints_.size());
00173 
00174 
00175   for (size_t j = 0; j < joints_.size(); j++) {
00176     pos[j] = joints_[j]->position_;
00177     vel[j] = joints_[j]->velocity_;
00178     acc[j] = (vel[j] - last_vel_[j]) / dt.toSec();
00179     eff[j] = joints_[j]->measured_effort_;
00180   }
00181 
00182   // vel_filter_->update(vel, vel);
00183   acc_filter_->update(acc, acc);
00184   eff_filter_->update(eff, eff);
00185 
00186   solver_->updateState(pos, vel, acc);
00187   solver_->getTorques(computed_eff);
00188 
00189   if (controller_state_publisher_ && controller_state_publisher_->trylock()) {
00190     controller_state_publisher_->msg_.header.stamp = time;
00191     controller_state_publisher_->msg_.position = pos;
00192     controller_state_publisher_->msg_.velocity = vel;      
00193     controller_state_publisher_->msg_.acceleration = acc;
00194     controller_state_publisher_->msg_.effort = eff;
00195     controller_state_publisher_->msg_.computed_effort = computed_eff;
00196     controller_state_publisher_->unlockAndPublish();
00197   }
00198 
00199   std::copy(vel.begin(), vel.end(), last_vel_.begin());
00200 
00201 }
00202 
00204 void IDController::stopping() { 
00205 
00206 }
00207 
00208 } // namespace


kelsey_sandbox
Author(s): kelsey
autogenerated on Wed Nov 27 2013 11:52:03