From 1002170ce57651f2731d1c436ce9058321e1542e Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Fri, 6 Mar 2020 10:07:40 +0100
Subject: [PATCH] [URDF] Move parser into the library.
---
src/CMakeLists.txt | 3 +-
src/parsers/urdf.hpp | 11 +-
src/parsers/urdf/geometry.cpp | 490 +++++++++++++++++
src/parsers/urdf/geometry.hxx | 456 +---------------
src/parsers/urdf/model.cpp | 300 +++++++++++
src/parsers/urdf/model.hxx | 959 ++++++++++++----------------------
src/parsers/urdf/utils.hpp | 72 +--
7 files changed, 1168 insertions(+), 1123 deletions(-)
create mode 100644 src/parsers/urdf/geometry.cpp
create mode 100644 src/parsers/urdf/model.cpp
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 8caacd2a1..af42c2b00 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -8,7 +8,8 @@
# ----------------------------------------------------
SET(${PROJECT_NAME}_SOURCES
- ${${PROJECT_NAME}_PARSERS_SOURCES}
+ parsers/urdf/model.cpp
+ parsers/urdf/geometry.cpp
)
# Extract the compile definitions of the project for export
diff --git a/src/parsers/urdf.hpp b/src/parsers/urdf.hpp
index 44a5f55e5..996007ea3 100644
--- a/src/parsers/urdf.hpp
+++ b/src/parsers/urdf.hpp
@@ -8,9 +8,14 @@
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/geometry.hpp"
-#include "pinocchio/parsers/urdf/types.hpp"
/// \cond
+// forward declaration of the unique type from urdfdom which is expose (mostly
+// for backward compatibility).
+namespace urdf {
+ class ModelInterface;
+}
+
namespace hpp
{
namespace fcl
@@ -72,7 +77,7 @@ namespace pinocchio
/// or ::urdf::parseURDFFile
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
ModelTpl<Scalar,Options,JointCollectionTpl> &
- buildModel(const ::urdf::ModelInterfaceSharedPtr & urdfTree,
+ buildModel(const ::urdf::ModelInterface* urdfTree,
const typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointModel & rootJoint,
ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const bool verbose = false);
@@ -89,7 +94,7 @@ namespace pinocchio
/// or ::urdf::parseURDFFile
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
ModelTpl<Scalar,Options,JointCollectionTpl> &
- buildModel(const ::urdf::ModelInterfaceSharedPtr & urdfTree,
+ buildModel(const ::urdf::ModelInterface* urdfTree,
ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const bool verbose = false);
diff --git a/src/parsers/urdf/geometry.cpp b/src/parsers/urdf/geometry.cpp
new file mode 100644
index 000000000..07bd42577
--- /dev/null
+++ b/src/parsers/urdf/geometry.cpp
@@ -0,0 +1,490 @@
+#include "pinocchio/parsers/urdf.hpp"
+#include "pinocchio/parsers/urdf/types.hpp"
+#include "pinocchio/parsers/utils.hpp"
+
+#include <boost/property_tree/xml_parser.hpp>
+#include <boost/property_tree/ptree.hpp>
+
+#include <urdf_model/model.h>
+#include <urdf_parser/urdf_parser.h>
+
+namespace pinocchio
+{
+ namespace urdf
+ {
+ namespace details
+ {
+ struct UrdfTree
+ {
+ typedef boost::property_tree::ptree ptree;
+ typedef std::map<std::string, const ptree&> LinkMap_t;
+
+ void parse (const std::string & xmlStr)
+ {
+ urdf_ = ::urdf::parseURDF(xmlStr);
+ if (!urdf_) {
+ throw std::invalid_argument ("Enable to parse URDF");
+ }
+
+ std::istringstream iss(xmlStr);
+ using namespace boost::property_tree;
+ read_xml(iss, tree_, xml_parser::no_comments);
+
+ BOOST_FOREACH(const ptree::value_type & link, tree_.get_child("robot")) {
+ if (link.first == "link") {
+ std::string name = link.second.get<std::string>("<xmlattr>.name");
+ links_.insert(std::pair<std::string,const ptree&>(name, link.second));
+ }
+ } // BOOST_FOREACH
+ }
+
+ bool isCapsule (const std::string & linkName,
+ const std::string & geomName) const
+ {
+ LinkMap_t::const_iterator _link = links_.find(linkName);
+ assert (_link != links_.end());
+ const ptree& link = _link->second;
+ if (link.count ("collision_checking") == 0)
+ return false;
+ BOOST_FOREACH(const ptree::value_type & cc, link.get_child("collision_checking")) {
+ if (cc.first == "capsule") {
+#ifdef PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
+ std::cerr << "Warning: support for tag link/collision_checking/capsule"
+ " is not available for URDFDOM < 0.3.0" << std::endl;
+#else
+ std::string name = cc.second.get<std::string>("<xmlattr>.name");
+ if (geomName == name) return true;
+#endif
+ }
+ } // BOOST_FOREACH
+
+ return false;
+ }
+
+ bool isMeshConvex (const std::string & linkName,
+ const std::string & geomName) const
+ {
+ LinkMap_t::const_iterator _link = links_.find(linkName);
+ assert (_link != links_.end());
+ const ptree& link = _link->second;
+ if (link.count ("collision_checking") == 0)
+ return false;
+ BOOST_FOREACH(const ptree::value_type & cc, link.get_child("collision_checking")) {
+ if (cc.first == "convex") {
+#ifdef PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
+ std::cerr << "Warning: support for tag link/collision_checking/convex"
+ " is not available for URDFDOM < 0.3.0" << std::endl;
+#else
+ std::string name = cc.second.get<std::string>("<xmlattr>.name");
+ if (geomName == name) return true;
+#endif
+ }
+ } // BOOST_FOREACH
+
+ return false;
+ }
+
+ // For standard URDF tags
+ ::urdf::ModelInterfaceSharedPtr urdf_;
+ // For other tags
+ ptree tree_;
+ // A mapping from link name to corresponding child of tree_
+ 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)
+ {
+ Vector3 & scale_ = PINOCCHIO_EIGEN_CONST_CAST(Vector3,scale);
+ scale_ <<
+ mesh->scale.x,
+ mesh->scale.y,
+ mesh->scale.z;
+ }
+
+#ifdef PINOCCHIO_WITH_HPP_FCL
+# if ( HPP_FCL_MAJOR_VERSION>1 || ( HPP_FCL_MAJOR_VERSION==1 && \
+ ( HPP_FCL_MINOR_VERSION>1 || ( HPP_FCL_MINOR_VERSION==1 && \
+ HPP_FCL_PATCH_VERSION>3))))
+# define PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3
+# endif
+
+ /**
+ * @brief Get a fcl::CollisionObject from an urdf geometry, searching
+ * for it in specified package directories
+ *
+ * @param[in] urdf_geometry A shared pointer on the input urdf Geometry
+ * @param[in] package_dirs A vector containing the different directories where to search for packages
+ * @param[out] meshPath The Absolute path of the mesh currently read
+ * @param[out] meshScale Scale of transformation currently applied to the mesh
+ *
+ * @return A shared pointer on the geometry converted as a fcl::CollisionGeometry
+ */
+ boost::shared_ptr<fcl::CollisionGeometry>
+ inline retrieveCollisionGeometry(const UrdfTree& tree,
+ fcl::MeshLoaderPtr& meshLoader,
+ const std::string& linkName,
+ const std::string& geomName,
+ const ::urdf::GeometrySharedPtr urdf_geometry,
+ const std::vector<std::string> & package_dirs,
+ std::string & meshPath,
+ Eigen::Vector3d & meshScale)
+ {
+ boost::shared_ptr<fcl::CollisionGeometry> geometry;
+
+ // Handle the case where collision geometry is a mesh
+ if (urdf_geometry->type == ::urdf::Geometry::MESH)
+ {
+ const ::urdf::MeshSharedPtr urdf_mesh = ::urdf::dynamic_pointer_cast< ::urdf::Mesh> (urdf_geometry);
+ std::string collisionFilename = urdf_mesh->filename;
+
+ meshPath = retrieveResourcePath(collisionFilename, package_dirs);
+ if (meshPath == "") {
+ std::stringstream ss;
+ ss << "Mesh " << collisionFilename << " could not be found.";
+ throw std::invalid_argument (ss.str());
+ }
+
+ fcl::Vec3f scale = fcl::Vec3f(urdf_mesh->scale.x,
+ urdf_mesh->scale.y,
+ urdf_mesh->scale.z
+ );
+
+ retrieveMeshScale(urdf_mesh, meshScale);
+
+ // Create FCL mesh by parsing Collada file.
+#ifdef PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3
+ hpp::fcl::BVHModelPtr_t bvh = meshLoader->load (meshPath, scale);
+ bool convex = tree.isMeshConvex (linkName, geomName);
+ if (convex) {
+ bvh->buildConvexRepresentation (false);
+ geometry = bvh->convex;
+ } else
+ geometry = bvh;
+#else
+ geometry = meshLoader->load (meshPath, scale);
+#endif
+ }
+
+ // Handle the case where collision geometry is a cylinder
+ // Use FCL capsules for cylinders
+ else if (urdf_geometry->type == ::urdf::Geometry::CYLINDER)
+ {
+ const bool is_capsule = tree.isCapsule(linkName, geomName);
+ meshScale << 1,1,1;
+ const ::urdf::CylinderSharedPtr collisionGeometry = ::urdf::dynamic_pointer_cast< ::urdf::Cylinder> (urdf_geometry);
+
+ double radius = collisionGeometry->radius;
+ double length = collisionGeometry->length;
+
+ // Create fcl capsule geometry.
+ if (is_capsule) {
+ meshPath = "CAPSULE";
+ geometry = boost::shared_ptr < fcl::CollisionGeometry >(new fcl::Capsule (radius, length));
+ } else {
+ meshPath = "CYLINDER";
+ geometry = boost::shared_ptr < fcl::CollisionGeometry >(new fcl::Cylinder (radius, length));
+ }
+ }
+ // Handle the case where collision geometry is a box.
+ else if (urdf_geometry->type == ::urdf::Geometry::BOX)
+ {
+ meshPath = "BOX";
+ meshScale << 1,1,1;
+ const ::urdf::BoxSharedPtr collisionGeometry = ::urdf::dynamic_pointer_cast< ::urdf::Box> (urdf_geometry);
+
+ double x = collisionGeometry->dim.x;
+ double y = collisionGeometry->dim.y;
+ double z = collisionGeometry->dim.z;
+
+ geometry = boost::shared_ptr < fcl::CollisionGeometry > (new fcl::Box (x, y, z));
+ }
+ // Handle the case where collision geometry is a sphere.
+ else if (urdf_geometry->type == ::urdf::Geometry::SPHERE)
+ {
+ meshPath = "SPHERE";
+ meshScale << 1,1,1;
+ const ::urdf::SphereSharedPtr collisionGeometry = ::urdf::dynamic_pointer_cast< ::urdf::Sphere> (urdf_geometry);
+
+ double radius = collisionGeometry->radius;
+
+ geometry = boost::shared_ptr < fcl::CollisionGeometry > (new fcl::Sphere (radius));
+ }
+ else throw std::invalid_argument("Unknown geometry type :");
+
+ if (!geometry)
+ {
+ throw std::invalid_argument("The polyhedron retrived is empty");
+ }
+
+ return geometry;
+ }
+#endif // PINOCCHIO_WITH_HPP_FCL
+
+ /**
+ * @brief Get the first geometry attached to a link
+ *
+ * @param[in] link The URDF link
+ *
+ * @return Either the first collision or visual
+ */
+ template<typename T>
+ inline PINOCCHIO_URDF_SHARED_PTR(const T)
+ getLinkGeometry(const ::urdf::LinkConstSharedPtr link);
+
+ template<>
+ inline ::urdf::CollisionConstSharedPtr
+ getLinkGeometry< ::urdf::Collision>(const ::urdf::LinkConstSharedPtr link)
+ {
+ return link->collision;
+ }
+
+ template<>
+ inline ::urdf::VisualConstSharedPtr
+ getLinkGeometry< ::urdf::Visual>(const ::urdf::LinkConstSharedPtr link)
+ {
+ return link->visual;
+ }
+
+
+ /**
+ * @brief Get the material values from the link visual object
+ *
+ * @param[in] Visual/Collision The Visual or the Collision object.
+ * @param[out] meshTexturePath The absolute file path containing the texture description.
+ * @param[out] meshColor The mesh RGBA vector.
+ * @param[in] package_dirs A vector containing the different directories where to search for packages
+ *
+ */
+ template<typename urdfObject>
+ inline bool getVisualMaterial(const PINOCCHIO_URDF_SHARED_PTR(urdfObject) urdf_object,std::string & meshTexturePath,
+ Eigen::Vector4d & meshColor, const std::vector<std::string> & package_dirs);
+
+ template<>
+ inline bool getVisualMaterial< ::urdf::Collision>
+ (const ::urdf::CollisionSharedPtr, std::string& meshTexturePath,
+ Eigen::Vector4d & meshColor, const std::vector<std::string> &)
+ {
+ meshColor << 0.9, 0.9, 0.9, 1.;
+ meshTexturePath = "";
+ return false;
+ }
+
+ template<>
+ inline bool getVisualMaterial< ::urdf::Visual>
+ (const ::urdf::VisualSharedPtr urdf_visual, std::string& meshTexturePath,
+ Eigen::Vector4d & meshColor, const std::vector<std::string> & package_dirs)
+ {
+ meshColor.setZero();
+ meshTexturePath = "";
+ bool overrideMaterial = false;
+ if(urdf_visual->material) {
+ overrideMaterial = true;
+ meshColor << urdf_visual->material->color.r, urdf_visual->material->color.g,
+ urdf_visual->material->color.b, urdf_visual->material->color.a;
+ if(urdf_visual->material->texture_filename!="")
+ meshTexturePath = retrieveResourcePath((urdf_visual)->material->texture_filename, package_dirs);
+ }
+ return overrideMaterial;
+ }
+
+ /**
+ * @brief Get the array of geometries attached to a link
+ *
+ * @param[in] link The URDF link
+ *
+ * @return the array of either collisions or visuals
+ */
+ template<typename T>
+ inline const std::vector< PINOCCHIO_URDF_SHARED_PTR(T) > &
+ getLinkGeometryArray(const ::urdf::LinkConstSharedPtr link);
+
+ template<>
+ inline const std::vector< ::urdf::CollisionSharedPtr> &
+ getLinkGeometryArray< ::urdf::Collision>(const ::urdf::LinkConstSharedPtr link)
+ {
+ return link->collision_array;
+ }
+
+ template<>
+ inline const std::vector< ::urdf::VisualSharedPtr> &
+ getLinkGeometryArray< ::urdf::Visual>(const ::urdf::LinkConstSharedPtr link)
+ {
+ return link->visual_array;
+ }
+
+ /**
+ * @brief Add the geometries attached to an URDF link to a GeometryModel, looking
+ * either for collisions or visuals
+ *
+ * @param[in] tree The URDF kinematic tree
+ * @param[in] meshLoader The FCL mesh loader to avoid duplications of already loaded geometries
+ * @param[in] link The current URDF link
+ * @param model The model to which is the GeometryModel associated
+ * @param geomModel The GeometryModel where the Collision Objects must be added
+ * @param[in] package_dirs A vector containing the different directories where to search for packages
+ *
+ */
+ template<typename GeometryType>
+ inline void addLinkGeometryToGeomModel(const UrdfTree & tree,
+ ::hpp::fcl::MeshLoaderPtr & meshLoader,
+ ::urdf::LinkConstSharedPtr link,
+ UrdfGeomVisitorBase& visitor,
+ GeometryModel & geomModel,
+ const std::vector<std::string> & package_dirs)
+ {
+#ifndef PINOCCHIO_WITH_HPP_FCL
+ PINOCCHIO_UNUSED_VARIABLE(tree);
+ PINOCCHIO_UNUSED_VARIABLE(meshLoader);
+#endif // PINOCCHIO_WITH_HPP_FCL
+
+ typedef std::vector< PINOCCHIO_URDF_SHARED_PTR(GeometryType) > VectorSharedT;
+ typedef GeometryModel::SE3 SE3;
+
+ if(getLinkGeometry<GeometryType>(link))
+ {
+ std::string meshPath = "";
+
+ Eigen::Vector3d meshScale(Eigen::Vector3d::Ones());
+
+ const std::string & link_name = link->name;
+
+ VectorSharedT geometries_array = getLinkGeometryArray<GeometryType>(link);
+
+ FrameIndex frame_id;
+ UrdfGeomVisitorBase::Frame frame = visitor.getBodyFrame (link_name, frame_id);
+ SE3 body_placement = frame.placement;
+
+ std::size_t objectId = 0;
+ for (typename VectorSharedT::const_iterator i = geometries_array.begin();i != geometries_array.end(); ++i)
+ {
+ meshPath.clear();
+#ifdef PINOCCHIO_WITH_HPP_FCL
+
+#ifdef PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
+ const std::string & geom_name = (*i)->group_name;
+#else
+ const std::string & geom_name = (*i)->name;
+#endif // PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
+ const GeometryObject::CollisionGeometryPtr geometry =
+ retrieveCollisionGeometry(tree, meshLoader, link_name, geom_name,
+ (*i)->geometry, package_dirs, meshPath, meshScale);
+#else
+ ::urdf::MeshSharedPtr urdf_mesh = ::urdf::dynamic_pointer_cast< ::urdf::Mesh> ((*i)->geometry);
+ if (urdf_mesh)
+ {
+ meshPath = retrieveResourcePath(urdf_mesh->filename, package_dirs);
+ retrieveMeshScale(urdf_mesh, meshScale);
+ }
+
+ const boost::shared_ptr<fcl::CollisionGeometry> geometry(new fcl::CollisionGeometry());
+#endif // PINOCCHIO_WITH_HPP_FCL
+
+ Eigen::Vector4d meshColor;
+ std::string meshTexturePath;
+ bool overrideMaterial = getVisualMaterial<GeometryType>((*i), meshTexturePath, meshColor, package_dirs);
+
+ SE3 geomPlacement = body_placement * convertFromUrdf((*i)->origin);
+ std::ostringstream geometry_object_suffix;
+ geometry_object_suffix << "_" << objectId;
+ const std::string & geometry_object_name = std::string(link_name + geometry_object_suffix.str());
+ GeometryObject geometry_object(geometry_object_name,
+ frame_id, frame.parent,
+ geometry,
+ geomPlacement, meshPath, meshScale,
+ overrideMaterial, meshColor, meshTexturePath);
+ geomModel.addGeometryObject(geometry_object);
+ ++objectId;
+ }
+ }
+ }
+
+ /**
+ * @brief Recursive procedure for reading the URDF tree, looking for geometries
+ * This function fill the geometric model whith geometry objects retrieved from the URDF tree
+ *
+ * @param[in] tree The URDF kinematic tree
+ * @param[in] meshLoader The FCL mesh loader to avoid duplications of already loaded geometries
+ * @param[in] link The current URDF link
+ * @param model The model to which is the GeometryModel associated
+ * @param geomModel The GeometryModel where the Collision Objects must be added
+ * @param[in] package_dirs A vector containing the different directories where to search for packages
+ * @param[in] type The type of objects that must be loaded ( can be VISUAL or COLLISION)
+ *
+ */
+ 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)
+ {
+
+ switch(type)
+ {
+ case COLLISION:
+ addLinkGeometryToGeomModel<::urdf::Collision >(tree, meshLoader, link, visitor, geomModel, package_dirs);
+ break;
+ case VISUAL:
+ addLinkGeometryToGeomModel<::urdf::Visual >(tree, meshLoader, link, visitor, geomModel, package_dirs);
+ break;
+ default:
+ break;
+ }
+
+ BOOST_FOREACH(::urdf::LinkConstSharedPtr child,link->child_links)
+ {
+ recursiveParseTreeForGeom(tree, meshLoader, child, visitor, geomModel, package_dirs,type);
+ }
+
+ }
+
+ void parseTreeForGeom(UrdfGeomVisitorBase& visitor,
+ const std::istream& xmlStream,
+ const GeometryType type,
+ GeometryModel & geomModel,
+ const std::vector<std::string> & package_dirs,
+ ::hpp::fcl::MeshLoaderPtr meshLoader)
+ {
+ std::string xmlStr;
+ {
+ std::ostringstream os;
+ os << xmlStream.rdbuf();
+ xmlStr = os.str();
+ }
+
+ details::UrdfTree tree;
+ tree.parse (xmlStr);
+
+ std::vector<std::string> hint_directories(package_dirs);
+
+ // Append the ROS_PACKAGE_PATH
+ std::vector<std::string> ros_pkg_paths = rosPaths();
+ hint_directories.insert(hint_directories.end(), ros_pkg_paths.begin(), ros_pkg_paths.end());
+
+#ifdef PINOCCHIO_WITH_HPP_FCL
+ if (!meshLoader) meshLoader = fcl::MeshLoaderPtr(new fcl::MeshLoader);
+#endif // ifdef PINOCCHIO_WITH_HPP_FCL
+
+ recursiveParseTreeForGeom(tree, meshLoader, tree.urdf_->getRoot(),
+ visitor, geomModel, hint_directories,type);
+ }
+ } // namespace details
+ } // namespace urdf
+} // namespace pinocchio
diff --git a/src/parsers/urdf/geometry.hxx b/src/parsers/urdf/geometry.hxx
index 57814f14d..5c09261df 100644
--- a/src/parsers/urdf/geometry.hxx
+++ b/src/parsers/urdf/geometry.hxx
@@ -6,19 +6,10 @@
#define __pinocchio_multibody_parsers_urdf_geometry_hxx__
#include "pinocchio/parsers/urdf.hpp"
-#include "pinocchio/parsers/urdf/utils.hpp"
-#include "pinocchio/parsers/utils.hpp"
-
-#include <boost/property_tree/xml_parser.hpp>
-#include <boost/property_tree/ptree.hpp>
-
-#include <urdf_model/model.h>
-#include <urdf_parser/urdf_parser.h>
#include <iostream>
#include <sstream>
#include <iomanip>
-#include <boost/foreach.hpp>
#include <boost/shared_ptr.hpp>
#ifdef PINOCCHIO_WITH_HPP_FCL
@@ -32,395 +23,32 @@ namespace pinocchio
{
namespace details
{
- struct UrdfTree
+ struct UrdfGeomVisitorBase
{
- typedef boost::property_tree::ptree ptree;
- typedef std::map<std::string, const ptree&> LinkMap_t;
-
- void parse (const std::string & xmlStr)
- {
- urdf_ = ::urdf::parseURDF(xmlStr);
- if (!urdf_) {
- throw std::invalid_argument ("Enable to parse URDF");
- }
-
- std::istringstream iss(xmlStr);
- using namespace boost::property_tree;
- read_xml(iss, tree_, xml_parser::no_comments);
-
- BOOST_FOREACH(const ptree::value_type & link, tree_.get_child("robot")) {
- if (link.first == "link") {
- std::string name = link.second.get<std::string>("<xmlattr>.name");
- links_.insert(std::pair<std::string,const ptree&>(name, link.second));
- }
- } // BOOST_FOREACH
- }
-
- bool isCapsule (const std::string & linkName,
- const std::string & geomName) const
- {
- LinkMap_t::const_iterator _link = links_.find(linkName);
- assert (_link != links_.end());
- const ptree& link = _link->second;
- if (link.count ("collision_checking") == 0)
- return false;
- BOOST_FOREACH(const ptree::value_type & cc, link.get_child("collision_checking")) {
- if (cc.first == "capsule") {
-#ifdef PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
- std::cerr << "Warning: support for tag link/collision_checking/capsule"
- " is not available for URDFDOM < 0.3.0" << std::endl;
-#else
- std::string name = cc.second.get<std::string>("<xmlattr>.name");
- if (geomName == name) return true;
-#endif
- }
- } // BOOST_FOREACH
-
- return false;
- }
+ typedef FrameTpl<urdf_value_type, 0> Frame;
- bool isMeshConvex (const std::string & linkName,
- const std::string & geomName) const
- {
- LinkMap_t::const_iterator _link = links_.find(linkName);
- assert (_link != links_.end());
- const ptree& link = _link->second;
- if (link.count ("collision_checking") == 0)
- return false;
- BOOST_FOREACH(const ptree::value_type & cc, link.get_child("collision_checking")) {
- if (cc.first == "convex") {
-#ifdef PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
- std::cerr << "Warning: support for tag link/collision_checking/convex"
- " is not available for URDFDOM < 0.3.0" << std::endl;
-#else
- std::string name = cc.second.get<std::string>("<xmlattr>.name");
- if (geomName == name) return true;
-#endif
- }
- } // BOOST_FOREACH
-
- return false;
- }
-
- // For standard URDF tags
- ::urdf::ModelInterfaceSharedPtr urdf_;
- // For other tags
- ptree tree_;
- // A mapping from link name to corresponding child of tree_
- LinkMap_t links_;
+ virtual Frame getBodyFrame (const std::string& name, FrameIndex& fid) const = 0;
};
-
- template<typename Vector3>
- static void retrieveMeshScale(const ::urdf::MeshSharedPtr & mesh,
- const Eigen::MatrixBase<Vector3> & scale)
- {
- Vector3 & scale_ = PINOCCHIO_EIGEN_CONST_CAST(Vector3,scale);
- scale_ <<
- mesh->scale.x,
- mesh->scale.y,
- mesh->scale.z;
- }
-#ifdef PINOCCHIO_WITH_HPP_FCL
-# if ( HPP_FCL_MAJOR_VERSION>1 || ( HPP_FCL_MAJOR_VERSION==1 && \
- ( HPP_FCL_MINOR_VERSION>1 || ( HPP_FCL_MINOR_VERSION==1 && \
- HPP_FCL_PATCH_VERSION>3))))
-# define PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3
-# endif
-
- /**
- * @brief Get a fcl::CollisionObject from an urdf geometry, searching
- * for it in specified package directories
- *
- * @param[in] urdf_geometry A shared pointer on the input urdf Geometry
- * @param[in] package_dirs A vector containing the different directories where to search for packages
- * @param[out] meshPath The Absolute path of the mesh currently read
- * @param[out] meshScale Scale of transformation currently applied to the mesh
- *
- * @return A shared pointer on the geometry converted as a fcl::CollisionGeometry
- */
- boost::shared_ptr<fcl::CollisionGeometry>
- inline retrieveCollisionGeometry(const UrdfTree& tree,
- fcl::MeshLoaderPtr& meshLoader,
- const std::string& linkName,
- const std::string& geomName,
- const ::urdf::GeometrySharedPtr urdf_geometry,
- const std::vector<std::string> & package_dirs,
- std::string & meshPath,
- Eigen::Vector3d & meshScale)
+ template<typename _Scalar, int _Options, template<typename,int> class JointCollectionTpl>
+ struct UrdfGeomVisitor : UrdfGeomVisitorBase
{
- boost::shared_ptr<fcl::CollisionGeometry> geometry;
+ typedef ModelTpl<_Scalar,_Options,JointCollectionTpl> Model;
+ const Model& model;
- // Handle the case where collision geometry is a mesh
- if (urdf_geometry->type == ::urdf::Geometry::MESH)
- {
- const ::urdf::MeshSharedPtr urdf_mesh = ::urdf::dynamic_pointer_cast< ::urdf::Mesh> (urdf_geometry);
- std::string collisionFilename = urdf_mesh->filename;
-
- meshPath = retrieveResourcePath(collisionFilename, package_dirs);
- if (meshPath == "") {
- std::stringstream ss;
- ss << "Mesh " << collisionFilename << " could not be found.";
- throw std::invalid_argument (ss.str());
- }
-
- fcl::Vec3f scale = fcl::Vec3f(urdf_mesh->scale.x,
- urdf_mesh->scale.y,
- urdf_mesh->scale.z
- );
-
- retrieveMeshScale(urdf_mesh, meshScale);
-
- // Create FCL mesh by parsing Collada file.
-#ifdef PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3
- hpp::fcl::BVHModelPtr_t bvh = meshLoader->load (meshPath, scale);
- bool convex = tree.isMeshConvex (linkName, geomName);
- if (convex) {
- bvh->buildConvexRepresentation (false);
- geometry = bvh->convex;
- } else
- geometry = bvh;
-#else
- geometry = meshLoader->load (meshPath, scale);
-#endif
- }
+ UrdfGeomVisitor (const Model& model) : model(model) {}
- // Handle the case where collision geometry is a cylinder
- // Use FCL capsules for cylinders
- else if (urdf_geometry->type == ::urdf::Geometry::CYLINDER)
- {
- const bool is_capsule = tree.isCapsule(linkName, geomName);
- meshScale << 1,1,1;
- const ::urdf::CylinderSharedPtr collisionGeometry = ::urdf::dynamic_pointer_cast< ::urdf::Cylinder> (urdf_geometry);
-
- double radius = collisionGeometry->radius;
- double length = collisionGeometry->length;
-
- // Create fcl capsule geometry.
- if (is_capsule) {
- meshPath = "CAPSULE";
- geometry = boost::shared_ptr < fcl::CollisionGeometry >(new fcl::Capsule (radius, length));
- } else {
- meshPath = "CYLINDER";
- geometry = boost::shared_ptr < fcl::CollisionGeometry >(new fcl::Cylinder (radius, length));
- }
- }
- // Handle the case where collision geometry is a box.
- else if (urdf_geometry->type == ::urdf::Geometry::BOX)
- {
- meshPath = "BOX";
- meshScale << 1,1,1;
- const ::urdf::BoxSharedPtr collisionGeometry = ::urdf::dynamic_pointer_cast< ::urdf::Box> (urdf_geometry);
-
- double x = collisionGeometry->dim.x;
- double y = collisionGeometry->dim.y;
- double z = collisionGeometry->dim.z;
-
- geometry = boost::shared_ptr < fcl::CollisionGeometry > (new fcl::Box (x, y, z));
- }
- // Handle the case where collision geometry is a sphere.
- else if (urdf_geometry->type == ::urdf::Geometry::SPHERE)
+ Frame getBodyFrame (const std::string& link_name, FrameIndex& fid) const
{
- meshPath = "SPHERE";
- meshScale << 1,1,1;
- const ::urdf::SphereSharedPtr collisionGeometry = ::urdf::dynamic_pointer_cast< ::urdf::Sphere> (urdf_geometry);
-
- double radius = collisionGeometry->radius;
-
- geometry = boost::shared_ptr < fcl::CollisionGeometry > (new fcl::Sphere (radius));
- }
- else throw std::invalid_argument("Unknown geometry type :");
-
- if (!geometry)
- {
- throw std::invalid_argument("The polyhedron retrived is empty");
- }
-
- return geometry;
- }
-#endif // PINOCCHIO_WITH_HPP_FCL
-
- /**
- * @brief Get the first geometry attached to a link
- *
- * @param[in] link The URDF link
- *
- * @return Either the first collision or visual
- */
- template<typename T>
- inline PINOCCHIO_URDF_SHARED_PTR(const T)
- getLinkGeometry(const ::urdf::LinkConstSharedPtr link);
-
- template<>
- inline ::urdf::CollisionConstSharedPtr
- getLinkGeometry< ::urdf::Collision>(const ::urdf::LinkConstSharedPtr link)
- {
- return link->collision;
- }
-
- template<>
- inline ::urdf::VisualConstSharedPtr
- getLinkGeometry< ::urdf::Visual>(const ::urdf::LinkConstSharedPtr link)
- {
- return link->visual;
- }
-
-
- /**
- * @brief Get the material values from the link visual object
- *
- * @param[in] Visual/Collision The Visual or the Collision object.
- * @param[out] meshTexturePath The absolute file path containing the texture description.
- * @param[out] meshColor The mesh RGBA vector.
- * @param[in] package_dirs A vector containing the different directories where to search for packages
- *
- */
- template<typename urdfObject>
- inline bool getVisualMaterial(const PINOCCHIO_URDF_SHARED_PTR(urdfObject) urdf_object,std::string & meshTexturePath,
- Eigen::Vector4d & meshColor, const std::vector<std::string> & package_dirs);
-
- template<>
- inline bool getVisualMaterial< ::urdf::Collision>
- (const ::urdf::CollisionSharedPtr, std::string& meshTexturePath,
- Eigen::Vector4d & meshColor, const std::vector<std::string> &)
- {
- meshColor << 0.9, 0.9, 0.9, 1.;
- meshTexturePath = "";
- return false;
- }
-
- template<>
- inline bool getVisualMaterial< ::urdf::Visual>
- (const ::urdf::VisualSharedPtr urdf_visual, std::string& meshTexturePath,
- Eigen::Vector4d & meshColor, const std::vector<std::string> & package_dirs)
- {
- meshColor.setZero();
- meshTexturePath = "";
- bool overrideMaterial = false;
- if(urdf_visual->material) {
- overrideMaterial = true;
- meshColor << urdf_visual->material->color.r, urdf_visual->material->color.g,
- urdf_visual->material->color.b, urdf_visual->material->color.a;
- if(urdf_visual->material->texture_filename!="")
- meshTexturePath = retrieveResourcePath((urdf_visual)->material->texture_filename, package_dirs);
- }
- return overrideMaterial;
- }
-
- /**
- * @brief Get the array of geometries attached to a link
- *
- * @param[in] link The URDF link
- *
- * @return the array of either collisions or visuals
- */
- template<typename T>
- inline const std::vector< PINOCCHIO_URDF_SHARED_PTR(T) > &
- getLinkGeometryArray(const ::urdf::LinkConstSharedPtr link);
-
- template<>
- inline const std::vector< ::urdf::CollisionSharedPtr> &
- getLinkGeometryArray< ::urdf::Collision>(const ::urdf::LinkConstSharedPtr link)
- {
- return link->collision_array;
- }
-
- template<>
- inline const std::vector< ::urdf::VisualSharedPtr> &
- getLinkGeometryArray< ::urdf::Visual>(const ::urdf::LinkConstSharedPtr link)
- {
- return link->visual_array;
- }
-
- /**
- * @brief Add the geometries attached to an URDF link to a GeometryModel, looking
- * either for collisions or visuals
- *
- * @param[in] tree The URDF kinematic tree
- * @param[in] meshLoader The FCL mesh loader to avoid duplications of already loaded geometries
- * @param[in] link The current URDF link
- * @param model The model to which is the GeometryModel associated
- * @param geomModel The GeometryModel where the Collision Objects must be added
- * @param[in] package_dirs A vector containing the different directories where to search for packages
- *
- */
- template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename GeometryType>
- inline void addLinkGeometryToGeomModel(const UrdfTree & tree,
- ::hpp::fcl::MeshLoaderPtr & meshLoader,
- ::urdf::LinkConstSharedPtr link,
- const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
- GeometryModel & geomModel,
- const std::vector<std::string> & package_dirs)
- {
-#ifndef PINOCCHIO_WITH_HPP_FCL
- PINOCCHIO_UNUSED_VARIABLE(tree);
- PINOCCHIO_UNUSED_VARIABLE(meshLoader);
-#endif // PINOCCHIO_WITH_HPP_FCL
-
- typedef std::vector< PINOCCHIO_URDF_SHARED_PTR(GeometryType) > VectorSharedT;
- typedef GeometryModel::SE3 SE3;
-
- if(getLinkGeometry<GeometryType>(link))
- {
- std::string meshPath = "";
-
- Eigen::Vector3d meshScale(Eigen::Vector3d::Ones());
-
- const std::string & link_name = link->name;
-
- VectorSharedT geometries_array = getLinkGeometryArray<GeometryType>(link);
-
if (!model.existFrame(link_name, BODY))
{
throw std::invalid_argument("No link " + link_name + " in model");
}
- FrameIndex frame_id = model.getFrameId(link_name, BODY);
- SE3 body_placement = model.frames[frame_id].placement;
- PINOCCHIO_CHECK_INPUT_ARGUMENT(model.frames[frame_id].type == BODY);
-
- std::size_t objectId = 0;
- for (typename VectorSharedT::const_iterator i = geometries_array.begin();i != geometries_array.end(); ++i)
- {
- meshPath.clear();
-#ifdef PINOCCHIO_WITH_HPP_FCL
-
-#ifdef PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
- const std::string & geom_name = (*i)->group_name;
-#else
- const std::string & geom_name = (*i)->name;
-#endif // PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
- const GeometryObject::CollisionGeometryPtr geometry =
- retrieveCollisionGeometry(tree, meshLoader, link_name, geom_name,
- (*i)->geometry, package_dirs, meshPath, meshScale);
-#else
- ::urdf::MeshSharedPtr urdf_mesh = ::urdf::dynamic_pointer_cast< ::urdf::Mesh> ((*i)->geometry);
- if (urdf_mesh)
- {
- meshPath = retrieveResourcePath(urdf_mesh->filename, package_dirs);
- retrieveMeshScale(urdf_mesh, meshScale);
- }
-
- const boost::shared_ptr<fcl::CollisionGeometry> geometry(new fcl::CollisionGeometry());
-#endif // PINOCCHIO_WITH_HPP_FCL
-
- Eigen::Vector4d meshColor;
- std::string meshTexturePath;
- bool overrideMaterial = getVisualMaterial<GeometryType>((*i), meshTexturePath, meshColor, package_dirs);
-
- SE3 geomPlacement = body_placement * convertFromUrdf((*i)->origin);
- std::ostringstream geometry_object_suffix;
- geometry_object_suffix << "_" << objectId;
- const std::string & geometry_object_name = std::string(link_name + geometry_object_suffix.str());
- GeometryObject geometry_object(geometry_object_name,
- frame_id, model.frames[frame_id].parent,
- geometry,
- geomPlacement, meshPath, meshScale,
- overrideMaterial, meshColor, meshTexturePath);
- geomModel.addGeometryObject(geometry_object);
- ++objectId;
- }
+ fid = model.getFrameId(link_name, BODY);
+ PINOCCHIO_CHECK_INPUT_ARGUMENT(model.frames[fid].type == BODY);
+ return model.frames[fid].template cast<urdf_value_type>();
}
- }
+ };
/**
* @brief Recursive procedure for reading the URDF tree, looking for geometries
@@ -435,34 +63,12 @@ namespace pinocchio
* @param[in] type The type of objects that must be loaded ( can be VISUAL or COLLISION)
*
*/
- template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
- void parseTreeForGeom(const UrdfTree& tree,
- ::hpp::fcl::MeshLoaderPtr& meshLoader,
- ::urdf::LinkConstSharedPtr link,
- const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
+ void parseTreeForGeom(UrdfGeomVisitorBase& visitor,
+ const std::istream& xmlStream,
+ const GeometryType type,
GeometryModel & geomModel,
const std::vector<std::string> & package_dirs,
- const GeometryType type)
- {
-
- switch(type)
- {
- case COLLISION:
- addLinkGeometryToGeomModel<Scalar,Options,JointCollectionTpl, ::urdf::Collision >(tree, meshLoader, link, model, geomModel, package_dirs);
- break;
- case VISUAL:
- addLinkGeometryToGeomModel<Scalar,Options,JointCollectionTpl, ::urdf::Visual >(tree, meshLoader, link, model, geomModel, package_dirs);
- break;
- default:
- break;
- }
-
- BOOST_FOREACH(::urdf::LinkConstSharedPtr child,link->child_links)
- {
- parseTreeForGeom(tree, meshLoader, child, model, geomModel, package_dirs,type);
- }
-
- }
+ ::hpp::fcl::MeshLoaderPtr meshLoader);
} // namespace details
@@ -492,35 +98,13 @@ namespace pinocchio
const std::vector<std::string> & package_dirs,
::hpp::fcl::MeshLoaderPtr meshLoader)
{
- std::string xmlStr;
- {
- std::ostringstream os;
- os << xmlStream.rdbuf();
- xmlStr = os.str();
- }
-
- details::UrdfTree tree;
- tree.parse (xmlStr);
-
- std::vector<std::string> hint_directories(package_dirs);
-
- // Append the ROS_PACKAGE_PATH
- std::vector<std::string> ros_pkg_paths = rosPaths();
- hint_directories.insert(hint_directories.end(), ros_pkg_paths.begin(), ros_pkg_paths.end());
-
-#ifdef PINOCCHIO_WITH_HPP_FCL
- if (!meshLoader) meshLoader = fcl::MeshLoaderPtr(new fcl::MeshLoader);
-#endif // ifdef PINOCCHIO_WITH_HPP_FCL
-
- details::parseTreeForGeom(tree, meshLoader, tree.urdf_->getRoot(), model, geomModel, hint_directories,type);
+ details::UrdfGeomVisitor<Scalar, Options, JointCollectionTpl> visitor (model);
+ details::parseTreeForGeom (visitor, xmlStream, type, geomModel,
+ package_dirs, meshLoader);
return geomModel;
}
} // namespace urdf
} // namespace pinocchio
-#ifdef PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3
-# undef PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3
-#endif
-
#endif // ifndef __pinocchio_multibody_parsers_urdf_geometry_hxx__
diff --git a/src/parsers/urdf/model.cpp b/src/parsers/urdf/model.cpp
new file mode 100644
index 000000000..63ffe7f02
--- /dev/null
+++ b/src/parsers/urdf/model.cpp
@@ -0,0 +1,300 @@
+//
+// Copyright (c) 2015-2020 CNRS
+// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
+//
+
+#include "pinocchio/parsers/urdf.hpp"
+
+#include <urdf_model/model.h>
+#include <urdf_parser/urdf_parser.h>
+
+#include <sstream>
+#include <boost/foreach.hpp>
+#include <limits>
+
+namespace pinocchio
+{
+ namespace urdf
+ {
+ namespace details
+ {
+ ///
+ /// \brief Convert URDF Inertial quantity to Spatial Inertia.
+ ///
+ /// \param[in] Y The input URDF Inertia.
+ ///
+ /// \return The converted Spatial Inertia pinocchio::Inertia.
+ ///
+ inline 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();
+
+ Eigen::Matrix3d 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)
+ {
+ 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 getParentJointFrame(const ::urdf::LinkConstSharedPtr link,
+ UrdfVisitorBase& model)
+ {
+ PINOCCHIO_CHECK_INPUT_ARGUMENT(link && link->getParent());
+
+ FrameIndex id = -1;
+ std::string parent_joint_name;
+ if (!link->getParent()->parent_joint)
+ {
+ try {
+ id = model.getJointFrameId("root_joint");
+ } catch (const std::invalid_argument&) {
+ id = 0;
+ }
+ }
+ else
+ id = model.getJointFrameId(link->getParent()->parent_joint->name);
+
+ return id;
+ }
+
+ ///
+ /// \brief Recursive procedure for reading the URDF tree.
+ /// The function returns an exception as soon as a necessary Inertia or Joint information are missing.
+ ///
+ /// \param[in] link The current URDF link.
+ /// \param[in] model The model where the link must be added.
+ ///
+ void parseTree(::urdf::LinkConstSharedPtr link,
+ UrdfVisitorBase& model)
+ {
+ typedef typename UrdfVisitorBase::Scalar Scalar;
+ typedef typename UrdfVisitorBase::SE3 SE3;
+ typedef typename UrdfVisitorBase::Vector Vector;
+ typedef typename UrdfVisitorBase::Vector3 Vector3;
+ typedef typename Model::FrameIndex FrameIndex;
+
+ // Parent joint of the current body
+ const ::urdf::JointConstSharedPtr joint =
+ ::urdf::const_pointer_cast< ::urdf::Joint>(link->parent_joint);
+
+ if(joint) // if the link is not the root of the tree
+ {
+ PINOCCHIO_CHECK_INPUT_ARGUMENT(link->getParent());
+
+ const std::string & joint_name = joint->name;
+ const std::string & link_name = link->name;
+ const std::string & parent_link_name = link->getParent()->name;
+ std::ostringstream joint_info;
+
+ FrameIndex parentFrameId = getParentJointFrame(link, model);
+
+ // Transformation from the parent link to the joint origin
+ const SE3 jointPlacement
+ = convertFromUrdf(joint->parent_to_joint_origin_transform);
+
+ const Inertia Y = convertFromUrdf(link->inertial);
+
+ Vector max_effort(1), max_velocity(1), min_config(1), max_config(1);
+ Vector3 axis (joint->axis.x, joint->axis.y, joint->axis.z);
+
+ const Scalar infty = std::numeric_limits<Scalar>::infinity();
+
+ switch(joint->type)
+ {
+ case ::urdf::Joint::FLOATING:
+ joint_info << "joint FreeFlyer";
+
+ max_effort = Vector::Constant(6, infty);
+ max_velocity = Vector::Constant(6, infty);
+ min_config = Vector::Constant(7,-infty);
+ max_config = Vector::Constant(7, infty);
+ min_config.tail<4>().setConstant(-1.01);
+ max_config.tail<4>().setConstant( 1.01);
+
+ model.addJointAndBody(UrdfVisitorBase::FLOATING, axis,
+ parentFrameId,jointPlacement,joint->name,
+ Y,link->name,
+ max_effort,max_velocity,min_config,max_config);
+ break;
+
+ case ::urdf::Joint::REVOLUTE:
+ joint_info << "joint REVOLUTE with axis";
+
+ // TODO I think the URDF standard forbids REVOLUTE with no limits.
+ assert(joint->limits);
+ if (joint->limits)
+ {
+ max_effort << joint->limits->effort;
+ max_velocity << joint->limits->velocity;
+ min_config << joint->limits->lower;
+ max_config << joint->limits->upper;
+ }
+
+ model.addJointAndBody(UrdfVisitorBase::REVOLUTE, axis,
+ parentFrameId,jointPlacement,joint->name,
+ Y,link->name,
+ max_effort,max_velocity,min_config,max_config);
+ break;
+
+ case ::urdf::Joint::CONTINUOUS: // Revolute joint with no joint limits
+ joint_info << "joint CONTINUOUS with axis";
+
+ min_config.resize(2);
+ max_config.resize(2);
+ min_config << -1.01, -1.01;
+ max_config << 1.01, 1.01;
+
+ if(joint->limits)
+ {
+ max_effort << joint->limits->effort;
+ max_velocity << joint->limits->velocity;
+ }
+
+ model.addJointAndBody(UrdfVisitorBase::CONTINUOUS, axis,
+ parentFrameId,jointPlacement,joint->name,
+ Y,link->name,
+ max_effort,max_velocity,min_config,max_config);
+ break;
+
+ case ::urdf::Joint::PRISMATIC:
+ joint_info << "joint PRISMATIC with axis";
+
+ // TODO I think the URDF standard forbids REVOLUTE with no limits.
+ assert(joint->limits);
+ if (joint->limits)
+ {
+ max_effort << joint->limits->effort;
+ max_velocity << joint->limits->velocity;
+ min_config << joint->limits->lower;
+ max_config << joint->limits->upper;
+ }
+
+ model.addJointAndBody(UrdfVisitorBase::PRISMATIC, axis,
+ parentFrameId,jointPlacement,joint->name,
+ Y,link->name,
+ max_effort,max_velocity,min_config,max_config);
+ break;
+
+ case ::urdf::Joint::PLANAR:
+ joint_info << "joint PLANAR with normal axis along Z";
+
+ max_effort = Vector::Constant(3, infty);
+ max_velocity = Vector::Constant(3, infty);
+ min_config = Vector::Constant(4,-infty);
+ max_config = Vector::Constant(4, infty);
+ min_config.tail<2>().setConstant(-1.01);
+ max_config.tail<2>().setConstant( 1.01);
+
+ model.addJointAndBody(UrdfVisitorBase::PLANAR, axis,
+ parentFrameId,jointPlacement,joint->name,
+ Y,link->name,
+ max_effort,max_velocity,min_config,max_config);
+ break;
+
+ case ::urdf::Joint::FIXED:
+ // In case of fixed joint, if link has inertial tag:
+ // -add the inertia of the link to his parent in the model
+ // Otherwise do nothing.
+ // In all cases:
+ // -let all the children become children of parent
+ // -inform the parser of the offset to apply
+ // -add fixed body in model to display it in gepetto-viewer
+
+ joint_info << "fixed joint";
+ model.addFixedJointAndBody(parentFrameId, jointPlacement,
+ joint_name, Y, link_name);
+ break;
+
+ default:
+ throw std::invalid_argument("The type of joint " + joint_name + " is not supported.");
+ break;
+ }
+
+ model
+ << "Adding Body" << '\n'
+ << '\"' << link_name << "\" connected to \"" << parent_link_name << "\" through joint \"" << joint_name << "\"\n"
+ << "joint type: " << joint_info.str() << '\n'
+ << "joint placement:\n" << jointPlacement << '\n'
+ << "body info: " << '\n'
+ << " mass: " << Y.mass() << '\n'
+ << " lever: " << Y.lever().transpose() << '\n'
+ << " inertia elements (Ixx,Iyx,Iyy,Izx,Izy,Izz): " << Y.inertia().data().transpose() << '\n' << '\n';
+ }
+ else if (link->getParent())
+ throw std::invalid_argument(link->name + " - joint information missing.");
+
+ BOOST_FOREACH(::urdf::LinkConstSharedPtr child, link->child_links)
+ {
+ parseTree(child, model);
+ }
+ }
+
+ ///
+ /// \brief Parse a tree with a specific root joint linking the model to the environment.
+ /// The function returns an exception as soon as a necessary Inertia or Joint information are missing.
+ ///
+ /// \param[in] link The current URDF link.
+ /// \param[in] model The model where the link must be added.
+ ///
+ void parseRootTree(const ::urdf::ModelInterface * urdfTree,
+ UrdfVisitorBase& model)
+ {
+ model.setName(urdfTree->getName());
+
+ ::urdf::LinkConstSharedPtr root_link = urdfTree->getRoot();
+ model.addRootJoint(convertFromUrdf(root_link->inertial), root_link->name);
+
+ BOOST_FOREACH(::urdf::LinkConstSharedPtr child, root_link->child_links)
+ {
+ parseTree(child, model);
+ }
+ }
+
+ void parseRootTree(const std::string & filename,
+ UrdfVisitorBase& model)
+ {
+ ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile (filename);
+ if (urdfTree)
+ return parseRootTree (urdfTree.get(), model);
+ else
+ throw std::invalid_argument("The file " + filename + " does not "
+ "contain a valid URDF model.");
+ }
+
+ void parseRootTreeFromXML(const std::string & xmlString,
+ UrdfVisitorBase& model)
+ {
+ ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDF(xmlString);
+ if (urdfTree)
+ return parseRootTree (urdfTree.get(), model);
+ else
+ throw std::invalid_argument("The XML stream does not contain a valid "
+ "URDF model.");
+ }
+ } // namespace details
+ } // namespace urdf
+} // namespace pinocchio
diff --git a/src/parsers/urdf/model.hxx b/src/parsers/urdf/model.hxx
index a31fd28db..785fa36f3 100644
--- a/src/parsers/urdf/model.hxx
+++ b/src/parsers/urdf/model.hxx
@@ -8,12 +8,8 @@
#include "pinocchio/math/matrix.hpp"
#include "pinocchio/parsers/urdf.hpp"
-#include "pinocchio/parsers/urdf/utils.hpp"
#include "pinocchio/multibody/model.hpp"
-#include <urdf_model/model.h>
-#include <urdf_parser/urdf_parser.h>
-
#include <sstream>
#include <boost/foreach.hpp>
#include <limits>
@@ -22,590 +18,358 @@ namespace pinocchio
{
namespace urdf
{
- namespace details
- {
- const FrameType JOINT_OR_FIXED_JOINT = (FrameType) (JOINT | FIXED_JOINT);
-
- template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
- FrameIndex getParentJointFrame(const ::urdf::LinkConstSharedPtr link,
- const ModelTpl<Scalar,Options,JointCollectionTpl> & model)
- {
- typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
- typedef typename Model::Frame Frame;
- typedef typename Model::FrameIndex FrameIndex;
-
- PINOCCHIO_CHECK_INPUT_ARGUMENT(link && link->getParent());
-
- FrameIndex id;
- if (!link->getParent()->parent_joint)
- {
- if (model.existFrame("root_joint", JOINT_OR_FIXED_JOINT))
- id = model.getFrameId("root_joint", JOINT_OR_FIXED_JOINT);
- else
- id = 0;
- }
- else
- {
- if (model.existFrame(link->getParent()->parent_joint->name, JOINT_OR_FIXED_JOINT))
- id = model.getFrameId (link->getParent()->parent_joint->name, JOINT_OR_FIXED_JOINT);
- else
- throw std::invalid_argument("Model does not have any joints named "
- + link->getParent()->parent_joint->name);
- }
-
- const Frame & f = model.frames[id];
- if (f.type == JOINT || f.type == FIXED_JOINT)
- return id;
- throw std::invalid_argument("Parent frame is not a JOINT neither a FIXED_JOINT");
- }
-
- template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
- void appendBodyToJoint(ModelTpl<Scalar,Options,JointCollectionTpl> & model,
- const FrameIndex fid,
- const ::urdf::InertialConstSharedPtr Y_ptr,
- const typename ModelTpl<Scalar,Options,JointCollectionTpl>::SE3 & placement,
- const std::string & body_name)
- {
- typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
- typedef typename Model::Frame Frame;
- typedef typename Model::SE3 SE3;
-
- const Frame & frame = model.frames[fid];
- const SE3 & p = frame.placement * placement;
- if(frame.parent > 0
- && Y_ptr
- && Y_ptr->mass > Eigen::NumTraits<double>::epsilon())
- {
- model.appendBodyToJoint(frame.parent, convertFromUrdf(*Y_ptr), p);
- }
-
- model.addBodyFrame(body_name, frame.parent, p, (int)fid);
- // Reference to model.frames[fid] can has changed because the vector
- // may have been reallocated.
- if (model.frames[fid].parent > 0)
- {
- assert ( !hasNaN(model.inertias[model.frames[fid].parent].lever())
- && !hasNaN(model.inertias[model.frames[fid].parent].inertia().data()));
- }
- }
-
- ///
- /// \brief Shortcut for adding a joint and directly append a body to it.
- ///
- template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename JointModel>
- void addJointAndBody(ModelTpl<Scalar,Options,JointCollectionTpl> & model,
- const JointModelBase<JointModel> & jmodel,
- const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex & parentFrameId,
- const typename ModelTpl<Scalar,Options,JointCollectionTpl>::SE3 & joint_placement,
- const std::string & joint_name,
- const ::urdf::InertialConstSharedPtr Y,
- const std::string & body_name,
- const typename JointModel::TangentVector_t & max_effort,
- const typename JointModel::TangentVector_t & max_velocity,
- const typename JointModel::ConfigVector_t & min_config,
- const typename JointModel::ConfigVector_t & max_config)
- {
- typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
- typedef typename Model::Frame Frame;
- typedef typename Model::SE3 SE3;
- typedef typename Model::JointIndex JointIndex;
- typedef typename Model::FrameIndex FrameIndex;
-
- JointIndex idx;
- const Frame & frame = model.frames[parentFrameId];
-
- idx = model.addJoint(frame.parent,jmodel,
- frame.placement * joint_placement,
- joint_name,
- max_effort,max_velocity,min_config,max_config
- );
- int res (model.addJointFrame(idx, (int)parentFrameId));
- if (res == -1) {
- std::ostringstream oss;
- oss << joint_name << " already inserted as a frame. Current frames "
- "are [";
- for (typename PINOCCHIO_ALIGNED_STD_VECTOR(Frame)::const_iterator it =
- model.frames.begin (); it != model.frames.end (); ++it) {
- oss << "\"" << it->name << "\",";
+ namespace details {
+ typedef double urdf_value_type;
+
+ template<typename _Scalar, int Options>
+ class UrdfVisitorBaseTpl {
+ public:
+ enum JointType {
+ REVOLUTE, CONTINUOUS, PRISMATIC, FLOATING, PLANAR
+ };
+
+ typedef _Scalar Scalar;
+ typedef SE3Tpl<Scalar,Options> SE3;
+ typedef InertiaTpl<Scalar,Options> Inertia;
+
+ typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
+ typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
+ typedef Eigen::Ref<Vector> VectorRef;
+ typedef Eigen::Ref<const Vector> VectorConstRef;
+
+ virtual void setName (const std::string& name) = 0;
+
+ virtual void addRootJoint (const Inertia& Y, const std::string & body_name) = 0;
+
+ virtual void addJointAndBody(
+ JointType type,
+ const Vector3& axis,
+ const FrameIndex & parentFrameId,
+ const SE3 & placement,
+ const std::string & joint_name,
+ const Inertia& Y,
+ const std::string & body_name,
+ const VectorConstRef& max_effort,
+ const VectorConstRef& max_velocity,
+ const VectorConstRef& min_config,
+ const VectorConstRef& max_config) = 0;
+
+ virtual void addFixedJointAndBody(
+ const FrameIndex & parentFrameId,
+ const SE3 & joint_placement,
+ const std::string & joint_name,
+ const Inertia& Y,
+ const std::string & body_name) = 0;
+
+ virtual void appendBodyToJoint(
+ const FrameIndex fid,
+ const Inertia& Y,
+ const SE3 & placement,
+ const std::string & body_name) = 0;
+
+ virtual FrameIndex getJointFrameId (
+ const std::string& frame_name) const = 0;
+
+ UrdfVisitorBaseTpl () : log (NULL) {}
+
+ template <typename T>
+ UrdfVisitorBaseTpl& operator<< (const T& t)
+ {
+ if (log != NULL) *log << t;
+ return *this;
}
- oss << "]";
- throw std::invalid_argument(oss.str().c_str());
- }
-
- FrameIndex jointFrameId = (FrameIndex) res; // C-style cast to remove polluting compilation warning. This is Bad practice. See issue #323 (rework indexes)
- appendBodyToJoint(model, jointFrameId, Y, SE3::Identity(), body_name);
- }
-
- template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename JointModel>
- void addJointAndBody(ModelTpl<Scalar,Options,JointCollectionTpl> & model,
- const JointModelBase<JointModel> & jmodel,
- const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex & parentFrameId,
- const typename ModelTpl<Scalar,Options,JointCollectionTpl>::SE3 & joint_placement,
- const std::string & joint_name,
- const ::urdf::InertialConstSharedPtr Y,
- const std::string & body_name)
- {
- const Scalar infty = std::numeric_limits<Scalar>::infinity();
-
- const typename JointModel::TangentVector_t max_effort = JointModel::TangentVector_t::Constant(jmodel.nv(), infty);
- const typename JointModel::TangentVector_t max_velocity = JointModel::TangentVector_t::Constant(jmodel.nv(), infty);
- const typename JointModel::ConfigVector_t min_config = JointModel::ConfigVector_t ::Constant(jmodel.nq(),-infty);
- const typename JointModel::ConfigVector_t max_config = JointModel::ConfigVector_t ::Constant(jmodel.nq(), infty);
-
- addJointAndBody(model,jmodel.derived(),parentFrameId,joint_placement,joint_name,Y,body_name,
- max_effort,max_velocity,min_config,max_config);
- }
-
- ///
- /// \brief Shortcut for adding a fixed joint and directly append a body to it.
- ///
- template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
- void addFixedJointAndBody(ModelTpl<Scalar,Options,JointCollectionTpl> & model,
- const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex & parentFrameId,
- const typename ModelTpl<Scalar,Options,JointCollectionTpl>::SE3 & joint_placement,
- const std::string & joint_name,
- const ::urdf::InertialConstSharedPtr Y,
- const std::string & body_name)
- {
- typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
- typedef typename Model::Frame Frame;
-
- const Frame & frame = model.frames[parentFrameId];
-
- int fid = model.addFrame(Frame(joint_name, frame.parent, parentFrameId,
- frame.placement * joint_placement, FIXED_JOINT)
- );
- if (fid < 0)
- throw std::invalid_argument("Fixed joint " + joint_name + " could not be added.");
-
- appendBodyToJoint(model, (FrameIndex)fid, Y, SE3::Identity(), body_name);
- }
-
- ///
- /// \brief Recursive procedure for reading the URDF tree.
- /// The function returns an exception as soon as a necessary Inertia or Joint information are missing.
- ///
- /// \param[in] link The current URDF link.
- /// \param[in] model The model where the link must be added.
- ///
- template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
- void parseTree(::urdf::LinkConstSharedPtr link,
- ModelTpl<Scalar,Options,JointCollectionTpl> & model,
- bool verbose)
- {
- typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
- typedef typename Model::JointCollection JointCollection;
- typedef typename Model::SE3 SE3;
- typedef typename Model::FrameIndex FrameIndex;
- // Parent joint of the current body
- const ::urdf::JointConstSharedPtr joint =
- ::urdf::const_pointer_cast< ::urdf::Joint>(link->parent_joint);
+ std::ostream* log;
+ };
- if(joint) // if the link is not the root of the tree
- {
- PINOCCHIO_CHECK_INPUT_ARGUMENT(link->getParent());
+ template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
+ class UrdfVisitor : public UrdfVisitorBaseTpl<Scalar, Options>
+ {
+ public:
+ typedef UrdfVisitorBaseTpl<Scalar, Options> Base;
+ typedef typename Base::JointType JointType;
+ typedef typename Base::Vector3 Vector3;
+ typedef typename Base::VectorConstRef VectorConstRef;
+ typedef typename Base::SE3 SE3;
+ typedef typename Base::Inertia Inertia;
- const std::string & joint_name = joint->name;
- const std::string & link_name = link->name;
- const std::string & parent_link_name = link->getParent()->name;
- std::ostringstream joint_info;
+ typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
+ typedef typename Model::JointCollection JointCollection;
+ typedef typename Model::JointModel JointModel;
+ typedef typename Model::Frame Frame;
- FrameIndex parentFrameId = getParentJointFrame(link, model);
+ Model& model;
- // Transformation from the parent link to the joint origin
- const SE3 jointPlacement
- = convertFromUrdf(joint->parent_to_joint_origin_transform).template cast<Scalar>();
+ UrdfVisitor (Model& model) : model(model) {}
- const ::urdf::InertialSharedPtr Y =
- ::urdf::const_pointer_cast< ::urdf::Inertial>(link->inertial);
+ void setName (const std::string& name)
+ {
+ model.name = name;
+ }
- switch(joint->type)
+ virtual void addRootJoint (const Inertia& Y, const std::string & body_name)
{
- case ::urdf::Joint::FLOATING:
- joint_info << "joint FreeFlyer";
- addJointAndBody(model,typename JointCollection::JointModelFreeFlyer(),
- parentFrameId,jointPlacement,joint->name,
- Y,link->name);
-
- break;
-
- case ::urdf::Joint::REVOLUTE:
- {
- joint_info << "joint REVOLUTE with axis";
-
- typedef JointModelRX::ConfigVector_t ConfigVector_t;
- typedef JointModelRX::TangentVector_t TangentVector_t;
-
- TangentVector_t max_effort;
- TangentVector_t max_velocity;
- ConfigVector_t lower_position;
- ConfigVector_t upper_position;
-
- if (joint->limits)
- {
- max_effort << joint->limits->effort;
- max_velocity << joint->limits->velocity;
- lower_position << joint->limits->lower;
- upper_position << joint->limits->upper;
- }
+ addFixedJointAndBody(0, SE3::Identity(), "root_joint", Y, body_name);
+ }
- CartesianAxis axis = extractCartesianAxis(joint->axis);
-
- switch(axis)
- {
- case AXIS_X:
- joint_info << " along X";
- addJointAndBody(model, typename JointCollection::JointModelRX(),
- parentFrameId,jointPlacement,joint->name,
- Y,link->name,
- max_effort,max_velocity,
- lower_position, upper_position);
- break;
-
- case AXIS_Y:
- joint_info << " along Y";
- addJointAndBody(model, typename JointCollection::JointModelRY(),
- parentFrameId,jointPlacement,joint->name,
- Y,link->name,
- max_effort,max_velocity,
- lower_position, upper_position);
- break;
-
- case AXIS_Z:
- joint_info << " along Z";
- addJointAndBody(model, typename JointCollection::JointModelRZ(),
- parentFrameId,jointPlacement,joint->name,
- Y,link->name,
- max_effort,max_velocity,
- lower_position, upper_position);
- break;
-
- case AXIS_UNALIGNED:
- {
- typename SE3::Vector3 joint_axis((Scalar)joint->axis.x,(Scalar)joint->axis.y,(Scalar)joint->axis.z);
- joint_info << " unaligned along (" << joint_axis.transpose() << ")";
-
- addJointAndBody(model, typename JointCollection::JointModelRevoluteUnaligned(joint_axis.normalized()),
- parentFrameId,jointPlacement,joint->name,
- Y,link->name,
- max_effort,max_velocity,
- lower_position, upper_position);
- break;
- }
- default:
- PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "The axis type of the revolute joint is of wrong type.");
- break;
- }
- break;
+ void addJointAndBody(
+ JointType type,
+ const Vector3& axis,
+ const FrameIndex & parentFrameId,
+ const SE3 & placement,
+ const std::string & joint_name,
+ const Inertia& Y,
+ const std::string & body_name,
+ const VectorConstRef& max_effort,
+ const VectorConstRef& max_velocity,
+ const VectorConstRef& min_config,
+ const VectorConstRef& max_config)
+ {
+ JointIndex idx;
+ const Frame & frame = model.frames[parentFrameId];
+
+ switch (type) {
+ case Base::FLOATING:
+ idx = model.addJoint(frame.parent,
+ typename JointCollection::JointModelFreeFlyer(),
+ frame.placement * placement,
+ joint_name,
+ max_effort,max_velocity,min_config,max_config
+ );
+ break;
+ case Base::REVOLUTE:
+ idx = addJoint<
+ typename JointCollection::JointModelRX,
+ typename JointCollection::JointModelRY,
+ typename JointCollection::JointModelRZ,
+ typename JointCollection::JointModelRevoluteUnaligned> (
+ axis, frame, placement, joint_name,
+ max_effort, max_velocity, min_config, max_config);
+ break;
+ case Base::CONTINUOUS:
+ idx = addJoint<
+ typename JointCollection::JointModelRUBX,
+ typename JointCollection::JointModelRUBY,
+ typename JointCollection::JointModelRUBZ,
+ typename JointCollection::JointModelRevoluteUnboundedUnaligned> (
+ axis, frame, placement, joint_name,
+ max_effort, max_velocity, min_config, max_config);
+ break;
+ case Base::PRISMATIC:
+ idx = addJoint<
+ typename JointCollection::JointModelPX,
+ typename JointCollection::JointModelPY,
+ typename JointCollection::JointModelPZ,
+ typename JointCollection::JointModelPrismaticUnaligned> (
+ axis, frame, placement, joint_name,
+ max_effort, max_velocity, min_config, max_config);
+ break;
+ case Base::PLANAR:
+ idx = model.addJoint(frame.parent,
+ typename JointCollection::JointModelPlanar(),
+ frame.placement * placement,
+ joint_name,
+ max_effort,max_velocity,min_config,max_config
+ );
+ break;
+ default:
+ PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "The joint type is not correct.");
+ };
+ int res (model.addJointFrame(idx, (int)parentFrameId));
+ if (res == -1) {
+ std::ostringstream oss;
+ oss << joint_name << " already inserted as a frame. Current frames "
+ "are [";
+ for (typename PINOCCHIO_ALIGNED_STD_VECTOR(Frame)::const_iterator it =
+ model.frames.begin (); it != model.frames.end (); ++it) {
+ oss << '"' << it->name << "\",";
}
+ oss << ']';
+ throw std::invalid_argument(oss.str());
+ }
- case ::urdf::Joint::CONTINUOUS: // Revolute joint with no joint limits
- {
- joint_info << "joint CONTINUOUS with axis";
+ FrameIndex jointFrameId = (FrameIndex) res; // C-style cast to remove polluting compilation warning. This is Bad practice. See issue #323 (rework indexes)
+ appendBodyToJoint(jointFrameId, Y, SE3::Identity(), body_name);
+ }
- typedef JointModelRUBX::ConfigVector_t ConfigVector_t;
- typedef JointModelRUBX::TangentVector_t TangentVector_t;
+ void addFixedJointAndBody(
+ const FrameIndex & parentFrameId,
+ const SE3 & joint_placement,
+ const std::string & joint_name,
+ const Inertia& Y,
+ const std::string & body_name)
+ {
+ const Frame & frame = model.frames[parentFrameId];
- TangentVector_t max_effort;
- TangentVector_t max_velocity;
- const ConfigVector_t::Scalar u = 1.01;
- ConfigVector_t lower_position(-u, -u);
- ConfigVector_t upper_position( u, u);
+ int fid = model.addFrame(Frame(joint_name, frame.parent, parentFrameId,
+ frame.placement * joint_placement, FIXED_JOINT)
+ );
+ if (fid < 0)
+ throw std::invalid_argument("Fixed joint " + joint_name + " could not be added.");
- if(joint->limits)
- {
- max_effort << joint->limits->effort;
- max_velocity << joint->limits->velocity;
- }
+ appendBodyToJoint((FrameIndex)fid, Y, SE3::Identity(), body_name);
+ }
- CartesianAxis axis = extractCartesianAxis(joint->axis);
-
- switch(axis)
- {
- case AXIS_X:
- joint_info << " along X";
- addJointAndBody(model, typename JointCollection::JointModelRUBX(),
- parentFrameId,jointPlacement,joint->name,
- Y,link->name,
- max_effort,max_velocity,
- lower_position, upper_position);
- break;
-
- case AXIS_Y:
- joint_info << " along Y";
- addJointAndBody(model, typename JointCollection::JointModelRUBY(),
- parentFrameId,jointPlacement,joint->name,
- Y,link->name,
- max_effort,max_velocity,
- lower_position, upper_position);
- break;
-
- case AXIS_Z:
- joint_info << " along Z";
- addJointAndBody(model, typename JointCollection::JointModelRUBZ(),
- parentFrameId,jointPlacement,joint->name,
- Y,link->name,
- max_effort,max_velocity,
- lower_position, upper_position);
- break;
-
- case AXIS_UNALIGNED:
- {
- typename SE3::Vector3 joint_axis((Scalar)joint->axis.x,(Scalar)joint->axis.y,(Scalar)joint->axis.z);
- joint_info << " unaligned along (" << joint_axis.transpose() << ")";
-
- typedef typename JointCollection::JointModelRevoluteUnboundedUnaligned::ConfigVector_t ConfigVector_t;
-
- const Scalar infty = std::numeric_limits<Scalar>::infinity();
- ConfigVector_t lower_position(ConfigVector_t::Constant(-infty));
- ConfigVector_t upper_position(ConfigVector_t::Constant(infty));
-
- addJointAndBody(model, typename JointCollection::JointModelRevoluteUnboundedUnaligned(joint_axis.normalized()),
- parentFrameId,jointPlacement,joint->name,
- Y,link->name,
- max_effort,max_velocity,
- lower_position,upper_position);
- break;
- }
-
- default:
- PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "The axis type of the revolute joint is of wrong type.");
- break;
- }
- break;
- }
+ void appendBodyToJoint(
+ const FrameIndex fid,
+ const Inertia& Y,
+ const SE3 & placement,
+ const std::string & body_name)
+ {
+ const Frame & frame = model.frames[fid];
+ const SE3 & p = frame.placement * placement;
+ if(frame.parent > 0 && Y.mass() > Eigen::NumTraits<Scalar>::epsilon())
+ {
+ model.appendBodyToJoint(frame.parent, Y, p);
+ }
+
+ model.addBodyFrame(body_name, frame.parent, p, (int)fid);
+ // Reference to model.frames[fid] can has changed because the vector
+ // may have been reallocated.
+ if (model.frames[fid].parent > 0)
+ {
+ assert ( !hasNaN(model.inertias[model.frames[fid].parent].lever())
+ && !hasNaN(model.inertias[model.frames[fid].parent].inertia().data()));
+ }
+ }
- case ::urdf::Joint::PRISMATIC:
- {
- joint_info << "joint PRISMATIC with axis";
+ FrameIndex getJointFrameId (const std::string& frame_name) const
+ {
+ static const FrameType JOINT_OR_FIXED_JOINT = (FrameType)(JOINT | FIXED_JOINT);
+ if (model.existFrame(frame_name, JOINT_OR_FIXED_JOINT)) {
+ FrameIndex fid = model.getFrameId (frame_name, JOINT_OR_FIXED_JOINT);
+ assert(model.frames[fid].type == JOINT
+ || model.frames[fid].type == FIXED_JOINT);
+ return fid;
+ } else
+ throw std::invalid_argument("Model does not have any joints named "
+ + frame_name);
+ }
- typedef JointModelRX::ConfigVector_t ConfigVector_t;
- typedef JointModelRX::TangentVector_t TangentVector_t;
+ private:
+ ///
+ /// \brief The four possible cartesian types of an 3D axis.
+ ///
+ enum CartesianAxis { AXIS_X=0, AXIS_Y=1, AXIS_Z=2, AXIS_UNALIGNED };
+
+ ///
+ /// \brief Extract the cartesian property of a particular 3D axis.
+ ///
+ /// \param[in] axis The input URDF axis.
+ ///
+ /// \return The property of the particular axis pinocchio::urdf::CartesianAxis.
+ ///
+ static inline CartesianAxis extractCartesianAxis (const Vector3 & axis)
+ {
+ if( axis == Vector3(1., 0., 0.))
+ return AXIS_X;
+ else if( axis == Vector3(0., 1., 0.))
+ return AXIS_Y;
+ else if( axis == Vector3(0., 0., 1.))
+ return AXIS_Z;
+ else
+ return AXIS_UNALIGNED;
+ }
- TangentVector_t max_effort;
- TangentVector_t max_velocity;
- ConfigVector_t lower_position;
- ConfigVector_t upper_position;
+ template <typename TypeX, typename TypeY, typename TypeZ,
+ typename TypeUnaligned>
+ JointIndex addJoint(
+ const Vector3& axis,
+ const Frame & frame,
+ const SE3 & placement,
+ const std::string & joint_name,
+ const VectorConstRef& max_effort,
+ const VectorConstRef& max_velocity,
+ const VectorConstRef& min_config,
+ const VectorConstRef& max_config)
+ {
+ CartesianAxis axisType = extractCartesianAxis(axis);
+ switch (axisType)
+ {
+ case AXIS_X:
+ return model.addJoint(frame.parent, TypeX(),
+ frame.placement * placement, joint_name,
+ max_effort,max_velocity,min_config,max_config);
+ break;
- if (joint->limits)
- {
- max_effort << joint->limits->effort;
- max_velocity << joint->limits->velocity;
- lower_position << joint->limits->lower;
- upper_position << joint->limits->upper;
- }
+ case AXIS_Y:
+ return model.addJoint(frame.parent, TypeY(),
+ frame.placement * placement, joint_name,
+ max_effort,max_velocity,min_config,max_config);
+ break;
- CartesianAxis axis = extractCartesianAxis(joint->axis);
- switch(axis)
- {
- case AXIS_X:
- joint_info << " along X";
- addJointAndBody(model, typename JointCollection::JointModelPX(),
- parentFrameId,jointPlacement,joint->name,
- Y,link->name,
- max_effort,max_velocity,
- lower_position, upper_position);
- break;
-
- case AXIS_Y:
-
- joint_info << " along Y";
- addJointAndBody(model, typename JointCollection::JointModelPY(),
- parentFrameId,jointPlacement,joint->name,
- Y,link->name,
- max_effort,max_velocity,
- lower_position, upper_position);
- break;
-
- case AXIS_Z:
- joint_info << " along Z";
- addJointAndBody(model, typename JointCollection::JointModelPZ(),
- parentFrameId,jointPlacement,joint->name,
- Y,link->name,
- max_effort,max_velocity,
- lower_position, upper_position);
- break;
-
- case AXIS_UNALIGNED:
- {
- typename SE3::Vector3 joint_axis((Scalar)joint->axis.x,(Scalar)joint->axis.y,(Scalar)joint->axis.z);
- joint_info << " unaligned along (" << joint_axis.transpose() << ")";
-
- addJointAndBody(model, typename JointCollection::JointModelPrismaticUnaligned(joint_axis.normalized()),
- parentFrameId,jointPlacement,joint->name,
- Y,link->name,
- max_effort,max_velocity,
- lower_position, upper_position);
- break;
- }
-
- default:
- PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "The axis type of the prismatic joint is of wrong type.");
- break;
- }
- break;
- }
+ case AXIS_Z:
+ return model.addJoint(frame.parent, TypeZ(),
+ frame.placement * placement, joint_name,
+ max_effort,max_velocity,min_config,max_config);
+ break;
- case ::urdf::Joint::PLANAR:
- {
- joint_info << "joint PLANAR with normal axis along Z";
+ case AXIS_UNALIGNED:
+ return model.addJoint(frame.parent, TypeUnaligned (axis.normalized()),
+ frame.placement * placement, joint_name,
+ max_effort,max_velocity,min_config,max_config);
+ break;
+ default:
+ PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "The axis type of the joint is of wrong type.");
+ break;
+ }
+ }
+ };
- typedef JointModelPlanar::ConfigVector_t ConfigVector_t;
- typedef JointModelPlanar::TangentVector_t TangentVector_t;
+ template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
+ class UrdfVisitorWithRootJoint : public UrdfVisitor<Scalar, Options, JointCollectionTpl>
+ {
+ public:
+ typedef UrdfVisitor<Scalar, Options, JointCollectionTpl> Base;
+ typedef typename Base::Inertia Inertia;
+ using Base::model;
+ using Base::appendBodyToJoint;
- TangentVector_t max_effort;
- TangentVector_t max_velocity;
- ConfigVector_t lower_position;
- ConfigVector_t upper_position;
+ typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
+ typedef typename Model::JointCollection JointCollection;
- if (joint->limits)
- {
- max_effort << joint->limits->effort;
- max_velocity << joint->limits->velocity;
- lower_position << joint->limits->lower;
- upper_position << joint->limits->upper;
- }
+ JointModel root_joint;
- addJointAndBody(model, typename JointCollection::JointModelPlanar(),
- parentFrameId,jointPlacement,joint->name,
- Y,link->name,
- max_effort,max_velocity,
- lower_position, upper_position);
+ UrdfVisitorWithRootJoint (Model& model, const JointModelBase<JointModel> & root_joint)
+ : Base (model), root_joint (root_joint.derived()) {}
+ void addRootJoint (const Inertia& Y, const std::string & body_name)
+ {
+ const Frame & frame = model.frames[0];
+
+ JointIndex idx = model.addJoint(frame.parent, root_joint,
+ SE3::Identity(), "root_joint"
+ //TODO ,max_effort,max_velocity,min_config,max_config
+ );
+ int res (model.addJointFrame(idx, 0));
+ if (res == -1) {
+ std::ostringstream oss;
+ oss << "root_joint already inserted as a frame. Current frames "
+ "are [";
+ for (typename PINOCCHIO_ALIGNED_STD_VECTOR(Frame)::const_iterator it =
+ model.frames.begin (); it != model.frames.end (); ++it) {
+ oss << '"' << it->name << "\",";
}
- break;
-
-
- case ::urdf::Joint::FIXED:
- // In case of fixed joint, if link has inertial tag:
- // -add the inertia of the link to his parent in the model
- // Otherwise do nothing.
- // In all cases:
- // -let all the children become children of parent
- // -inform the parser of the offset to apply
- // -add fixed body in model to display it in gepetto-viewer
-
- joint_info << "fixed joint";
- addFixedJointAndBody(model, parentFrameId, jointPlacement,
- joint_name, Y, link_name);
-
- break;
-
- default:
- {
- const std::string exception_message("The type of joint " + joint_name + " is not supported.");
- throw std::invalid_argument(exception_message);
- break;
- }
- }
+ oss << ']';
+ throw std::invalid_argument(oss.str());
+ }
- if(verbose)
- {
- const Inertia YY = (!Y) ? Inertia::Zero() : convertFromUrdf(*Y);
- std::cout << "Adding Body" << std::endl;
- std::cout << "\"" << link_name << "\" connected to " << "\"" << parent_link_name << "\" throw joint " << "\"" << joint_name << "\"" << std::endl;
- std::cout << "joint type: " << joint_info.str() << std::endl;
- std::cout << "joint placement:\n" << jointPlacement;
- std::cout << "body info: " << std::endl;
- std::cout << " " << "mass: " << YY.mass() << std::endl;
- std::cout << " " << "lever: " << YY.lever().transpose() << std::endl;
- std::cout << " " << "inertia elements (Ixx,Iyx,Iyy,Izx,Izy,Izz): " << YY.inertia().data().transpose() << std::endl << std::endl;
+ FrameIndex jointFrameId = (FrameIndex) res; // C-style cast to remove polluting compilation warning. This is Bad practice. See issue #323 (rework indexes)
+ appendBodyToJoint(jointFrameId, Y, SE3::Identity(), body_name);
}
- }
- else if (link->getParent())
- {
- const std::string exception_message (link->name + " - joint information missing.");
- throw std::invalid_argument(exception_message);
- }
-
- BOOST_FOREACH(::urdf::LinkConstSharedPtr child, link->child_links)
- {
- parseTree(child, model, verbose);
- }
- }
-
- ///
- /// \brief Parse a tree with a specific root joint linking the model to the environment.
- /// The function returns an exception as soon as a necessary Inertia or Joint information are missing.
- ///
- /// \param[in] link The current URDF link.
- /// \param[in] model The model where the link must be added.
- /// \param[in] verbose Print parsing info.
- ///
- template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
- void parseRootTree(::urdf::LinkConstSharedPtr root_link,
- ModelTpl<Scalar,Options,JointCollectionTpl> & model,
- const bool verbose)
- {
- addFixedJointAndBody(model, 0, SE3::Identity(), "root_joint",
- root_link->inertial, root_link->name);
-
- BOOST_FOREACH(::urdf::LinkConstSharedPtr child, root_link->child_links)
- {
- parseTree(child, model, verbose);
- }
- }
-
- ///
- /// \brief Parse a tree with a specific root joint linking the model to the environment.
- /// The function returns an exception as soon as a necessary Inertia or Joint information are missing.
- ///
- /// \param[in] link The current URDF link.
- /// \param[in] model The model where the link must be added.
- /// \param[in] root_joint The specific root joint.
- /// \param[in] verbose Print parsing info.
- ///
- template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename JointModel>
- void parseRootTree(::urdf::LinkConstSharedPtr root_link,
- ModelTpl<Scalar,Options,JointCollectionTpl> & model,
- const JointModelBase<JointModel> & root_joint,
- const bool verbose)
- {
- addJointAndBody(model,root_joint,
- 0,SE3::Identity(),"root_joint",
- root_link->inertial,root_link->name);
-
- BOOST_FOREACH(::urdf::LinkConstSharedPtr child, root_link->child_links)
- {
- parseTree(child, model, verbose);
- }
- }
- } // namespace details
-
- ///
- /// \brief Call parse root tree templated function
- ///
- template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
- struct ParseRootTreeVisitor : public boost::static_visitor<>
- {
- typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
- ::urdf::LinkConstSharedPtr m_root_link;
- Model & m_model;
- const bool m_verbose;
-
- ParseRootTreeVisitor(::urdf::LinkConstSharedPtr root_link,
- Model & model,
- const bool verbose)
- : m_root_link(root_link)
- , m_model(model)
- , m_verbose(verbose)
- {}
-
- template<typename JointModel>
- void operator()(const JointModelBase<JointModel> & root_joint) const
- {
- details::parseRootTree(m_root_link,m_model,root_joint,m_verbose);
- }
-
- static void run(::urdf::LinkConstSharedPtr root_link,
- Model & model,
- const typename Model::JointModel & root_joint,
- const bool verbose)
- {
- boost::apply_visitor(ParseRootTreeVisitor(root_link,model,verbose),root_joint);
- }
- }; // struct ParseRootTreeVisitor
+ };
+
+ typedef UrdfVisitorBaseTpl<double, 0> UrdfVisitorBase;
+
+ void parseRootTree(const ::urdf::ModelInterface * urdfTree,
+ UrdfVisitorBase& model);
+
+ void parseRootTree(const std::string & filename,
+ UrdfVisitorBase& model);
+
+ void parseRootTreeFromXML(const std::string & xmlString,
+ UrdfVisitorBase& model);
+ }
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
ModelTpl<Scalar,Options,JointCollectionTpl> &
@@ -613,39 +377,22 @@ namespace pinocchio
const typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointModel & root_joint,
ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const bool verbose)
-
{
- ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile (filename);
-
- if (urdfTree)
- {
- return buildModel (urdfTree, root_joint, model, verbose);
- }
- else
- {
- const std::string exception_message ("The file " + filename + " does not contain a valid URDF model.");
- throw std::invalid_argument(exception_message);
- }
+ details::UrdfVisitorWithRootJoint<Scalar, Options, JointCollectionTpl> visitor (model, root_joint);
+ if (verbose) visitor.log = &std::cout;
+ parseRootTree(filename, visitor);
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,
const bool verbose)
-
{
- ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile(filename);
- if (urdfTree)
- {
- return buildModel(urdfTree, model, verbose);
- }
- else
- {
- const std::string exception_message("The file " + filename + " does not contain a valid URDF model.");
- throw std::invalid_argument(exception_message);
- }
-
+ details::UrdfVisitor<Scalar, Options, JointCollectionTpl> visitor (model);
+ if (verbose) visitor.log = &std::cout;
+ parseRootTree(filename, visitor);
return model;
}
@@ -656,16 +403,9 @@ namespace pinocchio
ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const bool verbose)
{
- ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDF(xmlStream);
-
- if (urdfTree)
- return buildModel(urdfTree, rootJoint, model, verbose);
- else
- {
- const std::string exception_message ("The XML stream does not contain a valid URDF model.");
- throw std::invalid_argument(exception_message);
- }
-
+ details::UrdfVisitorWithRootJoint<Scalar, Options, JointCollectionTpl> visitor (model, rootJoint);
+ if (verbose) visitor.log = &std::cout;
+ parseRootTreeFromXML(xmlStream, visitor);
return model;
}
@@ -675,41 +415,36 @@ namespace pinocchio
ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const bool verbose)
{
- ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDF(xmlStream);
-
- if (urdfTree)
- return buildModel(urdfTree, model, verbose);
- else
- {
- const std::string exception_message ("The XML stream does not contain a valid URDF model.");
- throw std::invalid_argument(exception_message);
- }
-
+ details::UrdfVisitor<Scalar, Options, JointCollectionTpl> visitor (model);
+ if (verbose) visitor.log = &std::cout;
+ parseRootTreeFromXML(xmlStream, visitor);
return model;
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
ModelTpl<Scalar,Options,JointCollectionTpl> &
- buildModel(const ::urdf::ModelInterfaceSharedPtr & urdfTree,
- const typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointModel & root_joint,
+ buildModel(const ::urdf::ModelInterface* urdfTree,
+ const typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointModel & rootJoint,
ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const bool verbose)
{
- PINOCCHIO_CHECK_INPUT_ARGUMENT(urdfTree);
- model.name = urdfTree->getName();
- ParseRootTreeVisitor<Scalar,Options,JointCollectionTpl>::run(urdfTree->getRoot(),model,root_joint,verbose);
+ PINOCCHIO_CHECK_INPUT_ARGUMENT(urdfTree != NULL);
+ details::UrdfVisitorWithRootJoint<Scalar, Options, JointCollectionTpl> visitor (model, rootJoint);
+ if (verbose) visitor.log = &std::cout;
+ parseRootTree(urdfTree, visitor);
return model;
}
-
+
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
ModelTpl<Scalar,Options,JointCollectionTpl> &
- buildModel(const ::urdf::ModelInterfaceSharedPtr & urdfTree,
+ buildModel(const ::urdf::ModelInterface* urdfTree,
ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const bool verbose)
{
- PINOCCHIO_CHECK_INPUT_ARGUMENT(urdfTree);
- model.name = urdfTree->getName();
- details::parseRootTree(urdfTree->getRoot(),model,verbose);
+ PINOCCHIO_CHECK_INPUT_ARGUMENT(urdfTree != NULL);
+ details::UrdfVisitor<Scalar, Options, JointCollectionTpl> visitor (model);
+ if (verbose) visitor.log = &std::cout;
+ parseRootTree(urdfTree, visitor);
return model;
}
diff --git a/src/parsers/urdf/utils.hpp b/src/parsers/urdf/utils.hpp
index 1772939a9..056da1537 100644
--- a/src/parsers/urdf/utils.hpp
+++ b/src/parsers/urdf/utils.hpp
@@ -5,76 +5,6 @@
#ifndef __pinocchio_parsers_urdf_utils_hpp__
#define __pinocchio_parsers_urdf_utils_hpp__
-#include <urdf_model/model.h>
+#warning "This file is not deprecated and will be removed in future releases. There is no replacement."
-#include "pinocchio/spatial/se3.hpp"
-#include "pinocchio/spatial/inertia.hpp"
-
-namespace pinocchio
-{
- namespace urdf
- {
-
- ///
- /// \brief Convert URDF Inertial quantity to Spatial Inertia.
- ///
- /// \param[in] Y The input URDF Inertia.
- ///
- /// \return The converted Spatial Inertia pinocchio::Inertia.
- ///
- inline 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();
-
- Eigen::Matrix3d 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());
- }
-
- ///
- /// \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));
- }
-
- ///
- /// \brief The four possible cartesian types of an 3D axis.
- ///
- enum CartesianAxis { AXIS_X=0, AXIS_Y=1, AXIS_Z=2, AXIS_UNALIGNED };
-
- ///
- /// \brief Extract the cartesian property of a particular 3D axis.
- ///
- /// \param[in] axis The input URDF axis.
- ///
- /// \return The property of the particular axis pinocchio::urdf::CartesianAxis.
- ///
- inline CartesianAxis extractCartesianAxis (const ::urdf::Vector3 & axis)
- {
- if( (axis.x==1.0)&&(axis.y==0.0)&&(axis.z==0.0) )
- return AXIS_X;
- else if( (axis.x==0.0)&&(axis.y==1.0)&&(axis.z==0.0) )
- return AXIS_Y;
- else if( (axis.x==0.0)&&(axis.y==0.0)&&(axis.z==1.0) )
- return AXIS_Z;
- else
- return AXIS_UNALIGNED;
- }
-
- } //urdf
-} // se3
#endif // __pinocchio_parsers_urdf_utils_hpp__
--
GitLab