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

major update on collada exporter, now supports writing articulated systems along with parsing the dae meshes

This commit is contained in:
rdiankov 2010-12-07 12:47:01 +00:00
parent 2758eff268
commit 0c6cec8356
10 changed files with 1217 additions and 1440 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
collada_urdf/CMakeLists.txt
View File

@ -4,7 +4,7 @@ set(ROS_BUILD_TYPE Debug)
rosbuild_init() rosbuild_init()
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
rosbuild_add_library(collada_urdf src/collada_urdf.cpp src/collada_writer.cpp src/stl_loader.cpp) rosbuild_add_library(collada_urdf src/collada_urdf.cpp)
rosbuild_link_boost(collada_urdf system) rosbuild_link_boost(collada_urdf system)
rosbuild_add_executable(urdf_to_collada src/urdf_to_collada.cpp) rosbuild_add_executable(urdf_to_collada src/urdf_to_collada.cpp)

44
collada_urdf/include/collada_urdf/collada_urdf.h
View File

@ -1,7 +1,7 @@
/********************************************************************* /*********************************************************************
* Software License Agreement (BSD License) * Software License Agreement (BSD License)
* *
* Copyright (c) 2010, Willow Garage, Inc. * Copyright (c) 2010, Willow Garage, Inc., University of Tokyo
* All rights reserved. * All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
@ -32,17 +32,14 @@
* POSSIBILITY OF SUCH DAMAGE. * POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/ *********************************************************************/
/* Author: Tim Field */ /* Authors: Tim Field, Rosen Diankov */
#ifndef COLLADA_URDF_COLLADA_URDF_H #ifndef COLLADA_URDF_COLLADA_URDF_H
#define COLLADA_URDF_COLLADA_URDF_H #define COLLADA_URDF_COLLADA_URDF_H
#include <string> #include <string>
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <dae.h> #include <dae.h>
#include "urdf/model.h" #include "urdf/model.h"
namespace collada_urdf { namespace collada_urdf {
@ -53,33 +50,20 @@ public:
ColladaUrdfException(std::string const& what); ColladaUrdfException(std::string const& what);
}; };
/** Construct a COLLADA DOM from an URDF file enum WriteOptions
* \param file The filename from where to read the URDF {
* \param dom The resulting COLLADA DOM WO_IgnoreCollisionGeometry = 1, ///< if set, will use only the visual geometry
* \return true on success, false on failure };
*/
bool colladaFromUrdfFile(std::string const& file, boost::shared_ptr<DAE>& dom);
/** Construct a COLLADA DOM from a string containing URDF /** Construct a COLLADA DOM from an URDF model
* \param xml A string containing the XML description of the robot
* \param dom The resulting COLLADA DOM \param robot_model The initialized robot model
* \return true on success, false on failure \param dom The resulting COLLADA DOM
\param writeoptions A combination of \ref WriteOptions
\return true on success, false on failure
*/ */
bool colladaFromUrdfString(std::string const& xml, boost::shared_ptr<DAE>& dom); bool colladaFromUrdfModel(const urdf::Model& robot_model, boost::shared_ptr<DAE>& dom, int writeoptions=0);
/** Construct a COLLADA DOM from a TiXmlDocument containing URDF
* \param xml_doc The TiXmlDocument containing URDF
* \param dom The resulting COLLADA DOM
* \return true on success, false on failure
*/
bool colladaFromUrdfXml(TiXmlDocument* xml_doc, boost::shared_ptr<DAE>& dom);
/** Construct a COLLADA DOM from a URDF robot model
* \param robot_model The URDF robot model
* \param dom The resulting COLLADA DOM
* \return true on success, false on failure
*/
bool colladaFromUrdfModel(urdf::Model const& robot_model, boost::shared_ptr<DAE>& dom);
/** Write a COLLADA DOM to a file /** Write a COLLADA DOM to a file
* \param dom COLLADA DOM to write * \param dom COLLADA DOM to write

144
collada_urdf/include/collada_urdf/collada_writer.h
View File

@ -1,144 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redstributions 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.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* 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 OWNER 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.
*********************************************************************/
/* Author: Tim Field */
#ifndef COLLADA_URDF_COLLADA_WRITER_H
#define COLLADA_URDF_COLLADA_WRITER_H
#include "collada_urdf/collada_urdf.h"
#include <map>
#include <dae.h>
#include <dae/daeDocument.h>
#include <dae/daeErrorHandler.h>
#include <dae/domAny.h>
#include <dom/domCOLLADA.h>
#include <dom/domConstants.h>
#include <dom/domElements.h>
#include <dom/domTriangles.h>
#include <dom/domTypes.h>
#include <resource_retriever/retriever.h>
#include <urdf/model.h>
#include <urdf/pose.h>
#include <angles/angles.h>
namespace collada_urdf {
class Mesh;
/**
* Implements writing urdf::Model objects to a COLLADA DOM.
*
* The API for this class is unstable. The public API for collada_urdf is declared in collada_urdf.h.
*/
class ColladaWriter : public daeErrorHandler
{
private:
struct SCENE
{
domVisual_sceneRef vscene;
domKinematics_sceneRef kscene;
domPhysics_sceneRef pscene;
domInstance_with_extraRef viscene;
domInstance_kinematics_sceneRef kiscene;
domInstance_with_extraRef piscene;
};
public:
ColladaWriter(urdf::Model const& robot);
virtual ~ColladaWriter();
boost::shared_ptr<DAE> convert();
protected:
virtual void handleError(daeString msg);
virtual void handleWarning(daeString msg);
private:
void initDocument(std::string const& documentName);
SCENE createScene();
void setupPhysics(SCENE const& scene);
void addGeometries();
void loadMesh(std::string const& filename, domGeometryRef geometry, std::string const& geometry_id);
bool loadMeshWithSTLLoader(resource_retriever::MemoryResource const& resource, domGeometryRef geometry, std::string const& geometry_id);
void buildMeshFromSTLLoader(boost::shared_ptr<Mesh> stl_mesh, daeElementRef parent, std::string const& geometry_id);
void addKinematics(SCENE const& scene);
void addJoints(daeElementRef parent);
void addKinematicLink(boost::shared_ptr<urdf::Link const> urdf_link, daeElementRef parent, int& link_num);
void addVisuals(SCENE const& scene);
void addVisualLink(boost::shared_ptr<urdf::Link const> urdf_link, daeElementRef parent, int& link_num);
void addMaterials();
domEffectRef addEffect(std::string const& geometry_id, urdf::Color const& color_ambient, urdf::Color const& color_diffuse);
void addBindings(SCENE const& scene);
domTranslateRef addTranslate(daeElementRef parent, urdf::Vector3 const& position, daeElementRef before = NULL, bool ignore_zero_translations = false);
domRotateRef addRotate(daeElementRef parent, urdf::Rotation const& r, daeElementRef before = NULL, bool ignore_zero_rotations = false);
void addMimicJoint(domFormulaRef formula, const std::string& joint_sid,const std::string& joint_mimic_sid, double multiplier, double offset);
std::string getTimeStampString() const;
private:
static int s_doc_count_;
urdf::Model robot_;
boost::shared_ptr<DAE> collada_;
domCOLLADA* dom_;
domCOLLADA::domSceneRef scene_;
domLibrary_geometriesRef geometriesLib_;
domLibrary_visual_scenesRef visualScenesLib_;
domLibrary_kinematics_scenesRef kinematicsScenesLib_;
domLibrary_kinematics_modelsRef kinematicsModelsLib_;
domLibrary_jointsRef jointsLib_;
domLibrary_physics_scenesRef physicsScenesLib_;
domLibrary_materialsRef materialsLib_;
domLibrary_effectsRef effectsLib_;
domKinematics_modelRef kmodel_;
std::map<std::string, std::string> geometry_ids_; //!< link.name -> geometry.id
std::map<std::string, std::string> joint_sids_; //!< joint.name -> joint.sid
std::map<std::string, std::string> node_ids_; //!< joint.name -> node.id
};
}
#endif

