URDF robot model support

.gitignore

@@ -0,0 +1,4 @@
+build/
+*.so
+
+CMakeLists.txt.user

CMakeLists.txt

@@ -20,6 +20,8 @@ find_package(GLUT REQUIRED)
 find_package(GLEW REQUIRED)
 find_package(DEPTHSENSE)
 find_package(OpenNI2)
+find_package(Boost REQUIRED filesystem)
+find_package(urdfdom REQUIRED)
 
 # find packages with pkg-config
 find_package(PkgConfig)
@@ -35,15 +37,25 @@ include_directories(
     ${GLUT_INCLUDE_DIR}
     ${GLEW_INCLUDE_DIRS}
     ${tinyxml_INCLUDE_DIR}
+    ${Boost_INCLUDE_DIRS}
+    ${urdfdom_INCLUDE_DIRS}
 )
-link_directories(${tinyxml_LIBRARY_DIRS})
+
+link_directories(
+    ${tinyxml_LIBRARY_DIRS}
+    ${Boost_LIBRARY_DIRS}
+    ${urdfdom_LIBRARY_DIRS}
+)
+
 link_libraries(
     ${PNG_LIBRARIES}
     ${OPENGL_LIBRARIES}
     ${GLUT_glut_LIBRARY}
     ${GLEW_LIBRARIES}
     ${tinyxml_LIBRARIES}
     ${matheval_LIBRARIES}
+    ${Boost_LIBRARIES}
+    ${urdfdom_LIBRARIES}
 )
 
 # set dart source
@@ -84,6 +96,8 @@ set(dart_src
     ${PROJECT_SOURCE_DIR}/src/model/mirrored_model.cpp
     ${PROJECT_SOURCE_DIR}/src/model/model.h
     ${PROJECT_SOURCE_DIR}/src/model/model.cpp
+    ${PROJECT_SOURCE_DIR}/src/model/read_model_urdf.h
+    ${PROJECT_SOURCE_DIR}/src/model/read_model_urdf.cpp
 
     ${PROJECT_SOURCE_DIR}/src/optimization/kernels/obsToMod.h
     ${PROJECT_SOURCE_DIR}/src/optimization/kernels/modToObs.h
@@ -268,3 +282,22 @@ if(${BUILD_TESTS} AND ${GTEST_FOUND})
     )
     target_link_libraries(test dart)
 endif()
+
+# installation of library and header files
+install(TARGETS ${PROJECT_NAME} LIBRARY DESTINATION lib)
+install(DIRECTORY src/ DESTINATION include/dart FILES_MATCHING PATTERN "*.h")
+
+# setup pkg-config files
+SET(PKG_CONFIG_REQUIRES "tinyxml libpng libmatheval eigen3 cuda-7.5")
+SET(PKG_CONFIG_LIBDIR       "\${prefix}/lib" )
+SET(PKG_CONFIG_INCLUDEDIR   "\${prefix}/include" )
+SET(PKG_CONFIG_LIBS         "-L\${libdir} -l${PROJECT_NAME}" )
+SET(PKG_CONFIG_CFLAGS       "-I\${includedir}" )
+
+CONFIGURE_FILE(
+  "${CMAKE_CURRENT_SOURCE_DIR}/pkg-config.pc.cmake"
+  "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}.pc"
+)
+
+INSTALL(FILES "${CMAKE_BINARY_DIR}/${PROJECT_NAME}.pc"
+        DESTINATION lib/pkgconfig)

pkg-config.pc.cmake

@@ -0,0 +1,9 @@
+Name: ${PROJECT_NAME}
+Description: ${PROJECT_DESCRIPTION}
+Version: ${PROJECT_VERSION}
+Requires: ${PKG_CONFIG_REQUIRES}
+prefix=${CMAKE_INSTALL_PREFIX}
+includedir=${PKG_CONFIG_INCLUDEDIR}
+libdir=${PKG_CONFIG_LIBDIR}
+Libs: ${PKG_CONFIG_LIBS}
+Cflags: ${PKG_CONFIG_CFLAGS}

