Merge pull request #2413 from lvjonok/feature/parse-mjcf-equality

Feature: Support for Parsing MJCF `equality/connect` Tag for Closed Chains

Author: jcarpent

CHANGELOG.md

@@ -10,6 +10,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 
 - Default visualizer can be changed with `PINOCCHIO_VIEWER` environment variable ([#2419](https://github.com/stack-of-tasks/pinocchio/pull/2419))
 - Add more Python and C++ examples related to inverse kinematics with 3d tasks ([#2428](https://github.com/stack-of-tasks/pinocchio/pull/2428))
+- Add parsing of equality/connect tag for closed-loop chains for MJCF format ([#2413](https://github.com/stack-of-tasks/pinocchio/pull/2413))
 
 ### Fixed
 - Fix linkage of Boost.Serialization on Windows ([#2400](https://github.com/stack-of-tasks/pinocchio/pull/2400))

include/pinocchio/parsers/mjcf.hpp

@@ -7,6 +7,9 @@
 
 #include "pinocchio/multibody/model.hpp"
 #include "pinocchio/parsers/urdf.hpp"
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/multibody/geometry.hpp"
+#include "pinocchio/algorithm/contact-info.hpp"
 
 namespace pinocchio
 {
@@ -75,6 +78,38 @@ namespace pinocchio
       ModelTpl<Scalar, Options, JointCollectionTpl> & model,
       const bool verbose = false);
 
+    template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
+    ModelTpl<Scalar, Options, JointCollectionTpl> & buildModelFromXML(
+      const std::string & xmlStream,
+      const typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointModel & rootJoint,
+      ModelTpl<Scalar, Options, JointCollectionTpl> & model,
+      PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) & contact_models,
+      const bool verbose = false);
+
+    // TODO: update description, buildModel with contact model
+    template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
+    ModelTpl<Scalar, Options, JointCollectionTpl> & buildModel(
+      const std::string & filename,
+      const typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointModel & rootJoint,
+      ModelTpl<Scalar, Options, JointCollectionTpl> & model,
+      PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) & contact_models,
+      const bool verbose = false);
+
+    template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
+    ModelTpl<Scalar, Options, JointCollectionTpl> & buildModelFromXML(
+      const std::string & xmlStream,
+      ModelTpl<Scalar, Options, JointCollectionTpl> & model,
+      PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) & contact_models,
+      const bool verbose = false);
+
+    // TODO: update description, buildModel with contact model
+    template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
+    ModelTpl<Scalar, Options, JointCollectionTpl> & buildModel(
+      const std::string & filename,
+      ModelTpl<Scalar, Options, JointCollectionTpl> & model,
+      PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) & contact_models,
+      const bool verbose = false);
+
     /**
      * @brief      Build The GeometryModel from a Mjcf file
      *

include/pinocchio/parsers/mjcf/mjcf-graph.hpp

@@ -8,7 +8,7 @@
 #include "pinocchio/parsers/urdf.hpp"
 #include "pinocchio/multibody/model.hpp"
 #include "pinocchio/multibody/joint/joints.hpp"
-
+#include "pinocchio/algorithm/contact-info.hpp"
 #include <boost/property_tree/xml_parser.hpp>
 #include <boost/property_tree/ptree.hpp>
 #include <boost/foreach.hpp>
@@ -21,6 +21,7 @@
 #include <limits>
 #include <iostream>
 #include <unordered_map>
+#include <map>
 
 namespace pinocchio
 {
@@ -325,6 +326,55 @@ namespace pinocchio
         void goThroughElement(const ptree & el, const MjcfGraph & currentGraph);
       };
 
+      /*
+      typedef struct mjsEquality_ {      // equality specification
+        mjsElement* element;             // element type
+        mjString* name;                  // name
+        mjtEq type;                      // constraint type
+        double data[mjNEQDATA];          // type-dependent data
+        mjtByte active;                  // is equality initially active
+        mjString* name1;                 // name of object 1
+        mjString* name2;                 // name of object 2
+        mjtNum solref[mjNREF];           // solver reference
+        mjtNum solimp[mjNIMP];           // solver impedance
+        mjString* info;                  // message appended to errors
+      } mjsEquality;
+      */
+      struct PINOCCHIO_PARSERS_DLLAPI MjcfEquality
+      {
+        typedef boost::property_tree::ptree ptree;
+
+        // Optional name of the equality constraint
+        std::string name;
+
+        // Type of the constraint: (connect for now)
+        std::string type;
+
+        // // Optional class for setting unspecified attributes
+        // std::string class;
+
+        // active: 'true' or 'false' (default: 'true')
+        // solref and solimp
+
+        // Name of the first body participating in the constraint
+        std::string body1;
+        // Name of the second body participating in the constraint (optional, default: world)
+        std::string body2;
+
+        // Coordinates of the 3D anchor point where the two bodies are connected.
+        // Specified relative to the local coordinate frame of the first body.
+        Eigen::Vector3d anchor = Eigen::Vector3d::Zero();
+
+        // TODO: implement when weld is introduced
+        // This attribute specifies the relative pose (3D position followed by 4D quaternion
+        // orientation) of body2 relative to body1. If the quaternion part (i.e., last 4 components
+        // of the vector) are all zeros, as in the default setting, this attribute is ignored and
+        // the relative pose is the one corresponding to the model reference pose in qpos0. The
+        // unusual default is because all equality constraint types share the same default for their
+        // numeric parameters.
+        // Eigen::VectorXd relativePose = Eigen::VectorXd::Zero(7);
+      };
+
       /// @brief The graph which contains all information taken from the mjcf file
       struct PINOCCHIO_PARSERS_DLLAPI MjcfGraph
       {
@@ -337,6 +387,7 @@ namespace pinocchio
         typedef std::unordered_map<std::string, MjcfMesh> MeshMap_t;
         typedef std::unordered_map<std::string, MjcfTexture> TextureMap_t;
         typedef std::unordered_map<std::string, Eigen::VectorXd> ConfigMap_t;
+        typedef std::map<std::string, MjcfEquality> EqualityMap_t;
 
         // Compiler Info needed to properly parse the rest of file
         MjcfCompiler compilerInfo;
@@ -352,6 +403,8 @@ namespace pinocchio
         TextureMap_t mapOfTextures;
         // Map of model configurations
         ConfigMap_t mapOfConfigs;
+        // Map of equality constraints
+        EqualityMap_t mapOfEqualities;
 
         // reference configuration
         Eigen::VectorXd referenceConfig;
@@ -428,6 +481,10 @@ namespace pinocchio
         /// @param el ptree keyframe node
         void parseKeyFrame(const ptree & el);
 
+        /// @brief Parse all the info from the equality tag
+        /// @param el ptree equality node
+        void parseEquality(const ptree & el);
+
         /// @brief parse the mjcf file into a graph
         void parseGraph();
 
@@ -469,6 +526,13 @@ namespace pinocchio
         /// @param keyName Name of the keyframe
         void addKeyFrame(const Eigen::VectorXd & keyframe, const std::string & keyName);
 
+        /// @brief Parse the equality constraints and add them to the model
+        /// @param model Model to add the constraints to
+        /// @param contact_models Vector of contact models to add the constraints to
+        void parseContactInformation(
+          const Model & model,
+          PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) & contact_models);
+
         /// @brief Fill geometry model with all the info taken from the mjcf model file
         /// @param type Type of geometry to parse (COLLISION or VISUAL)
         /// @param geomModel geometry model to fill

include/pinocchio/parsers/mjcf/model.hxx

@@ -7,6 +7,8 @@
 
 #include "pinocchio/parsers/mjcf.hpp"
 #include "pinocchio/parsers/mjcf/mjcf-graph.hpp"
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/algorithm/contact-info.hpp"
 
 namespace pinocchio
 {
@@ -37,12 +39,46 @@ namespace pinocchio
 
       graph.parseGraphFromXML(xmlStr);
 
-      // // Use the Mjcf graph to create the model
+      // Use the Mjcf graph to create the model
       graph.parseRootTree();
 
       return model;
     }
 
+    template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
+    ModelTpl<Scalar, Options, JointCollectionTpl> & buildModel(
+      const std::string & filename,
+      ModelTpl<Scalar, Options, JointCollectionTpl> & model,
+      PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) & contact_models,
+      const bool verbose)
+    {
+      return buildModelFromXML(filename, model, contact_models, verbose);
+    }
+
+    template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
+    ModelTpl<Scalar, Options, JointCollectionTpl> & buildModelFromXML(
+      const std::string & xmlStr,
+      ModelTpl<Scalar, Options, JointCollectionTpl> & model,
+      PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) & contact_models,
+      const bool verbose)
+    {
+      ::pinocchio::urdf::details::UrdfVisitor<Scalar, Options, JointCollectionTpl> visitor(model);
+
+      typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph;
+
+      MjcfGraph graph(visitor, xmlStr);
+      if (verbose)
+        visitor.log = &std::cout;
+
+      graph.parseGraphFromXML(xmlStr);
+
+      // Use the Mjcf graph to create the model
+      graph.parseRootTree();
+      graph.parseContactInformation(model, contact_models);
+
+      return model;
+    }
+
     template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
     ModelTpl<Scalar, Options, JointCollectionTpl> & buildModel(
       const std::string & filename,
@@ -71,8 +107,45 @@ namespace pinocchio
 
       graph.parseGraphFromXML(xmlStr);
 
-      // // Use the Mjcf graph to create the model
+      // Use the Mjcf graph to create the model
+      graph.parseRootTree();
+
+      return model;
+    }
+
+    template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
+    ModelTpl<Scalar, Options, JointCollectionTpl> & buildModel(
+      const std::string & filename,
+      const typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointModel & rootJoint,
+      ModelTpl<Scalar, Options, JointCollectionTpl> & model,
+      PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) & contact_models,
+      const bool verbose)
+    {
+      return buildModelFromXML(filename, rootJoint, model, contact_models, verbose);
+    }
+
+    template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
+    ModelTpl<Scalar, Options, JointCollectionTpl> & buildModelFromXML(
+      const std::string & xmlStr,
+      const typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointModel & rootJoint,
+      ModelTpl<Scalar, Options, JointCollectionTpl> & model,
+      PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) & contact_models,
+      const bool verbose)
+    {
+      ::pinocchio::urdf::details::UrdfVisitorWithRootJoint<Scalar, Options, JointCollectionTpl>
+        visitor(model, rootJoint);
+
+      typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph;
+
+      MjcfGraph graph(visitor, xmlStr);
+      if (verbose)
+        visitor.log = &std::cout;
+
+      graph.parseGraphFromXML(xmlStr);
+
+      // Use the Mjcf graph to create the model
       graph.parseRootTree();
+      graph.parseContactInformation(model, contact_models);
 
       return model;
     }

