Commits

Martin Felis committed d40851d Merge

Merged in bchretien/rbdl/urdfdom (pull request #5)

Use urdfdom for urdfreader addon

Sorry for the late reply, but thanks a *lot*!

  • Participants
  • Parent commits af4ccde, b068992

Comments (0)

Files changed (8)

File addons/urdfreader/CMakeLists.txt

 
 LIST( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/CMake )
 
-INCLUDE ( CMake/ros.cmake)
 INCLUDE ( CMake/pkg-config.cmake)
 
-ADD_ROSPACK_DEPENDENCY ("urdf")
-ADD_ROSPACK_DEPENDENCY ("urdf_interface")
-ADD_ROSPACK_DEPENDENCY ("urdf_parser")
+# Find urdfdom
+FIND_PACKAGE(PkgConfig)
+PKG_CHECK_MODULES(URDFDOM REQUIRED urdfdom>=0.2.9)
+PKG_CHECK_MODULES(URDFDOM_HEADERS REQUIRED urdfdom_headers>=0.2.3)
 
-# We additionally link against urdf explicitly
-FIND_PACKAGE (URDF REQUIRED)
-INCLUDE_DIRECTORIES (${URDF_INCLUDE_DIR})
-
-INCLUDE_DIRECTORIES ( 
+INCLUDE_DIRECTORIES (
 	${CMAKE_CURRENT_BINARY_DIR}/include/rbdl
+	SYSTEM
+	${URDFDOM_INCLUDE_DIRS}
+	${URDFDOM_HEADERS_INCLUDE_DIRS}
 )
 
 SET_TARGET_PROPERTIES ( ${PROJECT_EXECUTABLES} PROPERTIES
 SET (CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
 
 # Options
-SET ( URDFREADER_SOURCES 
+SET ( URDFREADER_SOURCES
 	rbdl_urdfreader.cc
 	)
 
 
 ADD_EXECUTABLE (rbdl_urdfreader_util rbdl_urdfreader_util.cc)
 
-ROSPACK_USE_DEPENDENCY (rbdl_urdfreader urdf)
-ROSPACK_USE_DEPENDENCY (rbdl_urdfreader urdf_interface)
-
-ROSPACK_USE_DEPENDENCY (rbdl_urdfreader_util urdf)
-ROSPACK_USE_DEPENDENCY (rbdl_urdfreader_util urdf_interface)
 
 IF (BUILD_STATIC)
 	ADD_LIBRARY ( rbdl_urdfreader-static STATIC ${URDFREADER_SOURCES} )
 
-	ROSPACK_USE_DEPENDENCY (rbdl_urdfreader-static urdf)
-	ROSPACK_USE_DEPENDENCY (rbdl_urdfreader-static urdf_interface)
-
-  SET_TARGET_PROPERTIES ( rbdl_urdfreader-static PROPERTIES PREFIX "lib")
-  SET_TARGET_PROPERTIES ( rbdl_urdfreader-static PROPERTIES OUTPUT_NAME "rbdl_urdfreader")
+	SET_TARGET_PROPERTIES ( rbdl_urdfreader-static PROPERTIES PREFIX "lib")
+	SET_TARGET_PROPERTIES ( rbdl_urdfreader-static PROPERTIES OUTPUT_NAME "rbdl_urdfreader")
 
 	INSTALL (TARGETS rbdl_urdfreader-static
 	  LIBRARY DESTINATION lib
 
 TARGET_LINK_LIBRARIES (rbdl_urdfreader
 	rbdl
-	${URDF_LIBRARY}
+	${URDFDOM_LIBRARIES}
 	)
 
 TARGET_LINK_LIBRARIES (rbdl_urdfreader_util
 	ARCHIVE DESTINATION lib
 	)
 
-FILE ( GLOB headers 
+FILE ( GLOB headers
 	"${CMAKE_CURRENT_SOURCE_DIR}/*.h"
 	)
 

File addons/urdfreader/README.md

 Requirements
 ============
 
-This addon depends on ROS, the Robot Operating System (http://www.ros.org)
-and uses its urdf_parser and urdf_interface to load and access the model
-data in the files. You therefore need to have ROS and the package
-"robot-model" installed on your system to be able to compile this addon. So
-far only the ROS release "Fuerte" can be used with this code.
+This addon depends on urdfdom to load and access the model data in the URDF
+files.
 
-See http://www.ros.org/wiki/ROS/Installation for more details on how to
-install ROS.
+See https://github.com/ros/urdfdom for more details on how to
+install urdfdom.
 
 Warning
 =======

File addons/urdfreader/rbdl_urdfreader.cc

-#include "rbdl.h"
+#include <rbdl/rbdl.h>
+
 #include "rbdl_urdfreader.h"
 
 #include <assert.h>
 #include <map>
 #include <stack>
 
-#include "urdf/model.h"
+#include <urdf_model/model.h>
+#include <urdf_parser/urdf_parser.h>
 
 using namespace std;
 
 
 typedef boost::shared_ptr<urdf::Link> LinkPtr;
 typedef boost::shared_ptr<urdf::Joint> JointPtr;
+typedef boost::shared_ptr<urdf::ModelInterface> ModelPtr;
 
 typedef vector<LinkPtr> URDFLinkVector;
 typedef vector<JointPtr> URDFJointVector;
 typedef map<string, LinkPtr > URDFLinkMap;
 typedef map<string, JointPtr > URDFJointMap;
 
-bool construct_model (Model* rbdl_model, urdf::Model *urdf_model, bool verbose) {
+bool construct_model (Model* rbdl_model, ModelPtr urdf_model, bool verbose) {
 	boost::shared_ptr<urdf::Link> urdf_root_link;
 
 	URDFLinkMap link_map;
 	// add the bodies in a depth-first order of the model tree
 	link_stack.push (link_map[(urdf_model->getRoot()->name)]);
 
+	// add the root body
+	const boost::shared_ptr<const urdf::Link>& root = urdf_model->getRoot ();
+	Vector3d root_inertial_rpy;
+	Vector3d root_inertial_position;
+	Matrix3d root_inertial_inertia;
+	double root_inertial_mass;
+
+	if (root->inertial)
+	{
+		root_inertial_mass = root->inertial->mass;
+
+		root_inertial_position.set (
+			root->inertial->origin.position.x,
+			root->inertial->origin.position.y,
+			root->inertial->origin.position.z);
+
+		root_inertial_inertia(0,0) = root->inertial->ixx;
+		root_inertial_inertia(0,1) = root->inertial->ixy;
+		root_inertial_inertia(0,2) = root->inertial->ixz;
+
+		root_inertial_inertia(1,0) = root->inertial->ixy;
+		root_inertial_inertia(1,1) = root->inertial->iyy;
+		root_inertial_inertia(1,2) = root->inertial->iyz;
+
+		root_inertial_inertia(2,0) = root->inertial->ixz;
+		root_inertial_inertia(2,1) = root->inertial->iyz;
+		root_inertial_inertia(2,2) = root->inertial->izz;
+
+		root->inertial->origin.rotation.getRPY (root_inertial_rpy[0], root_inertial_rpy[1], root_inertial_rpy[2]);
+	}
+	Body root_link = Body (root_inertial_mass,
+	root_inertial_position,
+	root_inertial_inertia);
+	Joint root_joint = Joint (
+		SpatialVector (0., 0., 0., 1., 0., 0.),
+		SpatialVector (0., 0., 0., 0., 1., 0.),
+		SpatialVector (0., 0., 0., 0., 0., 1.),
+		SpatialVector (1., 0., 0., 0., 0., 0.),
+		SpatialVector (0., 1., 0., 0., 0., 0.),
+		SpatialVector (0., 0., 1., 0., 0., 0.));
+
+	SpatialTransform root_joint_frame = SpatialTransform ();
+
+	if (verbose) {
+		cout << "+ Adding Root Body " << endl;
+		cout << "  joint frame: " << root_joint_frame << endl;
+		cout << "  joint dofs : " << root_joint.mDoFCount << endl;
+		for (unsigned int j = 0; j < root_joint.mDoFCount; j++) {
+			cout << "    " << j << ": " << root_joint.mJointAxes[j].transpose() << endl;
+		}
+		cout << "  body inertia: " << endl << root_link.mSpatialInertia << endl;
+		cout << "  body mass   : " << root_link.mMass << endl;
+		cout << "  body name   : " << root->name << endl;
+	}
+
+	rbdl_model->AppendBody(root_joint_frame,
+		root_joint,
+		root_link,
+		root->name);
+
 	if (link_stack.top()->child_joints.size() > 0) {
 		joint_index_stack.push(0);
 	}
 				for (int i = 1; i < joint_index_stack.size() - 1; i++) {
 					cout << "  ";
 				}
-				cout << "joint '" << cur_joint->name << "' child link '" << link_stack.top()->name << " type = " << cur_joint->type << endl;
+				cout << "joint '" << cur_joint->name << "' child link '" << link_stack.top()->name << "' type = " << cur_joint->type << endl;
 			}
 
 			joint_names.push_back(cur_joint->name);
 
 		// determine where to add the current joint and child body
 		unsigned int rbdl_parent_id = 0;
-		
+
 		if (urdf_parent->name != "base_joint" && rbdl_model->mBodies.size() != 1)
 			rbdl_parent_id = rbdl_model->GetBodyId (urdf_parent->name.c_str());
 
-//		cout << "joint: " << urdf_joint.name << "\tparent = " << urdf_parent.name << " child = " << urdf_child.name << " parent_id = " << rbdl_parent_id << endl;
+		if (rbdl_parent_id == std::numeric_limits<unsigned int>::max())
+			cerr << "Error while processing joint '" << urdf_joint->name
+				<< "': parent link '" << urdf_parent->name
+				<< "' could not be found." << endl;
+
+		//cout << "joint: " << urdf_joint->name << "\tparent = " << urdf_parent->name << " child = " << urdf_child->name << " parent_id = " << rbdl_parent_id << endl;
 
 		// create the joint
 		Joint rbdl_joint;
 				urdf_joint->parent_to_joint_origin_transform.position.y,
 				urdf_joint->parent_to_joint_origin_transform.position.z
 				);
-		SpatialTransform rbdl_joint_frame = 
+		SpatialTransform rbdl_joint_frame =
 					Xrot (joint_rpy[0], Vector3d (1., 0., 0.))
 				* Xrot (joint_rpy[1], Vector3d (0., 1., 0.))
 				* Xrot (joint_rpy[2], Vector3d (0., 0., 1.))
 		if (verbose) {
 			cout << "+ Adding Body " << endl;
 			cout << "  parent_id  : " << rbdl_parent_id << endl;
-			cout << "  joint_frame: " << rbdl_joint_frame << endl;
+			cout << "  joint frame: " << rbdl_joint_frame << endl;
 			cout << "  joint dofs : " << rbdl_joint.mDoFCount << endl;
 			for (unsigned int j = 0; j < rbdl_joint.mDoFCount; j++) {
 				cout << "    " << j << ": " << rbdl_joint.mJointAxes[j].transpose() << endl;
 			}
 			cout << "  body inertia: " << endl << rbdl_body.mSpatialInertia << endl;
-			cout << "  body_name  : " << urdf_child->name << endl;
+			cout << "  body mass   : " << rbdl_body.mMass << endl;
+			cout << "  body name   : " << urdf_child->name << endl;
 		}
 
 		rbdl_model->AddBody (rbdl_parent_id, rbdl_joint_frame, rbdl_joint, rbdl_body, urdf_child->name);
 RBDL_DLLAPI bool read_urdf_model (const char* filename, Model* model, bool verbose) {
 	assert (model);
 
-	urdf::Model urdf_model;
-
 	cerr << "Warning: this code (RigidBodyDynamics::Addons::" << __func__ << "()) is not properly tested as" << endl;
 	cerr << "         I do not have a proper urdf model that I can use to validate the model loading." << endl;
 	cerr << "         Please use with care." << endl;
 
-	bool urdf_result = urdf_model.initFile (filename);
-	if (!urdf_result) {
+	boost::shared_ptr<urdf::ModelInterface> urdf_model = urdf::parseURDFFile (filename);
+
+	if (!urdf_model) {
 		cerr << "Error opening urdf file" << endl;
 	}
 
-	if (!construct_model (model, &urdf_model, verbose)) {
+	if (!construct_model (model, urdf_model, verbose)) {
 		cerr << "Error constructing model from urdf file." << endl;
 		return false;
 	}

File addons/urdfreader/rbdl_urdfreader_util.cc

-#include "rbdl.h"
-#include "rbdl_utils.h"
+#include <rbdl/rbdl.h>
+#include <rbdl/rbdl_utils.h>
+
 #include "rbdl_urdfreader.h"
 
 #include <iostream>
 	}
 
 	RigidBodyDynamics::Model model;
-	model.Init();
 
 	if (!RigidBodyDynamics::Addons::read_urdf_model(filename.c_str(), &model, verbose)) {
 		cerr << "Loading of urdf model failed!" << endl;

File examples/urdfreader/CMakeLists.txt

+PROJECT (RBDLEXAMPLE CXX)
+
+CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
+
+# We need to add the project source path to the CMake module path so that
+# the FindRBDL.cmake script can be found.
+LIST( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR} )
+
+# Search for the RBDL include directory and library
+FIND_PACKAGE (RBDL REQUIRED)
+FIND_PACKAGE (Eigen3 REQUIRED)
+
+# Add the include directory to the include paths
+INCLUDE_DIRECTORIES ( ${RBDL_INCLUDE_DIR} ${EIGEN3_INCLUDE_DIR} )
+
+# Create an executable
+ADD_EXECUTABLE (example_urdfreader example_urdfreader.cc)
+
+IF(BUILD_ADDON_URDFREADER)
+set_target_properties(example_urdfreader PROPERTIES COMPILE_DEFINITIONS "BUILD_ADDON_URDFREADER=ON")
+ENDIF()
+
+
+# And link the library against the executable
+TARGET_LINK_LIBRARIES ( example_urdfreader
+	${RBDL_LIBRARIES}
+	)

File examples/urdfreader/FindEigen3.cmake

+# - Try to find Eigen3 lib
+#
+# This module supports requiring a minimum version, e.g. you can do
+#   find_package(Eigen3 3.1.2)
+# to require version 3.1.2 or newer of Eigen3.
+#
+# Once done this will define
+#
+#  EIGEN3_FOUND - system has eigen lib with correct version
+#  EIGEN3_INCLUDE_DIR - the eigen include directory
+#  EIGEN3_VERSION - eigen version
+
+# Copyright (c) 2006, 2007 Montel Laurent, <montel@kde.org>
+# Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael@free.fr>
+# Copyright (c) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
+# Redistribution and use is allowed according to the terms of the 2-clause BSD license.
+
+if(NOT Eigen3_FIND_VERSION)
+  if(NOT Eigen3_FIND_VERSION_MAJOR)
+    set(Eigen3_FIND_VERSION_MAJOR 2)
+  endif(NOT Eigen3_FIND_VERSION_MAJOR)
+  if(NOT Eigen3_FIND_VERSION_MINOR)
+    set(Eigen3_FIND_VERSION_MINOR 91)
+  endif(NOT Eigen3_FIND_VERSION_MINOR)
+  if(NOT Eigen3_FIND_VERSION_PATCH)
+    set(Eigen3_FIND_VERSION_PATCH 0)
+  endif(NOT Eigen3_FIND_VERSION_PATCH)
+
+  set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}")
+endif(NOT Eigen3_FIND_VERSION)
+
+macro(_eigen3_check_version)
+  file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header)
+
+  string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}")
+  set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}")
+  string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}")
+  set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}")
+  string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}")
+  set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}")
+
+  set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION})
+  if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+    set(EIGEN3_VERSION_OK FALSE)
+  else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+    set(EIGEN3_VERSION_OK TRUE)
+  endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+
+  if(NOT EIGEN3_VERSION_OK)
+
+    message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, "
+                   "but at least version ${Eigen3_FIND_VERSION} is required")
+  endif(NOT EIGEN3_VERSION_OK)
+endmacro(_eigen3_check_version)
+
+if (EIGEN3_INCLUDE_DIR)
+
+  # in cache already
+  _eigen3_check_version()
+  set(EIGEN3_FOUND ${EIGEN3_VERSION_OK})
+
+else (EIGEN3_INCLUDE_DIR)
+
+  find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
+      PATHS
+      ${CMAKE_INSTALL_PREFIX}/include
+      ${KDE4_INCLUDE_DIR}
+      PATH_SUFFIXES eigen3 eigen
+    )
+
+  if(EIGEN3_INCLUDE_DIR)
+    _eigen3_check_version()
+  endif(EIGEN3_INCLUDE_DIR)
+
+  include(FindPackageHandleStandardArgs)
+  find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK)
+
+  mark_as_advanced(EIGEN3_INCLUDE_DIR)
+
+endif(EIGEN3_INCLUDE_DIR)