src/img_proc/organized_point_cloud.cu

@@ -34,7 +34,7 @@ __global__ void gpu_depthToVertices(const DepthType * depthIn,
 
     }
     else {
-        vertOut[index].w = 0;
+        vertOut[index].w = NAN;
     }
 
 }
@@ -67,7 +67,7 @@ __global__ void gpu_depthToVertices(const DepthType * depthIn,
 
     }
     else {
-        vertOut[index].w = 0;
+        vertOut[index].w = NAN;
     }
 
 }
@@ -103,7 +103,7 @@ __global__ void gpu_depthToVertices(const DepthType * depthIn,
 
     }
     else {
-        vertOut[index].w = 0;
+        vertOut[index].w = NAN;
     }
 
 }
@@ -140,7 +140,7 @@ __global__ void gpu_depthToVertices(const DepthType * depthIn,
 
     }
     else {
-        vertOut[index].w = 0;
+        vertOut[index].w = NAN;
     }
 
 }
@@ -198,7 +198,7 @@ __global__ void gpu_depthToVertices(const DepthType * depthIn,
 
     }
     else {
-        vertOut[index].w = 0;
+        vertOut[index].w = NAN;
     }
 
 }
@@ -257,7 +257,7 @@ __global__ void gpu_depthToVertices(const DepthType * depthIn,
 
     }
     else {
-        vertOut[index].w = 0;
+        vertOut[index].w = NAN;
     }
 
 }

src/mesh/assimp_mesh_reader.cpp

@@ -2,51 +2,110 @@
 
 #include <iostream>
 #include <assimp/cimport.h>
+#include <assimp/postprocess.h>
 #include <assimp/scene.h>
 #include <vector_types.h>
 #include <vector_functions.h>
 
