parsers/urdf: make convertFromUrdf common

src/CMakeLists.txt

@@ -7,6 +7,7 @@ IF(BUILD_WITH_URDF_SUPPORT)
   SET(${PROJECT_NAME}_SOURCES
     parsers/urdf/model.cpp
     parsers/urdf/geometry.cpp
+    parsers/urdf/utils.cpp
   )
 ENDIF()
 

src/parsers/urdf/geometry.cpp

@@ -4,6 +4,7 @@
 
 #include "pinocchio/parsers/urdf.hpp"
 #include "pinocchio/parsers/urdf/types.hpp"
+#include "pinocchio/parsers/urdf/utils.hpp"
 #include "pinocchio/parsers/utils.hpp"
 
 #include <boost/property_tree/xml_parser.hpp>
@@ -43,8 +44,8 @@ namespace pinocchio
           } // BOOST_FOREACH
         }
         
-        bool isCapsule (const std::string & linkName,
-                        const std::string & geomName) const
+        bool isCapsule(const std::string & linkName,
+                       const std::string & geomName) const
         {
           LinkMap_t::const_iterator _link = links_.find(linkName);
           assert (_link != links_.end());
@@ -66,8 +67,8 @@ namespace pinocchio
           return false;
         }
 
-        bool isMeshConvex (const std::string & linkName,
-                           const std::string & geomName) const
+        bool isMeshConvex(const std::string & linkName,
+                          const std::string & geomName) const
         {
           LinkMap_t::const_iterator _link = links_.find(linkName);
           assert (_link != links_.end());
@@ -97,20 +98,6 @@ namespace pinocchio
         LinkMap_t links_;
       };
 
-      ///
-      /// \brief Convert URDF Pose quantity to SE3.
-      ///
-      /// \param[in] M The input URDF Pose.
-      ///
-      /// \return The converted pose/transform pinocchio::SE3.
-      ///
-      inline SE3 convertFromUrdf (const ::urdf::Pose & M)
-      {
-        const ::urdf::Vector3 & p = M.position;
-        const ::urdf::Rotation & q = M.rotation;
-        return SE3( Eigen::Quaterniond(q.w,q.x,q.y,q.z).matrix(), Eigen::Vector3d(p.x,p.y,p.z));
-      }
-
       template<typename Vector3>
       static void retrieveMeshScale(const ::urdf::MeshSharedPtr & mesh,
                                     const Eigen::MatrixBase<Vector3> & scale)
@@ -346,7 +333,7 @@ namespace pinocchio
        *
        */
       template<typename GeometryType>
