collada_urdf.cpp
Go to the documentation of this file.
00001 // -*- coding: utf-8 -*-
00002 /*********************************************************************
00003 * Software License Agreement (BSD License)
00004 *
00005 *  Copyright (c) 2010, Willow Garage, Inc., University of Tokyo
00006 *  All rights reserved.
00007 *
00008 *  Redistribution and use in source and binary forms, with or without
00009 *  modification, are permitted provided that the following conditions
00010 *  are met:
00011 *
00012 *   * Redstributions of source code must retain the above copyright
00013 *     notice, this list of conditions and the following disclaimer.
00014 *   * Redistributions in binary form must reproduce the above
00015 *     copyright notice, this list of conditions and the following
00016 *     disclaimer in the documentation and/or other materials provided
00017 *     with the distribution.
00018 *   * Neither the name of the Willow Garage nor the names of its
00019 *     contributors may be used to endorse or promote products derived
00020 *     from this software without specific prior written permission.
00021 *
00022 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 *  POSSIBILITY OF SUCH DAMAGE.
00034 *********************************************************************/
00035 
00036 /* Authors: Rosen Diankov, Tim Field */
00037 
00038 #include "collada_urdf/collada_urdf.h"
00039 #include <map>
00040 #include <vector>
00041 #include <list>
00042 
00043 #include <dae.h>
00044 #include <dae/daeDocument.h>
00045 #include <dae/daeErrorHandler.h>
00046 #include <dae/domAny.h>
00047 #include <dom/domCOLLADA.h>
00048 #include <dom/domConstants.h>
00049 #include <dom/domElements.h>
00050 #include <dom/domTriangles.h>
00051 #include <dom/domTypes.h>
00052 #include <resource_retriever/retriever.h>
00053 #include <urdf/model.h>
00054 #include <urdf_model/pose.h>
00055 #include <angles/angles.h>
00056 #include <ros/assert.h>
00057 
00058 #include <boost/date_time/posix_time/posix_time.hpp>
00059 #include <boost/date_time/posix_time/posix_time_io.hpp>
00060 #include <boost/format.hpp>
00061 #include <boost/array.hpp>
00062 #include <boost/shared_ptr.hpp>
00063 #include <boost/scoped_ptr.hpp>
00064 
00065 #if defined(ASSIMP_UNIFIED_HEADER_NAMES)
00066 #include <assimp/scene.h>
00067 #include <assimp/LogStream.hpp>
00068 #include <assimp/DefaultLogger.hpp>
00069 #include <assimp/IOStream.hpp>
00070 #include <assimp/IOSystem.hpp>
00071 #include <assimp/Importer.hpp>
00072 #include <assimp/postprocess.h>
00073 #else
00074 #include <assimp.hpp>
00075 #include <aiScene.h>
00076 #include <aiPostProcess.h>
00077 #include <DefaultLogger.h>
00078 #include <IOStream.h>
00079 #include <IOSystem.h>
00080 #endif
00081 
00082 #include <geometric_shapes/shapes.h>
00083 #include <geometric_shapes/mesh_operations.h>
00084 
00085 #define FOREACH(it, v) for(typeof((v).begin())it = (v).begin(); it != (v).end(); (it)++)
00086 #define FOREACHC FOREACH
00087 
00088 using namespace std;
00089 
00090 namespace ColladaDOM150 { }
00091 
00092 namespace collada_urdf {
00093 
00094 using namespace ColladaDOM150;
00095 
00097 class ResourceIOStream : public Assimp::IOStream
00098 {
00099 public:
00100     ResourceIOStream(const resource_retriever::MemoryResource& res)
00101         : res_(res)
00102         , pos_(res.data.get())
00103     {
00104     }
00105 
00106     ~ResourceIOStream()
00107     {
00108     }
00109 
00110     size_t Read(void* buffer, size_t size, size_t count)
00111     {
00112         size_t to_read = size * count;
00113         if (pos_ + to_read > res_.data.get() + res_.size)
00114         {
00115             to_read = res_.size - (pos_ - res_.data.get());
00116         }
00117 
00118         memcpy(buffer, pos_, to_read);
00119         pos_ += to_read;
00120 
00121         return to_read;
00122     }
00123 
00124     size_t Write( const void* buffer, size_t size, size_t count) {
00125         ROS_BREAK(); return 0;
00126     }
00127 
00128     aiReturn Seek( size_t offset, aiOrigin origin)
00129     {
00130         uint8_t* new_pos = 0;
00131         switch (origin)
00132         {
00133         case aiOrigin_SET:
00134             new_pos = res_.data.get() + offset;
00135             break;
00136         case aiOrigin_CUR:
00137             new_pos = pos_ + offset; // TODO is this right?  can offset really not be negative
00138             break;
00139         case aiOrigin_END:
00140             new_pos = res_.data.get() + res_.size - offset; // TODO is this right?
00141             break;
00142         default:
00143             ROS_BREAK();
00144         }
00145 
00146         if (new_pos < res_.data.get() || new_pos > res_.data.get() + res_.size)
00147         {
00148             return aiReturn_FAILURE;
00149         }
00150 
00151         pos_ = new_pos;
00152         return aiReturn_SUCCESS;
00153     }
00154 
00155     size_t Tell() const
00156     {
00157         return pos_ - res_.data.get();
00158     }
00159 
00160     size_t FileSize() const
00161     {
00162         return res_.size;
00163     }
00164 
00165     void Flush() {
00166     }
00167 
00168 private:
00169     resource_retriever::MemoryResource res_;
00170     uint8_t* pos_;
00171 };
00172 
00173 namespace mathextra {
00174 
00175 // code from MagicSoftware by Dave Eberly
00176 const double g_fEpsilon = 1e-15;
00177 //===========================================================================
00178 
00179 
00180 #define distinctRoots 0                       // roots r0 < r1 < r2
00181 #define singleRoot 1                       // root r0
00182 #define floatRoot01 2                       // roots r0 = r1 < r2
00183 #define floatRoot12 4                       // roots r0 < r1 = r2
00184 #define tripleRoot 6                       // roots r0 = r1 = r2
00185 
00186 
00187 template <typename T, typename S>
00188 void Tridiagonal3 (S* mat, T* diag, T* subd)
00189 {
00190     T a, b, c, d, e, f, ell, q;
00191 
00192     a = mat[0*3+0];
00193     b = mat[0*3+1];
00194     c = mat[0*3+2];
00195     d = mat[1*3+1];
00196     e = mat[1*3+2];
00197     f = mat[2*3+2];
00198 
00199     subd[2] = 0.0;
00200     diag[0] = a;
00201     if ( fabs(c) >= g_fEpsilon ) {
00202         ell = (T)sqrt(b*b+c*c);
00203         b /= ell;
00204         c /= ell;
00205         q = 2*b*e+c*(f-d);
00206         diag[1] = d+c*q;
00207         diag[2] = f-c*q;
00208         subd[0] = ell;
00209         subd[1] = e-b*q;
00210         mat[0*3+0] = (S)1; mat[0*3+1] = (S)0; mat[0*3+2] = (T)0;
00211         mat[1*3+0] = (S)0; mat[1*3+1] = b; mat[1*3+2] = c;
00212         mat[2*3+0] = (S)0; mat[2*3+1] = c; mat[2*3+2] = -b;
00213     }
00214     else {
00215         diag[1] = d;
00216         diag[2] = f;
00217         subd[0] = b;
00218         subd[1] = e;
00219         mat[0*3+0] = (S)1; mat[0*3+1] = (S)0; mat[0*3+2] = (S)0;
00220         mat[1*3+0] = (S)0; mat[1*3+1] = (S)1; mat[1*3+2] = (S)0;
00221         mat[2*3+0] = (S)0; mat[2*3+1] = (S)0; mat[2*3+2] = (S)1;
00222     }
00223 }
00224 
00225 int CubicRoots (double c0, double c1, double c2, double *r0, double *r1, double *r2)
00226 {
00227     // polynomial is L^3-c2*L^2+c1*L-c0
00228 
00229     int maxiter = 50;
00230     double discr, temp, pval, pdval, b0, b1;
00231     int i;
00232 
00233     // find local extrema (if any) of p'(L) = 3*L^2-2*c2*L+c1
00234     discr = c2*c2-3*c1;
00235     if ( discr >= 0.0 ) {
00236         discr = (double)sqrt(discr);
00237         temp = (c2+discr)/3;
00238         pval = temp*(temp*(temp-c2)+c1)-c0;
00239         if ( pval >= 0.0 ) {
00240             // double root occurs before the positive local maximum
00241             (*r0) = (c2-discr)/3 - 1;  // initial guess for Newton's methods
00242             pval = 2*g_fEpsilon;
00243             for (i = 0; i < maxiter && fabs(pval) > g_fEpsilon; i++) {
00244                 pval = (*r0)*((*r0)*((*r0)-c2)+c1)-c0;
00245                 pdval = (*r0)*(3*(*r0)-2*c2)+c1;
00246                 (*r0) -= pval/pdval;
00247             }
00248 
00249             // Other two roots are solutions to quadratic equation
00250             // L^2 + ((*r0)-c2)*L + [(*r0)*((*r0)-c2)+c1] = 0.
00251             b1 = (*r0)-c2;
00252             b0 = (*r0)*((*r0)-c2)+c1;
00253             discr = b1*b1-4*b0;
00254             if ( discr < -g_fEpsilon )
00255             {
00256                 // single root r0
00257                 return singleRoot;
00258             }
00259             else
00260             {
00261                 int result = distinctRoots;
00262 
00263                 // roots r0 <= r1 <= r2
00264                 discr = sqrt(fabs(discr));
00265                 (*r1) = 0.5f*(-b1-discr);
00266                 (*r2) = 0.5f*(-b1+discr);
00267 
00268                 if ( fabs((*r0)-(*r1)) <= g_fEpsilon )
00269                 {
00270                     (*r0) = (*r1);
00271                     result |= floatRoot01;
00272                 }
00273                 if ( fabs((*r1)-(*r2)) <= g_fEpsilon )
00274                 {
00275                     (*r1) = (*r2);
00276                     result |= floatRoot12;
00277                 }
00278                 return result;
00279             }
00280         }
00281         else {
00282             // double root occurs after the negative local minimum
00283             (*r2) = temp + 1;  // initial guess for Newton's method
00284             pval = 2*g_fEpsilon;
00285             for (i = 0; i < maxiter && fabs(pval) > g_fEpsilon; i++) {
00286                 pval = (*r2)*((*r2)*((*r2)-c2)+c1)-c0;
00287                 pdval = (*r2)*(3*(*r2)-2*c2)+c1;
00288                 (*r2) -= pval/pdval;
00289             }
00290 
00291             // Other two roots are solutions to quadratic equation
00292             // L^2 + (r2-c2)*L + [r2*(r2-c2)+c1] = 0.
00293             b1 = (*r2)-c2;
00294             b0 = (*r2)*((*r2)-c2)+c1;
00295             discr = b1*b1-4*b0;
00296             if ( discr < -g_fEpsilon )
00297             {
00298                 // single root
00299                 (*r0) = (*r2);
00300                 return singleRoot;
00301             }
00302             else
00303             {
00304                 int result = distinctRoots;
00305 
00306                 // roots r0 <= r1 <= r2
00307                 discr = sqrt(fabs(discr));
00308                 (*r0) = 0.5f*(-b1-discr);
00309                 (*r1) = 0.5f*(-b1+discr);
00310 
00311                 if ( fabs((*r0)-(*r1)) <= g_fEpsilon )
00312                 {
00313                     (*r0) = (*r1);
00314                     result |= floatRoot01;
00315                 }
00316                 if ( fabs((*r1)-(*r2)) <= g_fEpsilon )
00317                 {
00318                     (*r1) = (*r2);
00319                     result |= floatRoot12;
00320                 }
00321                 return result;
00322             }
00323         }
00324     }
00325     else {
00326         // p(L) has one double root
00327         (*r0) = c0;
00328         pval = 2*g_fEpsilon;
00329         for (i = 0; i < maxiter && fabs(pval) > g_fEpsilon; i++) {
00330             pval = (*r0)*((*r0)*((*r0)-c2)+c1)-c0;
00331             pdval = (*r0)*(3*(*r0)-2*c2)+c1;
00332             (*r0) -= pval/pdval;
00333         }
00334         return singleRoot;
00335     }
00336 }
00337 
00338 //----------------------------------------------------------------------------
00339 template <class T>
00340 bool _QLAlgorithm3 (T* m_aafEntry, T* afDiag, T* afSubDiag)
00341 {
00342     // QL iteration with implicit shifting to reduce matrix from tridiagonal
00343     // to diagonal
00344 
00345     for (int i0 = 0; i0 < 3; i0++)
00346     {
00347         const int iMaxIter = 32;
00348         int iIter;
00349         for (iIter = 0; iIter < iMaxIter; iIter++)
00350         {
00351             int i1;
00352             for (i1 = i0; i1 <= 1; i1++)
00353             {
00354                 T fSum = fabs(afDiag[i1]) +
00355                          fabs(afDiag[i1+1]);
00356                 if ( fabs(afSubDiag[i1]) + fSum == fSum )
00357                     break;
00358             }
00359             if ( i1 == i0 )
00360                 break;
00361 
00362             T fTmp0 = (afDiag[i0+1]-afDiag[i0])/(2.0f*afSubDiag[i0]);
00363             T fTmp1 = sqrt(fTmp0*fTmp0+1.0f);
00364             if ( fTmp0 < 0.0f )
00365                 fTmp0 = afDiag[i1]-afDiag[i0]+afSubDiag[i0]/(fTmp0-fTmp1);
00366             else
00367                 fTmp0 = afDiag[i1]-afDiag[i0]+afSubDiag[i0]/(fTmp0+fTmp1);
00368             T fSin = 1.0f;
00369             T fCos = 1.0f;
00370             T fTmp2 = 0.0f;
00371             for (int i2 = i1-1; i2 >= i0; i2--)
00372             {
00373                 T fTmp3 = fSin*afSubDiag[i2];
00374                 T fTmp4 = fCos*afSubDiag[i2];
00375                 if ( fabs(fTmp3) >= fabs(fTmp0) )
00376                 {
00377                     fCos = fTmp0/fTmp3;
00378                     fTmp1 = sqrt(fCos*fCos+1.0f);
00379                     afSubDiag[i2+1] = fTmp3*fTmp1;
00380                     fSin = 1.0f/fTmp1;
00381                     fCos *= fSin;
00382                 }
00383                 else
00384                 {
00385                     fSin = fTmp3/fTmp0;
00386                     fTmp1 = sqrt(fSin*fSin+1.0f);
00387                     afSubDiag[i2+1] = fTmp0*fTmp1;
00388                     fCos = 1.0f/fTmp1;
00389                     fSin *= fCos;
00390                 }
00391                 fTmp0 = afDiag[i2+1]-fTmp2;
00392                 fTmp1 = (afDiag[i2]-fTmp0)*fSin+2.0f*fTmp4*fCos;
00393                 fTmp2 = fSin*fTmp1;
00394                 afDiag[i2+1] = fTmp0+fTmp2;
00395                 fTmp0 = fCos*fTmp1-fTmp4;
00396 
00397                 for (int iRow = 0; iRow < 3; iRow++)
00398                 {
00399                     fTmp3 = m_aafEntry[iRow*3+i2+1];
00400                     m_aafEntry[iRow*3+i2+1] = fSin*m_aafEntry[iRow*3+i2] +
00401                                               fCos*fTmp3;
00402                     m_aafEntry[iRow*3+i2] = fCos*m_aafEntry[iRow*3+i2] -
00403                                             fSin*fTmp3;
00404                 }
00405             }
00406             afDiag[i0] -= fTmp2;
00407             afSubDiag[i0] = fTmp0;
00408             afSubDiag[i1] = 0.0f;
00409         }
00410 
00411         if ( iIter == iMaxIter )
00412         {
00413             // should not get here under normal circumstances
00414             return false;
00415         }
00416     }
00417 
00418     return true;
00419 }
00420 
00421 bool QLAlgorithm3 (float* m_aafEntry, float* afDiag, float* afSubDiag)
00422 {
00423     return _QLAlgorithm3<float>(m_aafEntry, afDiag, afSubDiag);
00424 }
00425 
00426 bool QLAlgorithm3 (double* m_aafEntry, double* afDiag, double* afSubDiag)
00427 {
00428     return _QLAlgorithm3<double>(m_aafEntry, afDiag, afSubDiag);
00429 }
00430 
00431 void EigenSymmetric3(const double* fmat, double* afEigenvalue, double* fevecs)
00432 {
00433     double afSubDiag[3];
00434 
00435     memcpy(fevecs, fmat, sizeof(double)*9);
00436     Tridiagonal3(fevecs, afEigenvalue,afSubDiag);
00437     QLAlgorithm3(fevecs, afEigenvalue,afSubDiag);
00438 
00439     // make eigenvectors form a right--handed system
00440     double fDet = fevecs[0*3+0] * (fevecs[1*3+1] * fevecs[2*3+2] - fevecs[1*3+2] * fevecs[2*3+1]) +
00441                   fevecs[0*3+1] * (fevecs[1*3+2] * fevecs[2*3+0] - fevecs[1*3+0] * fevecs[2*3+2]) +
00442                   fevecs[0*3+2] * (fevecs[1*3+0] * fevecs[2*3+1] - fevecs[1*3+1] * fevecs[2*3+0]);
00443     if ( fDet < 0.0f )
00444     {
00445         fevecs[0*3+2] = -fevecs[0*3+2];
00446         fevecs[1*3+2] = -fevecs[1*3+2];
00447         fevecs[2*3+2] = -fevecs[2*3+2];
00448     }
00449 }
00450 /* end of MAGIC code */
00451 
00452 } // end namespace geometry
00453 
00455 class ResourceIOSystem : public Assimp::IOSystem
00456 {
00457 public:
00458     ResourceIOSystem()
00459     {
00460     }
00461 
00462     ~ResourceIOSystem()
00463     {
00464     }
00465 
00466     // Check whether a specific file exists
00467     bool Exists(const char* file) const
00468     {
00469         // Ugly -- two retrievals where there should be one (Exists + Open)
00470         // resource_retriever needs a way of checking for existence
00471         // TODO: cache this
00472         resource_retriever::MemoryResource res;
00473         try {
00474             res = retriever_.get(file);
00475         }
00476         catch (resource_retriever::Exception& e) {
00477             return false;
00478         }
00479 
00480         return true;
00481     }
00482 
00483     // Get the path delimiter character we'd like to see
00484     char getOsSeparator() const
00485     {
00486         return '/';
00487     }
00488 
00489     // ... and finally a method to open a custom stream
00490     Assimp::IOStream* Open(const char* file, const char* mode)
00491     {
00492         ROS_ASSERT(mode == std::string("r") || mode == std::string("rb"));
00493 
00494         // Ugly -- two retrievals where there should be one (Exists + Open)
00495         // resource_retriever needs a way of checking for existence
00496         resource_retriever::MemoryResource res;
00497         try {
00498             res = retriever_.get(file);
00499         }
00500         catch (resource_retriever::Exception& e) {
00501             return 0;
00502         }
00503 
00504         return new ResourceIOStream(res);
00505     }
00506 
00507     void Close(Assimp::IOStream* stream) {
00508         delete stream;
00509     }
00510 
00511 private:
00512     mutable resource_retriever::Retriever retriever_;
00513 };
00514 
00515 class Triangle
00516 {
00517 public:
00518     Triangle(const urdf::Vector3 &_p1, const urdf::Vector3 &_p2, const urdf::Vector3 &_p3) :
00519       p1(_p1), p2(_p2), p3(_p3)
00520     {}
00521     Triangle() { this->clear(); };
00522     urdf::Vector3 p1, p2, p3;
00523 
00524     void clear() { p1.clear(); p2.clear(); p3.clear(); };
00525 };
00526 
00528 class ColladaWriter : public daeErrorHandler
00529 {
00530 private:
00531     struct SCENE
00532     {
00533         domVisual_sceneRef vscene;
00534         domKinematics_sceneRef kscene;
00535         domPhysics_sceneRef pscene;
00536         domInstance_with_extraRef viscene;
00537         domInstance_kinematics_sceneRef kiscene;
00538         domInstance_with_extraRef piscene;
00539     };
00540 
00541     typedef std::map< urdf::LinkConstSharedPtr, urdf::Pose > MAPLINKPOSES;
00542     struct LINKOUTPUT
00543     {
00544         list<pair<int,string> > listusedlinks;
00545         list<pair<int,string> > listprocesseddofs;
00546         daeElementRef plink;
00547         domNodeRef pnode;
00548         MAPLINKPOSES _maplinkposes;
00549     };
00550 
00551     struct physics_model_output
00552     {
00553         domPhysics_modelRef pmodel;
00554         std::vector<std::string > vrigidbodysids;     
00555     };
00556 
00557     struct kinematics_model_output
00558     {
00559         struct axis_output
00560         {
00561             //axis_output(const string& sid, KinBody::JointConstPtr pjoint, int iaxis) : sid(sid), pjoint(pjoint), iaxis(iaxis) {}
00562             axis_output() : iaxis(0) {
00563             }
00564             string sid, nodesid;
00565             urdf::JointConstSharedPtr pjoint;
00566             int iaxis;
00567             string jointnodesid;
00568         };
00569         domKinematics_modelRef kmodel;
00570         std::vector<axis_output> vaxissids;
00571         std::vector<std::string > vlinksids;
00572         MAPLINKPOSES _maplinkposes;
00573     };
00574 
00575     struct axis_sids
00576     {
00577         axis_sids(const string& axissid, const string& valuesid, const string& jointnodesid) : axissid(axissid), valuesid(valuesid), jointnodesid(jointnodesid) {
00578         }
00579         string axissid, valuesid, jointnodesid;
00580     };
00581 
00582     struct instance_kinematics_model_output
00583     {
00584         domInstance_kinematics_modelRef ikm;
00585         std::vector<axis_sids> vaxissids;
00586         boost::shared_ptr<kinematics_model_output> kmout;
00587         std::vector<std::pair<std::string,std::string> > vkinematicsbindings;
00588     };
00589 
00590     struct instance_articulated_system_output
00591     {
00592         domInstance_articulated_systemRef ias;
00593         std::vector<axis_sids> vaxissids;
00594         std::vector<std::string > vlinksids;
00595         std::vector<std::pair<std::string,std::string> > vkinematicsbindings;
00596     };
00597 
00598     struct instance_physics_model_output
00599     {
00600         domInstance_physics_modelRef ipm;
00601         boost::shared_ptr<physics_model_output> pmout;
00602     };
00603 
00604     struct kinbody_models
00605     {
00606         std::string uri, kinematicsgeometryhash;
00607         boost::shared_ptr<kinematics_model_output> kmout;
00608         boost::shared_ptr<physics_model_output> pmout;
00609     };
00610 
00611 public:
00612     ColladaWriter(const urdf::Model& robot, int writeoptions) : _writeoptions(writeoptions), _robot(robot), _dom(NULL) {
00613         daeErrorHandler::setErrorHandler(this);
00614         _importer.SetIOHandler(new ResourceIOSystem());
00615     }
00616     virtual ~ColladaWriter() {
00617     }
00618 
00619     daeDocument* doc() {
00620         return _doc;
00621     }
00622 
00623     bool convert()
00624     {
00625         try {
00626             const char* documentName = "urdf_snapshot";
00627             daeInt error = _collada.getDatabase()->insertDocument(documentName, &_doc ); // also creates a collada root
00628             if (error != DAE_OK || _doc == NULL) {
00629                 throw ColladaUrdfException("Failed to create document");
00630             }
00631             _dom = daeSafeCast<domCOLLADA>(_doc->getDomRoot());
00632             _dom->setAttribute("xmlns:math","http://www.w3.org/1998/Math/MathML");
00633 
00634             //create the required asset tag
00635             domAssetRef asset = daeSafeCast<domAsset>( _dom->add( COLLADA_ELEMENT_ASSET ) );
00636             {
00637                 // facet becomes owned by locale, so no need to explicitly delete
00638                 boost::posix_time::time_facet* facet = new boost::posix_time::time_facet("%Y-%m-%dT%H:%M:%s");
00639                 std::stringstream ss;
00640                 ss.imbue(std::locale(ss.getloc(), facet));
00641                 ss << boost::posix_time::second_clock::local_time();
00642 
00643                 domAsset::domCreatedRef created = daeSafeCast<domAsset::domCreated>( asset->add( COLLADA_ELEMENT_CREATED ) );
00644                 created->setValue(ss.str().c_str());
00645                 domAsset::domModifiedRef modified = daeSafeCast<domAsset::domModified>( asset->add( COLLADA_ELEMENT_MODIFIED ) );
00646                 modified->setValue(ss.str().c_str());
00647 
00648                 domAsset::domContributorRef contrib = daeSafeCast<domAsset::domContributor>( asset->add( COLLADA_TYPE_CONTRIBUTOR ) );
00649                 domAsset::domContributor::domAuthoring_toolRef authoringtool = daeSafeCast<domAsset::domContributor::domAuthoring_tool>( contrib->add( COLLADA_ELEMENT_AUTHORING_TOOL ) );
00650                 authoringtool->setValue("URDF Collada Writer");
00651 
00652                 domAsset::domUnitRef units = daeSafeCast<domAsset::domUnit>( asset->add( COLLADA_ELEMENT_UNIT ) );
00653                 units->setMeter(1);
00654                 units->setName("meter");
00655 
00656                 domAsset::domUp_axisRef zup = daeSafeCast<domAsset::domUp_axis>( asset->add( COLLADA_ELEMENT_UP_AXIS ) );
00657                 zup->setValue(UP_AXIS_Z_UP);
00658             }
00659 
00660             _globalscene = _dom->getScene();
00661             if( !_globalscene ) {
00662                 _globalscene = daeSafeCast<domCOLLADA::domScene>( _dom->add( COLLADA_ELEMENT_SCENE ) );
00663             }
00664 
00665             _visualScenesLib = daeSafeCast<domLibrary_visual_scenes>(_dom->add(COLLADA_ELEMENT_LIBRARY_VISUAL_SCENES));
00666             _visualScenesLib->setId("vscenes");
00667             _geometriesLib = daeSafeCast<domLibrary_geometries>(_dom->add(COLLADA_ELEMENT_LIBRARY_GEOMETRIES));
00668             _geometriesLib->setId("geometries");
00669             _effectsLib = daeSafeCast<domLibrary_effects>(_dom->add(COLLADA_ELEMENT_LIBRARY_EFFECTS));
00670             _effectsLib->setId("effects");
00671             _materialsLib = daeSafeCast<domLibrary_materials>(_dom->add(COLLADA_ELEMENT_LIBRARY_MATERIALS));
00672             _materialsLib->setId("materials");
00673             _kinematicsModelsLib = daeSafeCast<domLibrary_kinematics_models>(_dom->add(COLLADA_ELEMENT_LIBRARY_KINEMATICS_MODELS));
00674             _kinematicsModelsLib->setId("kmodels");
00675             _articulatedSystemsLib = daeSafeCast<domLibrary_articulated_systems>(_dom->add(COLLADA_ELEMENT_LIBRARY_ARTICULATED_SYSTEMS));
00676             _articulatedSystemsLib->setId("asystems");
00677             _kinematicsScenesLib = daeSafeCast<domLibrary_kinematics_scenes>(_dom->add(COLLADA_ELEMENT_LIBRARY_KINEMATICS_SCENES));
00678             _kinematicsScenesLib->setId("kscenes");
00679             _physicsScenesLib = daeSafeCast<domLibrary_physics_scenes>(_dom->add(COLLADA_ELEMENT_LIBRARY_PHYSICS_SCENES));
00680             _physicsScenesLib->setId("pscenes");
00681             _physicsModelsLib = daeSafeCast<domLibrary_physics_models>(_dom->add(COLLADA_ELEMENT_LIBRARY_PHYSICS_MODELS));
00682             _physicsModelsLib->setId("pmodels");
00683             domExtraRef pextra_library_sensors = daeSafeCast<domExtra>(_dom->add(COLLADA_ELEMENT_EXTRA));
00684             pextra_library_sensors->setId("sensors");
00685             pextra_library_sensors->setType("library_sensors");
00686             _sensorsLib = daeSafeCast<domTechnique>(pextra_library_sensors->add(COLLADA_ELEMENT_TECHNIQUE));
00687             _sensorsLib->setProfile("OpenRAVE"); 
00688 
00689             _CreateScene();
00690             _WritePhysics();
00691             _WriteRobot();
00692             _WriteBindingsInstance_kinematics_scene();
00693             return true;
00694         }
00695         catch (ColladaUrdfException ex) {
00696             ROS_ERROR("Error converting: %s", ex.what());
00697             return false;
00698         }
00699     }
00700 
00701     bool writeTo(string const& file) {
00702         try {
00703             daeString uri = _doc->getDocumentURI()->getURI();
00704             _collada.writeTo(uri, file);
00705         } catch (ColladaUrdfException ex) {
00706             return false;
00707         }
00708         return true;
00709     }
00710 
00711 protected:
00712     virtual void handleError(daeString msg) {
00713         throw ColladaUrdfException(msg);
00714     }
00715     virtual void handleWarning(daeString msg) {
00716         std::cerr << "COLLADA DOM warning: " << msg << std::endl;
00717     }
00718 
00719     void _CreateScene()
00720     {
00721         // Create visual scene
00722         _scene.vscene = daeSafeCast<domVisual_scene>(_visualScenesLib->add(COLLADA_ELEMENT_VISUAL_SCENE));
00723         _scene.vscene->setId("vscene");
00724         _scene.vscene->setName("URDF Visual Scene");
00725 
00726         // Create kinematics scene
00727         _scene.kscene = daeSafeCast<domKinematics_scene>(_kinematicsScenesLib->add(COLLADA_ELEMENT_KINEMATICS_SCENE));
00728         _scene.kscene->setId("kscene");
00729         _scene.kscene->setName("URDF Kinematics Scene");
00730 
00731         // Create physic scene
00732         _scene.pscene = daeSafeCast<domPhysics_scene>(_physicsScenesLib->add(COLLADA_ELEMENT_PHYSICS_SCENE));
00733         _scene.pscene->setId("pscene");
00734         _scene.pscene->setName("URDF Physics Scene");
00735 
00736         // Create instance visual scene
00737         _scene.viscene = daeSafeCast<domInstance_with_extra>(_globalscene->add( COLLADA_ELEMENT_INSTANCE_VISUAL_SCENE ));
00738         _scene.viscene->setUrl( (string("#") + string(_scene.vscene->getID())).c_str() );
00739 
00740         // Create instance kinematics scene
00741         _scene.kiscene = daeSafeCast<domInstance_kinematics_scene>(_globalscene->add( COLLADA_ELEMENT_INSTANCE_KINEMATICS_SCENE ));
00742         _scene.kiscene->setUrl( (string("#") + string(_scene.kscene->getID())).c_str() );
00743 
00744         // Create instance physics scene
00745         _scene.piscene = daeSafeCast<domInstance_with_extra>(_globalscene->add( COLLADA_ELEMENT_INSTANCE_PHYSICS_SCENE ));
00746         _scene.piscene->setUrl( (string("#") + string(_scene.pscene->getID())).c_str() );
00747     }
00748 
00749     void _WritePhysics() {
00750         domPhysics_scene::domTechnique_commonRef common = daeSafeCast<domPhysics_scene::domTechnique_common>(_scene.pscene->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
00751         //  Create gravity
00752         domTargetable_float3Ref g = daeSafeCast<domTargetable_float3>(common->add(COLLADA_ELEMENT_GRAVITY));
00753         g->getValue().set3 (0,0,0);
00754     }
00755 
00757     void _WriteRobot(int id = 0)
00758     {
00759         ROS_DEBUG_STREAM(str(boost::format("writing robot as instance_articulated_system (%d) %s\n")%id%_robot.getName()));
00760         string asid = _ComputeId(str(boost::format("robot%d")%id));
00761         string askid = _ComputeId(str(boost::format("%s_kinematics")%asid));
00762         string asmid = _ComputeId(str(boost::format("%s_motion")%asid));
00763         string iassid = _ComputeId(str(boost::format("%s_inst")%asmid));
00764 
00765         domInstance_articulated_systemRef ias = daeSafeCast<domInstance_articulated_system>(_scene.kscene->add(COLLADA_ELEMENT_INSTANCE_ARTICULATED_SYSTEM));
00766         ias->setSid(iassid.c_str());
00767         ias->setUrl((string("#")+asmid).c_str());
00768         ias->setName(_robot.getName().c_str());
00769 
00770         _iasout.reset(new instance_articulated_system_output());
00771         _iasout->ias = ias;
00772 
00773         // motion info
00774         domArticulated_systemRef articulated_system_motion = daeSafeCast<domArticulated_system>(_articulatedSystemsLib->add(COLLADA_ELEMENT_ARTICULATED_SYSTEM));
00775         articulated_system_motion->setId(asmid.c_str());
00776         domMotionRef motion = daeSafeCast<domMotion>(articulated_system_motion->add(COLLADA_ELEMENT_MOTION));
00777         domMotion_techniqueRef mt = daeSafeCast<domMotion_technique>(motion->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
00778         domInstance_articulated_systemRef ias_motion = daeSafeCast<domInstance_articulated_system>(motion->add(COLLADA_ELEMENT_INSTANCE_ARTICULATED_SYSTEM));
00779         ias_motion->setUrl(str(boost::format("#%s")%askid).c_str());
00780 
00781         // kinematics info
00782         domArticulated_systemRef articulated_system_kinematics = daeSafeCast<domArticulated_system>(_articulatedSystemsLib->add(COLLADA_ELEMENT_ARTICULATED_SYSTEM));
00783         articulated_system_kinematics->setId(askid.c_str());
00784         domKinematicsRef kinematics = daeSafeCast<domKinematics>(articulated_system_kinematics->add(COLLADA_ELEMENT_KINEMATICS));
00785         domKinematics_techniqueRef kt = daeSafeCast<domKinematics_technique>(kinematics->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
00786 
00787         _WriteInstance_kinematics_model(kinematics,askid,id);
00788 
00789         for(size_t idof = 0; idof < _ikmout->vaxissids.size(); ++idof) {
00790             string axis_infosid = _ComputeId(str(boost::format("axis_info_inst%d")%idof));
00791             urdf::JointConstSharedPtr pjoint = _ikmout->kmout->vaxissids.at(idof).pjoint;
00792             BOOST_ASSERT(_mapjointindices[pjoint] == (int)idof);
00793             //int iaxis = _ikmout->kmout->vaxissids.at(idof).iaxis;
00794 
00795             //  Kinematics axis info
00796             domKinematics_axis_infoRef kai = daeSafeCast<domKinematics_axis_info>(kt->add(COLLADA_ELEMENT_AXIS_INFO));
00797             kai->setAxis(str(boost::format("%s/%s")%_ikmout->kmout->kmodel->getID()%_ikmout->kmout->vaxissids.at(idof).sid).c_str());
00798             kai->setSid(axis_infosid.c_str());
00799             bool bactive = !pjoint->mimic;
00800             double flower=0, fupper=0;
00801             if( pjoint->type != urdf::Joint::CONTINUOUS ) {
00802                 if( !!pjoint->limits ) {
00803                     flower = pjoint->limits->lower;
00804                     fupper = pjoint->limits->upper;
00805                 }
00806                 if( !!pjoint->safety ) {
00807                     flower = pjoint->safety->soft_lower_limit;
00808                     fupper = pjoint->safety->soft_upper_limit;
00809                 }
00810                 if( flower == fupper ) {
00811                     bactive = false;
00812                 }
00813                 double fmult = 1.0;
00814                 if( pjoint->type != urdf::Joint::PRISMATIC ) {
00815                     fmult = 180.0/M_PI;
00816                 }
00817                 domKinematics_limitsRef plimits = daeSafeCast<domKinematics_limits>(kai->add(COLLADA_ELEMENT_LIMITS));
00818                 daeSafeCast<domCommon_float_or_param::domFloat>(plimits->add(COLLADA_ELEMENT_MIN)->add(COLLADA_ELEMENT_FLOAT))->setValue(flower*fmult);
00819                 daeSafeCast<domCommon_float_or_param::domFloat>(plimits->add(COLLADA_ELEMENT_MAX)->add(COLLADA_ELEMENT_FLOAT))->setValue(fupper*fmult);
00820             }
00821 
00822             domCommon_bool_or_paramRef active = daeSafeCast<domCommon_bool_or_param>(kai->add(COLLADA_ELEMENT_ACTIVE));
00823             daeSafeCast<domCommon_bool_or_param::domBool>(active->add(COLLADA_ELEMENT_BOOL))->setValue(bactive);
00824             domCommon_bool_or_paramRef locked = daeSafeCast<domCommon_bool_or_param>(kai->add(COLLADA_ELEMENT_LOCKED));
00825             daeSafeCast<domCommon_bool_or_param::domBool>(locked->add(COLLADA_ELEMENT_BOOL))->setValue(false);
00826 
00827             //  Motion axis info
00828             domMotion_axis_infoRef mai = daeSafeCast<domMotion_axis_info>(mt->add(COLLADA_ELEMENT_AXIS_INFO));
00829             mai->setAxis(str(boost::format("%s/%s")%askid%axis_infosid).c_str());
00830             if( !!pjoint->limits ) {
00831                 domCommon_float_or_paramRef speed = daeSafeCast<domCommon_float_or_param>(mai->add(COLLADA_ELEMENT_SPEED));
00832                 daeSafeCast<domCommon_float_or_param::domFloat>(speed->add(COLLADA_ELEMENT_FLOAT))->setValue(pjoint->limits->velocity);
00833                 domCommon_float_or_paramRef accel = daeSafeCast<domCommon_float_or_param>(mai->add(COLLADA_ELEMENT_ACCELERATION));
00834                 daeSafeCast<domCommon_float_or_param::domFloat>(accel->add(COLLADA_ELEMENT_FLOAT))->setValue(pjoint->limits->effort);
00835             }
00836         }
00837 
00838         // write the bindings
00839         string asmsym = _ComputeId(str(boost::format("%s_%s")%asmid%_ikmout->ikm->getSid()));
00840         string assym = _ComputeId(str(boost::format("%s_%s")%_scene.kscene->getID()%_ikmout->ikm->getSid()));
00841         FOREACH(it, _ikmout->vkinematicsbindings) {
00842             domKinematics_newparamRef abm = daeSafeCast<domKinematics_newparam>(ias_motion->add(COLLADA_ELEMENT_NEWPARAM));
00843             abm->setSid(asmsym.c_str());
00844             daeSafeCast<domKinematics_newparam::domSIDREF>(abm->add(COLLADA_ELEMENT_SIDREF))->setValue(str(boost::format("%s/%s")%askid%it->first).c_str());
00845             domKinematics_bindRef ab = daeSafeCast<domKinematics_bind>(ias->add(COLLADA_ELEMENT_BIND));
00846             ab->setSymbol(assym.c_str());
00847             daeSafeCast<domKinematics_param>(ab->add(COLLADA_ELEMENT_PARAM))->setRef(str(boost::format("%s/%s")%asmid%asmsym).c_str());
00848             _iasout->vkinematicsbindings.push_back(make_pair(string(ab->getSymbol()), it->second));
00849         }
00850         for(size_t idof = 0; idof < _ikmout->vaxissids.size(); ++idof) {
00851             const axis_sids& kas = _ikmout->vaxissids.at(idof);
00852             domKinematics_newparamRef abm = daeSafeCast<domKinematics_newparam>(ias_motion->add(COLLADA_ELEMENT_NEWPARAM));
00853             abm->setSid(_ComputeId(str(boost::format("%s_%s")%asmid%kas.axissid)).c_str());
00854             daeSafeCast<domKinematics_newparam::domSIDREF>(abm->add(COLLADA_ELEMENT_SIDREF))->setValue(str(boost::format("%s/%s")%askid%kas.axissid).c_str());
00855             domKinematics_bindRef ab = daeSafeCast<domKinematics_bind>(ias->add(COLLADA_ELEMENT_BIND));
00856             ab->setSymbol(str(boost::format("%s_%s")%assym%kas.axissid).c_str());
00857             daeSafeCast<domKinematics_param>(ab->add(COLLADA_ELEMENT_PARAM))->setRef(str(boost::format("%s/%s_%s")%asmid%asmid%kas.axissid).c_str());
00858             string valuesid;
00859             if( kas.valuesid.size() > 0 ) {
00860                 domKinematics_newparamRef abmvalue = daeSafeCast<domKinematics_newparam>(ias_motion->add(COLLADA_ELEMENT_NEWPARAM));
00861                 abmvalue->setSid(_ComputeId(str(boost::format("%s_%s")%asmid%kas.valuesid)).c_str());
00862                 daeSafeCast<domKinematics_newparam::domSIDREF>(abmvalue->add(COLLADA_ELEMENT_SIDREF))->setValue(str(boost::format("%s/%s")%askid%kas.valuesid).c_str());
00863                 domKinematics_bindRef abvalue = daeSafeCast<domKinematics_bind>(ias->add(COLLADA_ELEMENT_BIND));
00864                 valuesid = _ComputeId(str(boost::format("%s_%s")%assym%kas.valuesid));
00865                 abvalue->setSymbol(valuesid.c_str());
00866                 daeSafeCast<domKinematics_param>(abvalue->add(COLLADA_ELEMENT_PARAM))->setRef(str(boost::format("%s/%s_%s")%asmid%asmid%kas.valuesid).c_str());
00867             }
00868             _iasout->vaxissids.push_back(axis_sids(ab->getSymbol(),valuesid,kas.jointnodesid));
00869         }
00870 
00871         boost::shared_ptr<instance_physics_model_output> ipmout = _WriteInstance_physics_model(id,_scene.pscene,_scene.pscene->getID(), _ikmout->kmout->_maplinkposes);
00872     }
00873 
00875     virtual void _WriteInstance_kinematics_model(daeElementRef parent, const string& sidscope, int id)
00876     {
00877         ROS_DEBUG_STREAM(str(boost::format("writing instance_kinematics_model %s\n")%_robot.getName()));
00878         boost::shared_ptr<kinematics_model_output> kmout = WriteKinematics_model(id);
00879 
00880         _ikmout.reset(new instance_kinematics_model_output());
00881         _ikmout->kmout = kmout;
00882         _ikmout->ikm = daeSafeCast<domInstance_kinematics_model>(parent->add(COLLADA_ELEMENT_INSTANCE_KINEMATICS_MODEL));
00883 
00884         string symscope, refscope;
00885         if( sidscope.size() > 0 ) {
00886             symscope = sidscope+string("_");
00887             refscope = sidscope+string("/");
00888         }
00889         string ikmsid = _ComputeId(str(boost::format("%s_inst")%kmout->kmodel->getID()));
00890         _ikmout->ikm->setUrl(str(boost::format("#%s")%kmout->kmodel->getID()).c_str());
00891         _ikmout->ikm->setSid(ikmsid.c_str());
00892 
00893         domKinematics_newparamRef kbind = daeSafeCast<domKinematics_newparam>(_ikmout->ikm->add(COLLADA_ELEMENT_NEWPARAM));
00894         kbind->setSid(_ComputeId(symscope+ikmsid).c_str());
00895         daeSafeCast<domKinematics_newparam::domSIDREF>(kbind->add(COLLADA_ELEMENT_SIDREF))->setValue((refscope+ikmsid).c_str());
00896         _ikmout->vkinematicsbindings.push_back(make_pair(string(kbind->getSid()), str(boost::format("visual%d/node%d")%id%_maplinkindices[_robot.getRoot()])));
00897 
00898         _ikmout->vaxissids.reserve(kmout->vaxissids.size());
00899         int i = 0;
00900         FOREACH(it,kmout->vaxissids) {
00901             domKinematics_newparamRef kbind = daeSafeCast<domKinematics_newparam>(_ikmout->ikm->add(COLLADA_ELEMENT_NEWPARAM));
00902             string ref = it->sid;
00903             size_t index = ref.find("/");
00904             while(index != string::npos) {
00905                 ref[index] = '.';
00906                 index = ref.find("/",index+1);
00907             }
00908             string sid = _ComputeId(symscope+ikmsid+"_"+ref);
00909             kbind->setSid(sid.c_str());
00910             daeSafeCast<domKinematics_newparam::domSIDREF>(kbind->add(COLLADA_ELEMENT_SIDREF))->setValue((refscope+ikmsid+"/"+it->sid).c_str());
00911             double value=0;
00912             double flower=0, fupper=0;
00913             if( !!it->pjoint->limits ) {
00914                 flower = it->pjoint->limits->lower;
00915                 fupper = it->pjoint->limits->upper;
00916             }
00917             if( flower > 0 || fupper < 0 ) {
00918                 value = 0.5*(flower+fupper);
00919             }
00920             domKinematics_newparamRef pvalueparam = daeSafeCast<domKinematics_newparam>(_ikmout->ikm->add(COLLADA_ELEMENT_NEWPARAM));
00921             pvalueparam->setSid((sid+string("_value")).c_str());
00922             daeSafeCast<domKinematics_newparam::domFloat>(pvalueparam->add(COLLADA_ELEMENT_FLOAT))->setValue(value);
00923             _ikmout->vaxissids.push_back(axis_sids(sid,pvalueparam->getSid(),kmout->vaxissids.at(i).jointnodesid));
00924             ++i;
00925         }
00926     }
00927 
00928     virtual boost::shared_ptr<kinematics_model_output> WriteKinematics_model(int id)
00929     {
00930         domKinematics_modelRef kmodel = daeSafeCast<domKinematics_model>(_kinematicsModelsLib->add(COLLADA_ELEMENT_KINEMATICS_MODEL));
00931         string kmodelid = _ComputeKinematics_modelId(id);
00932         kmodel->setId(kmodelid.c_str());
00933         kmodel->setName(_robot.getName().c_str());
00934 
00935         domKinematics_model_techniqueRef ktec = daeSafeCast<domKinematics_model_technique>(kmodel->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
00936 
00937         //  Create root node for the visual scene
00938         domNodeRef pnoderoot = daeSafeCast<domNode>(_scene.vscene->add(COLLADA_ELEMENT_NODE));
00939         string bodyid = _ComputeId(str(boost::format("visual%d")%id));
00940         pnoderoot->setId(bodyid.c_str());
00941         pnoderoot->setSid(bodyid.c_str());
00942         pnoderoot->setName(_robot.getName().c_str());
00943 
00944         //  Declare all the joints
00945         _mapjointindices.clear();
00946         int index=0;
00947         FOREACHC(itj, _robot.joints_) {
00948             _mapjointindices[itj->second] = index++;
00949         }
00950         _maplinkindices.clear();
00951         index=0;
00952         FOREACHC(itj, _robot.links_) {
00953             _maplinkindices[itj->second] = index++;
00954         }
00955         _mapmaterialindices.clear();
00956         index=0;
00957         FOREACHC(itj, _robot.materials_) {
00958             _mapmaterialindices[itj->second] = index++;
00959         }
00960 
00961         double lmin, lmax;
00962         vector<domJointRef> vdomjoints(_robot.joints_.size());
00963         boost::shared_ptr<kinematics_model_output> kmout(new kinematics_model_output());
00964         kmout->kmodel = kmodel;
00965         kmout->vaxissids.resize(_robot.joints_.size());
00966         kmout->vlinksids.resize(_robot.links_.size());
00967 
00968         FOREACHC(itjoint, _robot.joints_) {
00969             urdf::JointSharedPtr pjoint = itjoint->second;
00970             int index = _mapjointindices[itjoint->second];
00971             domJointRef pdomjoint = daeSafeCast<domJoint>(ktec->add(COLLADA_ELEMENT_JOINT));
00972             string jointid = _ComputeId(pjoint->name); //str(boost::format("joint%d")%index);
00973             pdomjoint->setSid(jointid.c_str() );
00974             pdomjoint->setName(pjoint->name.c_str());
00975             domAxis_constraintRef axis;
00976             if( !!pjoint->limits ) {
00977                 lmin=pjoint->limits->lower;
00978                 lmax=pjoint->limits->upper;
00979             }
00980             else {
00981                 lmin = lmax = 0;
00982             }
00983 
00984             double fmult = 1.0;
00985             switch(pjoint->type) {
00986             case urdf::Joint::REVOLUTE:
00987             case urdf::Joint::CONTINUOUS:
00988                 axis = daeSafeCast<domAxis_constraint>(pdomjoint->add(COLLADA_ELEMENT_REVOLUTE));
00989                 fmult = 180.0f/M_PI;
00990                 lmin*=fmult;
00991                 lmax*=fmult;
00992                 break;
00993             case urdf::Joint::PRISMATIC:
00994                 axis = daeSafeCast<domAxis_constraint>(pdomjoint->add(COLLADA_ELEMENT_PRISMATIC));
00995                 break;
00996             case urdf::Joint::FIXED:
00997                 axis = daeSafeCast<domAxis_constraint>(pdomjoint->add(COLLADA_ELEMENT_REVOLUTE));
00998                 lmin = 0;
00999                 lmax = 0;
01000                 fmult = 0;
01001                 break;
01002             default:
01003                 ROS_WARN_STREAM(str(boost::format("unsupported joint type specified %d")%(int)pjoint->type));
01004                 break;
01005             }
01006 
01007             if( !axis ) {
01008                 continue;
01009             }
01010 
01011             int ia = 0;
01012             string axisid = _ComputeId(str(boost::format("axis%d")%ia));
01013             axis->setSid(axisid.c_str());
01014             kmout->vaxissids.at(index).pjoint = pjoint;
01015             kmout->vaxissids.at(index).sid = jointid+string("/")+axisid;
01016             kmout->vaxissids.at(index).iaxis = ia;
01017             domAxisRef paxis = daeSafeCast<domAxis>(axis->add(COLLADA_ELEMENT_AXIS));
01018             paxis->getValue().setCount(3);
01019             paxis->getValue()[0] = pjoint->axis.x;
01020             paxis->getValue()[1] = pjoint->axis.y;
01021             paxis->getValue()[2] = pjoint->axis.z;
01022             if( pjoint->type != urdf::Joint::CONTINUOUS ) {
01023                 domJoint_limitsRef plimits = daeSafeCast<domJoint_limits>(axis->add(COLLADA_TYPE_LIMITS));
01024                 daeSafeCast<domMinmax>(plimits->add(COLLADA_ELEMENT_MIN))->getValue() = lmin;
01025                 daeSafeCast<domMinmax>(plimits->add(COLLADA_ELEMENT_MAX))->getValue() = lmax;
01026             }
01027             vdomjoints.at(index) = pdomjoint;
01028         }
01029 
01030         LINKOUTPUT childinfo = _WriteLink(_robot.getRoot(), ktec, pnoderoot, kmodel->getID());
01031         FOREACHC(itused, childinfo.listusedlinks) {
01032             kmout->vlinksids.at(itused->first) = itused->second;
01033         }
01034         FOREACH(itprocessed,childinfo.listprocesseddofs) {
01035             kmout->vaxissids.at(itprocessed->first).jointnodesid = itprocessed->second;
01036         }
01037         kmout->_maplinkposes = childinfo._maplinkposes;
01038 
01039         // create the formulas for all mimic joints
01040         FOREACHC(itjoint, _robot.joints_) {
01041             string jointsid = _ComputeId(itjoint->second->name);
01042             urdf::JointSharedPtr pjoint = itjoint->second;
01043             if( !pjoint->mimic ) {
01044                 continue;
01045             }
01046 
01047             domFormulaRef pf = daeSafeCast<domFormula>(ktec->add(COLLADA_ELEMENT_FORMULA));
01048             string formulaid = _ComputeId(str(boost::format("%s_formula")%jointsid));
01049             pf->setSid(formulaid.c_str());
01050             domCommon_float_or_paramRef ptarget = daeSafeCast<domCommon_float_or_param>(pf->add(COLLADA_ELEMENT_TARGET));
01051             string targetjointid = str(boost::format("%s/%s")%kmodel->getID()%jointsid);
01052             daeSafeCast<domCommon_param>(ptarget->add(COLLADA_TYPE_PARAM))->setValue(targetjointid.c_str());
01053 
01054             domTechniqueRef pftec = daeSafeCast<domTechnique>(pf->add(COLLADA_ELEMENT_TECHNIQUE));
01055             pftec->setProfile("OpenRAVE");
01056             // save position equation
01057             {
01058                 daeElementRef poselt = pftec->add("equation");
01059                 poselt->setAttribute("type","position");
01060                 // create a const0*joint+const1 formula
01061                 // <apply> <plus/> <apply> <times/> <cn>a</cn> x </apply> <cn>b</cn> </apply>
01062                 daeElementRef pmath_math = poselt->add("math");
01063                 daeElementRef pmath_apply = pmath_math->add("apply");
01064                 {
01065                     daeElementRef pmath_plus = pmath_apply->add("plus");
01066                     daeElementRef pmath_apply1 = pmath_apply->add("apply");
01067                     {
01068                         daeElementRef pmath_times = pmath_apply1->add("times");
01069                         daeElementRef pmath_const0 = pmath_apply1->add("cn");
01070                         pmath_const0->setCharData(str(boost::format("%f")%pjoint->mimic->multiplier));
01071                         daeElementRef pmath_symb = pmath_apply1->add("csymbol");
01072                         pmath_symb->setAttribute("encoding","COLLADA");
01073                         pmath_symb->setCharData(str(boost::format("%s/%s")%kmodel->getID()%_ComputeId(pjoint->mimic->joint_name)));
01074                     }
01075                     daeElementRef pmath_const1 = pmath_apply->add("cn");
01076                     pmath_const1->setCharData(str(boost::format("%f")%pjoint->mimic->offset));
01077                 }
01078             }
01079             // save first partial derivative
01080             {
01081                 daeElementRef derivelt = pftec->add("equation");
01082                 derivelt->setAttribute("type","first_partial");
01083                 derivelt->setAttribute("target",str(boost::format("%s/%s")%kmodel->getID()%_ComputeId(pjoint->mimic->joint_name)).c_str());
01084                 daeElementRef pmath_const0 = derivelt->add("cn");
01085                 pmath_const0->setCharData(str(boost::format("%f")%pjoint->mimic->multiplier));
01086             }
01087             // save second partial derivative
01088             {
01089                 daeElementRef derivelt = pftec->add("equation");
01090                 derivelt->setAttribute("type","second_partial");
01091                 derivelt->setAttribute("target",str(boost::format("%s/%s")%kmodel->getID()%_ComputeId(pjoint->mimic->joint_name)).c_str());
01092                 daeElementRef pmath_const0 = derivelt->add("cn");
01093                 pmath_const0->setCharData(str(boost::format("%f")%pjoint->mimic->multiplier));
01094             }
01095 
01096             {
01097                 domFormula_techniqueRef pfcommontec = daeSafeCast<domFormula_technique>(pf->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
01098                 // create a const0*joint+const1 formula
01099                 // <apply> <plus/> <apply> <times/> <cn>a</cn> x </apply> <cn>b</cn> </apply>
01100                 daeElementRef pmath_math = pfcommontec->add("math");
01101                 daeElementRef pmath_apply = pmath_math->add("apply");
01102                 {
01103                     daeElementRef pmath_plus = pmath_apply->add("plus");
01104                     daeElementRef pmath_apply1 = pmath_apply->add("apply");
01105                     {
01106                         daeElementRef pmath_times = pmath_apply1->add("times");
01107                         daeElementRef pmath_const0 = pmath_apply1->add("cn");
01108                         pmath_const0->setCharData(str(boost::format("%f")%pjoint->mimic->multiplier));
01109                         daeElementRef pmath_symb = pmath_apply1->add("csymbol");
01110                         pmath_symb->setAttribute("encoding","COLLADA");
01111                         pmath_symb->setCharData(str(boost::format("%s/%s")%kmodel->getID()%_ComputeId(pjoint->mimic->joint_name)));
01112                     }
01113                     daeElementRef pmath_const1 = pmath_apply->add("cn");
01114                     pmath_const1->setCharData(str(boost::format("%f")%pjoint->mimic->offset));
01115                 }
01116             }
01117         }
01118 
01119         return kmout;
01120     }
01121 
01128     virtual LINKOUTPUT _WriteLink(urdf::LinkConstSharedPtr plink, daeElementRef pkinparent, domNodeRef pnodeparent, const string& strModelUri)
01129     {
01130         LINKOUTPUT out;
01131         int linkindex = _maplinkindices[plink];
01132         string linksid = _ComputeId(plink->name);
01133         domLinkRef pdomlink = daeSafeCast<domLink>(pkinparent->add(COLLADA_ELEMENT_LINK));
01134         pdomlink->setName(plink->name.c_str());
01135         pdomlink->setSid(linksid.c_str());
01136 
01137         domNodeRef pnode = daeSafeCast<domNode>(pnodeparent->add(COLLADA_ELEMENT_NODE));
01138         string nodeid = _ComputeId(str(boost::format("v%s_node%d")%strModelUri%linkindex));
01139         pnode->setId( nodeid.c_str() );
01140         string nodesid = _ComputeId(str(boost::format("node%d")%linkindex));
01141         pnode->setSid(nodesid.c_str());
01142         pnode->setName(plink->name.c_str());
01143 
01144         urdf::GeometrySharedPtr geometry;
01145         urdf::MaterialSharedPtr material;
01146         urdf::Pose geometry_origin;
01147         if( !!plink->visual ) {
01148             geometry = plink->visual->geometry;
01149             material = plink->visual->material;
01150             geometry_origin = plink->visual->origin;
01151         }
01152         else if( !!plink->collision ) {
01153             geometry = plink->collision->geometry;
01154             geometry_origin = plink->collision->origin;
01155         }
01156 
01157         urdf::Pose geometry_origin_inv = _poseInverse(geometry_origin);
01158 
01159         if( !!geometry ) {
01160             bool write_visual = false;
01161             if ( !!plink->visual ) {
01162               if (plink->visual_array.size() > 1) {
01163                 int igeom = 0;
01164                 for (std::vector<urdf::VisualSharedPtr >::const_iterator it = plink->visual_array.begin();
01165                      it != plink->visual_array.end(); it++) {
01166                   // geom
01167                   string geomid = _ComputeId(str(boost::format("g%s_%s_geom%d")%strModelUri%linksid%igeom));
01168                   igeom++;
01169                   domGeometryRef pdomgeom;
01170                   if ( it != plink->visual_array.begin() ) {
01171                     urdf::Pose org_trans =  _poseMult(geometry_origin_inv, (*it)->origin);
01172                     pdomgeom = _WriteGeometry((*it)->geometry, geomid, &org_trans);
01173                   } else {
01174                     pdomgeom = _WriteGeometry((*it)->geometry, geomid);
01175                   }
01176                   domInstance_geometryRef pinstgeom = daeSafeCast<domInstance_geometry>(pnode->add(COLLADA_ELEMENT_INSTANCE_GEOMETRY));
01177                   pinstgeom->setUrl((string("#") + geomid).c_str());
01178                   // material
01179                   _WriteMaterial(pdomgeom->getID(), (*it)->material);
01180                   domBind_materialRef pmat = daeSafeCast<domBind_material>(pinstgeom->add(COLLADA_ELEMENT_BIND_MATERIAL));
01181                   domBind_material::domTechnique_commonRef pmattec = daeSafeCast<domBind_material::domTechnique_common>(pmat->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
01182                   domInstance_materialRef pinstmat = daeSafeCast<domInstance_material>(pmattec->add(COLLADA_ELEMENT_INSTANCE_MATERIAL));
01183                   pinstmat->setTarget(xsAnyURI(*pdomgeom, string("#")+geomid+string("_mat")));
01184                   pinstmat->setSymbol("mat0");
01185                   write_visual = true;
01186                 }
01187               }
01188             }
01189             if (!write_visual) {
01190               // just 1 visual
01191               int igeom = 0;
01192               string geomid = _ComputeId(str(boost::format("g%s_%s_geom%d")%strModelUri%linksid%igeom));
01193               domGeometryRef pdomgeom = _WriteGeometry(geometry, geomid);
01194               domInstance_geometryRef pinstgeom = daeSafeCast<domInstance_geometry>(pnode->add(COLLADA_ELEMENT_INSTANCE_GEOMETRY));
01195               pinstgeom->setUrl((string("#")+geomid).c_str());
01196 
01197               // material
01198               _WriteMaterial(pdomgeom->getID(), material);
01199               domBind_materialRef pmat = daeSafeCast<domBind_material>(pinstgeom->add(COLLADA_ELEMENT_BIND_MATERIAL));
01200               domBind_material::domTechnique_commonRef pmattec = daeSafeCast<domBind_material::domTechnique_common>(pmat->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
01201               domInstance_materialRef pinstmat = daeSafeCast<domInstance_material>(pmattec->add(COLLADA_ELEMENT_INSTANCE_MATERIAL));
01202               pinstmat->setTarget(xsAnyURI(*pdomgeom, string("#")+geomid+string("_mat")));
01203               pinstmat->setSymbol("mat0");
01204             }
01205         }
01206 
01207         _WriteTransformation(pnode, geometry_origin);
01208 
01209         // process all children
01210         FOREACHC(itjoint, plink->child_joints) {
01211             urdf::JointSharedPtr pjoint = *itjoint;
01212             int index = _mapjointindices[pjoint];
01213 
01214             // <attachment_full joint="k1/joint0">
01215             domLink::domAttachment_fullRef attachment_full = daeSafeCast<domLink::domAttachment_full>(pdomlink->add(COLLADA_ELEMENT_ATTACHMENT_FULL));
01216             string jointid = str(boost::format("%s/%s")%strModelUri%_ComputeId(pjoint->name));
01217             attachment_full->setJoint(jointid.c_str());
01218 
01219             LINKOUTPUT childinfo = _WriteLink(_robot.getLink(pjoint->child_link_name), attachment_full, pnode, strModelUri);
01220             FOREACH(itused,childinfo.listusedlinks) {
01221                 out.listusedlinks.push_back(make_pair(itused->first,linksid+string("/")+itused->second));
01222             }
01223             FOREACH(itprocessed,childinfo.listprocesseddofs) {
01224                 out.listprocesseddofs.push_back(make_pair(itprocessed->first,nodesid+string("/")+itprocessed->second));
01225             }
01226             FOREACH(itlinkpos,childinfo._maplinkposes) {
01227                 out._maplinkposes[itlinkpos->first] = _poseMult(pjoint->parent_to_joint_origin_transform,itlinkpos->second);
01228             }
01229             // rotate/translate elements
01230             string jointnodesid = _ComputeId(str(boost::format("node_%s_axis0")%pjoint->name));
01231             switch(pjoint->type) {
01232             case urdf::Joint::REVOLUTE:
01233             case urdf::Joint::CONTINUOUS:
01234             case urdf::Joint::FIXED: {
01235                 domRotateRef protate = daeSafeCast<domRotate>(childinfo.pnode->add(COLLADA_ELEMENT_ROTATE,0));
01236                 protate->setSid(jointnodesid.c_str());
01237                 protate->getValue().setCount(4);
01238                 protate->getValue()[0] = pjoint->axis.x;
01239                 protate->getValue()[1] = pjoint->axis.y;
01240                 protate->getValue()[2] = pjoint->axis.z;
01241                 protate->getValue()[3] = 0;
01242                 break;
01243             }
01244             case urdf::Joint::PRISMATIC: {
01245                 domTranslateRef ptrans = daeSafeCast<domTranslate>(childinfo.pnode->add(COLLADA_ELEMENT_TRANSLATE,0));
01246                 ptrans->setSid(jointnodesid.c_str());
01247                 ptrans->getValue().setCount(3);
01248                 ptrans->getValue()[0] = 0;
01249                 ptrans->getValue()[1] = 0;
01250                 ptrans->getValue()[2] = 0;
01251                 break;
01252             }
01253             default:
01254                 ROS_WARN_STREAM(str(boost::format("unsupported joint type specified %d")%(int)pjoint->type));
01255                 break;
01256             }
01257 
01258             _WriteTransformation(attachment_full, pjoint->parent_to_joint_origin_transform);
01259             _WriteTransformation(childinfo.pnode, pjoint->parent_to_joint_origin_transform);
01260             _WriteTransformation(childinfo.pnode, geometry_origin_inv); // have to do multiply by inverse since geometry transformation is not part of hierarchy
01261             out.listprocesseddofs.push_back(make_pair(index,string(childinfo.pnode->getSid())+string("/")+jointnodesid));
01262             // </attachment_full>
01263         }
01264 
01265         out._maplinkposes[plink] = urdf::Pose();
01266         out.listusedlinks.push_back(make_pair(linkindex,linksid));
01267         out.plink = pdomlink;
01268         out.pnode = pnode;
01269         return out;
01270     }
01271 
01272     domGeometryRef _WriteGeometry(urdf::GeometrySharedPtr geometry, const std::string& geometry_id, urdf::Pose *org_trans = NULL)
01273     {
01274         domGeometryRef cgeometry = daeSafeCast<domGeometry>(_geometriesLib->add(COLLADA_ELEMENT_GEOMETRY));
01275         cgeometry->setId(geometry_id.c_str());
01276         switch (geometry->type) {
01277         case urdf::Geometry::MESH: {
01278             urdf::Mesh* urdf_mesh = (urdf::Mesh*) geometry.get();
01279             cgeometry->setName(urdf_mesh->filename.c_str());
01280             _loadMesh(urdf_mesh->filename, cgeometry, urdf_mesh->scale, org_trans);
01281             break;
01282         }
01283         case urdf::Geometry::SPHERE: {
01284             shapes::Sphere sphere(static_cast<urdf::Sphere*>(geometry.get())->radius);
01285             boost::scoped_ptr<shapes::Mesh> mesh(shapes::createMeshFromShape(sphere));
01286             _loadVertices(mesh.get(), cgeometry);
01287             break;
01288         }
01289         case urdf::Geometry::BOX: {
01290             shapes::Box box(static_cast<urdf::Box*>(geometry.get())->dim.x,
01291                             static_cast<urdf::Box*>(geometry.get())->dim.y,
01292                             static_cast<urdf::Box*>(geometry.get())->dim.z);
01293             boost::scoped_ptr<shapes::Mesh> mesh(shapes::createMeshFromShape(box));
01294             _loadVertices(mesh.get(), cgeometry);
01295             break;
01296         }
01297         case urdf::Geometry::CYLINDER: {
01298             shapes::Cylinder cyl(static_cast<urdf::Cylinder*>(geometry.get())->radius,
01299                                  static_cast<urdf::Cylinder*>(geometry.get())->length);
01300             boost::scoped_ptr<shapes::Mesh> mesh(shapes::createMeshFromShape(cyl));
01301             _loadVertices(mesh.get(), cgeometry);
01302             break;
01303         }
01304         default: {
01305             throw ColladaUrdfException(str(boost::format("undefined geometry type %d, name %s")%(int)geometry->type%geometry_id));
01306         }
01307         }
01308         return cgeometry;
01309     }
01310 
01311     void _WriteMaterial(const string& geometry_id, urdf::MaterialSharedPtr material)
01312     {
01313         string effid = geometry_id+string("_eff");
01314         string matid = geometry_id+string("_mat");
01315         domMaterialRef pdommat = daeSafeCast<domMaterial>(_materialsLib->add(COLLADA_ELEMENT_MATERIAL));
01316         pdommat->setId(matid.c_str());
01317         domInstance_effectRef pdominsteff = daeSafeCast<domInstance_effect>(pdommat->add(COLLADA_ELEMENT_INSTANCE_EFFECT));
01318         pdominsteff->setUrl((string("#")+effid).c_str());
01319 
01320         urdf::Color ambient, diffuse;
01321         ambient.init("0.1 0.1 0.1 0");
01322         diffuse.init("1 1 1 0");
01323 
01324         if( !!material ) {
01325             ambient.r = diffuse.r = material->color.r;
01326             ambient.g = diffuse.g = material->color.g;
01327             ambient.b = diffuse.b = material->color.b;
01328             ambient.a = diffuse.a = material->color.a;
01329         }
01330 
01331         domEffectRef effect = _WriteEffect(geometry_id, ambient, diffuse);
01332 
01333         // <material id="g1.link0.geom0.eff">
01334         domMaterialRef dommaterial = daeSafeCast<domMaterial>(_materialsLib->add(COLLADA_ELEMENT_MATERIAL));
01335         string material_id = geometry_id + string("_mat");
01336         dommaterial->setId(material_id.c_str());
01337         {
01338             // <instance_effect url="#g1.link0.geom0.eff"/>
01339             domInstance_effectRef instance_effect = daeSafeCast<domInstance_effect>(dommaterial->add(COLLADA_ELEMENT_INSTANCE_EFFECT));
01340             string effect_id(effect->getId());
01341             instance_effect->setUrl((string("#") + effect_id).c_str());
01342         }
01343         // </material>
01344 
01345         domEffectRef pdomeff = _WriteEffect(effid, ambient, diffuse);
01346     }
01347 
01348     boost::shared_ptr<instance_physics_model_output> _WriteInstance_physics_model(int id, daeElementRef parent, const string& sidscope, const MAPLINKPOSES& maplinkposes)
01349     {
01350         boost::shared_ptr<physics_model_output> pmout = WritePhysics_model(id, maplinkposes);
01351         boost::shared_ptr<instance_physics_model_output> ipmout(new instance_physics_model_output());
01352         ipmout->pmout = pmout;
01353         ipmout->ipm = daeSafeCast<domInstance_physics_model>(parent->add(COLLADA_ELEMENT_INSTANCE_PHYSICS_MODEL));
01354         string bodyid = _ComputeId(str(boost::format("visual%d")%id));
01355         ipmout->ipm->setParent(xsAnyURI(*ipmout->ipm,string("#")+bodyid));
01356         string symscope, refscope;
01357         if( sidscope.size() > 0 ) {
01358             symscope = sidscope+string("_");
01359             refscope = sidscope+string("/");
01360         }
01361         string ipmsid = str(boost::format("%s_inst")%pmout->pmodel->getID());
01362         ipmout->ipm->setUrl(str(boost::format("#%s")%pmout->pmodel->getID()).c_str());
01363         ipmout->ipm->setSid(ipmsid.c_str());
01364 
01365         string kmodelid = _ComputeKinematics_modelId(id);
01366         for(size_t i = 0; i < pmout->vrigidbodysids.size(); ++i) {
01367             domInstance_rigid_bodyRef pirb = daeSafeCast<domInstance_rigid_body>(ipmout->ipm->add(COLLADA_ELEMENT_INSTANCE_RIGID_BODY));
01368             pirb->setBody(pmout->vrigidbodysids[i].c_str());
01369             pirb->setTarget(xsAnyURI(*pirb,str(boost::format("#v%s_node%d")%kmodelid%i)));
01370         }
01371 
01372         return ipmout;
01373     }
01374 
01375     boost::shared_ptr<physics_model_output> WritePhysics_model(int id, const MAPLINKPOSES& maplinkposes)
01376     {
01377         boost::shared_ptr<physics_model_output> pmout(new physics_model_output());
01378         pmout->pmodel = daeSafeCast<domPhysics_model>(_physicsModelsLib->add(COLLADA_ELEMENT_PHYSICS_MODEL));
01379         string pmodelid = str(boost::format("pmodel%d")%id);
01380         pmout->pmodel->setId(pmodelid.c_str());
01381         pmout->pmodel->setName(_robot.getName().c_str());
01382         FOREACHC(itlink,_robot.links_) {
01383             domRigid_bodyRef rigid_body = daeSafeCast<domRigid_body>(pmout->pmodel->add(COLLADA_ELEMENT_RIGID_BODY));
01384             string rigidsid = str(boost::format("rigid%d")%_maplinkindices[itlink->second]);
01385             pmout->vrigidbodysids.push_back(rigidsid);
01386             rigid_body->setSid(rigidsid.c_str());
01387             rigid_body->setName(itlink->second->name.c_str());
01388             domRigid_body::domTechnique_commonRef ptec = daeSafeCast<domRigid_body::domTechnique_common>(rigid_body->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
01389             urdf::InertialSharedPtr inertial = itlink->second->inertial;
01390             if( !!inertial ) {
01391                 daeSafeCast<domRigid_body::domTechnique_common::domDynamic>(ptec->add(COLLADA_ELEMENT_DYNAMIC))->setValue(xsBoolean(true)); 
01392                 domTargetable_floatRef mass = daeSafeCast<domTargetable_float>(ptec->add(COLLADA_ELEMENT_MASS));
01393                 mass->setValue(inertial->mass);
01394                 double fCovariance[9] = { inertial->ixx, inertial->ixy, inertial->ixz, inertial->ixy, inertial->iyy, inertial->iyz, inertial->ixz, inertial->iyz, inertial->izz};
01395                 double eigenvalues[3], eigenvectors[9];
01396                 mathextra::EigenSymmetric3(fCovariance,eigenvalues,eigenvectors);
01397                 boost::array<double,12> minertiaframe;
01398                 for(int j = 0; j < 3; ++j) {
01399                     minertiaframe[4*0+j] = eigenvectors[3*j];
01400                     minertiaframe[4*1+j] = eigenvectors[3*j+1];
01401                     minertiaframe[4*2+j] = eigenvectors[3*j+2];
01402                 }
01403                 urdf::Pose tinertiaframe;
01404                 tinertiaframe.rotation = _quatFromMatrix(minertiaframe);
01405                 tinertiaframe = _poseMult(inertial->origin,tinertiaframe);
01406 
01407                 domTargetable_float3Ref pinertia = daeSafeCast<domTargetable_float3>(ptec->add(COLLADA_ELEMENT_INERTIA));
01408                 pinertia->getValue().setCount(3);
01409                 pinertia->getValue()[0] = eigenvalues[0];
01410                 pinertia->getValue()[1] = eigenvalues[1];
01411                 pinertia->getValue()[2] = eigenvalues[2];
01412                 urdf::Pose posemassframe = _poseMult(maplinkposes.find(itlink->second)->second,tinertiaframe);
01413                 _WriteTransformation(ptec->add(COLLADA_ELEMENT_MASS_FRAME), posemassframe);
01414 
01415                 //            // create a shape for every geometry
01416                 //            int igeom = 0;
01417                 //            FOREACHC(itgeom, (*itlink)->GetGeometries()) {
01418                 //                domRigid_body::domTechnique_common::domShapeRef pdomshape = daeSafeCast<domRigid_body::domTechnique_common::domShape>(ptec->add(COLLADA_ELEMENT_SHAPE));
01419                 //                // 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?)
01420                 //                _WriteTransformation(pdomshape,tbaseinv*(*itlink)->GetTransform()*itgeom->GetTransform());
01421                 //                domInstance_geometryRef pinstgeom = daeSafeCast<domInstance_geometry>(pdomshape->add(COLLADA_ELEMENT_INSTANCE_GEOMETRY));
01422                 //                pinstgeom->setUrl(xsAnyURI(*pinstgeom,string("#")+_GetGeometryId(*itlink,igeom)));
01423                 //                ++igeom;
01424                 //            }
01425             }
01426         }
01427         return pmout;
01428     }
01429 
01430     void _loadVertices(const shapes::Mesh *mesh, domGeometryRef pdomgeom) {
01431         
01432         // convert the mesh into an STL binary (in memory)
01433         std::vector<char> buffer;
01434         shapes::writeSTLBinary(mesh, buffer);
01435         
01436         // Create an instance of the Importer class
01437         Assimp::Importer importer;
01438         
01439         // And have it read the given file with some postprocessing
01440         const aiScene* scene = importer.ReadFileFromMemory(reinterpret_cast<const void*>(&buffer[0]), buffer.size(),
01441                                                            aiProcess_Triangulate            |
01442                                                            aiProcess_JoinIdenticalVertices  |
01443                                                            aiProcess_SortByPType            |
01444                                                            aiProcess_OptimizeGraph          |
01445                                                            aiProcess_OptimizeMeshes, "stl");
01446         
01447         // Note: we do this mesh -> STL -> assimp mesh because the aiScene::aiScene symbol is hidden by default 
01448 
01449             domMeshRef pdommesh = daeSafeCast<domMesh>(pdomgeom->add(COLLADA_ELEMENT_MESH));
01450             domSourceRef pvertsource = daeSafeCast<domSource>(pdommesh->add(COLLADA_ELEMENT_SOURCE));
01451             domAccessorRef pacc;
01452             domFloat_arrayRef parray;
01453             {
01454                 pvertsource->setId(str(boost::format("%s_positions")%pdomgeom->getID()).c_str());
01455 
01456                 parray = daeSafeCast<domFloat_array>(pvertsource->add(COLLADA_ELEMENT_FLOAT_ARRAY));
01457                 parray->setId(str(boost::format("%s_positions-array")%pdomgeom->getID()).c_str());
01458                 parray->setDigits(6); // 6 decimal places
01459 
01460                 domSource::domTechnique_commonRef psourcetec = daeSafeCast<domSource::domTechnique_common>(pvertsource->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
01461                 pacc = daeSafeCast<domAccessor>(psourcetec->add(COLLADA_ELEMENT_ACCESSOR));
01462                 pacc->setSource(xsAnyURI(*parray, std::string("#")+string(parray->getID())));
01463                 pacc->setStride(3);
01464 
01465                 domParamRef px = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM));
01466                 px->setName("X"); px->setType("float");
01467                 domParamRef py = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM));
01468                 py->setName("Y"); py->setType("float");
01469                 domParamRef pz = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM));
01470                 pz->setName("Z"); pz->setType("float");
01471             }
01472             domVerticesRef pverts = daeSafeCast<domVertices>(pdommesh->add(COLLADA_ELEMENT_VERTICES));
01473             {
01474                 pverts->setId("vertices");
01475                 domInput_localRef pvertinput = daeSafeCast<domInput_local>(pverts->add(COLLADA_ELEMENT_INPUT));
01476                 pvertinput->setSemantic("POSITION");
01477                 pvertinput->setSource(domUrifragment(*pvertsource, std::string("#")+std::string(pvertsource->getID())));
01478             }
01479             _buildAiMesh(scene,scene->mRootNode,pdommesh,parray, pdomgeom->getID(), urdf::Vector3(1,1,1));
01480             pacc->setCount(parray->getCount());
01481     }
01482 
01483     void _loadMesh(std::string const& filename, domGeometryRef pdomgeom, const urdf::Vector3& scale, urdf::Pose *org_trans)
01484     {
01485         const aiScene* scene = _importer.ReadFile(filename, aiProcess_SortByPType|aiProcess_Triangulate); //|aiProcess_GenNormals|aiProcess_GenUVCoords|aiProcess_FlipUVs);
01486         if( !scene ) {
01487             ROS_WARN("failed to load resource %s",filename.c_str());
01488             return;
01489         }
01490         if( !scene->mRootNode ) {
01491             ROS_WARN("resource %s has no data",filename.c_str());
01492             return;
01493         }
01494         if (!scene->HasMeshes()) {
01495             ROS_WARN_STREAM(str(boost::format("No meshes found in file %s")%filename));
01496             return;
01497         }
01498         domMeshRef pdommesh = daeSafeCast<domMesh>(pdomgeom->add(COLLADA_ELEMENT_MESH));
01499         domSourceRef pvertsource = daeSafeCast<domSource>(pdommesh->add(COLLADA_ELEMENT_SOURCE));
01500         domAccessorRef pacc;
01501         domFloat_arrayRef parray;
01502         {
01503             pvertsource->setId(str(boost::format("%s_positions")%pdomgeom->getID()).c_str());
01504 
01505             parray = daeSafeCast<domFloat_array>(pvertsource->add(COLLADA_ELEMENT_FLOAT_ARRAY));
01506             parray->setId(str(boost::format("%s_positions-array")%pdomgeom->getID()).c_str());
01507             parray->setDigits(6); // 6 decimal places
01508 
01509             domSource::domTechnique_commonRef psourcetec = daeSafeCast<domSource::domTechnique_common>(pvertsource->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
01510             pacc = daeSafeCast<domAccessor>(psourcetec->add(COLLADA_ELEMENT_ACCESSOR));
01511             pacc->setSource(xsAnyURI(*parray, string("#")+string(parray->getID())));
01512             pacc->setStride(3);
01513 
01514             domParamRef px = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM));
01515             px->setName("X"); px->setType("float");
01516             domParamRef py = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM));
01517             py->setName("Y"); py->setType("float");
01518             domParamRef pz = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM));
01519             pz->setName("Z"); pz->setType("float");
01520         }
01521         domVerticesRef pverts = daeSafeCast<domVertices>(pdommesh->add(COLLADA_ELEMENT_VERTICES));
01522         {
01523             pverts->setId("vertices");
01524             domInput_localRef pvertinput = daeSafeCast<domInput_local>(pverts->add(COLLADA_ELEMENT_INPUT));
01525             pvertinput->setSemantic("POSITION");
01526             pvertinput->setSource(domUrifragment(*pvertsource, string("#")+string(pvertsource->getID())));
01527         }
01528         _buildAiMesh(scene,scene->mRootNode,pdommesh,parray, pdomgeom->getID(),scale,org_trans);
01529         pacc->setCount(parray->getCount());
01530     }
01531 
01532     void _buildAiMesh(const aiScene* scene, aiNode* node, domMeshRef pdommesh, domFloat_arrayRef parray, const string& geomid, const urdf::Vector3& scale, urdf::Pose *org_trans = NULL)
01533     {
01534         if( !node ) {
01535             return;
01536         }
01537         aiMatrix4x4 transform = node->mTransformation;
01538         aiNode *pnode = node->mParent;
01539         while (pnode) {
01540             // Don't convert to y-up orientation, which is what the root node in
01541             // Assimp does
01542             if (pnode->mParent != NULL) {
01543                 transform = pnode->mTransformation * transform;
01544             }
01545             pnode = pnode->mParent;
01546         }
01547 
01548         {
01549             uint32_t vertexOffset = parray->getCount();
01550             uint32_t nTotalVertices=0;
01551             for (uint32_t i = 0; i < node->mNumMeshes; i++) {
01552                 aiMesh* input_mesh = scene->mMeshes[node->mMeshes[i]];
01553                 nTotalVertices += input_mesh->mNumVertices;
01554             }
01555 
01556             parray->getValue().grow(parray->getCount()+nTotalVertices*3);
01557             parray->setCount(parray->getCount()+nTotalVertices);
01558 
01559             for (uint32_t i = 0; i < node->mNumMeshes; i++) {
01560                 aiMesh* input_mesh = scene->mMeshes[node->mMeshes[i]];
01561                 for (uint32_t j = 0; j < input_mesh->mNumVertices; j++) {
01562                     aiVector3D p = input_mesh->mVertices[j];
01563                     p *= transform;
01564                     if (org_trans) {
01565                       urdf::Vector3 vv;
01566                       vv.x = p.x*scale.x;
01567                       vv.y = p.y*scale.y;
01568                       vv.z = p.z*scale.z;
01569                       urdf::Vector3 nv = _poseMult(*org_trans, vv);
01570                       parray->getValue().append(nv.x);
01571                       parray->getValue().append(nv.y);
01572                       parray->getValue().append(nv.z);
01573                     } else {
01574                       parray->getValue().append(p.x*scale.x);
01575                       parray->getValue().append(p.y*scale.y);
01576                       parray->getValue().append(p.z*scale.z);
01577                     }
01578                 }
01579             }
01580 
01581             // in order to save space, separate triangles from poly lists
01582 
01583             vector<int> triangleindices, otherindices;
01584             int nNumOtherPrimitives = 0;
01585             for (uint32_t i = 0; i < node->mNumMeshes; i++) {
01586                 aiMesh* input_mesh = scene->mMeshes[node->mMeshes[i]];
01587                 uint32_t indexCount = 0, otherIndexCount = 0;
01588                 for (uint32_t j = 0; j < input_mesh->mNumFaces; j++) {
01589                     aiFace& face = input_mesh->mFaces[j];
01590                     if( face.mNumIndices == 3 ) {
01591                         indexCount += face.mNumIndices;
01592                     }
01593                     else {
01594                         otherIndexCount += face.mNumIndices;
01595                     }
01596                 }
01597                 triangleindices.reserve(triangleindices.size()+indexCount);
01598                 otherindices.reserve(otherindices.size()+otherIndexCount);
01599                 for (uint32_t j = 0; j < input_mesh->mNumFaces; j++) {
01600                     aiFace& face = input_mesh->mFaces[j];
01601                     if( face.mNumIndices == 3 ) {
01602                         triangleindices.push_back(vertexOffset+face.mIndices[0]);
01603                         triangleindices.push_back(vertexOffset+face.mIndices[1]);
01604                         triangleindices.push_back(vertexOffset+face.mIndices[2]);
01605                     }
01606                     else {
01607                         for (uint32_t k = 0; k < face.mNumIndices; ++k) {
01608                             otherindices.push_back(face.mIndices[k]+vertexOffset);
01609                         }
01610                         nNumOtherPrimitives++;
01611                     }
01612                 }
01613                 vertexOffset += input_mesh->mNumVertices;
01614             }
01615 
01616             if( triangleindices.size() > 0 ) {
01617                 domTrianglesRef ptris = daeSafeCast<domTriangles>(pdommesh->add(COLLADA_ELEMENT_TRIANGLES));
01618                 ptris->setCount(triangleindices.size()/3);
01619                 ptris->setMaterial("mat0");
01620                 domInput_local_offsetRef pvertoffset = daeSafeCast<domInput_local_offset>(ptris->add(COLLADA_ELEMENT_INPUT));
01621                 pvertoffset->setSemantic("VERTEX");
01622                 pvertoffset->setOffset(0);
01623                 pvertoffset->setSource(domUrifragment(*pdommesh->getVertices(), str(boost::format("#%s/vertices")%geomid)));
01624                 domPRef pindices = daeSafeCast<domP>(ptris->add(COLLADA_ELEMENT_P));
01625                 pindices->getValue().setCount(triangleindices.size());
01626                 for(size_t ind = 0; ind < triangleindices.size(); ++ind) {
01627                     pindices->getValue()[ind] = triangleindices[ind];
01628                 }
01629             }
01630 
01631             if( nNumOtherPrimitives > 0 ) {
01632                 domPolylistRef ptris = daeSafeCast<domPolylist>(pdommesh->add(COLLADA_ELEMENT_POLYLIST));
01633                 ptris->setCount(nNumOtherPrimitives);
01634                 ptris->setMaterial("mat0");
01635                 domInput_local_offsetRef pvertoffset = daeSafeCast<domInput_local_offset>(ptris->add(COLLADA_ELEMENT_INPUT));
01636                 pvertoffset->setSemantic("VERTEX");
01637                 pvertoffset->setSource(domUrifragment(*pdommesh->getVertices(), str(boost::format("#%s/vertices")%geomid)));
01638                 domPRef pindices = daeSafeCast<domP>(ptris->add(COLLADA_ELEMENT_P));
01639                 pindices->getValue().setCount(otherindices.size());
01640                 for(size_t ind = 0; ind < otherindices.size(); ++ind) {
01641                     pindices->getValue()[ind] = otherindices[ind];
01642                 }
01643 
01644                 domPolylist::domVcountRef pcount = daeSafeCast<domPolylist::domVcount>(ptris->add(COLLADA_ELEMENT_VCOUNT));
01645                 pcount->getValue().setCount(nNumOtherPrimitives);
01646                 uint32_t offset = 0;
01647                 for (uint32_t i = 0; i < node->mNumMeshes; i++) {
01648                     aiMesh* input_mesh = scene->mMeshes[node->mMeshes[i]];
01649                     for (uint32_t j = 0; j < input_mesh->mNumFaces; j++) {
01650                         aiFace& face = input_mesh->mFaces[j];
01651                         if( face.mNumIndices > 3 ) {
01652                             pcount->getValue()[offset++] = face.mNumIndices;
01653                         }
01654                     }
01655                 }
01656             }
01657         }
01658 
01659         for (uint32_t i=0; i < node->mNumChildren; ++i) {
01660             _buildAiMesh(scene, node->mChildren[i], pdommesh,parray,geomid,scale,org_trans);
01661         }
01662     }
01663 
01664 
01665     domEffectRef _WriteEffect(std::string const& effect_id, urdf::Color const& color_ambient, urdf::Color const& color_diffuse)
01666     {
01667         // <effect id="g1.link0.geom0.eff">
01668         domEffectRef effect = daeSafeCast<domEffect>(_effectsLib->add(COLLADA_ELEMENT_EFFECT));
01669         effect->setId(effect_id.c_str());
01670         {
01671             // <profile_COMMON>
01672             domProfile_commonRef profile = daeSafeCast<domProfile_common>(effect->add(COLLADA_ELEMENT_PROFILE_COMMON));
01673             {
01674                 // <technique sid="">
01675                 domProfile_common::domTechniqueRef technique = daeSafeCast<domProfile_common::domTechnique>(profile->add(COLLADA_ELEMENT_TECHNIQUE));
01676                 {
01677                     // <phong>
01678                     domProfile_common::domTechnique::domPhongRef phong = daeSafeCast<domProfile_common::domTechnique::domPhong>(technique->add(COLLADA_ELEMENT_PHONG));
01679                     {
01680                         // <ambient>
01681                         domFx_common_color_or_textureRef ambient = daeSafeCast<domFx_common_color_or_texture>(phong->add(COLLADA_ELEMENT_AMBIENT));
01682                         {
01683                             // <color>r g b a
01684                             domFx_common_color_or_texture::domColorRef ambient_color = daeSafeCast<domFx_common_color_or_texture::domColor>(ambient->add(COLLADA_ELEMENT_COLOR));
01685                             ambient_color->getValue().setCount(4);
01686                             ambient_color->getValue()[0] = color_ambient.r;
01687                             ambient_color->getValue()[1] = color_ambient.g;
01688                             ambient_color->getValue()[2] = color_ambient.b;
01689                             ambient_color->getValue()[3] = color_ambient.a;
01690                             // </color>
01691                         }
01692                         // </ambient>
01693 
01694                         // <diffuse>
01695                         domFx_common_color_or_textureRef diffuse = daeSafeCast<domFx_common_color_or_texture>(phong->add(COLLADA_ELEMENT_DIFFUSE));
01696                         {
01697                             // <color>r g b a
01698                             domFx_common_color_or_texture::domColorRef diffuse_color = daeSafeCast<domFx_common_color_or_texture::domColor>(diffuse->add(COLLADA_ELEMENT_COLOR));
01699                             diffuse_color->getValue().setCount(4);
01700                             diffuse_color->getValue()[0] = color_diffuse.r;
01701                             diffuse_color->getValue()[1] = color_diffuse.g;
01702                             diffuse_color->getValue()[2] = color_diffuse.b;
01703                             diffuse_color->getValue()[3] = color_diffuse.a;
01704                             // </color>
01705                         }
01706                         // </diffuse>
01707                     }
01708                     // </phong>
01709                 }
01710                 // </technique>
01711             }
01712             // </profile_COMMON>
01713         }
01714         // </effect>
01715 
01716         return effect;
01717     }
01718 
01722     void _WriteTransformation(daeElementRef pelt, const urdf::Pose& t)
01723     {
01724         domRotateRef prot = daeSafeCast<domRotate>(pelt->add(COLLADA_ELEMENT_ROTATE,0));
01725         domTranslateRef ptrans = daeSafeCast<domTranslate>(pelt->add(COLLADA_ELEMENT_TRANSLATE,0));
01726         ptrans->getValue().setCount(3);
01727         ptrans->getValue()[0] = t.position.x;
01728         ptrans->getValue()[1] = t.position.y;
01729         ptrans->getValue()[2] = t.position.z;
01730 
01731         prot->getValue().setCount(4);
01732         // extract axis from quaternion
01733         double sinang = t.rotation.x*t.rotation.x+t.rotation.y*t.rotation.y+t.rotation.z*t.rotation.z;
01734         if( std::fabs(sinang) < 1e-10 ) {
01735             prot->getValue()[0] = 1;
01736             prot->getValue()[1] = 0;
01737             prot->getValue()[2] = 0;
01738             prot->getValue()[3] = 0;
01739         }
01740         else {
01741             urdf::Rotation quat;
01742             if( t.rotation.w < 0 ) {
01743                 quat.x = -t.rotation.x;
01744                 quat.y = -t.rotation.y;
01745                 quat.z = -t.rotation.z;
01746                 quat.w = -t.rotation.w;
01747             }
01748             else {
01749                 quat = t.rotation;
01750             }
01751             sinang = std::sqrt(sinang);
01752             prot->getValue()[0] = quat.x/sinang;
01753             prot->getValue()[1] = quat.y/sinang;
01754             prot->getValue()[2] = quat.z/sinang;
01755             prot->getValue()[3] = angles::to_degrees(2.0*std::atan2(sinang,quat.w));
01756         }
01757     }
01758 
01759     // binding in instance_kinematics_scene
01760     void _WriteBindingsInstance_kinematics_scene()
01761     {
01762         FOREACHC(it, _iasout->vkinematicsbindings) {
01763             domBind_kinematics_modelRef pmodelbind = daeSafeCast<domBind_kinematics_model>(_scene.kiscene->add(COLLADA_ELEMENT_BIND_KINEMATICS_MODEL));
01764             pmodelbind->setNode(it->second.c_str());
01765             daeSafeCast<domCommon_param>(pmodelbind->add(COLLADA_ELEMENT_PARAM))->setValue(it->first.c_str());
01766         }
01767         FOREACHC(it, _iasout->vaxissids) {
01768             domBind_joint_axisRef pjointbind = daeSafeCast<domBind_joint_axis>(_scene.kiscene->add(COLLADA_ELEMENT_BIND_JOINT_AXIS));
01769             pjointbind->setTarget(it->jointnodesid.c_str());
01770             daeSafeCast<domCommon_param>(pjointbind->add(COLLADA_ELEMENT_AXIS)->add(COLLADA_TYPE_PARAM))->setValue(it->axissid.c_str());
01771             daeSafeCast<domCommon_param>(pjointbind->add(COLLADA_ELEMENT_VALUE)->add(COLLADA_TYPE_PARAM))->setValue(it->valuesid.c_str());
01772         }
01773     }
01774 
01775 private:
01776     static urdf::Vector3 _poseMult(const urdf::Pose& p, const urdf::Vector3& v)
01777     {
01778         double ww = 2 * p.rotation.x * p.rotation.x;
01779         double wx = 2 * p.rotation.x * p.rotation.y;
01780         double wy = 2 * p.rotation.x * p.rotation.z;
01781         double wz = 2 * p.rotation.x * p.rotation.w;
01782         double xx = 2 * p.rotation.y * p.rotation.y;
01783         double xy = 2 * p.rotation.y * p.rotation.z;
01784         double xz = 2 * p.rotation.y * p.rotation.w;
01785         double yy = 2 * p.rotation.z * p.rotation.z;
01786         double yz = 2 * p.rotation.z * p.rotation.w;
01787         urdf::Vector3 vnew;
01788         vnew.x = (1-xx-yy) * v.x + (wx-yz) * v.y + (wy+xz)*v.z + p.position.x;
01789         vnew.y = (wx+yz) * v.x + (1-ww-yy) * v.y + (xy-wz)*v.z + p.position.y;
01790         vnew.z = (wy-xz) * v.x + (xy+wz) * v.y + (1-ww-xx)*v.z + p.position.z;
01791         return vnew;
01792     }
01793 
01794     static urdf::Pose _poseInverse(const urdf::Pose& p)
01795     {
01796         urdf::Pose pinv;
01797         pinv.rotation.x = -p.rotation.x;
01798         pinv.rotation.y = -p.rotation.y;
01799         pinv.rotation.z = -p.rotation.z;
01800         pinv.rotation.w = p.rotation.w;
01801         urdf::Vector3 t = _poseMult(pinv,p.position);
01802         pinv.position.x = -t.x;
01803         pinv.position.y = -t.y;
01804         pinv.position.z = -t.z;
01805         return pinv;
01806     }
01807 
01808     static urdf::Rotation _quatMult(const urdf::Rotation& quat0, const urdf::Rotation& quat1)
01809     {
01810         urdf::Rotation q;
01811         q.x = quat0.w*quat1.x + quat0.x*quat1.w + quat0.y*quat1.z - quat0.z*quat1.y;
01812         q.y = quat0.w*quat1.y + quat0.y*quat1.w + quat0.z*quat1.x - quat0.x*quat1.z;
01813         q.z = quat0.w*quat1.z + quat0.z*quat1.w + quat0.x*quat1.y - quat0.y*quat1.x;
01814         q.w = quat0.w*quat1.w - quat0.x*quat1.x - quat0.y*quat1.y - quat0.z*quat1.z;
01815         double fnorm = std::sqrt(q.x*q.x+q.y*q.y+q.z*q.z+q.w*q.w);
01816         // don't touch the divides
01817         q.x /= fnorm;
01818         q.y /= fnorm;
01819         q.z /= fnorm;
01820         q.w /= fnorm;
01821         return q;
01822     }
01823 
01824     static urdf::Pose _poseMult(const urdf::Pose& p0, const urdf::Pose& p1)
01825     {
01826         urdf::Pose p;
01827         p.position = _poseMult(p0,p1.position);
01828         p.rotation = _quatMult(p0.rotation,p1.rotation);
01829         return p;
01830     }
01831 
01832     static urdf::Rotation _quatFromMatrix(const boost::array<double,12>& mat)
01833     {
01834         urdf::Rotation rot;
01835         double tr = mat[4*0+0] + mat[4*1+1] + mat[4*2+2];
01836         if (tr >= 0) {
01837             rot.w = tr + 1;
01838             rot.x = (mat[4*2+1] - mat[4*1+2]);
01839             rot.y = (mat[4*0+2] - mat[4*2+0]);
01840             rot.z = (mat[4*1+0] - mat[4*0+1]);
01841         }
01842         else {
01843             // find the largest diagonal element and jump to the appropriate case
01844             if (mat[4*1+1] > mat[4*0+0]) {
01845                 if (mat[4*2+2] > mat[4*1+1]) {
01846                     rot.z = (mat[4*2+2] - (mat[4*0+0] + mat[4*1+1])) + 1;
01847                     rot.x = (mat[4*2+0] + mat[4*0+2]);
01848                     rot.y = (mat[4*1+2] + mat[4*2+1]);
01849                     rot.w = (mat[4*1+0] - mat[4*0+1]);
01850                 }
01851                 else {
01852                     rot.y = (mat[4*1+1] - (mat[4*2+2] + mat[4*0+0])) + 1;
01853                     rot.z = (mat[4*1+2] + mat[4*2+1]);
01854                     rot.x = (mat[4*0+1] + mat[4*1+0]);
01855                     rot.w = (mat[4*0+2] - mat[4*2+0]);
01856                 }
01857             }
01858             else if (mat[4*2+2] > mat[4*0+0]) {
01859                 rot.z = (mat[4*2+2] - (mat[4*0+0] + mat[4*1+1])) + 1;
01860                 rot.x = (mat[4*2+0] + mat[4*0+2]);
01861                 rot.y = (mat[4*1+2] + mat[4*2+1]);
01862                 rot.w = (mat[4*1+0] - mat[4*0+1]);
01863             }
01864             else {
01865                 rot.x = (mat[4*0+0] - (mat[4*1+1] + mat[4*2+2])) + 1;
01866                 rot.y = (mat[4*0+1] + mat[4*1+0]);
01867                 rot.z = (mat[4*2+0] + mat[4*0+2]);
01868                 rot.w = (mat[4*2+1] - mat[4*1+2]);
01869             }
01870         }
01871         double fnorm = std::sqrt(rot.x*rot.x+rot.y*rot.y+rot.z*rot.z+rot.w*rot.w);
01872         // don't touch the divides
01873         rot.x /= fnorm;
01874         rot.y /= fnorm;
01875         rot.z /= fnorm;
01876         rot.w /= fnorm;
01877         return rot;
01878     }
01879 
01880     static std::string _ComputeKinematics_modelId(int id)
01881     {
01882         return _ComputeId(str(boost::format("kmodel%d")%id));
01883     }
01884 
01886     static std::string _ComputeId(const std::string& name)
01887     {
01888         std::string newname = name;
01889         for(size_t i = 0; i < newname.size(); ++i) {
01890             if( newname[i] == '/' || newname[i] == ' ' || newname[i] == '.' ) {
01891                 newname[i] = '_';
01892             }
01893         }
01894         return newname;
01895     }
01896 
01897     int _writeoptions;
01898 
01899     const urdf::Model& _robot;
01900     DAE _collada;
01901     domCOLLADA* _dom;
01902     daeDocument *_doc;
01903     domCOLLADA::domSceneRef _globalscene;
01904 
01905     domLibrary_visual_scenesRef _visualScenesLib;
01906     domLibrary_kinematics_scenesRef _kinematicsScenesLib;
01907     domLibrary_kinematics_modelsRef _kinematicsModelsLib;
01908     domLibrary_articulated_systemsRef _articulatedSystemsLib;
01909     domLibrary_physics_scenesRef _physicsScenesLib;
01910     domLibrary_physics_modelsRef _physicsModelsLib;
01911     domLibrary_materialsRef _materialsLib;
01912     domLibrary_effectsRef _effectsLib;
01913     domLibrary_geometriesRef _geometriesLib;
01914     domTechniqueRef _sensorsLib; 
01915     SCENE _scene;
01916 
01917     boost::shared_ptr<instance_kinematics_model_output> _ikmout;
01918     boost::shared_ptr<instance_articulated_system_output> _iasout;
01919     std::map< urdf::JointConstSharedPtr, int > _mapjointindices;
01920     std::map< urdf::LinkConstSharedPtr, int > _maplinkindices;
01921     std::map< urdf::MaterialConstSharedPtr, int > _mapmaterialindices;
01922     Assimp::Importer _importer;
01923 };
01924 
01925 
01926 ColladaUrdfException::ColladaUrdfException(std::string const& what)
01927     : std::runtime_error(what)
01928 {
01929 }
01930 
01931 bool WriteUrdfModelToColladaFile(urdf::Model const& robot_model, string const& file) {
01932     ColladaWriter writer(robot_model,0);
01933     if ( ! writer.convert() ) {
01934         std::cerr << std::endl << "Error converting document" << std::endl;
01935         return -1;
01936     }
01937     return writer.writeTo(file);
01938 }
01939 
01940 }


collada_urdf
Author(s): Tim Field, Rosen Diankov, Ioan Sucan , Jackie Kay
autogenerated on Sat Jun 8 2019 20:34:16