robot_model_loader.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Ioan Sucan, E. Gil Jones */
00036 
00037 #include <moveit/robot_model_loader/robot_model_loader.h>
00038 #include <moveit/profiler/profiler.h>
00039 #include <ros/ros.h>
00040 #include <typeinfo>
00041 
00042 robot_model_loader::RobotModelLoader::RobotModelLoader(const std::string &robot_description, bool load_kinematics_solvers)
00043 {
00044   Options opt(robot_description);
00045   opt.load_kinematics_solvers_ = load_kinematics_solvers;
00046   configure(opt);
00047 }
00048 
00049 robot_model_loader::RobotModelLoader::RobotModelLoader(const Options &opt)
00050 {
00051   configure(opt);
00052 }
00053 
00054 robot_model_loader::RobotModelLoader::~RobotModelLoader()
00055 {
00056   model_.reset();
00057   rdf_loader_.reset();
00058   kinematics_loader_.reset();
00059 }
00060 
00061 namespace
00062 {
00063 
00064 bool canSpecifyPosition(const robot_model::JointModel *jmodel, const unsigned int index)
00065 {
00066   bool ok = false;
00067   if (jmodel->getType() == robot_model::JointModel::PLANAR && index == 2)
00068     ROS_ERROR("Cannot specify position limits for orientation of planar joint '%s'", jmodel->getName().c_str());
00069   else
00070   if (jmodel->getType() == robot_model::JointModel::FLOATING && index > 2)
00071     ROS_ERROR("Cannot specify position limits for orientation of floating joint '%s'", jmodel->getName().c_str());
00072   else
00073   if (jmodel->getType() == robot_model::JointModel::REVOLUTE &&
00074       static_cast<const robot_model::RevoluteJointModel*>(jmodel)->isContinuous())
00075     ROS_ERROR("Cannot specify position limits for continuous joint '%s'", jmodel->getName().c_str());
00076   else
00077     ok = true;
00078   return ok;
00079 }
00080 }
00081 
00082 void robot_model_loader::RobotModelLoader::configure(const Options &opt)
00083 {
00084   moveit::tools::Profiler::ScopedStart prof_start;
00085   moveit::tools::Profiler::ScopedBlock prof_block("RobotModelLoader::configure");
00086 
00087   ros::WallTime start = ros::WallTime::now();
00088   if (opt.urdf_doc_ && opt.srdf_doc_)
00089     rdf_loader_.reset(new rdf_loader::RDFLoader(opt.urdf_doc_, opt.srdf_doc_));
00090   else
00091     if (!opt.urdf_string_.empty() && !opt.srdf_string_.empty())
00092       rdf_loader_.reset(new rdf_loader::RDFLoader(opt.urdf_string_, opt.srdf_string_));
00093     else
00094       rdf_loader_.reset(new rdf_loader::RDFLoader(opt.robot_description_));
00095   if (rdf_loader_->getURDF())
00096   {
00097     const boost::shared_ptr<srdf::Model> &srdf = rdf_loader_->getSRDF() ? rdf_loader_->getSRDF() : boost::shared_ptr<srdf::Model>(new srdf::Model());
00098     model_.reset(new robot_model::RobotModel(rdf_loader_->getURDF(), srdf));
00099   }
00100 
00101   if (model_ && !rdf_loader_->getRobotDescription().empty())
00102   {
00103     moveit::tools::Profiler::ScopedBlock prof_block2("RobotModelLoader::configure joint limits");
00104 
00105     // if there are additional joint limits specified in some .yaml file, read those in
00106     ros::NodeHandle nh("~");
00107 
00108     for (std::size_t i = 0; i < model_->getJointModels().size() ; ++i)
00109     {
00110       robot_model::JointModel *jmodel = model_->getJointModels()[i];
00111       std::vector<moveit_msgs::JointLimits> jlim = jmodel->getVariableBoundsMsg();
00112       for (std::size_t j = 0; j < jlim.size(); ++j)
00113       {
00114         std::string prefix = rdf_loader_->getRobotDescription() + "_planning/joint_limits/" + jlim[j].joint_name + "/";
00115 
00116         double max_position;
00117         if (nh.getParam(prefix + "max_position", max_position))
00118         {
00119           if (canSpecifyPosition(jmodel, j))
00120           {
00121             jlim[j].has_position_limits = true;
00122             jlim[j].max_position = max_position;
00123           }
00124         }
00125         double min_position;
00126         if (nh.getParam(prefix + "min_position", min_position))
00127         {
00128           if (canSpecifyPosition(jmodel, j))
00129           {
00130             jlim[j].has_position_limits = true;
00131             jlim[j].min_position = min_position;
00132           }
00133         }
00134         double max_velocity;
00135         if (nh.getParam(prefix + "max_velocity", max_velocity))
00136         {
00137           jlim[j].has_velocity_limits = true;
00138           jlim[j].max_velocity = max_velocity;
00139         }
00140         bool has_vel_limits;
00141         if (nh.getParam(prefix + "has_velocity_limits", has_vel_limits))
00142           jlim[j].has_velocity_limits = has_vel_limits;
00143 
00144         double max_acc;
00145         if (nh.getParam(prefix + "max_acceleration", max_acc))
00146         {
00147           jlim[j].has_acceleration_limits = true;
00148           jlim[j].max_acceleration = max_acc;
00149         }
00150         bool has_acc_limits;
00151         if (nh.getParam(prefix + "has_acceleration_limits", has_acc_limits))
00152           jlim[j].has_acceleration_limits = has_acc_limits;
00153       }
00154       jmodel->setVariableBounds(jlim);
00155     }
00156   }
00157 
00158   if (model_ && opt.load_kinematics_solvers_)
00159     loadKinematicsSolvers();
00160 
00161   ROS_DEBUG_STREAM_NAMED("robot_model_loader","Loaded kinematic model in " << (ros::WallTime::now() - start).toSec() << " seconds");
00162 }
00163 
00164 void robot_model_loader::RobotModelLoader::loadKinematicsSolvers(const kinematics_plugin_loader::KinematicsPluginLoaderPtr &kloader)
00165 {
00166   moveit::tools::Profiler::ScopedStart prof_start;
00167   moveit::tools::Profiler::ScopedBlock prof_block("RobotModelLoader::loadKinematicsSolvers");
00168 
00169   if (rdf_loader_ && model_)
00170   {
00171     // load the kinematics solvers
00172     if (kloader)
00173       kinematics_loader_ = kloader;
00174     else
00175       kinematics_loader_.reset(new kinematics_plugin_loader::KinematicsPluginLoader(rdf_loader_->getRobotDescription()));
00176     robot_model::SolverAllocatorFn kinematics_allocator = kinematics_loader_->getLoaderFunction(rdf_loader_->getSRDF());
00177     const std::vector<std::string> &groups = kinematics_loader_->getKnownGroups();
00178     std::stringstream ss;
00179     std::copy(groups.begin(), groups.end(), std::ostream_iterator<std::string>(ss, " "));
00180     ROS_DEBUG_STREAM("Loaded information about the following groups: '" << ss.str() << "'");
00181 
00182     std::map<std::string, robot_model::SolverAllocatorFn> imap;
00183     for (std::size_t i = 0 ; i < groups.size() ; ++i)
00184     {
00185       // Check if a group in kinematics.yaml exists in the srdf
00186       if (!model_->hasJointModelGroup(groups[i]))
00187         continue;
00188 
00189       const robot_model::JointModelGroup *jmg = model_->getJointModelGroup(groups[i]);
00190 
00191       kinematics::KinematicsBasePtr solver = kinematics_allocator(jmg);
00192       if(solver)
00193       {
00194         std::string error_msg;
00195         if(solver->supportsGroup(jmg, &error_msg))
00196         {
00197           imap[groups[i]] = kinematics_allocator;
00198         }
00199         else
00200         {
00201           ROS_ERROR("Kinematics solver %s does not support joint group %s.  Error: %s",
00202                     typeid(*solver).name(),
00203                     groups[i].c_str(),
00204                     error_msg.c_str());
00205         }
00206       }
00207       else
00208       {
00209         ROS_ERROR("Kinematics solver could not be instantiated for joint group %s.",
00210                   groups[i].c_str());
00211       }
00212     }
00213     model_->setKinematicsAllocators(imap);
00214 
00215     // set the default IK timeouts
00216     const std::map<std::string, double> &timeout = kinematics_loader_->getIKTimeout();
00217     for (std::map<std::string, double>::const_iterator it = timeout.begin() ; it != timeout.end() ; ++it)
00218     {
00219       if (!model_->hasJointModelGroup(it->first))
00220         continue;
00221       robot_model::JointModelGroup *jmg = model_->getJointModelGroup(it->first);
00222       jmg->setDefaultIKTimeout(it->second);
00223     }
00224 
00225     // set the default IK attempts
00226     const std::map<std::string, unsigned int> &attempts = kinematics_loader_->getIKAttempts();
00227     for (std::map<std::string, unsigned int>::const_iterator it = attempts.begin() ; it != attempts.end() ; ++it)
00228     {
00229       if (!model_->hasJointModelGroup(it->first))
00230         continue;
00231       robot_model::JointModelGroup *jmg = model_->getJointModelGroup(it->first);
00232       jmg->setDefaultIKAttempts(it->second);
00233     }
00234   }
00235 }


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Wed Aug 26 2015 12:43:30