robot_model.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, 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 /* Author: Ioan Sucan, E. Gil Jones */
00036 
00037 #include <moveit/robot_model/robot_model.h>
00038 #include <geometric_shapes/shape_operations.h>
00039 #include <boost/math/constants/constants.hpp>
00040 #include <moveit/profiler/profiler.h>
00041 #include <algorithm>
00042 #include <limits>
00043 #include <queue>
00044 #include <cmath>
00045 
00046 /* ------------------------ RobotModel ------------------------ */
00047 
00048 robot_model::RobotModel::RobotModel(const boost::shared_ptr<const urdf::ModelInterface> &urdf_model,
00049                                     const boost::shared_ptr<const srdf::Model> &srdf_model)
00050 {
00051   root_joint_ = NULL;
00052   urdf_ = urdf_model;
00053   srdf_ = srdf_model;
00054   buildModel(*urdf_model, *srdf_model);
00055 }
00056 
00057 robot_model::RobotModel::~RobotModel()
00058 {
00059   for (std::map<std::string, JointModelGroup*>::iterator it = joint_model_group_map_.begin() ; it != joint_model_group_map_.end() ; ++it)
00060     delete it->second;
00061   for (std::size_t i = 0 ; i < joint_model_vector_.size() ; ++i)
00062     delete joint_model_vector_[i];
00063   for (std::size_t i = 0 ; i < link_model_vector_.size() ; ++i)
00064     delete link_model_vector_[i];
00065 }
00066 
00067 void robot_model::RobotModel::buildModel(const urdf::ModelInterface &urdf_model, const srdf::Model &srdf_model)
00068 {
00069   moveit::Profiler::ScopedStart prof_start;
00070   moveit::Profiler::ScopedBlock prof_block("RobotModel::buildModel");
00071 
00072   root_joint_ = NULL;
00073   model_name_ = urdf_model.getName();
00074   if (urdf_model.getRoot())
00075   {
00076     const urdf::Link *root_link_ptr = urdf_model.getRoot().get();
00077     model_frame_ = '/' + root_link_ptr->name;
00078 
00079     root_joint_ = buildRecursive(NULL, root_link_ptr, srdf_model);
00080     root_link_ = root_joint_->child_link_model_;
00081     buildMimic(urdf_model);
00082     buildJointInfo();
00083 
00084     if (link_models_with_collision_geometry_vector_.empty())
00085       logWarn("No geometry is associated to any robot links");
00086 
00087     // build groups
00088     buildGroups(srdf_model);
00089     buildGroupStates(srdf_model);
00090 
00091     std::stringstream ss;
00092     printModelInfo(ss);
00093     logDebug("%s", ss.str().c_str());
00094   }
00095   else
00096     logWarn("No root link found");
00097 }
00098 
00099 void robot_model::RobotModel::buildJointInfo()
00100 {
00101   // construct additional maps for easy access by name
00102   variable_count_ = 0;
00103   std::vector<JointModel*> later;
00104   for (std::size_t i = 0 ; i < joint_model_vector_.size() ; ++i)
00105   {
00106     const std::vector<std::string> &name_order = joint_model_vector_[i]->getVariableNames();
00107     for (std::size_t j = 0 ; j < name_order.size() ; ++j)
00108       joint_model_vector_[i]->getVariableBounds(name_order[j], variable_bounds_[name_order[j]]);
00109     if (joint_model_vector_[i]->mimic_ == NULL)
00110     {
00111       // compute index map
00112       if (name_order.size() > 0)
00113       {
00114         for (std::size_t j = 0; j < name_order.size(); ++j)
00115         {
00116           joint_variables_index_map_[name_order[j]] = variable_count_ + j;
00117           active_variable_names_.push_back(name_order[j]);
00118         }
00119         joint_variables_index_map_[joint_model_vector_[i]->getName()] = variable_count_;
00120 
00121         // compute variable count
00122         variable_count_ += joint_model_vector_[i]->getVariableCount();
00123       }
00124     }
00125     else
00126       later.push_back(joint_model_vector_[i]);
00127   }
00128 
00129   for (std::size_t i = 0 ; i < later.size() ; ++i)
00130   {
00131     const std::vector<std::string>& name_order = later[i]->getVariableNames();
00132     const std::vector<std::string>& mim_name_order = later[i]->mimic_->getVariableNames();
00133     for (std::size_t j = 0; j < name_order.size(); ++j)
00134       joint_variables_index_map_[name_order[j]] = joint_variables_index_map_[mim_name_order[j]];
00135     joint_variables_index_map_[later[i]->getName()] = joint_variables_index_map_[later[i]->mimic_->getName()];
00136   }
00137 
00138   for (std::size_t i = 0 ; i < link_model_vector_.size() ; ++i)
00139   {
00140     LinkModelToAffine3dMap associated_transforms;
00141     computeFixedTransforms(link_model_vector_[i], Eigen::Affine3d::Identity(), associated_transforms);
00142     if (associated_transforms.size() > 1)
00143     {
00144       for (LinkModelToAffine3dMap::iterator it = associated_transforms.begin() ; it != associated_transforms.end() ; ++it)
00145       {
00146         it->first->associated_fixed_transforms_[link_model_vector_[i]] = it->second.inverse();
00147         link_model_vector_[i]->associated_fixed_transforms_[it->first] = it->second;
00148       }
00149     }
00150   }
00151 }
00152 
00153 void robot_model::RobotModel::buildGroupStates(const srdf::Model &srdf_model)
00154 {
00155   // copy the default states to the groups
00156   const std::vector<srdf::Model::GroupState> &ds = srdf_model.getGroupStates();
00157   for (std::size_t i = 0 ; i < ds.size() ; ++i)
00158   {
00159     std::map<std::string, JointModelGroup*>::const_iterator it = joint_model_group_map_.find(ds[i].group_);
00160     if (it != joint_model_group_map_.end())
00161       for (std::map<std::string, std::vector<double> >::const_iterator jt = ds[i].joint_values_.begin() ; jt != ds[i].joint_values_.end() ; ++jt)
00162       {
00163         const JointModel* jm = it->second->getJointModel(jt->first);
00164         if (jm)
00165         {
00166           const std::vector<std::string> &vn = jm->getVariableNames();
00167           if (vn.size() == jt->second.size())
00168             for (std::size_t j = 0 ; j < vn.size() ; ++j)
00169               it->second->default_states_[ds[i].name_][vn[j]] = jt->second[j];
00170           else
00171             logError("The model for joint '%s' requires %d variable values, but only %d variable values were supplied in default state '%s' for group '%s'",
00172                      jt->first.c_str(), (int)vn.size(), (int)jt->second.size(), ds[i].name_.c_str(), it->first.c_str());
00173         }
00174         else
00175           logError("Group state '%s' specifies value for joint '%s', but that joint is not part of group '%s'", ds[i].name_.c_str(),
00176                    jt->first.c_str(), it->first.c_str());
00177       }
00178     else
00179       logError("Group state '%s' specified for group '%s', but that group does not exist", ds[i].name_.c_str(), ds[i].group_.c_str());
00180   }
00181 }
00182 
00183 void robot_model::RobotModel::buildMimic(const urdf::ModelInterface &urdf_model)
00184 {
00185   // compute mimic joints
00186   for (std::size_t i = 0 ; i < joint_model_vector_.size() ; ++i)
00187   {
00188     const urdf::Joint *jm = urdf_model.getJoint(joint_model_vector_[i]->getName()).get();
00189     if (jm)
00190       if (jm->mimic)
00191       {
00192         joint_model_vector_[i]->mimic_offset_ = jm->mimic->offset;
00193         joint_model_vector_[i]->mimic_factor_ = jm->mimic->multiplier;
00194         std::map<std::string, JointModel*>::const_iterator jit = joint_model_map_.find(jm->mimic->joint_name);
00195         if (jit != joint_model_map_.end())
00196         {
00197           if (joint_model_vector_[i]->getVariableCount() == jit->second->getVariableCount())
00198             joint_model_vector_[i]->mimic_ = jit->second;
00199           else
00200             logError("Join '%s' cannot mimic joint '%s' because they have different number of DOF",
00201                      joint_model_vector_[i]->getName().c_str(), jm->mimic->joint_name.c_str());
00202         }
00203         else
00204           logError("Joint '%s' cannot mimic unknown joint '%s'", joint_model_vector_[i]->getName().c_str(), jm->mimic->joint_name.c_str());
00205       }
00206   }
00207   // in case we have a joint that mimics a joint that already mimics another joint, we can simplify things:
00208   bool change = true;
00209   while (change)
00210   {
00211     change = false;
00212     for (std::size_t i = 0 ; i < joint_model_vector_.size() ; ++i)
00213       if (joint_model_vector_[i]->mimic_)
00214         if (joint_model_vector_[i]->mimic_->mimic_)
00215         {
00216           joint_model_vector_[i]->mimic_ = joint_model_vector_[i]->mimic_->mimic_;
00217           joint_model_vector_[i]->mimic_offset_ += joint_model_vector_[i]->mimic_factor_ * joint_model_vector_[i]->mimic_->mimic_offset_;
00218           joint_model_vector_[i]->mimic_factor_ *= joint_model_vector_[i]->mimic_->mimic_factor_;
00219           change = true;
00220         }
00221   }
00222   // build mimic requests
00223   for (std::size_t i = 0 ; i < joint_model_vector_.size() ; ++i)
00224     if (joint_model_vector_[i]->mimic_ == joint_model_vector_[i])
00225     {
00226       joint_model_vector_[i]->mimic_ = NULL;
00227       logError("Joint '%s' mimicks itself. This is not allowed.", joint_model_vector_[i]->getName().c_str());
00228     }
00229     else
00230       if (joint_model_vector_[i]->mimic_)
00231         joint_model_vector_[i]->mimic_->mimic_requests_.push_back(joint_model_vector_[i]);
00232 }
00233 
00234 bool robot_model::RobotModel::hasEndEffector(const std::string& eef) const
00235 {
00236   return end_effectors_.find(eef) != end_effectors_.end();
00237 }
00238 
00239 const robot_model::JointModelGroup* robot_model::RobotModel::getEndEffector(const std::string& name) const
00240 {
00241   std::map<std::string, JointModelGroup*>::const_iterator it = end_effectors_.find(name);
00242   if (it == end_effectors_.end())
00243   {
00244     it = joint_model_group_map_.find(name);
00245     if (it != joint_model_group_map_.end() && it->second->isEndEffector())
00246       return it->second;
00247     logError("End-effector '%s' not found in model %s", name.c_str(), model_name_.c_str());
00248     return NULL;
00249   }
00250   return it->second;
00251 }
00252 
00253 robot_model::JointModelGroup* robot_model::RobotModel::getEndEffector(const std::string& name)
00254 {
00255   std::map<std::string, JointModelGroup*>::const_iterator it = end_effectors_.find(name);
00256   if (it == end_effectors_.end())
00257   {
00258     it = joint_model_group_map_.find(name);
00259     if (it != joint_model_group_map_.end() && it->second->isEndEffector())
00260       return it->second;
00261     logError("End-effector '%s' not found in model %s", name.c_str(), model_name_.c_str());
00262     return NULL;
00263   }
00264   return it->second;
00265 }
00266 
00267 bool robot_model::RobotModel::hasJointModelGroup(const std::string &name) const
00268 {
00269   return joint_model_group_map_.find(name) != joint_model_group_map_.end();
00270 }
00271 
00272 const robot_model::JointModelGroup* robot_model::RobotModel::getJointModelGroup(const std::string& name) const
00273 {
00274   std::map<std::string, JointModelGroup*>::const_iterator it = joint_model_group_map_.find(name);
00275   if (it == joint_model_group_map_.end())
00276   {
00277     logError("Group '%s' not found in model %s", name.c_str(), model_name_.c_str());
00278     return NULL;
00279   }
00280   return it->second;
00281 }
00282 
00283 robot_model::JointModelGroup* robot_model::RobotModel::getJointModelGroup(const std::string& name)
00284 {
00285   std::map<std::string, JointModelGroup*>::const_iterator it = joint_model_group_map_.find(name);
00286   if (it == joint_model_group_map_.end())
00287   {
00288     logError("Group '%s' not found in model %s", name.c_str(), model_name_.c_str());
00289     return NULL;
00290   }
00291   return it->second;
00292 }
00293 
00294 void robot_model::RobotModel::buildGroups(const srdf::Model &srdf_model)
00295 {
00296   const std::vector<srdf::Model::Group>& group_configs = srdf_model.getGroups();
00297 
00298   //the only thing tricky is dealing with subgroups
00299   std::vector<bool> processed(group_configs.size(), false);
00300 
00301   bool added = true;
00302   while (added)
00303   {
00304     added = false;
00305 
00306     //going to make passes until we can't do anything else
00307     for(unsigned int i = 0 ; i < group_configs.size() ; ++i)
00308       if (!processed[i])
00309       {
00310         //if we haven't processed, check and see if the dependencies are met yet
00311         bool all_subgroups_added = true;
00312         for(unsigned int j = 0; j < group_configs[i].subgroups_.size(); ++j)
00313           if (joint_model_group_map_.find(group_configs[i].subgroups_[j]) == joint_model_group_map_.end())
00314           {
00315             all_subgroups_added = false;
00316             break;
00317           }
00318         if (all_subgroups_added)
00319         {
00320           added = true;
00321           processed[i] = true;
00322           if (!addJointModelGroup(group_configs[i]))
00323             logWarn("Failed to add group '%s'", group_configs[i].name_.c_str());
00324         }
00325       }
00326   }
00327 
00328   for (unsigned int i = 0 ; i < processed.size() ; ++i)
00329     if (!processed[i])
00330       logWarn("Could not process group '%s' due to unmet subgroup dependencies", group_configs[i].name_.c_str());
00331 
00332   buildGroupsInfo_Subgroups(srdf_model);
00333   buildGroupsInfo_EndEffectors(srdf_model);
00334 }
00335 
00336 void robot_model::RobotModel::buildGroupsInfo_Subgroups(const srdf::Model &srdf_model)
00337 {
00338   // compute subgroups
00339   for (std::map<std::string, JointModelGroup*>::const_iterator it = joint_model_group_map_.begin() ; it != joint_model_group_map_.end(); ++it)
00340   {
00341     JointModelGroup *jmg = it->second;
00342     jmg->subgroup_names_.clear();
00343     std::set<const JointModel*> joints(jmg->getJointModels().begin(), jmg->getJointModels().end());
00344     for (std::map<std::string, JointModelGroup*>::const_iterator jt = joint_model_group_map_.begin() ; jt != joint_model_group_map_.end(); ++jt)
00345       if (jt->first != it->first)
00346       {
00347         bool ok = true;
00348         JointModelGroup *sub_jmg = jt->second;
00349         const std::vector<const JointModel*> &sub_joints = sub_jmg->getJointModels();
00350         for (std::size_t k = 0 ; k < sub_joints.size() ; ++k)
00351           if (joints.find(sub_joints[k]) == joints.end())
00352           {
00353             ok = false;
00354             break;
00355           }
00356         if (ok)
00357           jmg->subgroup_names_.push_back(sub_jmg->getName());
00358       }
00359   }
00360 }
00361 
00362 void robot_model::RobotModel::buildGroupsInfo_EndEffectors(const srdf::Model &srdf_model)
00363 {
00364   // set the end-effector flags
00365   const std::vector<srdf::Model::EndEffector> &eefs = srdf_model.getEndEffectors();
00366   for (std::map<std::string, JointModelGroup*>::const_iterator it = joint_model_group_map_.begin() ; it != joint_model_group_map_.end(); ++it)
00367   {
00368     // check if this group is a known end effector
00369     for (std::size_t k = 0 ; k < eefs.size() ; ++k)
00370       if (eefs[k].component_group_ == it->first)
00371       {
00372         // if it is, mark it as such
00373         it->second->is_end_effector_ = true;
00374         it->second->end_effector_name_ = eefs[k].name_;
00375         end_effectors_[eefs[k].name_] = it->second;
00376 
00377         JointModelGroup *eef_parent_group = NULL;
00378 
00379         // if a parent group is specified in SRDF, try to use it
00380         if (!eefs[k].parent_group_.empty())
00381         {
00382           std::map<std::string, JointModelGroup*>::const_iterator jt = joint_model_group_map_.find(eefs[k].parent_group_);
00383           if (jt != joint_model_group_map_.end())
00384           {
00385             if (jt->second->hasLinkModel(eefs[k].parent_link_))
00386             {
00387               if (jt->second != it->second)
00388                 eef_parent_group = jt->second;
00389               else
00390                 logError("Group '%s' for end-effector '%s' cannot be its own parent", eefs[k].parent_group_.c_str(), eefs[k].name_.c_str());
00391             }
00392             else
00393               logError("Group '%s' was specified as parent group for end-effector '%s' but it does not include the parent link '%s'",
00394                        eefs[k].parent_group_.c_str(), eefs[k].name_.c_str(), eefs[k].parent_link_.c_str());
00395           }
00396           else
00397             logError("Group name '%s' not found (specified as parent group for end-effector '%s')",
00398                      eefs[k].parent_group_.c_str(), eefs[k].name_.c_str());
00399         }
00400 
00401         if (eef_parent_group == NULL)
00402         {
00403           // check to see if there are groups that contain the parent link of this end effector.
00404           // record this information if found
00405           std::vector<JointModelGroup*> possible_parent_groups;
00406           for (std::map<std::string, JointModelGroup*>::const_iterator jt = joint_model_group_map_.begin() ; jt != joint_model_group_map_.end(); ++jt)
00407             if (jt->first != it->first)
00408             {
00409               if (jt->second->hasLinkModel(eefs[k].parent_link_))
00410                 possible_parent_groups.push_back(jt->second);
00411             }
00412           if (!possible_parent_groups.empty())
00413           {
00414             // if there are multiple options for the group that contains this end-effector,
00415             // we pick the group with fewest joints.
00416             std::size_t best = 0;
00417             for (std::size_t g = 1 ; g < possible_parent_groups.size() ; ++g)
00418               if (possible_parent_groups[g]->getJointModels().size() < possible_parent_groups[best]->getJointModels().size())
00419                 best = g;
00420             eef_parent_group = possible_parent_groups[best];
00421           }
00422         }
00423         if (eef_parent_group)
00424         {
00425           eef_parent_group->attached_end_effector_names_.push_back(eefs[k].name_);
00426           it->second->end_effector_parent_.first = eef_parent_group->getName();
00427         }
00428         else
00429           logWarn("Could not identify parent group for end-effector '%s'", eefs[k].name_.c_str());
00430         it->second->end_effector_parent_.second = eefs[k].parent_link_;
00431         break;
00432       }
00433   }
00434 }
00435 
00436 bool robot_model::RobotModel::addJointModelGroup(const srdf::Model::Group& gc)
00437 {
00438   if (joint_model_group_map_.find(gc.name_) != joint_model_group_map_.end())
00439   {
00440     logWarn("A group named '%s' already exists. Not adding.",  gc.name_.c_str());
00441     return false;
00442   }
00443 
00444   std::set<const JointModel*> jset;
00445 
00446   // add joints from chains
00447   for (std::size_t i = 0 ; i < gc.chains_.size() ; ++i)
00448   {
00449     const LinkModel* base_link = getLinkModel(gc.chains_[i].first);
00450     const LinkModel* tip_link = getLinkModel(gc.chains_[i].second);
00451     if (base_link && tip_link)
00452     {
00453       // go from tip, up the chain, until we hit the root or we find the base_link
00454       const LinkModel* lm = tip_link;
00455       std::vector<const JointModel*> cj;
00456       while (lm)
00457       {
00458         if (lm == base_link)
00459           break;
00460         cj.push_back(lm->getParentJointModel());
00461         lm = lm->getParentJointModel()->getParentLinkModel();
00462       }
00463       // if we did not find the base_link, we could have a chain like e.g.,
00464       // from one end-effector to another end-effector, so the root is in between
00465       if (lm != base_link)
00466       {
00467         // we go up the chain from the base this time, and see where we intersect the other chain
00468         lm = base_link;
00469         std::size_t index = 0;
00470         std::vector<const JointModel*> cj2;
00471         while (lm)
00472         {
00473           for (std::size_t j = 0 ; j < cj.size() ; ++j)
00474             if (cj[j] == lm->getParentJointModel())
00475             {
00476               index = j + 1;
00477               break;
00478             }
00479           if (index > 0)
00480             break;
00481           cj2.push_back(lm->getParentJointModel());
00482           lm = lm->getParentJointModel()->getParentLinkModel();
00483         }
00484         if (index > 0)
00485         {
00486           jset.insert(cj.begin(), cj.begin() + index);
00487           jset.insert(cj2.begin(), cj2.end());
00488         }
00489       }
00490       else
00491         // if we have a simple chain, just add the joints
00492         jset.insert(cj.begin(), cj.end());
00493     }
00494   }
00495 
00496   // add joints
00497   for (std::size_t i = 0 ; i < gc.joints_.size() ; ++i)
00498   {
00499     const JointModel *j = getJointModel(gc.joints_[i]);
00500     if (j)
00501       jset.insert(j);
00502   }
00503 
00504   // add joints that are parents of included links
00505   for (std::size_t i = 0 ; i < gc.links_.size() ; ++i)
00506   {
00507     const LinkModel *l = getLinkModel(gc.links_[i]);
00508     if (l)
00509       jset.insert(l->getParentJointModel());
00510   }
00511 
00512   // add joints from subgroups
00513   for (std::size_t i = 0 ; i < gc.subgroups_.size() ; ++i)
00514   {
00515     const JointModelGroup *sg = getJointModelGroup(gc.subgroups_[i]);
00516     if (sg)
00517     {
00518       //active joints
00519       const std::vector<const JointModel*> &js = sg->getJointModels();
00520       for (std::size_t j = 0 ; j < js.size() ; ++j)
00521         jset.insert(js[j]);
00522 
00523       //fixed joints
00524       const std::vector<const JointModel*> &fs = sg->getFixedJointModels();
00525       for (std::size_t j = 0 ; j < fs.size() ; ++j)
00526         jset.insert(fs[j]);
00527 
00528       //mimic joints
00529       const std::vector<const JointModel*> &ms = sg->getMimicJointModels();
00530       for (std::size_t j = 0 ; j < ms.size() ; ++j)
00531         jset.insert(ms[j]);
00532     }
00533   }
00534 
00535   if (jset.empty())
00536   {
00537     logWarn("Group '%s' must have at least one valid joint", gc.name_.c_str());
00538     return false;
00539   }
00540 
00541   std::vector<const JointModel*> joints;
00542   for (std::set<const JointModel*>::iterator it = jset.begin() ; it != jset.end() ; ++it)
00543     joints.push_back(*it);
00544 
00545   JointModelGroup *jmg = new JointModelGroup(gc.name_, joints, this);
00546   joint_model_group_map_[gc.name_] = jmg;
00547   joint_model_group_config_map_[gc.name_] = gc;
00548   joint_model_group_names_.push_back(gc.name_);
00549 
00550   // if the group is defined as a single chain, then we mark is as a chain aleady
00551   // (this is for the case where the chain does not consist of consecutive joints and would not be detected as a chain later)
00552   if (gc.chains_.size() == 1 && gc.joints_.empty() && gc.links_.empty() && gc.subgroups_.empty())
00553     jmg->is_chain_ = true;
00554 
00555   return true;
00556 }
00557 
00558 robot_model::JointModel* robot_model::RobotModel::buildRecursive(LinkModel *parent, const urdf::Link *urdf_link,
00559                                                                  const srdf::Model &srdf_model)
00560 {
00561   // construct the joint
00562   JointModel *joint = urdf_link->parent_joint ?
00563     constructJointModel(urdf_link->parent_joint.get(), urdf_link, srdf_model) :
00564     constructJointModel(NULL, urdf_link, srdf_model);
00565   if (joint == NULL)
00566     return NULL;
00567 
00568   // bookkeeping for the joint
00569   joint_model_map_[joint->name_] = joint;
00570   joint->tree_index_ = joint_model_vector_.size();
00571   joint_model_vector_.push_back(joint);
00572   joint_model_vector_const_.push_back(joint);
00573   joint_model_names_vector_.push_back(joint->getName());
00574   if (joint->getType() == JointModel::REVOLUTE && static_cast<const RevoluteJointModel*>(joint)->isContinuous())
00575     continuous_joint_model_vector_const_.push_back(joint);
00576   joint->parent_link_model_ = parent;
00577 
00578   // construct the link
00579   LinkModel *link = constructLinkModel(urdf_link);
00580   joint->child_link_model_ = link;
00581 
00582   // bookkeeping for the link
00583   link_model_map_[joint->child_link_model_->name_] = link;
00584   link->tree_index_ = link_model_vector_.size();
00585   link_model_vector_.push_back(link);
00586   link_model_vector_const_.push_back(link);
00587   link_model_names_vector_.push_back(link->getName());
00588   if (link->shape_)
00589   {
00590     link_models_with_collision_geometry_vector_.push_back(link);
00591     link_model_names_with_collision_geometry_vector_.push_back(link->getName());
00592   }
00593   link->parent_joint_model_ = joint;
00594 
00595   // recursively build child links (and joints)
00596   for (std::size_t i = 0 ; i < urdf_link->child_links.size() ; ++i)
00597   {
00598     JointModel* jm = buildRecursive(link, urdf_link->child_links[i].get(), srdf_model);
00599     if (jm)
00600       link->child_joint_models_.push_back(jm);
00601   }
00602   return joint;
00603 }
00604 
00605 robot_model::JointModel* robot_model::RobotModel::constructJointModel(const urdf::Joint *urdf_joint, const urdf::Link *child_link,
00606                                                                       const srdf::Model &srdf_model)
00607 {
00608   JointModel* result = NULL;
00609 
00610   // must be the root link transform
00611   if (urdf_joint)
00612   {
00613     switch (urdf_joint->type)
00614     {
00615     case urdf::Joint::REVOLUTE:
00616       {
00617         RevoluteJointModel *j = new RevoluteJointModel(urdf_joint->name);
00618         if (urdf_joint->safety)
00619         {
00620           j->variable_bounds_[0] = std::make_pair(urdf_joint->safety->soft_lower_limit, urdf_joint->safety->soft_upper_limit);
00621           if (urdf_joint->limits)
00622           {
00623             if (urdf_joint->limits->lower > j->variable_bounds_[0].first)
00624               j->variable_bounds_[0].first = urdf_joint->limits->lower;
00625             if (urdf_joint->limits->upper < j->variable_bounds_[0].second)
00626               j->variable_bounds_[0].second = urdf_joint->limits->upper;
00627           }
00628         }
00629         else
00630         {
00631           if (urdf_joint->limits)
00632             j->variable_bounds_[0] = std::make_pair(urdf_joint->limits->lower, urdf_joint->limits->upper);
00633           else
00634             j->variable_bounds_[0] = std::make_pair(0.0, 0.0);
00635         }
00636         if (urdf_joint->limits)
00637           j->max_velocity_ = fabs(urdf_joint->limits->velocity);
00638         j->continuous_ = false;
00639         j->axis_ = Eigen::Vector3d(urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z);
00640         result = j;
00641       }
00642       break;
00643     case urdf::Joint::CONTINUOUS:
00644       {
00645         RevoluteJointModel *j = new RevoluteJointModel(urdf_joint->name);
00646         j->continuous_ = true;
00647         j->variable_bounds_[0] = std::make_pair(-boost::math::constants::pi<double>(), boost::math::constants::pi<double>());
00648         if (urdf_joint->limits)
00649           j->max_velocity_ = fabs(urdf_joint->limits->velocity);
00650         j->axis_ = Eigen::Vector3d(urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z);
00651         result = j;
00652       }
00653       break;
00654     case urdf::Joint::PRISMATIC:
00655       {
00656         PrismaticJointModel *j = new PrismaticJointModel(urdf_joint->name);
00657         if(urdf_joint->safety)
00658         {
00659           j->variable_bounds_[0] = std::make_pair(urdf_joint->safety->soft_lower_limit, urdf_joint->safety->soft_upper_limit);
00660           if (urdf_joint->limits)
00661           {
00662             if (urdf_joint->limits->lower > j->variable_bounds_[0].first)
00663               j->variable_bounds_[0].first = urdf_joint->limits->lower;
00664             if (urdf_joint->limits->upper < j->variable_bounds_[0].second)
00665               j->variable_bounds_[0].second = urdf_joint->limits->upper;
00666           }
00667         }
00668         else
00669         {
00670           if (urdf_joint->limits)
00671             j->variable_bounds_[0] = std::make_pair(urdf_joint->limits->lower, urdf_joint->limits->upper);
00672           else
00673             j->variable_bounds_[0] = std::make_pair(0.0, 0.0);
00674         }
00675         if (urdf_joint->limits)
00676           j->max_velocity_ = fabs(urdf_joint->limits->velocity);
00677         j->axis_ = Eigen::Vector3d(urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z);
00678         result = j;
00679       }
00680       break;
00681     case urdf::Joint::FLOATING:
00682       result = new FloatingJointModel(urdf_joint->name);
00683       break;
00684     case urdf::Joint::PLANAR:
00685       result = new PlanarJointModel(urdf_joint->name);
00686       break;
00687     case urdf::Joint::FIXED:
00688       result = new FixedJointModel(urdf_joint->name);
00689       break;
00690     default:
00691       logError("Unknown joint type: %d", (int)urdf_joint->type);
00692       break;
00693     }
00694   }
00695   else
00696   {
00697     const std::vector<srdf::Model::VirtualJoint> &vjoints = srdf_model.getVirtualJoints();
00698     for (std::size_t i = 0 ; i < vjoints.size() ; ++i)
00699       if (vjoints[i].child_link_ == child_link->name && !vjoints[i].parent_frame_.empty())
00700       {
00701         if (vjoints[i].type_ == "fixed")
00702           result = new FixedJointModel(vjoints[i].name_);
00703         else if (vjoints[i].type_ == "planar")
00704           result = new PlanarJointModel(vjoints[i].name_);
00705         else if (vjoints[i].type_ == "floating")
00706           result = new FloatingJointModel(vjoints[i].name_);
00707         if (result)
00708         {
00709           // for fixed frames we still use the robot root link
00710           if (vjoints[i].type_ != "fixed")
00711           {
00712             model_frame_ = vjoints[i].parent_frame_;
00713             if (model_frame_[0] != '/')
00714               model_frame_ = '/' + model_frame_;
00715           }
00716           break;
00717         }
00718       }
00719     if (!result)
00720     {
00721       logInform("No root joint specified. Assuming fixed joint");
00722       result = new FixedJointModel("ASSUMED_FIXED_ROOT_JOINT");
00723     }
00724   }
00725 
00726   if (result)
00727   {
00728     for (std::size_t i = 0 ; i < result->variable_names_.size() ; ++i)
00729       result->variable_index_[result->variable_names_[i]] = i;
00730     result->setDistanceFactor(result->getStateSpaceDimension());
00731 
00732     const std::vector<srdf::Model::PassiveJoint> &pjoints = srdf_model.getPassiveJoints();
00733     for (std::size_t i = 0 ; i < pjoints.size() ; ++i)
00734     {
00735       if (result->getName() == pjoints[i].name_)
00736       {
00737         result->passive_ = true;
00738         break;
00739       }
00740     }
00741     result->computeDefaultVariableLimits();
00742   }
00743   return result;
00744 }
00745 
00746 namespace robot_model
00747 {
00748 static inline Eigen::Affine3d urdfPose2Affine3d(const urdf::Pose &pose)
00749 {
00750   Eigen::Quaterniond q(pose.rotation.w, pose.rotation.x, pose.rotation.y, pose.rotation.z);
00751   Eigen::Affine3d af(Eigen::Translation3d(pose.position.x, pose.position.y, pose.position.z)*q.toRotationMatrix());
00752   return af;
00753 }
00754 
00755 }
00756 
00757 robot_model::LinkModel* robot_model::RobotModel::constructLinkModel(const urdf::Link *urdf_link)
00758 {
00759   LinkModel *result = new LinkModel();
00760   result->name_ = urdf_link->name;
00761   if (urdf_link->collision && urdf_link->collision->geometry)
00762   {
00763     result->collision_origin_transform_ = urdfPose2Affine3d(urdf_link->collision->origin);
00764     result->shape_ = constructShape(urdf_link->collision->geometry.get());
00765     if (result->shape_)
00766     {
00767       if (shapes::constructMsgFromShape(result->shape_.get(), result->shape_msg_))
00768         result->shape_extents_ = shapes::computeShapeExtents(result->shape_msg_);
00769       else
00770         result->shape_extents_ = Eigen::Vector3d(0.0, 0.0, 0.0);
00771     }
00772   }
00773   else if (urdf_link->visual && urdf_link->visual->geometry)
00774   {
00775     result->collision_origin_transform_ = urdfPose2Affine3d(urdf_link->visual->origin);
00776     result->shape_ = constructShape(urdf_link->visual->geometry.get());
00777     if (result->shape_)
00778     {
00779       if (shapes::constructMsgFromShape(result->shape_.get(), result->shape_msg_))
00780         result->shape_extents_ = shapes::computeShapeExtents(result->shape_msg_);
00781       else
00782         result->shape_extents_ = Eigen::Vector3d(0.0, 0.0, 0.0);
00783     }
00784   }
00785   else
00786   {
00787     result->collision_origin_transform_.setIdentity();
00788     result->shape_.reset();
00789     result->shape_extents_ = Eigen::Vector3d(0.0, 0.0, 0.0);
00790   }
00791 
00792   // figure out visual mesh (try visual urdf tag first, collision tag otherwise
00793   if (urdf_link->visual && urdf_link->visual->geometry)
00794   {
00795     if (urdf_link->visual->geometry->type == urdf::Geometry::MESH)
00796     {
00797       const urdf::Mesh *mesh = static_cast<const urdf::Mesh*>(urdf_link->visual->geometry.get());
00798       if (!mesh->filename.empty())
00799       {
00800         result->visual_mesh_filename_ = mesh->filename;
00801         result->visual_mesh_scale_ = Eigen::Vector3d(mesh->scale.x, mesh->scale.y, mesh->scale.z);
00802       }
00803     }
00804   }
00805   else
00806     if (urdf_link->collision && urdf_link->collision->geometry)
00807     {
00808       if (urdf_link->collision->geometry->type == urdf::Geometry::MESH)
00809       {
00810         const urdf::Mesh *mesh = static_cast<const urdf::Mesh*>(urdf_link->collision->geometry.get());
00811         if (!mesh->filename.empty())
00812         {
00813           result->visual_mesh_filename_ = mesh->filename;
00814           result->visual_mesh_scale_ = Eigen::Vector3d(mesh->scale.x, mesh->scale.y, mesh->scale.z);
00815         }
00816       }
00817     }
00818 
00819   if (urdf_link->parent_joint)
00820     result->joint_origin_transform_ = urdfPose2Affine3d(urdf_link->parent_joint->parent_to_joint_origin_transform);
00821   else
00822     result->joint_origin_transform_.setIdentity();
00823 
00824   return result;
00825 }
00826 
00827 shapes::ShapePtr robot_model::RobotModel::constructShape(const urdf::Geometry *geom)
00828 {
00829   moveit::Profiler::ScopedBlock prof_block("RobotModel::constructShape");
00830 
00831   shapes::Shape *result = NULL;
00832   switch (geom->type)
00833   {
00834   case urdf::Geometry::SPHERE:
00835     result = new shapes::Sphere(static_cast<const urdf::Sphere*>(geom)->radius);
00836     break;
00837   case urdf::Geometry::BOX:
00838     {
00839       urdf::Vector3 dim = static_cast<const urdf::Box*>(geom)->dim;
00840       result = new shapes::Box(dim.x, dim.y, dim.z);
00841     }
00842     break;
00843   case urdf::Geometry::CYLINDER:
00844     result = new shapes::Cylinder(static_cast<const urdf::Cylinder*>(geom)->radius,
00845                                   static_cast<const urdf::Cylinder*>(geom)->length);
00846     break;
00847   case urdf::Geometry::MESH:
00848     {
00849       const urdf::Mesh *mesh = static_cast<const urdf::Mesh*>(geom);
00850       if (!mesh->filename.empty())
00851       {
00852         Eigen::Vector3d scale(mesh->scale.x, mesh->scale.y, mesh->scale.z);
00853         shapes::Mesh *m = shapes::createMeshFromResource(mesh->filename, scale);
00854         result = m;
00855       }
00856     }
00857     break;
00858   default:
00859     logError("Unknown geometry type: %d", (int)geom->type);
00860     break;
00861   }
00862 
00863   return shapes::ShapePtr(result);
00864 }
00865 
00866 const std::string& robot_model::RobotModel::getRootJointName() const
00867 {
00868   static const std::string empty;
00869   return getRoot() ? getRoot()->getName() : empty;
00870 }
00871 
00872 const std::string& robot_model::RobotModel::getRootLinkName() const
00873 {
00874   static const std::string empty;
00875   return getRootLink() ? getRootLink()->getName() : empty;
00876 }
00877 
00878 bool robot_model::RobotModel::hasJointModel(const std::string &name) const
00879 {
00880   return joint_model_map_.find(name) != joint_model_map_.end();
00881 }
00882 
00883 bool robot_model::RobotModel::hasLinkModel(const std::string &name) const
00884 {
00885   return link_model_map_.find(name) != link_model_map_.end();
00886 }
00887 
00888 const robot_model::JointModel* robot_model::RobotModel::getJointModel(const std::string &name) const
00889 {
00890   std::map<std::string, JointModel*>::const_iterator it = joint_model_map_.find(name);
00891   if (it == joint_model_map_.end())
00892   {
00893     logError("Joint '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
00894     return NULL;
00895   }
00896   else
00897     return it->second;
00898 }
00899 
00900 const robot_model::LinkModel* robot_model::RobotModel::getLinkModel(const std::string &name) const
00901 {
00902   std::map<std::string, LinkModel*>::const_iterator it = link_model_map_.find(name);
00903   if (it == link_model_map_.end())
00904   {
00905     logError("Link '%s' not found", name.c_str());
00906     return NULL;
00907   }
00908   else
00909     return it->second;
00910 }
00911 
00912 void robot_model::RobotModel::getChildLinkModels(const LinkModel *parent, std::vector<const LinkModel*> &links) const
00913 {
00914   links.clear();
00915   links.push_back(parent);
00916   std::queue<const LinkModel*> q;
00917   std::set<const LinkModel*> seen;
00918   q.push(parent);
00919   while (!q.empty())
00920   {
00921     const LinkModel* t = q.front();
00922     q.pop();
00923     if (seen.insert(t).second)
00924       for (std::size_t i = 0 ; i < t->child_joint_models_.size() ; ++i)
00925       {
00926         links.push_back(t->child_joint_models_[i]->child_link_model_);
00927         q.push(t->child_joint_models_[i]->child_link_model_);
00928         for (std::size_t j = 0 ; j < t->child_joint_models_[i]->mimic_requests_.size() ; ++j)
00929         {
00930           links.push_back(t->child_joint_models_[i]->mimic_requests_[j]->child_link_model_);
00931           q.push(t->child_joint_models_[i]->mimic_requests_[j]->child_link_model_);
00932         }
00933       }
00934   }
00935 }
00936 
00937 void robot_model::RobotModel::getChildLinkModels(const JointModel *parent, std::vector<const LinkModel*> &links) const
00938 {
00939   getChildLinkModels(parent->child_link_model_, links);
00940 }
00941 
00942 void robot_model::RobotModel::getChildJointModels(const LinkModel *parent, std::vector<const JointModel*> &joints) const
00943 {
00944   joints.clear();
00945   std::queue<const LinkModel*> q;
00946   std::set<const LinkModel*> seen;
00947   q.push(parent);
00948 
00949   while (!q.empty())
00950   {
00951     const LinkModel* t = q.front();
00952     q.pop();
00953     if (seen.insert(t).second)
00954       for (unsigned int i = 0 ; i < t->child_joint_models_.size() ; ++i)
00955       {
00956         joints.push_back(t->child_joint_models_[i]);
00957         q.push(t->child_joint_models_[i]->child_link_model_);
00958         for (std::size_t j = 0 ; j < t->child_joint_models_[i]->mimic_requests_.size() ; ++j)
00959         {
00960           joints.push_back(t->child_joint_models_[i]->mimic_requests_[j]);
00961           q.push(t->child_joint_models_[i]->mimic_requests_[j]->child_link_model_);
00962         }
00963       }
00964   }
00965 }
00966 
00967 void robot_model::RobotModel::getChildJointModels(const JointModel *parent, std::vector<const JointModel*> &joints) const
00968 {
00969   getChildJointModels(parent->child_link_model_, joints);
00970   joints.insert(joints.begin(), parent);
00971 }
00972 
00973 std::vector<std::string> robot_model::RobotModel::getChildLinkModelNames(const LinkModel *parent) const
00974 {
00975   std::vector<const LinkModel*> links;
00976   getChildLinkModels(parent, links);
00977   std::vector<std::string> ret_vec(links.size());
00978   for (std::size_t i = 0; i < links.size(); ++i)
00979     ret_vec[i] = links[i]->getName();
00980   return ret_vec;
00981 }
00982 
00983 std::vector<std::string> robot_model::RobotModel::getChildLinkModelNames(const JointModel *parent) const
00984 {
00985   std::vector<const LinkModel*> links;
00986   getChildLinkModels(parent, links);
00987   std::vector<std::string> ret_vec(links.size());
00988   for(unsigned int i = 0; i < links.size(); ++i)
00989     ret_vec[i] = links[i]->getName();
00990   return ret_vec;
00991 }
00992 
00993 std::vector<std::string> robot_model::RobotModel::getChildJointModelNames(const LinkModel *parent) const
00994 {
00995   std::vector<const JointModel*> joints;
00996   getChildJointModels(parent, joints);
00997   std::vector<std::string> ret_vec(joints.size());
00998   for(unsigned int i = 0 ; i < joints.size() ; ++i)
00999     ret_vec[i] = joints[i]->getName();
01000   return ret_vec;
01001 }
01002 
01003 std::vector<std::string> robot_model::RobotModel::getChildJointModelNames(const JointModel *parent) const
01004 {
01005   std::vector<const JointModel*> joints;
01006   getChildJointModels(parent, joints);
01007   std::vector<std::string> ret_vec(joints.size());
01008   for(unsigned int i = 0 ; i < joints.size(); ++i)
01009     ret_vec[i] = joints[i]->getName();
01010   return ret_vec;
01011 }
01012 
01013 
01014 void robot_model::RobotModel::getVariableRandomValues(random_numbers::RandomNumberGenerator &rng, std::vector<double> &values) const
01015 {
01016   for (std::size_t i = 0  ; i < joint_model_vector_.size() ; ++i)
01017     if (joint_model_vector_[i]->mimic_ == NULL)
01018       joint_model_vector_[i]->getVariableRandomValues(rng, values);
01019 }
01020 
01021 void robot_model::RobotModel::getVariableRandomValues(random_numbers::RandomNumberGenerator &rng, std::map<std::string, double> &values) const
01022 {
01023   for (std::size_t i = 0  ; i < joint_model_vector_.size() ; ++i)
01024     if (joint_model_vector_[i]->mimic_ == NULL)
01025       joint_model_vector_[i]->getVariableRandomValues(rng, values);
01026 }
01027 
01028 void robot_model::RobotModel::getVariableDefaultValues(std::vector<double> &values) const
01029 {
01030   for (std::size_t i = 0  ; i < joint_model_vector_.size() ; ++i)
01031     if (joint_model_vector_[i]->mimic_ == NULL)
01032       joint_model_vector_[i]->getVariableDefaultValues(values);
01033 }
01034 
01035 void robot_model::RobotModel::getVariableDefaultValues(std::map<std::string, double> &values) const
01036 {
01037   for (std::size_t i = 0  ; i < joint_model_vector_.size() ; ++i)
01038     if (joint_model_vector_[i]->mimic_ == NULL)
01039       joint_model_vector_[i]->getVariableDefaultValues(values);
01040 }
01041 
01042 void robot_model::RobotModel::setKinematicsAllocators(const std::map<std::string, SolverAllocatorFn> &allocators)
01043 {
01044   for (std::map<std::string, JointModelGroup*>::const_iterator it = joint_model_group_map_.begin() ; it != joint_model_group_map_.end() ; ++it)
01045   {
01046     JointModelGroup *jmg = it->second;
01047     std::pair<SolverAllocatorFn, SolverAllocatorMapFn> result;
01048     std::map<std::string, SolverAllocatorFn>::const_iterator jt = allocators.find(jmg->getName());
01049     if (jt == allocators.end())
01050     {
01051       // if an kinematics allocator is NOT available for this group, we try to see if we can use subgroups for IK
01052       std::set<const JointModel*> joints;
01053       joints.insert(jmg->getJointModels().begin(), jmg->getJointModels().end());
01054 
01055       std::vector<const JointModelGroup*> subs;
01056 
01057       // go through the groups that we know have IK allocators and see if they are included in the group that does not; if so, put that group in sub
01058       for (std::map<std::string, SolverAllocatorFn>::const_iterator kt = allocators.begin() ; kt != allocators.end() ; ++kt)
01059       {
01060         const JointModelGroup *sub = getJointModelGroup(kt->first);
01061         if (!sub)
01062         {
01063           subs.clear();
01064           break;
01065         }
01066         std::set<const JointModel*> sub_joints;
01067         sub_joints.insert(sub->getJointModels().begin(), sub->getJointModels().end());
01068 
01069         if (std::includes(joints.begin(), joints.end(), sub_joints.begin(), sub_joints.end()))
01070         {
01071           std::set<const JointModel*> resultj;
01072           std::set_difference(joints.begin(), joints.end(), sub_joints.begin(), sub_joints.end(),
01073                               std::inserter(resultj, resultj.end()));
01074           subs.push_back(sub);
01075           joints.swap(resultj);
01076         }
01077       }
01078 
01079       // if we found subgroups, pass that information to the planning group
01080       if (!subs.empty())
01081       {
01082         std::stringstream ss;
01083         for (std::size_t i = 0 ; i < subs.size() ; ++i)
01084         {
01085           ss << subs[i]->getName() << " ";
01086           result.second[subs[i]] = allocators.find(subs[i]->getName())->second;
01087         }
01088         logDebug("Added sub-group IK allocators for group '%s': [ %s]", jmg->getName().c_str(), ss.str().c_str());
01089       }
01090     }
01091     else
01092       // if the IK allocator is for this group, we use it
01093       result.first = jt->second;
01094     jmg->setSolverAllocators(result);
01095   }
01096 }
01097 
01098 void robot_model::RobotModel::printModelInfo(std::ostream &out) const
01099 {
01100   out << "Model " << model_name_ << " in frame " << model_frame_ << ", of dimension " << getVariableCount() << std::endl;
01101 
01102   std::ios_base::fmtflags old_flags = out.flags();
01103   out.setf(std::ios::fixed, std::ios::floatfield);
01104   std::streamsize old_prec = out.precision();
01105   out.precision(5);
01106   out << "Joint values bounds: " << std::endl;
01107   for (unsigned int i = 0 ; i < joint_model_vector_.size() ; ++i)
01108   {
01109     const std::vector<std::string> &vn = joint_model_vector_[i]->getVariableNames();
01110     for (std::vector<std::string>::const_iterator it = vn.begin() ; it != vn.end() ; ++it)
01111     {
01112       out << "   " << *it << " [";
01113       std::pair<double, double> b;
01114       joint_model_vector_[i]->getVariableBounds(*it, b);
01115       if (b.first <= -std::numeric_limits<double>::max())
01116         out << "DBL_MIN";
01117       else
01118         out << b.first;
01119       out << ", ";
01120       if (b.second >= std::numeric_limits<double>::max())
01121         out << "DBL_MAX";
01122       else
01123         out << b.second;
01124       out << "]";
01125       if (joint_model_vector_[i]->mimic_)
01126         out << " *";
01127       if (joint_model_vector_[i]->passive_)
01128         out << " +";
01129       out << std::endl;
01130     }
01131   }
01132   out << std::endl;
01133   out.precision(old_prec);
01134   out.flags(old_flags);
01135 
01136   out << "Available groups: " << std::endl;
01137   for (std::map<std::string, JointModelGroup*>::const_iterator it = joint_model_group_map_.begin() ; it != joint_model_group_map_.end() ; ++it)
01138   {
01139     out << "   " << it->first << " (of dimension " << it->second->getVariableCount() << "):" << std::endl;
01140     out << "     joints:" << std::endl;
01141     const std::vector<std::string> &jnt = it->second->getJointModelNames();
01142     for (std::size_t k = 0 ; k < jnt.size() ; ++k)
01143       out << "      " << jnt[k] << std::endl;
01144     out << "     links:" << std::endl;
01145     const std::vector<std::string> &lnk = it->second->getLinkModelNames();
01146     for (std::size_t k = 0 ; k < lnk.size() ; ++k)
01147       out << "      " << lnk[k] << std::endl;
01148     out << "     roots:" << std::endl;
01149     const std::vector<const JointModel*> &jr = it->second->getJointRoots();
01150     for (std::size_t k = 0 ; k < jr.size() ; ++k)
01151       out << "      " << jr[k]->getName() << std::endl;
01152 
01153   }
01154 }
01155 
01156 void robot_model::RobotModel::computeFixedTransforms(LinkModel *link, const Eigen::Affine3d &transform, LinkModelToAffine3dMap &associated_transforms)
01157 {
01158   associated_transforms[link] = transform;
01159   for (std::size_t i = 0 ; i < link->getChildJointModels().size() ; ++i)
01160     if (link->getChildJointModels()[i]->getType() == JointModel::FIXED)
01161       computeFixedTransforms(link->getChildJointModels()[i]->child_link_model_, transform * link->getJointOriginTransform(), associated_transforms);
01162 }


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Oct 6 2014 02:24:47