38 #include <boost/algorithm/string_regex.hpp>
39 #include <boost/math/constants/constants.hpp>
40 #include <geometry_msgs/Pose.h>
42 #include <urdf_parser/urdf_parser.h>
51 const std::string
LOGNAME =
"robot_model_builder";
63 std::string urdf_path;
64 if (robot_name ==
"pr2")
71 ros::package::getPath(
"moveit_resources_" + robot_name +
"_description") +
"/urdf/" + robot_name +
".urdf";
73 urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDFFile(urdf_path);
74 if (urdf_model ==
nullptr)
76 ROS_ERROR_NAMED(
LOGNAME,
"Cannot find URDF for %s. Make sure moveit_resources/your robot description is installed",
85 auto srdf_model = std::make_shared<srdf::Model>();
86 std::string srdf_path;
87 if (robot_name ==
"pr2")
94 ros::package::getPath(
"moveit_resources_" + robot_name +
"_moveit_config") +
"/config/" + robot_name +
".srdf";
96 srdf_model->initFile(*urdf_model, srdf_path);
100 void loadIKPluginForGroup(JointModelGroup* jmg,
const std::string& base_link,
const std::string& tip_link,
101 std::string plugin,
double timeout)
104 static std::weak_ptr<LoaderType> cached_loader;
105 std::shared_ptr<LoaderType> loader = cached_loader.lock();
108 loader = std::make_shared<LoaderType>(
"moveit_core",
"kinematics::KinematicsBase");
109 cached_loader = loader;
114 plugin =
"kdl_kinematics_plugin/KDLKinematicsPlugin";
116 jmg->setSolverAllocators(
118 kinematics::KinematicsBasePtr result = loader->createUniqueInstance(plugin);
120 result->setDefaultTimeout(timeout);
127 : urdf_model_(new
urdf::ModelInterface()), srdf_writer_(new
srdf::SRDFWriter())
129 urdf_model_->clear();
130 urdf_model_->name_ =
name;
132 auto base_link = std::make_shared<urdf::Link>();
133 base_link->name = base_link_name;
134 urdf_model_->links_.insert(std::make_pair(base_link_name, base_link));
136 srdf_writer_->robot_name_ =
name;
139 RobotModelBuilder& RobotModelBuilder::addChain(
const std::string& section,
const std::string& type,
140 const std::vector<geometry_msgs::Pose>& joint_origins,
141 urdf::Vector3 joint_axis)
143 std::vector<std::string> link_names;
144 boost::split_regex(link_names, section, boost::regex(
"->"));
145 if (link_names.empty())
152 if (!urdf_model_->getLink(link_names[0]))
159 if (!joint_origins.empty() && link_names.size() - 1 != joint_origins.size())
162 link_names.size(), joint_origins.size());
168 for (
size_t i = 1; i < link_names.size(); ++i)
171 if (urdf_model_->getLink(link_names[i]))
177 auto link = std::make_shared<urdf::Link>();
178 link->name = link_names[i];
179 urdf_model_->links_.insert(std::make_pair(link_names[i], link));
180 auto joint = std::make_shared<urdf::Joint>();
181 joint->name = link_names[i - 1] +
"-" + link_names[i] +
"-joint";
183 joint->parent_to_joint_origin_transform.clear();
184 if (!joint_origins.empty())
186 geometry_msgs::Pose o = joint_origins[i - 1];
187 joint->parent_to_joint_origin_transform.position = urdf::Vector3(o.position.x, o.position.y, o.position.z);
188 joint->parent_to_joint_origin_transform.rotation =
189 urdf::Rotation(o.orientation.x, o.orientation.y, o.orientation.z, o.orientation.w);
192 joint->parent_link_name = link_names[i - 1];
193 joint->child_link_name = link_names[i];
194 if (type ==
"planar")
195 joint->type = urdf::Joint::PLANAR;
196 else if (type ==
"floating")
197 joint->type = urdf::Joint::FLOATING;
198 else if (type ==
"revolute")
199 joint->type = urdf::Joint::REVOLUTE;
200 else if (type ==
"continuous")
201 joint->type = urdf::Joint::CONTINUOUS;
202 else if (type ==
"prismatic")
203 joint->type = urdf::Joint::PRISMATIC;
204 else if (type ==
"fixed")
205 joint->type = urdf::Joint::FIXED;
213 joint->axis = joint_axis;
214 if (joint->type == urdf::Joint::REVOLUTE || joint->type == urdf::Joint::PRISMATIC)
216 auto limits = std::make_shared<urdf::JointLimits>();
217 limits->lower = -boost::math::constants::pi<double>();
218 limits->upper = boost::math::constants::pi<double>();
220 joint->limits = limits;
222 urdf_model_->joints_.insert(std::make_pair(joint->name, joint));
227 RobotModelBuilder& RobotModelBuilder::addInertial(
const std::string& link_name,
double mass, geometry_msgs::Pose origin,
228 double ixx,
double ixy,
double ixz,
double iyy,
double iyz,
231 if (!urdf_model_->getLink(link_name))
238 auto inertial = std::make_shared<urdf::Inertial>();
239 inertial->origin.position = urdf::Vector3(origin.position.x, origin.position.y, origin.position.z);
240 inertial->origin.rotation =
241 urdf::Rotation(origin.orientation.x, origin.orientation.y, origin.orientation.z, origin.orientation.w);
242 inertial->mass = mass;
250 urdf::LinkSharedPtr link;
251 urdf_model_->getLink(link_name, link);
252 link->inertial = inertial;
257 RobotModelBuilder& RobotModelBuilder::addVisualBox(
const std::string& link_name,
const std::vector<double>& size,
258 geometry_msgs::Pose origin)
260 auto vis = std::make_shared<urdf::Visual>();
261 auto geometry = std::make_shared<urdf::Box>();
262 geometry->dim = urdf::Vector3(size[0], size[1], size[2]);
263 vis->geometry = geometry;
264 addLinkVisual(link_name, vis, origin);
268 RobotModelBuilder& RobotModelBuilder::addCollisionSphere(
const std::string& link_name,
double radius,
269 geometry_msgs::Pose origin)
271 auto coll = std::make_shared<urdf::Collision>();
272 auto geometry = std::make_shared<urdf::Sphere>();
273 geometry->radius = radius;
274 coll->geometry = geometry;
275 addLinkCollision(link_name, coll, origin);
279 RobotModelBuilder& RobotModelBuilder::addCollisionBox(
const std::string& link_name,
const std::vector<double>& dims,
280 geometry_msgs::Pose origin)
282 if (dims.size() != 3)
284 ROS_ERROR(
"There can only be 3 dimensions of a box (given %zu!)", dims.size());
288 auto coll = std::make_shared<urdf::Collision>();
289 auto geometry = std::make_shared<urdf::Box>();
290 geometry->dim = urdf::Vector3(dims[0], dims[1], dims[2]);
291 coll->geometry = geometry;
292 addLinkCollision(link_name, coll, origin);
296 RobotModelBuilder& RobotModelBuilder::addCollisionMesh(
const std::string& link_name,
const std::string& filename,
297 geometry_msgs::Pose origin)
299 auto coll = std::make_shared<urdf::Collision>();
300 auto geometry = std::make_shared<urdf::Mesh>();
301 geometry->filename = filename;
302 coll->geometry = geometry;
303 addLinkCollision(link_name, coll, origin);
307 void RobotModelBuilder::addLinkCollision(
const std::string& link_name,
const urdf::CollisionSharedPtr& collision,
308 geometry_msgs::Pose origin)
310 if (!urdf_model_->getLink(link_name))
316 collision->origin.position = urdf::Vector3(origin.position.x, origin.position.y, origin.position.z);
317 collision->origin.rotation =
318 urdf::Rotation(origin.orientation.x, origin.orientation.y, origin.orientation.z, origin.orientation.w);
320 urdf::LinkSharedPtr link;
321 urdf_model_->getLink(link_name, link);
322 link->collision_array.push_back(collision);
325 void RobotModelBuilder::addLinkVisual(
const std::string& link_name,
const urdf::VisualSharedPtr& vis,
326 geometry_msgs::Pose origin)
328 if (!urdf_model_->getLink(link_name))
334 vis->origin.position = urdf::Vector3(origin.position.x, origin.position.y, origin.position.z);
335 vis->origin.rotation =
336 urdf::Rotation(origin.orientation.x, origin.orientation.y, origin.orientation.z, origin.orientation.w);
338 urdf::LinkSharedPtr link;
339 urdf_model_->getLink(link_name, link);
340 if (!link->visual_array.empty())
342 link->visual_array.push_back(vis);
344 else if (link->visual)
346 link->visual_array.push_back(link->visual);
347 link->visual.reset();
348 link->visual_array.push_back(vis);
356 RobotModelBuilder& RobotModelBuilder::addVirtualJoint(
const std::string& parent_frame,
const std::string& child_link,
357 const std::string& type,
const std::string& name)
361 new_virtual_joint.
name_ = parent_frame +
"-" + child_link +
"-virtual_joint";
364 new_virtual_joint.
type_ = type;
367 srdf_writer_->virtual_joints_.push_back(new_virtual_joint);
371 RobotModelBuilder& RobotModelBuilder::addGroupChain(
const std::string& base_link,
const std::string& tip_link,
372 const std::string& name)
376 new_group.
name_ = base_link +
"-" + tip_link +
"-chain-group";
379 new_group.
chains_.push_back(std::make_pair(base_link, tip_link));
380 srdf_writer_->groups_.push_back(new_group);
384 RobotModelBuilder& RobotModelBuilder::addGroup(
const std::vector<std::string>& links,
385 const std::vector<std::string>& joints,
const std::string& name)
391 srdf_writer_->groups_.push_back(new_group);
395 RobotModelBuilder& RobotModelBuilder::addEndEffector(
const std::string& name,
const std::string& parent_link,
396 const std::string& parent_group,
397 const std::string& component_group)
404 srdf_writer_->end_effectors_.push_back(eef);
408 void RobotModelBuilder::addJointProperty(
const std::string& joint_name,
const std::string& property_name,
409 const std::string& value)
411 srdf_writer_->joint_properties_[joint_name][property_name] = value;
414 bool RobotModelBuilder::isValid()
419 moveit::core::RobotModelPtr RobotModelBuilder::build()
422 std::map<std::string, std::string> parent_link_tree;
423 parent_link_tree.clear();
427 urdf_model_->initTree(parent_link_tree);
429 catch (urdf::ParseError& e)
438 urdf_model_->initRoot(parent_link_tree);
440 catch (urdf::ParseError& e)
445 srdf_writer_->updateSRDFModel(*urdf_model_);
446 robot_model = std::make_shared<moveit::core::RobotModel>(urdf_model_, srdf_writer_->srdf_model_);