diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..7cb3dda
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,4 @@
+build/
+*.so
+
+CMakeLists.txt.user
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 895c91f..e103f80 100644
--- a/CMakeLists.txt
+++ b/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,8 +37,16 @@ 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}
@@ -44,6 +54,8 @@ link_libraries(
     ${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)
diff --git a/pkg-config.pc.cmake b/pkg-config.pc.cmake
new file mode 100644
index 0000000..99bec57
--- /dev/null
+++ b/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}
diff --git a/src/img_proc/organized_point_cloud.cu b/src/img_proc/organized_point_cloud.cu
index a65870c..c190776 100644
--- a/src/img_proc/organized_point_cloud.cu
+++ b/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;
     }
 
 }
diff --git a/src/mesh/assimp_mesh_reader.cpp b/src/mesh/assimp_mesh_reader.cpp
index c12bd5c..ade7bae 100644
--- a/src/mesh/assimp_mesh_reader.cpp
+++ b/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;
-
 }
 
 }
diff --git a/src/model/host_only_model.cpp b/src/model/host_only_model.cpp
index c8deeef..ca5f453 100644
--- a/src/model/host_only_model.cpp
+++ b/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);
diff --git a/src/model/mirrored_model.cpp b/src/model/mirrored_model.cpp
index f2eeb0a..2c8d44c 100644
--- a/src/model/mirrored_model.cpp
+++ b/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);
diff --git a/src/model/model.cpp b/src/model/model.cpp
index 8e77a4c..60ce006 100644
--- a/src/model/model.cpp
+++ b/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);
diff --git a/src/model/model.h b/src/model/model.h
index a5647ee..0f1f0f3 100644
--- a/src/model/model.h
+++ b/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,
diff --git a/src/model/read_model_urdf.cpp b/src/model/read_model_urdf.cpp
new file mode 100644
index 0000000..4c9145b
--- /dev/null
+++ b/src/model/read_model_urdf.cpp
@@ -0,0 +1,368 @@
+
+#include "read_model_urdf.h"
+
+// urdf parser and model
+#include <urdf_parser/urdf_parser.h>
+
+// boost for parsing file paths
+#include <boost/filesystem.hpp>
+#include <boost/algorithm/string/replace.hpp>
+
+// constants for determining and replacing the package path in URDF mesh paths
+#define PACKAGE_PATH_FILE "package.xml"
+#define PACKAGE_PATH_URI_SCHEME "package://"
+
+//#define PRNT_DBG
+
+namespace dart {
+
+/// type definitions for shared pointers
+typedef const boost::shared_ptr< const urdf::ModelInterface> ModelInterfaceConstPtr;
+
+typedef boost::shared_ptr< urdf::Link > LinkPtr;
+typedef boost::shared_ptr< const urdf::Link > LinkConstPtr;
+typedef boost::shared_ptr< urdf::Joint > JointPtr;
+typedef boost::shared_ptr< const urdf::Joint > JointConstPtr;
+
+typedef std::vector< boost::shared_ptr< urdf::Link > > LinkPtrVec;
+typedef std::vector< boost::shared_ptr< urdf::Joint > > JointPtrVec;
+
+/**
+ * @brief The MeshLoaderConfig struct
+ */
+struct MeshLoaderConfig {
+
+    /**
+     * @brief package_path store absolute path to URDF package rot directory
+     */
+    std::string package_path;
+
+    /**
+     * @brief mesh_extension_surrogate optionally replace mesh file extension
+     */
+    std::string mesh_extension_surrogate;
+
+    /**
+     * @brief colour RGB colour definition for untextured meshes
+     */
+    std::vector<uint8_t> colour;
+};
+
+/**
+ * @brief extract_joints get list of joints from std::map
+ * @param joint_map dictionary containing joint name and pointer
+ * @return vector of joint pointers
+ */
+JointPtrVec extract_joints(const std::map< std::string, JointPtr > &joint_map) {
+    JointPtrVec joints;
+    for(auto jm: joint_map) { joints.push_back(jm.second); }
+    return joints;
+}
+
+/**
+ * @brief print_joints print the name of all joints in given list
+ * @param joints vector of joints
+ */
+void print_joints(const JointPtrVec &joints) {
+    std::cout<<"joints total: "<<joints.size()<<std::endl;
+    for(auto j: joints) { std::cout<<"joint: "<<j->name<<std::endl; }
+}
+
+/**
+ * @brief print_links print the name of all links in given list
+ * @param links vector of links
+ */
+void print_links(const LinkPtrVec &links) {
+    std::cout<<"links total: "<<links.size()<<std::endl;
+    for(auto l: links) { std::cout<<"link: "<<l->name<<std::endl; }
+}
+
+/**
+ * @brief extract_frames process link and child joints recursively
+ * @param parent_id ID of parrent frame, that called this function
+ * @param link parent link
+ * @param urdf_model URDF model to read additional information from
+ * @param model DART model that is extended with links and joints of URDF model
+ * @param conf configuration structure to store permanent and global information
+ * @return true on success
+ * @return false on failure
+ */
+bool extract_frames(const int parent_id, LinkConstPtr &link, ModelInterfaceConstPtr &urdf_model, HostOnlyModel &model, const MeshLoaderConfig &conf) {
+
+    JointPtrVec joints = link->child_joints;
+
+#ifdef PRNT_DBG
+    std::cout<<"id: "<<parent_id<<" (link: "<<link->name<<", joints: "<<joints.size()<<")"<<std::endl;
+#endif
+
+    /////////////////////////////////////////////////////
+    /// get geometric volume
+
+    if(link->visual != NULL) {
+        boost::shared_ptr< urdf::Geometry > geometry = link->visual->geometry;
+
+        dart::GeomType geom_type;
+        std::string sx, sy, sz; // scale
+        std::string tx, ty, tz; // translation
+        std::string rx, ry, rz; // rotation
+        uint8_t r, g, b;        // colour
+        std::string mesh_path = "";
+
+        // translation
+        urdf::Vector3 pos = link->visual->origin.position;
+        tx = std::to_string(pos.x);
+        ty = std::to_string(pos.y);
+        tz = std::to_string(pos.z);
+
+        // rotation
+        urdf::Rotation rot = link->visual->origin.rotation;
+        double roll, pitch, yaw;
+        rot.getRPY(roll, pitch, yaw);
+        // right-hand coordinate system
+        rx = std::to_string(roll);    // roll: rotation around x-axis (facing forward)
+        ry = std::to_string(pitch);   // pitch: rotation around y-axis (facing right)
+        rz = std::to_string(yaw);     // yaw: rotation around z-axis (facing down)
+
+        switch(geometry->type) {
+        case urdf::Geometry::SPHERE: {
+            geom_type = PrimitiveSphereType;
+            double radius = dynamic_cast<urdf::Sphere*>(&*geometry)->radius;
+            sx = std::to_string(radius);
+            sy = std::to_string(radius);
+            sz = std::to_string(radius);
+        }
+            break;
+        case urdf::Geometry::BOX: {
+            geom_type = PrimitiveCubeType;
+            urdf::Vector3 size = dynamic_cast<urdf::Box*>(&*geometry)->dim;
+            sx = std::to_string(size.x);
+            sy = std::to_string(size.y);
+            sz = std::to_string(size.z);
+        }
+            break;
+        case urdf::Geometry::CYLINDER: {
+            geom_type = PrimitiveCylinderType;
+            double radius = dynamic_cast<urdf::Cylinder*>(&*geometry)->radius;
+            double length = dynamic_cast<urdf::Cylinder*>(&*geometry)->length;
+            sx = std::to_string(radius);
+            sy = std::to_string(radius);
+            sz = std::to_string(length);
+
+            // workaround: transform cylinder origin from URDF specification
+            // (origin at centre of cylinder volume) to DART (centre of bottom)
+            tz = std::to_string(pos.z - (length/2));
+        }
+            break;
+        case urdf::Geometry::MESH: {
+            geom_type = MeshType;
+            // geometry is a pointer to base class, we need to cast (*geometry)
+            mesh_path = dynamic_cast<urdf::Mesh*>(&*geometry)->filename;
+            urdf::Vector3 scale = dynamic_cast<urdf::Mesh*>(&*geometry)->scale;
+            sx = std::to_string(scale.x);
+            sy = std::to_string(scale.y);
+            sz = std::to_string(scale.z);
+            if(mesh_path.find(PACKAGE_PATH_URI_SCHEME) != std::string::npos) {
+                // we need to replace "package://" by full path
+                boost::algorithm::replace_first(mesh_path, PACKAGE_PATH_URI_SCHEME, conf.package_path);
+            }
+            else {
+                // prepend full path
+                mesh_path = conf.package_path + mesh_path;
+            }
+            if(!conf.mesh_extension_surrogate.empty()) {
+                mesh_path = boost::filesystem::path(mesh_path).
+                        replace_extension(conf.mesh_extension_surrogate).native();
+            }
+#ifdef PRNT_DBG
+            std::cout<<"loading mesh from: "<<mesh_path<<std::endl;
+#endif
+        }
+            break;
+        default:
+            std::cerr<<"unknown geometry"<<std::endl;
+        }
+
+        // colour
+        if(link->visual->material!=NULL) {
+            urdf::Color colour = link->visual->material->color;
+            r = colour.r*255;
+            g = colour.g*255;
+            b = colour.b*255;
+        }
+        else {
+            // use default values
+            r = conf.colour[0];
+            g = conf.colour[1];
+            b = conf.colour[2];
+        }
+
+        model.addGeometry(parent_id, geom_type,
+                          sx, sy, sz,
+                          tx, ty, tz,
+                          rx, ry, rz,
+                          r, g, b,
+                          mesh_path);
+
+    }
+
+    /////////////////////////////////////////////////////
+    /// get joint attributes
+    for(JointPtr j: joints) {
+        // joint attributes expected by DART
+        std::string name;                   // joint name
+        dart::JointType type;               // type
+        std::string min, max;               // joint limits
+        std::string posX, posY, posZ;       // 3D position
+        std::string oriX, oriY, oriZ;       // 3D orientation
+        std::string axisX, axisY, axisZ;    // rotation axis
+
+        // name
+        name = j->name;
+
+#ifdef PRNT_DBG
+        std::cout<<"processing joint: "<<name<<std::endl;
+#endif
+
+        // joint type
+        switch(j->type) {
+        case urdf::Joint::REVOLUTE:
+            type = dart::RotationalJoint;
+            break;
+        case urdf::Joint::PRISMATIC:
+            type = dart::PrismaticJoint;
+            break;
+        case urdf::Joint::FIXED:
+        case urdf::Joint::CONTINUOUS:
+            // ignore FIXED and CONTINUOUS
+            break;
+        default:
+            std::cerr<<"joint type ("<<j->type<<") is unsupported by DART"<<std::endl;
+        }
+
+        // position
+        urdf::Vector3 pos = j->parent_to_joint_origin_transform.position;
+        posX = std::to_string(pos.x);
+        posY = std::to_string(pos.y);
+        posZ = std::to_string(pos.z);
+
+        // orientation
+        urdf::Rotation rot = j->parent_to_joint_origin_transform.rotation;
+        double roll, pitch, yaw;
+        rot.getRPY(roll, pitch, yaw);
+        // right-hand coordinate system
+        oriX = std::to_string(roll);    // roll: rotation around x-axis (facing forward)
+        oriY = std::to_string(pitch);   // pitch: rotation around y-axis (facing right)
+        oriZ = std::to_string(yaw);     // yaw: rotation around z-axis (facing down)
+
+        // rotation axis
+        urdf::Vector3 axis = j->axis;
+        axisX = std::to_string(axis.x);
+        axisY = std::to_string(axis.y);
+        axisZ = std::to_string(axis.z);
+
+        // limits
+        // joints FIXED and CONTINUOUS have no limits
+        if(j->type == urdf::Joint::REVOLUTE || j->type == urdf::Joint::PRISMATIC) {
+            boost::shared_ptr<urdf::JointLimits> limits = j->limits;
+            min = std::to_string(limits->lower);
+            max = std::to_string(limits->upper);
+        }
+        else {
+#ifdef PRNT_DBG
+            std::cout<<"ignoring limits of ";
+            switch(j->type) {
+            case urdf::Joint::FIXED:
+                std::cout<<"FIXED"; break;
+            case urdf::Joint::CONTINUOUS:
+                std::cout<<"CONTINUOUS"; break;
+            default:
+                std::cout<<"type "<<j->type<<std::endl;
+            }
+            std::cout<<" joint: "<<name<<std::endl;
+#endif
+            min = "0";
+            max = "0";
+        }
+
+        // add frame with attributes
+        const int child_id =
+            model.addFrame(parent_id, type, posX, posY, posZ, oriX, oriY, oriZ,
+                           axisX, axisY, axisZ, min, max, name);
+
+        // follow edges
+        LinkConstPtr l_child = urdf_model->getLink(j->child_link_name);
+        extract_frames(child_id, l_child, urdf_model, model, conf);
+    } // iteration of joints
+
+    // nothing exceptional happened so far
+    return true;
+}
+
+/**
+ * @brief readModelURDF readModelURDF parse URDF model description and store kinematic and meshes in DART model format
+ */
+bool readModelURDF(const std::string &path, HostOnlyModel &model,
+                   const std::string &root_link_name,
+                   const std::string &mesh_extension_surrogate,
+                   const std::vector<uint8_t> &colour)
+{
+    // parse URDF file
+    ModelInterfaceConstPtr urdf_model = urdf::parseURDFFile(path);
+
+    // get robot name
+    std::cout<<"found URDF robot: "<<urdf_model->getName()<<std::endl;
+    model.setName(urdf_model->getName());
+
+    // fix DART model version (currently 0 and above is checked)
+    model.setModelVersion(1);
+
+    // get root link
+    boost::shared_ptr<const urdf::Link> l_root;
+    l_root = root_link_name.empty()? urdf_model->getRoot() : urdf_model->getLink(root_link_name);
+
+    if(l_root!=NULL) {
+        std::cout<<"root link: "<<l_root->name<<std::endl;
+
+        MeshLoaderConfig conf;
+
+        conf.colour = colour;
+
+        // get full absolute path
+        boost::filesystem::path fpath = boost::filesystem::canonical(path);
+
+        // search backwards for package path, e.g. directory that contains the package file
+        while(fpath.has_parent_path() && !boost::filesystem::is_regular_file(fpath / PACKAGE_PATH_FILE)) {
+            // go one step backward closer to root
+            fpath = fpath.parent_path();
+        }
+
+        if(!boost::filesystem::is_regular_file(fpath / PACKAGE_PATH_FILE)) {
+            // package path not found, use relative path
+            conf.package_path = boost::filesystem::canonical(path).parent_path().native()
+                    + boost::filesystem::path::preferred_separator;
+        }
+        else {
+            // store package path with trailing directory seperator
+            conf.package_path = fpath.branch_path().native() + boost::filesystem::path::preferred_separator;
+        }
+        std::cout<<"URDF package path: "<<conf.package_path<<std::endl;
+
+        conf.mesh_extension_surrogate = mesh_extension_surrogate;
+
+        // extract links and joints recursively
+        return extract_frames(0, l_root, urdf_model, model, conf);
+    }
+    else {
+        std::cerr<<"could not find root link "<<root_link_name<<std::endl;
+        return false;
+    }
+}
+
+const HostOnlyModel &readModelURDF(const std::string &path, const std::string &root_link_name, const std::string &mesh_extension_surrogate, const std::vector<uint8_t> &colour)
+{
+    HostOnlyModel *model = new HostOnlyModel();
+    readModelURDF(path, *model, root_link_name, mesh_extension_surrogate, colour);
+    return *model;
+}
+
+} // namespace dart
diff --git a/src/model/read_model_urdf.h b/src/model/read_model_urdf.h
new file mode 100644
index 0000000..d77d86f
--- /dev/null
+++ b/src/model/read_model_urdf.h
@@ -0,0 +1,30 @@
+#ifndef READ_MODEL_URDF_H
+#define READ_MODEL_URDF_H
+
+#include "model/host_only_model.h"
+
+namespace dart {
+
+/**
+ * @brief readModelURDF parse URDF model description and store kinematic and meshes in DART model format
+ * @param path relative or absolute path to URDF file
+ * @param model DART model that will be extended with frames from URDF model
+ * @param root_link_name optional root link name from where to start, if not provided the root link will be determined automatically
+ * @param mesh_extension_surrogate optional file extension to load meshes from different file types as provided in the URDF model description
+ * @return true on success
+ * @return false on failure
+ */
+bool readModelURDF(const std::string &path, HostOnlyModel & model, const std::string &root_link_name = "", const std::string &mesh_extension_surrogate = "", const std::vector<uint8_t> &colour = {127, 127, 127});
+
+/**
+ * @brief readModelURDF parse URDF model description and store kinematic and meshes in DART model format
+ * @param path relative or absolute path to URDF file
+ * @param root_link_name optional root link name from where to start, if not provided the root link will be determined automatically
+ * @param mesh_extension_surrogate optional file extension to load meshes from different file types as provided in the URDF model description
+ * @return instance of Dart model with frames imported from URDF model description
+ */
+const HostOnlyModel &readModelURDF(const std::string &path, const std::string &root_link_name = "", const std::string &mesh_extension_surrogate = "", const std::vector<uint8_t> &colour = {127, 127, 127});
+
+} // namespace dart
+
+#endif // READ_MODEL_URDF_H
diff --git a/src/optimization/kernels/obsToMod.cu b/src/optimization/kernels/obsToMod.cu
index ffa1627..8eb725e 100644
--- a/src/optimization/kernels/obsToMod.cu
+++ b/src/optimization/kernels/obsToMod.cu
@@ -629,7 +629,7 @@ void normEqnsObsToMod(const int dims,
                       float * dResult,
                       float4 * debugJs) {
 
-    std::cout << nElements << " points associated to model " << model.getModelID() << std::endl;
+    //std::cout << nElements << " points associated to model " << model.getModelID() << std::endl;
 
     dim3 block;
     if (height == 1) {
diff --git a/src/optimization/optimizer.cpp b/src/optimization/optimizer.cpp
index 9fbf2cd..5c12c94 100644
--- a/src/optimization/optimizer.cpp
+++ b/src/optimization/optimizer.cpp
@@ -696,7 +696,7 @@ void Optimizer::optimizePoses(std::vector<MirroredModel *> & models,
             // add damping matrix
             JTJ += *dampingMatrices[m];
 
-            std::cout << JTJ << std::endl << std::endl << std::endl;
+            //std::cout << JTJ << std::endl << std::endl << std::endl;
 
             for (int i=0; i<reducedDimensions; ++i) {
                 for (int j=i; j<reducedDimensions; ++j) {
diff --git a/src/pose/pose.cpp b/src/pose/pose.cpp
index eac10e0..483fe4a 100644
--- a/src/pose/pose.cpp
+++ b/src/pose/pose.cpp
@@ -45,6 +45,15 @@ Pose & Pose::operator=(const Pose & rhs) {
     return *this;
 }
 
+void Pose::setReducedArticulation(const std::map<std::string, float> &joint_map) {
+    // check all joints for available values from map
+    for(unsigned int i=0; i<_reduction->getReducedDimensions(); i++) {
+        // only access key (name) if it exists
+        if(joint_map.count(_reduction->getName(i))>0)
+            _reducedArticulation[i] = joint_map.at(_reduction->getName(i));
+    }
+}
+
 LinearPoseReduction::LinearPoseReduction(const int fullDims, const int redDims)
     : PoseReduction(fullDims,redDims), _A(fullDims*redDims), _b(new float[fullDims]) {
 
diff --git a/src/pose/pose.h b/src/pose/pose.h
index f9b73f9..8ad6355 100644
--- a/src/pose/pose.h
+++ b/src/pose/pose.h
@@ -3,6 +3,7 @@
 
 #include <string>
 #include <vector>
+#include <map>
 #include <string.h>
 #include "pose_reduction.h"
 #include "util/mirrored_memory.h"
@@ -36,6 +37,8 @@ class Pose {
 
     inline const float * getReducedArticulation() const { return _reducedArticulation; }
 
+    void setReducedArticulation(const std::map<std::string, float> &joint_map);
+
     virtual void projectReducedToFull() { return _reduction->projectReducedToFull(_reducedArticulation,_fullArticulation); }
 
     inline const float * getFirstDerivatives() const { return _reduction->getFirstDerivatives(); }
diff --git a/src/tracker.cpp b/src/tracker.cpp
index 0d58380..a223296 100644
--- a/src/tracker.cpp
+++ b/src/tracker.cpp
@@ -49,6 +49,9 @@ Tracker::~Tracker() {
         delete matrix;
     }
 
+    // delete HostOnlyModels in _models
+    for(auto model_ptr: _models) { delete model_ptr; }
+
     delete _pcSource;
     delete _optimizer;
 
@@ -66,13 +69,6 @@ bool Tracker::addModel(const std::string & filename,
                        const float collisionCloudDensity,
                        const bool cacheSdfs) {
 
-    HostOnlyModel model;
-    if (!readModelXML(filename.c_str(),model)) {
-        return false;
-    }
-
-    model.computeStructure();
-
     std::cout << "loading model from " << filename << std::endl;
 
     const int lastSlash = filename.find_last_of('/');
@@ -82,11 +78,67 @@ bool Tracker::addModel(const std::string & filename,
     const std::string modelName = filename.substr(substrStart, diff > 0 ? diff : filename.size() - substrStart);
     std::cout << "model name: " << modelName << std::endl;
 
+    HostOnlyModel *model = new HostOnlyModel();
+    bool ret;
+    if (!readModelXML(filename.c_str(), *model)) {
+        ret = false;
+    }
+    else {
+        model->setName(modelName);
+        ret = addModel(*model,
+                        modelSdfResolution,
+                        modelSdfPadding,
+                        obsSdfSize,
+                        obsSdfResolution,
+                        obsSdfOffset,
+                        poseReduction,
+                        collisionCloudDensity,
+                        cacheSdfs);
+    }
+    delete model;
+    return ret;
+}
+
+/**
+ * @brief Tracker::addModel pre-processes model and adds a copy to tracker
+ * @param model model to track, a copy will be created and the passed instance can be deleted after method call returns
+ * @param modelSdfResolution
+ * @param modelSdfPadding
+ * @param obsSdfSize
+ * @param obsSdfResolution
+ * @param obsSdfOffset
+ * @param poseReduction
+ * @param collisionCloudDensity
+ * @param cacheSdfs
+ * @return true on success
+ * @return false on failure
+ */
+bool Tracker::addModel(dart::HostOnlyModel &model,
+                       const float modelSdfResolution,
+                       const float modelSdfPadding,
+                       const int obsSdfSize,
+                       float obsSdfResolution,
+                       float3 obsSdfOffset,
+                       PoseReduction * poseReduction,
+                       const float collisionCloudDensity,
+                       const bool cacheSdfs) {
+
+    model.computeStructure();
+
+    unsigned int ngeom = 0;
+    for(unsigned int f=0; f<model.getNumFrames(); f++) {
+        ngeom += model.getFrameNumGeoms(f);
+    }
+    if(ngeom == 0) {
+        std::cerr<<"model "<<model.getName()<<" has no geometric shapes"<<std::endl;
+        return false;
+    }
+
 //    // TODO
 //    if (model.getNumGeoms() < 2) {
 //        model.voxelize2(modelSdfResolution,modelSdfPadding,cacheSdfs ? dart::stringFormat("model%02d",_mirroredModels.size()) : "");
 //    } else {
-        model.voxelize(modelSdfResolution,modelSdfPadding,cacheSdfs ? dart::stringFormat("/tmp/%s",modelName.c_str()) : "");
+        model.voxelize(modelSdfResolution,modelSdfPadding,cacheSdfs ? dart::stringFormat("/tmp/%s", model.getName().c_str()) : "");
 //    }
 
     if (obsSdfResolution <= 0 ) {
@@ -138,7 +190,9 @@ bool Tracker::addModel(const std::string & filename,
     }
     _estimatedPoses.push_back(Pose(poseReduction));
 
-    _filenames.push_back(filename);
+    // we need to copy as we don't know if memory was allocated on stack or heap
+    // and object pointed by stored pointer will be deallocated in deconstructor
+    _models.push_back( new HostOnlyModel(model) );
 
     // build collision cloud
     MirroredVector<float4> * collisionCloud = 0;
@@ -295,17 +349,17 @@ void Tracker::updateModel(const int modelNum,
     int modelID = _mirroredModels[modelNum]->getModelID();
     delete _mirroredModels[modelNum];
 
-    HostOnlyModel model;
-    readModelXML(_filenames[modelNum].c_str(),model);
+    // get reference to model
+    HostOnlyModel *model = _models[modelNum];
 
     for (std::map<std::string,float>::const_iterator it = _sizeParams[modelNum].begin();
          it != _sizeParams[modelNum].end(); ++it) {
-        model.setSizeParam(it->first,it->second);
+        (*model).setSizeParam(it->first,it->second);
     }
 
-    model.computeStructure();
-    model.voxelize(modelSdfResolution,modelSdfPadding);
-    MirroredModel * newModel = new MirroredModel(model,
+    (*model).computeStructure();
+    (*model).voxelize(modelSdfResolution,modelSdfPadding);
+    MirroredModel * newModel = new MirroredModel(*model,
                                                  make_uint3(obsSdfSize),
                                                  obsSdfResolution,
                                                  obsSdfCenter,
@@ -425,4 +479,12 @@ void Tracker::setIntersectionPotentialMatrix(const int modelNum, const int * mx)
     _intersectionPotentialMatrices[modelNum]->syncHostToDevice();
 }
 
+int Tracker::getModelIDbyName(const std::string &name) const {
+    for(unsigned int i=0; i<_models.size(); i++) {
+        if(_models[i]->getName() == name)
+            return i;
+    }
+    return -1;
+}
+
 }
diff --git a/src/tracker.h b/src/tracker.h
index 6d363db..4c72019 100644
--- a/src/tracker.h
+++ b/src/tracker.h
@@ -27,6 +27,16 @@ class Tracker {
     Tracker();
     ~Tracker();
 
+    bool addModel(dart::HostOnlyModel &model,
+                  const float modelSdfResolution = 0.005,
+                  const float modelSdfPadding = 0.10,
+                  const int obsSdfSize = 64,
+                  float obsSdfResolution = -1,
+                  float3 obsSdfCenter = make_float3(0,0,0),
+                  PoseReduction * poseReduction = 0,
+                  const float collisionCloudDensity = 1e5,
+                  const bool cacheSdfs = true);
+
     bool addModel(const std::string & filename,
                   const float modelSdfResolution = 0.005,
                   const float modelSdfPadding = 0.10,
@@ -75,8 +85,16 @@ class Tracker {
     inline const MirroredModel & getModel(const int modelNum) const { return * _mirroredModels[modelNum]; }
     inline MirroredModel & getModel(const int modelNum) { return * _mirroredModels[modelNum]; }
 
+    int getModelIDbyName(const std::string &name) const;
+
     inline const Pose & getPose(const int modelNum) const { return _estimatedPoses[modelNum]; }
     inline Pose & getPose(const int modelNum) { return _estimatedPoses[modelNum]; }
+    inline const Pose & getPose(const std::string &modelName) const {
+        return _estimatedPoses.at(getModelIDbyName(modelName));
+    }
+    inline Pose & getPose(const std::string &modelName) {
+        return _estimatedPoses.at(getModelIDbyName(modelName));
+    }
     inline std::map<std::string,float> & getSizeParams(const int modelNum) { return _sizeParams[modelNum]; }
     inline const float4 * getHostVertMap() { return _pcSource->getHostVertMap(); }
     inline const float4 * getHostNormMap() { return _pcSource->getHostNormMap(); }
@@ -144,7 +162,10 @@ class Tracker {
     std::vector<PoseReduction *> _ownedPoseReductions;
     std::vector<Pose> _estimatedPoses;
 
-    std::vector<std::string> _filenames;
+    /**
+     * @brief _models list of pointers to all added models
+     */
+    std::vector<HostOnlyModel*> _models;
 
     std::vector<Eigen::MatrixXf *> _dampingMatrices;
     std::vector<Prior *> _priors;