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,
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
00058
00059
00060
00061
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
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
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
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
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
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 }