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,
00043                                                        bool load_kinematics_solvers)
00044 {
00045   Options opt(robot_description);
00046   opt.load_kinematics_solvers_ = load_kinematics_solvers;
00047   configure(opt);
00048 }
00049 
00050 robot_model_loader::RobotModelLoader::RobotModelLoader(const Options& opt)
00051 {
00052   configure(opt);
00053 }
00054 
00055 robot_model_loader::RobotModelLoader::~RobotModelLoader()
00056 {
00057   // Make sure we destroy the robot model first. It contains the loaded
00058   // kinematics plugins, and those must be destroyed before the pluginlib class
00059   // that implements them is destroyed (that happens when kinematics_loader_ is
00060   // destroyed below). This is a workaround - ideally pluginlib would handle
00061   // this better.
00062   model_.reset();
00063   rdf_loader_.reset();
00064   kinematics_loader_.reset();
00065 }
00066 
00067 namespace
00068 {
00069 bool canSpecifyPosition(const robot_model::JointModel* jmodel, const unsigned int index)
00070 {
00071   bool ok = false;
00072   if (jmodel->getType() == robot_model::JointModel::PLANAR && index == 2)
00073     ROS_ERROR("Cannot specify position limits for orientation of planar joint '%s'", jmodel->getName().c_str());
00074   else if (jmodel->getType() == robot_model::JointModel::FLOATING && index > 2)
00075     ROS_ERROR("Cannot specify position limits for orientation of floating joint '%s'", jmodel->getName().c_str());
00076   else if (jmodel->getType() == robot_model::JointModel::REVOLUTE &&
00077            static_cast<const robot_model::RevoluteJointModel*>(jmodel)->isContinuous())
00078     ROS_ERROR("Cannot specify position limits for continuous joint '%s'", jmodel->getName().c_str());
00079   else
00080     ok = true;
00081   return ok;
00082 }
00083 }
00084 
00085 void robot_model_loader::RobotModelLoader::configure(const Options& opt)
00086 {
00087   moveit::tools::Profiler::ScopedStart prof_start;
00088   moveit::tools::Profiler::ScopedBlock prof_block("RobotModelLoader::configure");
00089 
00090   ros::WallTime start = ros::WallTime::now();
00091   if (opt.urdf_doc_ && opt.srdf_doc_)
00092     rdf_loader_.reset(new rdf_loader::RDFLoader(opt.urdf_doc_, opt.srdf_doc_));
00093   else if (!opt.urdf_string_.empty() && !opt.srdf_string_.empty())
00094     rdf_loader_.reset(new rdf_loader::RDFLoader(opt.urdf_string_, opt.srdf_string_));
00095   else
00096     rdf_loader_.reset(new rdf_loader::RDFLoader(opt.robot_description_));
00097   if (rdf_loader_->getURDF())
00098   {
00099     const boost::shared_ptr<srdf::Model>& srdf =
00100         rdf_loader_->getSRDF() ? rdf_loader_->getSRDF() : boost::shared_ptr<srdf::Model>(new srdf::Model());
00101     model_.reset(new robot_model::RobotModel(rdf_loader_->getURDF(), srdf));
00102   }
00103 
00104   if (model_ && !rdf_loader_->getRobotDescription().empty())
00105   {
00106     moveit::tools::Profiler::ScopedBlock prof_block2("RobotModelLoader::configure joint limits");
00107 
00108     // if there are additional joint limits specified in some .yaml file, read those in
00109     ros::NodeHandle nh("~");
00110 
00111     for (std::size_t i = 0; i < model_->getJointModels().size(); ++i)
00112     {
00113       robot_model::JointModel* jmodel = model_->getJointModels()[i];
00114       std::vector<moveit_msgs::JointLimits> jlim = jmodel->getVariableBoundsMsg();
00115       for (std::size_t j = 0; j < jlim.size(); ++j)
00116       {
00117         std::string prefix = rdf_loader_->getRobotDescription() + "_planning/joint_limits/" + jlim[j].joint_name + "/";
00118 
00119         double max_position;
00120         if (nh.getParam(prefix + "max_position", max_position))
00121         {
00122           if (canSpecifyPosition(jmodel, j))
00123           {
00124             jlim[j].has_position_limits = true;
00125             jlim[j].max_position = max_position;
00126           }
00127         }
00128         double min_position;
00129         if (nh.getParam(prefix + "min_position", min_position))
00130         {
00131           if (canSpecifyPosition(jmodel, j))
00132           {
00133             jlim[j].has_position_limits = true;
00134             jlim[j].min_position = min_position;
00135           }
00136         }
00137         double max_velocity;
00138         if (nh.getParam(prefix + "max_velocity", max_velocity))
00139         {
00140           jlim[j].has_velocity_limits = true;
00141           jlim[j].max_velocity = max_velocity;
00142         }
00143         bool has_vel_limits;
00144         if (nh.getParam(prefix + "has_velocity_limits", has_vel_limits))
00145           jlim[j].has_velocity_limits = has_vel_limits;
00146 
00147         double max_acc;
00148         if (nh.getParam(prefix + "max_acceleration", max_acc))
00149         {
00150           jlim[j].has_acceleration_limits = true;
00151           jlim[j].max_acceleration = max_acc;
00152         }
00153         bool has_acc_limits;
00154         if (nh.getParam(prefix + "has_acceleration_limits", has_acc_limits))
00155           jlim[j].has_acceleration_limits = has_acc_limits;
00156       }
00157       jmodel->setVariableBounds(jlim);
00158     }
00159   }
00160 
00161   if (model_ && opt.load_kinematics_solvers_)
00162     loadKinematicsSolvers();
00163 
00164   ROS_DEBUG_STREAM_NAMED("robot_model_loader", "Loaded kinematic model in " << (ros::WallTime::now() - start).toSec()
00165                                                                             << " seconds");
00166 }
00167 
00168 void robot_model_loader::RobotModelLoader::loadKinematicsSolvers(
00169     const kinematics_plugin_loader::KinematicsPluginLoaderPtr& kloader)
00170 {
00171   moveit::tools::Profiler::ScopedStart prof_start;
00172   moveit::tools::Profiler::ScopedBlock prof_block("RobotModelLoader::loadKinematicsSolvers");
00173 
00174   if (rdf_loader_ && model_)
00175   {
00176     // load the kinematics solvers
00177     if (kloader)
00178       kinematics_loader_ = kloader;
00179     else
00180       kinematics_loader_.reset(
00181           new kinematics_plugin_loader::KinematicsPluginLoader(rdf_loader_->getRobotDescription()));
00182     robot_model::SolverAllocatorFn kinematics_allocator = kinematics_loader_->getLoaderFunction(rdf_loader_->getSRDF());
00183     const std::vector<std::string>& groups = kinematics_loader_->getKnownGroups();
00184     std::stringstream ss;
00185     std::copy(groups.begin(), groups.end(), std::ostream_iterator<std::string>(ss, " "));
00186     ROS_DEBUG_STREAM("Loaded information about the following groups: '" << ss.str() << "'");
00187 
00188     std::map<std::string, robot_model::SolverAllocatorFn> imap;
00189     for (std::size_t i = 0; i < groups.size(); ++i)
00190     {
00191       // Check if a group in kinematics.yaml exists in the srdf
00192       if (!model_->hasJointModelGroup(groups[i]))
00193         continue;
00194 
00195       const robot_model::JointModelGroup* jmg = model_->getJointModelGroup(groups[i]);
00196 
00197       kinematics::KinematicsBasePtr solver = kinematics_allocator(jmg);
00198       if (solver)
00199       {
00200         std::string error_msg;
00201         if (solver->supportsGroup(jmg, &error_msg))
00202         {
00203           imap[groups[i]] = kinematics_allocator;
00204         }
00205         else
00206         {
00207           ROS_ERROR("Kinematics solver %s does not support joint group %s.  Error: %s", typeid(*solver).name(),
00208                     groups[i].c_str(), error_msg.c_str());
00209         }
00210       }
00211       else
00212       {
00213         ROS_ERROR("Kinematics solver could not be instantiated for joint group %s.", groups[i].c_str());
00214       }
00215     }
00216     model_->setKinematicsAllocators(imap);
00217 
00218     // set the default IK timeouts
00219     const std::map<std::string, double>& timeout = kinematics_loader_->getIKTimeout();
00220     for (std::map<std::string, double>::const_iterator it = timeout.begin(); it != timeout.end(); ++it)
00221     {
00222       if (!model_->hasJointModelGroup(it->first))
00223         continue;
00224       robot_model::JointModelGroup* jmg = model_->getJointModelGroup(it->first);
00225       jmg->setDefaultIKTimeout(it->second);
00226     }
00227 
00228     // set the default IK attempts
00229     const std::map<std::string, unsigned int>& attempts = kinematics_loader_->getIKAttempts();
00230     for (std::map<std::string, unsigned int>::const_iterator it = attempts.begin(); it != attempts.end(); ++it)
00231     {
00232       if (!model_->hasJointModelGroup(it->first))
00233         continue;
00234       robot_model::JointModelGroup* jmg = model_->getJointModelGroup(it->first);
00235       jmg->setDefaultIKAttempts(it->second);
00236     }
00237   }
00238 }


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Wed Jan 17 2018 03:32:07