Go to the documentation of this file.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/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
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
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
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
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
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 }