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>
46 #include <pluginlib/class_loader.hpp>
54 static const rclcpp::Logger LOGGER = rclcpp::get_logger(
"moveit_utils.robot_model_test_utils");
60 auto robot_model = std::make_shared<moveit::core::RobotModel>(urdf, srdf);
66 const std::string
package_name =
"moveit_resources_" + robot_name +
"_description";
68 std::string urdf_path;
69 if (robot_name ==
"pr2")
71 urdf_path = (res_path /
"urdf/robot.xml").
string();
75 urdf_path = (res_path /
"urdf" / (robot_name +
".urdf")).string();
77 urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDFFile(urdf_path);
78 if (urdf_model ==
nullptr)
80 RCLCPP_ERROR(LOGGER,
"Cannot find URDF for %s. Make sure moveit_resources_<your_robot_description> is installed",
89 auto srdf_model = std::make_shared<srdf::Model>();
90 std::string srdf_path;
91 if (robot_name ==
"pr2")
93 const std::string
package_name =
"moveit_resources_" + robot_name +
"_description";
95 srdf_path = (res_path /
"srdf/robot.xml").
string();
99 const std::string
package_name =
"moveit_resources_" + robot_name +
"_moveit_config";
101 srdf_path = (res_path /
"config" / (robot_name +
".srdf")).string();
103 srdf_model->initFile(*urdf_model, srdf_path);
108 const std::string& tip_link, std::string plugin,
double timeout)
110 using LoaderType = pluginlib::ClassLoader<kinematics::KinematicsBase>;
111 static std::weak_ptr<LoaderType> cached_loader;
112 std::shared_ptr<LoaderType> loader = cached_loader.lock();
115 loader = std::make_shared<LoaderType>(
"moveit_core",
"kinematics::KinematicsBase");
116 cached_loader = loader;
121 plugin =
"kdl_kinematics_plugin/KDLKinematicsPlugin";
125 kinematics::KinematicsBasePtr result = loader->createUniqueInstance(plugin);
127 result->setDefaultTimeout(timeout);
134 : urdf_model_(std::make_shared<urdf::ModelInterface>()), srdf_writer_(std::make_shared<srdf::SRDFWriter>())
136 urdf_model_->clear();
137 urdf_model_->name_ =
name;
139 auto base_link = std::make_shared<urdf::Link>();
140 base_link->name = base_link_name;
141 urdf_model_->links_.insert(std::make_pair(base_link_name, base_link));
143 srdf_writer_->robot_name_ =
name;
147 const std::vector<geometry_msgs::msg::Pose>& joint_origins, urdf::Vector3 joint_axis)
149 std::vector<std::string> link_names;
150 boost::split_regex(link_names, section, boost::regex(
"->"));
151 if (link_names.empty())
153 RCLCPP_ERROR(LOGGER,
"No links specified (empty section?)");
158 if (!urdf_model_->getLink(link_names[0]))
160 RCLCPP_ERROR(LOGGER,
"Link %s not present in builder yet!", link_names[0].c_str());
165 if (!joint_origins.empty() && link_names.size() - 1 != joint_origins.size())
167 RCLCPP_ERROR(LOGGER,
"There should be one more link (%zu) than there are joint origins (%zu)", link_names.size(),
168 joint_origins.size());
174 for (
size_t i = 1; i < link_names.size(); ++i)
177 if (urdf_model_->getLink(link_names[i]))
179 RCLCPP_ERROR(LOGGER,
"Link %s is already specified", link_names[i].c_str());
183 auto link = std::make_shared<urdf::Link>();
184 link->name = link_names[i];
185 urdf_model_->links_.insert(std::make_pair(link_names[i], link));
186 auto joint = std::make_shared<urdf::Joint>();
187 joint->name = link_names[i - 1] +
"-" + link_names[i] +
"-joint";
189 joint->parent_to_joint_origin_transform.clear();
190 if (!joint_origins.empty())
192 geometry_msgs::msg::Pose o = joint_origins[i - 1];
193 joint->parent_to_joint_origin_transform.position = urdf::Vector3(o.position.x, o.position.y, o.position.z);
194 joint->parent_to_joint_origin_transform.rotation =
195 urdf::Rotation(o.orientation.x, o.orientation.y, o.orientation.z, o.orientation.w);
198 joint->parent_link_name = link_names[i - 1];
199 joint->child_link_name = link_names[i];
200 if (type ==
"planar")
202 joint->type = urdf::Joint::PLANAR;
204 else if (type ==
"floating")
206 joint->type = urdf::Joint::FLOATING;
208 else if (type ==
"revolute")
210 joint->type = urdf::Joint::REVOLUTE;
212 else if (type ==
"continuous")
214 joint->type = urdf::Joint::CONTINUOUS;
216 else if (type ==
"prismatic")
218 joint->type = urdf::Joint::PRISMATIC;
220 else if (type ==
"fixed")
226 RCLCPP_ERROR(LOGGER,
"No such joint type as %s", type.c_str());
231 joint->axis = joint_axis;
232 if (joint->type == urdf::Joint::REVOLUTE || joint->type == urdf::Joint::PRISMATIC)
234 auto limits = std::make_shared<urdf::JointLimits>();
235 limits->lower = -M_PI;
236 limits->upper = M_PI;
238 joint->limits = limits;
240 urdf_model_->joints_.insert(std::make_pair(joint->name, joint));
245 double ixx,
double ixy,
double ixz,
double iyy,
double iyz,
double izz)
247 if (!urdf_model_->getLink(link_name))
249 RCLCPP_ERROR(LOGGER,
"Link %s not present in builder yet!", link_name.c_str());
254 auto inertial = std::make_shared<urdf::Inertial>();
255 inertial->origin.position = urdf::Vector3(origin.position.x, origin.position.y, origin.position.z);
256 inertial->origin.rotation =
257 urdf::Rotation(origin.orientation.x, origin.orientation.y, origin.orientation.z, origin.orientation.w);
258 inertial->mass = mass;
266 urdf::LinkSharedPtr link;
267 urdf_model_->getLink(link_name, link);
268 link->inertial = inertial;
272 geometry_msgs::msg::Pose origin)
274 auto vis = std::make_shared<urdf::Visual>();
275 auto geometry = std::make_shared<urdf::Box>();
276 geometry->dim = urdf::Vector3(size[0], size[1], size[2]);
277 vis->geometry = geometry;
278 addLinkVisual(link_name, vis, origin);
282 geometry_msgs::msg::Pose origin)
284 if (dims.size() != 3)
286 RCLCPP_ERROR(LOGGER,
"There can only be 3 dimensions of a box (given %zu!)", dims.size());
290 auto coll = std::make_shared<urdf::Collision>();
291 auto geometry = std::make_shared<urdf::Box>();
292 geometry->dim = urdf::Vector3(dims[0], dims[1], dims[2]);
293 coll->geometry = geometry;
294 addLinkCollision(link_name, coll, origin);
298 geometry_msgs::msg::Pose origin)
300 auto coll = std::make_shared<urdf::Collision>();
301 auto geometry = std::make_shared<urdf::Mesh>();
302 geometry->filename = filename;
303 coll->geometry = geometry;
304 addLinkCollision(link_name, coll, origin);
307 void RobotModelBuilder::addLinkCollision(
const std::string& link_name,
const urdf::CollisionSharedPtr& collision,
308 geometry_msgs::msg::Pose origin)
310 if (!urdf_model_->getLink(link_name))
312 RCLCPP_ERROR(LOGGER,
"Link %s not present in builder yet!", link_name.c_str());
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::msg::Pose origin)
328 if (!urdf_model_->getLink(link_name))
330 RCLCPP_ERROR(LOGGER,
"Link %s not present in builder yet!", link_name.c_str());
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);
357 const std::string& type,
const std::string&
name)
359 srdf::Model::VirtualJoint new_virtual_joint;
362 new_virtual_joint.name_ = parent_frame +
"-" + child_link +
"-virtual_joint";
366 new_virtual_joint.name_ =
name;
368 new_virtual_joint.type_ = type;
369 new_virtual_joint.parent_frame_ = parent_frame;
370 new_virtual_joint.child_link_ = child_link;
371 srdf_writer_->virtual_joints_.push_back(new_virtual_joint);
376 srdf::Model::Group new_group;
379 new_group.name_ = base_link +
"-" + tip_link +
"-chain-group";
383 new_group.name_ =
name;
385 new_group.chains_.push_back(std::make_pair(base_link, tip_link));
386 srdf_writer_->groups_.push_back(new_group);
390 const std::string&
name)
392 srdf::Model::Group new_group;
393 new_group.name_ =
name;
394 new_group.links_ = links;
395 new_group.joints_ = joints;
396 srdf_writer_->groups_.push_back(new_group);
400 const std::string& parent_group,
const std::string& component_group)
402 srdf::Model::EndEffector eef;
404 eef.parent_link_ = parent_link;
405 eef.parent_group_ = parent_group;
406 eef.component_group_ = component_group;
407 srdf_writer_->end_effectors_.push_back(eef);
411 const std::string& value)
413 srdf_writer_->joint_properties_[joint_name].push_back({ joint_name, property_name, value });
423 moveit::core::RobotModelPtr robot_model;
424 std::map<std::string, std::string> parent_link_tree;
425 parent_link_tree.clear();
429 urdf_model_->initTree(parent_link_tree);
431 catch (urdf::ParseError& e)
433 RCLCPP_ERROR(LOGGER,
"Failed to build tree: %s", e.what());
440 urdf_model_->initRoot(parent_link_tree);
442 catch (urdf::ParseError& e)
444 RCLCPP_ERROR(LOGGER,
"Failed to find root link: %s", e.what());
447 srdf_writer_->updateSRDFModel(*urdf_model_);
448 robot_model = std::make_shared<moveit::core::RobotModel>(urdf_model_, srdf_writer_->srdf_model_);
const std::string & getName() const
Get the name of the joint group.
void setSolverAllocators(const SolverAllocatorFn &solver, const SolverAllocatorMapFn &solver_map=SolverAllocatorMapFn())
const RobotModel & getParentModel() const
Get the kinematic model this group is part of.
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.
def get_package_share_directory(pkg_name)
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.
urdf::ModelInterfaceSharedPtr loadModelInterface(const std::string &robot_name)
Loads a URDF Model Interface from moveit_resources.
void loadIKPluginForGroup(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.
Main namespace for MoveIt.