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",
164  "Loaded kinematic model in " << (ros::WallTime::now() - start).toSec() << " 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
179  robot_model::SolverAllocatorFn kinematics_allocator = kinematics_loader_->getLoaderFunction(rdf_loader_->getSRDF());
180  const std::vector<std::string>& groups = kinematics_loader_->getKnownGroups();
181  std::stringstream ss;
182  std::copy(groups.begin(), groups.end(), std::ostream_iterator<std::string>(ss, " "));
183  ROS_DEBUG_STREAM("Loaded information about the following groups: '" << ss.str() << "'");
184  if (groups.empty() && !model_->getJointModelGroups().empty())
185  ROS_WARN("No kinematics plugins defined. Fill and load kinematics.yaml!");
186 
187  std::map<std::string, robot_model::SolverAllocatorFn> imap;
188  for (std::size_t i = 0; i < groups.size(); ++i)
189  {
190  // Check if a group in kinematics.yaml exists in the srdf
191  if (!model_->hasJointModelGroup(groups[i]))
192  continue;
193 
194  const robot_model::JointModelGroup* jmg = model_->getJointModelGroup(groups[i]);
195 
196  kinematics::KinematicsBasePtr solver = kinematics_allocator(jmg);
197  if (solver)
198  {
199  std::string error_msg;
200  if (solver->supportsGroup(jmg, &error_msg))
201  {
202  imap[groups[i]] = kinematics_allocator;
203  }
204  else
205  {
206  ROS_ERROR("Kinematics solver %s does not support joint group %s. Error: %s", typeid(*solver).name(),
207  groups[i].c_str(), error_msg.c_str());
208  }
209  }
210  else
211  {
212  ROS_ERROR("Kinematics solver could not be instantiated for joint group %s.", groups[i].c_str());
213  }
214  }
215  model_->setKinematicsAllocators(imap);
216 
217  // set the default IK timeouts
218  const std::map<std::string, double>& timeout = kinematics_loader_->getIKTimeout();
219  for (std::map<std::string, double>::const_iterator it = timeout.begin(); it != timeout.end(); ++it)
220  {
221  if (!model_->hasJointModelGroup(it->first))
222  continue;
223  robot_model::JointModelGroup* jmg = model_->getJointModelGroup(it->first);
224  jmg->setDefaultIKTimeout(it->second);
225  }
226  }
227 }
228 } // 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 Thu Jan 14 2021 03:59:22