00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <moveit/kinematics_plugin_loader/kinematics_plugin_loader.h>
00038 #include <moveit/rdf_loader/rdf_loader.h>
00039 #include <pluginlib/class_loader.h>
00040 #include <boost/thread/mutex.hpp>
00041 #include <sstream>
00042 #include <vector>
00043 #include <map>
00044 #include <ros/ros.h>
00045 #include <moveit/profiler/profiler.h>
00046
00047 namespace kinematics_plugin_loader
00048 {
00049
00050 class KinematicsPluginLoader::KinematicsLoaderImpl
00051 {
00052 public:
00060 KinematicsLoaderImpl(const std::string &robot_description,
00061 const std::map<std::string, std::vector<std::string> > &possible_kinematics_solvers,
00062 const std::map<std::string, std::vector<double> > &search_res,
00063 const std::map<std::string, std::vector<std::string> > &iksolver_to_tip_links) :
00064 robot_description_(robot_description),
00065 possible_kinematics_solvers_(possible_kinematics_solvers),
00066 search_res_(search_res),
00067 iksolver_to_tip_links_(iksolver_to_tip_links)
00068 {
00069 try
00070 {
00071 kinematics_loader_.reset(new pluginlib::ClassLoader<kinematics::KinematicsBase>("moveit_core", "kinematics::KinematicsBase"));
00072 }
00073 catch(pluginlib::PluginlibException& e)
00074 {
00075 ROS_ERROR("Unable to construct kinematics loader. Error: %s", e.what());
00076 }
00077 }
00078
00084 std::vector<std::string> chooseTipFrames(const robot_model::JointModelGroup *jmg)
00085 {
00086 std::vector<std::string> tips;
00087 std::map<std::string, std::vector<std::string> >::const_iterator ik_it = iksolver_to_tip_links_.find(jmg->getName());
00088
00089
00090 if (ik_it != iksolver_to_tip_links_.end())
00091 {
00092
00093 ROS_DEBUG_STREAM_NAMED("kinematics_plugin_loader","Chooing tip frame of kinematic solver for group "
00094 << jmg->getName() << " based on values in rosparam server.");
00095 tips = ik_it->second;
00096 }
00097 else
00098 {
00099
00100 ROS_DEBUG_STREAM_NAMED("kinematics_plugin_loader","Chooing tip frame of kinematic solver for group "
00101 << jmg->getName() << " based on last link in chain");
00102
00103 tips.push_back(jmg->getLinkModels().back()->getName());
00104 }
00105
00106
00107 if (tips.empty())
00108 {
00109 ROS_ERROR_STREAM_NAMED("kinematics_plugin_loader","Error choosing kinematic solver tip frame(s).");
00110 }
00111
00112
00113 std::stringstream tip_debug;
00114 tip_debug << "Planning group '" << jmg->getName() << "' has tip(s): ";
00115 for (std::size_t i = 0; i < tips.size(); ++i)
00116 tip_debug << tips[i] << ", ";
00117 ROS_DEBUG_STREAM_NAMED("kinematics_plugin_loader", tip_debug.str());
00118
00119 return tips;
00120 }
00121
00122 boost::shared_ptr<kinematics::KinematicsBase> allocKinematicsSolver(const robot_model::JointModelGroup *jmg)
00123 {
00124 boost::shared_ptr<kinematics::KinematicsBase> result;
00125 if (!jmg)
00126 {
00127 ROS_ERROR("Specified group is NULL. Cannot allocate kinematics solver.");
00128 return result;
00129 }
00130
00131 ROS_DEBUG("Received request to allocate kinematics solver for group '%s'", jmg->getName().c_str());
00132
00133 if (kinematics_loader_ && jmg)
00134 {
00135 std::map<std::string, std::vector<std::string> >::const_iterator it = possible_kinematics_solvers_.find(jmg->getName());
00136 if (it != possible_kinematics_solvers_.end())
00137 {
00138
00139 boost::mutex::scoped_lock slock(lock_);
00140
00141 for (std::size_t i = 0 ; !result && i < it->second.size() ; ++i)
00142 {
00143 try
00144 {
00145 result.reset(kinematics_loader_->createUnmanagedInstance(it->second[i]));
00146 if (result)
00147 {
00148 const std::vector<const robot_model::LinkModel*> &links = jmg->getLinkModels();
00149 if (!links.empty())
00150 {
00151 const std::string &base = links.front()->getParentJointModel()->getParentLinkModel() ?
00152 links.front()->getParentJointModel()->getParentLinkModel()->getName() : jmg->getParentModel().getModelFrame();
00153
00154
00155 const std::vector<std::string> tips = chooseTipFrames(jmg);
00156
00157
00158 double search_res = search_res_.find(jmg->getName())->second[i];
00159
00160 if (!result->initialize(robot_description_, jmg->getName(),
00161 (base.empty() || base[0] != '/') ? base : base.substr(1) , tips, search_res))
00162 {
00163 ROS_ERROR("Kinematics solver of type '%s' could not be initialized for group '%s'", it->second[i].c_str(), jmg->getName().c_str());
00164 result.reset();
00165 }
00166 else
00167 {
00168 result->setDefaultTimeout(jmg->getDefaultIKTimeout());
00169 ROS_DEBUG("Successfully allocated and initialized a kinematics solver of type '%s' with search resolution %lf for group '%s' at address %p",
00170 it->second[i].c_str(), search_res, jmg->getName().c_str(), result.get());
00171 }
00172 }
00173 else
00174 ROS_ERROR("No links specified for group '%s'", jmg->getName().c_str());
00175 }
00176 }
00177 catch (pluginlib::PluginlibException& e)
00178 {
00179 ROS_ERROR("The kinematics plugin (%s) failed to load. Error: %s", it->first.c_str(), e.what());
00180 }
00181 }
00182 }
00183 else
00184 ROS_DEBUG("No kinematics solver available for this group");
00185 }
00186
00187 if (!result)
00188 {
00189 ROS_DEBUG("No usable kinematics solver was found for this group.");
00190 ROS_DEBUG("Did you load kinematics.yaml into your node's namespace?");
00191 }
00192 return result;
00193 }
00194
00195 boost::shared_ptr<kinematics::KinematicsBase> allocKinematicsSolverWithCache(const robot_model::JointModelGroup *jmg)
00196 {
00197 {
00198 boost::mutex::scoped_lock slock(lock_);
00199 const std::vector<boost::shared_ptr<kinematics::KinematicsBase> > &vi = instances_[jmg];
00200 for (std::size_t i = 0 ; i < vi.size() ; ++i)
00201 if (vi[i].unique())
00202 {
00203 ROS_DEBUG("Reusing cached kinematics solver for group '%s'", jmg->getName().c_str());
00204 return vi[i];
00205 }
00206 }
00207
00208 boost::shared_ptr<kinematics::KinematicsBase> res = allocKinematicsSolver(jmg);
00209
00210 {
00211 boost::mutex::scoped_lock slock(lock_);
00212 instances_[jmg].push_back(res);
00213 return res;
00214 }
00215 }
00216
00217 void status() const
00218 {
00219 for (std::map<std::string, std::vector<std::string> >::const_iterator it = possible_kinematics_solvers_.begin() ; it != possible_kinematics_solvers_.end() ; ++it)
00220 for (std::size_t i = 0 ; i < it->second.size() ; ++i)
00221 ROS_INFO("Solver for group '%s': '%s' (search resolution = %lf)", it->first.c_str(), it->second[i].c_str(), search_res_.at(it->first)[i]);
00222 }
00223
00224 private:
00225
00226 std::string robot_description_;
00227 std::map<std::string, std::vector<std::string> > possible_kinematics_solvers_;
00228 std::map<std::string, std::vector<double> > search_res_;
00229 std::map<std::string, std::vector<std::string> > iksolver_to_tip_links_;
00230 boost::shared_ptr<pluginlib::ClassLoader<kinematics::KinematicsBase> > kinematics_loader_;
00231 std::map<const robot_model::JointModelGroup*,
00232 std::vector<boost::shared_ptr<kinematics::KinematicsBase> > > instances_;
00233 boost::mutex lock_;
00234 };
00235
00236 }
00237
00238 void kinematics_plugin_loader::KinematicsPluginLoader::status() const
00239 {
00240 if (loader_)
00241 loader_->status();
00242 else
00243 ROS_INFO("Loader function was never required");
00244 }
00245
00246 robot_model::SolverAllocatorFn kinematics_plugin_loader::KinematicsPluginLoader::getLoaderFunction()
00247 {
00248 moveit::tools::Profiler::ScopedStart prof_start;
00249 moveit::tools::Profiler::ScopedBlock prof_block("KinematicsPluginLoader::getLoaderFunction");
00250
00251 if (loader_)
00252 return boost::bind(&KinematicsPluginLoader::KinematicsLoaderImpl::allocKinematicsSolverWithCache, loader_.get(), _1);
00253
00254 rdf_loader::RDFLoader rml(robot_description_);
00255 robot_description_ = rml.getRobotDescription();
00256 return getLoaderFunction(rml.getSRDF());
00257 }
00258
00259 robot_model::SolverAllocatorFn kinematics_plugin_loader::KinematicsPluginLoader::getLoaderFunction(const boost::shared_ptr<srdf::Model> &srdf_model)
00260 {
00261 moveit::tools::Profiler::ScopedStart prof_start;
00262 moveit::tools::Profiler::ScopedBlock prof_block("KinematicsPluginLoader::getLoaderFunction(SRDF)");
00263
00264 if (!loader_)
00265 {
00266 ROS_DEBUG("Configuring kinematics solvers");
00267 groups_.clear();
00268
00269 std::map<std::string, std::vector<std::string> > possible_kinematics_solvers;
00270 std::map<std::string, std::vector<double> > search_res;
00271 std::map<std::string, std::vector<std::string> > iksolver_to_tip_links;
00272
00273 if (srdf_model)
00274 {
00275 const std::vector<srdf::Model::Group> &known_groups = srdf_model->getGroups();
00276 if (default_search_resolution_ <= std::numeric_limits<double>::epsilon())
00277 default_search_resolution_ = kinematics::KinematicsBase::DEFAULT_SEARCH_DISCRETIZATION;
00278
00279 if (default_solver_plugin_.empty())
00280 {
00281 ROS_DEBUG("Loading settings for kinematics solvers from the ROS param server ...");
00282
00283
00284 ros::NodeHandle nh("~");
00285
00286
00287 for (std::size_t i = 0 ; i < known_groups.size() ; ++i)
00288 {
00289 std::string base_param_name = known_groups[i].name_;
00290 ROS_DEBUG_NAMED("kinematics_plugin_loader","Looking for param %s ", (base_param_name + "/kinematics_solver").c_str());
00291 std::string ksolver_param_name;
00292 bool found = nh.searchParam(base_param_name + "/kinematics_solver", ksolver_param_name);
00293 if (!found || !nh.hasParam(ksolver_param_name))
00294 {
00295 base_param_name = robot_description_ + "_kinematics/" + known_groups[i].name_;
00296 ROS_DEBUG_NAMED("kinematics_plugin_loader","Looking for param %s ", (base_param_name + "/kinematics_solver").c_str());
00297 found = nh.searchParam(base_param_name + "/kinematics_solver", ksolver_param_name);
00298 }
00299 if (found)
00300 {
00301 ROS_DEBUG_NAMED("kinematics_plugin_loader","Found param %s", ksolver_param_name.c_str());
00302 std::string ksolver;
00303 if (nh.getParam(ksolver_param_name, ksolver))
00304 {
00305 std::stringstream ss(ksolver);
00306 bool first = true;
00307 while (ss.good() && !ss.eof())
00308 {
00309 if (first)
00310 {
00311 first = false;
00312 groups_.push_back(known_groups[i].name_);
00313 }
00314 std::string solver; ss >> solver >> std::ws;
00315 possible_kinematics_solvers[known_groups[i].name_].push_back(solver);
00316 ROS_DEBUG_NAMED("kinematics_plugin_loader","Using kinematics solver '%s' for group '%s'.", solver.c_str(), known_groups[i].name_.c_str());
00317 }
00318 }
00319 }
00320
00321 std::string ksolver_timeout_param_name;
00322 if (nh.searchParam(base_param_name + "/kinematics_solver_timeout", ksolver_timeout_param_name))
00323 {
00324 double ksolver_timeout;
00325 if (nh.getParam(ksolver_timeout_param_name, ksolver_timeout))
00326 ik_timeout_[known_groups[i].name_] = ksolver_timeout;
00327 else
00328 {
00329 int ksolver_timeout_i;
00330 if (nh.getParam(ksolver_timeout_param_name, ksolver_timeout_i))
00331 ik_timeout_[known_groups[i].name_] = ksolver_timeout_i;
00332 }
00333 }
00334
00335 std::string ksolver_attempts_param_name;
00336 if (nh.searchParam(base_param_name + "/kinematics_solver_attempts", ksolver_attempts_param_name))
00337 {
00338 int ksolver_attempts;
00339 if (nh.getParam(ksolver_attempts_param_name, ksolver_attempts))
00340 ik_attempts_[known_groups[i].name_] = ksolver_attempts;
00341 }
00342
00343 std::string ksolver_res_param_name;
00344 if (nh.searchParam(base_param_name + "/kinematics_solver_search_resolution", ksolver_res_param_name))
00345 {
00346 std::string ksolver_res;
00347 if (nh.getParam(ksolver_res_param_name, ksolver_res))
00348 {
00349 std::stringstream ss(ksolver_res);
00350 while (ss.good() && !ss.eof())
00351 {
00352 double res; ss >> res >> std::ws;
00353 search_res[known_groups[i].name_].push_back(res);
00354 }
00355 }
00356 else
00357 {
00358 double res;
00359 if (nh.getParam(ksolver_res_param_name, res))
00360 search_res[known_groups[i].name_].push_back(res);
00361 else
00362 {
00363 int res_i;
00364 if (nh.getParam(ksolver_res_param_name, res_i))
00365 search_res[known_groups[i].name_].push_back(res_i);
00366 }
00367 }
00368 }
00369
00370
00371
00372 std::string ksolver_ik_link_param_name;
00373 if (nh.searchParam(base_param_name + "/kinematics_solver_ik_link", ksolver_ik_link_param_name))
00374 {
00375 std::string ksolver_ik_link;
00376 if (nh.getParam(ksolver_ik_link_param_name, ksolver_ik_link))
00377 {
00378 ROS_WARN_STREAM_NAMED("kinematics_plugin_loader","Using kinematics_solver_ik_link rosparam is deprecated in favor of kinematics_solver_ik_links rosparam array.");
00379 iksolver_to_tip_links[known_groups[i].name_].push_back(ksolver_ik_link);
00380 }
00381 }
00382
00383
00384 std::string ksolver_ik_links_param_name;
00385 if (nh.searchParam(base_param_name + "/kinematics_solver_ik_links", ksolver_ik_links_param_name))
00386 {
00387 XmlRpc::XmlRpcValue ksolver_ik_links;
00388 if (nh.getParam(ksolver_ik_links_param_name, ksolver_ik_links))
00389 {
00390 if (ksolver_ik_links.getType() != XmlRpc::XmlRpcValue::TypeArray)
00391 {
00392 ROS_WARN_STREAM_NAMED("kinematics_plugin_loader","rosparam '" << ksolver_ik_links_param_name << "' should be an XmlRpc value type 'Array'");
00393 }
00394 else
00395 {
00396 for (int32_t j = 0; j < ksolver_ik_links.size(); ++j)
00397 {
00398 ROS_ASSERT(ksolver_ik_links[j].getType() == XmlRpc::XmlRpcValue::TypeString);
00399 ROS_DEBUG_STREAM_NAMED("kinematics_plugin_loader","found tip " << static_cast<std::string>(ksolver_ik_links[j]) << " for group " << known_groups[i].name_ );
00400 iksolver_to_tip_links[known_groups[i].name_].push_back( static_cast<std::string>(ksolver_ik_links[j]) );
00401 }
00402 }
00403 }
00404 }
00405
00406
00407 while (search_res[known_groups[i].name_].size() < possible_kinematics_solvers[known_groups[i].name_].size())
00408 search_res[known_groups[i].name_].push_back(default_search_resolution_);
00409 }
00410 }
00411 else
00412 {
00413 ROS_DEBUG("Using specified default settings for kinematics solvers ...");
00414 for (std::size_t i = 0 ; i < known_groups.size() ; ++i)
00415 {
00416 possible_kinematics_solvers[known_groups[i].name_].resize(1, default_solver_plugin_);
00417 search_res[known_groups[i].name_].resize(1, default_search_resolution_);
00418 ik_timeout_[known_groups[i].name_] = default_solver_timeout_;
00419 ik_attempts_[known_groups[i].name_] = default_ik_attempts_;
00420 groups_.push_back(known_groups[i].name_);
00421 }
00422 }
00423 }
00424
00425 loader_.reset(new KinematicsLoaderImpl(robot_description_, possible_kinematics_solvers, search_res, iksolver_to_tip_links));
00426 }
00427
00428 return boost::bind(&KinematicsPluginLoader::KinematicsLoaderImpl::allocKinematicsSolverWithCache, loader_.get(), _1);
00429 }