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 Transmission *t = NULL;
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_->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
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
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
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
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
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
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