src/parsers/mjcf/mjcf-graph.cpp

@@ -3,6 +3,8 @@
 //
 
 #include "pinocchio/parsers/mjcf/mjcf-graph.hpp"
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/algorithm/contact-info.hpp"
 
 namespace pinocchio
 {
@@ -735,6 +737,53 @@ namespace pinocchio
         }
       }
 
+      void MjcfGraph::parseEquality(const ptree & el)
+      {
+        for (const ptree::value_type & v : el)
+        {
+          std::string type = v.first;
+          // List of supported constraints from mjcf description
+          // equality -> connect
+
+          // The constraints below are not supported and will be ignored with the following
+          // warning: joint, flex, distance, weld
+          if (type != "connect")
+          {
+            // TODO(jcarpent): support extra constraint types such as joint, flex, distance, weld.
+            continue;
+          }
+
+          MjcfEquality eq;
+          eq.type = type;
+
+          // get the name of first body
+          auto body1 = v.second.get_optional<std::string>("<xmlattr>.body1");
+          if (body1)
+            eq.body1 = *body1;
+          else
+            throw std::invalid_argument("Equality constraint needs a first body");
+
+          // get the name of second body
+          auto body2 = v.second.get_optional<std::string>("<xmlattr>.body2");
+          if (body2)
+            eq.body2 = *body2;
+
+          // get the name of the constraint (if it exists)
+          auto name = v.second.get_optional<std::string>("<xmlattr>.name");
+          if (name)
+            eq.name = *name;
+          else
+            eq.name = eq.body1 + "_" + eq.body2 + "_constraint";
+
+          // get the anchor position
+          auto anchor = v.second.get_optional<std::string>("<xmlattr>.anchor");
+          if (anchor)
+            eq.anchor = internal::getVectorFromStream<3>(*anchor);
+
+          mapOfEqualities.insert(std::make_pair(eq.name, eq));
+        }
+      }
+
       void MjcfGraph::parseGraph()
       {
         boost::property_tree::ptree el;
@@ -772,6 +821,11 @@ namespace pinocchio
             boost::optional<std::string> childClass;
             parseJointAndBody(el.get_child("worldbody").get_child("body"), childClass);
           }