+#include <assert.h>
+
 namespace dart {
 
+template<typename T>
+size_t vec_byte_size(const typename std::vector<T>& vec) {
+    return sizeof(T) * vec.size();
+}
+
+aiMatrix4x4 getFullT(const std::vector<aiMatrix4x4> &transforms) {
+    // identity transform
+    aiMatrix4x4 T;
+    // concatenate all transformations
+    for(auto t : transforms)
+        T *= t;
+    return T;
+}
+
+void getMesh(const aiScene* const scene, const aiNode* const node,
+             std::vector<float3> &verts, std::vector<float3> &normals, std::vector<int3> &faces, std::vector<aiMatrix4x4> transforms)
+{
+    // start to count mesh ID from here
+    const unsigned int offset = verts.size();
+
+    // we need to skip the transformation from the scene to the root frame
+    if(node->mParent != NULL)
+        transforms.push_back(node->mTransformation);
+
+    const aiMatrix4x4 T = getFullT(transforms);
+
+    for(unsigned int m=0; m<node->mNumMeshes; m++) {
+        const aiMesh* const mesh = scene->mMeshes[node->mMeshes[m]];
+        if(mesh->mPrimitiveTypes == aiPrimitiveType_TRIANGLE) {
+            // transform each vertice
+            for(unsigned int v=0; v<mesh->mNumVertices; v++) {
+                const aiVector3D vert = T*mesh->mVertices[v];
+                verts.push_back(make_float3(vert.x, vert.y, vert.z));
+            }
+
+            // transform each normal
+            for(unsigned int v=0; v<mesh->mNumVertices && mesh->HasNormals(); v++) {
+                const aiVector3D norm = T*mesh->mNormals[v];
+                normals.push_back(make_float3(norm.x, norm.y, norm.z));
+            }
+
+            // get faces and store vertex ID with offsets for each new mesh
+            for(unsigned int f=0; f<mesh->mNumFaces; f++) {
+                const aiFace face = mesh->mFaces[f];
+                assert(face.mNumIndices == 3);
+                faces.push_back(make_int3(offset + face.mIndices[0],
+                                          offset + face.mIndices[1],
+                                          offset + face.mIndices[2]));
+            }
+        } // check aiPrimitiveType_TRIANGLE
+    }
+
+    // recursively for each child node
+    for(unsigned int i=0; i<node->mNumChildren; i++) {
+        const aiNode* const child = node->mChildren[i];
+        getMesh(scene, child, verts, normals, faces, transforms);
+    }
+}
+
 dart::Mesh * AssimpMeshReader::readMesh(const std::string filename) const {
 
-    const struct aiScene * scene = aiImportFile(filename.c_str(),0); //aiProcess_JoinIdenticalVertices);
+    // define import flags for assimp
+    const unsigned int import_flags = aiProcess_Triangulate |
+                                      aiProcess_SortByPType |
+                                      aiProcess_JoinIdenticalVertices;
+    // aiProcess_Triangulate: triangulate ploygones such that all vertices have 3 points
+    // aiProcess_SortByPType: split meshes with mixed types into separate meshes
+    // aiProcess_JoinIdenticalVertices: remove vertices with same coordinates
+
+    const struct aiScene * scene = aiImportFile(filename.c_str(), import_flags);
+
     if (scene == 0) {
         std::cerr << "error: " << aiGetErrorString() << std::endl;
-        return 0;
-    }
-
-    if (scene->mNumMeshes != 1) {
-        std::cerr << "there are " << scene->mNumMeshes << " meshes in " << filename << std::endl;
         aiReleaseImport(scene);
         return 0;
     }
 
-    aiMesh * aiMesh = scene->mMeshes[0];
+    std::vector<int3> faces;
+    std::vector<float3> verts;
+    std::vector<float3> normals;
+    std::vector<aiMatrix4x4> transforms;
 
-    dart::Mesh * mesh = new dart::Mesh(aiMesh->mNumVertices,aiMesh->mNumFaces);
+    // recursively obtain vertices, normals and faces from meshes
+    const aiNode* const rnode = scene->mRootNode;
+    getMesh(scene, rnode, verts, normals, faces, transforms);
 
-    for (int f=0; f<mesh->nFaces; ++f) {
-        aiFace& face = aiMesh->mFaces[f];
-        if (face.mNumIndices != 3) {
-            std::cerr << filename << " is not a triangle mesh" << std::endl;
-            delete mesh;
-            aiReleaseImport(scene);
-            return 0;
-        }
-        mesh->faces[f] = make_int3(face.mIndices[0],face.mIndices[1],face.mIndices[2]);
-    }
-    for (int v=0; v<mesh->nVertices; ++v) {
-        aiVector3D & vertex = aiMesh->mVertices[v];
-        mesh->vertices[v] = make_float3(vertex.x,vertex.y,vertex.z);
-        aiVector3D & normal = aiMesh->mNormals[v];
-        mesh->normals[v] = make_float3(normal.x,normal.y,normal.z);
-    }
+    // copy mesh  data to DART mesh
+    dart::Mesh* mesh = new dart::Mesh(verts.size(), faces.size());
+    memcpy(mesh->vertices, verts.data(), vec_byte_size(verts));
+    memcpy(mesh->normals, normals.data(), vec_byte_size(normals));
+    memcpy(mesh->faces, faces.data(), vec_byte_size(faces));
 
     aiReleaseImport(scene);
 
     return mesh;
-
 }
 
 }

src/model/host_only_model.cpp

