37#include <ament_index_cpp/get_package_share_directory.hpp>
38#include <boost/algorithm/string_regex.hpp>
40#include <geometry_msgs/msg/pose.hpp>
42#include <rclcpp/logger.hpp>
43#include <rclcpp/logging.hpp>
44#include <urdf_parser/urdf_parser.h>
47#include <pluginlib/class_loader.hpp>
67 auto robot_model = std::make_shared<moveit::core::RobotModel>(urdf, srdf);
73 const std::string package_name =
"moveit_resources_" + robot_name +
"_description";
74 std::filesystem::path res_path(ament_index_cpp::get_package_share_directory(package_name));
75 std::string urdf_path;
76 if (robot_name ==
"pr2")
78 urdf_path = (res_path /
"urdf/robot.xml").
string();
82 urdf_path = (res_path /
"urdf" / (robot_name +
".urdf")).string();
84 urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDFFile(urdf_path);
85 if (urdf_model ==
nullptr)
87 RCLCPP_ERROR(getLogger(),
88 "Cannot find URDF for %s. Make sure moveit_resources_<your_robot_description> is installed",
97 auto srdf_model = std::make_shared<srdf::Model>();
98 std::string srdf_path;
99 if (robot_name ==
"pr2")
101 const std::string package_name =
"moveit_resources_" + robot_name +
"_description";
102 std::filesystem::path res_path(ament_index_cpp::get_package_share_directory(package_name));
103 srdf_path = (res_path /
"srdf/robot.xml").
string();
107 const std::string package_name =
"moveit_resources_" + robot_name +
"_moveit_config";
108 std::filesystem::path res_path(ament_index_cpp::get_package_share_directory(package_name));
109 srdf_path = (res_path /
"config" / (robot_name +
".srdf")).string();
111 srdf_model->initFile(*urdf_model, srdf_path);
116 const std::string& tip_link, std::string plugin,
double timeout)
118 using LoaderType = pluginlib::ClassLoader<kinematics::KinematicsBase>;
119 static std::weak_ptr<LoaderType> cached_loader;
120 std::shared_ptr<LoaderType> loader = cached_loader.lock();
123 loader = std::make_shared<LoaderType>(
"moveit_core",
"kinematics::KinematicsBase");
124 cached_loader = loader;
129 plugin =
"kdl_kinematics_plugin/KDLKinematicsPlugin";
133 kinematics::KinematicsBasePtr result = loader->createUniqueInstance(plugin);
135 result->setDefaultTimeout(timeout);
142 : urdf_model_(std::make_shared<urdf::ModelInterface>()), srdf_writer_(std::make_shared<srdf::SRDFWriter>())
144 urdf_model_->clear();
145 urdf_model_->name_ = name;
147 auto base_link = std::make_shared<urdf::Link>();
148 base_link->name = base_link_name;
149 urdf_model_->links_.insert(std::make_pair(base_link_name, base_link));
151 srdf_writer_->robot_name_ = name;
155 const std::vector<geometry_msgs::msg::Pose>& joint_origins, urdf::Vector3 joint_axis)
157 std::vector<std::string> link_names;
158 boost::split_regex(link_names, section, boost::regex(
"->"));
159 if (link_names.empty())
161 RCLCPP_ERROR(getLogger(),
"No links specified (empty section?)");
166 if (!urdf_model_->getLink(link_names[0]))
168 RCLCPP_ERROR(getLogger(),
"Link %s not present in builder yet!", link_names[0].c_str());
173 if (!joint_origins.empty() && link_names.size() - 1 != joint_origins.size())
175 RCLCPP_ERROR(getLogger(),
"There should be one more link (%zu) than there are joint origins (%zu)",
176 link_names.size(), joint_origins.size());
182 for (
size_t i = 1; i < link_names.size(); ++i)
185 if (urdf_model_->getLink(link_names[i]))
187 RCLCPP_ERROR(getLogger(),
"Link %s is already specified", link_names[i].c_str());
191 auto link = std::make_shared<urdf::Link>();
192 link->name = link_names[i];
193 urdf_model_->links_.insert(std::make_pair(link_names[i], link));
194 auto joint = std::make_shared<urdf::Joint>();
195 joint->name = link_names[i - 1] +
"-" + link_names[i] +
"-joint";
197 joint->parent_to_joint_origin_transform.clear();
198 if (!joint_origins.empty())
200 geometry_msgs::msg::Pose o = joint_origins[i - 1];
201 joint->parent_to_joint_origin_transform.position = urdf::Vector3(o.position.x, o.position.y, o.position.z);
202 joint->parent_to_joint_origin_transform.rotation =
203 urdf::Rotation(o.orientation.x, o.orientation.y, o.orientation.z, o.orientation.w);
206 joint->parent_link_name = link_names[i - 1];
207 joint->child_link_name = link_names[i];
208 if (type ==
"planar")
210 joint->type = urdf::Joint::PLANAR;
212 else if (type ==
"floating")
214 joint->type = urdf::Joint::FLOATING;
216 else if (type ==
"revolute")
218 joint->type = urdf::Joint::REVOLUTE;
220 else if (type ==
"continuous")
222 joint->type = urdf::Joint::CONTINUOUS;
224 else if (type ==
"prismatic")
226 joint->type = urdf::Joint::PRISMATIC;
228 else if (type ==
"fixed")
230 joint->type = urdf::Joint::FIXED;
234 RCLCPP_ERROR(getLogger(),
"No such joint type as %s", type.c_str());
239 joint->axis = joint_axis;
240 if (joint->type == urdf::Joint::REVOLUTE || joint->type == urdf::Joint::PRISMATIC)
242 auto limits = std::make_shared<urdf::JointLimits>();
243 limits->lower = -M_PI;
244 limits->upper = M_PI;
246 joint->limits = limits;
248 urdf_model_->joints_.insert(std::make_pair(joint->name, joint));
253 double ixx,
double ixy,
double ixz,
double iyy,
double iyz,
double izz)
255 if (!urdf_model_->getLink(link_name))
257 RCLCPP_ERROR(getLogger(),
"Link %s not present in builder yet!", link_name.c_str());
262 auto inertial = std::make_shared<urdf::Inertial>();
263 inertial->origin.position = urdf::Vector3(origin.position.x, origin.position.y, origin.position.z);
264 inertial->origin.rotation =
265 urdf::Rotation(origin.orientation.x, origin.orientation.y, origin.orientation.z, origin.orientation.w);
266 inertial->mass = mass;
274 urdf::LinkSharedPtr link;
275 urdf_model_->getLink(link_name, link);
276 link->inertial = inertial;
280 geometry_msgs::msg::Pose origin)
282 auto vis = std::make_shared<urdf::Visual>();
283 auto geometry = std::make_shared<urdf::Box>();
284 geometry->dim = urdf::Vector3(size[0], size[1], size[2]);
285 vis->geometry = geometry;
286 addLinkVisual(link_name, vis, origin);
290 geometry_msgs::msg::Pose origin)
292 if (dims.size() != 3)
294 RCLCPP_ERROR(getLogger(),
"There can only be 3 dimensions of a box (given %zu!)", dims.size());
298 auto coll = std::make_shared<urdf::Collision>();
299 auto geometry = std::make_shared<urdf::Box>();
300 geometry->dim = urdf::Vector3(dims[0], dims[1], dims[2]);
301 coll->geometry = geometry;
302 addLinkCollision(link_name, coll, origin);
306 geometry_msgs::msg::Pose origin)
308 auto coll = std::make_shared<urdf::Collision>();
309 auto geometry = std::make_shared<urdf::Mesh>();
310 geometry->filename = filename;
311 coll->geometry = geometry;
312 addLinkCollision(link_name, coll, origin);
315void RobotModelBuilder::addLinkCollision(
const std::string& link_name,
const urdf::CollisionSharedPtr& collision,
316 geometry_msgs::msg::Pose origin)
318 if (!urdf_model_->getLink(link_name))
320 RCLCPP_ERROR(getLogger(),
"Link %s not present in builder yet!", link_name.c_str());
324 collision->origin.position = urdf::Vector3(origin.position.x, origin.position.y, origin.position.z);
325 collision->origin.rotation =
326 urdf::Rotation(origin.orientation.x, origin.orientation.y, origin.orientation.z, origin.orientation.w);
328 urdf::LinkSharedPtr link;
329 urdf_model_->getLink(link_name, link);
330 link->collision_array.push_back(collision);
333void RobotModelBuilder::addLinkVisual(
const std::string& link_name,
const urdf::VisualSharedPtr& vis,
334 geometry_msgs::msg::Pose origin)
336 if (!urdf_model_->getLink(link_name))
338 RCLCPP_ERROR(getLogger(),
"Link %s not present in builder yet!", link_name.c_str());
342 vis->origin.position = urdf::Vector3(origin.position.x, origin.position.y, origin.position.z);
343 vis->origin.rotation =
344 urdf::Rotation(origin.orientation.x, origin.orientation.y, origin.orientation.z, origin.orientation.w);
346 urdf::LinkSharedPtr link;
347 urdf_model_->getLink(link_name, link);
348 if (!link->visual_array.empty())
350 link->visual_array.push_back(vis);
352 else if (link->visual)
354 link->visual_array.push_back(link->visual);
355 link->visual.reset();
356 link->visual_array.push_back(vis);
365 const std::string& type,
const std::string& name)
367 srdf::Model::VirtualJoint new_virtual_joint;
370 new_virtual_joint.name_ = parent_frame +
"-" + child_link +
"-virtual_joint";
374 new_virtual_joint.name_ = name;
376 new_virtual_joint.type_ = type;
377 new_virtual_joint.parent_frame_ = parent_frame;
378 new_virtual_joint.child_link_ = child_link;
379 srdf_writer_->virtual_joints_.push_back(new_virtual_joint);
384 srdf::Model::Group new_group;
387 new_group.name_ = base_link +
"-" + tip_link +
"-chain-group";
391 new_group.name_ = name;
393 new_group.chains_.push_back(std::make_pair(base_link, tip_link));
394 srdf_writer_->groups_.push_back(new_group);
398 const std::string& name)
400 srdf::Model::Group new_group;
401 new_group.name_ = name;
402 new_group.links_ = links;
403 new_group.joints_ = joints;
404 srdf_writer_->groups_.push_back(new_group);
408 const std::string& parent_group,
const std::string& component_group)
410 srdf::Model::EndEffector eef;
412 eef.parent_link_ = parent_link;
413 eef.parent_group_ = parent_group;
414 eef.component_group_ = component_group;
415 srdf_writer_->end_effectors_.push_back(eef);
419 const std::string& value)
421 srdf_writer_->joint_properties_[joint_name].push_back({ joint_name, property_name, value });
431 moveit::core::RobotModelPtr robot_model;
432 std::map<std::string, std::string> parent_link_tree;
433 parent_link_tree.clear();
437 urdf_model_->initTree(parent_link_tree);
439 catch (urdf::ParseError& e)
441 RCLCPP_ERROR(getLogger(),
"Failed to build tree: %s", e.what());
448 urdf_model_->initRoot(parent_link_tree);
450 catch (urdf::ParseError& e)
452 RCLCPP_ERROR(getLogger(),
"Failed to find root link: %s", e.what());
455 srdf_writer_->updateSRDFModel(*urdf_model_);
456 robot_model = std::make_shared<moveit::core::RobotModel>(urdf_model_, srdf_writer_->srdf_model_);
const RobotModel & getParentModel() const
Get the kinematic model this group is part of.
void setSolverAllocators(const SolverAllocatorFn &solver, const SolverAllocatorMapFn &solver_map=SolverAllocatorMapFn())
const std::string & getName() const
Get the name of the joint group.
void addChain(const std::string §ion, const std::string &type, const std::vector< geometry_msgs::msg::Pose > &joint_origins={}, urdf::Vector3 joint_axis=urdf::Vector3(1.0, 0.0, 0.0))
Adds a chain of links and joints to the builder. The joint names are generated automatically as "<par...
void addInertial(const std::string &link_name, double mass, geometry_msgs::msg::Pose origin, double ixx, double ixy, double ixz, double iyy, double iyz, double izz)
void addEndEffector(const std::string &name, const std::string &parent_link, const std::string &parent_group="", const std::string &component_group="")
void addCollisionBox(const std::string &link_name, const std::vector< double > &dims, geometry_msgs::msg::Pose origin)
Adds a collision box to a specific link.
void addJointProperty(const std::string &joint_name, const std::string &property_name, const std::string &value)
Adds a new joint property.
bool isValid()
Returns true if the building process so far has been valid.
void addGroup(const std::vector< std::string > &links, const std::vector< std::string > &joints, const std::string &name)
Adds a new group using a list of links and a list of joints.
moveit::core::RobotModelPtr build()
Builds and returns the robot model added to the builder.
void addVirtualJoint(const std::string &parent_frame, const std::string &child_link, const std::string &type, const std::string &name="")
Adds a virtual joint to the SRDF.
void addGroupChain(const std::string &base_link, const std::string &tip_link, const std::string &name="")
Adds a new group using a chain of links. The group is the parent joint of each link in the chain.
void addCollisionMesh(const std::string &link_name, const std::string &filename, geometry_msgs::msg::Pose origin)
Adds a collision mesh to a specific link.
RobotModelBuilder(const std::string &name, const std::string &base_link_name)
Constructor, takes the names of the robot and the base link.
void addVisualBox(const std::string &link_name, const std::vector< double > &size, geometry_msgs::msg::Pose origin)
Adds a visual box to a specific link.
rclcpp::Logger getLogger()
std::map< const JointModelGroup *, SolverAllocatorFn > SolverAllocatorMapFn
Map from group instances to allocator functions & bijections.
moveit::core::RobotModelPtr loadTestingRobotModel(const std::string &robot_name)
Loads a robot from moveit_resources.
srdf::ModelSharedPtr loadSRDFModel(const std::string &robot_name)
Loads an SRDF Model from moveit_resources.
void loadIKPluginForGroup(const rclcpp::Node::SharedPtr &node, JointModelGroup *jmg, const std::string &base_link, const std::string &tip_link, std::string plugin="KDL", double timeout=0.1)
Load an IK solver plugin for the given joint model group.
urdf::ModelInterfaceSharedPtr loadModelInterface(const std::string &robot_name)
Loads a URDF Model Interface from moveit_resources.
Main namespace for MoveIt.
rclcpp::Logger getLogger(const std::string &name)
Creates a namespaced logger.