[C++][Python] Added a method to get neutral config from an srdf file + binded it

src/multibody/model.hxx

@@ -126,12 +126,26 @@ namespace se3
     nq += j.nq();
     nv += j.nv();
 
-    neutralConfigurations.conservativeResize(nq);neutralConfigurations.bottomRows<D::NQ>() = j.neutralConfiguration();
+    
     effortLimit.conservativeResize(nv);effortLimit.bottomRows<D::NV>() = effort;
     velocityLimit.conservativeResize(nv);velocityLimit.bottomRows<D::NV>() = velocity;
     lowerPositionLimit.conservativeResize(nq);lowerPositionLimit.bottomRows<D::NQ>() = lowPos;
     upperPositionLimit.conservativeResize(nq);upperPositionLimit.bottomRows<D::NQ>() = upPos;
 
+    // Ensure that limits are not inf
+    Eigen::VectorXd neutralConf((lowerPositionLimit.bottomRows<D::NQ>() + upperPositionLimit.bottomRows<D::NQ>())/2 );
+    
+    neutralConfigurations.conservativeResize(nq);
+    if ( std::isfinite(neutralConf.norm()) )
+    {
+      neutralConfigurations.bottomRows<D::NQ>() = neutralConf;
+    }
+    else
+    {
+      assert( false && "One of the position limit is inf or NaN");
+      neutralConfigurations.bottomRows<D::NQ>() = j.neutralConfiguration();
+    }
+
     addFrame((jointName!="")?jointName:random(8), idx, SE3::Identity(), JOINT);
 
     return idx;

src/multibody/parser/srdf.hpp

@@ -109,6 +109,89 @@ namespace se3
     
 #endif // ifdef WITH_HPP_FCL
     
+
+    ///
+    /// \brief Get the neutral configuration of a given model associated to a SRDF file.
+    ///        It throws if the SRDF file is incorrect.
+    ///
+    /// \param[in] model The Model for which we want the neutral config
+    /// \param[in] filename The complete path to the SRDF file.
+    /// \param[in] verbose Verbosity mode.
+    ///
+    /// \return The neutral configuration as an eigen vector
+    inline Eigen::VectorXd getNeutralConfigurationFromSrdf(Model & model,
+                                                           const std::string & filename,
+                                                           const bool verbose) throw (std::invalid_argument)
+    {
+      const Eigen::VectorXd neutralConfig(model.nq);
+
+      // Check extension
+      const std::string extension = filename.substr(filename.find_last_of('.')+1);
+      if (extension != "srdf")
+      {
+        const std::string exception_message (filename + " does not have the right extension.");
+        throw std::invalid_argument(exception_message);
+      }
+      
+      // Open file
+      std::ifstream srdf_stream(filename.c_str());
+      if (! srdf_stream.is_open())
+      {
+        const std::string exception_message (filename + " does not seem to be a valid file.");
+        throw std::invalid_argument(exception_message);
+      }
+      
+      // Read xml stream
+      using boost::property_tree::ptree;
+      ptree pt;
+      read_xml(srdf_stream, pt);
+      
+      // Iterate over all tags directly children of robot
+      BOOST_FOREACH(const ptree::value_type & v, pt.get_child("robot"))
+      {
+        // if we encounter a tag group_state
+        if (v.first == "group_state")
+        {
+          const std::string name = v.second.get<std::string>("<xmlattr>.name");
+          std::cout << name << std::endl;
+          // Ensure that it is the half_sitting tag
+          if( name == "half_sitting")
+          {
+            // Iterate over all the joint tags
+            BOOST_FOREACH(const ptree::value_type & joint, v.second)
+            {
+              if (joint.first == "joint")
+              {
+                std::string joint_name = joint.second.get<std::string>("<xmlattr>.name");
+                double joint_config = joint.second.get<double>("<xmlattr>.value");
+                if (verbose)
+                {
+                  std::cout << "(" << joint_name << " , " << joint_config << ")" << std::endl;
+                }
+                // Search in model the joint and its config id
+                Model::JointIndex joint_id = model.getJointId(joint_name);
+                const JointModel & joint = model.joints[joint_id];
+
+                if (joint_id != model.joints.size()) // != model.njoints
+                {
+                  model.neutralConfigurations(joint.idx_q()) = joint_config; // joint with 1 dof
+                  // model.neutralConfigurations.segment(joint.idx_q(),joint.nq()) = joint_config; // joint with more than 1 dof
+                }
+                else
+                {
+                  if (verbose) std::cout << "The Joint " << joint_name << " was not found in model" << std::endl;
+                }
+              }
+            }
+            return neutralConfig;
+          }
+
+          
+        }
+      } // BOOST_FOREACH
+      assert(false && "no half_sitting configuration found in the srdf file"); // Should we throw something here ?  
+      return neutralConfig; // warning : uninitialized vector is returned
+    }
   }
 } // namespace se3
 

src/python/parsers.hpp

@@ -37,6 +37,8 @@
   #include "pinocchio/multibody/parser/lua.hpp"
 #endif // #ifdef WITH_LUA
 
+#include "pinocchio/multibody/parser/srdf.hpp"
+
 namespace se3
 {
   namespace python
@@ -122,6 +124,14 @@ namespace se3
       }
 #endif // #ifdef WITH_LUA
 
+      static Eigen::VectorXd getNeutralConfigurationFromSrdf(ModelHandler & model,
+                                                             const std::string & filename,
+                                                             bool verbose
+                                                            )
+      {
+        return se3::srdf::getNeutralConfigurationFromSrdf(*model, filename, verbose);
+      }
+
       /* --- Expose --------------------------------------------------------- */
       static void expose();
     }; // struct ParsersPythonVisitor
@@ -173,6 +183,13 @@ namespace se3
               "Parse the urdf file given in input and return a proper pinocchio model "
               "(remember to create the corresponding data structure).");
 #endif // #ifdef WITH_LUA
+
+      bp::def("getNeutralConfigurationFromSrdf",getNeutralConfigurationFromSrdf,
+              // static_cast <ModelHandler (*) ( const std::string &, bool)> (&ParsersPythonVisitor::getNeutralConfigurationFromSrdf),
+              bp::args("Model for which we want the neutral config","srdf filename (string)", "verbosity"
+                       ),
+              "Get the neutral configuration of a given model associated to a SRDF file");
+
     }
     
   }