robot.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, Wim Meeussen
00032  */
00033 
00034 #include "pr2_mechanism_model/robot.h"
00035 #include "pr2_mechanism_model/transmission.h"
00036 #include <tinyxml.h>
00037 #include <urdf/model.h>
00038 #include <pluginlib/class_loader.h>
00039 #include "pr2_hardware_interface/hardware_interface.h"
00040 
00041 
00042 using namespace pr2_mechanism_model;
00043 using namespace pr2_hardware_interface;
00044 
00045 
00046 Robot::Robot(HardwareInterface *hw)
00047   :hw_(hw)
00048 {}
00049 
00050 
00051 bool Robot::initXml(TiXmlElement *root)
00052 {
00053   // check if current time is valid
00054   if (!hw_){
00055     ROS_ERROR("Mechanism Model received an invalid hardware interface");
00056     return false;
00057   }
00058 
00059   // Parses the xml into a robot model
00060   if (!robot_model_.initXml(root)){
00061     ROS_ERROR("Mechanism Model failed to parse the URDF xml into a robot model");
00062     return false;
00063   }
00064 
00065   // Creates the plugin loader for transmissions.
00066   transmission_loader_.reset(new pluginlib::ClassLoader<pr2_mechanism_model::Transmission>(
00067                                "pr2_mechanism_model", "pr2_mechanism_model::Transmission"));
00068 
00069   // Constructs the transmissions by parsing custom xml.
00070   TiXmlElement *xit = NULL;
00071   for (xit = root->FirstChildElement("transmission"); xit;
00072        xit = xit->NextSiblingElement("transmission"))
00073   {
00074     std::string type(xit->Attribute("type"));
00075     Transmission *t = NULL;
00076     try {
00077       // Backwards compatibility for using non-namespaced controller types
00078       if (!transmission_loader_->isClassAvailable(type))
00079       {
00080         std::vector<std::string> classes = transmission_loader_->getDeclaredClasses();
00081         for(unsigned int i = 0; i < classes.size(); ++i)
00082         {
00083           if(type == transmission_loader_->getName(classes[i]))
00084           {
00085             ROS_WARN("The deprecated transmission type %s was not found.  Using the namespaced version %s instead.  "
00086                      "Please update your urdf file to use the namespaced version.",
00087                      type.c_str(), classes[i].c_str());
00088             type = classes[i];
00089             break;
00090           }
00091         }
00092       }
00093       t = transmission_loader_->createClassInstance(type);
00094     }
00095     catch (const std::runtime_error &ex)
00096     {
00097       ROS_ERROR("Could not load class %s: %s", type.c_str(), ex.what());
00098     }
00099     /*
00100     catch(pluginlib::LibraryLoadException &ex)
00101     {
00102       ROS_ERROR("LibraryLoadException for transmission of type %s", type);
00103       ROS_ERROR("%s", ex.what());
00104       return false;
00105     }
00106     catch(pluginlib::CreateClassException &ex)
00107     {
00108       ROS_ERROR("CreateClassException for transmission of type %s", type);
00109       ROS_ERROR("%s", ex.what());
00110       return false;
00111     }
00112     catch(...)
00113     {
00114       ROS_ERROR("Could not construct transmission of type %s", type);
00115       return false;
00116     }
00117     */
00118 
00119     if (!t)
00120       ROS_ERROR("Unknown transmission type: %s", type.c_str());
00121     else if (!t->initXml(xit, this)){
00122       ROS_ERROR("Failed to initialize transmission");
00123       delete t;
00124     }
00125     else // Success!
00126       transmissions_.push_back(t);
00127   }
00128 
00129   return true;
00130 }
00131 
00132 ros::Time Robot::getTime()
00133 {
00134   return hw_->current_time_;
00135 }
00136 
00137 template <class T>
00138 int findIndexByName(const std::vector<T*>& v, const std::string &name)
00139 {
00140   for (unsigned int i = 0; i < v.size(); ++i)
00141   {
00142     if (v[i]->name_ == name)
00143       return i;
00144   }
00145   return -1;
00146 }
00147 
00148 int Robot::getTransmissionIndex(const std::string &name) const
00149 {
00150   return findIndexByName(transmissions_, name);
00151 }
00152 
00153 Actuator* Robot::getActuator(const std::string &name) const
00154 {
00155   return hw_->getActuator(name);
00156 }
00157 
00158 Transmission* Robot::getTransmission(const std::string &name) const
00159 {
00160   int i = getTransmissionIndex(name);
00161   return i >= 0 ? transmissions_[i] : NULL;
00162 }
00163 
00164 
00165 
00166 
00167 
00168 RobotState::RobotState(Robot *model)
00169   : model_(model)
00170 {
00171   assert(model_);
00172 
00173   transmissions_in_.resize(model->transmissions_.size());
00174   transmissions_out_.resize(model->transmissions_.size());
00175 
00176   // Creates a joint state for each transmission
00177   unsigned int js_size = 0;
00178   for (unsigned int i = 0; i < model_->transmissions_.size(); ++i)
00179   {
00180     Transmission *t = model_->transmissions_[i];
00181     for (unsigned int j = 0; j < t->actuator_names_.size(); ++j)
00182     {
00183       Actuator *act = model_->getActuator(t->actuator_names_[j]);
00184       assert(act != NULL);
00185       transmissions_in_[i].push_back(act);
00186     }
00187     js_size += t->joint_names_.size();
00188   }
00189 
00190   // Wires up the transmissions to the joint state
00191   joint_states_.resize(js_size);
00192   unsigned int js_id = 0;
00193   for (unsigned int i = 0; i < model_->transmissions_.size(); ++i)
00194   {
00195     Transmission *t = model_->transmissions_[i];
00196     for (unsigned int j = 0; j < t->joint_names_.size(); ++j)
00197     {
00198       joint_states_[js_id].joint_ = model_->robot_model_.getJoint(t->joint_names_[j]);
00199       joint_states_map_[t->joint_names_[j]] = &(joint_states_[js_id]);
00200       transmissions_out_[i].push_back(&(joint_states_[js_id]));
00201       js_id++;
00202     }
00203   }
00204 
00205   // warnings
00206   if (model_->transmissions_.empty())
00207     ROS_WARN("No transmissions were specified in the robot description.");
00208   if (js_size == 0)
00209     ROS_WARN("None of the joints in the robot desription matches up to a motor. The robot is uncontrollable.");
00210 }
00211 
00212 
00213 JointState *RobotState::getJointState(const std::string &name)
00214 {
00215   std::map<std::string, JointState*>::iterator it = joint_states_map_.find(name);
00216   if (it == joint_states_map_.end())
00217     return NULL;
00218   else
00219     return it->second;
00220 }
00221 
00222 const JointState *RobotState::getJointState(const std::string &name) const
00223 {
00224   std::map<std::string, JointState*>::const_iterator it = joint_states_map_.find(name);
00225   if (it == joint_states_map_.end())
00226     return NULL;
00227   else
00228     return it->second;
00229 }
00230 
00231 void RobotState::propagateActuatorPositionToJointPosition()
00232 {
00233   for (unsigned int i = 0; i < model_->transmissions_.size(); ++i)
00234   {
00235     model_->transmissions_[i]->propagatePosition(transmissions_in_[i],
00236                                                  transmissions_out_[i]);
00237   }
00238 
00239   for (unsigned int i = 0; i < joint_states_.size(); i++)
00240   {
00241     joint_states_[i].joint_statistics_.update(&(joint_states_[i]));
00242   }
00243 }
00244 
00245 void RobotState::propagateJointEffortToActuatorEffort()
00246 {
00247   for (unsigned int i = 0; i < model_->transmissions_.size(); ++i)
00248   {
00249     model_->transmissions_[i]->propagateEffort(transmissions_out_[i],
00250                                                transmissions_in_[i]);
00251   }
00252 }
00253 
00254 bool RobotState::isHalted()
00255 {
00256   for (unsigned int t = 0; t < transmissions_in_.size(); ++t){
00257     for (unsigned int a = 0; a < transmissions_in_[t].size(); a++){
00258       if (transmissions_in_[t][a]->state_.halted_)
00259         return true;
00260     }
00261   }
00262 
00263   return false;
00264 }
00265 
00266 void RobotState::enforceSafety()
00267 {
00268   for (unsigned int i = 0; i < joint_states_.size(); ++i)
00269   {
00270     joint_states_[i].enforceLimits();
00271   }
00272 }
00273 
00274 void RobotState::zeroCommands()
00275 {
00276   for (unsigned int i = 0; i < joint_states_.size(); ++i)
00277     joint_states_[i].commanded_effort_ = 0;
00278 }
00279 
00280 void RobotState::propagateJointPositionToActuatorPosition()
00281 {
00282   for (unsigned int i = 0; i < model_->transmissions_.size(); ++i)
00283   {
00284     model_->transmissions_[i]->propagatePositionBackwards(transmissions_out_[i],
00285                                                           transmissions_in_[i]);
00286   }
00287 }
00288 
00289 void RobotState::propagateActuatorEffortToJointEffort()
00290 {
00291   for (unsigned int i = 0; i < model_->transmissions_.size(); ++i)
00292   {
00293     model_->transmissions_[i]->propagateEffortBackwards(transmissions_in_[i],
00294                                                         transmissions_out_[i]);
00295   }
00296 }
00297 
00298 


pr2_mechanism_model
Author(s): Eric Berger berger@willowgarage.com, Stuart Glaser, Wim Meeussen
autogenerated on Mon Dec 2 2013 13:13:02