95
collada_urdf/include/collada_urdf/stl_loader.h
View File

@ -1,95 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redstributions 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.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* 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 OWNER 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.
*********************************************************************/
/* Author: Tim Field */
#ifndef COLLADA_URDF_STL_LOADER_H
#define COLLADA_URDF_STL_LOADER_H
#include <stdint.h>
#include <string>
#include <vector>
#include <boost/shared_ptr.hpp>
namespace collada_urdf {
class Vector3
{
public:
Vector3(float x, float y, float z);
bool operator==(Vector3 const& v) const;
float x;
float y;
float z;
};
class Mesh
{
public:
Mesh();
int getVertexIndex(Vector3 const& v) const;
void addVertex(Vector3 const& v);
void addNormal(Vector3 const& n);
void addIndex(unsigned int i);
public:
std::vector<Vector3> vertices;
std::vector<Vector3> normals;
std::vector<unsigned int> indices;
};
class STLLoader
{
public:
boost::shared_ptr<Mesh> load(std::string const& filename);
private:
FILE* file_;
boost::shared_ptr<Mesh> mesh_;
void readBinary();
uint32_t readLongInt();
uint16_t readShortInt();
float readFloat();
};
}
#endif

3
collada_urdf/manifest.xml
View File

@ -6,7 +6,7 @@
Implements robot-specific COLLADA extensions as defined by Implements robot-specific COLLADA extensions as defined by
http://openrave.programmingvision.com/index.php/Started:COLLADA http://openrave.programmingvision.com/index.php/Started:COLLADA
</description> </description>
<author>Tim Field</author> <author>Tim Field and Rosen Diankov</author>
<license>BSD</license> <license>BSD</license>
<review status="Doc reviewed" notes="2010-04-23"/> <review status="Doc reviewed" notes="2010-04-23"/>
<url>http://ros.org/wiki/collada_urdf</url> <url>http://ros.org/wiki/collada_urdf</url>
@ -15,6 +15,7 @@
<depend package="colladadom" /> <depend package="colladadom" />
<depend package="resource_retriever" /> <depend package="resource_retriever" />
<depend package="angles" /> <depend package="angles" />
<depend package="assimp"/>
<platform os="ubuntu" version="9.04"/> <platform os="ubuntu" version="9.04"/>
<platform os="ubuntu" version="9.10"/> <platform os="ubuntu" version="9.10"/>
<platform os="ubuntu" version="10.04"/> <platform os="ubuntu" version="10.04"/>

1233
collada_urdf/src/collada_urdf.cpp
View File

File diff suppressed because it is too large Load Diff

975
collada_urdf/src/collada_writer.cpp
View File

