Backporting joint properties from MoveIt2 (moveit#109)

This is a backport of moveit#77

Author: scchow

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<std::pair<std::string, std::string> > chains_;
+    std::vector<std::pair<std::string, std::string>> 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<std::string, std::vector<double> > joint_values_;
+    std::map<std::string, std::vector<double>> 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<std::string>& 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<JointProperty>& getJointProperties(const std::string& joint_name) const
+  {
+    std::map<std::string, std::vector<JointProperty>>::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<std::string, std::vector<JointProperty>>& 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<CollisionPair>& 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<Group> groups_;
@@ -279,6 +314,10 @@ class Model
   std::vector<CollisionPair> enabled_collision_pairs_;
   std::vector<CollisionPair> disabled_collision_pairs_;
   std::vector<PassiveJoint> passive_joints_;
+  std::map<std::string, std::vector<JointProperty>> joint_properties_;
+
+  // Empty joint property vector
+  static const std::vector<JointProperty> empty_vector_;
 };
 typedef std::shared_ptr<Model> ModelSharedPtr;
 typedef std::shared_ptr<const Model> ModelConstSharedPtr;

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<Model::CollisionPair> disabled_collision_pairs_;
   std::vector<Model::CollisionPair> enabled_collision_pairs_;
   std::vector<Model::PassiveJoint> passive_joints_;
+  std::map<std::string, std::vector<srdf::Model::JointProperty>> joint_properties_;
 
   // Store the SRDF Model for updating the kinematic_model
   ModelSharedPtr srdf_model_;

src/model.cpp

@@ -45,6 +45,24 @@
 
 using namespace tinyxml2;
 
+const std::vector<srdf::Model::JointProperty> 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;
 }

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

test/resources/pr2_desc.3-normalized.srdf

@@ -63,6 +63,8 @@
     <end_effector name="l_end_effector" parent_link="l_wrist_roll_link" group="l_end_effector"/>
     <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
     <virtual_joint name="world_joint" type="planar" parent_frame="odom" child_link="base_footprint"/>
+    <!--JOINT PROPERTIES: Purpose: Define a property for a particular joint (could be a virtual joint)-->
+    <joint_property joint_name="world_joint" property_name="angular_distance_weight" value="0.5"/>
     <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
     <disable_collisions link1="r_shoulder_pan_link" link2="r_shoulder_lift_link" reason="adjacent"/>
     <disable_collisions link1="r_shoulder_pan_link" link2="l_gripper_palm_link" reason=""/>

test/resources/pr2_desc.3.srdf

@@ -69,4 +69,9 @@
    <disable_collisions link1="r_shoulder_pan_link" link2="l_gripper_palm_link" />
    <!-- and many more disable_collisions tags -->
 
+  <joint_property joint_name="world_joint" property_name="angular_distance_weight" value="0.5" />
+
+   <!-- When parsing, this made up joint that is not present in the URDF is expected to print an error -->
+   <joint_property joint_name="made_up_joint" property_name="angular_distance_weight" value="0.5" />
+
 </robot>

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<srdf::Model::JointProperty>& 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<srdf::Model::JointProperty>& made_up_props = s.getJointProperties("made_up_joint");
+  EXPECT_EQ(made_up_props.size(), 0u);
+
+  const std::vector<srdf::Model::JointProperty>& 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)