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