robot_model.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, Ioan A. Sucan
00005  *  Copyright (c) 2013, Willow Garage, Inc.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the Willow Garage nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 /* Author: Ioan Sucan */
00037 
00038 #include <moveit/robot_model/robot_model.h>
00039 #include <geometric_shapes/shape_operations.h>
00040 #include <boost/math/constants/constants.hpp>
00041 #include <moveit/profiler/profiler.h>
00042 #include <algorithm>
00043 #include <limits>
00044 #include <queue>
00045 #include <cmath>
00046 #include "order_robot_model_items.inc"
00047 
00048 /* ------------------------ RobotModel ------------------------ */
00049 
00050 moveit::core::RobotModel::RobotModel(const boost::shared_ptr<const urdf::ModelInterface>& urdf_model,
00051                                      const boost::shared_ptr<const srdf::Model>& srdf_model)
00052 {
00053   root_joint_ = NULL;
00054   urdf_ = urdf_model;
00055   srdf_ = srdf_model;
00056   buildModel(*urdf_model, *srdf_model);
00057 }
00058 
00059 moveit::core::RobotModel::~RobotModel()
00060 {
00061   for (JointModelGroupMap::iterator it = joint_model_group_map_.begin(); it != joint_model_group_map_.end(); ++it)
00062     delete it->second;
00063   for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
00064     delete joint_model_vector_[i];
00065   for (std::size_t i = 0; i < link_model_vector_.size(); ++i)
00066     delete link_model_vector_[i];
00067 }
00068 
00069 const moveit::core::JointModel* moveit::core::RobotModel::getRootJoint() const
00070 {
00071   return root_joint_;
00072 }
00073 
00074 const moveit::core::LinkModel* moveit::core::RobotModel::getRootLink() const
00075 {
00076   return root_link_;
00077 }
00078 
00079 void moveit::core::RobotModel::buildModel(const urdf::ModelInterface& urdf_model, const srdf::Model& srdf_model)
00080 {
00081   moveit::tools::Profiler::ScopedStart prof_start;
00082   moveit::tools::Profiler::ScopedBlock prof_block("RobotModel::buildModel");
00083 
00084   root_joint_ = NULL;
00085   root_link_ = NULL;
00086   link_geometry_count_ = 0;
00087   variable_count_ = 0;
00088   model_name_ = urdf_model.getName();
00089   logInform("Loading robot model '%s'...", model_name_.c_str());
00090 
00091   if (urdf_model.getRoot())
00092   {
00093     const urdf::Link* root_link_ptr = urdf_model.getRoot().get();
00094     model_frame_ = '/' + root_link_ptr->name;
00095 
00096     logDebug("... building kinematic chain");
00097     root_joint_ = buildRecursive(NULL, root_link_ptr, srdf_model);
00098     if (root_joint_)
00099       root_link_ = root_joint_->getChildLinkModel();
00100     logDebug("... building mimic joints");
00101     buildMimic(urdf_model);
00102 
00103     logDebug("... computing joint indexing");
00104     buildJointInfo();
00105 
00106     if (link_models_with_collision_geometry_vector_.empty())
00107       logWarn("No geometry is associated to any robot links");
00108 
00109     // build groups
00110 
00111     logDebug("... constructing joint groups");
00112     buildGroups(srdf_model);
00113 
00114     logDebug("... constructing joint group states");
00115     buildGroupStates(srdf_model);
00116 
00117     // For debugging entire model
00118     if (false)
00119       printModelInfo(std::cout);
00120   }
00121   else
00122     logWarn("No root link found");
00123 }
00124 
00125 namespace moveit
00126 {
00127 namespace core
00128 {
00129 namespace
00130 {
00131 typedef std::map<const JointModel*, std::pair<std::set<const LinkModel*, OrderLinksByIndex>,
00132                                               std::set<const JointModel*, OrderJointsByIndex> > >
00133     DescMap;
00134 
00135 void computeDescendantsHelper(const JointModel* joint, std::vector<const JointModel*>& parents,
00136                               std::set<const JointModel*>& seen, DescMap& descendants)
00137 {
00138   if (!joint)
00139     return;
00140   if (seen.find(joint) != seen.end())
00141     return;
00142   seen.insert(joint);
00143 
00144   for (std::size_t i = 0; i < parents.size(); ++i)
00145     descendants[parents[i]].second.insert(joint);
00146 
00147   const LinkModel* lm = joint->getChildLinkModel();
00148   if (!lm)
00149     return;
00150 
00151   for (std::size_t i = 0; i < parents.size(); ++i)
00152     descendants[parents[i]].first.insert(lm);
00153   descendants[joint].first.insert(lm);
00154 
00155   parents.push_back(joint);
00156   const std::vector<const JointModel*>& ch = lm->getChildJointModels();
00157   for (std::size_t i = 0; i < ch.size(); ++i)
00158     computeDescendantsHelper(ch[i], parents, seen, descendants);
00159   const std::vector<const JointModel*>& mim = joint->getMimicRequests();
00160   for (std::size_t i = 0; i < mim.size(); ++i)
00161     computeDescendantsHelper(mim[i], parents, seen, descendants);
00162   parents.pop_back();
00163 }
00164 
00165 void computeCommonRootsHelper(const JointModel* joint, std::vector<int>& common_roots, int size)
00166 {
00167   if (!joint)
00168     return;
00169   const LinkModel* lm = joint->getChildLinkModel();
00170   if (!lm)
00171     return;
00172 
00173   const std::vector<const JointModel*>& ch = lm->getChildJointModels();
00174   for (std::size_t i = 0; i < ch.size(); ++i)
00175   {
00176     const std::vector<const JointModel*>& a = ch[i]->getDescendantJointModels();
00177     for (std::size_t j = i + 1; j < ch.size(); ++j)
00178     {
00179       const std::vector<const JointModel*>& b = ch[j]->getDescendantJointModels();
00180       for (std::size_t m = 0; m < b.size(); ++m)
00181         common_roots[ch[i]->getJointIndex() * size + b[m]->getJointIndex()] =
00182             common_roots[ch[i]->getJointIndex() + b[m]->getJointIndex() * size] = joint->getJointIndex();
00183       for (std::size_t k = 0; k < a.size(); ++k)
00184       {
00185         common_roots[a[k]->getJointIndex() * size + ch[j]->getJointIndex()] =
00186             common_roots[a[k]->getJointIndex() + ch[j]->getJointIndex() * size] = joint->getJointIndex();
00187         for (std::size_t m = 0; m < b.size(); ++m)
00188           common_roots[a[k]->getJointIndex() * size + b[m]->getJointIndex()] =
00189               common_roots[a[k]->getJointIndex() + b[m]->getJointIndex() * size] = joint->getJointIndex();
00190       }
00191     }
00192     computeCommonRootsHelper(ch[i], common_roots, size);
00193   }
00194 }
00195 }
00196 }
00197 }
00198 
00199 void moveit::core::RobotModel::computeCommonRoots()
00200 {
00201   // compute common roots for all pairs of joints;
00202   // there are 3 cases of pairs (X, Y):
00203   //    X != Y && X and Y are not descendants of one another
00204   //    X == Y
00205   //    X != Y && X and Y are descendants of one another
00206 
00207   // by default, the common root is always the global root;
00208   common_joint_roots_.resize(joint_model_vector_.size() * joint_model_vector_.size(), 0);
00209 
00210   // look at all descendants recursively; for two sibling nodes A, B, both children of X, all the pairs of respective
00211   // descendants of A and B
00212   // have X as the common root.
00213   computeCommonRootsHelper(root_joint_, common_joint_roots_, joint_model_vector_.size());
00214 
00215   for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
00216   {
00217     // the common root of a joint and itself is the same joint:
00218     common_joint_roots_[joint_model_vector_[i]->getJointIndex() * (1 + joint_model_vector_.size())] =
00219         joint_model_vector_[i]->getJointIndex();
00220 
00221     // a node N and one of its descendants have as common root the node N itself:
00222     const std::vector<const JointModel*>& d = joint_model_vector_[i]->getDescendantJointModels();
00223     for (std::size_t j = 0; j < d.size(); ++j)
00224       common_joint_roots_[d[j]->getJointIndex() * joint_model_vector_.size() +
00225                           joint_model_vector_[i]->getJointIndex()] =
00226           common_joint_roots_[d[j]->getJointIndex() +
00227                               joint_model_vector_[i]->getJointIndex() * joint_model_vector_.size()] =
00228               joint_model_vector_[i]->getJointIndex();
00229   }
00230 }
00231 
00232 void moveit::core::RobotModel::computeDescendants()
00233 {
00234   // compute the list of descendants for all joints
00235   std::vector<const JointModel*> parents;
00236   std::set<const JointModel*> seen;
00237 
00238   DescMap descendants;
00239   computeDescendantsHelper(root_joint_, parents, seen, descendants);
00240   for (DescMap::iterator it = descendants.begin(); it != descendants.end(); ++it)
00241   {
00242     JointModel* jm = const_cast<JointModel*>(it->first);
00243     for (std::set<const JointModel*>::const_iterator jt = it->second.second.begin(); jt != it->second.second.end();
00244          ++jt)
00245       jm->addDescendantJointModel(*jt);
00246     for (std::set<const LinkModel*>::const_iterator jt = it->second.first.begin(); jt != it->second.first.end(); ++jt)
00247       jm->addDescendantLinkModel(*jt);
00248   }
00249 }
00250 
00251 void moveit::core::RobotModel::buildJointInfo()
00252 {
00253   moveit::tools::Profiler::ScopedStart prof_start;
00254   moveit::tools::Profiler::ScopedBlock prof_block("RobotModel::buildJointInfo");
00255 
00256   // construct additional maps for easy access by name
00257   variable_count_ = 0;
00258   active_joint_model_start_index_.reserve(joint_model_vector_.size());
00259   variable_names_.reserve(joint_model_vector_.size());
00260   joints_of_variable_.reserve(joint_model_vector_.size());
00261 
00262   for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
00263   {
00264     joint_model_vector_[i]->setJointIndex(i);
00265     const std::vector<std::string>& name_order = joint_model_vector_[i]->getVariableNames();
00266 
00267     // compute index map
00268     if (!name_order.empty())
00269     {
00270       for (std::size_t j = 0; j < name_order.size(); ++j)
00271       {
00272         joint_variables_index_map_[name_order[j]] = variable_count_ + j;
00273         variable_names_.push_back(name_order[j]);
00274         joints_of_variable_.push_back(joint_model_vector_[i]);
00275       }
00276       if (joint_model_vector_[i]->getMimic() == NULL)
00277       {
00278         active_joint_model_start_index_.push_back(variable_count_);
00279         active_joint_model_vector_.push_back(joint_model_vector_[i]);
00280         active_joint_model_vector_const_.push_back(joint_model_vector_[i]);
00281         active_joint_models_bounds_.push_back(&joint_model_vector_[i]->getVariableBounds());
00282       }
00283 
00284       if (joint_model_vector_[i]->getType() == JointModel::REVOLUTE &&
00285           static_cast<const RevoluteJointModel*>(joint_model_vector_[i])->isContinuous())
00286         continuous_joint_model_vector_.push_back(joint_model_vector_[i]);
00287 
00288       joint_model_vector_[i]->setFirstVariableIndex(variable_count_);
00289       joint_variables_index_map_[joint_model_vector_[i]->getName()] = variable_count_;
00290 
00291       // compute variable count
00292       std::size_t vc = joint_model_vector_[i]->getVariableCount();
00293       variable_count_ += vc;
00294       if (vc == 1)
00295         single_dof_joints_.push_back(joint_model_vector_[i]);
00296       else
00297         multi_dof_joints_.push_back(joint_model_vector_[i]);
00298     }
00299   }
00300 
00301   for (std::size_t i = 0; i < link_model_vector_.size(); ++i)
00302   {
00303     LinkTransformMap associated_transforms;
00304     computeFixedTransforms(link_model_vector_[i], link_model_vector_[i]->getJointOriginTransform().inverse(),
00305                            associated_transforms);
00306     if (associated_transforms.size() > 1)
00307     {
00308       for (LinkTransformMap::iterator it = associated_transforms.begin(); it != associated_transforms.end(); ++it)
00309         if (it->first != link_model_vector_[i])
00310         {
00311           getLinkModel(it->first->getName())->addAssociatedFixedTransform(link_model_vector_[i], it->second.inverse());
00312           link_model_vector_[i]->addAssociatedFixedTransform(it->first, it->second);
00313         }
00314     }
00315   }
00316 
00317   computeDescendants();
00318   computeCommonRoots();  // must be called _after_ list of descendants was computed
00319 }
00320 
00321 void moveit::core::RobotModel::buildGroupStates(const srdf::Model& srdf_model)
00322 {
00323   // copy the default states to the groups
00324   const std::vector<srdf::Model::GroupState>& ds = srdf_model.getGroupStates();
00325   for (std::size_t i = 0; i < ds.size(); ++i)
00326   {
00327     if (hasJointModelGroup(ds[i].group_))
00328     {
00329       JointModelGroup* jmg = getJointModelGroup(ds[i].group_);
00330       std::map<std::string, double> state;
00331       for (std::map<std::string, std::vector<double> >::const_iterator jt = ds[i].joint_values_.begin();
00332            jt != ds[i].joint_values_.end(); ++jt)
00333       {
00334         if (jmg->hasJointModel(jt->first))
00335         {
00336           const JointModel* jm = jmg->getJointModel(jt->first);
00337           const std::vector<std::string>& vn = jm->getVariableNames();
00338           if (vn.size() == jt->second.size())
00339             for (std::size_t j = 0; j < vn.size(); ++j)
00340               state[vn[j]] = jt->second[j];
00341           else
00342             logError("The model for joint '%s' requires %d variable values, but only %d variable values were supplied "
00343                      "in default state '%s' for group '%s'",
00344                      jt->first.c_str(), (int)vn.size(), (int)jt->second.size(), ds[i].name_.c_str(),
00345                      jmg->getName().c_str());
00346         }
00347         else
00348           logError("Group state '%s' specifies value for joint '%s', but that joint is not part of group '%s'",
00349                    ds[i].name_.c_str(), jt->first.c_str(), jmg->getName().c_str());
00350       }
00351       if (!state.empty())
00352         jmg->addDefaultState(ds[i].name_, state);
00353     }
00354     else
00355       logError("Group state '%s' specified for group '%s', but that group does not exist", ds[i].name_.c_str(),
00356                ds[i].group_.c_str());
00357   }
00358 }
00359 
00360 void moveit::core::RobotModel::buildMimic(const urdf::ModelInterface& urdf_model)
00361 {
00362   // compute mimic joints
00363   for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
00364   {
00365     const urdf::Joint* jm = urdf_model.getJoint(joint_model_vector_[i]->getName()).get();
00366     if (jm)
00367       if (jm->mimic)
00368       {
00369         JointModelMap::const_iterator jit = joint_model_map_.find(jm->mimic->joint_name);
00370         if (jit != joint_model_map_.end())
00371         {
00372           if (joint_model_vector_[i]->getVariableCount() == jit->second->getVariableCount())
00373             joint_model_vector_[i]->setMimic(jit->second, jm->mimic->multiplier, jm->mimic->offset);
00374           else
00375             logError("Join '%s' cannot mimic joint '%s' because they have different number of DOF",
00376                      joint_model_vector_[i]->getName().c_str(), jm->mimic->joint_name.c_str());
00377         }
00378         else
00379           logError("Joint '%s' cannot mimic unknown joint '%s'", joint_model_vector_[i]->getName().c_str(),
00380                    jm->mimic->joint_name.c_str());
00381       }
00382   }
00383 
00384   // in case we have a joint that mimics a joint that already mimics another joint, we can simplify things:
00385   bool change = true;
00386   while (change)
00387   {
00388     change = false;
00389     for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
00390       if (joint_model_vector_[i]->getMimic())
00391       {
00392         if (joint_model_vector_[i]->getMimic()->getMimic())
00393         {
00394           joint_model_vector_[i]->setMimic(
00395               joint_model_vector_[i]->getMimic()->getMimic(),
00396               joint_model_vector_[i]->getMimicFactor() * joint_model_vector_[i]->getMimic()->getMimicFactor(),
00397               joint_model_vector_[i]->getMimicOffset() +
00398                   joint_model_vector_[i]->getMimicFactor() * joint_model_vector_[i]->getMimic()->getMimicOffset());
00399           change = true;
00400         }
00401         if (joint_model_vector_[i] == joint_model_vector_[i]->getMimic())
00402         {
00403           logError("Cycle found in joint that mimic each other. Ignoring all mimic joints.");
00404           for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
00405             joint_model_vector_[i]->setMimic(NULL, 0.0, 0.0);
00406           change = false;
00407           break;
00408         }
00409       }
00410   }
00411   // build mimic requests
00412   for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
00413     if (joint_model_vector_[i]->getMimic())
00414     {
00415       const_cast<JointModel*>(joint_model_vector_[i]->getMimic())->addMimicRequest(joint_model_vector_[i]);
00416       mimic_joints_.push_back(joint_model_vector_[i]);
00417     }
00418 }
00419 
00420 bool moveit::core::RobotModel::hasEndEffector(const std::string& eef) const
00421 {
00422   return end_effectors_map_.find(eef) != end_effectors_map_.end();
00423 }
00424 
00425 const moveit::core::JointModelGroup* moveit::core::RobotModel::getEndEffector(const std::string& name) const
00426 {
00427   JointModelGroupMap::const_iterator it = end_effectors_map_.find(name);
00428   if (it == end_effectors_map_.end())
00429   {
00430     it = joint_model_group_map_.find(name);
00431     if (it != joint_model_group_map_.end() && it->second->isEndEffector())
00432       return it->second;
00433     logError("End-effector '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
00434     return NULL;
00435   }
00436   return it->second;
00437 }
00438 
00439 moveit::core::JointModelGroup* moveit::core::RobotModel::getEndEffector(const std::string& name)
00440 {
00441   JointModelGroupMap::const_iterator it = end_effectors_map_.find(name);
00442   if (it == end_effectors_map_.end())
00443   {
00444     it = joint_model_group_map_.find(name);
00445     if (it != joint_model_group_map_.end() && it->second->isEndEffector())
00446       return it->second;
00447     logError("End-effector '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
00448     return NULL;
00449   }
00450   return it->second;
00451 }
00452 
00453 bool moveit::core::RobotModel::hasJointModelGroup(const std::string& name) const
00454 {
00455   return joint_model_group_map_.find(name) != joint_model_group_map_.end();
00456 }
00457 
00458 const moveit::core::JointModelGroup* moveit::core::RobotModel::getJointModelGroup(const std::string& name) const
00459 {
00460   JointModelGroupMap::const_iterator it = joint_model_group_map_.find(name);
00461   if (it == joint_model_group_map_.end())
00462   {
00463     logError("Group '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
00464     return NULL;
00465   }
00466   return it->second;
00467 }
00468 
00469 moveit::core::JointModelGroup* moveit::core::RobotModel::getJointModelGroup(const std::string& name)
00470 {
00471   JointModelGroupMap::const_iterator it = joint_model_group_map_.find(name);
00472   if (it == joint_model_group_map_.end())
00473   {
00474     logError("Group '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
00475     return NULL;
00476   }
00477   return it->second;
00478 }
00479 
00480 void moveit::core::RobotModel::buildGroups(const srdf::Model& srdf_model)
00481 {
00482   const std::vector<srdf::Model::Group>& group_configs = srdf_model.getGroups();
00483 
00484   // the only thing tricky is dealing with subgroups
00485   std::vector<bool> processed(group_configs.size(), false);
00486 
00487   bool added = true;
00488   while (added)
00489   {
00490     added = false;
00491 
00492     // going to make passes until we can't do anything else
00493     for (std::size_t i = 0; i < group_configs.size(); ++i)
00494       if (!processed[i])
00495       {
00496         // if we haven't processed, check and see if the dependencies are met yet
00497         bool all_subgroups_added = true;
00498         for (std::size_t j = 0; j < group_configs[i].subgroups_.size(); ++j)
00499           if (joint_model_group_map_.find(group_configs[i].subgroups_[j]) == joint_model_group_map_.end())
00500           {
00501             all_subgroups_added = false;
00502             break;
00503           }
00504         if (all_subgroups_added)
00505         {
00506           added = true;
00507           processed[i] = true;
00508           if (!addJointModelGroup(group_configs[i]))
00509             logWarn("Failed to add group '%s'", group_configs[i].name_.c_str());
00510         }
00511       }
00512   }
00513 
00514   for (std::size_t i = 0; i < processed.size(); ++i)
00515     if (!processed[i])
00516       logWarn("Could not process group '%s' due to unmet subgroup dependencies", group_configs[i].name_.c_str());
00517 
00518   for (JointModelGroupMap::const_iterator it = joint_model_group_map_.begin(); it != joint_model_group_map_.end(); ++it)
00519     joint_model_groups_.push_back(it->second);
00520   std::sort(joint_model_groups_.begin(), joint_model_groups_.end(), OrderGroupsByName());
00521   for (std::size_t i = 0; i < joint_model_groups_.size(); ++i)
00522   {
00523     joint_model_groups_const_.push_back(joint_model_groups_[i]);
00524     joint_model_group_names_.push_back(joint_model_groups_[i]->getName());
00525   }
00526 
00527   buildGroupsInfo_Subgroups(srdf_model);
00528   buildGroupsInfo_EndEffectors(srdf_model);
00529 }
00530 
00531 void moveit::core::RobotModel::buildGroupsInfo_Subgroups(const srdf::Model& srdf_model)
00532 {
00533   // compute subgroups
00534   for (JointModelGroupMap::const_iterator it = joint_model_group_map_.begin(); it != joint_model_group_map_.end(); ++it)
00535   {
00536     JointModelGroup* jmg = it->second;
00537     std::vector<std::string> subgroup_names;
00538     std::set<const JointModel*> joints(jmg->getJointModels().begin(), jmg->getJointModels().end());
00539     for (JointModelGroupMap::const_iterator jt = joint_model_group_map_.begin(); jt != joint_model_group_map_.end();
00540          ++jt)
00541       if (jt->first != it->first)
00542       {
00543         bool ok = true;
00544         JointModelGroup* sub_jmg = jt->second;
00545         const std::vector<const JointModel*>& sub_joints = sub_jmg->getJointModels();
00546         for (std::size_t k = 0; k < sub_joints.size(); ++k)
00547           if (joints.find(sub_joints[k]) == joints.end())
00548           {
00549             ok = false;
00550             break;
00551           }
00552         if (ok)
00553           subgroup_names.push_back(sub_jmg->getName());
00554       }
00555     if (!subgroup_names.empty())
00556       jmg->setSubgroupNames(subgroup_names);
00557   }
00558 }
00559 
00560 void moveit::core::RobotModel::buildGroupsInfo_EndEffectors(const srdf::Model& srdf_model)
00561 {
00562   // set the end-effector flags
00563   const std::vector<srdf::Model::EndEffector>& eefs = srdf_model.getEndEffectors();
00564   for (JointModelGroupMap::const_iterator it = joint_model_group_map_.begin(); it != joint_model_group_map_.end(); ++it)
00565   {
00566     // check if this group is a known end effector
00567     for (std::size_t k = 0; k < eefs.size(); ++k)
00568       if (eefs[k].component_group_ == it->first)
00569       {
00570         // if it is, mark it as such
00571         it->second->setEndEffectorName(eefs[k].name_);
00572         end_effectors_map_[eefs[k].name_] = it->second;
00573         end_effectors_.push_back(it->second);
00574 
00575         // check to see if there are groups that contain the parent link of this end effector.
00576         // record this information if found;
00577         std::vector<JointModelGroup*> possible_parent_groups;
00578         for (JointModelGroupMap::const_iterator jt = joint_model_group_map_.begin(); jt != joint_model_group_map_.end();
00579              ++jt)
00580           if (jt->first != it->first)
00581           {
00582             if (jt->second->hasLinkModel(eefs[k].parent_link_))
00583             {
00584               jt->second->attachEndEffector(eefs[k].name_);
00585               possible_parent_groups.push_back(jt->second);
00586             }
00587           }
00588 
00589         JointModelGroup* eef_parent_group = NULL;
00590         // if a parent group is specified in SRDF, try to use it
00591         if (!eefs[k].parent_group_.empty())
00592         {
00593           JointModelGroupMap::const_iterator jt = joint_model_group_map_.find(eefs[k].parent_group_);
00594           if (jt != joint_model_group_map_.end())
00595           {
00596             if (jt->second->hasLinkModel(eefs[k].parent_link_))
00597             {
00598               if (jt->second != it->second)
00599                 eef_parent_group = jt->second;
00600               else
00601                 logError("Group '%s' for end-effector '%s' cannot be its own parent", eefs[k].parent_group_.c_str(),
00602                          eefs[k].name_.c_str());
00603             }
00604             else
00605               logError("Group '%s' was specified as parent group for end-effector '%s' but it does not include the "
00606                        "parent link '%s'",
00607                        eefs[k].parent_group_.c_str(), eefs[k].name_.c_str(), eefs[k].parent_link_.c_str());
00608           }
00609           else
00610             logError("Group name '%s' not found (specified as parent group for end-effector '%s')",
00611                      eefs[k].parent_group_.c_str(), eefs[k].name_.c_str());
00612         }
00613 
00614         // if no parent group was specified, use a default one
00615         if (eef_parent_group == NULL)
00616           if (!possible_parent_groups.empty())
00617           {
00618             // if there are multiple options for the group that contains this end-effector,
00619             // we pick the group with fewest joints.
00620             std::size_t best = 0;
00621             for (std::size_t g = 1; g < possible_parent_groups.size(); ++g)
00622               if (possible_parent_groups[g]->getJointModels().size() <
00623                   possible_parent_groups[best]->getJointModels().size())
00624                 best = g;
00625             eef_parent_group = possible_parent_groups[best];
00626           }
00627 
00628         if (eef_parent_group)
00629         {
00630           it->second->setEndEffectorParent(eef_parent_group->getName(), eefs[k].parent_link_);
00631         }
00632         else
00633         {
00634           logWarn("Could not identify parent group for end-effector '%s'", eefs[k].name_.c_str());
00635           it->second->setEndEffectorParent("", eefs[k].parent_link_);
00636         }
00637         break;
00638       }
00639   }
00640   std::sort(end_effectors_.begin(), end_effectors_.end(), OrderGroupsByName());
00641 }
00642 
00643 bool moveit::core::RobotModel::addJointModelGroup(const srdf::Model::Group& gc)
00644 {
00645   if (joint_model_group_map_.find(gc.name_) != joint_model_group_map_.end())
00646   {
00647     logWarn("A group named '%s' already exists. Not adding.", gc.name_.c_str());
00648     return false;
00649   }
00650 
00651   std::set<const JointModel*> jset;
00652 
00653   // add joints from chains
00654   for (std::size_t i = 0; i < gc.chains_.size(); ++i)
00655   {
00656     const LinkModel* base_link = getLinkModel(gc.chains_[i].first);
00657     const LinkModel* tip_link = getLinkModel(gc.chains_[i].second);
00658     if (base_link && tip_link)
00659     {
00660       // go from tip, up the chain, until we hit the root or we find the base_link
00661       const LinkModel* lm = tip_link;
00662       std::vector<const JointModel*> cj;
00663       while (lm)
00664       {
00665         if (lm == base_link)
00666           break;
00667         cj.push_back(lm->getParentJointModel());
00668         lm = lm->getParentJointModel()->getParentLinkModel();
00669       }
00670       // if we did not find the base_link, we could have a chain like e.g.,
00671       // from one end-effector to another end-effector, so the root is in between
00672       if (lm != base_link)
00673       {
00674         // we go up the chain from the base this time, and see where we intersect the other chain
00675         lm = base_link;
00676         std::size_t index = 0;
00677         std::vector<const JointModel*> cj2;
00678         while (lm)
00679         {
00680           for (std::size_t j = 0; j < cj.size(); ++j)
00681             if (cj[j] == lm->getParentJointModel())
00682             {
00683               index = j + 1;
00684               break;
00685             }
00686           if (index > 0)
00687             break;
00688           cj2.push_back(lm->getParentJointModel());
00689           lm = lm->getParentJointModel()->getParentLinkModel();
00690         }
00691         if (index > 0)
00692         {
00693           jset.insert(cj.begin(), cj.begin() + index);
00694           jset.insert(cj2.begin(), cj2.end());
00695         }
00696       }
00697       else
00698         // if we have a simple chain, just add the joints
00699         jset.insert(cj.begin(), cj.end());
00700     }
00701   }
00702 
00703   // add joints
00704   for (std::size_t i = 0; i < gc.joints_.size(); ++i)
00705   {
00706     const JointModel* j = getJointModel(gc.joints_[i]);
00707     if (j)
00708       jset.insert(j);
00709   }
00710 
00711   // add joints that are parents of included links
00712   for (std::size_t i = 0; i < gc.links_.size(); ++i)
00713   {
00714     const LinkModel* l = getLinkModel(gc.links_[i]);
00715     if (l)
00716       jset.insert(l->getParentJointModel());
00717   }
00718 
00719   // add joints from subgroups
00720   for (std::size_t i = 0; i < gc.subgroups_.size(); ++i)
00721   {
00722     const JointModelGroup* sg = getJointModelGroup(gc.subgroups_[i]);
00723     if (sg)
00724     {
00725       // active joints
00726       const std::vector<const JointModel*>& js = sg->getJointModels();
00727       for (std::size_t j = 0; j < js.size(); ++j)
00728         jset.insert(js[j]);
00729 
00730       // fixed joints
00731       const std::vector<const JointModel*>& fs = sg->getFixedJointModels();
00732       for (std::size_t j = 0; j < fs.size(); ++j)
00733         jset.insert(fs[j]);
00734 
00735       // mimic joints
00736       const std::vector<const JointModel*>& ms = sg->getMimicJointModels();
00737       for (std::size_t j = 0; j < ms.size(); ++j)
00738         jset.insert(ms[j]);
00739     }
00740   }
00741 
00742   if (jset.empty())
00743   {
00744     logWarn("Group '%s' must have at least one valid joint", gc.name_.c_str());
00745     return false;
00746   }
00747 
00748   std::vector<const JointModel*> joints;
00749   for (std::set<const JointModel*>::iterator it = jset.begin(); it != jset.end(); ++it)
00750     joints.push_back(*it);
00751 
00752   JointModelGroup* jmg = new JointModelGroup(gc.name_, gc, joints, this);
00753   joint_model_group_map_[gc.name_] = jmg;
00754 
00755   return true;
00756 }
00757 
00758 moveit::core::JointModel* moveit::core::RobotModel::buildRecursive(LinkModel* parent, const urdf::Link* urdf_link,
00759                                                                    const srdf::Model& srdf_model)
00760 {
00761   // construct the joint
00762   JointModel* joint = urdf_link->parent_joint ?
00763                           constructJointModel(urdf_link->parent_joint.get(), urdf_link, srdf_model) :
00764                           constructJointModel(NULL, urdf_link, srdf_model);
00765   if (joint == NULL)
00766     return NULL;
00767 
00768   // bookkeeping for the joint
00769   joint_model_map_[joint->getName()] = joint;
00770   joint->setJointIndex(joint_model_vector_.size());
00771   joint_model_vector_.push_back(joint);
00772   joint_model_vector_const_.push_back(joint);
00773   joint_model_names_vector_.push_back(joint->getName());
00774   joint->setParentLinkModel(parent);
00775 
00776   // construct the link
00777   LinkModel* link = constructLinkModel(urdf_link);
00778   joint->setChildLinkModel(link);
00779   link->setParentLinkModel(parent);
00780 
00781   // bookkeeping for the link
00782   link_model_map_[joint->getChildLinkModel()->getName()] = link;
00783   link->setLinkIndex(link_model_vector_.size());
00784   link_model_vector_.push_back(link);
00785   link_model_vector_const_.push_back(link);
00786   link_model_names_vector_.push_back(link->getName());
00787   if (!link->getShapes().empty())
00788   {
00789     link_models_with_collision_geometry_vector_.push_back(link);
00790     link_model_names_with_collision_geometry_vector_.push_back(link->getName());
00791     link->setFirstCollisionBodyTransformIndex(link_geometry_count_);
00792     link_geometry_count_ += link->getShapes().size();
00793   }
00794   link->setParentJointModel(joint);
00795 
00796   // recursively build child links (and joints)
00797   for (std::size_t i = 0; i < urdf_link->child_links.size(); ++i)
00798   {
00799     JointModel* jm = buildRecursive(link, urdf_link->child_links[i].get(), srdf_model);
00800     if (jm)
00801       link->addChildJointModel(jm);
00802   }
00803   return joint;
00804 }
00805 
00806 namespace
00807 {
00808 // construct bounds for 1DOF joint
00809 static inline moveit::core::VariableBounds jointBoundsFromURDF(const urdf::Joint* urdf_joint)
00810 {
00811   moveit::core::VariableBounds b;
00812   if (urdf_joint->safety)
00813   {
00814     b.position_bounded_ = true;
00815     b.min_position_ = urdf_joint->safety->soft_lower_limit;
00816     b.max_position_ = urdf_joint->safety->soft_upper_limit;
00817     if (urdf_joint->limits)
00818     {
00819       if (urdf_joint->limits->lower > b.min_position_)
00820         b.min_position_ = urdf_joint->limits->lower;
00821       if (urdf_joint->limits->upper < b.max_position_)
00822         b.max_position_ = urdf_joint->limits->upper;
00823     }
00824   }
00825   else
00826   {
00827     if (urdf_joint->limits)
00828     {
00829       b.position_bounded_ = true;
00830       b.min_position_ = urdf_joint->limits->lower;
00831       b.max_position_ = urdf_joint->limits->upper;
00832     }
00833   }
00834   if (urdf_joint->limits)
00835   {
00836     b.max_velocity_ = fabs(urdf_joint->limits->velocity);
00837     b.min_velocity_ = -b.max_velocity_;
00838     b.velocity_bounded_ = b.max_velocity_ > std::numeric_limits<double>::epsilon();
00839   }
00840   return b;
00841 }
00842 }
00843 
00844 moveit::core::JointModel* moveit::core::RobotModel::constructJointModel(const urdf::Joint* urdf_joint,
00845                                                                         const urdf::Link* child_link,
00846                                                                         const srdf::Model& srdf_model)
00847 {
00848   JointModel* result = NULL;
00849 
00850   // must be the root link transform
00851   if (urdf_joint)
00852   {
00853     switch (urdf_joint->type)
00854     {
00855       case urdf::Joint::REVOLUTE:
00856       {
00857         RevoluteJointModel* j = new RevoluteJointModel(urdf_joint->name);
00858         j->setVariableBounds(j->getName(), jointBoundsFromURDF(urdf_joint));
00859         j->setContinuous(false);
00860         j->setAxis(Eigen::Vector3d(urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z));
00861         result = j;
00862       }
00863       break;
00864       case urdf::Joint::CONTINUOUS:
00865       {
00866         RevoluteJointModel* j = new RevoluteJointModel(urdf_joint->name);
00867         j->setVariableBounds(j->getName(), jointBoundsFromURDF(urdf_joint));
00868         j->setContinuous(true);
00869         j->setAxis(Eigen::Vector3d(urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z));
00870         result = j;
00871       }
00872       break;
00873       case urdf::Joint::PRISMATIC:
00874       {
00875         PrismaticJointModel* j = new PrismaticJointModel(urdf_joint->name);
00876         j->setVariableBounds(j->getName(), jointBoundsFromURDF(urdf_joint));
00877         j->setAxis(Eigen::Vector3d(urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z));
00878         result = j;
00879       }
00880       break;
00881       case urdf::Joint::FLOATING:
00882         result = new FloatingJointModel(urdf_joint->name);
00883         break;
00884       case urdf::Joint::PLANAR:
00885         result = new PlanarJointModel(urdf_joint->name);
00886         break;
00887       case urdf::Joint::FIXED:
00888         result = new FixedJointModel(urdf_joint->name);
00889         break;
00890       default:
00891         logError("Unknown joint type: %d", (int)urdf_joint->type);
00892         break;
00893     }
00894   }
00895   else
00896   {
00897     const std::vector<srdf::Model::VirtualJoint>& vjoints = srdf_model.getVirtualJoints();
00898     for (std::size_t i = 0; i < vjoints.size(); ++i)
00899     {
00900       if (vjoints[i].child_link_ != child_link->name)
00901       {
00902         logWarn("Skipping virtual joint '%s' because its child frame '%s' does not match the URDF frame '%s'",
00903                 vjoints[i].name_.c_str(), vjoints[i].child_link_.c_str(), child_link->name.c_str());
00904       }
00905       else if (vjoints[i].parent_frame_.empty())
00906       {
00907         logWarn("Skipping virtual joint '%s' because its parent frame is empty", vjoints[i].name_.c_str());
00908       }
00909       else
00910       {
00911         if (vjoints[i].type_ == "fixed")
00912           result = new FixedJointModel(vjoints[i].name_);
00913         else if (vjoints[i].type_ == "planar")
00914           result = new PlanarJointModel(vjoints[i].name_);
00915         else if (vjoints[i].type_ == "floating")
00916           result = new FloatingJointModel(vjoints[i].name_);
00917         if (result)
00918         {
00919           // for fixed frames we still use the robot root link
00920           if (vjoints[i].type_ != "fixed")
00921           {
00922             model_frame_ = vjoints[i].parent_frame_;
00923             if (model_frame_[0] != '/')
00924               model_frame_ = '/' + model_frame_;
00925           }
00926           break;
00927         }
00928       }
00929     }
00930     if (!result)
00931     {
00932       logInform("No root/virtual joint specified in SRDF. Assuming fixed joint");
00933       result = new FixedJointModel("ASSUMED_FIXED_ROOT_JOINT");
00934     }
00935   }
00936 
00937   if (result)
00938   {
00939     result->setDistanceFactor(result->getStateSpaceDimension());
00940     const std::vector<srdf::Model::PassiveJoint>& pjoints = srdf_model.getPassiveJoints();
00941     for (std::size_t i = 0; i < pjoints.size(); ++i)
00942     {
00943       if (result->getName() == pjoints[i].name_)
00944       {
00945         result->setPassive(true);
00946         break;
00947       }
00948     }
00949   }
00950 
00951   return result;
00952 }
00953 
00954 namespace
00955 {
00956 static inline Eigen::Affine3d urdfPose2Affine3d(const urdf::Pose& pose)
00957 {
00958   Eigen::Quaterniond q(pose.rotation.w, pose.rotation.x, pose.rotation.y, pose.rotation.z);
00959   Eigen::Affine3d af(Eigen::Translation3d(pose.position.x, pose.position.y, pose.position.z) * q.toRotationMatrix());
00960   return af;
00961 }
00962 }
00963 
00964 moveit::core::LinkModel* moveit::core::RobotModel::constructLinkModel(const urdf::Link* urdf_link)
00965 {
00966   LinkModel* result = new LinkModel(urdf_link->name);
00967 
00968   const std::vector<boost::shared_ptr<urdf::Collision> >& col_array =
00969       urdf_link->collision_array.empty() ? std::vector<boost::shared_ptr<urdf::Collision> >(1, urdf_link->collision) :
00970                                            urdf_link->collision_array;
00971 
00972   std::vector<shapes::ShapeConstPtr> shapes;
00973   EigenSTL::vector_Affine3d poses;
00974 
00975   for (std::size_t i = 0; i < col_array.size(); ++i)
00976     if (col_array[i] && col_array[i]->geometry)
00977     {
00978       shapes::ShapeConstPtr s = constructShape(col_array[i]->geometry.get());
00979       if (s)
00980       {
00981         shapes.push_back(s);
00982         poses.push_back(urdfPose2Affine3d(col_array[i]->origin));
00983       }
00984     }
00985   if (shapes.empty())
00986   {
00987     const std::vector<boost::shared_ptr<urdf::Visual> >& vis_array =
00988         urdf_link->visual_array.empty() ? std::vector<boost::shared_ptr<urdf::Visual> >(1, urdf_link->visual) :
00989                                           urdf_link->visual_array;
00990     for (std::size_t i = 0; i < vis_array.size(); ++i)
00991       if (vis_array[i] && vis_array[i]->geometry)
00992       {
00993         shapes::ShapeConstPtr s = constructShape(vis_array[i]->geometry.get());
00994         if (s)
00995         {
00996           shapes.push_back(s);
00997           poses.push_back(urdfPose2Affine3d(vis_array[i]->origin));
00998         }
00999       }
01000   }
01001 
01002   result->setGeometry(shapes, poses);
01003 
01004   // figure out visual mesh (try visual urdf tag first, collision tag otherwise
01005   if (urdf_link->visual && urdf_link->visual->geometry)
01006   {
01007     if (urdf_link->visual->geometry->type == urdf::Geometry::MESH)
01008     {
01009       const urdf::Mesh* mesh = static_cast<const urdf::Mesh*>(urdf_link->visual->geometry.get());
01010       if (!mesh->filename.empty())
01011         result->setVisualMesh(mesh->filename, urdfPose2Affine3d(urdf_link->visual->origin),
01012                               Eigen::Vector3d(mesh->scale.x, mesh->scale.y, mesh->scale.z));
01013     }
01014   }
01015   else if (urdf_link->collision && urdf_link->collision->geometry)
01016   {
01017     if (urdf_link->collision->geometry->type == urdf::Geometry::MESH)
01018     {
01019       const urdf::Mesh* mesh = static_cast<const urdf::Mesh*>(urdf_link->collision->geometry.get());
01020       if (!mesh->filename.empty())
01021         result->setVisualMesh(mesh->filename, urdfPose2Affine3d(urdf_link->collision->origin),
01022                               Eigen::Vector3d(mesh->scale.x, mesh->scale.y, mesh->scale.z));
01023     }
01024   }
01025 
01026   if (urdf_link->parent_joint)
01027     result->setJointOriginTransform(urdfPose2Affine3d(urdf_link->parent_joint->parent_to_joint_origin_transform));
01028 
01029   return result;
01030 }
01031 
01032 shapes::ShapePtr moveit::core::RobotModel::constructShape(const urdf::Geometry* geom)
01033 {
01034   moveit::tools::Profiler::ScopedBlock prof_block("RobotModel::constructShape");
01035 
01036   shapes::Shape* result = NULL;
01037   switch (geom->type)
01038   {
01039     case urdf::Geometry::SPHERE:
01040       result = new shapes::Sphere(static_cast<const urdf::Sphere*>(geom)->radius);
01041       break;
01042     case urdf::Geometry::BOX:
01043     {
01044       urdf::Vector3 dim = static_cast<const urdf::Box*>(geom)->dim;
01045       result = new shapes::Box(dim.x, dim.y, dim.z);
01046     }
01047     break;
01048     case urdf::Geometry::CYLINDER:
01049       result = new shapes::Cylinder(static_cast<const urdf::Cylinder*>(geom)->radius,
01050                                     static_cast<const urdf::Cylinder*>(geom)->length);
01051       break;
01052     case urdf::Geometry::MESH:
01053     {
01054       const urdf::Mesh* mesh = static_cast<const urdf::Mesh*>(geom);
01055       if (!mesh->filename.empty())
01056       {
01057         Eigen::Vector3d scale(mesh->scale.x, mesh->scale.y, mesh->scale.z);
01058         shapes::Mesh* m = shapes::createMeshFromResource(mesh->filename, scale);
01059         result = m;
01060       }
01061     }
01062     break;
01063     default:
01064       logError("Unknown geometry type: %d", (int)geom->type);
01065       break;
01066   }
01067 
01068   return shapes::ShapePtr(result);
01069 }
01070 
01071 bool moveit::core::RobotModel::hasJointModel(const std::string& name) const
01072 {
01073   return joint_model_map_.find(name) != joint_model_map_.end();
01074 }
01075 
01076 bool moveit::core::RobotModel::hasLinkModel(const std::string& name) const
01077 {
01078   return link_model_map_.find(name) != link_model_map_.end();
01079 }
01080 
01081 const moveit::core::JointModel* moveit::core::RobotModel::getJointModel(const std::string& name) const
01082 {
01083   JointModelMap::const_iterator it = joint_model_map_.find(name);
01084   if (it != joint_model_map_.end())
01085     return it->second;
01086   logError("Joint '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
01087   return NULL;
01088 }
01089 
01090 const moveit::core::JointModel* moveit::core::RobotModel::getJointModel(int index) const
01091 {
01092   if (index < 0 || index >= static_cast<int>(joint_model_vector_.size()))
01093   {
01094     logError("Joint index '%i' out of bounds of joints in model '%s'", index, model_name_.c_str());
01095     return NULL;
01096   }
01097   assert(joint_model_vector_[index]->getJointIndex() == index);
01098   return joint_model_vector_[index];
01099 }
01100 
01101 moveit::core::JointModel* moveit::core::RobotModel::getJointModel(const std::string& name)
01102 {
01103   JointModelMap::const_iterator it = joint_model_map_.find(name);
01104   if (it != joint_model_map_.end())
01105     return it->second;
01106   logError("Joint '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
01107   return NULL;
01108 }
01109 
01110 const moveit::core::LinkModel* moveit::core::RobotModel::getLinkModel(const std::string& name) const
01111 {
01112   LinkModelMap::const_iterator it = link_model_map_.find(name);
01113   if (it != link_model_map_.end())
01114     return it->second;
01115   logError("Link '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
01116   return NULL;
01117 }
01118 
01119 const moveit::core::LinkModel* moveit::core::RobotModel::getLinkModel(int index) const
01120 {
01121   if (index < 0 || index >= static_cast<int>(link_model_vector_.size()))
01122   {
01123     logError("Link index '%i' out of bounds of links in model '%s'", index, model_name_.c_str());
01124     return NULL;
01125   }
01126   assert(link_model_vector_[index]->getLinkIndex() == index);
01127   return link_model_vector_[index];
01128 }
01129 
01130 moveit::core::LinkModel* moveit::core::RobotModel::getLinkModel(const std::string& name)
01131 {
01132   LinkModelMap::const_iterator it = link_model_map_.find(name);
01133   if (it != link_model_map_.end())
01134     return it->second;
01135   logError("Link '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
01136   return NULL;
01137 }
01138 
01139 void moveit::core::RobotModel::updateMimicJoints(double* values) const
01140 {
01141   for (std::size_t i = 0; i < mimic_joints_.size(); ++i)
01142   {
01143     int src = mimic_joints_[i]->getMimic()->getFirstVariableIndex();
01144     int dest = mimic_joints_[i]->getFirstVariableIndex();
01145     values[dest] = values[src] * mimic_joints_[i]->getMimicFactor() + mimic_joints_[i]->getMimicOffset();
01146   }
01147 }
01148 
01149 void moveit::core::RobotModel::getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng,
01150                                                           double* values) const
01151 {
01152   for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
01153     active_joint_model_vector_[i]->getVariableRandomPositions(rng, values + active_joint_model_start_index_[i]);
01154   updateMimicJoints(values);
01155 }
01156 
01157 void moveit::core::RobotModel::getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng,
01158                                                           std::map<std::string, double>& values) const
01159 {
01160   std::vector<double> tmp(variable_count_);
01161   getVariableRandomPositions(rng, &tmp[0]);
01162   values.clear();
01163   for (std::size_t i = 0; i < variable_names_.size(); ++i)
01164     values[variable_names_[i]] = tmp[i];
01165 }
01166 
01167 void moveit::core::RobotModel::getVariableDefaultPositions(double* values) const
01168 {
01169   for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
01170     active_joint_model_vector_[i]->getVariableDefaultPositions(values + active_joint_model_start_index_[i]);
01171   updateMimicJoints(values);
01172 }
01173 
01174 void moveit::core::RobotModel::getVariableDefaultPositions(std::map<std::string, double>& values) const
01175 {
01176   std::vector<double> tmp(variable_count_);
01177   getVariableDefaultPositions(&tmp[0]);
01178   values.clear();
01179   for (std::size_t i = 0; i < variable_names_.size(); ++i)
01180     values[variable_names_[i]] = tmp[i];
01181 }
01182 
01183 void moveit::core::RobotModel::getMissingVariableNames(const std::vector<std::string>& variables,
01184                                                        std::vector<std::string>& missing_variables) const
01185 {
01186   missing_variables.clear();
01187   std::set<std::string> keys(variables.begin(), variables.end());
01188   for (std::size_t i = 0; i < variable_names_.size(); ++i)
01189     if (keys.find(variable_names_[i]) == keys.end())
01190       if (getJointOfVariable(variable_names_[i])->getMimic() == NULL)
01191         missing_variables.push_back(variable_names_[i]);
01192 }
01193 
01194 int moveit::core::RobotModel::getVariableIndex(const std::string& variable) const
01195 {
01196   VariableIndexMap::const_iterator it = joint_variables_index_map_.find(variable);
01197   if (it == joint_variables_index_map_.end())
01198     throw Exception("Variable '" + variable + "' is not known to model '" + model_name_ + "'");
01199   return it->second;
01200 }
01201 
01202 double moveit::core::RobotModel::getMaximumExtent(const JointBoundsVector& active_joint_bounds) const
01203 {
01204   double max_distance = 0.0;
01205   for (std::size_t j = 0; j < active_joint_model_vector_.size(); ++j)
01206     max_distance += active_joint_model_vector_[j]->getMaximumExtent(*active_joint_bounds[j]) *
01207                     active_joint_model_vector_[j]->getDistanceFactor();
01208   return max_distance;
01209 }
01210 
01211 bool moveit::core::RobotModel::satisfiesPositionBounds(const double* state,
01212                                                        const JointBoundsVector& active_joint_bounds,
01213                                                        double margin) const
01214 {
01215   assert(active_joint_bounds.size() == active_joint_model_vector_.size());
01216   for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
01217     if (!active_joint_model_vector_[i]->satisfiesPositionBounds(state + active_joint_model_start_index_[i],
01218                                                                 *active_joint_bounds[i], margin))
01219       return false;
01220   return true;
01221 }
01222 
01223 bool moveit::core::RobotModel::enforcePositionBounds(double* state, const JointBoundsVector& active_joint_bounds) const
01224 {
01225   assert(active_joint_bounds.size() == active_joint_model_vector_.size());
01226   bool change = false;
01227   for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
01228     if (active_joint_model_vector_[i]->enforcePositionBounds(state + active_joint_model_start_index_[i],
01229                                                              *active_joint_bounds[i]))
01230       change = true;
01231   if (change)
01232     updateMimicJoints(state);
01233   return change;
01234 }
01235 
01236 double moveit::core::RobotModel::distance(const double* state1, const double* state2) const
01237 {
01238   double d = 0.0;
01239   for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
01240     d += active_joint_model_vector_[i]->getDistanceFactor() *
01241          active_joint_model_vector_[i]->distance(state1 + active_joint_model_start_index_[i],
01242                                                  state2 + active_joint_model_start_index_[i]);
01243   return d;
01244 }
01245 
01246 void moveit::core::RobotModel::interpolate(const double* from, const double* to, double t, double* state) const
01247 {
01248   // we interpolate values only for active joint models (non-mimic)
01249   for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
01250     active_joint_model_vector_[i]->interpolate(from + active_joint_model_start_index_[i],
01251                                                to + active_joint_model_start_index_[i], t,
01252                                                state + active_joint_model_start_index_[i]);
01253   // now we update mimic as needed
01254   updateMimicJoints(state);
01255 }
01256 
01257 void moveit::core::RobotModel::setKinematicsAllocators(const std::map<std::string, SolverAllocatorFn>& allocators)
01258 {
01259   // we first set all the "simple" allocators -- where a group has one IK solver
01260   for (JointModelGroupMap::const_iterator it = joint_model_group_map_.begin(); it != joint_model_group_map_.end(); ++it)
01261   {
01262     std::map<std::string, SolverAllocatorFn>::const_iterator jt = allocators.find(it->second->getName());
01263     if (jt != allocators.end())
01264     {
01265       std::pair<SolverAllocatorFn, SolverAllocatorMapFn> result;
01266       result.first = jt->second;
01267       it->second->setSolverAllocators(result);
01268     }
01269   }
01270 
01271   // now we set compound IK solvers; we do this later because we need the index maps computed by the previous calls to
01272   // setSolverAllocators()
01273   for (JointModelGroupMap::const_iterator it = joint_model_group_map_.begin(); it != joint_model_group_map_.end(); ++it)
01274   {
01275     JointModelGroup* jmg = it->second;
01276     std::pair<SolverAllocatorFn, SolverAllocatorMapFn> result;
01277     std::map<std::string, SolverAllocatorFn>::const_iterator jt = allocators.find(jmg->getName());
01278     if (jt == allocators.end())
01279     {
01280       // if an kinematics allocator is NOT available for this group, we try to see if we can use subgroups for IK
01281       std::set<const JointModel*> joints;
01282       joints.insert(jmg->getJointModels().begin(), jmg->getJointModels().end());
01283 
01284       std::vector<const JointModelGroup*> subs;
01285 
01286       // go through the groups that we know have IK allocators and see if they are included in the group that does not;
01287       // if so, put that group in sub
01288       for (std::map<std::string, SolverAllocatorFn>::const_iterator kt = allocators.begin(); kt != allocators.end();
01289            ++kt)
01290       {
01291         const JointModelGroup* sub = getJointModelGroup(kt->first);
01292         if (!sub)
01293         {
01294           subs.clear();
01295           break;
01296         }
01297         std::set<const JointModel*> sub_joints;
01298         sub_joints.insert(sub->getJointModels().begin(), sub->getJointModels().end());
01299 
01300         if (std::includes(joints.begin(), joints.end(), sub_joints.begin(), sub_joints.end()))
01301         {
01302           std::set<const JointModel*> resultj;
01303           std::set_difference(joints.begin(), joints.end(), sub_joints.begin(), sub_joints.end(),
01304                               std::inserter(resultj, resultj.end()));
01305           subs.push_back(sub);
01306           joints.swap(resultj);
01307         }
01308       }
01309 
01310       // if we found subgroups, pass that information to the planning group
01311       if (!subs.empty())
01312       {
01313         std::stringstream ss;
01314         for (std::size_t i = 0; i < subs.size(); ++i)
01315         {
01316           ss << subs[i]->getName() << " ";
01317           result.second[subs[i]] = allocators.find(subs[i]->getName())->second;
01318         }
01319         logDebug("Added sub-group IK allocators for group '%s': [ %s]", jmg->getName().c_str(), ss.str().c_str());
01320       }
01321       jmg->setSolverAllocators(result);
01322     }
01323   }
01324 }
01325 
01326 void moveit::core::RobotModel::printModelInfo(std::ostream& out) const
01327 {
01328   out << "Model " << model_name_ << " in frame " << model_frame_ << ", using " << getVariableCount() << " variables"
01329       << std::endl;
01330 
01331   std::ios_base::fmtflags old_flags = out.flags();
01332   out.setf(std::ios::fixed, std::ios::floatfield);
01333   std::streamsize old_prec = out.precision();
01334   out.precision(5);
01335   out << "Joints: " << std::endl;
01336   for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
01337   {
01338     out << " '" << joint_model_vector_[i]->getName() << "' (" << joint_model_vector_[i]->getTypeName() << ")"
01339         << std::endl;
01340     out << "  * Joint Index: " << joint_model_vector_[i]->getJointIndex() << std::endl;
01341     const std::vector<std::string>& vn = joint_model_vector_[i]->getVariableNames();
01342     out << "  * " << vn.size() << (vn.size() > 1 ? " variables:" : (vn.empty() ? " variables" : " variable:"))
01343         << std::endl;
01344     int idx = joint_model_vector_[i]->getFirstVariableIndex();
01345     for (std::vector<std::string>::const_iterator it = vn.begin(); it != vn.end(); ++it)
01346     {
01347       out << "     * '" << *it << "', index " << idx++ << " in full state";
01348       if (joint_model_vector_[i]->getMimic())
01349         out << ", mimic '" << joint_model_vector_[i]->getMimic()->getName() << "'";
01350       if (joint_model_vector_[i]->isPassive())
01351         out << ", passive";
01352       out << std::endl;
01353       out << "        " << joint_model_vector_[i]->getVariableBounds(*it) << std::endl;
01354     }
01355   }
01356   out << std::endl;
01357   out.precision(old_prec);
01358   out.flags(old_flags);
01359   out << "Links: " << std::endl;
01360   for (std::size_t i = 0; i < link_model_vector_.size(); ++i)
01361   {
01362     out << " '" << link_model_vector_[i]->getName() << "' with " << link_model_vector_[i]->getShapes().size()
01363         << " geoms" << std::endl;
01364     if (link_model_vector_[i]->parentJointIsFixed())
01365       out << "   * "
01366           << "parent joint is fixed" << std::endl;
01367     if (link_model_vector_[i]->jointOriginTransformIsIdentity())
01368       out << "   * "
01369           << "joint origin transform is identity" << std::endl;
01370   }
01371 
01372   out << "Available groups: " << std::endl;
01373   for (std::size_t i = 0; i < joint_model_groups_.size(); ++i)
01374     joint_model_groups_[i]->printGroupInfo(out);
01375 }
01376 
01377 void moveit::core::RobotModel::computeFixedTransforms(const LinkModel* link, const Eigen::Affine3d& transform,
01378                                                       LinkTransformMap& associated_transforms)
01379 {
01380   associated_transforms[link] = transform * link->getJointOriginTransform();
01381   for (std::size_t i = 0; i < link->getChildJointModels().size(); ++i)
01382     if (link->getChildJointModels()[i]->getType() == JointModel::FIXED)
01383       computeFixedTransforms(link->getChildJointModels()[i]->getChildLinkModel(),
01384                              transform * link->getJointOriginTransform(), associated_transforms);
01385 }


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jan 17 2018 03:31:36