Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
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
00054 if (!hw_){
00055 ROS_ERROR("Mechanism Model received an invalid hardware interface");
00056 return false;
00057 }
00058
00059
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
00066 transmission_loader_.reset(new pluginlib::ClassLoader<pr2_mechanism_model::Transmission>(
00067 "pr2_mechanism_model", "pr2_mechanism_model::Transmission"));
00068
00069
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 boost::shared_ptr<Transmission> t;
00076 try {
00077
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_->createInstance(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 if (!t)
00101 ROS_ERROR("Unknown transmission type: %s", type.c_str());
00102 else if (!t->initXml(xit, this)){
00103 ROS_ERROR("Failed to initialize transmission");
00104 }
00105 else
00106 transmissions_.push_back(t);
00107 }
00108
00109 return true;
00110 }
00111
00112 ros::Time Robot::getTime()
00113 {
00114 return hw_->current_time_;
00115 }
00116
00117 template <class T>
00118 int findIndexByName(const std::vector<boost::shared_ptr<T> >& v,
00119 const std::string &name)
00120 {
00121 for (unsigned int i = 0; i < v.size(); ++i)
00122 {
00123 if (v[i]->name_ == name)
00124 return i;
00125 }
00126 return -1;
00127 }
00128
00129 int Robot::getTransmissionIndex(const std::string &name) const
00130 {
00131 return findIndexByName(transmissions_, name);
00132 }
00133
00134 Actuator* Robot::getActuator(const std::string &name) const
00135 {
00136 return hw_->getActuator(name);
00137 }
00138
00139 boost::shared_ptr<Transmission> Robot::getTransmission(const std::string &name) const
00140 {
00141 int i = getTransmissionIndex(name);
00142 return i >= 0 ? transmissions_[i] : boost::shared_ptr<Transmission>();
00143 }
00144
00145
00146
00147
00148
00149 RobotState::RobotState(Robot *model)
00150 : model_(model)
00151 {
00152 assert(model_);
00153
00154 transmissions_in_.resize(model->transmissions_.size());
00155 transmissions_out_.resize(model->transmissions_.size());
00156
00157
00158 unsigned int js_size = 0;
00159 for (unsigned int i = 0; i < model_->transmissions_.size(); ++i)
00160 {
00161 boost::shared_ptr<Transmission> t = model_->transmissions_[i];
00162 for (unsigned int j = 0; j < t->actuator_names_.size(); ++j)
00163 {
00164 Actuator *act = model_->getActuator(t->actuator_names_[j]);
00165 assert(act != NULL);
00166 transmissions_in_[i].push_back(act);
00167 }
00168 js_size += t->joint_names_.size();
00169 }
00170
00171
00172 joint_states_.resize(js_size);
00173 unsigned int js_id = 0;
00174 for (unsigned int i = 0; i < model_->transmissions_.size(); ++i)
00175 {
00176 boost::shared_ptr<Transmission> t = model_->transmissions_[i];
00177 for (unsigned int j = 0; j < t->joint_names_.size(); ++j)
00178 {
00179 joint_states_[js_id].joint_ = model_->robot_model_.getJoint(t->joint_names_[j]);
00180 joint_states_map_[t->joint_names_[j]] = &(joint_states_[js_id]);
00181 transmissions_out_[i].push_back(&(joint_states_[js_id]));
00182 js_id++;
00183 }
00184 }
00185
00186
00187 if (model_->transmissions_.empty())
00188 ROS_WARN("No transmissions were specified in the robot description.");
00189 if (js_size == 0)
00190 ROS_WARN("None of the joints in the robot desription matches up to a motor. The robot is uncontrollable.");
00191 }
00192
00193
00194 JointState *RobotState::getJointState(const std::string &name)
00195 {
00196 std::map<std::string, JointState*>::iterator it = joint_states_map_.find(name);
00197 if (it == joint_states_map_.end())
00198 return NULL;
00199 else
00200 return it->second;
00201 }
00202
00203 const JointState *RobotState::getJointState(const std::string &name) const
00204 {
00205 std::map<std::string, JointState*>::const_iterator it = joint_states_map_.find(name);
00206 if (it == joint_states_map_.end())
00207 return NULL;
00208 else
00209 return it->second;
00210 }
00211
00212 void RobotState::propagateActuatorPositionToJointPosition()
00213 {
00214 for (unsigned int i = 0; i < model_->transmissions_.size(); ++i)
00215 {
00216 model_->transmissions_[i]->propagatePosition(transmissions_in_[i],
00217 transmissions_out_[i]);
00218 }
00219
00220 for (unsigned int i = 0; i < joint_states_.size(); i++)
00221 {
00222 joint_states_[i].joint_statistics_.update(&(joint_states_[i]));
00223 }
00224 }
00225
00226 void RobotState::propagateJointEffortToActuatorEffort()
00227 {
00228 for (unsigned int i = 0; i < model_->transmissions_.size(); ++i)
00229 {
00230 model_->transmissions_[i]->propagateEffort(transmissions_out_[i],
00231 transmissions_in_[i]);
00232 }
00233 }
00234
00235 bool RobotState::isHalted()
00236 {
00237 for (unsigned int t = 0; t < transmissions_in_.size(); ++t){
00238 for (unsigned int a = 0; a < transmissions_in_[t].size(); a++){
00239 if (transmissions_in_[t][a]->state_.halted_)
00240 return true;
00241 }
00242 }
00243
00244 return false;
00245 }
00246
00247 void RobotState::enforceSafety()
00248 {
00249 for (unsigned int i = 0; i < joint_states_.size(); ++i)
00250 {
00251 joint_states_[i].enforceLimits();
00252 }
00253 }
00254
00255 void RobotState::zeroCommands()
00256 {
00257 for (unsigned int i = 0; i < joint_states_.size(); ++i)
00258 joint_states_[i].commanded_effort_ = 0;
00259 }
00260
00261 void RobotState::propagateJointPositionToActuatorPosition()
00262 {
00263 for (unsigned int i = 0; i < model_->transmissions_.size(); ++i)
00264 {
00265 model_->transmissions_[i]->propagatePositionBackwards(transmissions_out_[i],
00266 transmissions_in_[i]);
00267 }
00268 }
00269
00270 void RobotState::propagateActuatorEffortToJointEffort()
00271 {
00272 for (unsigned int i = 0; i < model_->transmissions_.size(); ++i)
00273 {
00274 model_->transmissions_[i]->propagateEffortBackwards(transmissions_in_[i],
00275 transmissions_out_[i]);
00276 }
00277 }
00278
00279