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

1821 lines
80 KiB
C++
Raw Blame History

// -*- coding: utf-8 -*-
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc., University of Tokyo
* 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.
*********************************************************************/
/* Authors: Rosen Diankov, Tim Field */
#include "collada_urdf/collada_urdf.h"
#include <map>
#include <vector>
#include <list>
#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_interface/pose.h>
#include <angles/angles.h>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/date_time/posix_time/posix_time_io.hpp>
#include <boost/format.hpp>
#include <assimp/assimp.hpp>
#include <assimp/aiScene.h>
#include <assimp/aiPostProcess.h>
#include <assimp/IOStream.h>
#include <assimp/IOSystem.h>
#define FOREACH(it, v) for(typeof((v).begin())it = (v).begin(); it != (v).end(); (it)++)
#define FOREACHC FOREACH
using namespace std;
namespace collada_urdf {
/// ResourceIOStream is copied from rviz (BSD, Willow Garage)
class ResourceIOStream : public Assimp::IOStream
{
public:
ResourceIOStream(const resource_retriever::MemoryResource& res)
: res_(res)
, pos_(res.data.get())
{
}
~ResourceIOStream()
{
}
size_t Read(void* buffer, size_t size, size_t count)
{
size_t to_read = size * count;
if (pos_ + to_read > res_.data.get() + res_.size)
{
to_read = res_.size - (pos_ - res_.data.get());
}
memcpy(buffer, pos_, to_read);
pos_ += to_read;
return to_read;
}
size_t Write( const void* buffer, size_t size, size_t count) {
ROS_BREAK(); return 0;
}
aiReturn Seek( size_t offset, aiOrigin origin)
{
uint8_t* new_pos = 0;
switch (origin)
{
case aiOrigin_SET:
new_pos = res_.data.get() + offset;
break;
case aiOrigin_CUR:
new_pos = pos_ + offset; // TODO is this right? can offset really not be negative
break;
case aiOrigin_END:
new_pos = res_.data.get() + res_.size - offset; // TODO is this right?
break;
default:
ROS_BREAK();
}
if (new_pos < res_.data.get() || new_pos > res_.data.get() + res_.size)
{
return aiReturn_FAILURE;
}
pos_ = new_pos;
return aiReturn_SUCCESS;
}
size_t Tell() const
{
return pos_ - res_.data.get();
}
size_t FileSize() const
{
return res_.size;
}
void Flush() {
}
private:
resource_retriever::MemoryResource res_;
uint8_t* pos_;
};
namespace mathextra {
// code from MagicSoftware by Dave Eberly
const double g_fEpsilon = 1e-15;
//===========================================================================
#define distinctRoots 0 // roots r0 < r1 < r2
#define singleRoot 1 // root r0
#define floatRoot01 2 // roots r0 = r1 < r2
#define floatRoot12 4 // roots r0 < r1 = r2
#define tripleRoot 6 // roots r0 = r1 = r2
template <typename T, typename S>
void Tridiagonal3 (S* mat, T* diag, T* subd)
{
T a, b, c, d, e, f, ell, q;
a = mat[0*3+0];
b = mat[0*3+1];
c = mat[0*3+2];
d = mat[1*3+1];
e = mat[1*3+2];
f = mat[2*3+2];
subd[2] = 0.0;
diag[0] = a;
if ( fabs(c) >= g_fEpsilon ) {
ell = (T)sqrt(b*b+c*c);
b /= ell;
c /= ell;
q = 2*b*e+c*(f-d);
diag[1] = d+c*q;
diag[2] = f-c*q;
subd[0] = ell;
subd[1] = e-b*q;
mat[0*3+0] = (S)1; mat[0*3+1] = (S)0; mat[0*3+2] = (T)0;
mat[1*3+0] = (S)0; mat[1*3+1] = b; mat[1*3+2] = c;
mat[2*3+0] = (S)0; mat[2*3+1] = c; mat[2*3+2] = -b;
}
else {
diag[1] = d;
diag[2] = f;
subd[0] = b;
subd[1] = e;
mat[0*3+0] = (S)1; mat[0*3+1] = (S)0; mat[0*3+2] = (S)0;
mat[1*3+0] = (S)0; mat[1*3+1] = (S)1; mat[1*3+2] = (S)0;
mat[2*3+0] = (S)0; mat[2*3+1] = (S)0; mat[2*3+2] = (S)1;
}
}
int CubicRoots (double c0, double c1, double c2, double *r0, double *r1, double *r2)
{
// polynomial is L^3-c2*L^2+c1*L-c0
int maxiter = 50;
double discr, temp, pval, pdval, b0, b1;
int i;
// find local extrema (if any) of p'(L) = 3*L^2-2*c2*L+c1
discr = c2*c2-3*c1;
if ( discr >= 0.0 ) {
discr = (double)sqrt(discr);
temp = (c2+discr)/3;
pval = temp*(temp*(temp-c2)+c1)-c0;
if ( pval >= 0.0 ) {
// double root occurs before the positive local maximum
(*r0) = (c2-discr)/3 - 1; // initial guess for Newton's methods
pval = 2*g_fEpsilon;
for (i = 0; i < maxiter && fabs(pval) > g_fEpsilon; i++) {
pval = (*r0)*((*r0)*((*r0)-c2)+c1)-c0;
pdval = (*r0)*(3*(*r0)-2*c2)+c1;
(*r0) -= pval/pdval;
}
// Other two roots are solutions to quadratic equation
// L^2 + ((*r0)-c2)*L + [(*r0)*((*r0)-c2)+c1] = 0.
b1 = (*r0)-c2;
b0 = (*r0)*((*r0)-c2)+c1;
discr = b1*b1-4*b0;
if ( discr < -g_fEpsilon )
{
// single root r0
return singleRoot;
}
else
{
int result = distinctRoots;
// roots r0 <= r1 <= r2
discr = sqrt(fabs(discr));
(*r1) = 0.5f*(-b1-discr);
(*r2) = 0.5f*(-b1+discr);
if ( fabs((*r0)-(*r1)) <= g_fEpsilon )
{
(*r0) = (*r1);
result |= floatRoot01;
}
if ( fabs((*r1)-(*r2)) <= g_fEpsilon )
{
(*r1) = (*r2);
result |= floatRoot12;
}
return result;
}
}
else {
// double root occurs after the negative local minimum
(*r2) = temp + 1; // initial guess for Newton's method
pval = 2*g_fEpsilon;
for (i = 0; i < maxiter && fabs(pval) > g_fEpsilon; i++) {
pval = (*r2)*((*r2)*((*r2)-c2)+c1)-c0;
pdval = (*r2)*(3*(*r2)-2*c2)+c1;
(*r2) -= pval/pdval;
}
// Other two roots are solutions to quadratic equation
// L^2 + (r2-c2)*L + [r2*(r2-c2)+c1] = 0.
b1 = (*r2)-c2;
b0 = (*r2)*((*r2)-c2)+c1;
discr = b1*b1-4*b0;
if ( discr < -g_fEpsilon )
{
// single root
(*r0) = (*r2);
return singleRoot;
}
else
{
int result = distinctRoots;
// roots r0 <= r1 <= r2
discr = sqrt(fabs(discr));
(*r0) = 0.5f*(-b1-discr);
(*r1) = 0.5f*(-b1+discr);
if ( fabs((*r0)-(*r1)) <= g_fEpsilon )
{
(*r0) = (*r1);
result |= floatRoot01;
}
if ( fabs((*r1)-(*r2)) <= g_fEpsilon )
{
(*r1) = (*r2);
result |= floatRoot12;
}
return result;
}
}
}
else {
// p(L) has one double root
(*r0) = c0;
pval = 2*g_fEpsilon;
for (i = 0; i < maxiter && fabs(pval) > g_fEpsilon; i++) {
pval = (*r0)*((*r0)*((*r0)-c2)+c1)-c0;
pdval = (*r0)*(3*(*r0)-2*c2)+c1;
(*r0) -= pval/pdval;
}
return singleRoot;
}
}
//----------------------------------------------------------------------------
template <class T>
bool _QLAlgorithm3 (T* m_aafEntry, T* afDiag, T* afSubDiag)
{
// QL iteration with implicit shifting to reduce matrix from tridiagonal
// to diagonal
for (int i0 = 0; i0 < 3; i0++)
{
const int iMaxIter = 32;
int iIter;
for (iIter = 0; iIter < iMaxIter; iIter++)
{
int i1;
for (i1 = i0; i1 <= 1; i1++)
{
T fSum = fabs(afDiag[i1]) +
fabs(afDiag[i1+1]);
if ( fabs(afSubDiag[i1]) + fSum == fSum )
break;
}
if ( i1 == i0 )
break;
T fTmp0 = (afDiag[i0+1]-afDiag[i0])/(2.0f*afSubDiag[i0]);
T fTmp1 = sqrt(fTmp0*fTmp0+1.0f);
if ( fTmp0 < 0.0f )
fTmp0 = afDiag[i1]-afDiag[i0]+afSubDiag[i0]/(fTmp0-fTmp1);
else
fTmp0 = afDiag[i1]-afDiag[i0]+afSubDiag[i0]/(fTmp0+fTmp1);
T fSin = 1.0f;
T fCos = 1.0f;
T fTmp2 = 0.0f;
for (int i2 = i1-1; i2 >= i0; i2--)
{
T fTmp3 = fSin*afSubDiag[i2];
T fTmp4 = fCos*afSubDiag[i2];
if ( fabs(fTmp3) >= fabs(fTmp0) )
{
fCos = fTmp0/fTmp3;
fTmp1 = sqrt(fCos*fCos+1.0f);
afSubDiag[i2+1] = fTmp3*fTmp1;
fSin = 1.0f/fTmp1;
fCos *= fSin;
}
else
{
fSin = fTmp3/fTmp0;
fTmp1 = sqrt(fSin*fSin+1.0f);
afSubDiag[i2+1] = fTmp0*fTmp1;
fCos = 1.0f/fTmp1;
fSin *= fCos;
}
fTmp0 = afDiag[i2+1]-fTmp2;
fTmp1 = (afDiag[i2]-fTmp0)*fSin+2.0f*fTmp4*fCos;
fTmp2 = fSin*fTmp1;
afDiag[i2+1] = fTmp0+fTmp2;
fTmp0 = fCos*fTmp1-fTmp4;
for (int iRow = 0; iRow < 3; iRow++)
{
fTmp3 = m_aafEntry[iRow*3+i2+1];
m_aafEntry[iRow*3+i2+1] = fSin*m_aafEntry[iRow*3+i2] +
fCos*fTmp3;
m_aafEntry[iRow*3+i2] = fCos*m_aafEntry[iRow*3+i2] -
fSin*fTmp3;
}
}
afDiag[i0] -= fTmp2;
afSubDiag[i0] = fTmp0;
afSubDiag[i1] = 0.0f;
}
if ( iIter == iMaxIter )
{
// should not get here under normal circumstances
return false;
}
}
return true;
}
bool QLAlgorithm3 (float* m_aafEntry, float* afDiag, float* afSubDiag)
{
return _QLAlgorithm3<float>(m_aafEntry, afDiag, afSubDiag);
}
bool QLAlgorithm3 (double* m_aafEntry, double* afDiag, double* afSubDiag)
{
return _QLAlgorithm3<double>(m_aafEntry, afDiag, afSubDiag);
}
void EigenSymmetric3(const double* fmat, double* afEigenvalue, double* fevecs)
{
double afSubDiag[3];
memcpy(fevecs, fmat, sizeof(double)*9);
Tridiagonal3(fevecs, afEigenvalue,afSubDiag);
QLAlgorithm3(fevecs, afEigenvalue,afSubDiag);
// make eigenvectors form a right--handed system
double fDet = fevecs[0*3+0] * (fevecs[1*3+1] * fevecs[2*3+2] - fevecs[1*3+2] * fevecs[2*3+1]) +
fevecs[0*3+1] * (fevecs[1*3+2] * fevecs[2*3+0] - fevecs[1*3+0] * fevecs[2*3+2]) +
fevecs[0*3+2] * (fevecs[1*3+0] * fevecs[2*3+1] - fevecs[1*3+1] * fevecs[2*3+0]);
if ( fDet < 0.0f )
{
fevecs[0*3+2] = -fevecs[0*3+2];
fevecs[1*3+2] = -fevecs[1*3+2];
fevecs[2*3+2] = -fevecs[2*3+2];
}
}
/* end of MAGIC code */
} // end namespace geometry
/// ResourceIOSystem is copied from rviz (BSD, Willow Garage)
class ResourceIOSystem : public Assimp::IOSystem
{
public:
ResourceIOSystem()
{
}
~ResourceIOSystem()
{
}
// Check whether a specific file exists
bool Exists(const char* file) const
{
// Ugly -- two retrievals where there should be one (Exists + Open)
// resource_retriever needs a way of checking for existence
// TODO: cache this
resource_retriever::MemoryResource res;
try {
res = retriever_.get(file);
}
catch (resource_retriever::Exception& e) {
return false;
}
return true;
}
// Get the path delimiter character we'd like to see
char getOsSeparator() const
{
return '/';
}
// ... and finally a method to open a custom stream
Assimp::IOStream* Open(const char* file, const char* mode)
{
ROS_ASSERT(mode == std::string("r") || mode == std::string("rb"));
// Ugly -- two retrievals where there should be one (Exists + Open)
// resource_retriever needs a way of checking for existence
resource_retriever::MemoryResource res;
try {
res = retriever_.get(file);
}
catch (resource_retriever::Exception& e) {
return 0;
}
return new ResourceIOStream(res);
}
void Close(Assimp::IOStream* stream) {
delete stream;
}
private:
mutable resource_retriever::Retriever retriever_;
};
/// \brief Implements writing urdf::Model objects to a COLLADA DOM.
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;
};
typedef std::map< boost::shared_ptr<const urdf::Link>, urdf::Pose > MAPLINKPOSES;
struct LINKOUTPUT
{
list<pair<int,string> > listusedlinks;
list<pair<int,string> > listprocesseddofs;
daeElementRef plink;
domNodeRef pnode;
MAPLINKPOSES _maplinkposes;
};
struct physics_model_output
{
domPhysics_modelRef pmodel;
std::vector<std::string > vrigidbodysids; ///< same ordering as the physics indices
};
struct kinematics_model_output
{
struct axis_output
{
//axis_output(const string& sid, KinBody::JointConstPtr pjoint, int iaxis) : sid(sid), pjoint(pjoint), iaxis(iaxis) {}
axis_output() : iaxis(0) {
}
string sid, nodesid;
boost::shared_ptr<const urdf::Joint> pjoint;
int iaxis;
string jointnodesid;
};
domKinematics_modelRef kmodel;
std::vector<axis_output> vaxissids;
std::vector<std::string > vlinksids;
MAPLINKPOSES _maplinkposes;
};
struct axis_sids
{
axis_sids(const string& axissid, const string& valuesid, const string& jointnodesid) : axissid(axissid), valuesid(valuesid), jointnodesid(jointnodesid) {
}
string axissid, valuesid, jointnodesid;
};
struct instance_kinematics_model_output
{
domInstance_kinematics_modelRef ikm;
std::vector<axis_sids> vaxissids;
boost::shared_ptr<kinematics_model_output> kmout;
std::vector<std::pair<std::string,std::string> > vkinematicsbindings;
};
struct instance_articulated_system_output
{
domInstance_articulated_systemRef ias;
std::vector<axis_sids> vaxissids;
std::vector<std::string > vlinksids;
std::vector<std::pair<std::string,std::string> > vkinematicsbindings;
};
struct instance_physics_model_output
{
domInstance_physics_modelRef ipm;
boost::shared_ptr<physics_model_output> pmout;
};
struct kinbody_models
{
std::string uri, kinematicsgeometryhash;
boost::shared_ptr<kinematics_model_output> kmout;
boost::shared_ptr<physics_model_output> pmout;
};
public:
ColladaWriter(const urdf::Model& robot, int writeoptions) : _writeoptions(writeoptions), _robot(robot), _dom(NULL) {
daeErrorHandler::setErrorHandler(this);
_collada.reset(new DAE);
_collada->setIOPlugin(NULL);
_collada->setDatabase(NULL);
_importer.SetIOHandler(new ResourceIOSystem());
}
virtual ~ColladaWriter() {
}
boost::shared_ptr<DAE> convert()
{
try {
const char* documentName = "urdf_snapshot";
daeDocument *doc = NULL;
daeInt error = _collada->getDatabase()->insertDocument(documentName, &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 the required asset tag
domAssetRef asset = daeSafeCast<domAsset>( _dom->add( COLLADA_ELEMENT_ASSET ) );
{
// 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;
ss.imbue(std::locale(ss.getloc(), facet));
ss << boost::posix_time::second_clock::local_time();
domAsset::domCreatedRef created = daeSafeCast<domAsset::domCreated>( asset->add( COLLADA_ELEMENT_CREATED ) );
created->setValue(ss.str().c_str());
domAsset::domModifiedRef modified = daeSafeCast<domAsset::domModified>( asset->add( COLLADA_ELEMENT_MODIFIED ) );
modified->setValue(ss.str().c_str());
domAsset::domContributorRef contrib = daeSafeCast<domAsset::domContributor>( asset->add( COLLADA_TYPE_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);
}
_globalscene = _dom->getScene();
if( !_globalscene ) {
_globalscene = 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");
_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");
_kinematicsModelsLib = daeSafeCast<domLibrary_kinematics_models>(_dom->add(COLLADA_ELEMENT_LIBRARY_KINEMATICS_MODELS));
_kinematicsModelsLib->setId("kmodels");
_articulatedSystemsLib = daeSafeCast<domLibrary_articulated_systems>(_dom->add(COLLADA_ELEMENT_LIBRARY_ARTICULATED_SYSTEMS));
_articulatedSystemsLib->setId("asystems");
_kinematicsScenesLib = daeSafeCast<domLibrary_kinematics_scenes>(_dom->add(COLLADA_ELEMENT_LIBRARY_KINEMATICS_SCENES));
_kinematicsScenesLib->setId("kscenes");
_physicsScenesLib = daeSafeCast<domLibrary_physics_scenes>(_dom->add(COLLADA_ELEMENT_LIBRARY_PHYSICS_SCENES));
_physicsScenesLib->setId("pscenes");
_physicsModelsLib = daeSafeCast<domLibrary_physics_models>(_dom->add(COLLADA_ELEMENT_LIBRARY_PHYSICS_MODELS));
_physicsModelsLib->setId("pmodels");
domExtraRef pextra_library_sensors = daeSafeCast<domExtra>(_dom->add(COLLADA_ELEMENT_EXTRA));
pextra_library_sensors->setId("sensors");
pextra_library_sensors->setType("library_sensors");
_sensorsLib = daeSafeCast<domTechnique>(pextra_library_sensors->add(COLLADA_ELEMENT_TECHNIQUE));
_sensorsLib->setProfile("OpenRAVE"); ///< documented profile on robot extensions
_CreateScene();
_WritePhysics();
_WriteRobot();
_WriteBindingsInstance_kinematics_scene();
return _collada;
}
catch (ColladaUrdfException ex) {
ROS_ERROR("Error converting: %s", ex.what());
return boost::shared_ptr<DAE>();
}
}
protected:
virtual void handleError(daeString msg) {
throw ColladaUrdfException(msg);
}
virtual void handleWarning(daeString msg) {
std::cerr << "COLLADA DOM warning: " << msg << std::endl;
}
void _CreateScene()
{
// Create visual scene
_scene.vscene = daeSafeCast<domVisual_scene>(_visualScenesLib->add(COLLADA_ELEMENT_VISUAL_SCENE));
_scene.vscene->setId("vscene");
_scene.vscene->setName("URDF Visual Scene");
// Create kinematics scene
_scene.kscene = daeSafeCast<domKinematics_scene>(_kinematicsScenesLib->add(COLLADA_ELEMENT_KINEMATICS_SCENE));
_scene.kscene->setId("kscene");
_scene.kscene->setName("URDF Kinematics Scene");
// Create physic scene
_scene.pscene = daeSafeCast<domPhysics_scene>(_physicsScenesLib->add(COLLADA_ELEMENT_PHYSICS_SCENE));
_scene.pscene->setId("pscene");
_scene.pscene->setName("URDF Physics Scene");
// Create instance visual scene
_scene.viscene = daeSafeCast<domInstance_with_extra>(_globalscene->add( COLLADA_ELEMENT_INSTANCE_VISUAL_SCENE ));
_scene.viscene->setUrl( (string("#") + string(_scene.vscene->getID())).c_str() );
// Create instance kinematics scene
_scene.kiscene = daeSafeCast<domInstance_kinematics_scene>(_globalscene->add( COLLADA_ELEMENT_INSTANCE_KINEMATICS_SCENE ));
_scene.kiscene->setUrl( (string("#") + string(_scene.kscene->getID())).c_str() );
// Create instance physics scene
_scene.piscene = daeSafeCast<domInstance_with_extra>(_globalscene->add( COLLADA_ELEMENT_INSTANCE_PHYSICS_SCENE ));
_scene.piscene->setUrl( (string("#") + string(_scene.pscene->getID())).c_str() );
}
void _WritePhysics() {
domPhysics_scene::domTechnique_commonRef common = daeSafeCast<domPhysics_scene::domTechnique_common>(_scene.pscene->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
// Create gravity
domTargetable_float3Ref g = daeSafeCast<domTargetable_float3>(common->add(COLLADA_ELEMENT_GRAVITY));
g->getValue().set3 (0,0,0);
}
/// \brief Write kinematic body in a given scene
void _WriteRobot(int id = 0)
{
ROS_DEBUG_STREAM(str(boost::format("writing robot as instance_articulated_system (%d) %s\n")%id%_robot.getName()));
string asid = _ComputeId(str(boost::format("robot%d")%id));
string askid = _ComputeId(str(boost::format("%s_kinematics")%asid));
string asmid = _ComputeId(str(boost::format("%s_motion")%asid));
string iassid = _ComputeId(str(boost::format("%s_inst")%asmid));
domInstance_articulated_systemRef ias = daeSafeCast<domInstance_articulated_system>(_scene.kscene->add(COLLADA_ELEMENT_INSTANCE_ARTICULATED_SYSTEM));
ias->setSid(iassid.c_str());
ias->setUrl((string("#")+asmid).c_str());
ias->setName(_robot.getName().c_str());
_iasout.reset(new instance_articulated_system_output());
_iasout->ias = ias;
// motion info
domArticulated_systemRef articulated_system_motion = daeSafeCast<domArticulated_system>(_articulatedSystemsLib->add(COLLADA_ELEMENT_ARTICULATED_SYSTEM));
articulated_system_motion->setId(asmid.c_str());
domMotionRef motion = daeSafeCast<domMotion>(articulated_system_motion->add(COLLADA_ELEMENT_MOTION));
domMotion_techniqueRef mt = daeSafeCast<domMotion_technique>(motion->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
domInstance_articulated_systemRef ias_motion = daeSafeCast<domInstance_articulated_system>(motion->add(COLLADA_ELEMENT_INSTANCE_ARTICULATED_SYSTEM));
ias_motion->setUrl(str(boost::format("#%s")%askid).c_str());
// kinematics info
domArticulated_systemRef articulated_system_kinematics = daeSafeCast<domArticulated_system>(_articulatedSystemsLib->add(COLLADA_ELEMENT_ARTICULATED_SYSTEM));
articulated_system_kinematics->setId(askid.c_str());
domKinematicsRef kinematics = daeSafeCast<domKinematics>(articulated_system_kinematics->add(COLLADA_ELEMENT_KINEMATICS));
domKinematics_techniqueRef kt = daeSafeCast<domKinematics_technique>(kinematics->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
_WriteInstance_kinematics_model(kinematics,askid,id);
for(size_t idof = 0; idof < _ikmout->vaxissids.size(); ++idof) {
string axis_infosid = _ComputeId(str(boost::format("axis_info_inst%d")%idof));
boost::shared_ptr<const urdf::Joint> pjoint = _ikmout->kmout->vaxissids.at(idof).pjoint;
BOOST_ASSERT(_mapjointindices[pjoint] == (int)idof);
//int iaxis = _ikmout->kmout->vaxissids.at(idof).iaxis;
// Kinematics axis info
domKinematics_axis_infoRef kai = daeSafeCast<domKinematics_axis_info>(kt->add(COLLADA_ELEMENT_AXIS_INFO));
kai->setAxis(str(boost::format("%s/%s")%_ikmout->kmout->kmodel->getID()%_ikmout->kmout->vaxissids.at(idof).sid).c_str());
kai->setSid(axis_infosid.c_str());
bool bactive = !pjoint->mimic;
double flower=0, fupper=0;
if( pjoint->type != urdf::Joint::CONTINUOUS ) {
if( !!pjoint->limits ) {
flower = pjoint->limits->lower;
fupper = pjoint->limits->upper;
}
if( !!pjoint->safety ) {
flower = pjoint->safety->soft_lower_limit;
fupper = pjoint->safety->soft_upper_limit;
}
if( flower == fupper ) {
bactive = false;
}
double fmult = 1.0;
if( pjoint->type != urdf::Joint::PRISMATIC ) {
fmult = 180.0/M_PI;
}
domKinematics_limitsRef plimits = daeSafeCast<domKinematics_limits>(kai->add(COLLADA_ELEMENT_LIMITS));
daeSafeCast<domCommon_float_or_param::domFloat>(plimits->add(COLLADA_ELEMENT_MIN)->add(COLLADA_ELEMENT_FLOAT))->setValue(flower*fmult);
daeSafeCast<domCommon_float_or_param::domFloat>(plimits->add(COLLADA_ELEMENT_MAX)->add(COLLADA_ELEMENT_FLOAT))->setValue(fupper*fmult);
}
domCommon_bool_or_paramRef active = daeSafeCast<domCommon_bool_or_param>(kai->add(COLLADA_ELEMENT_ACTIVE));
daeSafeCast<domCommon_bool_or_param::domBool>(active->add(COLLADA_ELEMENT_BOOL))->setValue(bactive);
domCommon_bool_or_paramRef locked = daeSafeCast<domCommon_bool_or_param>(kai->add(COLLADA_ELEMENT_LOCKED));
daeSafeCast<domCommon_bool_or_param::domBool>(locked->add(COLLADA_ELEMENT_BOOL))->setValue(false);
// Motion axis info
domMotion_axis_infoRef mai = daeSafeCast<domMotion_axis_info>(mt->add(COLLADA_ELEMENT_AXIS_INFO));
mai->setAxis(str(boost::format("%s/%s")%askid%axis_infosid).c_str());
if( !!pjoint->limits ) {
domCommon_float_or_paramRef speed = daeSafeCast<domCommon_float_or_param>(mai->add(COLLADA_ELEMENT_SPEED));
daeSafeCast<domCommon_float_or_param::domFloat>(speed->add(COLLADA_ELEMENT_FLOAT))->setValue(pjoint->limits->velocity);
domCommon_float_or_paramRef accel = daeSafeCast<domCommon_float_or_param>(mai->add(COLLADA_ELEMENT_ACCELERATION));
daeSafeCast<domCommon_float_or_param::domFloat>(accel->add(COLLADA_ELEMENT_FLOAT))->setValue(pjoint->limits->effort);
}
}
// write the bindings
string asmsym = _ComputeId(str(boost::format("%s_%s")%asmid%_ikmout->ikm->getSid()));
string assym = _ComputeId(str(boost::format("%s_%s")%_scene.kscene->getID()%_ikmout->ikm->getSid()));
FOREACH(it, _ikmout->vkinematicsbindings) {
domKinematics_newparamRef abm = daeSafeCast<domKinematics_newparam>(ias_motion->add(COLLADA_ELEMENT_NEWPARAM));
abm->setSid(asmsym.c_str());
daeSafeCast<domKinematics_newparam::domSIDREF>(abm->add(COLLADA_ELEMENT_SIDREF))->setValue(str(boost::format("%s/%s")%askid%it->first).c_str());
domKinematics_bindRef ab = daeSafeCast<domKinematics_bind>(ias->add(COLLADA_ELEMENT_BIND));
ab->setSymbol(assym.c_str());
daeSafeCast<domKinematics_param>(ab->add(COLLADA_ELEMENT_PARAM))->setRef(str(boost::format("%s/%s")%asmid%asmsym).c_str());
_iasout->vkinematicsbindings.push_back(make_pair(string(ab->getSymbol()), it->second));
}
for(size_t idof = 0; idof < _ikmout->vaxissids.size(); ++idof) {
const axis_sids& kas = _ikmout->vaxissids.at(idof);
domKinematics_newparamRef abm = daeSafeCast<domKinematics_newparam>(ias_motion->add(COLLADA_ELEMENT_NEWPARAM));
abm->setSid(_ComputeId(str(boost::format("%s_%s")%asmid%kas.axissid)).c_str());
daeSafeCast<domKinematics_newparam::domSIDREF>(abm->add(COLLADA_ELEMENT_SIDREF))->setValue(str(boost::format("%s/%s")%askid%kas.axissid).c_str());
domKinematics_bindRef ab = daeSafeCast<domKinematics_bind>(ias->add(COLLADA_ELEMENT_BIND));
ab->setSymbol(str(boost::format("%s_%s")%assym%kas.axissid).c_str());
daeSafeCast<domKinematics_param>(ab->add(COLLADA_ELEMENT_PARAM))->setRef(str(boost::format("%s/%s_%s")%asmid%asmid%kas.axissid).c_str());
string valuesid;
if( kas.valuesid.size() > 0 ) {
domKinematics_newparamRef abmvalue = daeSafeCast<domKinematics_newparam>(ias_motion->add(COLLADA_ELEMENT_NEWPARAM));
abmvalue->setSid(_ComputeId(str(boost::format("%s_%s")%asmid%kas.valuesid)).c_str());
daeSafeCast<domKinematics_newparam::domSIDREF>(abmvalue->add(COLLADA_ELEMENT_SIDREF))->setValue(str(boost::format("%s/%s")%askid%kas.valuesid).c_str());
domKinematics_bindRef abvalue = daeSafeCast<domKinematics_bind>(ias->add(COLLADA_ELEMENT_BIND));
valuesid = _ComputeId(str(boost::format("%s_%s")%assym%kas.valuesid));
abvalue->setSymbol(valuesid.c_str());
daeSafeCast<domKinematics_param>(abvalue->add(COLLADA_ELEMENT_PARAM))->setRef(str(boost::format("%s/%s_%s")%asmid%asmid%kas.valuesid).c_str());
}
_iasout->vaxissids.push_back(axis_sids(ab->getSymbol(),valuesid,kas.jointnodesid));
}
boost::shared_ptr<instance_physics_model_output> ipmout = _WriteInstance_physics_model(id,_scene.pscene,_scene.pscene->getID(), _ikmout->kmout->_maplinkposes);
}
/// \brief Write kinematic body in a given scene
virtual void _WriteInstance_kinematics_model(daeElementRef parent, const string& sidscope, int id)
{
ROS_DEBUG_STREAM(str(boost::format("writing instance_kinematics_model %s\n")%_robot.getName()));
boost::shared_ptr<kinematics_model_output> kmout = WriteKinematics_model(id);
_ikmout.reset(new instance_kinematics_model_output());
_ikmout->kmout = kmout;
_ikmout->ikm = daeSafeCast<domInstance_kinematics_model>(parent->add(COLLADA_ELEMENT_INSTANCE_KINEMATICS_MODEL));
string symscope, refscope;
if( sidscope.size() > 0 ) {
symscope = sidscope+string("_");
refscope = sidscope+string("/");
}
string ikmsid = _ComputeId(str(boost::format("%s_inst")%kmout->kmodel->getID()));
_ikmout->ikm->setUrl(str(boost::format("#%s")%kmout->kmodel->getID()).c_str());
_ikmout->ikm->setSid(ikmsid.c_str());
domKinematics_newparamRef kbind = daeSafeCast<domKinematics_newparam>(_ikmout->ikm->add(COLLADA_ELEMENT_NEWPARAM));
kbind->setSid(_ComputeId(symscope+ikmsid).c_str());
daeSafeCast<domKinematics_newparam::domSIDREF>(kbind->add(COLLADA_ELEMENT_SIDREF))->setValue((refscope+ikmsid).c_str());
_ikmout->vkinematicsbindings.push_back(make_pair(string(kbind->getSid()), str(boost::format("visual%d/node%d")%id%_maplinkindices[_robot.getRoot()])));
_ikmout->vaxissids.reserve(kmout->vaxissids.size());
int i = 0;
FOREACH(it,kmout->vaxissids) {
domKinematics_newparamRef kbind = daeSafeCast<domKinematics_newparam>(_ikmout->ikm->add(COLLADA_ELEMENT_NEWPARAM));
string ref = it->sid;
size_t index = ref.find("/");
while(index != string::npos) {
ref[index] = '.';
index = ref.find("/",index+1);
}
string sid = _ComputeId(symscope+ikmsid+"_"+ref);
kbind->setSid(sid.c_str());
daeSafeCast<domKinematics_newparam::domSIDREF>(kbind->add(COLLADA_ELEMENT_SIDREF))->setValue((refscope+ikmsid+"/"+it->sid).c_str());
double value=0;
double flower=0, fupper=0;
if( !!it->pjoint->limits ) {
flower = it->pjoint->limits->lower;
fupper = it->pjoint->limits->upper;
}
if( flower > 0 || fupper < 0 ) {
value = 0.5*(flower+fupper);
}
domKinematics_newparamRef pvalueparam = daeSafeCast<domKinematics_newparam>(_ikmout->ikm->add(COLLADA_ELEMENT_NEWPARAM));
pvalueparam->setSid((sid+string("_value")).c_str());
daeSafeCast<domKinematics_newparam::domFloat>(pvalueparam->add(COLLADA_ELEMENT_FLOAT))->setValue(value);
_ikmout->vaxissids.push_back(axis_sids(sid,pvalueparam->getSid(),kmout->vaxissids.at(i).jointnodesid));
++i;
}
}
virtual boost::shared_ptr<kinematics_model_output> WriteKinematics_model(int id)
{
domKinematics_modelRef kmodel = daeSafeCast<domKinematics_model>(_kinematicsModelsLib->add(COLLADA_ELEMENT_KINEMATICS_MODEL));
string kmodelid = _ComputeKinematics_modelId(id);
kmodel->setId(kmodelid.c_str());
kmodel->setName(_robot.getName().c_str());
domKinematics_model_techniqueRef ktec = daeSafeCast<domKinematics_model_technique>(kmodel->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
// Create root node for the visual scene
domNodeRef pnoderoot = daeSafeCast<domNode>(_scene.vscene->add(COLLADA_ELEMENT_NODE));
string bodyid = _ComputeId(str(boost::format("visual%d")%id));
pnoderoot->setId(bodyid.c_str());
pnoderoot->setSid(bodyid.c_str());
pnoderoot->setName(_robot.getName().c_str());
// Declare all the joints
_mapjointindices.clear();
int index=0;
FOREACHC(itj, _robot.joints_) {
_mapjointindices[itj->second] = index++;
}
_maplinkindices.clear();
index=0;
FOREACHC(itj, _robot.links_) {
_maplinkindices[itj->second] = index++;
}
_mapmaterialindices.clear();
index=0;
FOREACHC(itj, _robot.materials_) {
_mapmaterialindices[itj->second] = index++;
}
double lmin, lmax;
vector<domJointRef> vdomjoints(_robot.joints_.size());
boost::shared_ptr<kinematics_model_output> kmout(new kinematics_model_output());
kmout->kmodel = kmodel;
kmout->vaxissids.resize(_robot.joints_.size());
kmout->vlinksids.resize(_robot.links_.size());
FOREACHC(itjoint, _robot.joints_) {
boost::shared_ptr<urdf::Joint> pjoint = itjoint->second;
int index = _mapjointindices[itjoint->second];
domJointRef pdomjoint = daeSafeCast<domJoint>(ktec->add(COLLADA_ELEMENT_JOINT));
string jointid = _ComputeId(pjoint->name); //str(boost::format("joint%d")%index);
pdomjoint->setSid(jointid.c_str() );
pdomjoint->setName(pjoint->name.c_str());
domAxis_constraintRef axis;
if( !!pjoint->limits ) {
lmin=pjoint->limits->lower;
lmax=pjoint->limits->upper;
}
else {
lmin = lmax = 0;
}
double fmult = 1.0;
switch(pjoint->type) {
case urdf::Joint::REVOLUTE:
case urdf::Joint::CONTINUOUS:
axis = daeSafeCast<domAxis_constraint>(pdomjoint->add(COLLADA_ELEMENT_REVOLUTE));
fmult = 180.0f/M_PI;
lmin*=fmult;
lmax*=fmult;
break;
case urdf::Joint::PRISMATIC:
axis = daeSafeCast<domAxis_constraint>(pdomjoint->add(COLLADA_ELEMENT_PRISMATIC));
break;
case urdf::Joint::FIXED:
axis = daeSafeCast<domAxis_constraint>(pdomjoint->add(COLLADA_ELEMENT_REVOLUTE));
lmin = 0;
lmax = 0;
fmult = 0;
break;
default:
ROS_WARN_STREAM(str(boost::format("unsupported joint type specified %d")%(int)pjoint->type));
break;
}
if( !axis ) {
continue;
}
int ia = 0;
string axisid = _ComputeId(str(boost::format("axis%d")%ia));
axis->setSid(axisid.c_str());
kmout->vaxissids.at(index).pjoint = pjoint;
kmout->vaxissids.at(index).sid = jointid+string("/")+axisid;
kmout->vaxissids.at(index).iaxis = ia;
domAxisRef paxis = daeSafeCast<domAxis>(axis->add(COLLADA_ELEMENT_AXIS));
paxis->getValue().setCount(3);
paxis->getValue()[0] = pjoint->axis.x;
paxis->getValue()[1] = pjoint->axis.y;
paxis->getValue()[2] = pjoint->axis.z;
if( pjoint->type != urdf::Joint::CONTINUOUS ) {
domJoint_limitsRef plimits = daeSafeCast<domJoint_limits>(axis->add(COLLADA_TYPE_LIMITS));
daeSafeCast<domMinmax>(plimits->add(COLLADA_ELEMENT_MIN))->getValue() = lmin;
daeSafeCast<domMinmax>(plimits->add(COLLADA_ELEMENT_MAX))->getValue() = lmax;
}
vdomjoints.at(index) = pdomjoint;
}
LINKOUTPUT childinfo = _WriteLink(_robot.getRoot(), ktec, pnoderoot, kmodel->getID());
FOREACHC(itused, childinfo.listusedlinks) {
kmout->vlinksids.at(itused->first) = itused->second;
}
FOREACH(itprocessed,childinfo.listprocesseddofs) {
kmout->vaxissids.at(itprocessed->first).jointnodesid = itprocessed->second;
}
kmout->_maplinkposes = childinfo._maplinkposes;
// create the formulas for all mimic joints
FOREACHC(itjoint, _robot.joints_) {
string jointsid = _ComputeId(itjoint->second->name);
boost::shared_ptr<urdf::Joint> pjoint = itjoint->second;
if( !pjoint->mimic ) {
continue;
}
domFormulaRef pf = daeSafeCast<domFormula>(ktec->add(COLLADA_ELEMENT_FORMULA));
string formulaid = _ComputeId(str(boost::format("%s_formula")%jointsid));
pf->setSid(formulaid.c_str());
domCommon_float_or_paramRef ptarget = daeSafeCast<domCommon_float_or_param>(pf->add(COLLADA_ELEMENT_TARGET));
string targetjointid = str(boost::format("%s/%s")%kmodel->getID()%jointsid);
daeSafeCast<domCommon_param>(ptarget->add(COLLADA_TYPE_PARAM))->setValue(targetjointid.c_str());
domTechniqueRef pftec = daeSafeCast<domTechnique>(pf->add(COLLADA_ELEMENT_TECHNIQUE));
pftec->setProfile("OpenRAVE");
// save position equation
{
daeElementRef poselt = pftec->add("equation");
poselt->setAttribute("type","position");
// create a const0*joint+const1 formula
// <apply> <plus/> <apply> <times/> <cn>a</cn> x </apply> <cn>b</cn> </apply>
daeElementRef pmath_math = poselt->add("math");
daeElementRef pmath_apply = pmath_math->add("apply");
{
daeElementRef pmath_plus = pmath_apply->add("plus");
daeElementRef pmath_apply1 = pmath_apply->add("apply");
{
daeElementRef pmath_times = pmath_apply1->add("times");
daeElementRef pmath_const0 = pmath_apply1->add("cn");
pmath_const0->setCharData(str(boost::format("%f")%pjoint->mimic->multiplier));
daeElementRef pmath_symb = pmath_apply1->add("csymbol");
pmath_symb->setAttribute("encoding","COLLADA");
pmath_symb->setCharData(str(boost::format("%s/%s")%kmodel->getID()%_ComputeId(pjoint->mimic->joint_name)));
}
daeElementRef pmath_const1 = pmath_apply->add("cn");
pmath_const1->setCharData(str(boost::format("%f")%pjoint->mimic->offset));
}
}
// save first partial derivative
{
daeElementRef derivelt = pftec->add("equation");
derivelt->setAttribute("type","first_partial");
derivelt->setAttribute("target",str(boost::format("%s/%s")%kmodel->getID()%_ComputeId(pjoint->mimic->joint_name)).c_str());
daeElementRef pmath_const0 = derivelt->add("cn");
pmath_const0->setCharData(str(boost::format("%f")%pjoint->mimic->multiplier));
}
// save second partial derivative
{
daeElementRef derivelt = pftec->add("equation");
derivelt->setAttribute("type","second_partial");
derivelt->setAttribute("target",str(boost::format("%s/%s")%kmodel->getID()%_ComputeId(pjoint->mimic->joint_name)).c_str());
daeElementRef pmath_const0 = derivelt->add("cn");
pmath_const0->setCharData("0");
}
{
domFormula_techniqueRef pfcommontec = daeSafeCast<domFormula_technique>(pf->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
// create a const0*joint+const1 formula
// <apply> <plus/> <apply> <times/> <cn>a</cn> x </apply> <cn>b</cn> </apply>
daeElementRef pmath_math = pfcommontec->add("math");
daeElementRef pmath_apply = pmath_math->add("apply");
{
daeElementRef pmath_plus = pmath_apply->add("plus");
daeElementRef pmath_apply1 = pmath_apply->add("apply");
{
daeElementRef pmath_times = pmath_apply1->add("times");
daeElementRef pmath_const0 = pmath_apply1->add("cn");
pmath_const0->setCharData(str(boost::format("%f")%pjoint->mimic->multiplier));
daeElementRef pmath_symb = pmath_apply1->add("csymbol");
pmath_symb->setAttribute("encoding","COLLADA");
pmath_symb->setCharData(str(boost::format("%s/%s")%kmodel->getID()%_ComputeId(pjoint->mimic->joint_name)));
}
daeElementRef pmath_const1 = pmath_apply->add("cn");
pmath_const1->setCharData(str(boost::format("%f")%pjoint->mimic->offset));
}
}
}
return kmout;
}
/// \brief Write link of a kinematic body
///
/// \param link Link to write
/// \param pkinparent Kinbody parent
/// \param pnodeparent Node parent
/// \param strModelUri
virtual LINKOUTPUT _WriteLink(boost::shared_ptr<const urdf::Link> plink, daeElementRef pkinparent, domNodeRef pnodeparent, const string& strModelUri)
{
LINKOUTPUT out;
int linkindex = _maplinkindices[plink];
string linksid = _ComputeId(plink->name);
domLinkRef pdomlink = daeSafeCast<domLink>(pkinparent->add(COLLADA_ELEMENT_LINK));
pdomlink->setName(plink->name.c_str());
pdomlink->setSid(linksid.c_str());
domNodeRef pnode = daeSafeCast<domNode>(pnodeparent->add(COLLADA_ELEMENT_NODE));
string nodeid = _ComputeId(str(boost::format("v%s_node%d")%strModelUri%linkindex));
pnode->setId( nodeid.c_str() );
string nodesid = _ComputeId(str(boost::format("node%d")%linkindex));
pnode->setSid(nodesid.c_str());
pnode->setName(plink->name.c_str());
boost::shared_ptr<urdf::Geometry> geometry;
boost::shared_ptr<urdf::Material> material;
urdf::Pose geometry_origin;
if( !!plink->visual ) {
geometry = plink->visual->geometry;
material = plink->visual->material;
geometry_origin = plink->visual->origin;
}
else if( !!plink->collision ) {
geometry = plink->collision->geometry;
geometry_origin = plink->collision->origin;
}
if( !!geometry ) {
int igeom = 0;
string geomid = _ComputeId(str(boost::format("g%s_%s_geom%d")%strModelUri%linksid%igeom));
domGeometryRef pdomgeom = _WriteGeometry(geometry, geomid);
domInstance_geometryRef pinstgeom = daeSafeCast<domInstance_geometry>(pnode->add(COLLADA_ELEMENT_INSTANCE_GEOMETRY));
pinstgeom->setUrl((string("#")+geomid).c_str());
// material
_WriteMaterial(pdomgeom->getID(), material);
domBind_materialRef pmat = daeSafeCast<domBind_material>(pinstgeom->add(COLLADA_ELEMENT_BIND_MATERIAL));
domBind_material::domTechnique_commonRef pmattec = daeSafeCast<domBind_material::domTechnique_common>(pmat->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
domInstance_materialRef pinstmat = daeSafeCast<domInstance_material>(pmattec->add(COLLADA_ELEMENT_INSTANCE_MATERIAL));
pinstmat->setTarget(xsAnyURI(*pdomgeom, string("#")+geomid+string("_mat")));
pinstmat->setSymbol("mat0");
}
_WriteTransformation(pnode, geometry_origin);
urdf::Pose geometry_origin_inv = _poseInverse(geometry_origin);
// process all children
FOREACHC(itjoint, plink->child_joints) {
boost::shared_ptr<urdf::Joint> pjoint = *itjoint;
int index = _mapjointindices[pjoint];
// <attachment_full joint="k1/joint0">
domLink::domAttachment_fullRef attachment_full = daeSafeCast<domLink::domAttachment_full>(pdomlink->add(COLLADA_ELEMENT_ATTACHMENT_FULL));
string jointid = str(boost::format("%s/%s")%strModelUri%_ComputeId(pjoint->name));
attachment_full->setJoint(jointid.c_str());
LINKOUTPUT childinfo = _WriteLink(_robot.getLink(pjoint->child_link_name), attachment_full, pnode, strModelUri);
FOREACH(itused,childinfo.listusedlinks) {
out.listusedlinks.push_back(make_pair(itused->first,linksid+string("/")+itused->second));
}
FOREACH(itprocessed,childinfo.listprocesseddofs) {
out.listprocesseddofs.push_back(make_pair(itprocessed->first,nodesid+string("/")+itprocessed->second));
}
FOREACH(itlinkpos,childinfo._maplinkposes) {
out._maplinkposes[itlinkpos->first] = _poseMult(pjoint->parent_to_joint_origin_transform,itlinkpos->second);
}
// rotate/translate elements
string jointnodesid = _ComputeId(str(boost::format("node_%s_axis0")%pjoint->name));
switch(pjoint->type) {
case urdf::Joint::REVOLUTE:
case urdf::Joint::CONTINUOUS:
case urdf::Joint::FIXED: {
domRotateRef protate = daeSafeCast<domRotate>(childinfo.pnode->add(COLLADA_ELEMENT_ROTATE,0));
protate->setSid(jointnodesid.c_str());
protate->getValue().setCount(4);
protate->getValue()[0] = pjoint->axis.x;
protate->getValue()[1] = pjoint->axis.y;
protate->getValue()[2] = pjoint->axis.z;
protate->getValue()[3] = 0;
break;
}
case urdf::Joint::PRISMATIC: {
domTranslateRef ptrans = daeSafeCast<domTranslate>(childinfo.pnode->add(COLLADA_ELEMENT_TRANSLATE,0));
ptrans->setSid(jointnodesid.c_str());
ptrans->getValue().setCount(3);
ptrans->getValue()[0] = 0;
ptrans->getValue()[1] = 0;
ptrans->getValue()[2] = 0;
break;
}
default:
ROS_WARN_STREAM(str(boost::format("unsupported joint type specified %d")%(int)pjoint->type));
break;
}
_WriteTransformation(attachment_full, pjoint->parent_to_joint_origin_transform);
_WriteTransformation(childinfo.pnode, pjoint->parent_to_joint_origin_transform);
_WriteTransformation(childinfo.pnode, geometry_origin_inv); // have to do multiply by inverse since geometry transformation is not part of hierarchy
out.listprocesseddofs.push_back(make_pair(index,string(childinfo.pnode->getSid())+string("/")+jointnodesid));
// </attachment_full>
}
out._maplinkposes[plink] = urdf::Pose();
out.listusedlinks.push_back(make_pair(linkindex,linksid));
out.plink = pdomlink;
out.pnode = pnode;
return out;
}
domGeometryRef _WriteGeometry(boost::shared_ptr<urdf::Geometry> geometry, const std::string& geometry_id)
{
domGeometryRef cgeometry = daeSafeCast<domGeometry>(_geometriesLib->add(COLLADA_ELEMENT_GEOMETRY));
cgeometry->setId(geometry_id.c_str());
switch (geometry->type) {
case urdf::Geometry::MESH: {
urdf::Mesh* urdf_mesh = (urdf::Mesh*) geometry.get();
_loadMesh(urdf_mesh->filename, cgeometry, urdf_mesh->scale);
break;
}
case urdf::Geometry::SPHERE: {
ROS_WARN_STREAM(str(boost::format("cannot export sphere geometries yet! %s")%geometry_id));
break;
}
case urdf::Geometry::BOX: {
ROS_WARN_STREAM(str(boost::format("cannot export box geometries yet! %s")%geometry_id));
break;
}
case urdf::Geometry::CYLINDER: {
ROS_WARN_STREAM(str(boost::format("cannot export cylinder geometries yet! %s")%geometry_id));
break;
}
default: {
throw ColladaUrdfException(str(boost::format("undefined geometry type %d, name %s")%(int)geometry->type%geometry_id));
}
}
return cgeometry;
}
void _WriteMaterial(const string& geometry_id, boost::shared_ptr<urdf::Material> material)
{
string effid = geometry_id+string("_eff");
string matid = geometry_id+string("_mat");
domMaterialRef pdommat = daeSafeCast<domMaterial>(_materialsLib->add(COLLADA_ELEMENT_MATERIAL));
pdommat->setId(matid.c_str());
domInstance_effectRef pdominsteff = daeSafeCast<domInstance_effect>(pdommat->add(COLLADA_ELEMENT_INSTANCE_EFFECT));
pdominsteff->setUrl((string("#")+effid).c_str());
urdf::Color ambient, diffuse;
ambient.init("0.1 0.1 0.1 0");
diffuse.init("1 1 1 0");
if( !!material ) {
ambient.r = diffuse.r = material->color.r;
ambient.g = diffuse.g = material->color.g;
ambient.b = diffuse.b = material->color.b;
ambient.a = diffuse.a = material->color.a;
}
domEffectRef effect = _WriteEffect(geometry_id, ambient, diffuse);
// <material id="g1.link0.geom0.eff">
domMaterialRef dommaterial = daeSafeCast<domMaterial>(_materialsLib->add(COLLADA_ELEMENT_MATERIAL));
string material_id = geometry_id + string("_mat");
dommaterial->setId(material_id.c_str());
{
// <instance_effect url="#g1.link0.geom0.eff"/>
domInstance_effectRef instance_effect = daeSafeCast<domInstance_effect>(dommaterial->add(COLLADA_ELEMENT_INSTANCE_EFFECT));
string effect_id(effect->getId());
instance_effect->setUrl((string("#") + effect_id).c_str());
}
// </material>
domEffectRef pdomeff = _WriteEffect(effid, ambient, diffuse);
}
boost::shared_ptr<instance_physics_model_output> _WriteInstance_physics_model(int id, daeElementRef parent, const string& sidscope, const MAPLINKPOSES& maplinkposes)
{
boost::shared_ptr<physics_model_output> pmout = WritePhysics_model(id, maplinkposes);
boost::shared_ptr<instance_physics_model_output> ipmout(new instance_physics_model_output());
ipmout->pmout = pmout;
ipmout->ipm = daeSafeCast<domInstance_physics_model>(parent->add(COLLADA_ELEMENT_INSTANCE_PHYSICS_MODEL));
string bodyid = _ComputeId(str(boost::format("visual%d")%id));
ipmout->ipm->setParent(xsAnyURI(*ipmout->ipm,string("#")+bodyid));
string symscope, refscope;
if( sidscope.size() > 0 ) {
symscope = sidscope+string("_");
refscope = sidscope+string("/");
}
string ipmsid = str(boost::format("%s_inst")%pmout->pmodel->getID());
ipmout->ipm->setUrl(str(boost::format("#%s")%pmout->pmodel->getID()).c_str());
ipmout->ipm->setSid(ipmsid.c_str());
string kmodelid = _ComputeKinematics_modelId(id);
for(size_t i = 0; i < pmout->vrigidbodysids.size(); ++i) {
domInstance_rigid_bodyRef pirb = daeSafeCast<domInstance_rigid_body>(ipmout->ipm->add(COLLADA_ELEMENT_INSTANCE_RIGID_BODY));
pirb->setBody(pmout->vrigidbodysids[i].c_str());
pirb->setTarget(xsAnyURI(*pirb,str(boost::format("#v%s_node%d")%kmodelid%i)));
}
return ipmout;
}
boost::shared_ptr<physics_model_output> WritePhysics_model(int id, const MAPLINKPOSES& maplinkposes)
{
boost::shared_ptr<physics_model_output> pmout(new physics_model_output());
pmout->pmodel = daeSafeCast<domPhysics_model>(_physicsModelsLib->add(COLLADA_ELEMENT_PHYSICS_MODEL));
string pmodelid = str(boost::format("pmodel%d")%id);
pmout->pmodel->setId(pmodelid.c_str());
pmout->pmodel->setName(_robot.getName().c_str());
FOREACHC(itlink,_robot.links_) {
domRigid_bodyRef rigid_body = daeSafeCast<domRigid_body>(pmout->pmodel->add(COLLADA_ELEMENT_RIGID_BODY));
string rigidsid = str(boost::format("rigid%d")%_maplinkindices[itlink->second]);
pmout->vrigidbodysids.push_back(rigidsid);
rigid_body->setSid(rigidsid.c_str());
rigid_body->setName(itlink->second->name.c_str());
domRigid_body::domTechnique_commonRef ptec = daeSafeCast<domRigid_body::domTechnique_common>(rigid_body->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
boost::shared_ptr<urdf::Inertial> inertial = itlink->second->inertial;
if( !!inertial ) {
daeSafeCast<domRigid_body::domTechnique_common::domDynamic>(ptec->add(COLLADA_ELEMENT_DYNAMIC))->setValue(xsBoolean(true)); //!!inertial));
domTargetable_floatRef mass = daeSafeCast<domTargetable_float>(ptec->add(COLLADA_ELEMENT_MASS));
mass->setValue(inertial->mass);
double fCovariance[9] = { inertial->ixx, inertial->ixy, inertial->ixz, inertial->ixy, inertial->iyy, inertial->iyz, inertial->ixz, inertial->iyz, inertial->izz};
double eigenvalues[3], eigenvectors[9];
mathextra::EigenSymmetric3(fCovariance,eigenvalues,eigenvectors);
boost::array<double,12> minertiaframe;
for(int j = 0; j < 3; ++j) {
minertiaframe[4*0+j] = eigenvectors[3*j];
minertiaframe[4*1+j] = eigenvectors[3*j+1];
minertiaframe[4*2+j] = eigenvectors[3*j+2];
}
urdf::Pose tinertiaframe;
tinertiaframe.rotation = _quatFromMatrix(minertiaframe);
tinertiaframe = _poseMult(inertial->origin,tinertiaframe);
domTargetable_float3Ref pinertia = daeSafeCast<domTargetable_float3>(ptec->add(COLLADA_ELEMENT_INERTIA));
pinertia->getValue().setCount(3);
pinertia->getValue()[0] = eigenvalues[0];
pinertia->getValue()[1] = eigenvalues[1];
pinertia->getValue()[2] = eigenvalues[2];
urdf::Pose posemassframe = _poseMult(maplinkposes.find(itlink->second)->second,tinertiaframe);
_WriteTransformation(ptec->add(COLLADA_ELEMENT_MASS_FRAME), posemassframe);
// // create a shape for every geometry
// int igeom = 0;
// FOREACHC(itgeom, (*itlink)->GetGeometries()) {
// domRigid_body::domTechnique_common::domShapeRef pdomshape = daeSafeCast<domRigid_body::domTechnique_common::domShape>(ptec->add(COLLADA_ELEMENT_SHAPE));
// // there is a weird bug here where _WriteTranformation will fail to create rotate/translate elements in instance_geometry is created first... (is this part of the spec?)
// _WriteTransformation(pdomshape,tbaseinv*(*itlink)->GetTransform()*itgeom->GetTransform());
// domInstance_geometryRef pinstgeom = daeSafeCast<domInstance_geometry>(pdomshape->add(COLLADA_ELEMENT_INSTANCE_GEOMETRY));
// pinstgeom->setUrl(xsAnyURI(*pinstgeom,string("#")+_GetGeometryId(*itlink,igeom)));
// ++igeom;
// }
}
}
return pmout;
}
void _loadMesh(std::string const& filename, domGeometryRef pdomgeom, const urdf::Vector3& scale)
{
const aiScene* scene = _importer.ReadFile(filename, aiProcess_SortByPType|aiProcess_Triangulate); //|aiProcess_GenNormals|aiProcess_GenUVCoords|aiProcess_FlipUVs);
if( !scene ) {
ROS_WARN("failed to load resource %s",filename.c_str());
return;
}
if( !scene->mRootNode ) {
ROS_WARN("resource %s has no data",filename.c_str());
return;
}
if (!scene->HasMeshes()) {
ROS_WARN_STREAM(str(boost::format("No meshes found in file %s")%filename));
return;
}
domMeshRef pdommesh = daeSafeCast<domMesh>(pdomgeom->add(COLLADA_ELEMENT_MESH));
domSourceRef pvertsource = daeSafeCast<domSource>(pdommesh->add(COLLADA_ELEMENT_SOURCE));
domAccessorRef pacc;
domFloat_arrayRef parray;
{
pvertsource->setId(str(boost::format("%s_positions")%pdomgeom->getID()).c_str());
parray = daeSafeCast<domFloat_array>(pvertsource->add(COLLADA_ELEMENT_FLOAT_ARRAY));
parray->setId(str(boost::format("%s_positions-array")%pdomgeom->getID()).c_str());
parray->setDigits(6); // 6 decimal places
domSource::domTechnique_commonRef psourcetec = daeSafeCast<domSource::domTechnique_common>(pvertsource->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
pacc = daeSafeCast<domAccessor>(psourcetec->add(COLLADA_ELEMENT_ACCESSOR));
pacc->setSource(xsAnyURI(*parray, string("#")+string(parray->getID())));
pacc->setStride(3);
domParamRef px = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM));
px->setName("X"); px->setType("float");
domParamRef py = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM));
py->setName("Y"); py->setType("float");
domParamRef pz = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM));
pz->setName("Z"); pz->setType("float");
}
domVerticesRef pverts = daeSafeCast<domVertices>(pdommesh->add(COLLADA_ELEMENT_VERTICES));
{
pverts->setId("vertices");
domInput_localRef pvertinput = daeSafeCast<domInput_local>(pverts->add(COLLADA_ELEMENT_INPUT));
pvertinput->setSemantic("POSITION");
pvertinput->setSource(domUrifragment(*pvertsource, string("#")+string(pvertsource->getID())));
}
_buildAiMesh(scene,scene->mRootNode,pdommesh,parray, pdomgeom->getID(),scale);
pacc->setCount(parray->getCount());
}
void _buildAiMesh(const aiScene* scene, aiNode* node, domMeshRef pdommesh, domFloat_arrayRef parray, const string& geomid, const urdf::Vector3& scale)
{
if( !node ) {
return;
}
aiMatrix4x4 transform = node->mTransformation;
aiNode *pnode = node->mParent;
while (pnode) {
// Don't convert to y-up orientation, which is what the root node in
// Assimp does
if (pnode->mParent != NULL) {
transform = pnode->mTransformation * transform;
}
pnode = pnode->mParent;
}
{
uint32_t vertexOffset = parray->getCount();
uint32_t nTotalVertices=0;
for (uint32_t i = 0; i < node->mNumMeshes; i++) {
aiMesh* input_mesh = scene->mMeshes[node->mMeshes[i]];
nTotalVertices += input_mesh->mNumVertices;
}
parray->getValue().grow(parray->getCount()+nTotalVertices*3);
parray->setCount(parray->getCount()+nTotalVertices);
for (uint32_t i = 0; i < node->mNumMeshes; i++) {
aiMesh* input_mesh = scene->mMeshes[node->mMeshes[i]];
for (uint32_t j = 0; j < input_mesh->mNumVertices; j++) {
aiVector3D p = input_mesh->mVertices[j];
p *= transform;
parray->getValue().append(p.x*scale.x);
parray->getValue().append(p.y*scale.y);
parray->getValue().append(p.z*scale.z);
}
}
// in order to save space, separate triangles from poly lists
vector<int> triangleindices, otherindices;
int nNumOtherPrimitives = 0;
for (uint32_t i = 0; i < node->mNumMeshes; i++) {
aiMesh* input_mesh = scene->mMeshes[node->mMeshes[i]];
uint32_t indexCount = 0, otherIndexCount = 0;
for (uint32_t j = 0; j < input_mesh->mNumFaces; j++) {
aiFace& face = input_mesh->mFaces[j];
if( face.mNumIndices == 3 ) {
indexCount += face.mNumIndices;
}
else {
otherIndexCount += face.mNumIndices;
}
}
triangleindices.reserve(triangleindices.size()+indexCount);
otherindices.reserve(otherindices.size()+otherIndexCount);
for (uint32_t j = 0; j < input_mesh->mNumFaces; j++) {
aiFace& face = input_mesh->mFaces[j];
if( face.mNumIndices == 3 ) {
triangleindices.push_back(vertexOffset+face.mIndices[0]);
triangleindices.push_back(vertexOffset+face.mIndices[1]);
triangleindices.push_back(vertexOffset+face.mIndices[2]);
}
else {
for (uint32_t k = 0; k < face.mNumIndices; ++k) {
otherindices.push_back(face.mIndices[k]+vertexOffset);
}
nNumOtherPrimitives++;
}
}
vertexOffset += input_mesh->mNumVertices;
}
if( triangleindices.size() > 0 ) {
domTrianglesRef ptris = daeSafeCast<domTriangles>(pdommesh->add(COLLADA_ELEMENT_TRIANGLES));
ptris->setCount(triangleindices.size()/3);
ptris->setMaterial("mat0");
domInput_local_offsetRef pvertoffset = daeSafeCast<domInput_local_offset>(ptris->add(COLLADA_ELEMENT_INPUT));
pvertoffset->setSemantic("VERTEX");
pvertoffset->setOffset(0);
pvertoffset->setSource(domUrifragment(*pdommesh->getVertices(), str(boost::format("#%s/vertices")%geomid)));
domPRef pindices = daeSafeCast<domP>(ptris->add(COLLADA_ELEMENT_P));
pindices->getValue().setCount(triangleindices.size());
for(size_t ind = 0; ind < triangleindices.size(); ++ind) {
pindices->getValue()[ind] = triangleindices[ind];
}
}
if( nNumOtherPrimitives > 0 ) {
domPolylistRef ptris = daeSafeCast<domPolylist>(pdommesh->add(COLLADA_ELEMENT_POLYLIST));
ptris->setCount(nNumOtherPrimitives);
ptris->setMaterial("mat0");
domInput_local_offsetRef pvertoffset = daeSafeCast<domInput_local_offset>(ptris->add(COLLADA_ELEMENT_INPUT));
pvertoffset->setSemantic("VERTEX");
pvertoffset->setSource(domUrifragment(*pdommesh->getVertices(), str(boost::format("#%s/vertices")%geomid)));
domPRef pindices = daeSafeCast<domP>(ptris->add(COLLADA_ELEMENT_P));
pindices->getValue().setCount(otherindices.size());
for(size_t ind = 0; ind < otherindices.size(); ++ind) {
pindices->getValue()[ind] = otherindices[ind];
}
domPolylist::domVcountRef pcount = daeSafeCast<domPolylist::domVcount>(ptris->add(COLLADA_ELEMENT_VCOUNT));
pcount->getValue().setCount(nNumOtherPrimitives);
uint32_t offset = 0;
for (uint32_t i = 0; i < node->mNumMeshes; i++) {
aiMesh* input_mesh = scene->mMeshes[node->mMeshes[i]];
for (uint32_t j = 0; j < input_mesh->mNumFaces; j++) {
aiFace& face = input_mesh->mFaces[j];
if( face.mNumIndices > 3 ) {
pcount->getValue()[offset++] = face.mNumIndices;
}
}
}
}
}
for (uint32_t i=0; i < node->mNumChildren; ++i) {
_buildAiMesh(scene, node->mChildren[i], pdommesh,parray,geomid,scale);
}
}
domEffectRef _WriteEffect(std::string const& effect_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));
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;
}
/// \brief Write transformation
/// \param pelt Element to transform
/// \param t Transform to write
void _WriteTransformation(daeElementRef pelt, const urdf::Pose& t)
{
domRotateRef prot = daeSafeCast<domRotate>(pelt->add(COLLADA_ELEMENT_ROTATE,0));
domTranslateRef ptrans = daeSafeCast<domTranslate>(pelt->add(COLLADA_ELEMENT_TRANSLATE,0));
ptrans->getValue().setCount(3);
ptrans->getValue()[0] = t.position.x;
ptrans->getValue()[1] = t.position.y;
ptrans->getValue()[2] = t.position.z;
prot->getValue().setCount(4);
// extract axis from quaternion
double sinang = t.rotation.x*t.rotation.x+t.rotation.y*t.rotation.y+t.rotation.z*t.rotation.z;
if( std::fabs(sinang) < 1e-10 ) {
prot->getValue()[0] = 1;
prot->getValue()[1] = 0;
prot->getValue()[2] = 0;
prot->getValue()[3] = 0;
}
else {
urdf::Rotation quat;
if( t.rotation.w < 0 ) {
quat.x = -t.rotation.x;
quat.y = -t.rotation.y;
quat.z = -t.rotation.z;
quat.w = -t.rotation.w;
}
else {
quat = t.rotation;
}
sinang = std::sqrt(sinang);
prot->getValue()[0] = quat.x/sinang;
prot->getValue()[1] = quat.y/sinang;
prot->getValue()[2] = quat.z/sinang;
prot->getValue()[3] = angles::to_degrees(2.0*std::atan2(sinang,quat.w));
}
}
// binding in instance_kinematics_scene
void _WriteBindingsInstance_kinematics_scene()
{
FOREACHC(it, _iasout->vkinematicsbindings) {
domBind_kinematics_modelRef pmodelbind = daeSafeCast<domBind_kinematics_model>(_scene.kiscene->add(COLLADA_ELEMENT_BIND_KINEMATICS_MODEL));
pmodelbind->setNode(it->second.c_str());
daeSafeCast<domCommon_param>(pmodelbind->add(COLLADA_ELEMENT_PARAM))->setValue(it->first.c_str());
}
FOREACHC(it, _iasout->vaxissids) {
domBind_joint_axisRef pjointbind = daeSafeCast<domBind_joint_axis>(_scene.kiscene->add(COLLADA_ELEMENT_BIND_JOINT_AXIS));
pjointbind->setTarget(it->jointnodesid.c_str());
daeSafeCast<domCommon_param>(pjointbind->add(COLLADA_ELEMENT_AXIS)->add(COLLADA_TYPE_PARAM))->setValue(it->axissid.c_str());
daeSafeCast<domCommon_param>(pjointbind->add(COLLADA_ELEMENT_VALUE)->add(COLLADA_TYPE_PARAM))->setValue(it->valuesid.c_str());
}
}
private:
static urdf::Vector3 _poseMult(const urdf::Pose& p, const urdf::Vector3& v)
{
double ww = 2 * p.rotation.x * p.rotation.x;
double wx = 2 * p.rotation.x * p.rotation.y;
double wy = 2 * p.rotation.x * p.rotation.z;
double wz = 2 * p.rotation.x * p.rotation.w;
double xx = 2 * p.rotation.y * p.rotation.y;
double xy = 2 * p.rotation.y * p.rotation.z;
double xz = 2 * p.rotation.y * p.rotation.w;
double yy = 2 * p.rotation.z * p.rotation.z;
double yz = 2 * p.rotation.z * p.rotation.w;
urdf::Vector3 vnew;
vnew.x = (1-xx-yy) * v.x + (wx-yz) * v.y + (wy+xz)*v.z + p.position.x;
vnew.y = (wx+yz) * v.x + (1-ww-yy) * v.y + (xy-wz)*v.z + p.position.y;
vnew.z = (wy-xz) * v.x + (xy+wz) * v.y + (1-ww-xx)*v.z + p.position.z;
return vnew;
}
static urdf::Pose _poseInverse(const urdf::Pose& p)
{
urdf::Pose pinv;
pinv.rotation.x = -p.rotation.x;
pinv.rotation.y = -p.rotation.y;
pinv.rotation.z = -p.rotation.z;
pinv.rotation.w = p.rotation.w;
urdf::Vector3 t = _poseMult(pinv,p.position);
pinv.position.x = -t.x;
pinv.position.y = -t.y;
pinv.position.z = -t.z;
return pinv;
}
static urdf::Rotation _quatMult(const urdf::Rotation& quat0, const urdf::Rotation& quat1)
{
urdf::Rotation q;
q.x = quat0.w*quat1.x + quat0.x*quat1.w + quat0.y*quat1.z - quat0.z*quat1.y;
q.y = quat0.w*quat1.y + quat0.y*quat1.w + quat0.z*quat1.x - quat0.x*quat1.z;
q.z = quat0.w*quat1.z + quat0.z*quat1.w + quat0.x*quat1.y - quat0.y*quat1.x;
q.w = quat0.w*quat1.w - quat0.x*quat1.x - quat0.y*quat1.y - quat0.z*quat1.z;
double fnorm = std::sqrt(q.x*q.x+q.y*q.y+q.z*q.z+q.w*q.w);
// don't touch the divides
q.x /= fnorm;
q.y /= fnorm;
q.z /= fnorm;
q.w /= fnorm;
return q;
}
static urdf::Pose _poseMult(const urdf::Pose& p0, const urdf::Pose& p1)
{
urdf::Pose p;
p.position = _poseMult(p0,p1.position);
p.rotation = _quatMult(p0.rotation,p1.rotation);
return p;
}
static urdf::Rotation _quatFromMatrix(const boost::array<double,12>& mat)
{
urdf::Rotation rot;
double tr = mat[4*0+0] + mat[4*1+1] + mat[4*2+2];
if (tr >= 0) {
rot.w = tr + 1;
rot.x = (mat[4*2+1] - mat[4*1+2]);
rot.y = (mat[4*0+2] - mat[4*2+0]);
rot.z = (mat[4*1+0] - mat[4*0+1]);
}
else {
// find the largest diagonal element and jump to the appropriate case
if (mat[4*1+1] > mat[4*0+0]) {
if (mat[4*2+2] > mat[4*1+1]) {
rot.z = (mat[4*2+2] - (mat[4*0+0] + mat[4*1+1])) + 1;
rot.x = (mat[4*2+0] + mat[4*0+2]);
rot.y = (mat[4*1+2] + mat[4*2+1]);
rot.w = (mat[4*1+0] - mat[4*0+1]);
}
else {
rot.y = (mat[4*1+1] - (mat[4*2+2] + mat[4*0+0])) + 1;
rot.z = (mat[4*1+2] + mat[4*2+1]);
rot.x = (mat[4*0+1] + mat[4*1+0]);
rot.w = (mat[4*0+2] - mat[4*2+0]);
}
}
else if (mat[4*2+2] > mat[4*0+0]) {
rot.z = (mat[4*2+2] - (mat[4*0+0] + mat[4*1+1])) + 1;
rot.x = (mat[4*2+0] + mat[4*0+2]);
rot.y = (mat[4*1+2] + mat[4*2+1]);
rot.w = (mat[4*1+0] - mat[4*0+1]);
}
else {
rot.x = (mat[4*0+0] - (mat[4*1+1] + mat[4*2+2])) + 1;
rot.y = (mat[4*0+1] + mat[4*1+0]);
rot.z = (mat[4*2+0] + mat[4*0+2]);
rot.w = (mat[4*2+1] - mat[4*1+2]);
}
}
double fnorm = std::sqrt(rot.x*rot.x+rot.y*rot.y+rot.z*rot.z+rot.w*rot.w);
// don't touch the divides
rot.x /= fnorm;
rot.y /= fnorm;
rot.z /= fnorm;
rot.w /= fnorm;
return rot;
}
static std::string _ComputeKinematics_modelId(int id)
{
return _ComputeId(str(boost::format("kmodel%d")%id));
}
/// \brief computes a collada-compliant sid from the urdf name
static std::string _ComputeId(const std::string& name)
{
std::string newname = name;
for(size_t i = 0; i < newname.size(); ++i) {
if( newname[i] == '/' || newname[i] == ' ' || newname[i] == '.' ) {
newname[i] = '_';
}
}
return newname;
}
int _writeoptions;
const urdf::Model& _robot;
boost::shared_ptr<DAE> _collada;
domCOLLADA* _dom;
domCOLLADA::domSceneRef _globalscene;
domLibrary_visual_scenesRef _visualScenesLib;
domLibrary_kinematics_scenesRef _kinematicsScenesLib;
domLibrary_kinematics_modelsRef _kinematicsModelsLib;
domLibrary_articulated_systemsRef _articulatedSystemsLib;
domLibrary_physics_scenesRef _physicsScenesLib;
domLibrary_physics_modelsRef _physicsModelsLib;
domLibrary_materialsRef _materialsLib;
domLibrary_effectsRef _effectsLib;
domLibrary_geometriesRef _geometriesLib;
domTechniqueRef _sensorsLib; ///< custom sensors library
SCENE _scene;
boost::shared_ptr<instance_kinematics_model_output> _ikmout;
boost::shared_ptr<instance_articulated_system_output> _iasout;
std::map< boost::shared_ptr<const urdf::Joint>, int > _mapjointindices;
std::map< boost::shared_ptr<const urdf::Link>, int > _maplinkindices;
std::map< boost::shared_ptr<const urdf::Material>, int > _mapmaterialindices;
Assimp::Importer _importer;
};
ColladaUrdfException::ColladaUrdfException(std::string const& what)
: std::runtime_error(what)
{
}
bool colladaFromUrdfFile(string const& file, boost::shared_ptr<DAE>& dom) {
TiXmlDocument urdf_xml;
if (!urdf_xml.LoadFile(file)) {
ROS_ERROR("Could not load XML file");
return false;
}
return colladaFromUrdfXml(&urdf_xml, dom);
}
bool colladaFromUrdfString(string const& xml, boost::shared_ptr<DAE>& dom) {
TiXmlDocument urdf_xml;
if (urdf_xml.Parse(xml.c_str()) == 0) {
ROS_ERROR("Could not parse XML document");
return false;
}
return colladaFromUrdfXml(&urdf_xml, dom);
}
bool colladaFromUrdfXml(TiXmlDocument* xml_doc, boost::shared_ptr<DAE>& dom) {
urdf::Model robot_model;
if (!robot_model.initXml(xml_doc)) {
ROS_ERROR("Could not generate robot model");
return false;
}
return colladaFromUrdfModel(robot_model, dom);
}
bool colladaFromUrdfModel(urdf::Model const& robot_model, boost::shared_ptr<DAE>& dom) {
ColladaWriter writer(robot_model,0);
dom = writer.convert();
return dom != boost::shared_ptr<DAE>();
}
bool colladaToFile(boost::shared_ptr<DAE> dom, string const& file) {
daeString uri = dom->getDoc(0)->getDocumentURI()->getURI();
return dom->writeTo(uri, file);
}
}
Reference in New Issue View Git Blame Copy Permalink