Verified Commit 42821251 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

parsers/urdf: make convertFromUrdf common

parent 605e3c25
......@@ -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()
......
......@@ -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)
......
......@@ -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;
......
#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));
}
}
}
}
......@@ -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__
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment