bkinnightskytw
/
kdl_parser
mirror of https://github.com/existedinnettw/kdl_parser.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

collada_urdf: library_geometries finished

This commit is contained in:
tfield 2010-03-05 07:09:36 +00:00
parent 1a48be8bb4
commit 9ef55ab12d
3 changed files with 174 additions and 140 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
collada_urdf/src/STLLoader.cpp
View File

@ -54,33 +54,17 @@ bool Vector3::operator==(const Vector3& v) const {
Mesh::Mesh() {
}
bool Mesh::hasVertex(const Vector3& v) const {
for (std::vector<Vector3>::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); }
//
@ -105,11 +89,13 @@ void STLLoader::readBinary(FILE* filein, Mesh* mesh) {
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)) {
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
}

5
collada_urdf/src/STLLoader.h
View File

@ -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<Vector3> vertices;
std::vector<Vector3> normals;
std::vector<unsigned int> indices;

223
collada_urdf/src/urdf_to_collada.cpp
View File

@ -84,21 +84,19 @@ public:
urdf::Model* robot_;
Assimp::Importer importer_;
boost::shared_ptr<DAE> 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<domCOLLADA::domScene>(dom_->createAndPlace(COLLADA_ELEMENT_SCENE));
visualScenesLib_ = daeSafeCast<domLibrary_visual_scenes>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_VISUAL_SCENES));
visualScenesLib_->setId("visual_scenes");
visualScenesLib_->setId("vscenes");
geometriesLib_ = daeSafeCast<domLibrary_geometries>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_GEOMETRIES));
geometriesLib_->setId("geometries");
kinematicsScenesLib_ = daeSafeCast<domLibrary_kinematics_scenes>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_KINEMATICS_SCENES));
kinematicsScenesLib_->setId("kinematics_scenes");
kinematicsModelsLib_ = daeSafeCast<domLibrary_kinematics_models>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_KINEMATICS_MODELS));
kinematicsModelsLib_->setId("kinematics_models");
jointsLib_ = daeSafeCast<domLibrary_joints>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_JOINTS));
jointsLib_->setId("joints");
/*
effectsLib_ = daeSafeCast<domLibrary_effects>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_EFFECTS));
effectsLib_->setId("effects");
materialsLib_ = daeSafeCast<domLibrary_materials>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_MATERIALS));
materialsLib_->setId("materials");
kinematicsModelsLib_ = daeSafeCast<domLibrary_kinematics_models>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_KINEMATICS_MODELS));
kinematicsModelsLib_->setId("kinematics_models");
articulatedSystemsLib_ = daeSafeCast<domLibrary_articulated_systems>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_ARTICULATED_SYSTEMS));
articulatedSystemsLib_->setId("articulated_systems");
kinematicsScenesLib_ = daeSafeCast<domLibrary_kinematics_scenes>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_KINEMATICS_SCENES));
kinematicsScenesLib_->setId("kinematics_scenes");
physicsScenesLib_ = daeSafeCast<domLibrary_physics_scenes>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_PHYSICS_SCENES));
physicsScenesLib_->setId("physics_scenes");
jointsLib_ = daeSafeCast<domLibrary_joints>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_JOINTS));
jointsLib_->setId("joints");
physicsScenesLib_ = daeSafeCast<domLibrary_physics_scenes>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_PHYSICS_SCENES));
*/
}
virtual ~ColladaWriter() {
@ -174,31 +175,32 @@ public:
// Create visual scene
s.vscene = daeSafeCast<domVisual_scene>(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<domInstance_with_extra>(scene_->createAndPlace(COLLADA_ELEMENT_INSTANCE_VISUAL_SCENE));
s.viscene->setUrl((string("#") + string(s.vscene->getID())).c_str());
// Create kinematics scene
s.kscene = daeSafeCast<domKinematics_scene>(kinematicsScenesLib_->createAndPlace(COLLADA_ELEMENT_KINEMATICS_SCENE));
s.kscene->setId("kinematics_scene");
s.kscene->setName("URDF Kinematics Scene");
// Create physic scene
s.pscene = daeSafeCast<domPhysics_scene>(physicsScenesLib_->createAndPlace(COLLADA_ELEMENT_PHYSICS_SCENE));
s.pscene->setId("physics_scene");
s.pscene->setName("URDF Physics Scene");
// Create instance visual scene
s.viscene = daeSafeCast<domInstance_with_extra>(scene_->createAndPlace(COLLADA_ELEMENT_INSTANCE_VISUAL_SCENE));
s.viscene->setUrl((string("#") + string(s.vscene->getID())).c_str());
// Create instance kinematics scene
s.kiscene = daeSafeCast<domInstance_kinematics_scene>(scene_->createAndPlace(COLLADA_ELEMENT_INSTANCE_KINEMATICS_SCENE));
s.kiscene->setUrl((string("#") + string(s.kscene->getID())).c_str());
/*
// Create physics scene
s.pscene = daeSafeCast<domPhysics_scene>(physicsScenesLib_->createAndPlace(COLLADA_ELEMENT_PHYSICS_SCENE));
s.pscene->setId("physics_scene");
s.pscene->setName("URDF Physics Scene");
// Create instance physics scene
s.piscene = daeSafeCast<domInstance_with_extra>(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<domPhysics_scene::domTechnique_common>(scene.pscene->createAndPlace(COLLADA_ELEMENT_TECHNIQUE_COMMON));
// Create gravity
domTargetable_float3Ref g = daeSafeCast<domTargetable_float3>(common->createAndPlace (COLLADA_ELEMENT_GRAVITY));
domTargetable_float3Ref g = daeSafeCast<domTargetable_float3>(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<string, boost::shared_ptr<urdf::Link> >::const_iterator i = robot_->links_.begin(); i != robot_->links_.end(); i++) {
boost::shared_ptr<urdf::Link> 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
// <geometry id="g1.link0.geom0">
domGeometryRef geometry = daeSafeCast<domGeometry>(geometriesLib_->createAndPlace(COLLADA_ELEMENT_GEOMETRY));
loadMesh(filename, geometry);
string geometry_id = string("g1.link") + boost::lexical_cast<string>(link_num) + string(".geom0");
geometry->setId(geometry_id.c_str());
{
loadMesh(filename, geometry, geometry_id);
}
// </geometry>
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) {
// <mesh>
domMeshRef mesh = daeSafeCast<domMesh>(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;
// <source id="g1.link0.geom0.positions">
domSourceRef positions_source = daeSafeCast<domSource>(mesh->createAndPlace(COLLADA_ELEMENT_SOURCE));
positions_source->setId((geometry_id + string(".positions")).c_str());
{
// <float_array id="g1.link0.geom0.positions-array" count="4533" digits="6">
domFloat_arrayRef positions_array = daeSafeCast<domFloat_array>(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;
}
void buildMeshFromAssimp(aiNode* node, daeElementRef parent) {
// <technique_common>
domSource::domTechnique_commonRef source_tech = daeSafeCast<domSource::domTechnique_common>(positions_source->createAndPlace(COLLADA_ELEMENT_TECHNIQUE_COMMON));
// <accessor count="4533" source="#g1.link0.geom0.positions-array" stride="3">
domAccessorRef accessor = daeSafeCast<domAccessor>(source_tech->createAndPlace(COLLADA_ELEMENT_ACCESSOR));
accessor->setCount(num_vertices);
accessor->setSource(xsAnyURI(*positions_array, string("#") + geometry_id + string(".positions-array")));
accessor->setStride(3);
// <param name="X" type="float"/>
// <param name="Y" type="float"/>
// <param name="Z" type="float"/>
domParamRef px = daeSafeCast<domParam>(accessor->createAndPlace(COLLADA_ELEMENT_PARAM)); px->setName("X"); px->setType("float");
domParamRef py = daeSafeCast<domParam>(accessor->createAndPlace(COLLADA_ELEMENT_PARAM)); py->setName("Y"); py->setType("float");
domParamRef pz = daeSafeCast<domParam>(accessor->createAndPlace(COLLADA_ELEMENT_PARAM)); pz->setName("Z"); pz->setType("float");
}
// <vertices id="vertices">
domVerticesRef vertices = daeSafeCast<domVertices>(mesh->createAndPlace(COLLADA_ELEMENT_VERTICES));
vertices->setId("vertices");
{
// <input semantic="POSITION" source="#g1.link0.geom0.positions"/>
domInput_localRef vertices_input = daeSafeCast<domInput_local>(vertices->createAndPlace(COLLADA_ELEMENT_INPUT));
vertices_input->setSemantic("POSITION");
vertices_input->setSource(domUrifragment(*positions_source, string("#") + geometry_id + string(".positions")));
}
// </vertices>
// <triangles count="1511" material="mat0">
domTrianglesRef triangles = daeSafeCast<domTriangles>(mesh->createAndPlace(COLLADA_ELEMENT_TRIANGLES));
{
triangles->setCount(num_faces);
triangles->setMaterial("mat0");
// <input offset="0" semantic="VERTEX" source="#g1.link0.geom0/vertices" set="0"/>
domInput_local_offsetRef vertex_offset = daeSafeCast<domInput_local_offset>(triangles->createAndPlace(COLLADA_ELEMENT_INPUT));
vertex_offset->setSemantic("VERTEX");
vertex_offset->setOffset(0);
vertex_offset->setSource(domUrifragment(*positions_source, string("#") + geometry_id + string("/vertices")));
// <p>0 1 2 3 ...
domPRef indices = daeSafeCast<domP>(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];
// </p>
}
// </triangles>
}
// </mesh>
}
// 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");
// <mesh>
domMeshRef mesh = daeSafeCast<domMesh>(parent->createAndPlace(COLLADA_ELEMENT_MESH));
{
for (unsigned int i = 0; i < node->mNumMeshes; i++) {
aiMesh* aMesh = scene->mMeshes[i];
domMeshRef mesh = daeSafeCast<domMesh>(parent->createAndPlace(COLLADA_ELEMENT_MESH));
{
// Add in the indices for each face
for (unsigned int j = 0; j < aMesh->mNumFaces; j++) {
aiFace* aFace = &(aMesh->mFaces[j]);
@ -353,10 +438,10 @@ public:
domSourceRef positions_source = daeSafeCast<domSource>(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<domFloat_array>(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);
@ -390,55 +475,19 @@ public:
// subMesh->AddTexCoord(0,0);
*/
}
/*
domSource::domTechnique_commonRef psourcetec = daeSafeCast<domSource::domTechnique_common>(pvertsource->createAndPlace(COLLADA_ELEMENT_TECHNIQUE_COMMON));
domAccessorRef pacc = daeSafeCast<domAccessor>(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<domParam>(pacc->createAndPlace(COLLADA_ELEMENT_PARAM)); px->setName("X"); px->setType("float");
domParamRef py = daeSafeCast<domParam>(pacc->createAndPlace(COLLADA_ELEMENT_PARAM)); py->setName("Y"); py->setType("float");
domParamRef pz = daeSafeCast<domParam>(pacc->createAndPlace(COLLADA_ELEMENT_PARAM)); pz->setName("Z"); pz->setType("float");
*/
}
domTrianglesRef triangles = daeSafeCast<domTriangles>(mesh->createAndPlace(COLLADA_ELEMENT_TRIANGLES));
{
triangles->setCount(aMesh->mNumFaces / 3);
//triangles->setMaterial("mat0");
/*
domInput_local_offsetRef pvertoffset = daeSafeCast<domInput_local_offset>(ptris->createAndPlace(COLLADA_ELEMENT_INPUT));
pvertoffset->setSemantic("VERTEX");
pvertoffset->setOffset(0);
pvertoffset->setSource(domUrifragment(*pverts, string("#") + parentid + string("/vertices")));
domPRef pindices = daeSafeCast<domP>(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<domVertices>(pdommesh->createAndPlace(COLLADA_ELEMENT_VERTICES));
{
pverts->setId("vertices");
domInput_localRef pvertinput = daeSafeCast<domInput_local>(pverts->createAndPlace(COLLADA_ELEMENT_INPUT));
pvertinput->setSemantic("POSITION");
pvertinput->setSource(domUrifragment(*pvertsource, string("#")+parentid+string(".positions")));
}
}
*/
void addJoints() {
for (map<string, boost::shared_ptr<urdf::Joint> >::const_iterator i = robot_->joints_.begin(); i != robot_->joints_.end(); i++) {
boost::shared_ptr<urdf::Joint> 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<domAxis_constraintRef> axes(1);
axes[0] = daeSafeCast<domAxis_constraint>(joint->createAndPlace(COLLADA_ELEMENT_PRISMATIC));
axes[0] = daeSafeCast<domAxis_constraint>(joint->createAndPlace(COLLADA_ELEMENT_REVOLUTE));
axes[0]->setSid("axis0");
domAxisRef axis = daeSafeCast<domAxis>(axes[0]->createAndPlace(COLLADA_ELEMENT_AXIS));
axis->getValue().setCount(3);