-      inline void addLinkGeometryToGeomModel(const UrdfTree & tree,
+      static void addLinkGeometryToGeomModel(const UrdfTree & tree,
                                              ::hpp::fcl::MeshLoaderPtr & meshLoader,
                                              ::urdf::LinkConstSharedPtr link,
                                              UrdfGeomVisitorBase& visitor,
@@ -433,12 +420,12 @@ namespace pinocchio
        *
        */
       void recursiveParseTreeForGeom(const UrdfTree& tree,
-                            ::hpp::fcl::MeshLoaderPtr& meshLoader,
-                            ::urdf::LinkConstSharedPtr link,
-                            UrdfGeomVisitorBase& visitor,
-                            GeometryModel & geomModel,
-                            const std::vector<std::string> & package_dirs,
-                            const GeometryType type)
+                                     ::hpp::fcl::MeshLoaderPtr& meshLoader,
+                                     ::urdf::LinkConstSharedPtr link,
+                                     UrdfGeomVisitorBase& visitor,
+                                     GeometryModel & geomModel,
+                                     const std::vector<std::string> & package_dirs,
+                                     const GeometryType type)
       {
         
         switch(type)

src/parsers/urdf/model.cpp

@@ -4,6 +4,7 @@
 //
 
 #include "pinocchio/parsers/urdf.hpp"
+#include "pinocchio/parsers/urdf/utils.hpp"
 
 #include <urdf_model/model.h>
 #include <urdf_parser/urdf_parser.h>
@@ -25,43 +26,29 @@ namespace pinocchio
       ///
       /// \return The converted Spatial Inertia pinocchio::Inertia.
       ///
-      inline Inertia convertFromUrdf (const ::urdf::Inertial & Y)
+      static Inertia convertFromUrdf(const ::urdf::Inertial & Y)
       {
         const ::urdf::Vector3 & p = Y.origin.position;
         const ::urdf::Rotation & q = Y.origin.rotation;
 
-        const Eigen::Vector3d com(p.x,p.y,p.z);
-        const Eigen::Matrix3d & R = Eigen::Quaterniond(q.w,q.x,q.y,q.z).matrix();
+        const Inertia::Vector3 com(p.x,p.y,p.z);
+        const Inertia::Matrix3 & R = Eigen::Quaterniond(q.w,q.x,q.y,q.z).matrix();
 
-        Eigen::Matrix3d I; I <<
-          Y.ixx,Y.ixy,Y.ixz,
-          Y.ixy,Y.iyy,Y.iyz,
-          Y.ixz,Y.iyz,Y.izz;
+        Inertia::Matrix3 I;
+        I << Y.ixx,Y.ixy,Y.ixz,
+             Y.ixy,Y.iyy,Y.iyz,
+             Y.ixz,Y.iyz,Y.izz;
         return Inertia(Y.mass,com,R*I*R.transpose());
       }
 
-      inline Inertia convertFromUrdf (const ::urdf::InertialSharedPtr & Y)
+      static Inertia convertFromUrdf(const ::urdf::InertialSharedPtr & Y)
       {
-        if (Y) return convertFromUrdf (*Y);
+        if (Y) return convertFromUrdf(*Y);
         return Inertia::Zero();
       }
 
-      ///
-      /// \brief Convert URDF Pose quantity to SE3.
-      ///
-      /// \param[in] M The input URDF Pose.
-      ///
-      /// \return The converted pose/transform pinocchio::SE3.
-      ///
-      inline SE3 convertFromUrdf (const ::urdf::Pose & M)
-      {
-        const ::urdf::Vector3 & p = M.position;
-        const ::urdf::Rotation & q = M.rotation;
-        return SE3( Eigen::Quaterniond(q.w,q.x,q.y,q.z).matrix(), Eigen::Vector3d(p.x,p.y,p.z));
-      }
-
-      FrameIndex getParentLinkFrame(const ::urdf::LinkConstSharedPtr link,
-                                    UrdfVisitorBase& model)
+      static FrameIndex getParentLinkFrame(const ::urdf::LinkConstSharedPtr link,
+                                           UrdfVisitorBase & model)
       {
         PINOCCHIO_CHECK_INPUT_ARGUMENT(link && link->getParent());
         FrameIndex id = model.getBodyId(link->getParent()->name);
@@ -76,7 +63,7 @@ namespace pinocchio
       /// \param[in] model The model where the link must be added.
       ///
       void parseTree(::urdf::LinkConstSharedPtr link,
-                     UrdfVisitorBase& model)
+                     UrdfVisitorBase & model)
       {
         typedef UrdfVisitorBase::Scalar Scalar;
         typedef UrdfVisitorBase::SE3 SE3;

src/parsers/urdf/utils.cpp

+#include "pinocchio/parsers/urdf/utils.hpp"
+
+namespace pinocchio
+{
+  namespace urdf
+  {
+    namespace details
+    {
+    
+      SE3 convertFromUrdf(const ::urdf::Pose & M)
+      {
+        const ::urdf::Vector3 & p = M.position;
+        const ::urdf::Rotation & q = M.rotation;
+        return SE3(SE3::Quaternion(q.w,q.x,q.y,q.z).matrix(),
+                   SE3::Vector3(p.x,p.y,p.z));
+      }
+    }
+  }
+}

src/parsers/urdf/utils.hpp

@@ -5,6 +5,25 @@
 #ifndef __pinocchio_parsers_urdf_utils_hpp__
 #define __pinocchio_parsers_urdf_utils_hpp__
 
-#warning "This file is now deprecated and will be removed in future releases. There is no replacement."
+#include "pinocchio/spatial/se3.hpp"
+#include <urdf_model/pose.h>
+
+namespace pinocchio
+{
+  namespace urdf
+  {
+    namespace details
+    {
+      ///
+      /// \brief Convert URDF Pose quantity to SE3.
+      ///
+      /// \param[in] M The input URDF Pose.
+      ///
+      /// \return The converted pose/transform pinocchio::SE3.
+      ///
+      SE3 convertFromUrdf(const ::urdf::Pose & M);
+    }
+  }
+}
 
 #endif // __pinocchio_parsers_urdf_utils_hpp__