+
+          if (v.first == "equality")
+          {
+            parseEquality(el.get_child("equality"));
+          }
         }
       }
 
@@ -1040,6 +1094,37 @@ namespace pinocchio
           throw std::invalid_argument("Keyframe size does not match model size");
       }
 
+      void MjcfGraph::parseContactInformation(
+        const Model & model,
+        PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) & contact_models)
+      {
+        for (const auto & entry : mapOfEqualities)
+        {
+          const MjcfEquality & eq = entry.second;
+
+          SE3 jointPlacement;
+          jointPlacement.setIdentity();
+          jointPlacement.translation() = eq.anchor;
+
+          // Get Joint Indices from the model
+          const JointIndex body1 = urdfVisitor.getParentId(eq.body1);
+
+          // when body2 is not specified, we link to the world
+          if (eq.body2 == "")
+          {
+            RigidConstraintModel rcm(CONTACT_3D, model, body1, jointPlacement, LOCAL);
+            contact_models.push_back(rcm);
+          }
+          else
+          {
+            const JointIndex body2 = urdfVisitor.getParentId(eq.body2);
+            RigidConstraintModel rcm(
+              CONTACT_3D, model, body1, jointPlacement, body2, jointPlacement.inverse(), LOCAL);
+            contact_models.push_back(rcm);
+          }
+        }
+      }
+
       void MjcfGraph::parseRootTree()
       {
         urdfVisitor.setName(modelName);