File examples/urdfreader/FindRBDL.cmake

+# Searches for RBDL includes and library files
+#
+# Sets the variables
+#   RBDL_FOUND
+#   RBDL_INCLUDE_DIR
+#   RBDL_LIBRARIES
+
+SET (RBDL_FOUND FALSE)
+
+FIND_PATH (RBDL_INCLUDE_DIR rbdl/rbdl.h
+	/usr/include
+	/usr/local/include
+	$ENV{HOME}/local/include
+	$ENV{RBDL_PATH}/src
+	$ENV{RBDL_PATH}/include
+	$ENV{RBDL_INCLUDE_PATH}
+	)
+FIND_LIBRARY (RBDL_LIBRARY NAMES rbdl	PATHS
+	/usr/lib
+	/usr/local/lib
+	$ENV{HOME}/local/lib
+	$ENV{RBDL_PATH}
+	$ENV{RBDL_LIBRARY_PATH}
+	)
+
+FIND_LIBRARY (RBDL_URDFREADER_LIBRARY NAMES rbdl_urdfreader	PATHS
+	/usr/lib
+	/usr/local/lib
+	$ENV{HOME}/local/lib
+	$ENV{RBDL_PATH}
+	$ENV{RBDL_LIBRARY_PATH}
+	)
+
+IF (RBDL_INCLUDE_DIR AND RBDL_LIBRARY)
+	SET (RBDL_FOUND TRUE)
+ENDIF (RBDL_INCLUDE_DIR AND RBDL_LIBRARY)
+
+IF (RBDL_LIBRARY AND RBDL_URDFREADER_LIBRARY)
+	SET (RBDL_LIBRARIES ${RBDL_LIBRARY} ${RBDL_URDFREADER_LIBRARY})
+ELSE (RBDL_LIBRARY AND RBDL_URDFREADER_LIBRARY)
+	SET (RBDL_LIBRARIES ${RBDL_LIBRARY})
+ENDIF(RBDL_LIBRARY AND RBDL_URDFREADER_LIBRARY)
+
+IF (RBDL_FOUND)
+   IF (NOT RBDL_FIND_QUIETLY)
+      MESSAGE(STATUS "Found RBDL: ${RBDL_LIBRARY}")
+
+			IF (RBDL_URDFREADER_LIBRARY)
+				MESSAGE(STATUS "Found RBDL Addon urdfreader: ${RBDL_URDFREADER_LIBRARY}")
+			ENDIF (RBDL_URDFREADER_LIBRARY)
+   ENDIF (NOT RBDL_FIND_QUIETLY)
+ELSE (RBDL_FOUND)
+   IF (RBDL_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find RBDL")
+   ENDIF (RBDL_FIND_REQUIRED)
+ENDIF (RBDL_FOUND)
+
+MARK_AS_ADVANCED (
+	RBDL_INCLUDE_DIR
+	RBDL_LIBRARIES
+	RBDL_LIBRARY
+	RBDL_URDFREADER_LIBRARY
+	)

File examples/urdfreader/example_urdfreader.cc

+/*
+ * RBDL - Rigid Body Dynamics Library
+ * Copyright (c) 2011-2012 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
+ *
+ * Licensed under the zlib license. See LICENSE for more details.
+ */
+
+#include <iostream>
+
+#include <rbdl/rbdl.h>
+
+#ifndef BUILD_ADDON_URDFREADER
+	#error "Error: RBDL addon BUILD_ADDON_URDFREADER not activated."
+#endif
+
+#include <rbdl/addons/urdfreader/rbdl_urdfreader.h>
+
+using namespace RigidBodyDynamics;
+using namespace RigidBodyDynamics::Math;
+
+int main (int argc, char* argv[]) {
+	rbdl_check_api_version (RBDL_API_VERSION);
+
+	Model* model = new Model();
+
+	if (!Addons::read_urdf_model ("./samplemodel.urdf", model, false)) {
+		std::cerr << "Error loading model ./samplemodel.urdf" << std::endl;
+		abort();
+	}
+
+	VectorNd Q = VectorNd::Zero (model->dof_count);
+	VectorNd QDot = VectorNd::Zero (model->dof_count);
+	VectorNd Tau = VectorNd::Zero (model->dof_count);
+	VectorNd QDDot = VectorNd::Zero (model->dof_count);
+
+ 	ForwardDynamics (*model, Q, QDot, Tau, QDDot);
+
+	std::cout << Q.transpose() << std::endl;
+	std::cout << QDDot.transpose() << std::endl;
+
+	delete model;
+
+ 	return 0;
+}
+