@@ -470,23 +470,19 @@ void HostOnlyModel::setPose(const Pose & pose) {
     _T_mc = pose.getTransformCameraToModel();
 
     // compute transforms from frame to model
-    int j = 0;
     for (int f=1; f<getNumFrames(); ++f) {
-
-        const float p = std::min(std::max(getJointMin(j),pose.getArticulation()[j]),getJointMax(j));
-
         const int joint = f-1;
+        const float p = std::min(std::max(getJointMin(joint),pose.getArticulation()[joint]),getJointMax(joint));
+
         SE3 T_pf = getTransformJointAxisToParent(joint);
         switch(_jointTypes[joint]) {
         case RotationalJoint:
             T_pf = T_pf*SE3Fromse3(se3(0, 0, 0,
                                                    p*_axes[joint].x, p*_axes[joint].y, p*_axes[joint].z));
-            ++j;
             break;
         case PrismaticJoint:
             T_pf = T_pf*SE3Fromse3(se3(p*_axes[joint].x, p*_axes[joint].y, p*_axes[joint].z,
                                                    0, 0, 0));
-            ++j;
             break;
         }
         const int parent = getFrameParent(f);

src/model/mirrored_model.cpp

@@ -184,23 +184,18 @@ void MirroredModel::setPose(const Pose & pose) {
     _T_cm = pose.getTransformModelToCamera();
     _T_mc = pose.getTransformCameraToModel();
 
-    int j = 0;
     for (int f=1; f<getNumFrames(); ++f) {
-
-        float p = std::min(std::max(getJointMin(j),pose.getArticulation()[j]),getJointMax(j));
-
         const int joint = f-1;
+        float p = std::min(std::max(getJointMin(joint),pose.getArticulation()[joint]),getJointMax(joint));
         SE3 T_pf = getTransformJointAxisToParent(joint);
         switch(_jointTypes->hostPtr()[joint]) {
             case RotationalJoint:
                 T_pf = T_pf*SE3Fromse3(se3(0, 0, 0,
                                            p*_jointAxes->hostPtr()[joint].x, p*_jointAxes->hostPtr()[joint].y, p*_jointAxes->hostPtr()[joint].z));
-                ++j;
                 break;
             case PrismaticJoint:
                 T_pf = T_pf*SE3Fromse3(se3(p*_jointAxes->hostPtr()[joint].x, p*_jointAxes->hostPtr()[joint].y, p*_jointAxes->hostPtr()[joint].z,
                                            0, 0, 0));
-                ++j;
                 break;
         }
         const int parent = getFrameParent(f);

src/model/model.cpp

@@ -21,6 +21,7 @@ Model::Model(const Model & copy) {
     _T_cm = copy._T_cm;
     _jointLimits = copy._jointLimits;
     _jointNames = copy._jointNames;
+    _name = copy._name;
 
 }
 
@@ -205,6 +206,14 @@ void Model::renderVoxelizedFrame(const int frameNumber, const char* args) const
     }
 }
 
+int Model::getJointIdByName(const std::string &name) {
+    const auto pos = std::find(_jointNames.begin(), _jointNames.end(), name);
+    if(pos != _jointNames.end())
+        return std::distance(_jointNames.begin(), pos);
+    else
+        throw std::range_error("requested frame '"+name+"' does not exist");
+}
+
 void Model::renderSdf(const dart::Grid3D<float> & sdf, float levelSet) const {
 
     glDisable(GL_BLEND);

src/model/model.h

@@ -101,6 +101,7 @@ class Model {
     inline float getJointMax(const int joint) const { return _jointLimits[joint].y; }
 
     const std::string & getJointName(const int joint) const { return _jointNames[joint]; }
+    int getJointIdByName(const std::string &name);
     void renderSdf(const dart::Grid3D<float> & sdf, float levelSet) const;
 
     void getArticulatedBoundingBox(float3 & mins, float3 & maxs, const float modelSdfPadding, const int nSweepPoints = 4);
@@ -112,6 +113,10 @@ class Model {
 
     virtual const SE3 getTransformJointAxisToParent(const int joint) const = 0;
 
+    const std::string getName() const { return _name; }
+
+    void setName(const std::string name) { _name = name; }
+
 protected:
     int _dimensionality;
 
@@ -141,6 +146,8 @@ class Model {
 
     std::map<int,uint> _meshNumbers;
 
+    std::string _name;
+
     // geometry-level rendering
     void renderGeometries(void (Model::*prerenderFunc)(const int, const int, const char *) const,
                           const char * args = 0,