@ -1,975 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redstributions 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.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* 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 OWNER 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.
*********************************************************************/
/* Author: Tim Field */
#include "collada_urdf/collada_writer.h"
#include "collada_urdf/stl_loader.h"
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/date_time/posix_time/posix_time_io.hpp>
#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH
using std::string;
using std::map;
using std::vector;
using boost::shared_ptr;
namespace collada_urdf {
int ColladaWriter::s_doc_count_ = 0;
ColladaWriter::ColladaWriter(urdf::Model const& robot) : robot_(robot), dom_(NULL) {
daeErrorHandler::setErrorHandler(this);
collada_.reset(new DAE);
collada_->setIOPlugin(NULL);
collada_->setDatabase(NULL);
}
ColladaWriter::~ColladaWriter() { }
shared_ptr<DAE> ColladaWriter::convert() {
try {
string doc_count_str = boost::lexical_cast<string>(s_doc_count_++);
initDocument(string("collada_urdf_") + doc_count_str + string(".dae"));
SCENE scene = createScene();
setupPhysics(scene);
addGeometries();
addKinematics(scene);
addVisuals(scene);
addMaterials();
addBindings(scene);
return collada_;
}
catch (ColladaUrdfException ex) {
ROS_ERROR("Error converting: %s", ex.what());
return shared_ptr<DAE>();
}
}
// Implementation
void ColladaWriter::handleError(daeString msg) {
throw ColladaUrdfException(msg);
}
void ColladaWriter::handleWarning(daeString msg) {
std::cerr << "COLLADA DOM warning: " << msg << std::endl;
}
void ColladaWriter::initDocument(string const& documentName) {
// Create the document
daeDocument* doc = NULL;
daeInt error = collada_->getDatabase()->insertDocument(documentName.c_str(), &doc); // also creates a collada root
if (error != DAE_OK || doc == NULL)
throw ColladaUrdfException("Failed to create document");
dom_ = daeSafeCast<domCOLLADA>(doc->getDomRoot());
dom_->setAttribute("xmlns:math", "http://www.w3.org/1998/Math/MathML");
// Create asset elements
domAssetRef asset = daeSafeCast<domAsset>(dom_->add(COLLADA_ELEMENT_ASSET));
{
string date = getTimeStampString();
domAsset::domCreatedRef created = daeSafeCast<domAsset::domCreated>(asset->add(COLLADA_ELEMENT_CREATED));
created->setValue(date.c_str());
domAsset::domModifiedRef modified = daeSafeCast<domAsset::domModified>(asset->add(COLLADA_ELEMENT_MODIFIED));
modified->setValue(date.c_str());
domAsset::domContributorRef contrib = daeSafeCast<domAsset::domContributor>(asset->add(COLLADA_ELEMENT_CONTRIBUTOR));
domAsset::domContributor::domAuthoring_toolRef authoringtool = daeSafeCast<domAsset::domContributor::domAuthoring_tool>(contrib->add(COLLADA_ELEMENT_AUTHORING_TOOL));
authoringtool->setValue("URDF Collada Writer");
domAsset::domUnitRef units = daeSafeCast<domAsset::domUnit>(asset->add(COLLADA_ELEMENT_UNIT));
units->setMeter(1);
units->setName("meter");
domAsset::domUp_axisRef zup = daeSafeCast<domAsset::domUp_axis>(asset->add(COLLADA_ELEMENT_UP_AXIS));
zup->setValue(UP_AXIS_Z_UP);
}
// Create top-level elements
scene_ = daeSafeCast<domCOLLADA::domScene>(dom_->add(COLLADA_ELEMENT_SCENE));
visualScenesLib_ = daeSafeCast<domLibrary_visual_scenes>(dom_->add(COLLADA_ELEMENT_LIBRARY_VISUAL_SCENES));
visualScenesLib_->setId("vscenes");
geometriesLib_ = daeSafeCast<domLibrary_geometries>(dom_->add(COLLADA_ELEMENT_LIBRARY_GEOMETRIES));
geometriesLib_->setId("geometries");
kinematicsScenesLib_ = daeSafeCast<domLibrary_kinematics_scenes>(dom_->add(COLLADA_ELEMENT_LIBRARY_KINEMATICS_SCENES));
kinematicsScenesLib_->setId("kscenes");
kinematicsModelsLib_ = daeSafeCast<domLibrary_kinematics_models>(dom_->add(COLLADA_ELEMENT_LIBRARY_KINEMATICS_MODELS));
kinematicsModelsLib_->setId("kmodels");
jointsLib_ = daeSafeCast<domLibrary_joints>(dom_->add(COLLADA_ELEMENT_LIBRARY_JOINTS));
jointsLib_->setId("joints");
physicsScenesLib_ = daeSafeCast<domLibrary_physics_scenes>(dom_->add(COLLADA_ELEMENT_LIBRARY_PHYSICS_SCENES));
physicsScenesLib_->setId("physics_scenes");
effectsLib_ = daeSafeCast<domLibrary_effects>(dom_->add(COLLADA_ELEMENT_LIBRARY_EFFECTS));
effectsLib_->setId("effects");
materialsLib_ = daeSafeCast<domLibrary_materials>(dom_->add(COLLADA_ELEMENT_LIBRARY_MATERIALS));
materialsLib_->setId("materials");
}
ColladaWriter::SCENE ColladaWriter::createScene() {
SCENE s;
// Create visual scene
s.vscene = daeSafeCast<domVisual_scene>(visualScenesLib_->add(COLLADA_ELEMENT_VISUAL_SCENE));
s.vscene->setId("vscene");
s.vscene->setName("URDF Visual Scene");
// Create instance visual scene
s.viscene = daeSafeCast<domInstance_with_extra>(scene_->add(COLLADA_ELEMENT_INSTANCE_VISUAL_SCENE));
s.viscene->setUrl((string("#") + string(s.vscene->getID())).c_str());
// Create kinematics scene
s.kscene = daeSafeCast<domKinematics_scene>(kinematicsScenesLib_->add(COLLADA_ELEMENT_KINEMATICS_SCENE));
s.kscene->setId("kscene");
s.kscene->setName("URDF Kinematics Scene");
// Create instance kinematics scene
s.kiscene = daeSafeCast<domInstance_kinematics_scene>(scene_->add(COLLADA_ELEMENT_INSTANCE_KINEMATICS_SCENE));
s.kiscene->setUrl((string("#") + string(s.kscene->getID())).c_str());
// Create physics scene
s.pscene = daeSafeCast<domPhysics_scene>(physicsScenesLib_->add(COLLADA_ELEMENT_PHYSICS_SCENE));
s.pscene->setId("pscene");
s.pscene->setName("URDF Physics Scene");
// Create instance physics scene
s.piscene = daeSafeCast<domInstance_with_extra>(scene_->add(COLLADA_ELEMENT_INSTANCE_PHYSICS_SCENE));
s.piscene->setUrl((string("#") + string(s.pscene->getID())).c_str());
return s;
}
void ColladaWriter::setupPhysics(SCENE const& scene) {
// <technique_common>
domPhysics_scene::domTechnique_commonRef common = daeSafeCast<domPhysics_scene::domTechnique_common>(scene.pscene->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
{
// <gravity>0 0 0
domTargetable_float3Ref g = daeSafeCast<domTargetable_float3>(common->add(COLLADA_ELEMENT_GRAVITY));
g->getValue().set3(0.0, 0.0, 0.0);
// </gravity>
}
// </technique_common>
}
void ColladaWriter::addGeometries() {
int link_num = 0;
for (map<string, shared_ptr<urdf::Link> >::const_iterator i = robot_.links_.begin(); i != robot_.links_.end(); i++) {
shared_ptr<urdf::Link> urdf_link = i->second;
boost::shared_ptr< urdf::Geometry > geometry;
if( !!urdf_link->visual ) {
geometry = urdf_link->visual->geometry;
}
else if (!!urdf_link->collision ) {
geometry = urdf_link->collision->geometry;
}
if( !geometry ) {
continue;
}
switch (geometry->type) {
case urdf::Geometry::MESH: {
urdf::Mesh* urdf_mesh = (urdf::Mesh*) geometry.get();
string filename = urdf_mesh->filename;
urdf::Vector3 scale = urdf_mesh->scale; // @todo use scale
// <geometry id="g1.link0.geom0">
domGeometryRef geometry = daeSafeCast<domGeometry>(geometriesLib_->add(COLLADA_ELEMENT_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_ids_[urdf_link->name] = geometry_id;
// </geometry>
link_num++;
break;
}
case urdf::Geometry::SPHERE: {
std::cerr << "Warning: geometry type SPHERE of link " << urdf_link->name << " not exported" << std::endl;
break;
}
case urdf::Geometry::BOX: {
std::cerr << "Warning: geometry type BOX of link " << urdf_link->name << " not exported" << std::endl;
break;
}
case urdf::Geometry::CYLINDER: {
std::cerr << "Warning: geometry type CYLINDER of link " << urdf_link->name << " not exported" << std::endl;
break;
}
default: {
std::cerr << "Warning: geometry type " << geometry->type << " of link " << urdf_link->name << " not exported" << std::endl;
break;
}
}
}
}
void ColladaWriter::loadMesh(string const& filename, domGeometryRef geometry, string const& geometry_id) {
// Load the mesh
resource_retriever::MemoryResource resource;
resource_retriever::Retriever retriever;
try {
resource = retriever.get(filename.c_str());
}
catch (resource_retriever::Exception& e) {
std::cerr << "Unable to load mesh file " << filename << ": " << e.what() << std::endl;
return;
}
// Try assimp first, then STLLoader
try {
loadMeshWithSTLLoader(resource, geometry, geometry_id);
}
catch (ColladaUrdfException e) {
std::cerr << "Unable to load mesh file " << filename << ": " << e.what() << std::endl;
}
}
bool ColladaWriter::loadMeshWithSTLLoader(resource_retriever::MemoryResource const& resource, domGeometryRef geometry, string const& geometry_id) {
// Write the resource to a temporary file
char tmp_filename[] = "/tmp/collada_urdf_XXXXXX";
int fd = mkstemp(tmp_filename);
if (fd == -1)
throw ColladaUrdfException("Couldn't create temporary file");
if ((uint32_t) write(fd, resource.data.get(), resource.size) != resource.size) {
close(fd);
unlink(tmp_filename);
throw ColladaUrdfException("Couldn't write resource to file");
}
close(fd);
// Import the mesh using STLLoader
STLLoader loader;
shared_ptr<Mesh> stl_mesh = loader.load(string(tmp_filename));
if (stl_mesh == shared_ptr<Mesh>()) {
unlink(tmp_filename);
throw ColladaUrdfException("Couldn't import STL mesh");
}
// Build the COLLADA mesh
buildMeshFromSTLLoader(stl_mesh, geometry, geometry_id);
// Delete the temporary file
unlink(tmp_filename);
return true;
}
void ColladaWriter::buildMeshFromSTLLoader(shared_ptr<Mesh> stl_mesh, daeElementRef parent, string const& geometry_id) {
// <mesh>
domMeshRef mesh = daeSafeCast<domMesh>(parent->add(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->add(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->add(COLLADA_ELEMENT_FLOAT_ARRAY));
positions_array->setId((geometry_id + string(".positions-array")).c_str());
positions_array->setCount(num_vertices * 3);
positions_array->setDigits(6); // 6 decimal places
positions_array->getValue().setCount(num_vertices * 3);
for (unsigned int j = 0; j < num_vertices; j++) {
positions_array->getValue()[j * 3 ] = stl_mesh->vertices[j].x;
positions_array->getValue()[j * 3 + 1] = stl_mesh->vertices[j].y;
positions_array->getValue()[j * 3 + 2] = stl_mesh->vertices[j].z;
}
// </float_array>
// <technique_common>
domSource::domTechnique_commonRef source_tech = daeSafeCast<domSource::domTechnique_common>(positions_source->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
{
// <accessor count="4533" source="#g1.link0.geom0.positions-array" stride="3">
domAccessorRef accessor = daeSafeCast<domAccessor>(source_tech->add(COLLADA_ELEMENT_ACCESSOR));
accessor->setCount(num_vertices / 3);
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->add(COLLADA_ELEMENT_PARAM)); px->setName("X"); px->setType("float");
domParamRef py = daeSafeCast<domParam>(accessor->add(COLLADA_ELEMENT_PARAM)); py->setName("Y"); py->setType("float");
domParamRef pz = daeSafeCast<domParam>(accessor->add(COLLADA_ELEMENT_PARAM)); pz->setName("Z"); pz->setType("float");
}
// </accessor>
}
// </technique_common>
}
// <vertices id="vertices">
domVerticesRef vertices = daeSafeCast<domVertices>(mesh->add(COLLADA_ELEMENT_VERTICES));
string vertices_id = geometry_id + string(".vertices");
vertices->setId(vertices_id.c_str());
{
// <input semantic="POSITION" source="#g1.link0.geom0.positions"/>
domInput_localRef vertices_input = daeSafeCast<domInput_local>(vertices->add(COLLADA_ELEMENT_INPUT));
vertices_input->setSemantic("POSITION");
vertices_input->setSource(domUrifragment(*positions_source, string("#") + string(positions_source->getId())));
}
// </vertices>
// <triangles count="1511" material="mat0">
domTrianglesRef triangles = daeSafeCast<domTriangles>(mesh->add(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->add(COLLADA_ELEMENT_INPUT));
vertex_offset->setSemantic("VERTEX");
vertex_offset->setOffset(0);
vertex_offset->setSource(domUrifragment(*positions_source, string("#") + vertices_id));
{
// <p>0 1 2 3 ...
domPRef indices = daeSafeCast<domP>(triangles->add(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>
}
void ColladaWriter::addJoints(daeElementRef parent) {
int joint_num = 0;
for (map<string, shared_ptr<urdf::Joint> >::const_iterator i = robot_.joints_.begin(); i != robot_.joints_.end(); i++) {
shared_ptr<urdf::Joint> urdf_joint = i->second;
// <joint name="base_laser_joint" sid="joint0">
domJointRef joint = daeSafeCast<domJoint>(parent->add(COLLADA_ELEMENT_JOINT));
string joint_sid = string("joint") + boost::lexical_cast<string>(joint_num);
joint_num++;
joint->setName(urdf_joint->name.c_str());
joint->setSid(joint_sid.c_str());
joint_sids_[urdf_joint->name] = joint_sid;
double axis_x = urdf_joint->axis.x;
double axis_y = urdf_joint->axis.y;
double axis_z = urdf_joint->axis.z;
if (axis_x == 0.0 && axis_y == 0.0 && axis_z == 0.0) {
axis_x = 1.0;
axis_y = 0.0;
axis_z = 0.0;
}
switch (urdf_joint->type)
{
case urdf::Joint::REVOLUTE: {
// <revolute sid="axis0">
domAxis_constraintRef revolute = daeSafeCast<domAxis_constraint>(joint->add(COLLADA_ELEMENT_REVOLUTE));
revolute->setSid("axis0");
{
// <axis>
domAxisRef axis = daeSafeCast<domAxis>(revolute->add(COLLADA_ELEMENT_AXIS));
{
axis->getValue().setCount(3);
axis->getValue()[0] = axis_x;
axis->getValue()[1] = axis_y;
axis->getValue()[2] = axis_z;
}
// </axis>
// <limits>
domJoint_limitsRef limits = daeSafeCast<domJoint_limits>(revolute->add(COLLADA_ELEMENT_LIMITS));
{
daeSafeCast<domMinmax>(limits->add(COLLADA_ELEMENT_MIN))->getValue() = angles::to_degrees(urdf_joint->limits->lower);
daeSafeCast<domMinmax>(limits->add(COLLADA_ELEMENT_MAX))->getValue() = angles::to_degrees(urdf_joint->limits->upper);
}
// </limits>
}
// </revolute>
break;
}
case urdf::Joint::CONTINUOUS: {
// Model as a REVOLUTE joint without limits
// <revolute sid="axis0">
domAxis_constraintRef revolute = daeSafeCast<domAxis_constraint>(joint->add(COLLADA_ELEMENT_REVOLUTE));
revolute->setSid("axis0");
{
// <axis>
domAxisRef axis = daeSafeCast<domAxis>(revolute->add(COLLADA_ELEMENT_AXIS));
{
axis->getValue().setCount(3);
axis->getValue()[0] = axis_x;
axis->getValue()[1] = axis_y;
axis->getValue()[2] = axis_z;
}
// </axis>
}
// </revolute>
break;
}
case urdf::Joint::PRISMATIC: {
// <prismatic sid="axis0">
domAxis_constraintRef prismatic = daeSafeCast<domAxis_constraint>(joint->add(COLLADA_ELEMENT_PRISMATIC));
prismatic->setSid("axis0");
{
// <axis>
domAxisRef axis = daeSafeCast<domAxis>(prismatic->add(COLLADA_ELEMENT_AXIS));
{
axis->getValue().setCount(3);
axis->getValue()[0] = axis_x;
axis->getValue()[1] = axis_y;
axis->getValue()[2] = axis_z;
}
// </axis>
// <limits>
domJoint_limitsRef limits = daeSafeCast<domJoint_limits>(prismatic->add(COLLADA_ELEMENT_LIMITS));
{
daeSafeCast<domMinmax>(limits->add(COLLADA_ELEMENT_MIN))->getValue() = urdf_joint->limits->lower;
daeSafeCast<domMinmax>(limits->add(COLLADA_ELEMENT_MAX))->getValue() = urdf_joint->limits->upper;
}
// </limits>
}
// </prismatic>
break;
}
case urdf::Joint::FIXED: {
// Model as a REVOLUTE joint with no leeway
domAxis_constraintRef revolute = daeSafeCast<domAxis_constraint>(joint->add(COLLADA_ELEMENT_REVOLUTE));
revolute->setSid("axis0");
{
// <axis>
domAxisRef axis = daeSafeCast<domAxis>(revolute->add(COLLADA_ELEMENT_AXIS));
{
axis->getValue().setCount(3);
axis->getValue()[0] = axis_x;
axis->getValue()[1] = axis_y;
axis->getValue()[2] = axis_z;
}
// </axis>
// <limits>
domJoint_limitsRef limits = daeSafeCast<domJoint_limits>(revolute->add(COLLADA_ELEMENT_LIMITS));
{
daeSafeCast<domMinmax>(limits->add(COLLADA_ELEMENT_MIN))->getValue() = 0.0;
daeSafeCast<domMinmax>(limits->add(COLLADA_ELEMENT_MAX))->getValue() = 0.0;
}
// </limits>
}
// </revolute>
break;
}
case urdf::Joint::UNKNOWN: {
std::cerr << "Joint type UNKNOWN of joint " << urdf_joint->name << " is unsupported" << std::endl;
break;
}
case urdf::Joint::FLOATING: {
std::cerr << "Joint type FLOATING of joint " << urdf_joint->name << " is unsupported" << std::endl;
break;
}
case urdf::Joint::PLANAR: {
std::cerr << "Joint type PLANAR of joint " << urdf_joint->name << " is unsupported" << std::endl;
break;
}
default: {
std::cerr << "Joint type " << urdf_joint->type << " of joint " << urdf_joint->name << " is unsupported" << std::endl;
break;
}
}
}
}
void ColladaWriter::addBindings(SCENE const& scene) {
string model_id = kmodel_->getID();
string inst_model_sid = string("inst_") + model_id;
// <bind_kinematics_scene>
// <bind_kinematics_model node="node0">
domBind_kinematics_modelRef kmodel_bind = daeSafeCast<domBind_kinematics_model>(scene.kiscene->add(COLLADA_ELEMENT_BIND_KINEMATICS_MODEL));
kmodel_bind->setNode("v1.node0"); // @todo
daeSafeCast<domCommon_param>(kmodel_bind->add(COLLADA_ELEMENT_PARAM))->setValue(string(string(scene.kscene->getID()) + string(".") + inst_model_sid).c_str());
for (map<string, shared_ptr<urdf::Joint> >::const_iterator i = robot_.joints_.begin(); i != robot_.joints_.end(); i++) {
shared_ptr<urdf::Joint> urdf_joint = i->second;
int idof = 0; // @todo assuming 1 dof joints
string joint_sid = joint_sids_[urdf_joint->name];
string axis_name = string("axis") + boost::lexical_cast<string>(idof);
string joint_axis_sid = string("kscene.inst_") + model_id + string(".") + joint_sid + string(".") + axis_name;
string joint_axis_value_sid = joint_axis_sid + string("_value");
// <bind_joint_axis target="node0/joint_1_axis0">
domBind_joint_axisRef joint_bind = daeSafeCast<domBind_joint_axis>(scene.kiscene->add(COLLADA_ELEMENT_BIND_JOINT_AXIS));
string node_name = node_ids_[urdf_joint->name];
joint_bind->setTarget((node_name + string("/node_") + joint_sid + string("_") + axis_name).c_str());
{
// <axis>
domCommon_sidref_or_paramRef axis_bind = daeSafeCast<domCommon_sidref_or_param>(joint_bind->add(COLLADA_ELEMENT_AXIS));
{
daeSafeCast<domCommon_param>(axis_bind->add(COLLADA_ELEMENT_PARAM))->setValue(joint_axis_sid.c_str());
}
// </axis>
// <value>
domCommon_float_or_paramRef value_bind = daeSafeCast<domCommon_float_or_param>(joint_bind->add(COLLADA_ELEMENT_VALUE));
{
daeSafeCast<domCommon_param>(value_bind->add(COLLADA_ELEMENT_PARAM))->setValue(joint_axis_value_sid.c_str());
}
}
// </bind_joint_axis>
}
}
void ColladaWriter::addKinematics(SCENE const& scene) {
// <kinematics_model id="k1" name="pr2">
domKinematics_modelRef kmodel = daeSafeCast<domKinematics_model>(kinematicsModelsLib_->add(COLLADA_ELEMENT_KINEMATICS_MODEL));
kmodel->setId("k1");
kmodel->setName(robot_.getName().c_str());
{
// <technique_common>
domKinematics_model_techniqueRef technique = daeSafeCast<domKinematics_model_technique>(kmodel->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
addJoints(technique);
// </technique_common>
// <link ...>
int link_num = 0;
addKinematicLink(robot_.getRoot(), technique, link_num);
// </link>
// add a formula for each mimic joint
for (map<string, shared_ptr<urdf::Joint> >::const_iterator i = robot_.joints_.begin(); i != robot_.joints_.end(); i++) {
shared_ptr<urdf::Joint> urdf_joint = i->second;
if( !!urdf_joint->mimic ) {
string joint_sid = string("k1/")+joint_sids_[urdf_joint->name];
string joint_mimic_sid = string("k1/")+joint_sids_[urdf_joint->mimic->joint_name];
// <formula>
addMimicJoint(daeSafeCast<domFormula>(technique->add(COLLADA_ELEMENT_FORMULA)), joint_sid,joint_mimic_sid,urdf_joint->mimic->multiplier,urdf_joint->mimic->offset);
// </formula>
}
}
}
kmodel_ = kmodel;
// </kinematics_model>
string model_id = kmodel->getID();
string inst_model_sid = string("inst_") + model_id;
// <instance_kinematics_model url="#k1" sid="inst_k1">
domInstance_kinematics_modelRef ikm = daeSafeCast<domInstance_kinematics_model>(scene.kscene->add(COLLADA_ELEMENT_INSTANCE_KINEMATICS_MODEL));
ikm->setUrl((string("#") + model_id).c_str());
ikm->setSid(inst_model_sid.c_str());
{
// <newparam sid="kscene.inst_k1">
domKinematics_newparamRef newparam_model = daeSafeCast<domKinematics_newparam>(ikm->add(COLLADA_ELEMENT_NEWPARAM));
string newparam_model_sid = string("kscene.inst_") + model_id;
newparam_model->setSid(newparam_model_sid.c_str());
{
// <SIDREF>kscene/inst_k1</SIDREF>
string model_sidref = string("kscene/inst_") + model_id;
daeSafeCast<domKinematics_newparam::domSIDREF>(newparam_model->add(COLLADA_ELEMENT_SIDREF))->setValue(model_sidref.c_str());
}
// </newparam>
for (map<string, shared_ptr<urdf::Joint> >::const_iterator i = robot_.joints_.begin(); i != robot_.joints_.end(); i++) {
shared_ptr<urdf::Joint> urdf_joint = i->second;
int idof = 0; // @todo assuming 1 dof joints
string joint_sid = joint_sids_[urdf_joint->name];
string axis_name = string("axis") + boost::lexical_cast<string>(idof);
// <newparam sid="kscene.inst_k1.joint0.axis0">
domKinematics_newparamRef newparam = daeSafeCast<domKinematics_newparam>(ikm->add(COLLADA_ELEMENT_NEWPARAM));
string newparam_sid = string("kscene.inst_") + model_id + string(".") + joint_sid + string(".") + axis_name;
newparam->setSid(newparam_sid.c_str());
{
// <SIDREF>kscene/inst_k1/joint0/axis0</SIDREF>
string sidref = string("kscene/inst_") + model_id + string("/") + joint_sid + string("/") + axis_name;
daeSafeCast<domKinematics_newparam::domSIDREF>(newparam->add(COLLADA_ELEMENT_SIDREF))->setValue(sidref.c_str());
}
// </newparam>
// <newparam sid="kscene.inst_k1.joint0.axis0_value">
domKinematics_newparamRef newparam_value = daeSafeCast<domKinematics_newparam>(ikm->add(COLLADA_ELEMENT_NEWPARAM));
string newparam_value_sid = string("kscene.inst_") + model_id + string(".") + joint_sid + string(".") + axis_name + string("_value");
newparam_value->setSid(newparam_value_sid.c_str());
{
// <float>0</float>
daeSafeCast<domKinematics_newparam::domFloat>(newparam_value->add(COLLADA_ELEMENT_FLOAT))->setValue(0.0f);
}
// </newparam>
}
}
// </instance_kinematics_model>
}
void ColladaWriter::addKinematicLink(shared_ptr<const urdf::Link> urdf_link, daeElementRef parent, int& link_num) {
// <link sid="link0" name="base_link">
domLinkRef link = daeSafeCast<domLink>(parent->add(COLLADA_ELEMENT_LINK));
string link_sid = string("link") + boost::lexical_cast<string>(link_num);
link->setName(urdf_link->name.c_str());
link->setSid(link_sid.c_str());
link_num++;
foreach(shared_ptr<urdf::Joint> urdf_joint, urdf_link->child_joints) {
// <attachment_full joint="k1/joint0">
domLink::domAttachment_fullRef attachment_full = daeSafeCast<domLink::domAttachment_full>(link->add(COLLADA_ELEMENT_ATTACHMENT_FULL));
string attachment_joint = string("k1/") + joint_sids_[urdf_joint->name];
attachment_full->setJoint(attachment_joint.c_str());
{
// <translate>x y z</translate>
addTranslate(attachment_full, urdf_joint->parent_to_joint_origin_transform.position, NULL, true);
// <rotate>x y z w</rotate>
addRotate(attachment_full, urdf_joint->parent_to_joint_origin_transform.rotation, NULL, true);
addKinematicLink(robot_.getLink(urdf_joint->child_link_name), attachment_full, link_num);
}
// </attachment_full>
}
// </link>
}
void ColladaWriter::addVisuals(SCENE const& scene) {
// <node id="v1" name="pr2">
domNodeRef root_node = daeSafeCast<domNode>(scene.vscene->add(COLLADA_ELEMENT_NODE));
root_node->setId("v1");
root_node->setName(robot_.getName().c_str());
{
int link_num = 0;
addVisualLink(robot_.getRoot(), root_node, link_num);
}
}
void ColladaWriter::addMaterials() {
urdf::Color ambient, diffuse;
ambient.init("1 1 1 0");
diffuse.init("1 1 1 0");
for (map<string, shared_ptr<urdf::Link> >::const_iterator i = robot_.links_.begin(); i != robot_.links_.end(); i++) {
shared_ptr<urdf::Link> urdf_link = i->second;
map<string, string>::const_iterator j = geometry_ids_.find(urdf_link->name);
if (j == geometry_ids_.end())
continue;
if (!urdf_link->visual->material->texture_filename.empty()) {
ambient.r = ambient.g = ambient.b = 1; ambient.a = 0;
diffuse.r = diffuse.g = diffuse.b = 1; diffuse.a = 0;
}
else {
ambient.r = diffuse.r = urdf_link->visual->material->color.r;
ambient.g = diffuse.g = urdf_link->visual->material->color.g;
ambient.b = diffuse.b = urdf_link->visual->material->color.b;
ambient.a = diffuse.a = urdf_link->visual->material->color.a;
}
string geometry_id = j->second;
domEffectRef effect = addEffect(geometry_id, ambient, diffuse);
// <material id="g1.link0.geom0.eff">
domMaterialRef material = daeSafeCast<domMaterial>(materialsLib_->add(COLLADA_ELEMENT_MATERIAL));
string material_id = geometry_id + string(".mat");
material->setId(material_id.c_str());
{
// <instance_effect url="#g1.link0.geom0.eff"/>
domInstance_effectRef instance_effect = daeSafeCast<domInstance_effect>(material->add(COLLADA_ELEMENT_INSTANCE_EFFECT));
string effect_id(effect->getId());
instance_effect->setUrl((string("#") + effect_id).c_str());
}
// </material>
}
}
domEffectRef ColladaWriter::addEffect(string const& geometry_id, urdf::Color const& color_ambient, urdf::Color const& color_diffuse)
{
// <effect id="g1.link0.geom0.eff">
domEffectRef effect = daeSafeCast<domEffect>(effectsLib_->add(COLLADA_ELEMENT_EFFECT));
string effect_id = geometry_id + string(".eff");
effect->setId(effect_id.c_str());
{
// <profile_COMMON>
domProfile_commonRef profile = daeSafeCast<domProfile_common>(effect->add(COLLADA_ELEMENT_PROFILE_COMMON));
{
// <technique sid="">
domProfile_common::domTechniqueRef technique = daeSafeCast<domProfile_common::domTechnique>(profile->add(COLLADA_ELEMENT_TECHNIQUE));
{
// <phong>
domProfile_common::domTechnique::domPhongRef phong = daeSafeCast<domProfile_common::domTechnique::domPhong>(technique->add(COLLADA_ELEMENT_PHONG));
{
// <ambient>
domFx_common_color_or_textureRef ambient = daeSafeCast<domFx_common_color_or_texture>(phong->add(COLLADA_ELEMENT_AMBIENT));
{
// <color>r g b a
domFx_common_color_or_texture::domColorRef ambient_color = daeSafeCast<domFx_common_color_or_texture::domColor>(ambient->add(COLLADA_ELEMENT_COLOR));
ambient_color->getValue().setCount(4);
ambient_color->getValue()[0] = color_ambient.r;
ambient_color->getValue()[1] = color_ambient.g;
ambient_color->getValue()[2] = color_ambient.b;
ambient_color->getValue()[3] = color_ambient.a;
// </color>
}
// </ambient>
// <diffuse>
domFx_common_color_or_textureRef diffuse = daeSafeCast<domFx_common_color_or_texture>(phong->add(COLLADA_ELEMENT_DIFFUSE));
{
// <color>r g b a
domFx_common_color_or_texture::domColorRef diffuse_color = daeSafeCast<domFx_common_color_or_texture::domColor>(diffuse->add(COLLADA_ELEMENT_COLOR));
diffuse_color->getValue().setCount(4);
diffuse_color->getValue()[0] = color_diffuse.r;
diffuse_color->getValue()[1] = color_diffuse.g;
diffuse_color->getValue()[2] = color_diffuse.b;
diffuse_color->getValue()[3] = color_diffuse.a;
// </color>
}
// </diffuse>
}
// </phong>
}
// </technique>
}
// </profile_COMMON>
}
// </effect>
return effect;
}
void ColladaWriter::addVisualLink(shared_ptr<urdf::Link const> urdf_link, daeElementRef parent, int& link_num) {
// <node id="v1.node0" name="base_link" sid="node0">
domNodeRef node = daeSafeCast<domNode>(parent->add(COLLADA_ELEMENT_NODE));
string node_sid = string("node") + boost::lexical_cast<string>(link_num);
string node_id = string("v1.") + node_sid;
node->setName(urdf_link->name.c_str());
node->setSid(node_sid.c_str());
node->setId(node_id.c_str());
link_num++;
domNodeRef parent_node = node;
{
if (!!urdf_link->parent_joint) {
// <translate>x y z</translate>
addTranslate(node, urdf_link->parent_joint->parent_to_joint_origin_transform.position, NULL, true);
// <rotate>x y z w</rotate>
addRotate(node, urdf_link->parent_joint->parent_to_joint_origin_transform.rotation, NULL, true);
domRotateRef joint_rotate;
// <rotate sid="node_joint0_axis0">x y z angle</rotate>
//joint_rotate = addRotate(parent_node, urdf_link->parent_joint->parent_to_joint_origin_transform.rotation);
joint_rotate = daeSafeCast<domRotate>(parent_node->add(COLLADA_ELEMENT_ROTATE));
joint_rotate->getValue().setCount(4);
joint_rotate->getValue()[0] = urdf_link->parent_joint->axis.x;
joint_rotate->getValue()[1] = urdf_link->parent_joint->axis.y;
joint_rotate->getValue()[2] = urdf_link->parent_joint->axis.z;
joint_rotate->getValue()[3] = 0;
string joint_sid = joint_sids_[urdf_link->parent_joint->name];
string joint_rotate_sid = string("node_") + joint_sid + string("_axis0");
joint_rotate->setSid(joint_rotate_sid.c_str());
node_ids_[urdf_link->parent_joint->name] = node_id;
}
if (!!urdf_link->visual) {
// <node id="v1.node0.visual" name="visual" sid="visual">
domNodeRef visual_node = daeSafeCast<domNode>(node->add(COLLADA_ELEMENT_NODE));
string visual_sid("visual");
string visual_id = node_id + "." + visual_sid;
visual_node->setName("visual");
visual_node->setSid(visual_sid.c_str());
visual_node->setId(visual_id.c_str());
parent_node = visual_node;
// <translate>x y z</translate>
addTranslate(parent_node, urdf_link->visual->origin.position, NULL, true);
// <rotate>x y z w</rotate>
addRotate(parent_node, urdf_link->visual->origin.rotation, NULL, true);
}
// <instance_geometry url="#g1.link0.geom">
map<string, string>::const_iterator i = geometry_ids_.find(urdf_link->name);
if (i != geometry_ids_.end()) {
domInstance_geometryRef instance_geometry = daeSafeCast<domInstance_geometry>(parent_node->add(COLLADA_ELEMENT_INSTANCE_GEOMETRY));
string geometry_id = i->second;
string instance_geometry_url = string("#") + geometry_id;
instance_geometry->setUrl(instance_geometry_url.c_str());
{
// <bind_material>
domBind_materialRef bind_material = daeSafeCast<domBind_material>(instance_geometry->add(COLLADA_ELEMENT_BIND_MATERIAL));
{
// <technique_common>
domBind_material::domTechnique_commonRef technique_common = daeSafeCast<domBind_material::domTechnique_common>(bind_material->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
{
// <instance_material>
domInstance_materialRef instance_material = daeSafeCast<domInstance_material>(technique_common->add(COLLADA_ELEMENT_INSTANCE_MATERIAL));
instance_material->setTarget((instance_geometry_url + string(".mat")).c_str());
instance_material->setSymbol("mat0");
// </instance_material>
}
// </technique_common>
}
// </bind_material>
}
}
// </instance_geometry>
// <node ...>
foreach(shared_ptr<urdf::Link> link2, urdf_link->child_links)
addVisualLink(link2, node, link_num);
// </node>
}
// </node>
}
domTranslateRef ColladaWriter::addTranslate(daeElementRef parent, urdf::Vector3 const& position, daeElementRef before, bool ignore_zero_translations) {
if (ignore_zero_translations && position.x == 0.0 && position.y == 0.0 && position.z == 0.0)
return NULL;
// <translate>x y z</translate>
domTranslateRef trans;
if (before) {
trans = daeSafeCast<domTranslate>(parent->createElement(COLLADA_ELEMENT_TRANSLATE));
parent->addBefore(trans, before);
}
else
trans = daeSafeCast<domTranslate>(parent->add(COLLADA_ELEMENT_TRANSLATE));
trans->getValue().setCount(3);
trans->getValue()[0] = position.x;
trans->getValue()[1] = position.y;
trans->getValue()[2] = position.z;
return trans;
}
domRotateRef ColladaWriter::addRotate(daeElementRef parent, urdf::Rotation const& r, daeElementRef before, bool ignore_zero_rotations) {
double ax, ay, az, aa;
// Convert from quaternion to axis-angle
double sqr_len = r.x * r.x + r.y * r.y + r.z * r.z;
if (sqr_len > 0) {
aa = 2 * acos(r.w);
double inv_len = 1.0 / sqrt(sqr_len);
ax = r.x * inv_len;
ay = r.y * inv_len;
az = r.z * inv_len;
}
else {
if (ignore_zero_rotations)
return NULL;
// Angle is 0 (mod 2*pi), so no need to add rotate node
aa = 0.0;
ax = 1.0;
ay = 0.0;
az = 0.0;
}
// <rotate>x y z w</rotate>
domRotateRef rot;
if (before) {
rot = daeSafeCast<domRotate>(parent->createElement(COLLADA_ELEMENT_ROTATE));
parent->addBefore(rot, before);
}
else
rot = daeSafeCast<domRotate>(parent->add(COLLADA_ELEMENT_ROTATE));
rot->getValue().setCount(4);
rot->getValue()[0] = ax;
rot->getValue()[1] = ay;
rot->getValue()[2] = az;
rot->getValue()[3] = angles::to_degrees(aa);
return rot;
}
void ColladaWriter::addMimicJoint(domFormulaRef formula, const std::string& joint_sid,const std::string& joint_mimic_sid, double multiplier, double offset)
{
string sid = joint_sid+string(".formula");
formula->setSid(sid.c_str());
domCommon_float_or_paramRef ptarget = daeSafeCast<domCommon_float_or_param>(formula->createAndPlace(COLLADA_ELEMENT_TARGET));
daeSafeCast<domCommon_param>(ptarget->createAndPlace(COLLADA_TYPE_PARAM))->setValue(joint_sid.c_str());
domFormula_techniqueRef pftec = daeSafeCast<domFormula_technique>(formula->createAndPlace(COLLADA_ELEMENT_TECHNIQUE_COMMON));
// <apply> <plus/> <apply> <times/> <cn>a</cn> x </apply> <cn>b</cn> </apply>
daeElementRef pmath_math = pftec->createAndPlace("math");
daeElementRef pmath_apply = pmath_math->createAndPlace("apply");
{
daeElementRef pmath_plus = pmath_apply->createAndPlace("plus");
daeElementRef pmath_apply1 = pmath_apply->createAndPlace("apply");
{
daeElementRef pmath_times = pmath_apply1->createAndPlace("times");
daeElementRef pmath_const0 = pmath_apply1->createAndPlace("cn");
pmath_const0->setCharData(boost::lexical_cast<string>(multiplier));
daeElementRef pmath_symb = pmath_apply1->createAndPlace("csymbol");
pmath_symb->setAttribute("encoding","COLLADA");
pmath_symb->setCharData(joint_mimic_sid);
}
daeElementRef pmath_const1 = pmath_apply->createAndPlace("cn");
pmath_const1->setCharData(boost::lexical_cast<string>(offset));
}
}
string ColladaWriter::getTimeStampString() const {
// facet becomes owned by locale, so no need to explicitly delete
boost::posix_time::time_facet* facet = new boost::posix_time::time_facet("%Y-%m-%dT%H:%M:%s");
std::stringstream ss(std::stringstream::in | std::stringstream::out);
ss.imbue(std::locale(ss.getloc(), facet));
ss << boost::posix_time::second_clock::local_time();
string date;
ss >> date;
return date;
}
}

153
collada_urdf/src/stl_loader.cpp
View File

@ -1,153 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redstributions 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.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* 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 OWNER 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.
*********************************************************************/
/* Author: Tim Field */
#include "collada_urdf/stl_loader.h"
#include <ctype.h>
#include <stdio.h>
#include <string.h>
#include <iostream>
using std::string;
using boost::shared_ptr;
namespace collada_urdf {
// Vector3
Vector3::Vector3(float _x, float _y, float _z) : x(_x), y(_y), z(_z) { }
bool Vector3::operator==(Vector3 const& v) const {
return x == v.x && y == v.y && z == v.z;
}
// Mesh
Mesh::Mesh() {
}
int Mesh::getVertexIndex(const Vector3& v) const {
for (unsigned int i = 0; i < vertices.size(); i++)
if (vertices[i] == v)
return i;
return -1;
}
void Mesh::addVertex(Vector3 const& v) { vertices.push_back(v); }
void Mesh::addNormal(Vector3 const& n) { normals.push_back(n); }
void Mesh::addIndex(unsigned int i) { indices.push_back(i); }
// STLLoader
shared_ptr<Mesh> STLLoader::load(std::string const& filename) {
file_ = fopen(filename.c_str(), "r");
if (file_ == NULL)
return shared_ptr<Mesh>();
mesh_ = shared_ptr<Mesh>(new Mesh);
readBinary();
fclose(file_);
file_ = NULL;
return mesh_;
}
void STLLoader::readBinary() {
// 80 byte Header
for (int i = 0; i < 80; i++)
fgetc(file_);
int face_num = readLongInt();
for (int iface = 0; iface < face_num; iface++) {
float nx = readFloat();
float ny = readFloat();
float nz = readFloat();
Vector3 normal(nx, ny, nz);
for (int i = 0; i < 3; i++) {
float vx = readFloat();
float vy = readFloat();
float vz = readFloat();
Vector3 vertex(vx, vy, vz);
int index = mesh_->getVertexIndex(vertex);
if (index == -1) {
mesh_->addVertex(vertex);
mesh_->addNormal(normal);
index = mesh_->vertices.size() - 1;
}
mesh_->addIndex(index);
}
readShortInt(); // 2 byte attribute
}
}
float STLLoader::readFloat() {
float rval;
if (fread(&rval, sizeof(float), 1, file_) == 0)
std::cerr << "Error in STLLoader::readFloat" << std::endl;
return rval;
}
uint32_t STLLoader::readLongInt() {
union
{
uint32_t yint;
char ychar[4];
} y;
y.ychar[0] = fgetc(file_);
y.ychar[1] = fgetc(file_);
y.ychar[2] = fgetc(file_);
y.ychar[3] = fgetc(file_);
return y.yint;
}
uint16_t STLLoader::readShortInt() {
uint8_t c1 = fgetc(file_);
uint8_t c2 = fgetc(file_);
uint16_t ival = c1 | (c2 << 8);
return ival;
}
}

7
collada_urdf/src/urdf_to_collada.cpp
View File

@ -48,8 +48,13 @@ int main(int argc, char** argv)
std::string input_filename(argv[1]); std::string input_filename(argv[1]);
std::string output_filename(argv[2]); std::string output_filename(argv[2]);
urdf::Model robot_model;
if( !robot_model.initFile(input_filename) ) {
ROS_ERROR("failed to open urdf file %s",input_filename.c_str());
}
boost::shared_ptr<DAE> dom; boost::shared_ptr<DAE> dom;
if (!collada_urdf::colladaFromUrdfFile(input_filename, dom)) { if (!collada_urdf::colladaFromUrdfModel(robot_model, dom)) {
std::cerr << std::endl << "Error converting document" << std::endl; std::cerr << std::endl << "Error converting document" << std::endl;
return -1; return -1;
} }

1
urdf/manifest.xml
View File

@ -13,6 +13,7 @@
<depend package="tinyxml" /> <depend package="tinyxml" />
<depend package="roscpp" /> <depend package="roscpp" />
<depend package="colladadom" /> <depend package="colladadom" />
<export> <export>
<cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp -I${prefix}/srv/cpp" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lurdf"/> <cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp -I${prefix}/srv/cpp" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lurdf"/>
</export> </export>