joint_model_group.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, Dave Coleman */
00037 
00038 #include <moveit/robot_model/robot_model.h>
00039 #include <moveit/robot_model/joint_model_group.h>
00040 #include <moveit/robot_model/revolute_joint_model.h>
00041 #include <moveit/exceptions/exceptions.h>
00042 #include <console_bridge/console.h>
00043 #include <boost/lexical_cast.hpp>
00044 #include <algorithm>
00045 #include "order_robot_model_items.inc"
00046 
00047 namespace moveit
00048 {
00049 namespace core
00050 {
00051 namespace
00052 {
00053 // check if a parent or ancestor of joint is included in this group
00054 bool includesParent(const JointModel* joint, const JointModelGroup* group)
00055 {
00056   bool found = false;
00057   // if we find that an ancestor is also in the group, then the joint is not a root
00058   while (joint->getParentLinkModel() != NULL)
00059   {
00060     joint = joint->getParentLinkModel()->getParentJointModel();
00061     if (group->hasJointModel(joint->getName()) && joint->getVariableCount() > 0 && joint->getMimic() == NULL)
00062     {
00063       found = true;
00064       break;
00065     }
00066     else if (joint->getMimic() != NULL)
00067     {
00068       const JointModel* mjoint = joint->getMimic();
00069       if (group->hasJointModel(mjoint->getName()) && mjoint->getVariableCount() > 0 && mjoint->getMimic() == NULL)
00070         found = true;
00071       else if (includesParent(mjoint, group))
00072         found = true;
00073       if (found)
00074         break;
00075     }
00076   }
00077   return found;
00078 }
00079 
00080 // check if joint a is right below b, in the kinematic chain, with no active DOF missing
00081 bool jointPrecedes(const JointModel* a, const JointModel* b)
00082 {
00083   if (!a->getParentLinkModel())
00084     return false;
00085   const JointModel* p = a->getParentLinkModel()->getParentJointModel();
00086   while (p)
00087   {
00088     if (p == b)
00089       return true;
00090     if (p->getType() == JointModel::FIXED)
00091       p = p->getParentLinkModel() ? p->getParentLinkModel()->getParentJointModel() : NULL;
00092     else
00093       break;
00094   }
00095 
00096   return false;
00097 }
00098 }
00099 }
00100 }
00101 
00102 moveit::core::JointModelGroup::JointModelGroup(const std::string& group_name, const srdf::Model::Group& config,
00103                                                const std::vector<const JointModel*>& unsorted_group_joints,
00104                                                const RobotModel* parent_model)
00105   : parent_model_(parent_model)
00106   , name_(group_name)
00107   , common_root_(NULL)
00108   , variable_count_(0)
00109   , is_contiguous_index_list_(true)
00110   , is_chain_(false)
00111   , is_single_dof_(true)
00112   , config_(config)
00113 {
00114   // sort joints in Depth-First order
00115   joint_model_vector_ = unsorted_group_joints;
00116   std::sort(joint_model_vector_.begin(), joint_model_vector_.end(), OrderJointsByIndex());
00117   joint_model_name_vector_.reserve(joint_model_vector_.size());
00118 
00119   // figure out active joints, mimic joints, fixed joints
00120   // construct index maps, list of variables
00121   for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
00122   {
00123     joint_model_name_vector_.push_back(joint_model_vector_[i]->getName());
00124     joint_model_map_[joint_model_vector_[i]->getName()] = joint_model_vector_[i];
00125     unsigned int vc = joint_model_vector_[i]->getVariableCount();
00126     if (vc > 0)
00127     {
00128       if (vc > 1)
00129         is_single_dof_ = false;
00130       const std::vector<std::string>& name_order = joint_model_vector_[i]->getVariableNames();
00131       if (joint_model_vector_[i]->getMimic() == NULL)
00132       {
00133         active_joint_model_vector_.push_back(joint_model_vector_[i]);
00134         active_joint_model_name_vector_.push_back(joint_model_vector_[i]->getName());
00135         active_joint_model_start_index_.push_back(variable_count_);
00136         active_joint_models_bounds_.push_back(&joint_model_vector_[i]->getVariableBounds());
00137       }
00138       else
00139         mimic_joints_.push_back(joint_model_vector_[i]);
00140       for (std::size_t j = 0; j < name_order.size(); ++j)
00141       {
00142         variable_names_.push_back(name_order[j]);
00143         variable_names_set_.insert(name_order[j]);
00144       }
00145 
00146       int first_index = joint_model_vector_[i]->getFirstVariableIndex();
00147       for (std::size_t j = 0; j < name_order.size(); ++j)
00148       {
00149         variable_index_list_.push_back(first_index + j);
00150         joint_variables_index_map_[name_order[j]] = variable_count_ + j;
00151       }
00152       joint_variables_index_map_[joint_model_vector_[i]->getName()] = variable_count_;
00153 
00154       if (joint_model_vector_[i]->getType() == JointModel::REVOLUTE &&
00155           static_cast<const RevoluteJointModel*>(joint_model_vector_[i])->isContinuous())
00156         continuous_joint_model_vector_.push_back(joint_model_vector_[i]);
00157 
00158       variable_count_ += vc;
00159     }
00160     else
00161       fixed_joints_.push_back(joint_model_vector_[i]);
00162   }
00163 
00164   // now we need to find all the set of joints within this group
00165   // that root distinct subtrees
00166   for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
00167   {
00168     // if we find that an ancestor is also in the group, then the joint is not a root
00169     if (!includesParent(active_joint_model_vector_[i], this))
00170       joint_roots_.push_back(active_joint_model_vector_[i]);
00171   }
00172 
00173   // when updating this group within a state, it is useful to know
00174   // if the full state of a group is contiguous within the full state of the robot
00175   if (variable_index_list_.empty())
00176     is_contiguous_index_list_ = false;
00177   else
00178     for (std::size_t i = 1; i < variable_index_list_.size(); ++i)
00179       if (variable_index_list_[i] != variable_index_list_[i - 1] + 1)
00180       {
00181         is_contiguous_index_list_ = false;
00182         break;
00183       }
00184 
00185   // when updating/sampling a group state only, only mimic joints that have their parent within the group get updated.
00186   for (std::size_t i = 0; i < mimic_joints_.size(); ++i)
00187     // if the joint we mimic is also in this group, we will need to do updates when sampling
00188     if (hasJointModel(mimic_joints_[i]->getMimic()->getName()))
00189     {
00190       int src = joint_variables_index_map_[mimic_joints_[i]->getMimic()->getName()];
00191       int dest = joint_variables_index_map_[mimic_joints_[i]->getName()];
00192       GroupMimicUpdate mu(src, dest, mimic_joints_[i]->getMimicFactor(), mimic_joints_[i]->getMimicOffset());
00193       group_mimic_update_.push_back(mu);
00194     }
00195 
00196   // now we need to make another pass for group links (we include the fixed joints here)
00197   std::set<const LinkModel*> group_links_set;
00198   for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
00199     group_links_set.insert(joint_model_vector_[i]->getChildLinkModel());
00200   for (std::set<const LinkModel*>::iterator it = group_links_set.begin(); it != group_links_set.end(); ++it)
00201     link_model_vector_.push_back(*it);
00202   std::sort(link_model_vector_.begin(), link_model_vector_.end(), OrderLinksByIndex());
00203 
00204   for (std::size_t i = 0; i < link_model_vector_.size(); ++i)
00205   {
00206     link_model_map_[link_model_vector_[i]->getName()] = link_model_vector_[i];
00207     link_model_name_vector_.push_back(link_model_vector_[i]->getName());
00208     if (!link_model_vector_[i]->getShapes().empty())
00209     {
00210       link_model_with_geometry_vector_.push_back(link_model_vector_[i]);
00211       link_model_with_geometry_name_vector_.push_back(link_model_vector_[i]->getName());
00212     }
00213   }
00214 
00215   // compute the common root of this group
00216   if (!joint_roots_.empty())
00217   {
00218     common_root_ = joint_roots_[0];
00219     for (std::size_t i = 1; i < joint_roots_.size(); ++i)
00220       common_root_ = parent_model->getCommonRoot(joint_roots_[i], common_root_);
00221   }
00222 
00223   // compute updated links
00224   for (std::size_t i = 0; i < joint_roots_.size(); ++i)
00225   {
00226     const std::vector<const LinkModel*>& links = joint_roots_[i]->getDescendantLinkModels();
00227     updated_link_model_set_.insert(links.begin(), links.end());
00228   }
00229   for (std::set<const LinkModel*>::iterator it = updated_link_model_set_.begin(); it != updated_link_model_set_.end();
00230        ++it)
00231   {
00232     updated_link_model_name_set_.insert((*it)->getName());
00233     updated_link_model_vector_.push_back(*it);
00234     if (!(*it)->getShapes().empty())
00235     {
00236       updated_link_model_with_geometry_vector_.push_back(*it);
00237       updated_link_model_with_geometry_set_.insert(*it);
00238       updated_link_model_with_geometry_name_set_.insert((*it)->getName());
00239     }
00240   }
00241   std::sort(updated_link_model_vector_.begin(), updated_link_model_vector_.end(), OrderLinksByIndex());
00242   std::sort(updated_link_model_with_geometry_vector_.begin(), updated_link_model_with_geometry_vector_.end(),
00243             OrderLinksByIndex());
00244   for (std::size_t i = 0; i < updated_link_model_vector_.size(); ++i)
00245     updated_link_model_name_vector_.push_back(updated_link_model_vector_[i]->getName());
00246   for (std::size_t i = 0; i < updated_link_model_with_geometry_vector_.size(); ++i)
00247     updated_link_model_with_geometry_name_vector_.push_back(updated_link_model_with_geometry_vector_[i]->getName());
00248 
00249   // check if this group should actually be a chain
00250   if (joint_roots_.size() == 1 && active_joint_model_vector_.size() > 1)
00251   {
00252     bool chain = true;
00253     // due to our sorting, the joints are sorted in a DF fashion, so looking at them in reverse,
00254     // we should always get to the parent.
00255     for (std::size_t k = joint_model_vector_.size() - 1; k > 0; --k)
00256       if (!jointPrecedes(joint_model_vector_[k], joint_model_vector_[k - 1]))
00257       {
00258         chain = false;
00259         break;
00260       }
00261     if (chain)
00262       is_chain_ = true;
00263   }
00264 }
00265 
00266 moveit::core::JointModelGroup::~JointModelGroup()
00267 {
00268 }
00269 
00270 void moveit::core::JointModelGroup::setSubgroupNames(const std::vector<std::string>& subgroups)
00271 {
00272   subgroup_names_ = subgroups;
00273   subgroup_names_set_.clear();
00274   for (std::size_t i = 0; i < subgroup_names_.size(); ++i)
00275     subgroup_names_set_.insert(subgroup_names_[i]);
00276 }
00277 
00278 void moveit::core::JointModelGroup::getSubgroups(std::vector<const JointModelGroup*>& sub_groups) const
00279 {
00280   sub_groups.resize(subgroup_names_.size());
00281   for (std::size_t i = 0; i < subgroup_names_.size(); ++i)
00282     sub_groups[i] = parent_model_->getJointModelGroup(subgroup_names_[i]);
00283 }
00284 
00285 bool moveit::core::JointModelGroup::hasJointModel(const std::string& joint) const
00286 {
00287   return joint_model_map_.find(joint) != joint_model_map_.end();
00288 }
00289 
00290 bool moveit::core::JointModelGroup::hasLinkModel(const std::string& link) const
00291 {
00292   return link_model_map_.find(link) != link_model_map_.end();
00293 }
00294 
00295 const moveit::core::LinkModel* moveit::core::JointModelGroup::getLinkModel(const std::string& name) const
00296 {
00297   LinkModelMapConst::const_iterator it = link_model_map_.find(name);
00298   if (it == link_model_map_.end())
00299   {
00300     logError("Link '%s' not found in group '%s'", name.c_str(), name_.c_str());
00301     return NULL;
00302   }
00303   return it->second;
00304 }
00305 
00306 const moveit::core::JointModel* moveit::core::JointModelGroup::getJointModel(const std::string& name) const
00307 {
00308   JointModelMapConst::const_iterator it = joint_model_map_.find(name);
00309   if (it == joint_model_map_.end())
00310   {
00311     logError("Joint '%s' not found in group '%s'", name.c_str(), name_.c_str());
00312     return NULL;
00313   }
00314   return it->second;
00315 }
00316 
00317 void moveit::core::JointModelGroup::getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng,
00318                                                                double* values,
00319                                                                const JointBoundsVector& active_joint_bounds) const
00320 {
00321   assert(active_joint_bounds.size() == active_joint_model_vector_.size());
00322   for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
00323     active_joint_model_vector_[i]->getVariableRandomPositions(rng, values + active_joint_model_start_index_[i],
00324                                                               *active_joint_bounds[i]);
00325 
00326   updateMimicJoints(values);
00327 }
00328 
00329 void moveit::core::JointModelGroup::getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng,
00330                                                                      double* values,
00331                                                                      const JointBoundsVector& active_joint_bounds,
00332                                                                      const double* near, double distance) const
00333 {
00334   assert(active_joint_bounds.size() == active_joint_model_vector_.size());
00335   for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
00336     active_joint_model_vector_[i]->getVariableRandomPositionsNearBy(
00337         rng, values + active_joint_model_start_index_[i], *active_joint_bounds[i],
00338         near + active_joint_model_start_index_[i], distance);
00339   updateMimicJoints(values);
00340 }
00341 
00342 void moveit::core::JointModelGroup::getVariableRandomPositionsNearBy(
00343     random_numbers::RandomNumberGenerator& rng, double* values, const JointBoundsVector& active_joint_bounds,
00344     const double* near, const std::map<JointModel::JointType, double>& distance_map) const
00345 {
00346   assert(active_joint_bounds.size() == active_joint_model_vector_.size());
00347   for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
00348   {
00349     double distance = 0.0;
00350     std::map<moveit::core::JointModel::JointType, double>::const_iterator iter =
00351         distance_map.find(active_joint_model_vector_[i]->getType());
00352     if (iter != distance_map.end())
00353       distance = iter->second;
00354     else
00355       logWarn("Did not pass in distance for '%s'", active_joint_model_vector_[i]->getName().c_str());
00356     active_joint_model_vector_[i]->getVariableRandomPositionsNearBy(
00357         rng, values + active_joint_model_start_index_[i], *active_joint_bounds[i],
00358         near + active_joint_model_start_index_[i], distance);
00359   }
00360   updateMimicJoints(values);
00361 }
00362 
00363 void moveit::core::JointModelGroup::getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng,
00364                                                                      double* values,
00365                                                                      const JointBoundsVector& active_joint_bounds,
00366                                                                      const double* near,
00367                                                                      const std::vector<double>& distances) const
00368 {
00369   assert(active_joint_bounds.size() == active_joint_model_vector_.size());
00370   if (distances.size() != active_joint_model_vector_.size())
00371     throw Exception("When sampling random values nearby for group '" + name_ +
00372                     "', distances vector should be of size " +
00373                     boost::lexical_cast<std::string>(active_joint_model_vector_.size()) + ", but it is of size " +
00374                     boost::lexical_cast<std::string>(distances.size()));
00375   for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
00376     active_joint_model_vector_[i]->getVariableRandomPositionsNearBy(
00377         rng, values + active_joint_model_start_index_[i], *active_joint_bounds[i],
00378         near + active_joint_model_start_index_[i], distances[i]);
00379   updateMimicJoints(values);
00380 }
00381 
00382 bool moveit::core::JointModelGroup::satisfiesPositionBounds(const double* state,
00383                                                             const JointBoundsVector& active_joint_bounds,
00384                                                             double margin) const
00385 {
00386   assert(active_joint_bounds.size() == active_joint_model_vector_.size());
00387   for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
00388     if (!active_joint_model_vector_[i]->satisfiesPositionBounds(state + active_joint_model_start_index_[i],
00389                                                                 *active_joint_bounds[i], margin))
00390       return false;
00391   return true;
00392 }
00393 
00394 bool moveit::core::JointModelGroup::enforcePositionBounds(double* state,
00395                                                           const JointBoundsVector& active_joint_bounds) const
00396 {
00397   assert(active_joint_bounds.size() == active_joint_model_vector_.size());
00398   bool change = false;
00399   for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
00400     if (active_joint_model_vector_[i]->enforcePositionBounds(state + active_joint_model_start_index_[i],
00401                                                              *active_joint_bounds[i]))
00402       change = true;
00403   if (change)
00404     updateMimicJoints(state);
00405   return change;
00406 }
00407 
00408 double moveit::core::JointModelGroup::getMaximumExtent(const JointBoundsVector& active_joint_bounds) const
00409 {
00410   double max_distance = 0.0;
00411   for (std::size_t j = 0; j < active_joint_model_vector_.size(); ++j)
00412     max_distance += active_joint_model_vector_[j]->getMaximumExtent(*active_joint_bounds[j]) *
00413                     active_joint_model_vector_[j]->getDistanceFactor();
00414   return max_distance;
00415 }
00416 
00417 double moveit::core::JointModelGroup::distance(const double* state1, const double* state2) const
00418 {
00419   double d = 0.0;
00420   for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
00421     d += active_joint_model_vector_[i]->getDistanceFactor() *
00422          active_joint_model_vector_[i]->distance(state1 + active_joint_model_start_index_[i],
00423                                                  state2 + active_joint_model_start_index_[i]);
00424   return d;
00425 }
00426 
00427 void moveit::core::JointModelGroup::interpolate(const double* from, const double* to, double t, double* state) const
00428 {
00429   // we interpolate values only for active joint models (non-mimic)
00430   for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
00431     active_joint_model_vector_[i]->interpolate(from + active_joint_model_start_index_[i],
00432                                                to + active_joint_model_start_index_[i], t,
00433                                                state + active_joint_model_start_index_[i]);
00434 
00435   // now we update mimic as needed
00436   updateMimicJoints(state);
00437 }
00438 
00439 void moveit::core::JointModelGroup::updateMimicJoints(double* values) const
00440 {
00441   // update mimic (only local joints as we are dealing with a local group state)
00442   for (std::size_t i = 0; i < group_mimic_update_.size(); ++i)
00443     values[group_mimic_update_[i].dest] =
00444         values[group_mimic_update_[i].src] * group_mimic_update_[i].factor + group_mimic_update_[i].offset;
00445 }
00446 
00447 void moveit::core::JointModelGroup::addDefaultState(const std::string& name,
00448                                                     const std::map<std::string, double>& default_state)
00449 {
00450   default_states_[name] = default_state;
00451   default_states_names_.push_back(name);
00452 }
00453 
00454 bool moveit::core::JointModelGroup::getVariableDefaultPositions(const std::string& name,
00455                                                                 std::map<std::string, double>& values) const
00456 {
00457   std::map<std::string, std::map<std::string, double> >::const_iterator it = default_states_.find(name);
00458   if (it == default_states_.end())
00459     return false;
00460   values = it->second;
00461   return true;
00462 }
00463 
00464 void moveit::core::JointModelGroup::getVariableDefaultPositions(double* values) const
00465 {
00466   for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
00467     active_joint_model_vector_[i]->getVariableDefaultPositions(values + active_joint_model_start_index_[i]);
00468   updateMimicJoints(values);
00469 }
00470 
00471 void moveit::core::JointModelGroup::getVariableDefaultPositions(std::map<std::string, double>& values) const
00472 {
00473   std::vector<double> tmp(variable_count_);
00474   getVariableDefaultPositions(&tmp[0]);
00475   for (std::size_t i = 0; i < variable_names_.size(); ++i)
00476     values[variable_names_[i]] = tmp[i];
00477 }
00478 
00479 void moveit::core::JointModelGroup::setEndEffectorName(const std::string& name)
00480 {
00481   end_effector_name_ = name;
00482 }
00483 
00484 void moveit::core::JointModelGroup::setEndEffectorParent(const std::string& group, const std::string& link)
00485 {
00486   end_effector_parent_.first = group;
00487   end_effector_parent_.second = link;
00488 }
00489 
00490 void moveit::core::JointModelGroup::attachEndEffector(const std::string& eef_name)
00491 {
00492   attached_end_effector_names_.push_back(eef_name);
00493 }
00494 
00495 bool moveit::core::JointModelGroup::getEndEffectorTips(std::vector<std::string>& tips) const
00496 {
00497   // Get a vector of tip links
00498   std::vector<const LinkModel*> tip_links;
00499   if (!getEndEffectorTips(tip_links))
00500     return false;
00501 
00502   // Convert to string names
00503   tips.clear();
00504   for (std::size_t i = 0; i < tip_links.size(); ++i)
00505   {
00506     tips.push_back(tip_links[i]->getName());
00507   }
00508   return true;
00509 }
00510 
00511 bool moveit::core::JointModelGroup::getEndEffectorTips(std::vector<const LinkModel*>& tips) const
00512 {
00513   for (std::size_t i = 0; i < getAttachedEndEffectorNames().size(); ++i)
00514   {
00515     const JointModelGroup* eef = parent_model_->getEndEffector(getAttachedEndEffectorNames()[i]);
00516     if (!eef)
00517     {
00518       logError("Unable to find joint model group for eef");
00519       return false;
00520     }
00521     const std::string& eef_parent = eef->getEndEffectorParentGroup().second;
00522 
00523     const LinkModel* eef_link = parent_model_->getLinkModel(eef_parent);
00524     if (!eef_link)
00525     {
00526       logError("Unable to find end effector link for eef");
00527       return false;
00528     }
00529 
00530     tips.push_back(eef_link);
00531   }
00532   return true;
00533 }
00534 
00535 const moveit::core::LinkModel* moveit::core::JointModelGroup::getOnlyOneEndEffectorTip() const
00536 {
00537   std::vector<const moveit::core::LinkModel*> tips;
00538   getEndEffectorTips(tips);
00539   if (tips.size() == 1)
00540     return tips.front();
00541   else if (tips.size() > 1)
00542     logError("More than one end effector tip found for joint model group, so cannot return only one");
00543   else
00544     logError("No end effector tips found in joint model group");
00545   return NULL;
00546 }
00547 
00548 int moveit::core::JointModelGroup::getVariableGroupIndex(const std::string& variable) const
00549 {
00550   VariableIndexMap::const_iterator it = joint_variables_index_map_.find(variable);
00551   if (it == joint_variables_index_map_.end())
00552   {
00553     logError("Variable '%s' is not part of group '%s'", variable.c_str(), name_.c_str());
00554     return -1;
00555   }
00556   return it->second;
00557 }
00558 
00559 void moveit::core::JointModelGroup::setDefaultIKTimeout(double ik_timeout)
00560 {
00561   group_kinematics_.first.default_ik_timeout_ = ik_timeout;
00562   if (group_kinematics_.first.solver_instance_)
00563     group_kinematics_.first.solver_instance_->setDefaultTimeout(ik_timeout);
00564   for (KinematicsSolverMap::iterator it = group_kinematics_.second.begin(); it != group_kinematics_.second.end(); ++it)
00565     it->second.default_ik_timeout_ = ik_timeout;
00566 }
00567 
00568 void moveit::core::JointModelGroup::setDefaultIKAttempts(unsigned int ik_attempts)
00569 {
00570   group_kinematics_.first.default_ik_attempts_ = ik_attempts;
00571   for (KinematicsSolverMap::iterator it = group_kinematics_.second.begin(); it != group_kinematics_.second.end(); ++it)
00572     it->second.default_ik_attempts_ = ik_attempts;
00573 }
00574 
00575 bool moveit::core::JointModelGroup::computeIKIndexBijection(const std::vector<std::string>& ik_jnames,
00576                                                             std::vector<unsigned int>& joint_bijection) const
00577 {
00578   joint_bijection.clear();
00579   for (std::size_t i = 0; i < ik_jnames.size(); ++i)
00580   {
00581     VariableIndexMap::const_iterator it = joint_variables_index_map_.find(ik_jnames[i]);
00582     if (it == joint_variables_index_map_.end())
00583     {
00584       // skip reported fixed joints
00585       if (hasJointModel(ik_jnames[i]) && getJointModel(ik_jnames[i])->getType() == JointModel::FIXED)
00586         continue;
00587       logError("IK solver computes joint values for joint '%s' but group '%s' does not contain such a joint.",
00588                ik_jnames[i].c_str(), getName().c_str());
00589       return false;
00590     }
00591     const JointModel* jm = getJointModel(ik_jnames[i]);
00592     for (unsigned int k = 0; k < jm->getVariableCount(); ++k)
00593       joint_bijection.push_back(it->second + k);
00594   }
00595   return true;
00596 }
00597 
00598 void moveit::core::JointModelGroup::setSolverAllocators(
00599     const std::pair<SolverAllocatorFn, SolverAllocatorMapFn>& solvers)
00600 {
00601   if (solvers.first)
00602   {
00603     group_kinematics_.first.allocator_ = solvers.first;
00604     group_kinematics_.first.solver_instance_ = solvers.first(this);
00605     if (group_kinematics_.first.solver_instance_)
00606     {
00607       group_kinematics_.first.solver_instance_->setDefaultTimeout(group_kinematics_.first.default_ik_timeout_);
00608       group_kinematics_.first.solver_instance_const_ = group_kinematics_.first.solver_instance_;
00609       if (!computeIKIndexBijection(group_kinematics_.first.solver_instance_->getJointNames(),
00610                                    group_kinematics_.first.bijection_))
00611         group_kinematics_.first.reset();
00612     }
00613   }
00614   else
00615     // we now compute a joint bijection only if we have a solver map
00616     for (SolverAllocatorMapFn::const_iterator it = solvers.second.begin(); it != solvers.second.end(); ++it)
00617       if (it->first->getSolverInstance())
00618       {
00619         KinematicsSolver& ks = group_kinematics_.second[it->first];
00620         ks.allocator_ = it->second;
00621         ks.solver_instance_ = const_cast<JointModelGroup*>(it->first)->getSolverInstance();
00622         ks.solver_instance_const_ = ks.solver_instance_;
00623         ks.default_ik_timeout_ = group_kinematics_.first.default_ik_timeout_;
00624         ks.default_ik_attempts_ = group_kinematics_.first.default_ik_attempts_;
00625         if (!computeIKIndexBijection(ks.solver_instance_->getJointNames(), ks.bijection_))
00626         {
00627           group_kinematics_.second.clear();
00628           break;
00629         }
00630       }
00631 }
00632 
00633 bool moveit::core::JointModelGroup::canSetStateFromIK(const std::string& tip) const
00634 {
00635   const kinematics::KinematicsBaseConstPtr& solver = getSolverInstance();
00636   if (!solver || tip.empty())
00637     return false;
00638 
00639   const std::vector<std::string>& tip_frames = solver->getTipFrames();
00640 
00641   if (tip_frames.empty())
00642   {
00643     logDebug("Group %s has no tip frame(s)", name_.c_str());
00644     return false;
00645   }
00646 
00647   // loop through all tip frames supported by the JMG
00648   for (std::size_t i = 0; i < tip_frames.size(); ++i)
00649   {
00650     // remove frame reference, if specified
00651     const std::string& tip_local = tip[0] == '/' ? tip.substr(1) : tip;
00652     const std::string& tip_frame_local = tip_frames[i][0] == '/' ? tip_frames[i].substr(1) : tip_frames[i];
00653     logDebug("joint_model_group.canSetStateFromIK: comparing input tip: %s to this groups tip: %s ", tip_local.c_str(),
00654              tip_frame_local.c_str());
00655 
00656     // Check if the IK solver's tip is the same as the frame of inquiry
00657     if (tip_local != tip_frame_local)
00658     {
00659       // If not the same, check if this planning group includes the frame of inquiry
00660       if (hasLinkModel(tip_frame_local))
00661       {
00662         const LinkModel* lm = getLinkModel(tip_frame_local);
00663         const LinkTransformMap& fixed_links = lm->getAssociatedFixedTransforms();
00664         // Check if our frame of inquiry is located anywhere further down the chain (towards the tip of the arm)
00665         for (LinkTransformMap::const_iterator it = fixed_links.begin(); it != fixed_links.end(); ++it)
00666         {
00667           if (it->first->getName() == tip_local)
00668             return true;
00669         }
00670       }
00671     }
00672     else
00673       return true;
00674   }
00675 
00676   // Did not find any valid tip frame links to use
00677   return false;
00678 }
00679 
00680 void moveit::core::JointModelGroup::printGroupInfo(std::ostream& out) const
00681 {
00682   out << "Group '" << name_ << "' using " << variable_count_ << " variables" << std::endl;
00683   out << "  * Joints:" << std::endl;
00684   for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
00685     out << "    '" << joint_model_vector_[i]->getName() << "' (" << joint_model_vector_[i]->getTypeName() << ")"
00686         << std::endl;
00687   out << "  * Variables:" << std::endl;
00688   for (std::size_t i = 0; i < variable_names_.size(); ++i)
00689   {
00690     int local_idx = joint_variables_index_map_.find(variable_names_[i])->second;
00691     const JointModel* jm = parent_model_->getJointOfVariable(variable_names_[i]);
00692     out << "    '" << variable_names_[i] << "', index "
00693         << (jm->getFirstVariableIndex() + jm->getLocalVariableIndex(variable_names_[i])) << " in full state, index "
00694         << local_idx << " in group state";
00695     if (jm->getMimic())
00696       out << ", mimic '" << jm->getMimic()->getName() << "'";
00697     out << std::endl;
00698     out << "        " << parent_model_->getVariableBounds(variable_names_[i]) << std::endl;
00699   }
00700   out << "  * Variables Index List:" << std::endl;
00701   out << "    ";
00702   for (std::size_t i = 0; i < variable_index_list_.size(); ++i)
00703     out << variable_index_list_[i] << " ";
00704   if (is_contiguous_index_list_)
00705     out << "(contiguous)";
00706   else
00707     out << "(non-contiguous)";
00708   out << std::endl;
00709   if (group_kinematics_.first)
00710   {
00711     out << "  * Kinematics solver bijection:" << std::endl;
00712     out << "    ";
00713     for (std::size_t i = 0; i < group_kinematics_.first.bijection_.size(); ++i)
00714       out << group_kinematics_.first.bijection_[i] << " ";
00715     out << std::endl;
00716   }
00717   if (!group_kinematics_.second.empty())
00718   {
00719     out << "  * Compound kinematics solver:" << std::endl;
00720     for (KinematicsSolverMap::const_iterator it = group_kinematics_.second.begin();
00721          it != group_kinematics_.second.end(); ++it)
00722     {
00723       out << "    " << it->first->getName() << ":";
00724       for (std::size_t i = 0; i < it->second.bijection_.size(); ++i)
00725         out << " " << it->second.bijection_[i];
00726       out << std::endl;
00727     }
00728   }
00729 
00730   if (!group_mimic_update_.empty())
00731   {
00732     out << "  * Local Mimic Updates:" << std::endl;
00733     for (std::size_t i = 0; i < group_mimic_update_.size(); ++i)
00734       out << "    [" << group_mimic_update_[i].dest << "] = " << group_mimic_update_[i].factor << " * ["
00735           << group_mimic_update_[i].src << "] + " << group_mimic_update_[i].offset << std::endl;
00736   }
00737   out << std::endl;
00738 }


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jun 19 2019 19:23:49