From a4976b430586cf5918c7b6916f69c0460e6091e7 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Tue, 17 May 2016 18:10:11 +0100 Subject: [PATCH 01/35] installation of shared objects and header files --- CMakeLists.txt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 895c91f..21fb9b4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -268,3 +268,7 @@ 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") From 60ee351257e0a3286a13c252d67e122ff34c1664 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Tue, 17 May 2016 18:10:24 +0100 Subject: [PATCH 02/35] ignore temporary build files --- .gitignore | 2 ++ 1 file changed, 2 insertions(+) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..945cb10 --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +build/ +*.so From 14ba76d44cdc39076b427512bb6b0fe7a7222a5b Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Wed, 1 Jun 2016 15:17:57 +0100 Subject: [PATCH 03/35] pkg-config file for referencing dart in external projects --- .gitignore | 2 ++ CMakeLists.txt | 15 +++++++++++++++ pkg-config.pc.cmake | 9 +++++++++ 3 files changed, 26 insertions(+) create mode 100644 pkg-config.pc.cmake diff --git a/.gitignore b/.gitignore index 945cb10..7cb3dda 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,4 @@ build/ *.so + +CMakeLists.txt.user diff --git a/CMakeLists.txt b/CMakeLists.txt index 21fb9b4..db008b4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -272,3 +272,18 @@ 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") +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} From f280d336fb5a580c58914e1880973e2f140e54aa Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Tue, 31 May 2016 19:45:07 +0100 Subject: [PATCH 04/35] method for adding dart::HostOnlyModels externally (e.g. from other file format) --- src/model/model.h | 6 ++++++ src/tracker.cpp | 45 ++++++++++++++++++++++++++++++++++----------- src/tracker.h | 15 ++++++++++++++- 3 files changed, 54 insertions(+), 12 deletions(-) diff --git a/src/model/model.h b/src/model/model.h index a5647ee..43b5969 100644 --- a/src/model/model.h +++ b/src/model/model.h @@ -112,6 +112,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 +145,8 @@ class Model { std::map _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/tracker.cpp b/src/tracker.cpp index 0d58380..4a2d798 100644 --- a/src/tracker.cpp +++ b/src/tracker.cpp @@ -66,13 +66,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 +75,41 @@ 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; + if (!readModelXML(filename.c_str(), model)) { + return false; + } + else { + model.setName(modelName); + return addModel(model, + modelSdfResolution, + modelSdfPadding, + obsSdfSize, + obsSdfResolution, + obsSdfOffset, + poseReduction, + collisionCloudDensity, + cacheSdfs); + } +} + +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(); + // // 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 +161,7 @@ bool Tracker::addModel(const std::string & filename, } _estimatedPoses.push_back(Pose(poseReduction)); - _filenames.push_back(filename); + _models.push_back(model); // build collision cloud MirroredVector * collisionCloud = 0; @@ -295,8 +318,8 @@ void Tracker::updateModel(const int modelNum, int modelID = _mirroredModels[modelNum]->getModelID(); delete _mirroredModels[modelNum]; - HostOnlyModel model; - readModelXML(_filenames[modelNum].c_str(),model); + // restore model + HostOnlyModel model = _models[modelNum]; for (std::map::const_iterator it = _sizeParams[modelNum].begin(); it != _sizeParams[modelNum].end(); ++it) { diff --git a/src/tracker.h b/src/tracker.h index 6d363db..e2feb08 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, @@ -144,7 +154,10 @@ class Tracker { std::vector _ownedPoseReductions; std::vector _estimatedPoses; - std::vector _filenames; + /** + * @brief _models list of pointers to all added models + */ + std::vector _models; std::vector _dampingMatrices; std::vector _priors; From 9d06e66328cb60125a61e641a7d248a4e74151c3 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Wed, 1 Jun 2016 09:09:01 +0100 Subject: [PATCH 05/35] use model pointer to reduce duplicate memory --- src/tracker.cpp | 17 +++++++++++------ src/tracker.h | 2 +- 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/src/tracker.cpp b/src/tracker.cpp index 4a2d798..aca323d 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; @@ -75,13 +78,15 @@ 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; - if (!readModelXML(filename.c_str(), model)) { + HostOnlyModel *model = new HostOnlyModel(); + std::cout<<"new xml model at "<setName(modelName); + return addModel(*model, modelSdfResolution, modelSdfPadding, obsSdfSize, @@ -161,7 +166,7 @@ bool Tracker::addModel(dart::HostOnlyModel &model, } _estimatedPoses.push_back(Pose(poseReduction)); - _models.push_back(model); + _models.push_back(&model); // build collision cloud MirroredVector * collisionCloud = 0; @@ -319,7 +324,7 @@ void Tracker::updateModel(const int modelNum, delete _mirroredModels[modelNum]; // restore model - HostOnlyModel model = _models[modelNum]; + HostOnlyModel model = *(_models[modelNum]); for (std::map::const_iterator it = _sizeParams[modelNum].begin(); it != _sizeParams[modelNum].end(); ++it) { diff --git a/src/tracker.h b/src/tracker.h index e2feb08..219cdec 100644 --- a/src/tracker.h +++ b/src/tracker.h @@ -157,7 +157,7 @@ class Tracker { /** * @brief _models list of pointers to all added models */ - std::vector _models; + std::vector _models; std::vector _dampingMatrices; std::vector _priors; From 3dffecb57f15d6e1f20277c571b478485d04a1e1 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Wed, 1 Jun 2016 10:16:13 +0100 Subject: [PATCH 06/35] copy model because we don't know here it is allocated --- src/tracker.cpp | 26 ++++++++++++++++++++++---- 1 file changed, 22 insertions(+), 4 deletions(-) diff --git a/src/tracker.cpp b/src/tracker.cpp index aca323d..85575fb 100644 --- a/src/tracker.cpp +++ b/src/tracker.cpp @@ -80,13 +80,13 @@ bool Tracker::addModel(const std::string & filename, HostOnlyModel *model = new HostOnlyModel(); std::cout<<"new xml model at "<setName(modelName); - return addModel(*model, + ret = addModel(*model, modelSdfResolution, modelSdfPadding, obsSdfSize, @@ -96,8 +96,24 @@ bool Tracker::addModel(const std::string & filename, 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, @@ -166,7 +182,9 @@ bool Tracker::addModel(dart::HostOnlyModel &model, } _estimatedPoses.push_back(Pose(poseReduction)); - _models.push_back(&model); + // 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 * collisionCloud = 0; From 6b525798578a679d3d5816f79c36e2164d5a5282 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Wed, 1 Jun 2016 10:16:45 +0100 Subject: [PATCH 07/35] update the original model instead of creating a copy --- src/tracker.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/tracker.cpp b/src/tracker.cpp index 85575fb..94f4421 100644 --- a/src/tracker.cpp +++ b/src/tracker.cpp @@ -341,17 +341,17 @@ void Tracker::updateModel(const int modelNum, int modelID = _mirroredModels[modelNum]->getModelID(); delete _mirroredModels[modelNum]; - // restore model - HostOnlyModel model = *(_models[modelNum]); + // get reference to model + HostOnlyModel *model = _models[modelNum]; for (std::map::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, From 13b7d87ed154fff7e8aca10ff6d7b86d643b25fd Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Wed, 1 Jun 2016 16:47:28 +0100 Subject: [PATCH 08/35] sanity check for models without geometric volumes --- src/tracker.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/tracker.cpp b/src/tracker.cpp index 94f4421..c79f043 100644 --- a/src/tracker.cpp +++ b/src/tracker.cpp @@ -126,6 +126,15 @@ bool Tracker::addModel(dart::HostOnlyModel &model, model.computeStructure(); + unsigned int ngeom = 0; + for(unsigned int f=0; f Date: Wed, 1 Jun 2016 20:29:20 +0100 Subject: [PATCH 09/35] remove printing pointer --- src/tracker.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/tracker.cpp b/src/tracker.cpp index c79f043..27fcb92 100644 --- a/src/tracker.cpp +++ b/src/tracker.cpp @@ -79,7 +79,6 @@ bool Tracker::addModel(const std::string & filename, std::cout << "model name: " << modelName << std::endl; HostOnlyModel *model = new HostOnlyModel(); - std::cout<<"new xml model at "< Date: Thu, 2 Jun 2016 19:28:38 +0100 Subject: [PATCH 10/35] use aiPrimitiveType_TRIANGLE to find first triangle mesh in scene --- src/mesh/assimp_mesh_reader.cpp | 46 ++++++++++++++++++--------------- 1 file changed, 25 insertions(+), 21 deletions(-) diff --git a/src/mesh/assimp_mesh_reader.cpp b/src/mesh/assimp_mesh_reader.cpp index c12bd5c..bdccf84 100644 --- a/src/mesh/assimp_mesh_reader.cpp +++ b/src/mesh/assimp_mesh_reader.cpp @@ -11,36 +11,40 @@ namespace dart { dart::Mesh * AssimpMeshReader::readMesh(const std::string filename) const { const struct aiScene * scene = aiImportFile(filename.c_str(),0); //aiProcess_JoinIdenticalVertices); + 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]; + // find first triangle mesh + aiMesh * aiMesh = NULL; + // stop searching if end of list or if first mesh is found (aiMesh!=NULL) + for(unsigned int i = 0; imNumMeshes && aiMesh==NULL; i++) { + if(scene->mMeshes[i]->mPrimitiveTypes == aiPrimitiveType_TRIANGLE) + aiMesh = scene->mMeshes[i]; + else + std::cout<<"ignoring non-triangle mesh in "<mNumVertices,aiMesh->mNumFaces); + dart::Mesh * mesh = NULL; + if(aiMesh != NULL) { + mesh = new dart::Mesh(aiMesh->mNumVertices,aiMesh->mNumFaces); - for (int f=0; fnFaces; ++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; + for (int f=0; fnFaces; ++f) { + aiFace& face = aiMesh->mFaces[f]; + mesh->faces[f] = make_int3(face.mIndices[0],face.mIndices[1],face.mIndices[2]); } - mesh->faces[f] = make_int3(face.mIndices[0],face.mIndices[1],face.mIndices[2]); - } - for (int v=0; vnVertices; ++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); + for (int v=0; vnVertices; ++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); + } + } // if aiMesh!=NULL + else { + std::cerr << "there are no triangle meshes in " << filename << std::endl; } aiReleaseImport(scene); From afc731fb734518123058a04fe9222e348f2609ee Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Thu, 2 Jun 2016 19:30:06 +0100 Subject: [PATCH 11/35] pre-process scene to filter triangle meshes and triangulate polygones --- src/mesh/assimp_mesh_reader.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/mesh/assimp_mesh_reader.cpp b/src/mesh/assimp_mesh_reader.cpp index bdccf84..9759964 100644 --- a/src/mesh/assimp_mesh_reader.cpp +++ b/src/mesh/assimp_mesh_reader.cpp @@ -2,6 +2,7 @@ #include #include +#include #include #include #include @@ -10,7 +11,12 @@ namespace dart { 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_Triangulate: triangulate ploygones such that all vertices have 3 points + // aiProcess_SortByPType: split meshes with mixed types into separate meshes + + const struct aiScene * scene = aiImportFile(filename.c_str(), import_flags); //aiProcess_JoinIdenticalVertices); if (scene == 0) { std::cerr << "error: " << aiGetErrorString() << std::endl; From 32d712cf6cd6864374a1f4a54cd5bd110447cfb8 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Thu, 2 Jun 2016 19:33:28 +0100 Subject: [PATCH 12/35] check for presence of normals before accessing them --- src/mesh/assimp_mesh_reader.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/mesh/assimp_mesh_reader.cpp b/src/mesh/assimp_mesh_reader.cpp index 9759964..1425253 100644 --- a/src/mesh/assimp_mesh_reader.cpp +++ b/src/mesh/assimp_mesh_reader.cpp @@ -45,8 +45,17 @@ dart::Mesh * AssimpMeshReader::readMesh(const std::string filename) const { for (int v=0; vnVertices; ++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); + + } + + if(aiMesh->HasNormals()) { + for(unsigned int n=0; nmNumVertices; n++) { + aiVector3D & normal = aiMesh->mNormals[n]; + mesh->normals[n] = make_float3(normal.x,normal.y,normal.z); + } + } + else { + std::cout<<"triangle mesh in "< Date: Mon, 6 Jun 2016 19:13:05 +0100 Subject: [PATCH 13/35] add dependencies to pkg-config file --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index db008b4..b0d901f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -274,7 +274,7 @@ 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") +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}" ) From 5badbbdbf5449a3b05e1ecc329ba70e11823344d Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Tue, 7 Jun 2016 12:54:54 +0100 Subject: [PATCH 14/35] get model ID and Pose by providing model name --- src/model/model.cpp | 1 + src/tracker.cpp | 8 ++++++++ src/tracker.h | 5 +++++ 3 files changed, 14 insertions(+) diff --git a/src/model/model.cpp b/src/model/model.cpp index 8e77a4c..57c2bc4 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; } diff --git a/src/tracker.cpp b/src/tracker.cpp index 27fcb92..a223296 100644 --- a/src/tracker.cpp +++ b/src/tracker.cpp @@ -479,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 219cdec..253b581 100644 --- a/src/tracker.h +++ b/src/tracker.h @@ -85,8 +85,13 @@ 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 std::map & getSizeParams(const int modelNum) { return _sizeParams[modelNum]; } inline const float4 * getHostVertMap() { return _pcSource->getHostVertMap(); } inline const float4 * getHostNormMap() { return _pcSource->getHostNormMap(); } From 87c4bd0b232b9deb854cdca212c7d488d23e25d7 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Tue, 7 Jun 2016 12:55:33 +0100 Subject: [PATCH 15/35] remove cluttering matrix output --- src/optimization/kernels/obsToMod.cu | 2 +- src/optimization/optimizer.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) 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 & 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 Date: Tue, 7 Jun 2016 17:44:52 +0100 Subject: [PATCH 16/35] read values for all joints, but ignore transformations of fixed joints --- src/model/host_only_model.cpp | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) 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 Date: Thu, 9 Jun 2016 11:18:25 +0100 Subject: [PATCH 17/35] ignore depth values that are out of range completely Setting w=0 of a vertex results in its placement at the camera origin instead of removing it from the scene. By using w=NAN, these vertices are not visualized at all. --- src/img_proc/organized_point_cloud.cu | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) 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; } } From 82526403483910c02ecdf8f39e3015ea1a84c3a0 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Thu, 9 Jun 2016 17:16:26 +0100 Subject: [PATCH 18/35] find joint IDs by providing their name --- src/model/model.cpp | 8 ++++++++ src/model/model.h | 1 + 2 files changed, 9 insertions(+) diff --git a/src/model/model.cpp b/src/model/model.cpp index 57c2bc4..2ef4686 100644 --- a/src/model/model.cpp +++ b/src/model/model.cpp @@ -206,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 joint '"+name+"' does not exist"); // not found +} + void Model::renderSdf(const dart::Grid3D & sdf, float levelSet) const { glDisable(GL_BLEND); diff --git a/src/model/model.h b/src/model/model.h index 43b5969..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 & sdf, float levelSet) const; void getArticulatedBoundingBox(float3 & mins, float3 & maxs, const float modelSdfPadding, const int nSweepPoints = 4); From f79813ea3398c8d07b5b8f15b319c9d0e74e3732 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Mon, 13 Jun 2016 10:59:37 +0100 Subject: [PATCH 19/35] set joint values of pose by their name This enables the possibility to provide more joints than needed by the model. In this case, required joints are automatically taken from the provided map. --- src/pose/pose.cpp | 11 +++++++++++ src/pose/pose.h | 3 +++ 2 files changed, 14 insertions(+) diff --git a/src/pose/pose.cpp b/src/pose/pose.cpp index eac10e0..1387220 100644 --- a/src/pose/pose.cpp +++ b/src/pose/pose.cpp @@ -45,6 +45,17 @@ Pose & Pose::operator=(const Pose & rhs) { return *this; } +void Pose::setReducedArticulation(const std::map &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)); + else + std::cout<<"not found: "<<_reduction->getName(i)< #include +#include #include #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 &joint_map); + virtual void projectReducedToFull() { return _reduction->projectReducedToFull(_reducedArticulation,_fullArticulation); } inline const float * getFirstDerivatives() const { return _reduction->getFirstDerivatives(); } From 8746f45619e69e8d58bb03c2f7953b42a672763a Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Mon, 13 Jun 2016 17:48:37 +0100 Subject: [PATCH 20/35] return pose reference that can be written to (is not const) --- src/tracker.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/tracker.h b/src/tracker.h index 253b581..4c72019 100644 --- a/src/tracker.h +++ b/src/tracker.h @@ -92,6 +92,9 @@ class Tracker { 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 & getSizeParams(const int modelNum) { return _sizeParams[modelNum]; } inline const float4 * getHostVertMap() { return _pcSource->getHostVertMap(); } inline const float4 * getHostNormMap() { return _pcSource->getHostNormMap(); } From 2c41974826077ca1ad59b0a2d193595481dc3b33 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Mon, 13 Jun 2016 17:50:51 +0100 Subject: [PATCH 21/35] read all joints, but ignore transformation of fixed joints --- src/model/mirrored_model.cpp | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) 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; fhostPtr()[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); From a588de83930134ca5564e2fefcc98a3864e8e04c Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Fri, 17 Jun 2016 12:21:57 +0100 Subject: [PATCH 22/35] ignore message if joint is not present --- src/pose/pose.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/pose/pose.cpp b/src/pose/pose.cpp index 1387220..483fe4a 100644 --- a/src/pose/pose.cpp +++ b/src/pose/pose.cpp @@ -51,8 +51,6 @@ void Pose::setReducedArticulation(const std::map &joint_map) // only access key (name) if it exists if(joint_map.count(_reduction->getName(i))>0) _reducedArticulation[i] = joint_map.at(_reduction->getName(i)); - else - std::cout<<"not found: "<<_reduction->getName(i)< Date: Thu, 7 Jul 2016 19:36:43 +0100 Subject: [PATCH 23/35] error message indicating wrong frame name --- src/model/model.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/model/model.cpp b/src/model/model.cpp index 2ef4686..60ce006 100644 --- a/src/model/model.cpp +++ b/src/model/model.cpp @@ -211,7 +211,7 @@ int Model::getJointIdByName(const std::string &name) { if(pos != _jointNames.end()) return std::distance(_jointNames.begin(), pos); else - throw std::range_error("requested joint '"+name+"' does not exist"); // not found + throw std::range_error("requested frame '"+name+"' does not exist"); } void Model::renderSdf(const dart::Grid3D & sdf, float levelSet) const { From d42079c06247fb56c73d01bd75247b11f17599b8 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Tue, 23 Aug 2016 18:21:52 +0100 Subject: [PATCH 24/35] support collada meshes (*.dae) The kinematic tree in the mesh is traversed recursively to obtain all vertices, normals and faces and their transformation into the root node frame. For mehses without tree structure (e.g. obj) there is only one node which is the root node. --- src/mesh/assimp_mesh_reader.cpp | 116 +++++++++++++++++++++----------- 1 file changed, 78 insertions(+), 38 deletions(-) diff --git a/src/mesh/assimp_mesh_reader.cpp b/src/mesh/assimp_mesh_reader.cpp index 1425253..ade7bae 100644 --- a/src/mesh/assimp_mesh_reader.cpp +++ b/src/mesh/assimp_mesh_reader.cpp @@ -7,16 +7,80 @@ #include #include +#include + namespace dart { +template +size_t vec_byte_size(const typename std::vector& vec) { + return sizeof(T) * vec.size(); +} + +aiMatrix4x4 getFullT(const std::vector &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 &verts, std::vector &normals, std::vector &faces, std::vector 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; mmNumMeshes; m++) { + const aiMesh* const mesh = scene->mMeshes[node->mMeshes[m]]; + if(mesh->mPrimitiveTypes == aiPrimitiveType_TRIANGLE) { + // transform each vertice + for(unsigned int v=0; vmNumVertices; 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; vmNumVertices && 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; fmNumFaces; 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; imNumChildren; i++) { + const aiNode* const child = node->mChildren[i]; + getMesh(scene, child, verts, normals, faces, transforms); + } +} + dart::Mesh * AssimpMeshReader::readMesh(const std::string filename) const { // define import flags for assimp - const unsigned int import_flags = aiProcess_Triangulate | aiProcess_SortByPType; + 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); //aiProcess_JoinIdenticalVertices); + const struct aiScene * scene = aiImportFile(filename.c_str(), import_flags); if (scene == 0) { std::cerr << "error: " << aiGetErrorString() << std::endl; @@ -24,48 +88,24 @@ dart::Mesh * AssimpMeshReader::readMesh(const std::string filename) const { return 0; } - // find first triangle mesh - aiMesh * aiMesh = NULL; - // stop searching if end of list or if first mesh is found (aiMesh!=NULL) - for(unsigned int i = 0; imNumMeshes && aiMesh==NULL; i++) { - if(scene->mMeshes[i]->mPrimitiveTypes == aiPrimitiveType_TRIANGLE) - aiMesh = scene->mMeshes[i]; - else - std::cout<<"ignoring non-triangle mesh in "<mNumVertices,aiMesh->mNumFaces); - - for (int f=0; fnFaces; ++f) { - aiFace& face = aiMesh->mFaces[f]; - mesh->faces[f] = make_int3(face.mIndices[0],face.mIndices[1],face.mIndices[2]); - } - for (int v=0; vnVertices; ++v) { - aiVector3D & vertex = aiMesh->mVertices[v]; - mesh->vertices[v] = make_float3(vertex.x,vertex.y,vertex.z); + std::vector faces; + std::vector verts; + std::vector normals; + std::vector transforms; - } + // recursively obtain vertices, normals and faces from meshes + const aiNode* const rnode = scene->mRootNode; + getMesh(scene, rnode, verts, normals, faces, transforms); - if(aiMesh->HasNormals()) { - for(unsigned int n=0; nmNumVertices; n++) { - aiVector3D & normal = aiMesh->mNormals[n]; - mesh->normals[n] = make_float3(normal.x,normal.y,normal.z); - } - } - else { - std::cout<<"triangle mesh in "<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; - } } From 7b15f9d098ab2cb294a53e4401300b496d02135c Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Fri, 3 Jun 2016 13:47:30 +0100 Subject: [PATCH 25/35] support for parsing URDF model descriptions in DART Link and Joints are extracted recursively from the root link that is automatically determined or can be provided by the user. Joints are stores as frames in the DART model, Links define the geometric representation. The package path is determined automatically and a optional mesh file extension can be provided to load meshes from other file types than provided in the URDF description. --- src/model/read_model_urdf.cpp | 314 ++++++++++++++++++++++++++++++++++ src/model/read_model_urdf.h | 21 +++ 2 files changed, 335 insertions(+) create mode 100644 src/model/read_model_urdf.cpp create mode 100644 src/model/read_model_urdf.h diff --git a/src/model/read_model_urdf.cpp b/src/model/read_model_urdf.cpp new file mode 100644 index 0000000..f2f6c16 --- /dev/null +++ b/src/model/read_model_urdf.cpp @@ -0,0 +1,314 @@ + +#include + +// urdf parser and model +#include + +// boost for parsing file paths +#include +#include + +// constants for determining and replacing the package path in URDF mesh paths +#define PACKAGE_PATH_FILE "package.xml" +#define PACKAGE_PATH_URI_SCHEME "package://" + +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::pair mesh_extension_surrogate; + std::string mesh_extension_surrogate; + /** + * @brief colour RGB colour definition for untextured meshes + */ + std::vector colour = {127, 127, 127}; +}; + +/** + * @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: "<name<name<child_joints; + + std::cout<<"id: "<name<<", joints: "<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 = ""; + + switch(geometry->type) { + case urdf::Geometry::SPHERE: { + geom_type = PrimitiveSphereType; + double radius = dynamic_cast(&*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(&*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(&*geometry)->radius; + double length = dynamic_cast(&*geometry)->length; + sx = std::to_string(length); + sy = std::to_string(radius); + sz = std::to_string(radius); + } + break; + case urdf::Geometry::MESH: { + geom_type = MeshType; + // geometry is a pointer to base class, we need to cast (*geometry) + mesh_path = dynamic_cast(&*geometry)->filename; + urdf::Vector3 scale = dynamic_cast(&*geometry)->scale; + sx = std::to_string(scale.x); + sy = std::to_string(scale.y); + sz = std::to_string(scale.z); + // we need to replace "package://" by full path + boost::algorithm::replace_first(mesh_path, PACKAGE_PATH_URI_SCHEME, conf.package_path); + if(!conf.mesh_extension_surrogate.empty()) { + mesh_path = boost::filesystem::path(mesh_path). + replace_extension(conf.mesh_extension_surrogate).native(); + } + std::cout<<"loading mesh from: "<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) + + // gray + 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; + + std::cout<<"processing joint: "<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 ("<type<<") is unsupported by DART"<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 limits = j->limits; + min = std::to_string(limits->lower); + max = std::to_string(limits->upper); + } + else { + switch(j->type) { + case urdf::Joint::FIXED: + std::cout<<"ignoring limits of FIXED joint: "<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) +{ + // parse URDF file + ModelInterfaceConstPtr urdf_model = urdf::parseURDFFile(path); + + // get robot name + std::cout<<"found URDF robot: "<getName()<getName()); + + // get root link + boost::shared_ptr 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: "<name< + +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 = ""); + +} // namespace dart + +#endif // READ_MODEL_URDF_H From 576004fe5baf35689fd05d834e152b9d9d2afacb Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Tue, 7 Jun 2016 17:57:29 +0100 Subject: [PATCH 26/35] aggregate common message parts --- src/model/read_model_urdf.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/model/read_model_urdf.cpp b/src/model/read_model_urdf.cpp index f2f6c16..cd1fd85 100644 --- a/src/model/read_model_urdf.cpp +++ b/src/model/read_model_urdf.cpp @@ -239,12 +239,14 @@ bool extract_frames(const int parent_id, LinkConstPtr &link, ModelInterfaceConst max = std::to_string(limits->upper); } else { + std::cout<<"ignoring limits of "; switch(j->type) { case urdf::Joint::FIXED: - std::cout<<"ignoring limits of FIXED joint: "< Date: Fri, 10 Jun 2016 16:00:22 +0100 Subject: [PATCH 27/35] readModelURDF function that returns a new model instead of modifying provided one --- src/model/read_model_urdf.cpp | 13 ++++++++++--- src/model/read_model_urdf.h | 11 ++++++++++- 2 files changed, 20 insertions(+), 4 deletions(-) diff --git a/src/model/read_model_urdf.cpp b/src/model/read_model_urdf.cpp index cd1fd85..06eacd2 100644 --- a/src/model/read_model_urdf.cpp +++ b/src/model/read_model_urdf.cpp @@ -268,9 +268,9 @@ bool extract_frames(const int parent_id, LinkConstPtr &link, ModelInterfaceConst /** * @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) +bool readModelURDF(const std::string &path, HostOnlyModel &model, + const std::string &root_link_name, + const std::string &mesh_extension_surrogate) { // parse URDF file ModelInterfaceConstPtr urdf_model = urdf::parseURDFFile(path); @@ -313,4 +313,11 @@ bool readModelURDF(const std::string path, HostOnlyModel &model, } } +const HostOnlyModel &readModelURDF(const std::string &path, const std::string &root_link_name, const std::string &mesh_extension_surrogate) +{ + HostOnlyModel *model = new HostOnlyModel(); + readModelURDF(path, *model, root_link_name, mesh_extension_surrogate); + return *model; +} + } // namespace dart diff --git a/src/model/read_model_urdf.h b/src/model/read_model_urdf.h index 328c2fb..6e2f946 100644 --- a/src/model/read_model_urdf.h +++ b/src/model/read_model_urdf.h @@ -14,7 +14,16 @@ namespace dart { * @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 = ""); +bool readModelURDF(const std::string &path, HostOnlyModel & model, const std::string &root_link_name = "", const std::string &mesh_extension_surrogate = ""); + +/** + * @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 = ""); } // namespace dart From 3da60a770e0212cfee40662d9ec4abf5af5006ac Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Mon, 13 Jun 2016 14:51:30 +0100 Subject: [PATCH 28/35] enable and fix all warnings --- src/model/read_model_urdf.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/model/read_model_urdf.cpp b/src/model/read_model_urdf.cpp index 06eacd2..a3f94f5 100644 --- a/src/model/read_model_urdf.cpp +++ b/src/model/read_model_urdf.cpp @@ -245,6 +245,8 @@ bool extract_frames(const int parent_id, LinkConstPtr &link, ModelInterfaceConst std::cout<<"FIXED"; break; case urdf::Joint::CONTINUOUS: std::cout<<"CONTINUOUS"; break; + default: + std::cout<<"type "<type< Date: Tue, 14 Jun 2016 15:16:34 +0100 Subject: [PATCH 29/35] interpret mesh path in URDF as relative path If file "package.xml" does exists in paths down to the root and package URI "package://" is not used in the mesh path, the given mesh path will be interpreted as path relative to the *.urdf file. --- src/model/read_model_urdf.cpp | 21 +++++++++++++++++---- 1 file changed, 17 insertions(+), 4 deletions(-) diff --git a/src/model/read_model_urdf.cpp b/src/model/read_model_urdf.cpp index a3f94f5..bc9a909 100644 --- a/src/model/read_model_urdf.cpp +++ b/src/model/read_model_urdf.cpp @@ -136,8 +136,14 @@ bool extract_frames(const int parent_id, LinkConstPtr &link, ModelInterfaceConst sx = std::to_string(scale.x); sy = std::to_string(scale.y); sz = std::to_string(scale.z); - // we need to replace "package://" by full path - boost::algorithm::replace_first(mesh_path, PACKAGE_PATH_URI_SCHEME, conf.package_path); + 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(); @@ -299,8 +305,15 @@ bool readModelURDF(const std::string &path, HostOnlyModel &model, fpath = fpath.parent_path(); } - // store package path with trailing directory seperator - conf.package_path = fpath.branch_path().native() + boost::filesystem::path::preferred_separator; + 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: "< Date: Tue, 14 Jun 2016 15:18:20 +0100 Subject: [PATCH 30/35] use colours from material description if provided --- src/model/read_model_urdf.cpp | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/src/model/read_model_urdf.cpp b/src/model/read_model_urdf.cpp index bc9a909..8d3cdd5 100644 --- a/src/model/read_model_urdf.cpp +++ b/src/model/read_model_urdf.cpp @@ -170,10 +170,19 @@ bool extract_frames(const int parent_id, LinkConstPtr &link, ModelInterfaceConst ry = std::to_string(pitch); // pitch: rotation around y-axis (facing right) rz = std::to_string(yaw); // yaw: rotation around z-axis (facing down) - // gray - r = conf.colour[0]; - g = conf.colour[1]; - b = conf.colour[2]; + // 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, From ed61a5f73c2a8d8a0582822787e62e9271897b5c Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Tue, 12 Jul 2016 19:09:57 +0100 Subject: [PATCH 31/35] fix DART model version to 1 --- src/model/read_model_urdf.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/model/read_model_urdf.cpp b/src/model/read_model_urdf.cpp index 8d3cdd5..426e893 100644 --- a/src/model/read_model_urdf.cpp +++ b/src/model/read_model_urdf.cpp @@ -296,6 +296,9 @@ bool readModelURDF(const std::string &path, HostOnlyModel &model, std::cout<<"found URDF robot: "<getName()<getName()); + // fix DART model version (currently 0 and above is checked) + model.setModelVersion(1); + // get root link boost::shared_ptr l_root; l_root = root_link_name.empty()? urdf_model->getRoot() : urdf_model->getLink(root_link_name); From 10331ec2db37ca5da5b4fb79906cfa41079cc175 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Wed, 24 Aug 2016 12:02:38 +0100 Subject: [PATCH 32/35] deactivate debug messages by default --- src/model/read_model_urdf.cpp | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/src/model/read_model_urdf.cpp b/src/model/read_model_urdf.cpp index 426e893..c3e62ff 100644 --- a/src/model/read_model_urdf.cpp +++ b/src/model/read_model_urdf.cpp @@ -12,6 +12,8 @@ #define PACKAGE_PATH_FILE "package.xml" #define PACKAGE_PATH_URI_SCHEME "package://" +//#define PRNT_DBG + namespace dart { /// type definitions for shared pointers @@ -29,15 +31,17 @@ 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::pair mesh_extension_surrogate; std::string mesh_extension_surrogate; + /** * @brief colour RGB colour definition for untextured meshes */ @@ -87,7 +91,9 @@ bool extract_frames(const int parent_id, LinkConstPtr &link, ModelInterfaceConst JointPtrVec joints = link->child_joints; +#ifdef PRNT_DBG std::cout<<"id: "<name<<", joints: "<name; +#ifdef PRNT_DBG std::cout<<"processing joint: "<type) { @@ -254,6 +264,7 @@ bool extract_frames(const int parent_id, LinkConstPtr &link, ModelInterfaceConst max = std::to_string(limits->upper); } else { +#ifdef PRNT_DBG std::cout<<"ignoring limits of "; switch(j->type) { case urdf::Joint::FIXED: @@ -264,6 +275,7 @@ bool extract_frames(const int parent_id, LinkConstPtr &link, ModelInterfaceConst std::cout<<"type "<type< Date: Wed, 24 Aug 2016 15:06:19 +0100 Subject: [PATCH 33/35] integrate URDF in cmake file --- CMakeLists.txt | 16 +++++++++++++++- src/model/read_model_urdf.cpp | 2 +- src/model/read_model_urdf.h | 2 +- 3 files changed, 17 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b0d901f..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 diff --git a/src/model/read_model_urdf.cpp b/src/model/read_model_urdf.cpp index c3e62ff..2272ede 100644 --- a/src/model/read_model_urdf.cpp +++ b/src/model/read_model_urdf.cpp @@ -1,5 +1,5 @@ -#include +#include "read_model_urdf.h" // urdf parser and model #include diff --git a/src/model/read_model_urdf.h b/src/model/read_model_urdf.h index 6e2f946..5d4e000 100644 --- a/src/model/read_model_urdf.h +++ b/src/model/read_model_urdf.h @@ -1,7 +1,7 @@ #ifndef READ_MODEL_URDF_H #define READ_MODEL_URDF_H -#include +#include "model/host_only_model.h" namespace dart { From 81ddd7cd4adce0f81141736792b0f45c12e99291 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Wed, 14 Sep 2016 11:45:13 +0100 Subject: [PATCH 34/35] fix cylinder dimensions and origin --- src/model/read_model_urdf.cpp | 38 +++++++++++++++++++---------------- 1 file changed, 21 insertions(+), 17 deletions(-) diff --git a/src/model/read_model_urdf.cpp b/src/model/read_model_urdf.cpp index 2272ede..e792491 100644 --- a/src/model/read_model_urdf.cpp +++ b/src/model/read_model_urdf.cpp @@ -108,6 +108,21 @@ bool extract_frames(const int parent_id, LinkConstPtr &link, ModelInterfaceConst 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; @@ -129,9 +144,13 @@ bool extract_frames(const int parent_id, LinkConstPtr &link, ModelInterfaceConst geom_type = PrimitiveCylinderType; double radius = dynamic_cast(&*geometry)->radius; double length = dynamic_cast(&*geometry)->length; - sx = std::to_string(length); + sx = std::to_string(radius); sy = std::to_string(radius); - sz = 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: { @@ -163,21 +182,6 @@ bool extract_frames(const int parent_id, LinkConstPtr &link, ModelInterfaceConst std::cerr<<"unknown geometry"<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) - // colour if(link->visual->material!=NULL) { urdf::Color colour = link->visual->material->color; From 071df0ffe30df2f3ac134e8a930582b0f7482f5a Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Wed, 21 Sep 2016 17:13:22 +0100 Subject: [PATCH 35/35] optional mesh colour if no material properties are given --- src/model/read_model_urdf.cpp | 11 +++++++---- src/model/read_model_urdf.h | 4 ++-- 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/src/model/read_model_urdf.cpp b/src/model/read_model_urdf.cpp index e792491..4c9145b 100644 --- a/src/model/read_model_urdf.cpp +++ b/src/model/read_model_urdf.cpp @@ -45,7 +45,7 @@ struct MeshLoaderConfig { /** * @brief colour RGB colour definition for untextured meshes */ - std::vector colour = {127, 127, 127}; + std::vector colour; }; /** @@ -303,7 +303,8 @@ bool extract_frames(const int parent_id, LinkConstPtr &link, ModelInterfaceConst */ bool readModelURDF(const std::string &path, HostOnlyModel &model, const std::string &root_link_name, - const std::string &mesh_extension_surrogate) + const std::string &mesh_extension_surrogate, + const std::vector &colour) { // parse URDF file ModelInterfaceConstPtr urdf_model = urdf::parseURDFFile(path); @@ -324,6 +325,8 @@ bool readModelURDF(const std::string &path, HostOnlyModel &model, MeshLoaderConfig conf; + conf.colour = colour; + // get full absolute path boost::filesystem::path fpath = boost::filesystem::canonical(path); @@ -355,10 +358,10 @@ bool readModelURDF(const std::string &path, HostOnlyModel &model, } } -const HostOnlyModel &readModelURDF(const std::string &path, const std::string &root_link_name, const std::string &mesh_extension_surrogate) +const HostOnlyModel &readModelURDF(const std::string &path, const std::string &root_link_name, const std::string &mesh_extension_surrogate, const std::vector &colour) { HostOnlyModel *model = new HostOnlyModel(); - readModelURDF(path, *model, root_link_name, mesh_extension_surrogate); + readModelURDF(path, *model, root_link_name, mesh_extension_surrogate, colour); return *model; } diff --git a/src/model/read_model_urdf.h b/src/model/read_model_urdf.h index 5d4e000..d77d86f 100644 --- a/src/model/read_model_urdf.h +++ b/src/model/read_model_urdf.h @@ -14,7 +14,7 @@ namespace dart { * @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 = ""); +bool readModelURDF(const std::string &path, HostOnlyModel & model, const std::string &root_link_name = "", const std::string &mesh_extension_surrogate = "", const std::vector &colour = {127, 127, 127}); /** * @brief readModelURDF parse URDF model description and store kinematic and meshes in DART model format @@ -23,7 +23,7 @@ bool readModelURDF(const std::string &path, HostOnlyModel & model, const std::st * @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 HostOnlyModel &readModelURDF(const std::string &path, const std::string &root_link_name = "", const std::string &mesh_extension_surrogate = "", const std::vector &colour = {127, 127, 127}); } // namespace dart