robot_model_loader.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan, E. Gil Jones */
36 
39 #include <ros/ros.h>
40 #include <typeinfo>
41 
42 robot_model_loader::RobotModelLoader::RobotModelLoader(const std::string& robot_description,
43  bool load_kinematics_solvers)
44 {
45  Options opt(robot_description);
46  opt.load_kinematics_solvers_ = load_kinematics_solvers;
47  configure(opt);
48 }
49 
51 {
52  configure(opt);
53 }
54 
56 {
57  // Make sure we destroy the robot model first. It contains the loaded
58  // kinematics plugins, and those must be destroyed before the pluginlib class
59  // that implements them is destroyed (that happens when kinematics_loader_ is
60  // destroyed below). This is a workaround - ideally pluginlib would handle
61  // this better.
62  model_.reset();
63  rdf_loader_.reset();
64  kinematics_loader_.reset();
65 }
66 
67 namespace
68 {
69 bool canSpecifyPosition(const robot_model::JointModel* jmodel, const unsigned int index)
70 {
71  bool ok = false;
72  if (jmodel->getType() == robot_model::JointModel::PLANAR && index == 2)
73  ROS_ERROR("Cannot specify position limits for orientation of planar joint '%s'", jmodel->getName().c_str());
74  else if (jmodel->getType() == robot_model::JointModel::FLOATING && index > 2)
75  ROS_ERROR("Cannot specify position limits for orientation of floating joint '%s'", jmodel->getName().c_str());
76  else if (jmodel->getType() == robot_model::JointModel::REVOLUTE &&
77  static_cast<const robot_model::RevoluteJointModel*>(jmodel)->isContinuous())
78  ROS_ERROR("Cannot specify position limits for continuous joint '%s'", jmodel->getName().c_str());
79  else
80  ok = true;
81  return ok;
82 }
83 }
84 
86 {
88  moveit::tools::Profiler::ScopedBlock prof_block("RobotModelLoader::configure");
89 
91  if (opt.urdf_doc_ && opt.srdf_doc_)
93  else if (!opt.urdf_string_.empty() && !opt.srdf_string_.empty())
95  else
97  if (rdf_loader_->getURDF())
98  {
99  const srdf::ModelSharedPtr& srdf =
100  rdf_loader_->getSRDF() ? rdf_loader_->getSRDF() : srdf::ModelSharedPtr(new srdf::Model());
101  model_.reset(new robot_model::RobotModel(rdf_loader_->getURDF(), srdf));
102  }
103 
104  if (model_ && !rdf_loader_->getRobotDescription().empty())
105  {
106  moveit::tools::Profiler::ScopedBlock prof_block2("RobotModelLoader::configure joint limits");
107 
108  // if there are additional joint limits specified in some .yaml file, read those in
109  ros::NodeHandle nh("~");
110 
111  for (std::size_t i = 0; i < model_->getJointModels().size(); ++i)
112  {
113  robot_model::JointModel* jmodel = model_->getJointModels()[i];
114  std::vector<moveit_msgs::JointLimits> jlim = jmodel->getVariableBoundsMsg();
115  for (std::size_t j = 0; j < jlim.size(); ++j)
116  {
117  std::string prefix = rdf_loader_->getRobotDescription() + "_planning/joint_limits/" + jlim[j].joint_name + "/";
118 
119  double max_position;
120  if (nh.getParam(prefix + "max_position", max_position))
121  {
122  if (canSpecifyPosition(jmodel, j))
123  {
124  jlim[j].has_position_limits = true;
125  jlim[j].max_position = max_position;
126  }
127  }
128  double min_position;
129  if (nh.getParam(prefix + "min_position", min_position))
130  {
131  if (canSpecifyPosition(jmodel, j))
132  {
133  jlim[j].has_position_limits = true;
134  jlim[j].min_position = min_position;
135  }
136  }
137  double max_velocity;
138  if (nh.getParam(prefix + "max_velocity", max_velocity))
139  {
140  jlim[j].has_velocity_limits = true;
141  jlim[j].max_velocity = max_velocity;
142  }
143  bool has_vel_limits;
144  if (nh.getParam(prefix + "has_velocity_limits", has_vel_limits))
145  jlim[j].has_velocity_limits = has_vel_limits;
146 
147  double max_acc;
148  if (nh.getParam(prefix + "max_acceleration", max_acc))
149  {
150  jlim[j].has_acceleration_limits = true;
151  jlim[j].max_acceleration = max_acc;
152  }
153  bool has_acc_limits;
154  if (nh.getParam(prefix + "has_acceleration_limits", has_acc_limits))
155  jlim[j].has_acceleration_limits = has_acc_limits;
156  }
157  jmodel->setVariableBounds(jlim);
158  }
159  }
160 
161  if (model_ && opt.load_kinematics_solvers_)
163 
164  ROS_DEBUG_STREAM_NAMED("robot_model_loader", "Loaded kinematic model in " << (ros::WallTime::now() - start).toSec()
165  << " seconds");
166 }
167 
169  const kinematics_plugin_loader::KinematicsPluginLoaderPtr& kloader)
170 {
172  moveit::tools::Profiler::ScopedBlock prof_block("RobotModelLoader::loadKinematicsSolvers");
173 
174  if (rdf_loader_ && model_)
175  {
176  // load the kinematics solvers
177  if (kloader)
178  kinematics_loader_ = kloader;
179  else
180  kinematics_loader_.reset(
182  robot_model::SolverAllocatorFn kinematics_allocator = kinematics_loader_->getLoaderFunction(rdf_loader_->getSRDF());
183  const std::vector<std::string>& groups = kinematics_loader_->getKnownGroups();
184  std::stringstream ss;
185  std::copy(groups.begin(), groups.end(), std::ostream_iterator<std::string>(ss, " "));
186  ROS_DEBUG_STREAM("Loaded information about the following groups: '" << ss.str() << "'");
187 
188  std::map<std::string, robot_model::SolverAllocatorFn> imap;
189  for (std::size_t i = 0; i < groups.size(); ++i)
190  {
191  // Check if a group in kinematics.yaml exists in the srdf
192  if (!model_->hasJointModelGroup(groups[i]))
193  continue;
194 
195  const robot_model::JointModelGroup* jmg = model_->getJointModelGroup(groups[i]);
196 
197  kinematics::KinematicsBasePtr solver = kinematics_allocator(jmg);
198  if (solver)
199  {
200  std::string error_msg;
201  if (solver->supportsGroup(jmg, &error_msg))
202  {
203  imap[groups[i]] = kinematics_allocator;
204  }
205  else
206  {
207  ROS_ERROR("Kinematics solver %s does not support joint group %s. Error: %s", typeid(*solver).name(),
208  groups[i].c_str(), error_msg.c_str());
209  }
210  }
211  else
212  {
213  ROS_ERROR("Kinematics solver could not be instantiated for joint group %s.", groups[i].c_str());
214  }
215  }
216  model_->setKinematicsAllocators(imap);
217 
218  // set the default IK timeouts
219  const std::map<std::string, double>& timeout = kinematics_loader_->getIKTimeout();
220  for (std::map<std::string, double>::const_iterator it = timeout.begin(); it != timeout.end(); ++it)
221  {
222  if (!model_->hasJointModelGroup(it->first))
223  continue;
224  robot_model::JointModelGroup* jmg = model_->getJointModelGroup(it->first);
225  jmg->setDefaultIKTimeout(it->second);
226  }
227 
228  // set the default IK attempts
229  const std::map<std::string, unsigned int>& attempts = kinematics_loader_->getIKAttempts();
230  for (std::map<std::string, unsigned int>::const_iterator it = attempts.begin(); it != attempts.end(); ++it)
231  {
232  if (!model_->hasJointModelGroup(it->first))
233  continue;
234  robot_model::JointModelGroup* jmg = model_->getJointModelGroup(it->first);
235  jmg->setDefaultIKAttempts(it->second);
236  }
237  }
238 }
void loadKinematicsSolvers(const kinematics_plugin_loader::KinematicsPluginLoaderPtr &kloader=kinematics_plugin_loader::KinematicsPluginLoaderPtr())
Load the kinematics solvers into the kinematic model. This is done by default, unless disabled explic...
#define ROS_DEBUG_STREAM_NAMED(name, args)
TiXmlDocument * urdf_doc_
The parsed XML content of the URDF and SRDF documents.
Default constructor.
Definition: rdf_loader.h:53
kinematics_plugin_loader::KinematicsPluginLoaderPtr kinematics_loader_
std::string robot_description_
The string name corresponding to the ROS param where the URDF is loaded; Using the same parameter nam...
bool load_kinematics_solvers_
Flag indicating whether the kinematics solvers should be loaded as well, using specified ROS paramete...
unsigned int index
Structure that encodes the options to be passed to the RobotModelLoader constructor.
boost::shared_ptr< Model > ModelSharedPtr
#define ROS_DEBUG_STREAM(args)
RobotModelLoader(const Options &opt=Options())
Default constructor.
static WallTime now()
bool getParam(const std::string &key, std::string &s) const
std::string urdf_string_
The string content of the URDF and SRDF documents. Loading from string is attempted only if loading f...
#define ROS_ERROR(...)
Helper class for loading kinematics solvers.


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Mon Jan 15 2018 03:51:18