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>
50 const std::string
LOGNAME =
"robot_model_builder";
62 std::string urdf_path;
63 if (robot_name ==
"pr2")
70 ros::package::getPath(
"moveit_resources_" + robot_name +
"_description") +
"/urdf/" + robot_name +
".urdf";
72 urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDFFile(urdf_path);
73 if (urdf_model ==
nullptr)
75 ROS_ERROR_NAMED(
LOGNAME,
"Cannot find URDF for %s. Make sure moveit_resources/your robot description is installed",
85 std::string srdf_path;
86 if (robot_name ==
"pr2")
93 ros::package::getPath(
"moveit_resources_" + robot_name +
"_moveit_config") +
"/config/" + robot_name +
".srdf";
95 srdf_model->initFile(*urdf_model, srdf_path);
100 : urdf_model_(new
urdf::ModelInterface()), srdf_writer_(new
srdf::SRDFWriter())
102 urdf_model_->clear();
103 urdf_model_->name_ =
name;
105 urdf::LinkSharedPtr base_link(
new urdf::Link);
106 base_link->name = base_link_name;
107 urdf_model_->links_.insert(std::make_pair(base_link_name, base_link));
109 srdf_writer_->robot_name_ =
name;
113 const std::vector<geometry_msgs::Pose>& joint_origins)
115 std::vector<std::string> link_names;
116 boost::split_regex(link_names, section, boost::regex(
"->"));
117 if (link_names.empty())
131 if (!joint_origins.empty() && link_names.size() - 1 != joint_origins.size())
134 link_names.size(), joint_origins.size());
140 for (
size_t i = 1; i < link_names.size(); ++i)
149 urdf::LinkSharedPtr link(
new urdf::Link);
150 link->name = link_names[i];
151 urdf_model_->links_.insert(std::make_pair(link_names[i], link));
152 urdf::JointSharedPtr joint(
new urdf::Joint);
153 joint->name = link_names[i - 1] +
"-" + link_names[i] +
"-joint";
155 joint->parent_to_joint_origin_transform.clear();
156 if (!joint_origins.empty())
158 geometry_msgs::Pose o = joint_origins[i - 1];
159 joint->parent_to_joint_origin_transform.position = urdf::Vector3(o.position.x, o.position.y, o.position.z);
160 joint->parent_to_joint_origin_transform.rotation =
161 urdf::Rotation(o.orientation.x, o.orientation.y, o.orientation.z, o.orientation.w);
164 joint->parent_link_name = link_names[i - 1];
165 joint->child_link_name = link_names[i];
166 if (type ==
"planar")
167 joint->type = urdf::Joint::PLANAR;
168 else if (type ==
"floating")
169 joint->type = urdf::Joint::FLOATING;
170 else if (type ==
"revolute")
171 joint->type = urdf::Joint::REVOLUTE;
172 else if (type ==
"continuous")
173 joint->type = urdf::Joint::CONTINUOUS;
174 else if (type ==
"prismatic")
175 joint->type = urdf::Joint::PRISMATIC;
176 else if (type ==
"fixed")
177 joint->type = urdf::Joint::FIXED;
185 joint->axis = urdf::Vector3(1.0, 0.0, 0.0);
186 if (joint->type == urdf::Joint::REVOLUTE || joint->type == urdf::Joint::PRISMATIC)
188 urdf::JointLimitsSharedPtr limits(
new urdf::JointLimits);
189 limits->lower = -boost::math::constants::pi<double>();
190 limits->upper = boost::math::constants::pi<double>();
192 joint->limits = limits;
194 urdf_model_->joints_.insert(std::make_pair(joint->name, joint));
199 double ixy,
double ixz,
double iyy,
double iyz,
double izz)
208 urdf::InertialSharedPtr inertial(
new urdf::Inertial);
209 inertial->origin.position = urdf::Vector3(origin.position.x, origin.position.y, origin.position.z);
210 inertial->origin.rotation =
211 urdf::Rotation(origin.orientation.x, origin.orientation.y, origin.orientation.z, origin.orientation.w);
212 inertial->mass = mass;
220 urdf::LinkSharedPtr link;
222 link->inertial = inertial;
226 geometry_msgs::Pose origin)
228 urdf::VisualSharedPtr vis(
new urdf::Visual);
229 urdf::BoxSharedPtr geometry(
new urdf::Box);
230 geometry->dim = urdf::Vector3(size[0], size[1], size[2]);
231 vis->geometry = geometry;
236 geometry_msgs::Pose origin)
238 if (dims.size() != 3)
240 ROS_ERROR(
"There can only be 3 dimensions of a box (given %zu!)", dims.size());
244 urdf::CollisionSharedPtr coll(
new urdf::Collision);
245 urdf::BoxSharedPtr geometry(
new urdf::Box);
246 geometry->dim = urdf::Vector3(dims[0], dims[1], dims[2]);
247 coll->geometry = geometry;
252 geometry_msgs::Pose origin)
254 urdf::CollisionSharedPtr coll(
new urdf::Collision);
255 urdf::MeshSharedPtr geometry(
new urdf::Mesh);
256 geometry->filename = filename;
257 coll->geometry = geometry;
262 geometry_msgs::Pose origin)
270 collision->origin.position = urdf::Vector3(origin.position.x, origin.position.y, origin.position.z);
271 collision->origin.rotation =
272 urdf::Rotation(origin.orientation.x, origin.orientation.y, origin.orientation.z, origin.orientation.w);
274 urdf::LinkSharedPtr link;
276 link->collision_array.push_back(collision);
280 geometry_msgs::Pose origin)
288 vis->origin.position = urdf::Vector3(origin.position.x, origin.position.y, origin.position.z);
289 vis->origin.rotation =
290 urdf::Rotation(origin.orientation.x, origin.orientation.y, origin.orientation.z, origin.orientation.w);
292 urdf::LinkSharedPtr link;
294 if (!link->visual_array.empty())
296 link->visual_array.push_back(vis);
298 else if (link->visual)
300 link->visual_array.push_back(link->visual);
301 link->visual.reset();
302 link->visual_array.push_back(vis);
311 const std::string& type,
const std::string& name)
315 new_virtual_joint.
name_ = parent_frame +
"-" + child_link +
"-virtual_joint";
318 new_virtual_joint.
type_ = type;
321 srdf_writer_->virtual_joints_.push_back(new_virtual_joint);
328 new_group.
name_ = base_link +
"-" + tip_link +
"-chain-group";
331 new_group.
chains_.push_back(std::make_pair(base_link, tip_link));
336 const std::string& name)
346 const std::string& parent_group,
const std::string& component_group)
364 std::map<std::string, std::string> parent_link_tree;
365 parent_link_tree.clear();
371 catch (urdf::ParseError& e)
382 catch (urdf::ParseError& e)