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 namespace robot_model_loader
43 {
44 RobotModelLoader::RobotModelLoader(const std::string& robot_description, bool load_kinematics_solvers)
45 {
46  Options opt(robot_description);
47  opt.load_kinematics_solvers_ = load_kinematics_solvers;
48  configure(opt);
49 }
50 
52 {
53  configure(opt);
54 }
55 
57 {
58  // Make sure we destroy the robot model first. It contains the loaded
59  // kinematics plugins, and those must be destroyed before the pluginlib class
60  // that implements them is destroyed (that happens when kinematics_loader_ is
61  // destroyed below). This is a workaround - ideally pluginlib would handle
62  // this better.
63  model_.reset();
64  rdf_loader_.reset();
65  kinematics_loader_.reset();
66 }
67 
68 namespace
69 {
70 bool canSpecifyPosition(const robot_model::JointModel* jmodel, const unsigned int index)
71 {
72  bool ok = false;
73  if (jmodel->getType() == robot_model::JointModel::PLANAR && index == 2)
74  ROS_ERROR("Cannot specify position limits for orientation of planar joint '%s'", jmodel->getName().c_str());
75  else if (jmodel->getType() == robot_model::JointModel::FLOATING && index > 2)
76  ROS_ERROR("Cannot specify position limits for orientation of floating joint '%s'", jmodel->getName().c_str());
77  else if (jmodel->getType() == robot_model::JointModel::REVOLUTE &&
78  static_cast<const robot_model::RevoluteJointModel*>(jmodel)->isContinuous())
79  ROS_ERROR("Cannot specify position limits for continuous joint '%s'", jmodel->getName().c_str());
80  else
81  ok = true;
82  return ok;
83 }
84 } // namespace
85 
87 {
89  moveit::tools::Profiler::ScopedBlock prof_block("RobotModelLoader::configure");
90 
92  if (!opt.urdf_string_.empty() && !opt.srdf_string_.empty())
94  else
96  if (rdf_loader_->getURDF())
97  {
98  const srdf::ModelSharedPtr& srdf =
99  rdf_loader_->getSRDF() ? rdf_loader_->getSRDF() : srdf::ModelSharedPtr(new srdf::Model());
100  model_.reset(new robot_model::RobotModel(rdf_loader_->getURDF(), srdf));
101  }
102 
103  if (model_ && !rdf_loader_->getRobotDescription().empty())
104  {
105  moveit::tools::Profiler::ScopedBlock prof_block2("RobotModelLoader::configure joint limits");
106 
107  // if there are additional joint limits specified in some .yaml file, read those in
108  ros::NodeHandle nh("~");
109 
110  for (std::size_t i = 0; i < model_->getJointModels().size(); ++i)
111  {
112  robot_model::JointModel* jmodel = model_->getJointModels()[i];
113  std::vector<moveit_msgs::JointLimits> jlim = jmodel->getVariableBoundsMsg();
114  for (std::size_t j = 0; j < jlim.size(); ++j)
115  {
116  std::string prefix = rdf_loader_->getRobotDescription() + "_planning/joint_limits/" + jlim[j].joint_name + "/";
117 
118  double max_position;
119  if (nh.getParam(prefix + "max_position", max_position))
120  {
121  if (canSpecifyPosition(jmodel, j))
122  {
123  jlim[j].has_position_limits = true;
124  jlim[j].max_position = max_position;
125  }
126  }
127  double min_position;
128  if (nh.getParam(prefix + "min_position", min_position))
129  {
130  if (canSpecifyPosition(jmodel, j))
131  {
132  jlim[j].has_position_limits = true;
133  jlim[j].min_position = min_position;
134  }
135  }
136  double max_velocity;
137  if (nh.getParam(prefix + "max_velocity", max_velocity))
138  {
139  jlim[j].has_velocity_limits = true;
140  jlim[j].max_velocity = max_velocity;
141  }
142  bool has_vel_limits;
143  if (nh.getParam(prefix + "has_velocity_limits", has_vel_limits))
144  jlim[j].has_velocity_limits = has_vel_limits;
145 
146  double max_acc;
147  if (nh.getParam(prefix + "max_acceleration", max_acc))
148  {
149  jlim[j].has_acceleration_limits = true;
150  jlim[j].max_acceleration = max_acc;
151  }
152  bool has_acc_limits;
153  if (nh.getParam(prefix + "has_acceleration_limits", has_acc_limits))
154  jlim[j].has_acceleration_limits = has_acc_limits;
155  }
156  jmodel->setVariableBounds(jlim);
157  }
158  }
159 
160  if (model_ && opt.load_kinematics_solvers_)
162 
163  ROS_DEBUG_STREAM_NAMED("robot_model_loader", "Loaded kinematic model in " << (ros::WallTime::now() - start).toSec()
164  << " seconds");
165 }
166 
167 void RobotModelLoader::loadKinematicsSolvers(const kinematics_plugin_loader::KinematicsPluginLoaderPtr& kloader)
168 {
170  moveit::tools::Profiler::ScopedBlock prof_block("RobotModelLoader::loadKinematicsSolvers");
171 
172  if (rdf_loader_ && model_)
173  {
174  // load the kinematics solvers
175  if (kloader)
176  kinematics_loader_ = kloader;
177  else
178  kinematics_loader_.reset(
180  robot_model::SolverAllocatorFn kinematics_allocator = kinematics_loader_->getLoaderFunction(rdf_loader_->getSRDF());
181  const std::vector<std::string>& groups = kinematics_loader_->getKnownGroups();
182  std::stringstream ss;
183  std::copy(groups.begin(), groups.end(), std::ostream_iterator<std::string>(ss, " "));
184  ROS_DEBUG_STREAM("Loaded information about the following groups: '" << ss.str() << "'");
185  if (groups.empty() && !model_->getJointModelGroups().empty())
186  ROS_WARN("No kinematics plugins defined. Fill and load kinematics.yaml!");
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 }
229 } // namespace robot_model_loader
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)
ROSCPP_DECL void start()
Default constructor.
Definition: rdf_loader.h:51
#define ROS_ERROR(...)
#define ROS_WARN(...)
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.
bool getParam(const std::string &key, std::string &s) const
ROSCPP_DECL bool ok()
#define ROS_DEBUG_STREAM(args)
RobotModelLoader(const Options &opt=Options())
Default constructor.
std::shared_ptr< Model > ModelSharedPtr
static WallTime now()
std::string urdf_string_
The string content of the URDF and SRDF documents. Loading from string is attempted only if loading f...
Helper class for loading kinematics solvers.


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Sun Apr 14 2019 02:52:59