From f009ddca770faadc211d18aca83fc8c7d2b95480 Mon Sep 17 00:00:00 2001 From: Scott Chow <11098209+scchow@users.noreply.github.com> Date: Tue, 28 Feb 2023 08:56:05 -0800 Subject: [PATCH] Backporting joint properties from MoveIt2 (#109) This is a backport of #77 --- include/srdfdom/model.h | 43 +++++++++- include/srdfdom/srdf_writer.h | 8 ++ src/model.cpp | 100 ++++++++++++++-------- src/srdf_writer.cpp | 27 ++++++ test/resources/pr2_desc.3-normalized.srdf | 2 + test/resources/pr2_desc.3.srdf | 5 ++ test/test_parser.cpp | 14 +++ 7 files changed, 163 insertions(+), 36 deletions(-) diff --git a/include/srdfdom/model.h b/include/srdfdom/model.h index 6f99a7a..d95f44e 100644 --- a/include/srdfdom/model.h +++ b/include/srdfdom/model.h @@ -95,7 +95,7 @@ class Model /// be added to the group. Each chain is specified as a /// pair of base link and tip link. It is checked that the /// chain is indeed a chain in the specified URDF. - std::vector > chains_; + std::vector> chains_; /// It is sometimes convenient to refer to the content of /// another group. A group can include the content of the @@ -150,7 +150,7 @@ class Model /// The values of joints for this state. Each joint can have a value. We use a vector for the 'value' to support /// multi-DOF joints - std::map > joint_values_; + std::map> joint_values_; }; /// The definition of a sphere @@ -201,6 +201,19 @@ class Model return name_; } + // Some joints may have additional properties. + struct JointProperty + { + /// The name of the joint that this property belongs to + std::string joint_name_; + + /// The name of the property + std::string property_name_; + + /// The value of the property. Type not specified. + std::string value_; + }; + /// Get the list of links that should have collision checking disabled by default (and only selectively enabled) const std::vector& getNoDefaultCollisionLinks() const { @@ -255,10 +268,31 @@ class Model return link_sphere_approximations_; } + /// Get the joint properties for a particular joint (empty vector if none) + const std::vector& getJointProperties(const std::string& joint_name) const + { + std::map>::const_iterator iter = joint_properties_.find(joint_name); + if (iter == joint_properties_.end()) + { + // We return a standard empty vector here rather than insert a new empty vector + // into the map in order to keep the method const + return empty_vector_; + } + return iter->second; + } + + /// Get the joint properties list + const std::map>& getJointProperties() const + { + return joint_properties_; + } + /// Clear the model void clear(); private: + bool isValidJoint(const urdf::ModelInterface& urdf_model, const std::string& name) const; + void loadVirtualJoints(const urdf::ModelInterface& urdf_model, tinyxml2::XMLElement* robot_xml); void loadGroups(const urdf::ModelInterface& urdf_model, tinyxml2::XMLElement* robot_xml); void loadGroupStates(const urdf::ModelInterface& urdf_model, tinyxml2::XMLElement* robot_xml); @@ -268,6 +302,7 @@ class Model void loadCollisionPairs(const urdf::ModelInterface& urdf_model, tinyxml2::XMLElement* robot_xml, const char* tag_name, std::vector& pairs); void loadPassiveJoints(const urdf::ModelInterface& urdf_model, tinyxml2::XMLElement* robot_xml); + void loadJointProperties(const urdf::ModelInterface& urdf_model, tinyxml2::XMLElement* robot_xml); std::string name_; std::vector groups_; @@ -279,6 +314,10 @@ class Model std::vector enabled_collision_pairs_; std::vector disabled_collision_pairs_; std::vector passive_joints_; + std::map> joint_properties_; + + // Empty joint property vector + static const std::vector empty_vector_; }; typedef std::shared_ptr ModelSharedPtr; typedef std::shared_ptr ModelConstSharedPtr; diff --git a/include/srdfdom/srdf_writer.h b/include/srdfdom/srdf_writer.h index b02711b..90ad43f 100644 --- a/include/srdfdom/srdf_writer.h +++ b/include/srdfdom/srdf_writer.h @@ -167,6 +167,13 @@ class SRDFWriter */ void createPassiveJointsXML(tinyxml2::XMLElement* root); + /** + * Generate XML for SRDF joint properties + * + * @param root - TinyXML root element to attach sub elements to + */ + void createJointPropertiesXML(tinyxml2::XMLElement* root); + protected: /** * Generate XML for SRDF disabled/enabled collisions of robot link pairs @@ -190,6 +197,7 @@ class SRDFWriter std::vector disabled_collision_pairs_; std::vector enabled_collision_pairs_; std::vector passive_joints_; + std::map> joint_properties_; // Store the SRDF Model for updating the kinematic_model ModelSharedPtr srdf_model_; diff --git a/src/model.cpp b/src/model.cpp index 9b1b27f..8d587f4 100644 --- a/src/model.cpp +++ b/src/model.cpp @@ -45,6 +45,24 @@ using namespace tinyxml2; +const std::vector srdf::Model::empty_vector_; + +bool srdf::Model::isValidJoint(const urdf::ModelInterface& urdf_model, const std::string& name) const +{ + if (urdf_model.getJoint(name)) + { + return true; + } + for (const srdf::Model::VirtualJoint& vj : virtual_joints_) + { + if (vj.name_ == name) + { + return true; + } + } + return false; +} + void srdf::Model::loadVirtualJoints(const urdf::ModelInterface& urdf_model, XMLElement* robot_xml) { for (XMLElement* vj_xml = robot_xml->FirstChildElement("virtual_joint"); vj_xml; @@ -145,20 +163,10 @@ void srdf::Model::loadGroups(const urdf::ModelInterface& urdf_model, XMLElement* continue; } std::string jname_str = boost::trim_copy(std::string(jname)); - if (!urdf_model.getJoint(jname_str)) + if (!isValidJoint(urdf_model, jname_str)) { - bool missing = true; - for (std::size_t k = 0; k < virtual_joints_.size(); ++k) - if (virtual_joints_[k].name_ == jname_str) - { - missing = false; - break; - } - if (missing) - { - CONSOLE_BRIDGE_logError("Joint '%s' declared as part of group '%s' is not known to the URDF", jname, gname); - continue; - } + CONSOLE_BRIDGE_logError("Joint '%s' declared as part of group '%s' is not known to the URDF", jname, gname); + continue; } g.joints_.push_back(jname_str); } @@ -333,21 +341,11 @@ void srdf::Model::loadGroupStates(const urdf::ModelInterface& urdf_model, XMLEle continue; } std::string jname_str = boost::trim_copy(std::string(jname)); - if (!urdf_model.getJoint(jname_str)) + if (!isValidJoint(urdf_model, jname_str)) { - bool missing = true; - for (std::size_t k = 0; k < virtual_joints_.size(); ++k) - if (virtual_joints_[k].name_ == jname_str) - { - missing = false; - break; - } - if (missing) - { - CONSOLE_BRIDGE_logError("Joint '%s' declared as part of group state '%s' is not known to the URDF", jname, - sname); - continue; - } + CONSOLE_BRIDGE_logError("Joint '%s' declared as part of group state '%s' is not known to the URDF", jname, + sname); + continue; } try { @@ -597,13 +595,7 @@ void srdf::Model::loadPassiveJoints(const urdf::ModelInterface& urdf_model, XMLE PassiveJoint pj; pj.name_ = boost::trim_copy(std::string(name)); - // see if a virtual joint was marked as passive - bool vjoint = false; - for (std::size_t i = 0; !vjoint && i < virtual_joints_.size(); ++i) - if (virtual_joints_[i].name_ == pj.name_) - vjoint = true; - - if (!vjoint && !urdf_model.getJoint(pj.name_)) + if (!isValidJoint(urdf_model, pj.name_)) { CONSOLE_BRIDGE_logError("Joint '%s' marked as passive is not known to the URDF. Ignoring.", name); continue; @@ -612,6 +604,45 @@ void srdf::Model::loadPassiveJoints(const urdf::ModelInterface& urdf_model, XMLE } } +void srdf::Model::loadJointProperties(const urdf::ModelInterface& urdf_model, XMLElement* robot_xml) +{ + for (XMLElement* prop_xml = robot_xml->FirstChildElement("joint_property"); prop_xml; + prop_xml = prop_xml->NextSiblingElement("joint_property")) + { + const char* jname = prop_xml->Attribute("joint_name"); + const char* pname = prop_xml->Attribute("property_name"); + const char* pval = prop_xml->Attribute("value"); + if (!jname) + { + CONSOLE_BRIDGE_logError("joint_property is missing a joint name"); + continue; + } + if (!pname) + { + CONSOLE_BRIDGE_logError("Property name for joint '%s' is not specified", jname); + continue; + } + if (!pval) + { + CONSOLE_BRIDGE_logError("Value is not specified for property '%s' of joint '%s'", pname, jname); + continue; + } + + JointProperty jp; + jp.joint_name_ = boost::trim_copy(std::string(jname)); + jp.property_name_ = boost::trim_copy(std::string(pname)); + jp.value_ = std::string(pval); + + if (!isValidJoint(urdf_model, jp.joint_name_)) + { + CONSOLE_BRIDGE_logError("Property defined for a joint '%s' that is not known to the URDF. Ignoring.", + jp.joint_name_.c_str()); + continue; + } + joint_properties_[jp.joint_name_].push_back(jp); + } +} + bool srdf::Model::initXml(const urdf::ModelInterface& urdf_model, XMLElement* robot_xml) { clear(); @@ -642,6 +673,7 @@ bool srdf::Model::initXml(const urdf::ModelInterface& urdf_model, XMLElement* ro loadCollisionPairs(urdf_model, robot_xml, "enable_collisions", enabled_collision_pairs_); loadCollisionPairs(urdf_model, robot_xml, "disable_collisions", disabled_collision_pairs_); loadPassiveJoints(urdf_model, robot_xml); + loadJointProperties(urdf_model, robot_xml); return true; } diff --git a/src/srdf_writer.cpp b/src/srdf_writer.cpp index e907719..d24787d 100644 --- a/src/srdf_writer.cpp +++ b/src/srdf_writer.cpp @@ -107,6 +107,7 @@ void SRDFWriter::initModel(const urdf::ModelInterface& robot_model, const srdf:: end_effectors_ = srdf_model_->getEndEffectors(); group_states_ = srdf_model_->getGroupStates(); passive_joints_ = srdf_model_->getPassiveJoints(); + joint_properties_ = srdf_model_->getJointProperties(); // Copy the robot name b/c the root xml element requires this attribute robot_name_ = robot_model.getName(); @@ -193,6 +194,9 @@ void SRDFWriter::generateSRDF(XMLDocument& document) // Add Passive Joints createPassiveJointsXML(robot_root); + // Add Joint Properties + createJointPropertiesXML(robot_root); + // Add Link Sphere approximations createLinkSphereApproximationsXML(robot_root); @@ -498,4 +502,27 @@ void SRDFWriter::createPassiveJointsXML(XMLElement* root) root->InsertEndChild(p_joint); } } + +void SRDFWriter::createJointPropertiesXML(tinyxml2::XMLElement* root) +{ + XMLDocument* doc = root->GetDocument(); + + if (!joint_properties_.empty()) + { + XMLComment* comment = doc->NewComment( + "JOINT PROPERTIES: Purpose: Define a property for a particular joint (could be a virtual joint)"); + root->InsertEndChild(comment); + } + for (const auto& joint_properties : joint_properties_) + { + for (const auto& joint_property : joint_properties.second) + { + XMLElement* p_joint = doc->NewElement("joint_property"); + p_joint->SetAttribute("joint_name", joint_property.joint_name_.c_str()); + p_joint->SetAttribute("property_name", joint_property.property_name_.c_str()); + p_joint->SetAttribute("value", joint_property.value_.c_str()); + root->InsertEndChild(p_joint); + } + } +} } // namespace srdf diff --git a/test/resources/pr2_desc.3-normalized.srdf b/test/resources/pr2_desc.3-normalized.srdf index d2e7260..60d9dec 100644 --- a/test/resources/pr2_desc.3-normalized.srdf +++ b/test/resources/pr2_desc.3-normalized.srdf @@ -63,6 +63,8 @@ + + diff --git a/test/resources/pr2_desc.3.srdf b/test/resources/pr2_desc.3.srdf index 0a6dcb9..6aa7a4b 100644 --- a/test/resources/pr2_desc.3.srdf +++ b/test/resources/pr2_desc.3.srdf @@ -69,4 +69,9 @@ + + + + + diff --git a/test/test_parser.cpp b/test/test_parser.cpp index 045d1f5..2e7344c 100644 --- a/test/test_parser.cpp +++ b/test/test_parser.cpp @@ -177,6 +177,20 @@ TEST(TestCpp, testComplex) EXPECT_TRUE(s.getEndEffectors()[index].name_ == "r_end_effector"); EXPECT_TRUE(s.getEndEffectors()[index].component_group_ == "r_end_effector"); EXPECT_TRUE(s.getEndEffectors()[index].parent_link_ == "r_wrist_roll_link"); + + // Joint Properties + const std::vector& gripper_props = s.getJointProperties("r_gripper_tool_joint"); + EXPECT_EQ(gripper_props.size(), 0u); + + // When parsing, this made up joint that is not present in the URDF is expected to print an error + // AND the property should not be made available in the srdf::Model + const std::vector& made_up_props = s.getJointProperties("made_up_joint"); + EXPECT_EQ(made_up_props.size(), 0u); + + const std::vector& world_props = s.getJointProperties("world_joint"); + ASSERT_EQ(world_props.size(), 1u); + EXPECT_EQ(world_props[0].property_name_, "angular_distance_weight"); + EXPECT_EQ(world_props[0].value_, "0.5"); } TEST(TestCpp, testReadWrite)