From 9ef55ab12da6fc5410263ad0a52ab87182b5d326 Mon Sep 17 00:00:00 2001 From: tfield Date: Fri, 5 Mar 2010 07:09:36 +0000 Subject: [PATCH] collada_urdf: library_geometries finished --- collada_urdf/src/STLLoader.cpp | 36 ++-- collada_urdf/src/STLLoader.h | 5 +- collada_urdf/src/urdf_to_collada.cpp | 273 ++++++++++++++++----------- 3 files changed, 174 insertions(+), 140 deletions(-) diff --git a/collada_urdf/src/STLLoader.cpp b/collada_urdf/src/STLLoader.cpp index 0443c68..e811e8a 100644 --- a/collada_urdf/src/STLLoader.cpp +++ b/collada_urdf/src/STLLoader.cpp @@ -54,33 +54,17 @@ bool Vector3::operator==(const Vector3& v) const { Mesh::Mesh() { } -bool Mesh::hasVertex(const Vector3& v) const { - for (std::vector::const_iterator i = vertices.begin(); i != vertices.end(); i++) - if (v == *i) - return true; - - return false; -} - -unsigned int Mesh::getVertexIndex(const Vector3& v) const { +int Mesh::getVertexIndex(const Vector3& v) const { for (unsigned int i = 0; i < vertices.size(); i++) if (vertices[i] == v) - return true; + return i; - return false; + return -1; } -void Mesh::addVertex(const Vector3& v) { - vertices.push_back(v); -} - -void Mesh::addNormal(const Vector3& n) { - normals.push_back(n); -} - -void Mesh::addIndex(unsigned int i) { - indices.push_back(i); -} +void Mesh::addVertex(const Vector3& v) { vertices.push_back(v); } +void Mesh::addNormal(const Vector3& n) { normals.push_back(n); } +void Mesh::addIndex(unsigned int i) { indices.push_back(i); } // @@ -104,12 +88,14 @@ void STLLoader::readBinary(FILE* filein, Mesh* mesh) { for (int iface = 0; iface < face_num; iface++) { Vector3 normal(readFloat(filein), readFloat(filein), readFloat(filein)); for (int i = 0; i < 3; i++) { - Vector3 vertex(readFloat(filein), readFloat(filein), readFloat(filein)); - if (!mesh->hasVertex(vertex)) { + Vector3 vertex(readFloat(filein), readFloat(filein), readFloat(filein)); + int index = mesh->getVertexIndex(vertex); + if (index == -1) { mesh->addVertex(vertex); mesh->addNormal(normal); + index = mesh->vertices.size() - 1; } - mesh->addIndex(mesh->getVertexIndex(vertex)); + mesh->addIndex(index); } readShortInt(filein); // 2 byte attribute } diff --git a/collada_urdf/src/STLLoader.h b/collada_urdf/src/STLLoader.h index d01e560..8c7d768 100644 --- a/collada_urdf/src/STLLoader.h +++ b/collada_urdf/src/STLLoader.h @@ -67,14 +67,13 @@ namespace collada_urdf public: Mesh(); - bool hasVertex(const Vector3& v) const; - unsigned int getVertexIndex(const Vector3& v) const; + int getVertexIndex(const Vector3& v) const; void addVertex(const Vector3& v); void addNormal(const Vector3& n); void addIndex(unsigned int i); - private: + public: std::vector vertices; std::vector normals; std::vector indices; diff --git a/collada_urdf/src/urdf_to_collada.cpp b/collada_urdf/src/urdf_to_collada.cpp index 2011d6d..4011542 100644 --- a/collada_urdf/src/urdf_to_collada.cpp +++ b/collada_urdf/src/urdf_to_collada.cpp @@ -84,21 +84,19 @@ public: urdf::Model* robot_; - Assimp::Importer importer_; - boost::shared_ptr collada_; domCOLLADA* dom_; domCOLLADA::domSceneRef scene_; - domLibrary_jointsRef jointsLib_; + domLibrary_geometriesRef geometriesLib_; domLibrary_visual_scenesRef visualScenesLib_; domLibrary_kinematics_scenesRef kinematicsScenesLib_; domLibrary_kinematics_modelsRef kinematicsModelsLib_; - domLibrary_articulated_systemsRef articulatedSystemsLib_; + domLibrary_jointsRef jointsLib_; domLibrary_physics_scenesRef physicsScenesLib_; domLibrary_materialsRef materialsLib_; domLibrary_effectsRef effectsLib_; - domLibrary_geometriesRef geometriesLib_; + //domLibrary_articulated_systemsRef articulatedSystemsLib_; public: ColladaWriter(urdf::Model* robot) : robot_(robot) { @@ -145,23 +143,26 @@ public: scene_ = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_SCENE)); visualScenesLib_ = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_VISUAL_SCENES)); - visualScenesLib_->setId("visual_scenes"); + visualScenesLib_->setId("vscenes"); geometriesLib_ = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_GEOMETRIES)); geometriesLib_->setId("geometries"); + kinematicsScenesLib_ = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_KINEMATICS_SCENES)); + kinematicsScenesLib_->setId("kinematics_scenes"); + kinematicsModelsLib_ = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_KINEMATICS_MODELS)); + kinematicsModelsLib_->setId("kinematics_models"); + jointsLib_ = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_JOINTS)); + jointsLib_->setId("joints"); + + /* effectsLib_ = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_EFFECTS)); effectsLib_->setId("effects"); materialsLib_ = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_MATERIALS)); materialsLib_->setId("materials"); - kinematicsModelsLib_ = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_KINEMATICS_MODELS)); - kinematicsModelsLib_->setId("kinematics_models"); articulatedSystemsLib_ = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_ARTICULATED_SYSTEMS)); articulatedSystemsLib_->setId("articulated_systems"); - kinematicsScenesLib_ = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_KINEMATICS_SCENES)); - kinematicsScenesLib_->setId("kinematics_scenes"); - physicsScenesLib_ = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_PHYSICS_SCENES)); physicsScenesLib_->setId("physics_scenes"); - jointsLib_ = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_JOINTS)); - jointsLib_->setId("joints"); + physicsScenesLib_ = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_PHYSICS_SCENES)); + */ } virtual ~ColladaWriter() { @@ -174,31 +175,32 @@ public: // Create visual scene s.vscene = daeSafeCast(visualScenesLib_->createAndPlace(COLLADA_ELEMENT_VISUAL_SCENE)); - s.vscene->setId("visual_scene"); + s.vscene->setId("vscene"); s.vscene->setName("URDF Visual Scene"); + // Create instance visual scene + s.viscene = daeSafeCast(scene_->createAndPlace(COLLADA_ELEMENT_INSTANCE_VISUAL_SCENE)); + s.viscene->setUrl((string("#") + string(s.vscene->getID())).c_str()); + // Create kinematics scene s.kscene = daeSafeCast(kinematicsScenesLib_->createAndPlace(COLLADA_ELEMENT_KINEMATICS_SCENE)); s.kscene->setId("kinematics_scene"); s.kscene->setName("URDF Kinematics Scene"); - // Create physic scene - s.pscene = daeSafeCast(physicsScenesLib_->createAndPlace(COLLADA_ELEMENT_PHYSICS_SCENE)); - s.pscene->setId("physics_scene"); - s.pscene->setName("URDF Physics Scene"); - - // Create instance visual scene - s.viscene = daeSafeCast(scene_->createAndPlace(COLLADA_ELEMENT_INSTANCE_VISUAL_SCENE)); - s.viscene->setUrl((string("#") + string(s.vscene->getID())).c_str()); - // Create instance kinematics scene s.kiscene = daeSafeCast(scene_->createAndPlace(COLLADA_ELEMENT_INSTANCE_KINEMATICS_SCENE)); s.kiscene->setUrl((string("#") + string(s.kscene->getID())).c_str()); + /* + // Create physics scene + s.pscene = daeSafeCast(physicsScenesLib_->createAndPlace(COLLADA_ELEMENT_PHYSICS_SCENE)); + s.pscene->setId("physics_scene"); + s.pscene->setName("URDF Physics Scene"); + // Create instance physics scene s.piscene = daeSafeCast(scene_->createAndPlace(COLLADA_ELEMENT_INSTANCE_PHYSICS_SCENE)); s.piscene->setUrl((string("#") + string(s.pscene->getID())).c_str()); - + */ return s; } @@ -213,11 +215,13 @@ public: bool writeScene() { SCENE scene = createScene(); + /* domPhysics_scene::domTechnique_commonRef common = daeSafeCast(scene.pscene->createAndPlace(COLLADA_ELEMENT_TECHNIQUE_COMMON)); // Create gravity - domTargetable_float3Ref g = daeSafeCast(common->createAndPlace (COLLADA_ELEMENT_GRAVITY)); + domTargetable_float3Ref g = daeSafeCast(common->createAndPlace(COLLADA_ELEMENT_GRAVITY)); g->getValue().set3(0.0, 0.0, 1.0); + */ addJoints(); addKinematics(); @@ -229,6 +233,8 @@ public: } void addGeometries() { + int link_num = 0; + for (map >::const_iterator i = robot_->links_.begin(); i != robot_->links_.end(); i++) { boost::shared_ptr urdf_link = i->second; @@ -237,14 +243,21 @@ public: switch (urdf_link->visual->geometry->type) { case urdf::Geometry::MESH: { - urdf::Mesh* mesh = (urdf::Mesh*) urdf_link->visual->geometry.get(); + urdf::Mesh* urdf_mesh = (urdf::Mesh*) urdf_link->visual->geometry.get(); - string filename = mesh->filename; - urdf::Vector3 scale = mesh->scale; + string filename = urdf_mesh->filename; + urdf::Vector3 scale = urdf_mesh->scale; // @todo use scale + // domGeometryRef geometry = daeSafeCast(geometriesLib_->createAndPlace(COLLADA_ELEMENT_GEOMETRY)); - loadMesh(filename, geometry); + string geometry_id = string("g1.link") + boost::lexical_cast(link_num) + string(".geom0"); + geometry->setId(geometry_id.c_str()); + { + loadMesh(filename, geometry, geometry_id); + } + // + link_num++; break; } case urdf::Geometry::SPHERE: { @@ -267,7 +280,7 @@ public: } } - void loadMesh(const string& filename, domGeometryRef mesh) { + void loadMesh(const string& filename, domGeometryRef geometry, const string& geometry_id) { // Load the mesh resource_retriever::MemoryResource resource; resource_retriever::Retriever retriever; @@ -280,22 +293,23 @@ public: } // Try assimp first, then STLLoader - if (!loadMeshWithAssimp(resource, mesh)) - if (!loadMeshWithSTLLoader(resource, mesh)) + if (!loadMeshWithAssimp(resource, geometry, geometry_id)) + if (!loadMeshWithSTLLoader(resource, geometry, geometry_id)) cerr << "*** Can't load " << filename << endl; } - bool loadMeshWithAssimp(const resource_retriever::MemoryResource& resource, domGeometryRef mesh) { + bool loadMeshWithAssimp(const resource_retriever::MemoryResource& resource, domGeometryRef geometry, const string& geometry_id) { // Import the mesh using assimp - const aiScene* scene = importer_.ReadFileFromMemory(resource.data.get(), resource.size, aiProcess_SortByPType /* aiProcess_CalcTangentSpace | aiProcess_Triangulate | aiProcess_JoinIdenticalVertices */); + Assimp::Importer importer; + const aiScene* scene = importer.ReadFileFromMemory(resource.data.get(), resource.size, aiProcess_SortByPType /* aiProcess_CalcTangentSpace | aiProcess_Triangulate | aiProcess_JoinIdenticalVertices */); if (!scene) return false; - buildMeshFromAssimp(scene->mRootNode, mesh); + buildMeshFromAssimp(scene, scene->mRootNode, geometry, geometry_id); return true; } - bool loadMeshWithSTLLoader(const resource_retriever::MemoryResource& resource, domGeometryRef mesh) { + bool loadMeshWithSTLLoader(const resource_retriever::MemoryResource& resource, domGeometryRef geometry, const string& geometry_id) { // Write the resource to a temporary file char tmp_filename[] = "/tmp/collada_urdf_XXXXXX"; int fd = mkstemp(tmp_filename); @@ -305,7 +319,7 @@ public: // Import the mesh using STLLoader STLLoader loader; Mesh* stl_mesh = loader.load(string(tmp_filename)); - buildMeshFromSTLLoader(stl_mesh, mesh); + buildMeshFromSTLLoader(stl_mesh, geometry, geometry_id); delete stl_mesh; // Delete the temporary file @@ -314,16 +328,88 @@ public: return true; } - void buildMeshFromSTLLoader(Mesh* mesh, daeElementRef parent) { - // + void buildMeshFromSTLLoader(Mesh* stl_mesh, daeElementRef parent, const string& geometry_id) { + // + domMeshRef mesh = daeSafeCast(parent->createAndPlace(COLLADA_ELEMENT_MESH)); + { + unsigned int num_vertices = stl_mesh->vertices.size(); + unsigned int num_indices = stl_mesh->indices.size(); + unsigned int num_faces = num_indices / 3; + + // + domSourceRef positions_source = daeSafeCast(mesh->createAndPlace(COLLADA_ELEMENT_SOURCE)); + positions_source->setId((geometry_id + string(".positions")).c_str()); + { + // + domFloat_arrayRef positions_array = daeSafeCast(positions_source->createAndPlace(COLLADA_ELEMENT_FLOAT_ARRAY)); + positions_array->setId((geometry_id + string(".positions-array")).c_str()); + positions_array->setCount(num_vertices); + positions_array->setDigits(6); // 6 decimal places + positions_array->getValue().setCount(num_vertices); + + for (unsigned int j = 0; j < num_vertices; j++) { + positions_array->getValue()[j] = stl_mesh->vertices[j].x; + positions_array->getValue()[j] = stl_mesh->vertices[j].y; + positions_array->getValue()[j] = stl_mesh->vertices[j].z; + } + + // + domSource::domTechnique_commonRef source_tech = daeSafeCast(positions_source->createAndPlace(COLLADA_ELEMENT_TECHNIQUE_COMMON)); + + // + domAccessorRef accessor = daeSafeCast(source_tech->createAndPlace(COLLADA_ELEMENT_ACCESSOR)); + accessor->setCount(num_vertices); + accessor->setSource(xsAnyURI(*positions_array, string("#") + geometry_id + string(".positions-array"))); + accessor->setStride(3); + + // + // + // + domParamRef px = daeSafeCast(accessor->createAndPlace(COLLADA_ELEMENT_PARAM)); px->setName("X"); px->setType("float"); + domParamRef py = daeSafeCast(accessor->createAndPlace(COLLADA_ELEMENT_PARAM)); py->setName("Y"); py->setType("float"); + domParamRef pz = daeSafeCast(accessor->createAndPlace(COLLADA_ELEMENT_PARAM)); pz->setName("Z"); pz->setType("float"); + } + + // + domVerticesRef vertices = daeSafeCast(mesh->createAndPlace(COLLADA_ELEMENT_VERTICES)); + vertices->setId("vertices"); + { + // + domInput_localRef vertices_input = daeSafeCast(vertices->createAndPlace(COLLADA_ELEMENT_INPUT)); + vertices_input->setSemantic("POSITION"); + vertices_input->setSource(domUrifragment(*positions_source, string("#") + geometry_id + string(".positions"))); + } + // + + // + domTrianglesRef triangles = daeSafeCast(mesh->createAndPlace(COLLADA_ELEMENT_TRIANGLES)); + { + triangles->setCount(num_faces); + triangles->setMaterial("mat0"); + + // + domInput_local_offsetRef vertex_offset = daeSafeCast(triangles->createAndPlace(COLLADA_ELEMENT_INPUT)); + vertex_offset->setSemantic("VERTEX"); + vertex_offset->setOffset(0); + vertex_offset->setSource(domUrifragment(*positions_source, string("#") + geometry_id + string("/vertices"))); + + //

0 1 2 3 ... + domPRef indices = daeSafeCast(triangles->createAndPlace(COLLADA_ELEMENT_P)); + indices->getValue().setCount(num_indices); + for (unsigned int i = 0; i < num_indices; i++) + indices->getValue()[i] = stl_mesh->indices[i]; + //

+ } + // + } + // } - void buildMeshFromAssimp(aiNode* node, daeElementRef parent) { + // EXPERIMENTAL: untested + void buildMeshFromAssimp(const aiScene* scene, aiNode* node, daeElementRef parent, const string& geometry_id) { if (node == NULL) return; - const aiScene* scene = importer_.GetScene(); - aiMatrix4x4 transform = node->mTransformation; aiNode* pnode = node->mParent; @@ -334,13 +420,12 @@ public: pnode = pnode->mParent; } - string parentid("mesh"); - - for (unsigned int i = 0; i < node->mNumMeshes; i++) { - aiMesh* aMesh = scene->mMeshes[i]; - - domMeshRef mesh = daeSafeCast(parent->createAndPlace(COLLADA_ELEMENT_MESH)); - { + // + domMeshRef mesh = daeSafeCast(parent->createAndPlace(COLLADA_ELEMENT_MESH)); + { + for (unsigned int i = 0; i < node->mNumMeshes; i++) { + aiMesh* aMesh = scene->mMeshes[i]; + // Add in the indices for each face for (unsigned int j = 0; j < aMesh->mNumFaces; j++) { aiFace* aFace = &(aMesh->mFaces[j]); @@ -350,95 +435,59 @@ public: //subMesh->AddIndex(aFace->mIndices[k]); } } - + domSourceRef positions_source = daeSafeCast(mesh->createAndPlace(COLLADA_ELEMENT_SOURCE)); { - positions_source->setId((parentid + string(".positions")).c_str()); - + positions_source->setId((geometry_id + string(".positions")).c_str()); + domFloat_arrayRef positions_array = daeSafeCast(positions_source->createAndPlace(COLLADA_ELEMENT_FLOAT_ARRAY)); - positions_array->setId((parentid + string(".positions-array")).c_str()); + positions_array->setId((geometry_id + string(".positions-array")).c_str()); positions_array->setCount(aMesh->mNumVertices); positions_array->setDigits(6); // 6 decimal places positions_array->getValue().setCount(3 * aMesh->mNumVertices); - + for (unsigned int j = 0; j < aMesh->mNumVertices; j++) { aiVector3D p; p.x = aMesh->mVertices[j].x; p.y = aMesh->mVertices[j].y; p.z = aMesh->mVertices[j].z; - + p *= transform; - + positions_array->getValue()[j] = p.x; positions_array->getValue()[j] = p.y; positions_array->getValue()[j] = p.z; - + /* - if (aMesh->HasNormals()) { - p.x = aMesh->mNormals[j].x; - p.y = aMesh->mNormals[j].y; - p.z = aMesh->mNormals[j].z; - } - - // @todo add normal - //subMesh->AddNormal(p.x, p.y, p.z); - - // @todo add tex coord - //if (aMesh->mNumUVComponents[0]) - // subMesh->AddTexCoord(aMesh->mTextureCoords[0][j].x, 1.0 -aMesh->mTextureCoords[0][j].y); - //else - // subMesh->AddTexCoord(0,0); - */ + if (aMesh->HasNormals()) { + p.x = aMesh->mNormals[j].x; + p.y = aMesh->mNormals[j].y; + p.z = aMesh->mNormals[j].z; + } + + // @todo add normal + //subMesh->AddNormal(p.x, p.y, p.z); + + // @todo add tex coord + //if (aMesh->mNumUVComponents[0]) + // subMesh->AddTexCoord(aMesh->mTextureCoords[0][j].x, 1.0 -aMesh->mTextureCoords[0][j].y); + //else + // subMesh->AddTexCoord(0,0); + */ } - - /* - domSource::domTechnique_commonRef psourcetec = daeSafeCast(pvertsource->createAndPlace(COLLADA_ELEMENT_TECHNIQUE_COMMON)); - domAccessorRef pacc = daeSafeCast(psourcetec->createAndPlace(COLLADA_ELEMENT_ACCESSOR)); - pacc->setCount(aMesh->mNumVertices); - pacc->setSource(xsAnyURI(*positions_array, string("#") + parentid + string(".positions-array"))); - pacc->setStride(3); - - domParamRef px = daeSafeCast(pacc->createAndPlace(COLLADA_ELEMENT_PARAM)); px->setName("X"); px->setType("float"); - domParamRef py = daeSafeCast(pacc->createAndPlace(COLLADA_ELEMENT_PARAM)); py->setName("Y"); py->setType("float"); - domParamRef pz = daeSafeCast(pacc->createAndPlace(COLLADA_ELEMENT_PARAM)); pz->setName("Z"); pz->setType("float"); - */ } - + domTrianglesRef triangles = daeSafeCast(mesh->createAndPlace(COLLADA_ELEMENT_TRIANGLES)); { triangles->setCount(aMesh->mNumFaces / 3); - //triangles->setMaterial("mat0"); - - /* - domInput_local_offsetRef pvertoffset = daeSafeCast(ptris->createAndPlace(COLLADA_ELEMENT_INPUT)); - pvertoffset->setSemantic("VERTEX"); - pvertoffset->setOffset(0); - pvertoffset->setSource(domUrifragment(*pverts, string("#") + parentid + string("/vertices"))); - domPRef pindices = daeSafeCast(ptris->createAndPlace(COLLADA_ELEMENT_P)); - pindices->getValue().setCount(mesh.indices.size()); - for (size_t ind = 0; ind < mesh.indices.size(); ++ind) - pindices->getValue()[ind] = mesh.indices[ind]; - */ } } for (unsigned int i = 0; i < node->mNumChildren; i++) - buildMeshFromAssimp(node->mChildren[i], mesh); + buildMeshFromAssimp(scene, node->mChildren[i], mesh, geometry_id); } } - /* - domGeometryRef writeGeometry(const string& parentid) { - domVerticesRef pverts = daeSafeCast(pdommesh->createAndPlace(COLLADA_ELEMENT_VERTICES)); - { - pverts->setId("vertices"); - domInput_localRef pvertinput = daeSafeCast(pverts->createAndPlace(COLLADA_ELEMENT_INPUT)); - pvertinput->setSemantic("POSITION"); - pvertinput->setSource(domUrifragment(*pvertsource, string("#")+parentid+string(".positions"))); - } - } - */ - void addJoints() { for (map >::const_iterator i = robot_->joints_.begin(); i != robot_->joints_.end(); i++) { boost::shared_ptr urdf_joint = i->second; @@ -500,11 +549,11 @@ public: break; } case urdf::Joint::FIXED: { - // Model as a PRISMATIC joint with no limits + // Model as a REVOLUTE joint with no leeway // joint.axis vector axes(1); - axes[0] = daeSafeCast(joint->createAndPlace(COLLADA_ELEMENT_PRISMATIC)); + axes[0] = daeSafeCast(joint->createAndPlace(COLLADA_ELEMENT_REVOLUTE)); axes[0]->setSid("axis0"); domAxisRef axis = daeSafeCast(axes[0]->createAndPlace(COLLADA_ELEMENT_AXIS)); axis->getValue().setCount(3);