Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Switch to using Eigen for Quaternion and Matrix. #21

Merged
merged 2 commits into from Jan 5, 2018
Merged

Switch to using Eigen for Quaternion and Matrix. #21

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
c5152ca
Switch to using Eigen for Quaternion and Matrix.
clalancette Nov 21, 2017
4edadbf
Simplify Eigen3 dependency.
clalancette Jan 5, 2018
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
26 changes: 23 additions & 3 deletions collada_urdf/CMakeLists.txt
View file
Open in desktop
@@ -1,12 +1,12 @@
cmake_minimum_required(VERSION 2.8.12)
project(collada_urdf)

find_package(catkin REQUIRED COMPONENTS angles collada_parser resource_retriever urdf geometric_shapes tf cmake_modules)
find_package(catkin REQUIRED COMPONENTS angles collada_parser resource_retriever urdf geometric_shapes cmake_modules)

catkin_package(
LIBRARIES ${PROJECT_NAME}
INCLUDE_DIRS include
DEPENDS angles collada_parser resource_retriever urdf geometric_shapes tf)
DEPENDS angles collada_parser resource_retriever urdf geometric_shapes)

include(CheckCXXCompilerFlag)
check_cxx_compiler_flag(-std=c++11 HAS_STD_CPP11_FLAG)
Expand Down Expand Up @@ -48,13 +48,33 @@ endif()
find_package(Boost REQUIRED COMPONENTS system filesystem program_options)
include_directories(${Boost_INCLUDE_DIR})

# We don't build anything here, but in order to export the right
# build flags in catkin_package(), we still have to find Eigen3 here. Note
# that this is somewhat complicated because of our need to support Ubuntu
# all the way back to saucy. First we look for the Eigen3 cmake module
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

REP 3 says kinetic targets xenial and above. libeigen3-dev is in trusty and above. I think this block can be replaced with find_package(Eigen3 REQUIRED)

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Ah, good point. It looks like Kinetic used to support Wily, but we've long stopped worrying about that. Fixed in 4edadbf

# provided by the libeigen3-dev on newer Ubuntu. If that fails, then we
# fall-back to the version provided by cmake_modules, which is a stand-in.
find_package(Eigen3 QUIET)
if(NOT EIGEN3_FOUND)
# saucy does not have Eigen3, but have Eigen instead
find_package(cmake_modules REQUIRED)
find_package(Eigen REQUIRED)
set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS})
endif()

# Note that eigen 3.2 (on Ubuntu Wily) only provides EIGEN3_INCLUDE_DIR,
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Same here, xenial through bionic is 3.3.*

# not EIGEN3_INCLUDE_DIRS, so we have to set the latter from the former.
if(NOT EIGEN3_INCLUDE_DIRS)
set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
endif()

find_package(COLLADA_DOM 2.3 COMPONENTS 1.5)
if( COLLADA_DOM_FOUND )
include_directories(${COLLADA_DOM_INCLUDE_DIRS})
link_directories(${COLLADA_DOM_LIBRARY_DIRS})
endif()

include_directories(${catkin_INCLUDE_DIRS})
include_directories(${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS})
link_directories(${catkin_LIBRARY_DIRS})

add_library(${PROJECT_NAME} src/collada_urdf.cpp)
Expand Down
3 changes: 1 addition & 2 deletions collada_urdf/package.xml
View file
Open in desktop
Expand Up @@ -31,8 +31,8 @@
<build_depend>roscpp</build_depend>
<build_depend>urdf</build_depend>
<build_depend>geometric_shapes</build_depend>
<build_depend>tf</build_depend>
<build_depend>cmake_modules</build_depend>
<build_depend>eigen</build_depend>

<run_depend>angles</run_depend>
<run_depend>assimp</run_depend>
Expand All @@ -43,7 +43,6 @@
<run_depend>resource_retriever</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>urdf</run_depend>
<run_depend>tf</run_depend>
<run_depend>geometric_shapes</run_depend>

