From 3f92882b719f326dbc1030db8bf3e9e0c3bb9535 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Mon, 6 Jul 2015 17:46:37 -0500 Subject: [PATCH 1/9] Made DartLoader error without SEGFAULTing. Replaced SkeletonPtr() with nullptr. --- dart/utils/urdf/DartLoader.cpp | 75 +++++++++++++++++++++------------- dart/utils/urdf/DartLoader.h | 6 +-- 2 files changed, 49 insertions(+), 32 deletions(-) diff --git a/dart/utils/urdf/DartLoader.cpp b/dart/utils/urdf/DartLoader.cpp index 0680ddb1f57c5..ffcee6cf863c9 100644 --- a/dart/utils/urdf/DartLoader.cpp +++ b/dart/utils/urdf/DartLoader.cpp @@ -174,12 +174,13 @@ std::string DartLoader::getFullFilePath(const std::string& _filename) const if(packageDirectory == mPackageDirectories.end()) { - dtwarn << "[DartLoader] Trying to load a URDF that uses package '" - << fullpath.substr(scheme, authority_end-scheme) - << "' (the full line is '" << fullpath - << "'), but we do not know the path to that package directory. " - << "Please use addPackageDirectory(~) to allow us to find the " - << "package directory.\n"; + dterr << "[DartLoader] Trying to load a URDF that uses package '" + << fullpath.substr(scheme, authority_end-scheme) + << "' (the full line is '" << fullpath + << "'), but we do not know the path to that package directory. " + << "Please use addPackageDirectory(~) to allow us to find the " + << "package directory.\n"; + fullpath = ""; } else { @@ -279,8 +280,10 @@ dynamics::SkeletonPtr DartLoader::modelInterfaceToSkeleton(const urdf::ModelInte else { root = root->child_links[0].get(); - dynamics::BodyNode::Properties rootProperties = - createDartNodeProperties(root); + dynamics::BodyNode::Properties rootProperties; + if (!createDartNodeProperties(root, &rootProperties)) + return nullptr; + rootNode = createDartJointAndNode( root->parent_joint.get(), rootProperties, nullptr, skeleton); if(nullptr == rootNode) @@ -292,8 +295,10 @@ dynamics::SkeletonPtr DartLoader::modelInterfaceToSkeleton(const urdf::ModelInte } else { - dynamics::BodyNode::Properties rootProperties = - createDartNodeProperties(root); + dynamics::BodyNode::Properties rootProperties; + if (!createDartNodeProperties(root, &rootProperties)) + return nullptr; + std::pair pair = skeleton->createJointAndBodyNodePair( nullptr, dynamics::FreeJoint::Properties( @@ -305,25 +310,34 @@ dynamics::SkeletonPtr DartLoader::modelInterfaceToSkeleton(const urdf::ModelInte for(size_t i = 0; i < root->child_links.size(); i++) { - createSkeletonRecursive(skeleton, root->child_links[i].get(), rootNode); + if (!createSkeletonRecursive(skeleton, root->child_links[i].get(), rootNode)) + return nullptr; + } return skeleton; } -void DartLoader::createSkeletonRecursive( +bool DartLoader::createSkeletonRecursive( dynamics::SkeletonPtr _skel, const urdf::Link* _lk, dynamics::BodyNode* _parentNode) { - dynamics::BodyNode::Properties properties = createDartNodeProperties(_lk); + dynamics::BodyNode::Properties properties; + if (!createDartNodeProperties(_lk, &properties)) + return false; + dynamics::BodyNode* node = createDartJointAndNode( _lk->parent_joint.get(), properties, _parentNode, _skel); + if(!node) + return false; for(unsigned int i = 0; i < _lk->child_links.size(); ++i) { - createSkeletonRecursive(_skel, _lk->child_links[i].get(), node); + if (!createSkeletonRecursive(_skel, _lk->child_links[i].get(), node)) + return false; } + return true; } @@ -442,7 +456,6 @@ dynamics::BodyNode* DartLoader::createDartJointAndNode( { dterr << "[DartLoader::createDartJoint] Unsupported joint type (" << _jt->type << ")\n"; - assert(false); return nullptr; } } @@ -453,16 +466,16 @@ dynamics::BodyNode* DartLoader::createDartJointAndNode( /** * @function createDartNode */ -dynamics::BodyNode::Properties DartLoader::createDartNodeProperties( - const urdf::Link* _lk) +bool DartLoader::createDartNodeProperties( + const urdf::Link* _lk, dynamics::BodyNode::Properties *node) { - dynamics::BodyNode::Properties node(_lk->name); + node->mName = _lk->name; // Load Inertial information if(_lk->inertial) { urdf::Pose origin = _lk->inertial->origin; - node.mInertia.setLocalCOM(toEigen(origin.position)); - node.mInertia.setMass(_lk->inertial->mass); + node->mInertia.setLocalCOM(toEigen(origin.position)); + node->mInertia.setMass(_lk->inertial->mass); Eigen::Matrix3d J; J << _lk->inertial->ixx, _lk->inertial->ixy, _lk->inertial->ixz, @@ -472,7 +485,7 @@ dynamics::BodyNode::Properties DartLoader::createDartNodeProperties( origin.rotation.y, origin.rotation.z)); J = R * J * R.transpose(); - node.mInertia.setMoment(J(0,0), J(1,1), J(2,2), + node->mInertia.setMoment(J(0,0), J(1,1), J(2,2), J(0,1), J(0,2), J(1,2)); } @@ -480,19 +493,20 @@ dynamics::BodyNode::Properties DartLoader::createDartNodeProperties( for(unsigned int i = 0; i < _lk->visual_array.size(); i++) { if(dynamics::ShapePtr shape = createShape(_lk->visual_array[i].get())) - { - node.mVizShapes.push_back(shape); - } + node->mVizShapes.push_back(shape); + else + return false; } // Set collision information for(unsigned int i = 0; i < _lk->collision_array.size(); i++) { - if(dynamics::ShapePtr shape = createShape(_lk->collision_array[i].get())) { - node.mColShapes.push_back(shape); - } + if(dynamics::ShapePtr shape = createShape(_lk->collision_array[i].get())) + node->mColShapes.push_back(shape); + else + return false; } - return node; + return true; } @@ -535,13 +549,16 @@ dynamics::ShapePtr DartLoader::createShape(const VisualOrCollision* _vizOrCol) else if(urdf::Mesh* mesh = dynamic_cast(_vizOrCol->geometry.get())) { std::string fullPath = getFullFilePath(mesh->filename); + if (fullPath.empty()) + return nullptr; + const aiScene* model = dynamics::MeshShape::loadMesh( fullPath ); if(!model) { dtwarn << "[DartLoader::createShape] Assimp could not load a model from " << "the file '" << fullPath << "'\n"; - shape = nullptr; + return nullptr; } else { diff --git a/dart/utils/urdf/DartLoader.h b/dart/utils/urdf/DartLoader.h index 670e2009bd3a6..b1b15d7beab22 100644 --- a/dart/utils/urdf/DartLoader.h +++ b/dart/utils/urdf/DartLoader.h @@ -92,7 +92,7 @@ class DartLoader { void parseWorldToEntityPaths(const std::string& _xml_string); dart::dynamics::SkeletonPtr modelInterfaceToSkeleton(const urdf::ModelInterface* _model); - void createSkeletonRecursive(dynamics::SkeletonPtr _skel, const urdf::Link* _lk, dynamics::BodyNode* _parent); + bool createSkeletonRecursive(dynamics::SkeletonPtr _skel, const urdf::Link* _lk, dynamics::BodyNode* _parent); template dynamics::ShapePtr createShape(const VisualOrCollision* _vizOrCol); @@ -103,8 +103,8 @@ class DartLoader { dynamics::BodyNode* _parent, dynamics::SkeletonPtr _skeleton); - dynamics::BodyNode::Properties createDartNodeProperties( - const urdf::Link* _lk); + bool createDartNodeProperties( + const urdf::Link* _lk, dynamics::BodyNode::Properties *properties); Eigen::Isometry3d toEigen(const urdf::Pose& _pose); Eigen::Vector3d toEigen(const urdf::Vector3& _vector); From b6e29a25a9f375cc7742289a14e656184a5d9944 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Tue, 7 Jul 2015 17:07:10 -0500 Subject: [PATCH 2/9] Replaced 'unsigned int' with 'size_t'. --- dart/utils/urdf/DartLoader.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/dart/utils/urdf/DartLoader.cpp b/dart/utils/urdf/DartLoader.cpp index ffcee6cf863c9..2d7f4c9854811 100644 --- a/dart/utils/urdf/DartLoader.cpp +++ b/dart/utils/urdf/DartLoader.cpp @@ -332,7 +332,7 @@ bool DartLoader::createSkeletonRecursive( if(!node) return false; - for(unsigned int i = 0; i < _lk->child_links.size(); ++i) + for(size_t i = 0; i < _lk->child_links.size(); ++i) { if (!createSkeletonRecursive(_skel, _lk->child_links[i].get(), node)) return false; @@ -490,7 +490,7 @@ bool DartLoader::createDartNodeProperties( } // Set visual information - for(unsigned int i = 0; i < _lk->visual_array.size(); i++) + for(size_t i = 0; i < _lk->visual_array.size(); i++) { if(dynamics::ShapePtr shape = createShape(_lk->visual_array[i].get())) node->mVizShapes.push_back(shape); @@ -499,7 +499,7 @@ bool DartLoader::createDartNodeProperties( } // Set collision information - for(unsigned int i = 0; i < _lk->collision_array.size(); i++) { + for(size_t i = 0; i < _lk->collision_array.size(); i++) { if(dynamics::ShapePtr shape = createShape(_lk->collision_array[i].get())) node->mColShapes.push_back(shape); else From a986fac80535551ba700c23bb92dc38781fabf5e Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Tue, 7 Jul 2015 20:46:35 -0500 Subject: [PATCH 3/9] Fixed another SEGFAULT in MeshShape::loadMesh. --- dart/dynamics/MeshShape.cpp | 8 +++++--- dart/utils/urdf/DartLoader.cpp | 25 +++++++++++-------------- 2 files changed, 16 insertions(+), 17 deletions(-) diff --git a/dart/dynamics/MeshShape.cpp b/dart/dynamics/MeshShape.cpp index 666c0c8f17261..4453c8bd15eb2 100644 --- a/dart/dynamics/MeshShape.cpp +++ b/dart/dynamics/MeshShape.cpp @@ -273,9 +273,11 @@ const aiScene* MeshShape::loadMesh(const std::string& _fileName) { aiProcess_OptimizeMeshes, nullptr, propertyStore); if(!scene) - dtwarn << "[MeshShape] Assimp could not load file: '" << _fileName << "'. " - << "This will likely result in a segmentation fault " - << "if you attempt to use the nullptr we return." << std::endl; + { + dtwarn << "[MeshShape::loadMesh] Assimp could not load file: '" + << _fileName << "'.\n"; + return nullptr; + } aiReleasePropertyStore(propertyStore); // Assimp rotates collada files such that the up-axis (specified in the diff --git a/dart/utils/urdf/DartLoader.cpp b/dart/utils/urdf/DartLoader.cpp index 2d7f4c9854811..29032ddbc55c7 100644 --- a/dart/utils/urdf/DartLoader.cpp +++ b/dart/utils/urdf/DartLoader.cpp @@ -550,28 +550,25 @@ dynamics::ShapePtr DartLoader::createShape(const VisualOrCollision* _vizOrCol) { std::string fullPath = getFullFilePath(mesh->filename); if (fullPath.empty()) + { + dtwarn << "[DartLoader::createShape] Skipping URDF mesh with empty" + " filename. We are returning a nullptr.\n"; return nullptr; + } const aiScene* model = dynamics::MeshShape::loadMesh( fullPath ); - if(!model) - { - dtwarn << "[DartLoader::createShape] Assimp could not load a model from " - << "the file '" << fullPath << "'\n"; - return nullptr; - } - else - { - shape = dynamics::ShapePtr(new dynamics::MeshShape( - Eigen::Vector3d(mesh->scale.x, mesh->scale.y, mesh->scale.z), model, fullPath)); - } + return nullptr; // MeshShape::loadMesh logs a warning + + shape = dynamics::ShapePtr(new dynamics::MeshShape( + Eigen::Vector3d(mesh->scale.x, mesh->scale.y, mesh->scale.z), model, fullPath)); } // Unknown geometry type else { - dtwarn << "[DartLoader::createShape] Unknown urdf Shape type " - << "(we only know of Sphere, Box, Cylinder, and Mesh). " - << "We are returning a nullptr." << std::endl; + dtwarn << "[DartLoader::createShape] Unknown URDF shape type" + " (we only know of Sphere, Box, Cylinder, and Mesh)." + " We are returning a nullptr.\n"; return nullptr; } From 6b2a90668f2854a6ff09ec7dd4696d12f38a1769 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Sun, 12 Jul 2015 17:23:04 -0500 Subject: [PATCH 4/9] Switched from pointer to reference out-param. --- dart/utils/urdf/DartLoader.cpp | 8 ++++---- dart/utils/urdf/DartLoader.h | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/dart/utils/urdf/DartLoader.cpp b/dart/utils/urdf/DartLoader.cpp index 29032ddbc55c7..d9e7531097701 100644 --- a/dart/utils/urdf/DartLoader.cpp +++ b/dart/utils/urdf/DartLoader.cpp @@ -310,7 +310,7 @@ dynamics::SkeletonPtr DartLoader::modelInterfaceToSkeleton(const urdf::ModelInte for(size_t i = 0; i < root->child_links.size(); i++) { - if (!createSkeletonRecursive(skeleton, root->child_links[i].get(), rootNode)) + if (!createSkeletonRecursive(skeleton, root->child_links[i].get(), *rootNode)) return nullptr; } @@ -321,20 +321,20 @@ dynamics::SkeletonPtr DartLoader::modelInterfaceToSkeleton(const urdf::ModelInte bool DartLoader::createSkeletonRecursive( dynamics::SkeletonPtr _skel, const urdf::Link* _lk, - dynamics::BodyNode* _parentNode) + dynamics::BodyNode& _parentNode) { dynamics::BodyNode::Properties properties; if (!createDartNodeProperties(_lk, &properties)) return false; dynamics::BodyNode* node = createDartJointAndNode( - _lk->parent_joint.get(), properties, _parentNode, _skel); + _lk->parent_joint.get(), properties, &_parentNode, _skel); if(!node) return false; for(size_t i = 0; i < _lk->child_links.size(); ++i) { - if (!createSkeletonRecursive(_skel, _lk->child_links[i].get(), node)) + if (!createSkeletonRecursive(_skel, _lk->child_links[i].get(), *node)) return false; } return true; diff --git a/dart/utils/urdf/DartLoader.h b/dart/utils/urdf/DartLoader.h index b1b15d7beab22..6f731315c0680 100644 --- a/dart/utils/urdf/DartLoader.h +++ b/dart/utils/urdf/DartLoader.h @@ -92,7 +92,7 @@ class DartLoader { void parseWorldToEntityPaths(const std::string& _xml_string); dart::dynamics::SkeletonPtr modelInterfaceToSkeleton(const urdf::ModelInterface* _model); - bool createSkeletonRecursive(dynamics::SkeletonPtr _skel, const urdf::Link* _lk, dynamics::BodyNode* _parent); + bool createSkeletonRecursive(dynamics::SkeletonPtr _skel, const urdf::Link* _lk, dynamics::BodyNode& _parent); template dynamics::ShapePtr createShape(const VisualOrCollision* _vizOrCol); From c620a0178b9cf8545136ed041d56acec0fccf526 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Sun, 12 Jul 2015 19:12:03 -0500 Subject: [PATCH 5/9] Revert "Switched from pointer to reference out-param." This reverts commit 6b2a90668f2854a6ff09ec7dd4696d12f38a1769. --- dart/utils/urdf/DartLoader.cpp | 8 ++++---- dart/utils/urdf/DartLoader.h | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/dart/utils/urdf/DartLoader.cpp b/dart/utils/urdf/DartLoader.cpp index d9e7531097701..29032ddbc55c7 100644 --- a/dart/utils/urdf/DartLoader.cpp +++ b/dart/utils/urdf/DartLoader.cpp @@ -310,7 +310,7 @@ dynamics::SkeletonPtr DartLoader::modelInterfaceToSkeleton(const urdf::ModelInte for(size_t i = 0; i < root->child_links.size(); i++) { - if (!createSkeletonRecursive(skeleton, root->child_links[i].get(), *rootNode)) + if (!createSkeletonRecursive(skeleton, root->child_links[i].get(), rootNode)) return nullptr; } @@ -321,20 +321,20 @@ dynamics::SkeletonPtr DartLoader::modelInterfaceToSkeleton(const urdf::ModelInte bool DartLoader::createSkeletonRecursive( dynamics::SkeletonPtr _skel, const urdf::Link* _lk, - dynamics::BodyNode& _parentNode) + dynamics::BodyNode* _parentNode) { dynamics::BodyNode::Properties properties; if (!createDartNodeProperties(_lk, &properties)) return false; dynamics::BodyNode* node = createDartJointAndNode( - _lk->parent_joint.get(), properties, &_parentNode, _skel); + _lk->parent_joint.get(), properties, _parentNode, _skel); if(!node) return false; for(size_t i = 0; i < _lk->child_links.size(); ++i) { - if (!createSkeletonRecursive(_skel, _lk->child_links[i].get(), *node)) + if (!createSkeletonRecursive(_skel, _lk->child_links[i].get(), node)) return false; } return true; diff --git a/dart/utils/urdf/DartLoader.h b/dart/utils/urdf/DartLoader.h index 6f731315c0680..b1b15d7beab22 100644 --- a/dart/utils/urdf/DartLoader.h +++ b/dart/utils/urdf/DartLoader.h @@ -92,7 +92,7 @@ class DartLoader { void parseWorldToEntityPaths(const std::string& _xml_string); dart::dynamics::SkeletonPtr modelInterfaceToSkeleton(const urdf::ModelInterface* _model); - bool createSkeletonRecursive(dynamics::SkeletonPtr _skel, const urdf::Link* _lk, dynamics::BodyNode& _parent); + bool createSkeletonRecursive(dynamics::SkeletonPtr _skel, const urdf::Link* _lk, dynamics::BodyNode* _parent); template dynamics::ShapePtr createShape(const VisualOrCollision* _vizOrCol); From 37092ded8dd3f60217503432f694a78e81f70437 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Sun, 12 Jul 2015 19:18:31 -0500 Subject: [PATCH 6/9] Switched from pointer to reference out-param. --- dart/utils/urdf/DartLoader.cpp | 20 ++++++++++---------- dart/utils/urdf/DartLoader.h | 2 +- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/dart/utils/urdf/DartLoader.cpp b/dart/utils/urdf/DartLoader.cpp index 29032ddbc55c7..5b533622fee04 100644 --- a/dart/utils/urdf/DartLoader.cpp +++ b/dart/utils/urdf/DartLoader.cpp @@ -281,7 +281,7 @@ dynamics::SkeletonPtr DartLoader::modelInterfaceToSkeleton(const urdf::ModelInte { root = root->child_links[0].get(); dynamics::BodyNode::Properties rootProperties; - if (!createDartNodeProperties(root, &rootProperties)) + if (!createDartNodeProperties(root, rootProperties)) return nullptr; rootNode = createDartJointAndNode( @@ -296,7 +296,7 @@ dynamics::SkeletonPtr DartLoader::modelInterfaceToSkeleton(const urdf::ModelInte else { dynamics::BodyNode::Properties rootProperties; - if (!createDartNodeProperties(root, &rootProperties)) + if (!createDartNodeProperties(root, rootProperties)) return nullptr; std::pair pair = @@ -324,7 +324,7 @@ bool DartLoader::createSkeletonRecursive( dynamics::BodyNode* _parentNode) { dynamics::BodyNode::Properties properties; - if (!createDartNodeProperties(_lk, &properties)) + if (!createDartNodeProperties(_lk, properties)) return false; dynamics::BodyNode* node = createDartJointAndNode( @@ -467,15 +467,15 @@ dynamics::BodyNode* DartLoader::createDartJointAndNode( * @function createDartNode */ bool DartLoader::createDartNodeProperties( - const urdf::Link* _lk, dynamics::BodyNode::Properties *node) + const urdf::Link* _lk, dynamics::BodyNode::Properties &node) { - node->mName = _lk->name; + node.mName = _lk->name; // Load Inertial information if(_lk->inertial) { urdf::Pose origin = _lk->inertial->origin; - node->mInertia.setLocalCOM(toEigen(origin.position)); - node->mInertia.setMass(_lk->inertial->mass); + node.mInertia.setLocalCOM(toEigen(origin.position)); + node.mInertia.setMass(_lk->inertial->mass); Eigen::Matrix3d J; J << _lk->inertial->ixx, _lk->inertial->ixy, _lk->inertial->ixz, @@ -485,7 +485,7 @@ bool DartLoader::createDartNodeProperties( origin.rotation.y, origin.rotation.z)); J = R * J * R.transpose(); - node->mInertia.setMoment(J(0,0), J(1,1), J(2,2), + node.mInertia.setMoment(J(0,0), J(1,1), J(2,2), J(0,1), J(0,2), J(1,2)); } @@ -493,7 +493,7 @@ bool DartLoader::createDartNodeProperties( for(size_t i = 0; i < _lk->visual_array.size(); i++) { if(dynamics::ShapePtr shape = createShape(_lk->visual_array[i].get())) - node->mVizShapes.push_back(shape); + node.mVizShapes.push_back(shape); else return false; } @@ -501,7 +501,7 @@ bool DartLoader::createDartNodeProperties( // Set collision information for(size_t i = 0; i < _lk->collision_array.size(); i++) { if(dynamics::ShapePtr shape = createShape(_lk->collision_array[i].get())) - node->mColShapes.push_back(shape); + node.mColShapes.push_back(shape); else return false; } diff --git a/dart/utils/urdf/DartLoader.h b/dart/utils/urdf/DartLoader.h index b1b15d7beab22..d5d1bd72b9e37 100644 --- a/dart/utils/urdf/DartLoader.h +++ b/dart/utils/urdf/DartLoader.h @@ -104,7 +104,7 @@ class DartLoader { dynamics::SkeletonPtr _skeleton); bool createDartNodeProperties( - const urdf::Link* _lk, dynamics::BodyNode::Properties *properties); + const urdf::Link* _lk, dynamics::BodyNode::Properties &properties); Eigen::Isometry3d toEigen(const urdf::Pose& _pose); Eigen::Vector3d toEigen(const urdf::Vector3& _vector); From e56f2c385d95ca2f3772c37df81638a2c89c16d3 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Fri, 17 Jul 2015 21:00:13 -0500 Subject: [PATCH 7/9] Added unittests for DartLoader. --- data/urdf/test/invalid.urdf | 1 + data/urdf/test/invalid_mesh.stl | 1 + data/urdf/test/invalid_mesh.urdf | 10 +++ data/urdf/test/missing_mesh.urdf | 10 +++ data/urdf/test/missing_package.urdf | 10 +++ data/urdf/test/primitive_geometry.urdf | 12 ++++ unittests/testDartLoader.cpp | 89 ++++++++++++++++++++++++++ 7 files changed, 133 insertions(+) create mode 100644 data/urdf/test/invalid.urdf create mode 100644 data/urdf/test/invalid_mesh.stl create mode 100644 data/urdf/test/invalid_mesh.urdf create mode 100644 data/urdf/test/missing_mesh.urdf create mode 100644 data/urdf/test/missing_package.urdf create mode 100644 data/urdf/test/primitive_geometry.urdf create mode 100644 unittests/testDartLoader.cpp diff --git a/data/urdf/test/invalid.urdf b/data/urdf/test/invalid.urdf new file mode 100644 index 0000000000000..657c9dd641453 --- /dev/null +++ b/data/urdf/test/invalid.urdf @@ -0,0 +1 @@ +This is not a valid URDF file. diff --git a/data/urdf/test/invalid_mesh.stl b/data/urdf/test/invalid_mesh.stl new file mode 100644 index 0000000000000..d79c56ca0d6d3 --- /dev/null +++ b/data/urdf/test/invalid_mesh.stl @@ -0,0 +1 @@ +This is not a valid STL file. diff --git a/data/urdf/test/invalid_mesh.urdf b/data/urdf/test/invalid_mesh.urdf new file mode 100644 index 0000000000000..e87d16d8b1152 --- /dev/null +++ b/data/urdf/test/invalid_mesh.urdf @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/data/urdf/test/missing_mesh.urdf b/data/urdf/test/missing_mesh.urdf new file mode 100644 index 0000000000000..0b9281c124138 --- /dev/null +++ b/data/urdf/test/missing_mesh.urdf @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/data/urdf/test/missing_package.urdf b/data/urdf/test/missing_package.urdf new file mode 100644 index 0000000000000..9e72b0ced5c03 --- /dev/null +++ b/data/urdf/test/missing_package.urdf @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/data/urdf/test/primitive_geometry.urdf b/data/urdf/test/primitive_geometry.urdf new file mode 100644 index 0000000000000..3b4489aca9642 --- /dev/null +++ b/data/urdf/test/primitive_geometry.urdf @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/unittests/testDartLoader.cpp b/unittests/testDartLoader.cpp new file mode 100644 index 0000000000000..b5e2323659d88 --- /dev/null +++ b/unittests/testDartLoader.cpp @@ -0,0 +1,89 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael Koval + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include "dart/utils/urdf/DartLoader.h" + +using dart::utils::DartLoader; + +TEST(DartLoader, parseSkeleton_NonExistantPathReturnsNull) +{ + DartLoader loader; + EXPECT_EQ(nullptr, + loader.parseSkeleton(DART_DATA_PATH"skel/test/does_not_exist.urdf")); +} + +TEST(DartLoader, parseSkeleton_InvalidUrdfReturnsNull) +{ + DartLoader loader; + EXPECT_EQ(nullptr, + loader.parseSkeleton(DART_DATA_PATH"urdf/test/invalid.urdf")); +} + +TEST(DartLoader, parseSkeleton_MissingMeshReturnsNull) +{ + DartLoader loader; + EXPECT_EQ(nullptr, + loader.parseSkeleton(DART_DATA_PATH"urdf/test/missing_mesh.urdf")); +} + +TEST(DartLoader, parseSkeleton_InvalidMeshReturnsNull) +{ + DartLoader loader; + EXPECT_EQ(nullptr, + loader.parseSkeleton(DART_DATA_PATH"urdf/test/invalid_mesh.urdf")); +} + +TEST(DartLoader, parseSkeleton_MissingPackageReturnsNull) +{ + DartLoader loader; + EXPECT_EQ(nullptr, + loader.parseSkeleton(DART_DATA_PATH"urdf/test/missing_package.urdf")); +} + +TEST(DartLoader, parseSkeleton_LoadsPrimitiveGeometry) +{ + DartLoader loader; + EXPECT_TRUE(nullptr != + loader.parseSkeleton(DART_DATA_PATH"urdf/test/primitive_geometry.urdf")); +} + +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From f54c5586821c180314c4b884650adea164a814f4 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Fri, 17 Jul 2015 21:13:08 -0500 Subject: [PATCH 8/9] Split SdfParser and SkelParser tests. --- unittests/testSdfParser.cpp | 83 +++++++++++++++++++ .../{testParser.cpp => testSkelParser.cpp} | 25 ------ 2 files changed, 83 insertions(+), 25 deletions(-) create mode 100644 unittests/testSdfParser.cpp rename unittests/{testParser.cpp => testSkelParser.cpp} (95%) diff --git a/unittests/testSdfParser.cpp b/unittests/testSdfParser.cpp new file mode 100644 index 0000000000000..40fee1f91738d --- /dev/null +++ b/unittests/testSdfParser.cpp @@ -0,0 +1,83 @@ +/* + * Copyright (c) 2013-2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include "TestHelpers.h" + +#include "dart/dynamics/SoftBodyNode.h" +#include "dart/dynamics/RevoluteJoint.h" +#include "dart/dynamics/PlanarJoint.h" +#include "dart/dynamics/Skeleton.h" +#include "dart/simulation/World.h" +#include "dart/utils/sdf/SdfParser.h" + +using namespace dart; +using namespace math; +using namespace dynamics; +using namespace simulation; +using namespace utils; + +//============================================================================== +TEST(SdfParser, SDFSingleBodyWithoutJoint) +{ + // Regression test for #444 + WorldPtr world + = SdfParser::readSdfFile( + DART_DATA_PATH"/sdf/test/single_bodynode_skeleton.world"); + EXPECT_TRUE(world != nullptr); + + SkeletonPtr skel = world->getSkeleton(0); + EXPECT_TRUE(skel != nullptr); + EXPECT_EQ(skel->getNumBodyNodes(), 1); + EXPECT_EQ(skel->getNumJoints(), 1); + + BodyNodePtr bodyNode = skel->getBodyNode(0); + EXPECT_TRUE(bodyNode != nullptr); + EXPECT_EQ(bodyNode->getNumVisualizationShapes(), 1); + EXPECT_EQ(bodyNode->getNumCollisionShapes(), 1); + + JointPtr joint = skel->getJoint(0); + EXPECT_TRUE(joint != nullptr); + EXPECT_EQ(joint->getType(), FreeJoint::getStaticType()); +} + +//============================================================================== +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/unittests/testParser.cpp b/unittests/testSkelParser.cpp similarity index 95% rename from unittests/testParser.cpp rename to unittests/testSkelParser.cpp index 2e6284856de8e..405dad2f7ce79 100644 --- a/unittests/testParser.cpp +++ b/unittests/testSkelParser.cpp @@ -45,7 +45,6 @@ #include "dart/simulation/World.h" #include "dart/simulation/World.h" #include "dart/utils/SkelParser.h" -#include "dart/utils/sdf/SdfParser.h" using namespace dart; using namespace math; @@ -438,30 +437,6 @@ TEST(Parser, JointDynamicsElements) EXPECT_EQ(joint1->getSpringStiffness (2), 1.0); } -//============================================================================== -TEST(Parser, SDFSingleBodyWithoutJoint) -{ - // Regression test for #444 - WorldPtr world - = SdfParser::readSdfFile( - DART_DATA_PATH"/sdf/test/single_bodynode_skeleton.world"); - EXPECT_TRUE(world != nullptr); - - SkeletonPtr skel = world->getSkeleton(0); - EXPECT_TRUE(skel != nullptr); - EXPECT_EQ(skel->getNumBodyNodes(), 1); - EXPECT_EQ(skel->getNumJoints(), 1); - - BodyNodePtr bodyNode = skel->getBodyNode(0); - EXPECT_TRUE(bodyNode != nullptr); - EXPECT_EQ(bodyNode->getNumVisualizationShapes(), 1); - EXPECT_EQ(bodyNode->getNumCollisionShapes(), 1); - - JointPtr joint = skel->getJoint(0); - EXPECT_TRUE(joint != nullptr); - EXPECT_EQ(joint->getType(), FreeJoint::getStaticType()); -} - //============================================================================== int main(int argc, char* argv[]) { From 5f77e0b8bddac525a5c4580059cfa5029b3294a5 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Fri, 17 Jul 2015 21:15:54 -0500 Subject: [PATCH 9/9] Fixed name of the SkelParser test suite. --- unittests/testSkelParser.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/unittests/testSkelParser.cpp b/unittests/testSkelParser.cpp index 405dad2f7ce79..a3785aaa80c8e 100644 --- a/unittests/testSkelParser.cpp +++ b/unittests/testSkelParser.cpp @@ -53,7 +53,7 @@ using namespace simulation; using namespace utils; //============================================================================== -TEST(Parser, DataStructure) +TEST(SkelParser, DataStructure) { bool valBool = true; int valInt = -3; @@ -97,7 +97,7 @@ TEST(Parser, DataStructure) } //============================================================================== -TEST(Parser, EmptyWorld) +TEST(SkelParser, EmptyWorld) { WorldPtr world = SkelParser::readWorld(DART_DATA_PATH"skel/test/empty.skel"); @@ -114,7 +114,7 @@ TEST(Parser, EmptyWorld) } //============================================================================== -TEST(Parser, SinglePendulum) +TEST(SkelParser, SinglePendulum) { WorldPtr world = SkelParser::readWorld( DART_DATA_PATH"skel/test/single_pendulum.skel"); @@ -134,7 +134,7 @@ TEST(Parser, SinglePendulum) } //============================================================================== -TEST(Parser, SerialChain) +TEST(SkelParser, SerialChain) { WorldPtr world = SkelParser::readWorld( DART_DATA_PATH"skel/test/serial_chain_ball_joint.skel"); @@ -154,7 +154,7 @@ TEST(Parser, SerialChain) } //============================================================================== -TEST(Parser, RigidAndSoftBodies) +TEST(SkelParser, RigidAndSoftBodies) { using namespace dart; using namespace math; @@ -179,7 +179,7 @@ TEST(Parser, RigidAndSoftBodies) } //============================================================================== -TEST(Parser, PlanarJoint) +TEST(SkelParser, PlanarJoint) { using namespace dart; using namespace math; @@ -328,7 +328,7 @@ TEST(SKEL_PARSER, JointActuatorType) EXPECT_EQ(joint4->getActuatorType(), Joint::VELOCITY);} //============================================================================== -TEST(Parser, DofAttributes) +TEST(SkelParser, DofAttributes) { WorldPtr world = SkelParser::readWorld( DART_DATA_PATH"/skel/test/dof_attribute_test.skel"); @@ -405,7 +405,7 @@ TEST(Parser, DofAttributes) } //============================================================================== -TEST(Parser, JointDynamicsElements) +TEST(SkelParser, JointDynamicsElements) { WorldPtr world = SkelParser::readWorld(