</package>
47 changes: 27 additions & 20 deletions collada_urdf/src/collada_to_urdf.cpp
View file
Open in desktop
Expand Up @@ -28,8 +28,7 @@
#include <boost/filesystem/path.hpp>
#include <boost/filesystem/operations.hpp>

#include <tf/LinearMath/Transform.h>
#include <tf/LinearMath/Quaternion.h>
#include <Eigen/Geometry>

#undef GAZEBO_1_3

Expand Down Expand Up @@ -340,19 +339,27 @@ void addChildLinkNamesXML(urdf::LinkConstSharedPtr link, ofstream& os)
os << " <mass value=\"" << link->inertial->mass << "\" />" << endl;
os << " <origin ";

tf::Quaternion qt (link->inertial->origin.rotation.x,
link->inertial->origin.rotation.y,
link->inertial->origin.rotation.z,
link->inertial->origin.rotation.w);
tf::Matrix3x3 mat (qt);
tf::Matrix3x3 tmat (mat.transpose());
tf::Matrix3x3 imat (link->inertial->ixx, link->inertial->ixy, link->inertial->ixz,
link->inertial->ixy, link->inertial->iyy, link->inertial->iyz,
link->inertial->ixz, link->inertial->iyz, link->inertial->izz);
Eigen::Quaterniond qt(link->inertial->origin.rotation.w,
link->inertial->origin.rotation.x,
link->inertial->origin.rotation.y,
link->inertial->origin.rotation.z);
Eigen::Matrix3d mat(qt);
Eigen::Matrix3d tmat(mat.transpose());
Eigen::Matrix3d imat;
imat(0, 0) = link->inertial->ixx;
imat(0, 1) = link->inertial->ixy;
imat(0, 2) = link->inertial->ixz;
imat(1, 0) = link->inertial->ixy;
imat(1, 1) = link->inertial->iyy;
imat(1, 2) = link->inertial->iyz;
imat(2, 0) = link->inertial->ixz;
imat(2, 1) = link->inertial->iyz;
imat(2, 2) = link->inertial->izz;

#define DEBUG_MAT(mat) \
cout << "#2f((" << mat[0][0] << " " << mat[0][1] << " " << mat[0][2] << ")"; \
cout << "(" << mat[1][0] << " " << mat[1][1] << " " << mat[1][2] << ")"; \
cout << "(" << mat[2][0] << " " << mat[2][1] << " " << mat[2][2] << "))" << endl;
cout << "#2f((" << mat(0, 0) << " " << mat(0, 1) << " " << mat(0, 2) << ")"; \
cout << "(" << mat(1, 0) << " " << mat(1, 1) << " " << mat(1, 2) << ")"; \
cout << "(" << mat(2, 0) << " " << mat(2, 1) << " " << mat(2, 2) << "))" << endl;

#if DEBUG
DEBUG_MAT(mat);
Expand All @@ -372,12 +379,12 @@ void addChildLinkNamesXML(urdf::LinkConstSharedPtr link, ofstream& os)
PRINT_ORIGIN_XML(os, t_pose);
os << "/>" << endl;

os << " <inertia ixx=\"" << imat[0][0] << "\" ";
os << "ixy=\"" << imat[0][1] << "\" ";
os << "ixz=\"" << imat[0][2] << "\" ";
os << "iyy=\"" << imat[1][1] << "\" ";
os << "iyz=\"" << imat[1][2] << "\" ";
os << "izz=\"" << imat[2][2] << "\"/>" << endl;
os << " <inertia ixx=\"" << imat(0, 0) << "\" ";
os << "ixy=\"" << imat(0, 1) << "\" ";
os << "ixz=\"" << imat(0, 2) << "\" ";
os << "iyy=\"" << imat(1, 1) << "\" ";
os << "iyz=\"" << imat(1, 2) << "\" ";
os << "izz=\"" << imat(2, 2) << "\"/>" << endl;
os << " </inertial>" << endl;
}
}
Expand Down