00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #include <vector>
00037 #include <list>
00038 #include <map>
00039 #include <stdint.h>
00040 #include <cstdlib>
00041 #include <cmath>
00042 #include <string>
00043 #include <sstream>
00044
00045 #include <dae.h>
00046 #include <dae/daeErrorHandler.h>
00047 #include <dom/domCOLLADA.h>
00048 #include <dae/domAny.h>
00049 #include <dom/domConstants.h>
00050 #include <dom/domTriangles.h>
00051 #include <dae/daeStandardURIResolver.h>
00052
00053 #include <boost/assert.hpp>
00054 #include <boost/format.hpp>
00055 #include <boost/shared_ptr.hpp>
00056
00057 #include <ros/ros.h>
00058 #include <collada_parser/collada_parser.h>
00059 #include <urdf_model/model.h>
00060
00061 #ifndef HAVE_MKSTEMPS
00062 #include <fstream>
00063 #include <fcntl.h>
00064 #endif
00065 #ifndef HAVE_MKSTEMPS
00066 #include <fstream>
00067 #include <fcntl.h>
00068 #endif
00069
00070 #define FOREACH(it, v) for(typeof((v).begin())it = (v).begin(); it != (v).end(); (it)++)
00071 #define FOREACHC FOREACH
00072
00073 namespace ColladaDOM150 { }
00074
00075 namespace urdf {
00076
00077 using namespace ColladaDOM150;
00078
00079 class UnlinkFilename
00080 {
00081 public:
00082 UnlinkFilename(const std::string& filename) : _filename(filename) {
00083 }
00084 virtual ~UnlinkFilename() {
00085 unlink(_filename.c_str());
00086 }
00087 std::string _filename;
00088 };
00089 static std::list<boost::shared_ptr<UnlinkFilename> > _listTempFilenames;
00090
00091 class ColladaModelReader : public daeErrorHandler
00092 {
00093
00094 class JointAxisBinding
00095 {
00096 public:
00097 JointAxisBinding(daeElementRef pvisualtrans, domAxis_constraintRef pkinematicaxis, domCommon_float_or_paramRef jointvalue, domKinematics_axis_infoRef kinematics_axis_info, domMotion_axis_infoRef motion_axis_info) : pvisualtrans(pvisualtrans), pkinematicaxis(pkinematicaxis), jointvalue(jointvalue), kinematics_axis_info(kinematics_axis_info), motion_axis_info(motion_axis_info) {
00098 BOOST_ASSERT( !!pkinematicaxis );
00099 daeElement* pae = pvisualtrans->getParentElement();
00100 while (!!pae) {
00101 visualnode = daeSafeCast<domNode> (pae);
00102 if (!!visualnode) {
00103 break;
00104 }
00105 pae = pae->getParentElement();
00106 }
00107
00108 if (!visualnode) {
00109 ROS_WARN_STREAM(str(boost::format("couldn't find parent node of element id %s, sid %s\n")%pkinematicaxis->getID()%pkinematicaxis->getSid()));
00110 }
00111 }
00112
00113 daeElementRef pvisualtrans;
00114 domAxis_constraintRef pkinematicaxis;
00115 domCommon_float_or_paramRef jointvalue;
00116 domNodeRef visualnode;
00117 domKinematics_axis_infoRef kinematics_axis_info;
00118 domMotion_axis_infoRef motion_axis_info;
00119 };
00120
00122 class LinkBinding
00123 {
00124 public:
00125 domNodeRef node;
00126 domLinkRef domlink;
00127 domInstance_rigid_bodyRef irigidbody;
00128 domRigid_bodyRef rigidbody;
00129 domNodeRef nodephysicsoffset;
00130 };
00131
00133 class KinematicsSceneBindings
00134 {
00135 public:
00136 std::list< std::pair<domNodeRef,domInstance_kinematics_modelRef> > listKinematicsVisualBindings;
00137 std::list<JointAxisBinding> listAxisBindings;
00138 std::list<LinkBinding> listLinkBindings;
00139
00140 bool AddAxisInfo(const domInstance_kinematics_model_Array& arr, domKinematics_axis_infoRef kinematics_axis_info, domMotion_axis_infoRef motion_axis_info)
00141 {
00142 if( !kinematics_axis_info ) {
00143 return false;
00144 }
00145 for(size_t ik = 0; ik < arr.getCount(); ++ik) {
00146 daeElement* pelt = daeSidRef(kinematics_axis_info->getAxis(), arr[ik]->getUrl().getElement()).resolve().elt;
00147 if( !!pelt ) {
00148
00149 bool bfound = false;
00150 FOREACH(itbinding,listAxisBindings) {
00151 if( itbinding->pkinematicaxis.cast() == pelt ) {
00152 itbinding->kinematics_axis_info = kinematics_axis_info;
00153 if( !!motion_axis_info ) {
00154 itbinding->motion_axis_info = motion_axis_info;
00155 }
00156 bfound = true;
00157 break;
00158 }
00159 }
00160 if( !bfound ) {
00161 ROS_WARN_STREAM(str(boost::format("could not find binding for axis: %s, %s\n")%kinematics_axis_info->getAxis()%pelt->getAttribute("sid")));
00162 return false;
00163 }
00164 return true;
00165 }
00166 }
00167 ROS_WARN_STREAM(str(boost::format("could not find kinematics axis target: %s\n")%kinematics_axis_info->getAxis()));
00168 return false;
00169 }
00170 };
00171
00172 struct USERDATA
00173 {
00174 USERDATA() {
00175 }
00176 USERDATA(double scale) : scale(scale) {
00177 }
00178 double scale;
00179 boost::shared_ptr<void> p;
00180 };
00181
00182 enum GeomType {
00183 GeomNone = 0,
00184 GeomBox = 1,
00185 GeomSphere = 2,
00186 GeomCylinder = 3,
00187 GeomTrimesh = 4,
00188 };
00189
00190 struct GEOMPROPERTIES
00191 {
00192 Pose _t;
00193 Vector3 vGeomData;
00194
00195
00196
00197 Color diffuseColor, ambientColor;
00198 std::vector<Vector3> vertices;
00199 std::vector<int> indices;
00200
00202 GeomType type;
00203
00204
00205
00206
00207 static void GenerateSphereTriangulation(std::vector<Vector3> realvertices, std::vector<int> realindices, int levels)
00208 {
00209 const double GTS_M_ICOSAHEDRON_X = 0.850650808352039932181540497063011072240401406;
00210 const double GTS_M_ICOSAHEDRON_Y = 0.525731112119133606025669084847876607285497935;
00211 const double GTS_M_ICOSAHEDRON_Z = 0;
00212 std::vector<Vector3> tempvertices[2];
00213 std::vector<int> tempindices[2];
00214
00215 tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_Z, +GTS_M_ICOSAHEDRON_X, -GTS_M_ICOSAHEDRON_Y));
00216 tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_X, +GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z));
00217 tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z, -GTS_M_ICOSAHEDRON_X));
00218 tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z, +GTS_M_ICOSAHEDRON_X));
00219 tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_X, -GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z));
00220 tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_Z, +GTS_M_ICOSAHEDRON_X, +GTS_M_ICOSAHEDRON_Y));
00221 tempvertices[0].push_back(Vector3(-GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z, +GTS_M_ICOSAHEDRON_X));
00222 tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_Z, -GTS_M_ICOSAHEDRON_X, -GTS_M_ICOSAHEDRON_Y));
00223 tempvertices[0].push_back(Vector3(-GTS_M_ICOSAHEDRON_X, +GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z));
00224 tempvertices[0].push_back(Vector3(-GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z, -GTS_M_ICOSAHEDRON_X));
00225 tempvertices[0].push_back(Vector3(-GTS_M_ICOSAHEDRON_X, -GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z));
00226 tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_Z, -GTS_M_ICOSAHEDRON_X, +GTS_M_ICOSAHEDRON_Y));
00227
00228 const int nindices=60;
00229 int indices[nindices] = {
00230 0, 1, 2,
00231 1, 3, 4,
00232 3, 5, 6,
00233 2, 4, 7,
00234 5, 6, 8,
00235 2, 7, 9,
00236 0, 5, 8,
00237 7, 9, 10,
00238 0, 1, 5,
00239 7, 10, 11,
00240 1, 3, 5,
00241 6, 10, 11,
00242 3, 6, 11,
00243 9, 10, 8,
00244 3, 4, 11,
00245 6, 8, 10,
00246 4, 7, 11,
00247 1, 2, 4,
00248 0, 8, 9,
00249 0, 2, 9
00250 };
00251
00252 Vector3 v[3];
00253
00254 for(int i = 0; i < nindices; i += 3 ) {
00255 v[0] = tempvertices[0][indices[i]];
00256 v[1] = tempvertices[0][indices[i+1]];
00257 v[2] = tempvertices[0][indices[i+2]];
00258 if( _dot3(v[0], _cross3(_sub3(v[1],v[0]),_sub3(v[2],v[0]))) < 0 ) {
00259 std::swap(indices[i], indices[i+1]);
00260 }
00261 }
00262
00263 tempindices[0].resize(nindices);
00264 std::copy(&indices[0],&indices[nindices],tempindices[0].begin());
00265 std::vector<Vector3>* curvertices = &tempvertices[0], *newvertices = &tempvertices[1];
00266 std::vector<int> *curindices = &tempindices[0], *newindices = &tempindices[1];
00267 while(levels-- > 0) {
00268
00269 newvertices->resize(0);
00270 newvertices->reserve(2*curvertices->size());
00271 newvertices->insert(newvertices->end(), curvertices->begin(), curvertices->end());
00272 newindices->resize(0);
00273 newindices->reserve(4*curindices->size());
00274
00275 std::map< uint64_t, int > mapnewinds;
00276 std::map< uint64_t, int >::iterator it;
00277
00278 for(size_t i = 0; i < curindices->size(); i += 3) {
00279
00280 v[0] = curvertices->at(curindices->at(i));
00281 v[1] = curvertices->at(curindices->at(i+1));
00282 v[2] = curvertices->at(curindices->at(i+2));
00283
00284 int inds[3];
00285 for(int j = 0; j < 3; ++j) {
00286 uint64_t key = ((uint64_t)curindices->at(i+j)<<32)|(uint64_t)curindices->at(i + ((j+1)%3));
00287 it = mapnewinds.find(key);
00288
00289 if( it == mapnewinds.end() ) {
00290 inds[j] = mapnewinds[key] = mapnewinds[(key<<32)|(key>>32)] = (int)newvertices->size();
00291 newvertices->push_back(_normalize3(_add3(v[j],v[(j+1)%3 ])));
00292 }
00293 else {
00294 inds[j] = it->second;
00295 }
00296 }
00297
00298 newindices->push_back(curindices->at(i)); newindices->push_back(inds[0]); newindices->push_back(inds[2]);
00299 newindices->push_back(inds[0]); newindices->push_back(curindices->at(i+1)); newindices->push_back(inds[1]);
00300 newindices->push_back(inds[2]); newindices->push_back(inds[0]); newindices->push_back(inds[1]);
00301 newindices->push_back(inds[2]); newindices->push_back(inds[1]); newindices->push_back(curindices->at(i+2));
00302 }
00303
00304 std::swap(newvertices,curvertices);
00305 std::swap(newindices,curindices);
00306 }
00307
00308 realvertices = *curvertices;
00309 realindices = *curindices;
00310 }
00311
00312 bool InitCollisionMesh(double fTessellation=1.0)
00313 {
00314 if( type == GeomTrimesh ) {
00315 return true;
00316 }
00317 indices.clear();
00318 vertices.clear();
00319
00320 if( fTessellation < 0.01f ) {
00321 fTessellation = 0.01f;
00322 }
00323
00324 switch(type) {
00325 case GeomSphere: {
00326
00327 GenerateSphereTriangulation(vertices,indices, 3 + (int)(logf(fTessellation) / logf(2.0f)) );
00328 double fRadius = vGeomData.x;
00329 FOREACH(it, vertices) {
00330 it->x *= fRadius;
00331 it->y *= fRadius;
00332 it->z *= fRadius;
00333 }
00334 break;
00335 }
00336 case GeomBox: {
00337
00338 Vector3 ex = vGeomData;
00339 Vector3 v[8] = { Vector3(ex.x, ex.y, ex.z),
00340 Vector3(ex.x, ex.y, -ex.z),
00341 Vector3(ex.x, -ex.y, ex.z),
00342 Vector3(ex.x, -ex.y, -ex.z),
00343 Vector3(-ex.x, ex.y, ex.z),
00344 Vector3(-ex.x, ex.y, -ex.z),
00345 Vector3(-ex.x, -ex.y, ex.z),
00346 Vector3(-ex.x, -ex.y, -ex.z) };
00347 const int nindices = 36;
00348 int startindices[] = {
00349 0, 1, 2,
00350 1, 2, 3,
00351 4, 5, 6,
00352 5, 6, 7,
00353 0, 1, 4,
00354 1, 4, 5,
00355 2, 3, 6,
00356 3, 6, 7,
00357 0, 2, 4,
00358 2, 4, 6,
00359 1, 3, 5,
00360 3, 5, 7
00361 };
00362
00363 for(int i = 0; i < nindices; i += 3 ) {
00364 Vector3 v1 = v[startindices[i]];
00365 Vector3 v2 = v[startindices[i+1]];
00366 Vector3 v3 = v[startindices[i+2]];
00367 if( _dot3(v1, _sub3(v2,_cross3(v1, _sub3(v3,v1)))) < 0 ) {
00368 std::swap(indices[i], indices[i+1]);
00369 }
00370 }
00371
00372 vertices.resize(8);
00373 std::copy(&v[0],&v[8],vertices.begin());
00374 indices.resize(nindices);
00375 std::copy(&startindices[0],&startindices[nindices],indices.begin());
00376 break;
00377 }
00378 case GeomCylinder: {
00379
00380 double rad = vGeomData.x, len = vGeomData.y*0.5f;
00381
00382 int numverts = (int)(fTessellation*24.0f) + 3;
00383 double dtheta = 2 * M_PI / (double)numverts;
00384 vertices.push_back(Vector3(0,0,len));
00385 vertices.push_back(Vector3(0,0,-len));
00386 vertices.push_back(Vector3(rad,0,len));
00387 vertices.push_back(Vector3(rad,0,-len));
00388
00389 for(int i = 0; i < numverts+1; ++i) {
00390 double s = rad * std::sin(dtheta * (double)i);
00391 double c = rad * std::cos(dtheta * (double)i);
00392
00393 int off = (int)vertices.size();
00394 vertices.push_back(Vector3(c, s, len));
00395 vertices.push_back(Vector3(c, s, -len));
00396
00397 indices.push_back(0); indices.push_back(off); indices.push_back(off-2);
00398 indices.push_back(1); indices.push_back(off-1); indices.push_back(off+1);
00399 indices.push_back(off-2); indices.push_back(off); indices.push_back(off-1);
00400 indices.push_back(off); indices.push_back(off-1); indices.push_back(off+1);
00401 }
00402 break;
00403 }
00404 default:
00405 BOOST_ASSERT(0);
00406 }
00407 return true;
00408 }
00409 };
00410
00411 public:
00412 ColladaModelReader(boost::shared_ptr<ModelInterface> model) : _dom(NULL), _nGlobalSensorId(0), _nGlobalManipulatorId(0), _model(model) {
00413 daeErrorHandler::setErrorHandler(this);
00414 _resourcedir = ".";
00415 }
00416 virtual ~ColladaModelReader() {
00417 _vuserdata.clear();
00418 _collada.reset();
00419 DAE::cleanup();
00420 }
00421
00422 bool InitFromFile(const std::string& filename) {
00423 ROS_DEBUG_STREAM(str(boost::format("init COLLADA reader version: %s, namespace: %s, filename: %s\n")%COLLADA_VERSION%COLLADA_NAMESPACE%filename));
00424 _collada.reset(new DAE);
00425 _dom = (domCOLLADA*)_collada->open(filename);
00426 if (!_dom) {
00427 return false;
00428 }
00429 _filename=filename;
00430
00431 size_t maxchildren = _countChildren(_dom);
00432 _vuserdata.resize(0);
00433 _vuserdata.reserve(maxchildren);
00434
00435 double dScale = 1.0;
00436 _processUserData(_dom, dScale);
00437 ROS_DEBUG_STREAM(str(boost::format("processed children: %d/%d\n")%_vuserdata.size()%maxchildren));
00438 return _Extract();
00439 }
00440
00441 bool InitFromData(const std::string& pdata) {
00442 ROS_DEBUG_STREAM(str(boost::format("init COLLADA reader version: %s, namespace: %s\n")%COLLADA_VERSION%COLLADA_NAMESPACE));
00443 _collada.reset(new DAE);
00444 _dom = (domCOLLADA*)_collada->openFromMemory(".",pdata.c_str());
00445 if (!_dom) {
00446 return false;
00447 }
00448
00449 size_t maxchildren = _countChildren(_dom);
00450 _vuserdata.resize(0);
00451 _vuserdata.reserve(maxchildren);
00452
00453 double dScale = 1.0;
00454 _processUserData(_dom, dScale);
00455 ROS_DEBUG_STREAM(str(boost::format("processed children: %d/%d\n")%_vuserdata.size()%maxchildren));
00456 return _Extract();
00457 }
00458
00459 protected:
00460
00462 bool _Extract()
00463 {
00464 _model->clear();
00465 std::list< std::pair<domInstance_kinematics_modelRef, boost::shared_ptr<KinematicsSceneBindings> > > listPossibleBodies;
00466 domCOLLADA::domSceneRef allscene = _dom->getScene();
00467 if( !allscene ) {
00468 return false;
00469 }
00470
00471
00472 for (size_t iscene = 0; iscene < allscene->getInstance_kinematics_scene_array().getCount(); iscene++) {
00473 domInstance_kinematics_sceneRef kiscene = allscene->getInstance_kinematics_scene_array()[iscene];
00474 domKinematics_sceneRef kscene = daeSafeCast<domKinematics_scene> (kiscene->getUrl().getElement().cast());
00475 if (!kscene) {
00476 continue;
00477 }
00478 boost::shared_ptr<KinematicsSceneBindings> bindings(new KinematicsSceneBindings());
00479 _ExtractKinematicsVisualBindings(allscene->getInstance_visual_scene(),kiscene,*bindings);
00480 _ExtractPhysicsBindings(allscene,*bindings);
00481 for(size_t ias = 0; ias < kscene->getInstance_articulated_system_array().getCount(); ++ias) {
00482 if( _ExtractArticulatedSystem(kscene->getInstance_articulated_system_array()[ias], *bindings) ) {
00483 _PostProcess();
00484 return true;
00485 }
00486 }
00487 for(size_t ikmodel = 0; ikmodel < kscene->getInstance_kinematics_model_array().getCount(); ++ikmodel) {
00488 listPossibleBodies.push_back(std::make_pair(kscene->getInstance_kinematics_model_array()[ikmodel], bindings));
00489 }
00490 }
00491
00492 FOREACH(it, listPossibleBodies) {
00493 if( _ExtractKinematicsModel(it->first, *it->second) ) {
00494 _PostProcess();
00495 return true;
00496 }
00497 }
00498
00499 return false;
00500 }
00501
00502 void _PostProcess()
00503 {
00504 std::map<std::string, std::string> parent_link_tree;
00505
00506 try
00507 {
00508 _model->initTree(parent_link_tree);
00509 }
00510 catch(ParseError &e)
00511 {
00512 ROS_ERROR("Failed to build tree: %s", e.what());
00513 }
00514
00515
00516 try
00517 {
00518 _model->initRoot(parent_link_tree);
00519 }
00520 catch(ParseError &e)
00521 {
00522 ROS_ERROR("Failed to find root link: %s", e.what());
00523 }
00524 }
00525
00527 bool _ExtractArticulatedSystem(domInstance_articulated_systemRef ias, KinematicsSceneBindings& bindings)
00528 {
00529 if( !ias ) {
00530 return false;
00531 }
00532 ROS_DEBUG_STREAM(str(boost::format("instance articulated system sid %s\n")%ias->getSid()));
00533 domArticulated_systemRef articulated_system = daeSafeCast<domArticulated_system> (ias->getUrl().getElement().cast());
00534 if( !articulated_system ) {
00535 return false;
00536 }
00537
00538 boost::shared_ptr<std::string> pinterface_type = _ExtractInterfaceType(ias->getExtra_array());
00539 if( !pinterface_type ) {
00540 pinterface_type = _ExtractInterfaceType(articulated_system->getExtra_array());
00541 }
00542 if( !!pinterface_type ) {
00543 ROS_DEBUG_STREAM(str(boost::format("robot type: %s")%(*pinterface_type)));
00544 }
00545
00546
00547 if( _model->name_.size() == 0 && !!ias->getName() ) {
00548 _model->name_ = ias->getName();
00549 }
00550 if( _model->name_.size() == 0 && !!ias->getSid()) {
00551 _model->name_ = ias->getSid();
00552 }
00553 if( _model->name_.size() == 0 && !!articulated_system->getName() ) {
00554 _model->name_ = articulated_system->getName();
00555 }
00556 if( _model->name_.size() == 0 && !!articulated_system->getId()) {
00557 _model->name_ = articulated_system->getId();
00558 }
00559
00560 if( !!articulated_system->getMotion() ) {
00561 domInstance_articulated_systemRef ias_new = articulated_system->getMotion()->getInstance_articulated_system();
00562 if( !!articulated_system->getMotion()->getTechnique_common() ) {
00563 for(size_t i = 0; i < articulated_system->getMotion()->getTechnique_common()->getAxis_info_array().getCount(); ++i) {
00564 domMotion_axis_infoRef motion_axis_info = articulated_system->getMotion()->getTechnique_common()->getAxis_info_array()[i];
00565
00566 domKinematics_axis_infoRef kinematics_axis_info = daeSafeCast<domKinematics_axis_info>(daeSidRef(motion_axis_info->getAxis(), ias_new->getUrl().getElement()).resolve().elt);
00567 if( !!kinematics_axis_info ) {
00568
00569 daeElement* pparent = kinematics_axis_info->getParent();
00570 while(!!pparent && pparent->typeID() != domKinematics::ID()) {
00571 pparent = pparent->getParent();
00572 }
00573 BOOST_ASSERT(!!pparent);
00574 bindings.AddAxisInfo(daeSafeCast<domKinematics>(pparent)->getInstance_kinematics_model_array(), kinematics_axis_info, motion_axis_info);
00575 }
00576 else {
00577 ROS_WARN_STREAM(str(boost::format("failed to find kinematics axis %s\n")%motion_axis_info->getAxis()));
00578 }
00579 }
00580 }
00581 if( !_ExtractArticulatedSystem(ias_new,bindings) ) {
00582 return false;
00583 }
00584 }
00585 else {
00586 if( !articulated_system->getKinematics() ) {
00587 ROS_WARN_STREAM(str(boost::format("collada <kinematics> tag empty? instance_articulated_system=%s\n")%ias->getID()));
00588 return true;
00589 }
00590
00591 if( !!articulated_system->getKinematics()->getTechnique_common() ) {
00592 for(size_t i = 0; i < articulated_system->getKinematics()->getTechnique_common()->getAxis_info_array().getCount(); ++i) {
00593 bindings.AddAxisInfo(articulated_system->getKinematics()->getInstance_kinematics_model_array(), articulated_system->getKinematics()->getTechnique_common()->getAxis_info_array()[i], NULL);
00594 }
00595 }
00596
00597 for(size_t ik = 0; ik < articulated_system->getKinematics()->getInstance_kinematics_model_array().getCount(); ++ik) {
00598 _ExtractKinematicsModel(articulated_system->getKinematics()->getInstance_kinematics_model_array()[ik],bindings);
00599 }
00600 }
00601
00602 _ExtractRobotAttachedActuators(articulated_system);
00603 _ExtractRobotManipulators(articulated_system);
00604 _ExtractRobotAttachedSensors(articulated_system);
00605 return true;
00606 }
00607
00608 bool _ExtractKinematicsModel(domInstance_kinematics_modelRef ikm, KinematicsSceneBindings& bindings)
00609 {
00610 if( !ikm ) {
00611 return false;
00612 }
00613 ROS_DEBUG_STREAM(str(boost::format("instance kinematics model sid %s\n")%ikm->getSid()));
00614 domKinematics_modelRef kmodel = daeSafeCast<domKinematics_model> (ikm->getUrl().getElement().cast());
00615 if (!kmodel) {
00616 ROS_WARN_STREAM(str(boost::format("%s does not reference valid kinematics\n")%ikm->getSid()));
00617 return false;
00618 }
00619 domPhysics_modelRef pmodel;
00620 boost::shared_ptr<std::string> pinterface_type = _ExtractInterfaceType(ikm->getExtra_array());
00621 if( !pinterface_type ) {
00622 pinterface_type = _ExtractInterfaceType(kmodel->getExtra_array());
00623 }
00624 if( !!pinterface_type ) {
00625 ROS_DEBUG_STREAM(str(boost::format("kinbody interface type: %s")%(*pinterface_type)));
00626 }
00627
00628
00629 domNodeRef pvisualnode;
00630 FOREACH(it, bindings.listKinematicsVisualBindings) {
00631 if( it->second == ikm ) {
00632 pvisualnode = it->first;
00633 break;
00634 }
00635 }
00636 if( !pvisualnode ) {
00637 ROS_WARN_STREAM(str(boost::format("failed to find visual node for instance kinematics model %s\n")%ikm->getSid()));
00638 return false;
00639 }
00640
00641 if( _model->name_.size() == 0 && !!ikm->getName() ) {
00642 _model->name_ = ikm->getName();
00643 }
00644 if( _model->name_.size() == 0 && !!ikm->getID() ) {
00645 _model->name_ = ikm->getID();
00646 }
00647
00648 if (!_ExtractKinematicsModel(kmodel, pvisualnode, pmodel, bindings)) {
00649 ROS_WARN_STREAM(str(boost::format("failed to load kinbody from kinematics model %s\n")%kmodel->getID()));
00650 return false;
00651 }
00652 return true;
00653 }
00654
00656 bool _ExtractKinematicsModel(domKinematics_modelRef kmodel, domNodeRef pnode, domPhysics_modelRef pmodel, const KinematicsSceneBindings& bindings)
00657 {
00658 std::vector<domJointRef> vdomjoints;
00659 ROS_DEBUG_STREAM(str(boost::format("kinematics model: %s\n")%_model->name_));
00660 if( !!pnode ) {
00661 ROS_DEBUG_STREAM(str(boost::format("node name: %s\n")%pnode->getId()));
00662 }
00663
00664
00665 domKinematics_model_techniqueRef ktec = kmodel->getTechnique_common();
00666
00667
00668 for (size_t ijoint = 0; ijoint < ktec->getJoint_array().getCount(); ++ijoint) {
00669 vdomjoints.push_back(ktec->getJoint_array()[ijoint]);
00670 }
00671
00672
00673 for (size_t ijoint = 0; ijoint < ktec->getInstance_joint_array().getCount(); ++ijoint) {
00674 domJointRef pelt = daeSafeCast<domJoint> (ktec->getInstance_joint_array()[ijoint]->getUrl().getElement());
00675 if (!pelt) {
00676 ROS_WARN_STREAM("failed to get joint from instance\n");
00677 }
00678 else {
00679 vdomjoints.push_back(pelt);
00680 }
00681 }
00682
00683 ROS_DEBUG_STREAM(str(boost::format("Number of root links in the kmodel %d\n")%ktec->getLink_array().getCount()));
00684 for (size_t ilink = 0; ilink < ktec->getLink_array().getCount(); ++ilink) {
00685 domLinkRef pdomlink = ktec->getLink_array()[ilink];
00686 _RootOrigin = _poseFromMatrix(_ExtractFullTransform(pdomlink));
00687 ROS_DEBUG("RootOrigin: %s %lf %lf %lf %lf %lf %lf %lf",
00688 pdomlink->getName(),
00689 _RootOrigin.position.x, _RootOrigin.position.y, _RootOrigin.position.z,
00690 _RootOrigin.rotation.x, _RootOrigin.rotation.y, _RootOrigin.rotation.z, _RootOrigin.rotation.w);
00691
00692 domNodeRef pvisualnode;
00693 FOREACH(it, bindings.listKinematicsVisualBindings) {
00694 if(strcmp(it->first->getName() ,pdomlink->getName()) == 0) {
00695 pvisualnode = it->first;
00696 break;
00697 }
00698 }
00699 if (!!pvisualnode) {
00700 _VisualRootOrigin = _poseFromMatrix(_getNodeParentTransform(pvisualnode));
00701 ROS_DEBUG("VisualRootOrigin: %s %lf %lf %lf %lf %lf %lf %lf",
00702 pdomlink->getName(),
00703 _VisualRootOrigin.position.x, _VisualRootOrigin.position.y, _VisualRootOrigin.position.z,
00704 _VisualRootOrigin.rotation.x, _VisualRootOrigin.rotation.y, _VisualRootOrigin.rotation.z, _VisualRootOrigin.rotation.w);
00705 }
00706 _ExtractLink(pdomlink, ilink == 0 ? pnode : domNodeRef(), Pose(), Pose(), vdomjoints, bindings);
00707 }
00708
00709
00710 for (size_t iform = 0; iform < ktec->getFormula_array().getCount(); ++iform) {
00711 domFormulaRef pf = ktec->getFormula_array()[iform];
00712 if (!pf->getTarget()) {
00713 ROS_WARN_STREAM("formula target not valid\n");
00714 continue;
00715 }
00716
00717
00718 boost::shared_ptr<Joint> pjoint = _getJointFromRef(pf->getTarget()->getParam()->getValue(),pf);
00719 if (!pjoint) {
00720 continue;
00721 }
00722
00723 if (!!pf->getTechnique_common()) {
00724 daeElementRef peltmath;
00725 daeTArray<daeElementRef> children;
00726 pf->getTechnique_common()->getChildren(children);
00727 for (size_t ichild = 0; ichild < children.getCount(); ++ichild) {
00728 daeElementRef pelt = children[ichild];
00729 if (_checkMathML(pelt,std::string("math")) ) {
00730 peltmath = pelt;
00731 }
00732 else {
00733 ROS_WARN_STREAM(str(boost::format("unsupported formula element: %s\n")%pelt->getElementName()));
00734 }
00735 }
00736 if (!!peltmath) {
00737
00738
00739 double a = 1, b = 0;
00740 daeElementRef psymboljoint;
00741 BOOST_ASSERT(peltmath->getChildren().getCount()>0);
00742 daeElementRef papplyelt = peltmath->getChildren()[0];
00743 BOOST_ASSERT(_checkMathML(papplyelt,"apply"));
00744 BOOST_ASSERT(papplyelt->getChildren().getCount()>0);
00745 if( _checkMathML(papplyelt->getChildren()[0],"plus") ) {
00746 BOOST_ASSERT(papplyelt->getChildren().getCount()==3);
00747 daeElementRef pa = papplyelt->getChildren()[1];
00748 daeElementRef pb = papplyelt->getChildren()[2];
00749 if( !_checkMathML(papplyelt->getChildren()[1],"apply") ) {
00750 std::swap(pa,pb);
00751 }
00752 if( !_checkMathML(pa,"csymbol") ) {
00753 BOOST_ASSERT(_checkMathML(pa,"apply"));
00754 BOOST_ASSERT(_checkMathML(pa->getChildren()[0],"times"));
00755 if( _checkMathML(pa->getChildren()[1],"csymbol") ) {
00756 psymboljoint = pa->getChildren()[1];
00757 BOOST_ASSERT(_checkMathML(pa->getChildren()[2],"cn"));
00758 std::stringstream ss(pa->getChildren()[2]->getCharData());
00759 ss >> a;
00760 }
00761 else {
00762 psymboljoint = pa->getChildren()[2];
00763 BOOST_ASSERT(_checkMathML(pa->getChildren()[1],"cn"));
00764 std::stringstream ss(pa->getChildren()[1]->getCharData());
00765 ss >> a;
00766 }
00767 }
00768 else {
00769 psymboljoint = pa;
00770 }
00771 BOOST_ASSERT(_checkMathML(pb,"cn"));
00772 {
00773 std::stringstream ss(pb->getCharData());
00774 ss >> b;
00775 }
00776 }
00777 else if( _checkMathML(papplyelt->getChildren()[0],"minus") ) {
00778 BOOST_ASSERT(_checkMathML(papplyelt->getChildren()[1],"csymbol"));
00779 a = -1;
00780 psymboljoint = papplyelt->getChildren()[1];
00781 }
00782 else {
00783 BOOST_ASSERT(_checkMathML(papplyelt->getChildren()[0],"csymbol"));
00784 psymboljoint = papplyelt->getChildren()[0];
00785 }
00786 BOOST_ASSERT(psymboljoint->hasAttribute("encoding"));
00787 BOOST_ASSERT(psymboljoint->getAttribute("encoding")==std::string("COLLADA"));
00788 boost::shared_ptr<Joint> pbasejoint = _getJointFromRef(psymboljoint->getCharData().c_str(),pf);
00789 if( !!pbasejoint ) {
00790
00791 pjoint->mimic.reset(new JointMimic());
00792 pjoint->mimic->joint_name = pbasejoint->name;
00793 pjoint->mimic->multiplier = a;
00794 pjoint->mimic->offset = b;
00795 ROS_DEBUG_STREAM(str(boost::format("assigning joint %s to mimic %s %f %f\n")%pjoint->name%pbasejoint->name%a%b));
00796 }
00797 }
00798 }
00799 }
00800 return true;
00801 }
00802
00804 boost::shared_ptr<Link> _ExtractLink(const domLinkRef pdomlink,const domNodeRef pdomnode, const Pose& tParentWorldLink, const Pose& tParentLink, const std::vector<domJointRef>& vdomjoints, const KinematicsSceneBindings& bindings) {
00805 const std::list<JointAxisBinding>& listAxisBindings = bindings.listAxisBindings;
00806
00807 std::string linkname = _ExtractLinkName(pdomlink);
00808 if( linkname.size() == 0 ) {
00809 ROS_WARN_STREAM("<link> has no name or id, falling back to <node>!\n");
00810 if( !!pdomnode ) {
00811 if (!!pdomnode->getName()) {
00812 linkname = pdomnode->getName();
00813 }
00814 if( linkname.size() == 0 && !!pdomnode->getID()) {
00815 linkname = pdomnode->getID();
00816 }
00817 }
00818 }
00819
00820 boost::shared_ptr<Link> plink;
00821 _model->getLink(linkname,plink);
00822 if( !plink ) {
00823 plink.reset(new Link());
00824 plink->name = linkname;
00825 plink->visual.reset(new Visual());
00826 plink->visual->material_name = "";
00827 plink->visual->material.reset(new Material());
00828 plink->visual->material->name = "Red";
00829 plink->visual->material->color.r = 0.0;
00830 plink->visual->material->color.g = 1.0;
00831 plink->visual->material->color.b = 0.0;
00832 plink->visual->material->color.a = 1.0;
00833 plink->inertial.reset();
00834 _model->links_.insert(std::make_pair(linkname,plink));
00835 }
00836
00837 _getUserData(pdomlink)->p = plink;
00838 if( !!pdomnode ) {
00839 ROS_DEBUG_STREAM(str(boost::format("Node Id %s and Name %s\n")%pdomnode->getId()%pdomnode->getName()));
00840 }
00841
00842
00843 domInstance_rigid_bodyRef irigidbody;
00844 domRigid_bodyRef rigidbody;
00845 bool bFoundBinding = false;
00846 FOREACH(itlinkbinding, bindings.listLinkBindings) {
00847 if( !!pdomnode->getID() && !!itlinkbinding->node->getID() && strcmp(pdomnode->getID(),itlinkbinding->node->getID()) == 0 ) {
00848 bFoundBinding = true;
00849 irigidbody = itlinkbinding->irigidbody;
00850 rigidbody = itlinkbinding->rigidbody;
00851 }
00852 }
00853
00854 if( !!rigidbody && !!rigidbody->getTechnique_common() ) {
00855 domRigid_body::domTechnique_commonRef rigiddata = rigidbody->getTechnique_common();
00856 if( !!rigiddata->getMass() ) {
00857 if ( !plink->inertial ) {
00858 plink->inertial.reset(new Inertial());
00859 }
00860 plink->inertial->mass = rigiddata->getMass()->getValue();
00861 }
00862 if( !!rigiddata->getInertia() ) {
00863 if ( !plink->inertial ) {
00864 plink->inertial.reset(new Inertial());
00865 }
00866 plink->inertial->ixx = rigiddata->getInertia()->getValue()[0];
00867 plink->inertial->iyy = rigiddata->getInertia()->getValue()[1];
00868 plink->inertial->izz = rigiddata->getInertia()->getValue()[2];
00869 }
00870 if( !!rigiddata->getMass_frame() ) {
00871 if ( !plink->inertial ) {
00872 plink->inertial.reset(new Inertial());
00873 }
00874
00875 Pose tlink = _poseFromMatrix(_ExtractFullTransform(pdomlink));
00876 plink->inertial->origin = _poseMult(_poseInverse(_poseMult(_poseInverse(_RootOrigin),
00877 _poseMult(tParentWorldLink, tlink))),
00878 _poseFromMatrix(_ExtractFullTransform(rigiddata->getMass_frame())));
00879 }
00880 }
00881
00882 std::list<GEOMPROPERTIES> listGeomProperties;
00883 if (!pdomlink) {
00884 ROS_WARN_STREAM("Extract object NOT kinematics !!!\n");
00885 _ExtractGeometry(pdomnode,listGeomProperties,listAxisBindings,Pose());
00886 }
00887 else {
00888 ROS_DEBUG_STREAM(str(boost::format("Attachment link elements: %d\n")%pdomlink->getAttachment_full_array().getCount()));
00889 Pose tlink = _poseFromMatrix(_ExtractFullTransform(pdomlink));
00890 ROS_DEBUG("tlink: %s: %lf %lf %lf %lf %lf %lf %lf",
00891 linkname.c_str(),
00892 tlink.position.x, tlink.position.y, tlink.position.z,
00893 tlink.rotation.x, tlink.rotation.y, tlink.rotation.z, tlink.rotation.w);
00894 plink->visual->origin = _poseMult(tParentLink, tlink);
00895
00896
00897
00898
00899 _ExtractGeometry(pdomnode, listGeomProperties, listAxisBindings,
00900 _poseMult(_poseMult(tParentWorldLink,tlink), plink->visual->origin));
00901
00902 ROS_DEBUG_STREAM(str(boost::format("After ExtractGeometry Attachment link elements: %d\n")%pdomlink->getAttachment_full_array().getCount()));
00903
00904
00905 for (size_t iatt = 0; iatt < pdomlink->getAttachment_full_array().getCount(); ++iatt) {
00906 domLink::domAttachment_fullRef pattfull = pdomlink->getAttachment_full_array()[iatt];
00907
00908
00909 Pose tatt = _poseFromMatrix(_ExtractFullTransform(pattfull));
00910
00911
00912 daeElement* peltjoint = daeSidRef(pattfull->getJoint(), pattfull).resolve().elt;
00913 domJointRef pdomjoint = daeSafeCast<domJoint> (peltjoint);
00914
00915 if (!pdomjoint) {
00916 domInstance_jointRef pdomijoint = daeSafeCast<domInstance_joint> (peltjoint);
00917 if (!!pdomijoint) {
00918 pdomjoint = daeSafeCast<domJoint> (pdomijoint->getUrl().getElement().cast());
00919 }
00920 }
00921
00922 if (!pdomjoint || pdomjoint->typeID() != domJoint::ID()) {
00923 ROS_WARN_STREAM(str(boost::format("could not find attached joint %s!\n")%pattfull->getJoint()));
00924 return boost::shared_ptr<Link>();
00925 }
00926
00927
00928 if (!pattfull->getLink()) {
00929 ROS_WARN_STREAM(str(boost::format("joint %s needs to be attached to a valid link\n")%pdomjoint->getID()));
00930 continue;
00931 }
00932
00933
00934 daeTArray<domAxis_constraintRef> vdomaxes = pdomjoint->getChildrenByType<domAxis_constraint>();
00935 domNodeRef pchildnode;
00936
00937
00938 FOREACHC(itaxisbinding,listAxisBindings) {
00939 for (size_t ic = 0; ic < vdomaxes.getCount(); ++ic) {
00940
00941 if (vdomaxes[ic] == itaxisbinding->pkinematicaxis) {
00942 pchildnode = itaxisbinding->visualnode;
00943 break;
00944 }
00945 }
00946 if( !!pchildnode ) {
00947 break;
00948 }
00949 }
00950 if (!pchildnode) {
00951 ROS_DEBUG_STREAM(str(boost::format("joint %s has no visual binding\n")%pdomjoint->getID()));
00952 }
00953
00954
00955 std::vector<boost::shared_ptr<Joint> > vjoints(vdomaxes.getCount());
00956 for (size_t ic = 0; ic < vdomaxes.getCount(); ++ic) {
00957 bool joint_active = true;
00958 FOREACHC(itaxisbinding,listAxisBindings) {
00959 if (vdomaxes[ic] == itaxisbinding->pkinematicaxis) {
00960 if( !!itaxisbinding->kinematics_axis_info ) {
00961 if( !!itaxisbinding->kinematics_axis_info->getActive() ) {
00962 joint_active = resolveBool(itaxisbinding->kinematics_axis_info->getActive(),itaxisbinding->kinematics_axis_info);
00963 }
00964 }
00965 break;
00966 }
00967 }
00968
00969 boost::shared_ptr<Joint> pjoint(new Joint());
00970 pjoint->limits.reset(new JointLimits());
00971 pjoint->limits->velocity = 0.0;
00972 pjoint->limits->effort = 0.0;
00973 pjoint->parent_link_name = plink->name;
00974
00975 if( !!pdomjoint->getName() ) {
00976 pjoint->name = pdomjoint->getName();
00977 }
00978 else {
00979 pjoint->name = str(boost::format("dummy%d")%_model->joints_.size());
00980 }
00981
00982 if( !joint_active ) {
00983 ROS_INFO_STREAM(str(boost::format("joint %s is passive, but adding to hierarchy\n")%pjoint->name));
00984 }
00985
00986 domAxis_constraintRef pdomaxis = vdomaxes[ic];
00987 if( strcmp(pdomaxis->getElementName(), "revolute") == 0 ) {
00988 pjoint->type = Joint::REVOLUTE;
00989 }
00990 else if( strcmp(pdomaxis->getElementName(), "prismatic") == 0 ) {
00991 pjoint->type = Joint::PRISMATIC;
00992 }
00993 else {
00994 ROS_WARN_STREAM(str(boost::format("unsupported joint type: %s\n")%pdomaxis->getElementName()));
00995 }
00996
00997 _getUserData(pdomjoint)->p = pjoint;
00998 _getUserData(pdomaxis)->p = boost::shared_ptr<int>(new int(_model->joints_.size()));
00999 _model->joints_[pjoint->name] = pjoint;
01000 vjoints[ic] = pjoint;
01001 }
01002
01003 boost::shared_ptr<Link> pchildlink = _ExtractLink(pattfull->getLink(), pchildnode, _poseMult(_poseMult(tParentWorldLink,tlink), tatt), tatt, vdomjoints, bindings);
01004
01005 if (!pchildlink) {
01006 ROS_WARN_STREAM(str(boost::format("Link has no child: %s\n")%plink->name));
01007 continue;
01008 }
01009
01010 int numjoints = 0;
01011 for (size_t ic = 0; ic < vdomaxes.getCount(); ++ic) {
01012 domKinematics_axis_infoRef kinematics_axis_info;
01013 domMotion_axis_infoRef motion_axis_info;
01014 FOREACHC(itaxisbinding,listAxisBindings) {
01015 bool bfound = false;
01016 if (vdomaxes[ic] == itaxisbinding->pkinematicaxis) {
01017 kinematics_axis_info = itaxisbinding->kinematics_axis_info;
01018 motion_axis_info = itaxisbinding->motion_axis_info;
01019 bfound = true;
01020 break;
01021 }
01022 }
01023 domAxis_constraintRef pdomaxis = vdomaxes[ic];
01024 if (!pchildlink) {
01025
01026
01027 ROS_WARN_STREAM(str(boost::format("creating dummy link %s, num joints %d\n")%plink->name%numjoints));
01028
01029 std::stringstream ss;
01030 ss << plink->name;
01031 ss <<"_dummy" << numjoints;
01032 pchildlink.reset(new Link());
01033 pchildlink->name = ss.str();
01034 _model->links_.insert(std::make_pair(pchildlink->name,pchildlink));
01035 }
01036
01037 ROS_DEBUG_STREAM(str(boost::format("Joint %s assigned %d \n")%vjoints[ic]->name%ic));
01038 boost::shared_ptr<Joint> pjoint = vjoints[ic];
01039 pjoint->child_link_name = pchildlink->name;
01040
01041
01042 {
01043 Vector3 ax(pdomaxis->getAxis()->getValue()[0],
01044 pdomaxis->getAxis()->getValue()[1],
01045 pdomaxis->getAxis()->getValue()[2]);
01046
01047
01048 ax = tatt.rotation * ax;
01049
01050 pjoint->axis.x = ax.x;
01051 pjoint->axis.y = ax.y;
01052 pjoint->axis.z = ax.z;
01053 }
01054
01055 if (!motion_axis_info) {
01056 ROS_WARN_STREAM(str(boost::format("No motion axis info for joint %s\n")%pjoint->name));
01057 }
01058
01059
01060 if (!!motion_axis_info) {
01061 if (!!motion_axis_info->getSpeed()) {
01062 pjoint->limits->velocity = resolveFloat(motion_axis_info->getSpeed(),motion_axis_info);
01063 ROS_DEBUG("... Joint Speed: %f...\n",pjoint->limits->velocity);
01064 }
01065 if (!!motion_axis_info->getAcceleration()) {
01066 pjoint->limits->effort = resolveFloat(motion_axis_info->getAcceleration(),motion_axis_info);
01067 ROS_DEBUG("... Joint effort: %f...\n",pjoint->limits->effort);
01068 }
01069 }
01070
01071 bool joint_locked = false;
01072 bool kinematics_limits = false;
01073
01074 if (!!kinematics_axis_info) {
01075 if (!!kinematics_axis_info->getLocked()) {
01076 joint_locked = resolveBool(kinematics_axis_info->getLocked(),kinematics_axis_info);
01077 }
01078
01079 if (joint_locked) {
01080 if( pjoint->type == Joint::REVOLUTE || pjoint->type ==Joint::PRISMATIC) {
01081 ROS_WARN_STREAM("lock joint!!\n");
01082 pjoint->limits->lower = 0;
01083 pjoint->limits->upper = 0;
01084 }
01085 }
01086 else if (kinematics_axis_info->getLimits()) {
01087 kinematics_limits = true;
01088 double fscale = (pjoint->type == Joint::REVOLUTE) ? (M_PI/180.0f) : _GetUnitScale(kinematics_axis_info);
01089 if( pjoint->type == Joint::REVOLUTE || pjoint->type ==Joint::PRISMATIC) {
01090 pjoint->limits->lower = fscale*(double)(resolveFloat(kinematics_axis_info->getLimits()->getMin(),kinematics_axis_info));
01091 pjoint->limits->upper = fscale*(double)(resolveFloat(kinematics_axis_info->getLimits()->getMax(),kinematics_axis_info));
01092 if ( pjoint->limits->lower == 0 && pjoint->limits->upper == 0 ) {
01093 pjoint->type = Joint::FIXED;
01094 }
01095 }
01096 }
01097 }
01098
01099
01100 if (!kinematics_axis_info || (!joint_locked && !kinematics_limits)) {
01101
01102 if( !pdomaxis->getLimits() ) {
01103 ROS_DEBUG_STREAM(str(boost::format("There are NO LIMITS in joint %s:%d ...\n")%pjoint->name%kinematics_limits));
01104 if( pjoint->type == Joint::REVOLUTE ) {
01105 pjoint->type = Joint::CONTINUOUS;
01106 pjoint->limits->lower = -M_PI;
01107 pjoint->limits->upper = M_PI;
01108 }
01109 else {
01110 pjoint->limits->lower = -100000;
01111 pjoint->limits->upper = 100000;
01112 }
01113 }
01114 else {
01115 ROS_DEBUG_STREAM(str(boost::format("There are LIMITS in joint %s ...\n")%pjoint->name));
01116 double fscale = (pjoint->type == Joint::REVOLUTE) ? (M_PI/180.0f) : _GetUnitScale(pdomaxis);
01117 pjoint->limits->lower = (double)pdomaxis->getLimits()->getMin()->getValue()*fscale;
01118 pjoint->limits->upper = (double)pdomaxis->getLimits()->getMax()->getValue()*fscale;
01119 if ( pjoint->limits->lower == 0 && pjoint->limits->upper == 0 ) {
01120 pjoint->type = Joint::FIXED;
01121 }
01122 }
01123 }
01124
01125
01126 pjoint->parent_to_joint_origin_transform = _poseMult(tatt,_poseFromMatrix(_ExtractFullTransform(pattfull->getLink())));
01127 if (pjoint->limits->velocity == 0.0) {
01128 pjoint->limits->velocity = pjoint->type == Joint::PRISMATIC ? 0.01 : 0.5f;
01129 }
01130 pchildlink.reset();
01131 ++numjoints;
01132 }
01133 }
01134 }
01135
01136 if( pdomlink->getAttachment_start_array().getCount() > 0 ) {
01137 ROS_WARN("urdf collada reader does not support attachment_start\n");
01138 }
01139 if( pdomlink->getAttachment_end_array().getCount() > 0 ) {
01140 ROS_WARN("urdf collada reader does not support attachment_end\n");
01141 }
01142
01143 plink->visual->geometry = _CreateGeometry(plink->name, listGeomProperties);
01144
01145
01146
01147
01148
01149
01150 if( !plink->visual->geometry ) {
01151 plink->visual.reset();
01152 plink->collision.reset();
01153 } else {
01154
01155 plink->collision.reset(new Collision());
01156 plink->collision->geometry = plink->visual->geometry;
01157 plink->collision->origin = plink->visual->origin;
01158 }
01159
01160
01161
01162
01163
01164
01165
01166 return plink;
01167 }
01168
01169 boost::shared_ptr<Geometry> _CreateGeometry(const std::string& name, const std::list<GEOMPROPERTIES>& listGeomProperties)
01170 {
01171 std::vector<std::vector<Vector3> > vertices;
01172 std::vector<std::vector<int> > indices;
01173 std::vector<Color> ambients;
01174 std::vector<Color> diffuses;
01175 unsigned int index, vert_counter;
01176 vertices.resize(listGeomProperties.size());
01177 indices.resize(listGeomProperties.size());
01178 ambients.resize(listGeomProperties.size());
01179 diffuses.resize(listGeomProperties.size());
01180 index = 0;
01181 vert_counter = 0;
01182 FOREACHC(it, listGeomProperties) {
01183 vertices[index].resize(it->vertices.size());
01184 for(size_t i = 0; i < it->vertices.size(); ++i) {
01185 vertices[index][i] = _poseMult(it->_t, it->vertices[i]);
01186 vert_counter++;
01187 }
01188 indices[index].resize(it->indices.size());
01189 for(size_t i = 0; i < it->indices.size(); ++i) {
01190 indices[index][i] = it->indices[i];
01191 }
01192 ambients[index] = it->ambientColor;
01193 diffuses[index] = it->diffuseColor;
01194
01195
01196 ambients[index].r *= 0.5; ambients[index].g *= 0.5; ambients[index].b *= 0.5;
01197 if ( ambients[index].r == 0.0 ) {
01198 ambients[index].r = 0.0001;
01199 }
01200 if ( ambients[index].g == 0.0 ) {
01201 ambients[index].g = 0.0001;
01202 }
01203 if ( ambients[index].b == 0.0 ) {
01204 ambients[index].b = 0.0001;
01205 }
01206 index++;
01207 }
01208
01209 if (vert_counter == 0) {
01210 boost::shared_ptr<Mesh> ret;
01211 ret.reset();
01212 return ret;
01213 }
01214
01215 boost::shared_ptr<Mesh> geometry(new Mesh());
01216 geometry->type = Geometry::MESH;
01217 geometry->scale.x = 1;
01218 geometry->scale.y = 1;
01219 geometry->scale.z = 1;
01220
01221
01222 std::stringstream daedata;
01223 daedata << str(boost::format("<?xml version=\"1.0\" encoding=\"utf-8\"?>\n\
01224 <COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n\
01225 <asset>\n\
01226 <contributor>\n\
01227 <author>Rosen Diankov</author>\n\
01228 <comments>\n\
01229 robot_model/urdf temporary collada geometry\n\
01230 </comments>\n\
01231 </contributor>\n\
01232 <unit name=\"meter\" meter=\"1.0\"/>\n\
01233 <up_axis>Y_UP</up_axis>\n\
01234 </asset>\n\
01235 <library_materials>\n"));
01236 for(unsigned int i=0; i < index; i++) {
01237 daedata << str(boost::format("\
01238 <material id=\"blinn2%d\" name=\"blinn2%d\">\n\
01239 <instance_effect url=\"#blinn2-fx%d\"/>\n\
01240 </material>\n")%i%i%i);
01241 }
01242 daedata << "\
01243 </library_materials>\n\
01244 <library_effects>\n";
01245 for(unsigned int i=0; i < index; i++) {
01246 daedata << str(boost::format("\
01247 <effect id=\"blinn2-fx%d\">\n\
01248 <profile_COMMON>\n\
01249 <technique sid=\"common\">\n\
01250 <phong>\n\
01251 <ambient>\n\
01252 <color>%f %f %f %f</color>\n\
01253 </ambient>\n\
01254 <diffuse>\n\
01255 <color>%f %f %f %f</color>\n\
01256 </diffuse>\n\
01257 <specular>\n\
01258 <color>0 0 0 1</color>\n\
01259 </specular>\n\
01260 </phong>\n\
01261 </technique>\n\
01262 </profile_COMMON>\n\
01263 </effect>\n")%i%ambients[i].r%ambients[i].g%ambients[i].b%ambients[i].a%diffuses[i].r%diffuses[i].g%diffuses[i].b%diffuses[i].a);
01264 }
01265 daedata << str(boost::format("\
01266 </library_effects>\n\
01267 <library_geometries>\n"));
01268
01269 for(unsigned int i=0; i < index; i++) {
01270 daedata << str(boost::format("\
01271 <geometry id=\"base2_M1KShape%d\" name=\"base2_M1KShape%d\">\n\
01272 <mesh>\n\
01273 <source id=\"geo0.positions\">\n\
01274 <float_array id=\"geo0.positions-array\" count=\"%d\">")%i%i%(vertices[i].size()*3));
01275 FOREACH(it,vertices[i]) {
01276 daedata << it->x << " " << it->y << " " << it->z << " ";
01277 }
01278 daedata << str(boost::format("\n\
01279 </float_array>\n\
01280 <technique_common>\n\
01281 <accessor count=\"%d\" source=\"#geo0.positions-array\" stride=\"3\">\n\
01282 <param name=\"X\" type=\"float\"/>\n\
01283 <param name=\"Y\" type=\"float\"/>\n\
01284 <param name=\"Z\" type=\"float\"/>\n\
01285 </accessor>\n\
01286 </technique_common>\n\
01287 </source>\n\
01288 <vertices id=\"geo0.vertices\">\n\
01289 <input semantic=\"POSITION\" source=\"#geo0.positions\"/>\n\
01290 </vertices>\n\
01291 <triangles count=\"%d\" material=\"lambert2SG\">\n\
01292 <input offset=\"0\" semantic=\"VERTEX\" source=\"#geo0.vertices\"/>\n\
01293 <p>")%vertices[i].size()%(indices[i].size()/3));
01294
01295 FOREACH(it,indices[i]) {
01296 daedata << *it << " ";
01297 }
01298 daedata << str(boost::format("</p>\n\
01299 </triangles>\n\
01300 </mesh>\n\
01301 </geometry>\n"));
01302 }
01303 daedata << str(boost::format("\
01304 </library_geometries>\n\
01305 <library_visual_scenes>\n\
01306 <visual_scene id=\"VisualSceneNode\" name=\"base1d_med\">\n\
01307 <node id=\"%s\" name=\"%s\" type=\"NODE\">\n")%name%name);
01308 for(unsigned int i=0; i < index; i++) {
01309 daedata << str(boost::format("\
01310 <instance_geometry url=\"#base2_M1KShape%i\">\n\
01311 <bind_material>\n\
01312 <technique_common>\n\
01313 <instance_material symbol=\"lambert2SG\" target=\"#blinn2%i\"/>\n\
01314 </technique_common>\n\
01315 </bind_material>\n\
01316 </instance_geometry>\n")%i%i);
01317 }
01318 daedata << str(boost::format("\
01319 </node>\n\
01320 </visual_scene>\n\
01321 </library_visual_scenes>\n\
01322 <scene>\n\
01323 <instance_visual_scene url=\"#VisualSceneNode\"/>\n\
01324 </scene>\n\
01325 </COLLADA>"));
01326
01327 #ifdef HAVE_MKSTEMPS
01328 geometry->filename = str(boost::format("/tmp/collada_model_reader_%s_XXXXXX.dae")%name);
01329 int fd = mkstemps(&geometry->filename[0],4);
01330 #else
01331 int fd = -1;
01332 for(int iter = 0; iter < 1000; ++iter) {
01333 geometry->filename = str(boost::format("/tmp/collada_model_reader_%s_%d.dae")%name%rand());
01334 if( !!std::ifstream(geometry->filename.c_str())) {
01335 fd = open(geometry->filename.c_str(),O_WRONLY|O_CREAT|O_EXCL);
01336 if( fd != -1 ) {
01337 break;
01338 }
01339 }
01340 }
01341 if( fd == -1 ) {
01342 ROS_ERROR("failed to open geometry dae file %s",geometry->filename.c_str());
01343 return geometry;
01344 }
01345 #endif
01346
01347 std::string daedatastr = daedata.str();
01348 if( (size_t)write(fd,daedatastr.c_str(),daedatastr.size()) != daedatastr.size() ) {
01349 ROS_ERROR("failed to write to geometry dae file %s",geometry->filename.c_str());
01350 }
01351 close(fd);
01352 _listTempFilenames.push_back(boost::shared_ptr<UnlinkFilename>(new UnlinkFilename(geometry->filename)));
01353 geometry->filename = std::string("file://") + geometry->filename;
01354 return geometry;
01355 }
01356
01360 void _ExtractGeometry(const domNodeRef pdomnode,std::list<GEOMPROPERTIES>& listGeomProperties, const std::list<JointAxisBinding>& listAxisBindings, const Pose& tlink)
01361 {
01362 if( !pdomnode ) {
01363 return;
01364 }
01365
01366 ROS_DEBUG_STREAM(str(boost::format("ExtractGeometry(node,link) of %s\n")%pdomnode->getName()));
01367
01368
01369 for (size_t i = 0; i < pdomnode->getNode_array().getCount(); i++) {
01370
01371 bool contains=false;
01372 FOREACHC(it,listAxisBindings) {
01373
01374 if ( (pdomnode->getNode_array()[i]) == (it->visualnode)) {
01375 contains=true;
01376 break;
01377 }
01378 }
01379 if (contains) {
01380 continue;
01381 }
01382
01383 _ExtractGeometry(pdomnode->getNode_array()[i],listGeomProperties, listAxisBindings,tlink);
01384
01385
01386
01387
01388 }
01389
01390 unsigned int nGeomBefore = listGeomProperties.size();
01391
01392
01393 for (size_t igeom = 0; igeom < pdomnode->getInstance_geometry_array().getCount(); ++igeom) {
01394 domInstance_geometryRef domigeom = pdomnode->getInstance_geometry_array()[igeom];
01395 domGeometryRef domgeom = daeSafeCast<domGeometry> (domigeom->getUrl().getElement());
01396 if (!domgeom) {
01397 continue;
01398 }
01399
01400
01401 std::map<std::string, domMaterialRef> mapmaterials;
01402 if (!!domigeom->getBind_material() && !!domigeom->getBind_material()->getTechnique_common()) {
01403 const domInstance_material_Array& matarray = domigeom->getBind_material()->getTechnique_common()->getInstance_material_array();
01404 for (size_t imat = 0; imat < matarray.getCount(); ++imat) {
01405 domMaterialRef pmat = daeSafeCast<domMaterial>(matarray[imat]->getTarget().getElement());
01406 if (!!pmat) {
01407 mapmaterials[matarray[imat]->getSymbol()] = pmat;
01408 }
01409 }
01410 }
01411
01412
01413 _ExtractGeometry(domgeom, mapmaterials, listGeomProperties);
01414 }
01415
01416 std::list<GEOMPROPERTIES>::iterator itgeom= listGeomProperties.begin();
01417 for (unsigned int i=0; i< nGeomBefore; i++) {
01418 itgeom++;
01419 }
01420
01421 boost::array<double,12> tmnodegeom = _poseMult(_matrixFromPose(_poseInverse(tlink)),
01422 _poseMult(_poseMult(_matrixFromPose(_poseInverse(_VisualRootOrigin)),
01423 _getNodeParentTransform(pdomnode)),
01424 _ExtractFullTransform(pdomnode)));
01425 Pose tnodegeom;
01426 Vector3 vscale(1,1,1);
01427 _decompose(tmnodegeom, tnodegeom, vscale);
01428
01429 ROS_DEBUG_STREAM("tnodegeom: " << pdomnode->getName()
01430 << tnodegeom.position.x << " " << tnodegeom.position.y << " " << tnodegeom.position.z << " / "
01431 << tnodegeom.rotation.x << " " << tnodegeom.rotation.y << " " << tnodegeom.rotation.z << " "
01432 << tnodegeom.rotation.w);
01433
01434
01435
01436
01437
01438
01439
01440
01441 for (; itgeom != listGeomProperties.end(); itgeom++) {
01442 itgeom->_t = tnodegeom;
01443 switch (itgeom->type) {
01444 case GeomBox:
01445 itgeom->vGeomData.x *= vscale.x;
01446 itgeom->vGeomData.y *= vscale.y;
01447 itgeom->vGeomData.z *= vscale.z;
01448 break;
01449 case GeomSphere: {
01450 itgeom->vGeomData.x *= std::max(vscale.z, std::max(vscale.x, vscale.y));
01451 break;
01452 }
01453 case GeomCylinder:
01454 itgeom->vGeomData.x *= std::max(vscale.x, vscale.y);
01455 itgeom->vGeomData.y *= vscale.z;
01456 break;
01457 case GeomTrimesh:
01458 for(size_t i = 0; i < itgeom->vertices.size(); ++i ) {
01459 itgeom->vertices[i] = _poseMult(tmnodegeom,itgeom->vertices[i]);
01460 }
01461 itgeom->_t = Pose();
01462 break;
01463 default:
01464 ROS_WARN_STREAM(str(boost::format("unknown geometry type: %d\n")%itgeom->type));
01465 }
01466 }
01467 }
01468
01472 void _FillGeometryColor(const domMaterialRef pmat, GEOMPROPERTIES& geom)
01473 {
01474 if( !!pmat && !!pmat->getInstance_effect() ) {
01475 domEffectRef peffect = daeSafeCast<domEffect>(pmat->getInstance_effect()->getUrl().getElement().cast());
01476 if( !!peffect ) {
01477 domProfile_common::domTechnique::domPhongRef pphong = daeSafeCast<domProfile_common::domTechnique::domPhong>(peffect->getDescendant(daeElement::matchType(domProfile_common::domTechnique::domPhong::ID())));
01478 if( !!pphong ) {
01479 if( !!pphong->getAmbient() && !!pphong->getAmbient()->getColor() ) {
01480 domFx_color c = pphong->getAmbient()->getColor()->getValue();
01481 geom.ambientColor.r = c[0];
01482 geom.ambientColor.g = c[1];
01483 geom.ambientColor.b = c[2];
01484 geom.ambientColor.a = c[3];
01485 }
01486 if( !!pphong->getDiffuse() && !!pphong->getDiffuse()->getColor() ) {
01487 domFx_color c = pphong->getDiffuse()->getColor()->getValue();
01488 geom.diffuseColor.r = c[0];
01489 geom.diffuseColor.g = c[1];
01490 geom.diffuseColor.b = c[2];
01491 geom.diffuseColor.a = c[3];
01492 }
01493 }
01494 }
01495 }
01496 }
01497
01503 bool _ExtractGeometry(const domTrianglesRef triRef, const domVerticesRef vertsRef, const std::map<std::string,domMaterialRef>& mapmaterials, std::list<GEOMPROPERTIES>& listGeomProperties)
01504 {
01505 if( !triRef ) {
01506 return false;
01507 }
01508 listGeomProperties.push_back(GEOMPROPERTIES());
01509 GEOMPROPERTIES& geom = listGeomProperties.back();
01510 geom.type = GeomTrimesh;
01511
01512
01513 if( !!triRef->getMaterial() ) {
01514 std::map<std::string,domMaterialRef>::const_iterator itmat = mapmaterials.find(triRef->getMaterial());
01515 if( itmat != mapmaterials.end() ) {
01516 _FillGeometryColor(itmat->second,geom);
01517 }
01518 }
01519
01520 size_t triangleIndexStride = 0, vertexoffset = -1;
01521 domInput_local_offsetRef indexOffsetRef;
01522
01523 for (unsigned int w=0; w<triRef->getInput_array().getCount(); w++) {
01524 size_t offset = triRef->getInput_array()[w]->getOffset();
01525 daeString str = triRef->getInput_array()[w]->getSemantic();
01526 if (!strcmp(str,"VERTEX")) {
01527 indexOffsetRef = triRef->getInput_array()[w];
01528 vertexoffset = offset;
01529 }
01530 if (offset> triangleIndexStride) {
01531 triangleIndexStride = offset;
01532 }
01533 }
01534 triangleIndexStride++;
01535
01536 const domList_of_uints& indexArray =triRef->getP()->getValue();
01537 geom.indices.reserve(triRef->getCount()*3);
01538 geom.vertices.reserve(triRef->getCount()*3);
01539 for (size_t i=0; i<vertsRef->getInput_array().getCount(); ++i) {
01540 domInput_localRef localRef = vertsRef->getInput_array()[i];
01541 daeString str = localRef->getSemantic();
01542 if ( strcmp(str,"POSITION") == 0 ) {
01543 const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
01544 if( !node ) {
01545 continue;
01546 }
01547 double fUnitScale = _GetUnitScale(node);
01548 const domFloat_arrayRef flArray = node->getFloat_array();
01549 if (!!flArray) {
01550 const domList_of_floats& listFloats = flArray->getValue();
01551 int k=vertexoffset;
01552 int vertexStride = 3;
01553 for(size_t itri = 0; itri < triRef->getCount(); ++itri) {
01554 if(k+2*triangleIndexStride < indexArray.getCount() ) {
01555 for (int j=0; j<3; j++) {
01556 int index0 = indexArray.get(k)*vertexStride;
01557 domFloat fl0 = listFloats.get(index0);
01558 domFloat fl1 = listFloats.get(index0+1);
01559 domFloat fl2 = listFloats.get(index0+2);
01560 k+=triangleIndexStride;
01561 geom.indices.push_back(geom.vertices.size());
01562 geom.vertices.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale));
01563 }
01564 }
01565 }
01566 }
01567 else {
01568 ROS_WARN_STREAM("float array not defined!\n");
01569 }
01570 break;
01571 }
01572 }
01573 if( geom.indices.size() != 3*triRef->getCount() ) {
01574 ROS_WARN_STREAM("triangles declares wrong count!\n");
01575 }
01576 geom.InitCollisionMesh();
01577 return true;
01578 }
01579
01584 bool _ExtractGeometry(const domTrifansRef triRef, const domVerticesRef vertsRef, const std::map<std::string,domMaterialRef>& mapmaterials, std::list<GEOMPROPERTIES>& listGeomProperties)
01585 {
01586 if( !triRef ) {
01587 return false;
01588 }
01589 listGeomProperties.push_back(GEOMPROPERTIES());
01590 GEOMPROPERTIES& geom = listGeomProperties.back();
01591 geom.type = GeomTrimesh;
01592
01593
01594 if( !!triRef->getMaterial() ) {
01595 std::map<std::string,domMaterialRef>::const_iterator itmat = mapmaterials.find(triRef->getMaterial());
01596 if( itmat != mapmaterials.end() ) {
01597 _FillGeometryColor(itmat->second,geom);
01598 }
01599 }
01600
01601 size_t triangleIndexStride = 0, vertexoffset = -1;
01602 domInput_local_offsetRef indexOffsetRef;
01603
01604 for (unsigned int w=0; w<triRef->getInput_array().getCount(); w++) {
01605 size_t offset = triRef->getInput_array()[w]->getOffset();
01606 daeString str = triRef->getInput_array()[w]->getSemantic();
01607 if (!strcmp(str,"VERTEX")) {
01608 indexOffsetRef = triRef->getInput_array()[w];
01609 vertexoffset = offset;
01610 }
01611 if (offset> triangleIndexStride) {
01612 triangleIndexStride = offset;
01613 }
01614 }
01615 triangleIndexStride++;
01616 size_t primitivecount = triRef->getCount();
01617 if( primitivecount > triRef->getP_array().getCount() ) {
01618 ROS_WARN_STREAM("trifans has incorrect count\n");
01619 primitivecount = triRef->getP_array().getCount();
01620 }
01621 for(size_t ip = 0; ip < primitivecount; ++ip) {
01622 domList_of_uints indexArray =triRef->getP_array()[ip]->getValue();
01623 for (size_t i=0; i<vertsRef->getInput_array().getCount(); ++i) {
01624 domInput_localRef localRef = vertsRef->getInput_array()[i];
01625 daeString str = localRef->getSemantic();
01626 if ( strcmp(str,"POSITION") == 0 ) {
01627 const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
01628 if( !node ) {
01629 continue;
01630 }
01631 double fUnitScale = _GetUnitScale(node);
01632 const domFloat_arrayRef flArray = node->getFloat_array();
01633 if (!!flArray) {
01634 const domList_of_floats& listFloats = flArray->getValue();
01635 int k=vertexoffset;
01636 int vertexStride = 3;
01637 size_t usedindices = 3*(indexArray.getCount()-2);
01638 if( geom.indices.capacity() < geom.indices.size()+usedindices ) {
01639 geom.indices.reserve(geom.indices.size()+usedindices);
01640 }
01641 if( geom.vertices.capacity() < geom.vertices.size()+indexArray.getCount() ) {
01642 geom.vertices.reserve(geom.vertices.size()+indexArray.getCount());
01643 }
01644 size_t startoffset = (int)geom.vertices.size();
01645 while(k < (int)indexArray.getCount() ) {
01646 int index0 = indexArray.get(k)*vertexStride;
01647 domFloat fl0 = listFloats.get(index0);
01648 domFloat fl1 = listFloats.get(index0+1);
01649 domFloat fl2 = listFloats.get(index0+2);
01650 k+=triangleIndexStride;
01651 geom.vertices.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale));
01652 }
01653 for(size_t ivert = startoffset+2; ivert < geom.vertices.size(); ++ivert) {
01654 geom.indices.push_back(startoffset);
01655 geom.indices.push_back(ivert-1);
01656 geom.indices.push_back(ivert);
01657 }
01658 }
01659 else {
01660 ROS_WARN_STREAM("float array not defined!\n");
01661 }
01662 break;
01663 }
01664 }
01665 }
01666
01667 geom.InitCollisionMesh();
01668 return false;
01669 }
01670
01675 bool _ExtractGeometry(const domTristripsRef triRef, const domVerticesRef vertsRef, const std::map<std::string,domMaterialRef>& mapmaterials, std::list<GEOMPROPERTIES>& listGeomProperties)
01676 {
01677 if( !triRef ) {
01678 return false;
01679 }
01680 listGeomProperties.push_back(GEOMPROPERTIES());
01681 GEOMPROPERTIES& geom = listGeomProperties.back();
01682 geom.type = GeomTrimesh;
01683
01684
01685 if( !!triRef->getMaterial() ) {
01686 std::map<std::string,domMaterialRef>::const_iterator itmat = mapmaterials.find(triRef->getMaterial());
01687 if( itmat != mapmaterials.end() ) {
01688 _FillGeometryColor(itmat->second,geom);
01689 }
01690 }
01691 size_t triangleIndexStride = 0, vertexoffset = -1;
01692 domInput_local_offsetRef indexOffsetRef;
01693
01694 for (unsigned int w=0; w<triRef->getInput_array().getCount(); w++) {
01695 size_t offset = triRef->getInput_array()[w]->getOffset();
01696 daeString str = triRef->getInput_array()[w]->getSemantic();
01697 if (!strcmp(str,"VERTEX")) {
01698 indexOffsetRef = triRef->getInput_array()[w];
01699 vertexoffset = offset;
01700 }
01701 if (offset> triangleIndexStride) {
01702 triangleIndexStride = offset;
01703 }
01704 }
01705 triangleIndexStride++;
01706 size_t primitivecount = triRef->getCount();
01707 if( primitivecount > triRef->getP_array().getCount() ) {
01708 ROS_WARN_STREAM("tristrips has incorrect count\n");
01709 primitivecount = triRef->getP_array().getCount();
01710 }
01711 for(size_t ip = 0; ip < primitivecount; ++ip) {
01712 domList_of_uints indexArray = triRef->getP_array()[ip]->getValue();
01713 for (size_t i=0; i<vertsRef->getInput_array().getCount(); ++i) {
01714 domInput_localRef localRef = vertsRef->getInput_array()[i];
01715 daeString str = localRef->getSemantic();
01716 if ( strcmp(str,"POSITION") == 0 ) {
01717 const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
01718 if( !node ) {
01719 continue;
01720 }
01721 double fUnitScale = _GetUnitScale(node);
01722 const domFloat_arrayRef flArray = node->getFloat_array();
01723 if (!!flArray) {
01724 const domList_of_floats& listFloats = flArray->getValue();
01725 int k=vertexoffset;
01726 int vertexStride = 3;
01727 size_t usedindices = indexArray.getCount()-2;
01728 if( geom.indices.capacity() < geom.indices.size()+usedindices ) {
01729 geom.indices.reserve(geom.indices.size()+usedindices);
01730 }
01731 if( geom.vertices.capacity() < geom.vertices.size()+indexArray.getCount() ) {
01732 geom.vertices.reserve(geom.vertices.size()+indexArray.getCount());
01733 }
01734
01735 size_t startoffset = (int)geom.vertices.size();
01736 while(k < (int)indexArray.getCount() ) {
01737 int index0 = indexArray.get(k)*vertexStride;
01738 domFloat fl0 = listFloats.get(index0);
01739 domFloat fl1 = listFloats.get(index0+1);
01740 domFloat fl2 = listFloats.get(index0+2);
01741 k+=triangleIndexStride;
01742 geom.vertices.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale));
01743 }
01744
01745 bool bFlip = false;
01746 for(size_t ivert = startoffset+2; ivert < geom.vertices.size(); ++ivert) {
01747 geom.indices.push_back(ivert-2);
01748 geom.indices.push_back(bFlip ? ivert : ivert-1);
01749 geom.indices.push_back(bFlip ? ivert-1 : ivert);
01750 bFlip = !bFlip;
01751 }
01752 }
01753 else {
01754 ROS_WARN_STREAM("float array not defined!\n");
01755 }
01756 break;
01757 }
01758 }
01759 }
01760
01761 geom.InitCollisionMesh();
01762 return false;
01763 }
01764
01769 bool _ExtractGeometry(const domPolylistRef triRef, const domVerticesRef vertsRef, const std::map<std::string,domMaterialRef>& mapmaterials, std::list<GEOMPROPERTIES>& listGeomProperties)
01770 {
01771 if( !triRef ) {
01772 return false;
01773 }
01774 listGeomProperties.push_back(GEOMPROPERTIES());
01775 GEOMPROPERTIES& geom = listGeomProperties.back();
01776 geom.type = GeomTrimesh;
01777
01778
01779 if( !!triRef->getMaterial() ) {
01780 std::map<std::string,domMaterialRef>::const_iterator itmat = mapmaterials.find(triRef->getMaterial());
01781 if( itmat != mapmaterials.end() ) {
01782 _FillGeometryColor(itmat->second,geom);
01783 }
01784 }
01785
01786 size_t triangleIndexStride = 0,vertexoffset = -1;
01787 domInput_local_offsetRef indexOffsetRef;
01788 for (unsigned int w=0; w<triRef->getInput_array().getCount(); w++) {
01789 size_t offset = triRef->getInput_array()[w]->getOffset();
01790 daeString str = triRef->getInput_array()[w]->getSemantic();
01791 if (!strcmp(str,"VERTEX")) {
01792 indexOffsetRef = triRef->getInput_array()[w];
01793 vertexoffset = offset;
01794 }
01795 if (offset> triangleIndexStride) {
01796 triangleIndexStride = offset;
01797 }
01798 }
01799 triangleIndexStride++;
01800 const domList_of_uints& indexArray =triRef->getP()->getValue();
01801 for (size_t i=0; i<vertsRef->getInput_array().getCount(); ++i) {
01802 domInput_localRef localRef = vertsRef->getInput_array()[i];
01803 daeString str = localRef->getSemantic();
01804 if ( strcmp(str,"POSITION") == 0 ) {
01805 const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
01806 if( !node ) {
01807 continue;
01808 }
01809 double fUnitScale = _GetUnitScale(node);
01810 const domFloat_arrayRef flArray = node->getFloat_array();
01811 if (!!flArray) {
01812 const domList_of_floats& listFloats = flArray->getValue();
01813 size_t k=vertexoffset;
01814 int vertexStride = 3;
01815 for(size_t ipoly = 0; ipoly < triRef->getVcount()->getValue().getCount(); ++ipoly) {
01816 size_t numverts = triRef->getVcount()->getValue()[ipoly];
01817 if(numverts > 0 && k+(numverts-1)*triangleIndexStride < indexArray.getCount() ) {
01818 size_t startoffset = geom.vertices.size();
01819 for (size_t j=0; j<numverts; j++) {
01820 int index0 = indexArray.get(k)*vertexStride;
01821 domFloat fl0 = listFloats.get(index0);
01822 domFloat fl1 = listFloats.get(index0+1);
01823 domFloat fl2 = listFloats.get(index0+2);
01824 k+=triangleIndexStride;
01825 geom.vertices.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale));
01826 }
01827 for(size_t ivert = startoffset+2; ivert < geom.vertices.size(); ++ivert) {
01828 geom.indices.push_back(startoffset);
01829 geom.indices.push_back(ivert-1);
01830 geom.indices.push_back(ivert);
01831 }
01832 }
01833 }
01834 }
01835 else {
01836 ROS_WARN_STREAM("float array not defined!\n");
01837 }
01838 break;
01839 }
01840 }
01841 geom.InitCollisionMesh();
01842 return false;
01843 }
01844
01848 bool _ExtractGeometry(const domGeometryRef geom, const std::map<std::string,domMaterialRef>& mapmaterials, std::list<GEOMPROPERTIES>& listGeomProperties)
01849 {
01850 if( !geom ) {
01851 return false;
01852 }
01853 std::vector<Vector3> vconvexhull;
01854 if (geom->getMesh()) {
01855 const domMeshRef meshRef = geom->getMesh();
01856 for (size_t tg = 0; tg<meshRef->getTriangles_array().getCount(); tg++) {
01857 _ExtractGeometry(meshRef->getTriangles_array()[tg], meshRef->getVertices(), mapmaterials, listGeomProperties);
01858 }
01859 for (size_t tg = 0; tg<meshRef->getTrifans_array().getCount(); tg++) {
01860 _ExtractGeometry(meshRef->getTrifans_array()[tg], meshRef->getVertices(), mapmaterials, listGeomProperties);
01861 }
01862 for (size_t tg = 0; tg<meshRef->getTristrips_array().getCount(); tg++) {
01863 _ExtractGeometry(meshRef->getTristrips_array()[tg], meshRef->getVertices(), mapmaterials, listGeomProperties);
01864 }
01865 for (size_t tg = 0; tg<meshRef->getPolylist_array().getCount(); tg++) {
01866 _ExtractGeometry(meshRef->getPolylist_array()[tg], meshRef->getVertices(), mapmaterials, listGeomProperties);
01867 }
01868 if( meshRef->getPolygons_array().getCount()> 0 ) {
01869 ROS_WARN_STREAM("openrave does not support collada polygons\n");
01870 }
01871
01872
01873
01874
01875
01876
01877
01878
01879
01880
01881
01882
01883
01884
01885
01886
01887
01888
01889
01890
01891
01892
01893
01894
01895
01896
01897
01898
01899
01900
01901 return true;
01902 }
01903 else if (geom->getConvex_mesh()) {
01904 {
01905 const domConvex_meshRef convexRef = geom->getConvex_mesh();
01906 daeElementRef otherElemRef = convexRef->getConvex_hull_of().getElement();
01907 if ( !!otherElemRef ) {
01908 domGeometryRef linkedGeom = *(domGeometryRef*)&otherElemRef;
01909 ROS_WARN_STREAM( "otherLinked\n");
01910 }
01911 else {
01912 ROS_WARN("convexMesh polyCount = %d\n",(int)convexRef->getPolygons_array().getCount());
01913 ROS_WARN("convexMesh triCount = %d\n",(int)convexRef->getTriangles_array().getCount());
01914 }
01915 }
01916
01917 const domConvex_meshRef convexRef = geom->getConvex_mesh();
01918
01919 daeString urlref2 = convexRef->getConvex_hull_of().getOriginalURI();
01920 if (urlref2) {
01921 daeElementRef otherElemRef = convexRef->getConvex_hull_of().getElement();
01922
01923
01924 for ( size_t i = 0; i < _dom->getLibrary_geometries_array().getCount(); i++) {
01925 domLibrary_geometriesRef libgeom = _dom->getLibrary_geometries_array()[i];
01926 for (size_t i = 0; i < libgeom->getGeometry_array().getCount(); i++) {
01927 domGeometryRef lib = libgeom->getGeometry_array()[i];
01928 if (!strcmp(lib->getId(),urlref2+1)) {
01929
01930 domMesh *meshElement = lib->getMesh();
01931 if (meshElement) {
01932 const domVerticesRef vertsRef = meshElement->getVertices();
01933 for (size_t i=0; i<vertsRef->getInput_array().getCount(); i++) {
01934 domInput_localRef localRef = vertsRef->getInput_array()[i];
01935 daeString str = localRef->getSemantic();
01936 if ( strcmp(str,"POSITION") == 0) {
01937 const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
01938 if( !node ) {
01939 continue;
01940 }
01941 double fUnitScale = _GetUnitScale(node);
01942 const domFloat_arrayRef flArray = node->getFloat_array();
01943 if (!!flArray) {
01944 vconvexhull.reserve(vconvexhull.size()+flArray->getCount());
01945 const domList_of_floats& listFloats = flArray->getValue();
01946 for (size_t k=0; k+2<flArray->getCount(); k+=3) {
01947 domFloat fl0 = listFloats.get(k);
01948 domFloat fl1 = listFloats.get(k+1);
01949 domFloat fl2 = listFloats.get(k+2);
01950 vconvexhull.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale));
01951 }
01952 }
01953 }
01954 }
01955 }
01956 }
01957 }
01958 }
01959 }
01960 else {
01961
01962 const domVerticesRef vertsRef = convexRef->getVertices();
01963 for (size_t i=0; i<vertsRef->getInput_array().getCount(); i++) {
01964 domInput_localRef localRef = vertsRef->getInput_array()[i];
01965 daeString str = localRef->getSemantic();
01966 if ( strcmp(str,"POSITION") == 0 ) {
01967 const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
01968 if( !node ) {
01969 continue;
01970 }
01971 double fUnitScale = _GetUnitScale(node);
01972 const domFloat_arrayRef flArray = node->getFloat_array();
01973 if (!!flArray) {
01974 const domList_of_floats& listFloats = flArray->getValue();
01975 vconvexhull.reserve(vconvexhull.size()+flArray->getCount());
01976 for (size_t k=0; k+2<flArray->getCount(); k+=3) {
01977 domFloat fl0 = listFloats.get(k);
01978 domFloat fl1 = listFloats.get(k+1);
01979 domFloat fl2 = listFloats.get(k+2);
01980 vconvexhull.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale));
01981 }
01982 }
01983 }
01984 }
01985 }
01986
01987 if( vconvexhull.size()> 0 ) {
01988 listGeomProperties.push_back(GEOMPROPERTIES());
01989 GEOMPROPERTIES& geom = listGeomProperties.back();
01990 geom.type = GeomTrimesh;
01991
01992
01993 geom.InitCollisionMesh();
01994 }
01995 return true;
01996 }
01997
01998 return false;
01999 }
02000
02002 void _ExtractRobotAttachedActuators(const domArticulated_systemRef as)
02003 {
02004
02005 for(size_t ie = 0; ie < as->getExtra_array().getCount(); ++ie) {
02006 domExtraRef pextra = as->getExtra_array()[ie];
02007 if( strcmp(pextra->getType(), "attach_actuator") == 0 ) {
02008
02009 domTechniqueRef tec = _ExtractOpenRAVEProfile(pextra->getTechnique_array());
02010 if( !!tec ) {
02011 boost::shared_ptr<Joint> pjoint;
02012 daeElementRef domactuator;
02013 {
02014 daeElementRef bact = tec->getChild("bind_actuator");
02015 pjoint = _getJointFromRef(bact->getAttribute("joint").c_str(), as);
02016 if (!pjoint) continue;
02017 }
02018 {
02019 daeElementRef iact = tec->getChild("instance_actuator");
02020 if(!iact) continue;
02021 std::string instance_url = iact->getAttribute("url");
02022 domactuator = daeURI(*iact, instance_url).getElement();
02023 if(!domactuator) continue;
02024 }
02025 daeElement *nom_torque = domactuator->getChild("nominal_torque");
02026 if ( !! nom_torque ) {
02027 if( !! pjoint->limits ) {
02028 pjoint->limits->effort = boost::lexical_cast<double>(nom_torque->getCharData());
02029 ROS_DEBUG("effort limit at joint (%s) is over written by %f",
02030 pjoint->name.c_str(), pjoint->limits->effort);
02031 }
02032 }
02033 }
02034 }
02035 }
02036 }
02037
02039 void _ExtractRobotManipulators(const domArticulated_systemRef as)
02040 {
02041 ROS_DEBUG("collada manipulators not supported yet");
02042 }
02043
02045 void _ExtractRobotAttachedSensors(const domArticulated_systemRef as)
02046 {
02047 ROS_DEBUG("collada sensors not supported yet");
02048 }
02049
02050 inline daeElementRef _getElementFromUrl(const daeURI &uri)
02051 {
02052 return daeStandardURIResolver(*_collada).resolveElement(uri);
02053 }
02054
02055 static daeElement* searchBinding(domCommon_sidref_or_paramRef paddr, daeElementRef parent)
02056 {
02057 if( !!paddr->getSIDREF() ) {
02058 return daeSidRef(paddr->getSIDREF()->getValue(),parent).resolve().elt;
02059 }
02060 if (!!paddr->getParam()) {
02061 return searchBinding(paddr->getParam()->getValue(),parent);
02062 }
02063 return NULL;
02064 }
02065
02069 static daeElement* searchBinding(daeString ref, daeElementRef parent)
02070 {
02071 if( !parent ) {
02072 return NULL;
02073 }
02074 daeElement* pelt = NULL;
02075 domKinematics_sceneRef kscene = daeSafeCast<domKinematics_scene>(parent.cast());
02076 if( !!kscene ) {
02077 pelt = searchBindingArray(ref,kscene->getInstance_articulated_system_array());
02078 if( !!pelt ) {
02079 return pelt;
02080 }
02081 return searchBindingArray(ref,kscene->getInstance_kinematics_model_array());
02082 }
02083 domArticulated_systemRef articulated_system = daeSafeCast<domArticulated_system>(parent.cast());
02084 if( !!articulated_system ) {
02085 if( !!articulated_system->getKinematics() ) {
02086 pelt = searchBindingArray(ref,articulated_system->getKinematics()->getInstance_kinematics_model_array());
02087 if( !!pelt ) {
02088 return pelt;
02089 }
02090 }
02091 if( !!articulated_system->getMotion() ) {
02092 return searchBinding(ref,articulated_system->getMotion()->getInstance_articulated_system());
02093 }
02094 return NULL;
02095 }
02096
02097 daeElementRef pbindelt;
02098 const domKinematics_bind_Array* pbindarray = NULL;
02099 const domKinematics_newparam_Array* pnewparamarray = NULL;
02100 domInstance_articulated_systemRef ias = daeSafeCast<domInstance_articulated_system>(parent.cast());
02101 if( !!ias ) {
02102 pbindarray = &ias->getBind_array();
02103 pbindelt = ias->getUrl().getElement();
02104 pnewparamarray = &ias->getNewparam_array();
02105 }
02106 if( !pbindarray || !pbindelt ) {
02107 domInstance_kinematics_modelRef ikm = daeSafeCast<domInstance_kinematics_model>(parent.cast());
02108 if( !!ikm ) {
02109 pbindarray = &ikm->getBind_array();
02110 pbindelt = ikm->getUrl().getElement();
02111 pnewparamarray = &ikm->getNewparam_array();
02112 }
02113 }
02114 if( !!pbindarray && !!pbindelt ) {
02115 for (size_t ibind = 0; ibind < pbindarray->getCount(); ++ibind) {
02116 domKinematics_bindRef pbind = (*pbindarray)[ibind];
02117 if( !!pbind->getSymbol() && strcmp(pbind->getSymbol(), ref) == 0 ) {
02118
02119 if( !!pbind->getParam() ) {
02120 return daeSidRef(pbind->getParam()->getRef(), pbindelt).resolve().elt;
02121 }
02122 else if( !!pbind->getSIDREF() ) {
02123 return daeSidRef(pbind->getSIDREF()->getValue(), pbindelt).resolve().elt;
02124 }
02125 }
02126 }
02127 for(size_t inewparam = 0; inewparam < pnewparamarray->getCount(); ++inewparam) {
02128 domKinematics_newparamRef newparam = (*pnewparamarray)[inewparam];
02129 if( !!newparam->getSid() && strcmp(newparam->getSid(), ref) == 0 ) {
02130 if( !!newparam->getSIDREF() ) {
02131 return daeSidRef(newparam->getSIDREF()->getValue(),pbindelt).resolve().elt;
02132 }
02133 ROS_WARN_STREAM(str(boost::format("newparam sid=%s does not have SIDREF\n")%newparam->getSid()));
02134 }
02135 }
02136 }
02137 ROS_WARN_STREAM(str(boost::format("failed to get binding '%s' for element: %s\n")%ref%parent->getElementName()));
02138 return NULL;
02139 }
02140
02141 static daeElement* searchBindingArray(daeString ref, const domInstance_articulated_system_Array& paramArray)
02142 {
02143 for(size_t iikm = 0; iikm < paramArray.getCount(); ++iikm) {
02144 daeElement* pelt = searchBinding(ref,paramArray[iikm].cast());
02145 if( !!pelt ) {
02146 return pelt;
02147 }
02148 }
02149 return NULL;
02150 }
02151
02152 static daeElement* searchBindingArray(daeString ref, const domInstance_kinematics_model_Array& paramArray)
02153 {
02154 for(size_t iikm = 0; iikm < paramArray.getCount(); ++iikm) {
02155 daeElement* pelt = searchBinding(ref,paramArray[iikm].cast());
02156 if( !!pelt ) {
02157 return pelt;
02158 }
02159 }
02160 return NULL;
02161 }
02162
02163 template <typename U> static xsBoolean resolveBool(domCommon_bool_or_paramRef paddr, const U& parent) {
02164 if( !!paddr->getBool() ) {
02165 return paddr->getBool()->getValue();
02166 }
02167 if( !paddr->getParam() ) {
02168 ROS_WARN_STREAM("param not specified, setting to 0\n");
02169 return false;
02170 }
02171 for(size_t iparam = 0; iparam < parent->getNewparam_array().getCount(); ++iparam) {
02172 domKinematics_newparamRef pnewparam = parent->getNewparam_array()[iparam];
02173 if( !!pnewparam->getSid() && strcmp(pnewparam->getSid(), paddr->getParam()->getValue()) == 0 ) {
02174 if( !!pnewparam->getBool() ) {
02175 return pnewparam->getBool()->getValue();
02176 }
02177 else if( !!pnewparam->getSIDREF() ) {
02178 domKinematics_newparam::domBoolRef ptarget = daeSafeCast<domKinematics_newparam::domBool>(daeSidRef(pnewparam->getSIDREF()->getValue(), pnewparam).resolve().elt);
02179 if( !ptarget ) {
02180 ROS_WARN("failed to resolve %s from %s\n", pnewparam->getSIDREF()->getValue(), paddr->getID());
02181 continue;
02182 }
02183 return ptarget->getValue();
02184 }
02185 }
02186 }
02187 ROS_WARN_STREAM(str(boost::format("failed to resolve %s\n")%paddr->getParam()->getValue()));
02188 return false;
02189 }
02190 template <typename U> static domFloat resolveFloat(domCommon_float_or_paramRef paddr, const U& parent) {
02191 if( !!paddr->getFloat() ) {
02192 return paddr->getFloat()->getValue();
02193 }
02194 if( !paddr->getParam() ) {
02195 ROS_WARN_STREAM("param not specified, setting to 0\n");
02196 return 0;
02197 }
02198 for(size_t iparam = 0; iparam < parent->getNewparam_array().getCount(); ++iparam) {
02199 domKinematics_newparamRef pnewparam = parent->getNewparam_array()[iparam];
02200 if( !!pnewparam->getSid() && strcmp(pnewparam->getSid(), paddr->getParam()->getValue()) == 0 ) {
02201 if( !!pnewparam->getFloat() ) {
02202 return pnewparam->getFloat()->getValue();
02203 }
02204 else if( !!pnewparam->getSIDREF() ) {
02205 domKinematics_newparam::domFloatRef ptarget = daeSafeCast<domKinematics_newparam::domFloat>(daeSidRef(pnewparam->getSIDREF()->getValue(), pnewparam).resolve().elt);
02206 if( !ptarget ) {
02207 ROS_WARN("failed to resolve %s from %s\n", pnewparam->getSIDREF()->getValue(), paddr->getID());
02208 continue;
02209 }
02210 return ptarget->getValue();
02211 }
02212 }
02213 }
02214 ROS_WARN_STREAM(str(boost::format("failed to resolve %s\n")%paddr->getParam()->getValue()));
02215 return 0;
02216 }
02217
02218 static bool resolveCommon_float_or_param(daeElementRef pcommon, daeElementRef parent, float& f)
02219 {
02220 daeElement* pfloat = pcommon->getChild("float");
02221 if( !!pfloat ) {
02222 std::stringstream sfloat(pfloat->getCharData());
02223 sfloat >> f;
02224 return !!sfloat;
02225 }
02226 daeElement* pparam = pcommon->getChild("param");
02227 if( !!pparam ) {
02228 if( pparam->hasAttribute("ref") ) {
02229 ROS_WARN_STREAM("cannot process param ref\n");
02230 }
02231 else {
02232 daeElement* pelt = daeSidRef(pparam->getCharData(),parent).resolve().elt;
02233 if( !!pelt ) {
02234 ROS_WARN_STREAM(str(boost::format("found param ref: %s from %s\n")%pelt->getCharData()%pparam->getCharData()));
02235 }
02236 }
02237 }
02238 return false;
02239 }
02240
02241 static boost::array<double,12> _matrixIdentity()
02242 {
02243 boost::array<double,12> m = {{1,0,0,0,0,1,0,0,0,0,1,0}};
02244 return m;
02245 };
02246
02248 static boost::array<double,12> _getTransform(daeElementRef pelt)
02249 {
02250 boost::array<double,12> m = _matrixIdentity();
02251 domRotateRef protate = daeSafeCast<domRotate>(pelt);
02252 if( !!protate ) {
02253 m = _matrixFromAxisAngle(Vector3(protate->getValue()[0],protate->getValue()[1],protate->getValue()[2]), (double)(protate->getValue()[3]*(M_PI/180.0)));
02254 return m;
02255 }
02256
02257 domTranslateRef ptrans = daeSafeCast<domTranslate>(pelt);
02258 if( !!ptrans ) {
02259 double scale = _GetUnitScale(pelt);
02260 m[3] = ptrans->getValue()[0]*scale;
02261 m[7] = ptrans->getValue()[1]*scale;
02262 m[11] = ptrans->getValue()[2]*scale;
02263 return m;
02264 }
02265
02266 domMatrixRef pmat = daeSafeCast<domMatrix>(pelt);
02267 if( !!pmat ) {
02268 double scale = _GetUnitScale(pelt);
02269 for(int i = 0; i < 3; ++i) {
02270 m[4*i+0] = pmat->getValue()[4*i+0];
02271 m[4*i+1] = pmat->getValue()[4*i+1];
02272 m[4*i+2] = pmat->getValue()[4*i+2];
02273 m[4*i+3] = pmat->getValue()[4*i+3]*scale;
02274 }
02275 return m;
02276 }
02277
02278 domScaleRef pscale = daeSafeCast<domScale>(pelt);
02279 if( !!pscale ) {
02280 m[0] = pscale->getValue()[0];
02281 m[4*1+1] = pscale->getValue()[1];
02282 m[4*2+2] = pscale->getValue()[2];
02283 return m;
02284 }
02285
02286 domLookatRef pcamera = daeSafeCast<domLookat>(pelt);
02287 if( pelt->typeID() == domLookat::ID() ) {
02288 ROS_ERROR_STREAM("look at transform not implemented\n");
02289 return m;
02290 }
02291
02292 domSkewRef pskew = daeSafeCast<domSkew>(pelt);
02293 if( !!pskew ) {
02294 ROS_ERROR_STREAM("skew transform not implemented\n");
02295 }
02296
02297 return m;
02298 }
02299
02302 template <typename T> static boost::array<double,12> _getNodeParentTransform(const T pelt) {
02303 domNodeRef pnode = daeSafeCast<domNode>(pelt->getParent());
02304 if( !pnode ) {
02305 return _matrixIdentity();
02306 }
02307 return _poseMult(_getNodeParentTransform(pnode), _ExtractFullTransform(pnode));
02308 }
02309
02311 template <typename T> static boost::array<double,12> _ExtractFullTransform(const T pelt) {
02312 boost::array<double,12> t = _matrixIdentity();
02313 for(size_t i = 0; i < pelt->getContents().getCount(); ++i) {
02314 t = _poseMult(t, _getTransform(pelt->getContents()[i]));
02315 }
02316 return t;
02317 }
02318
02320 template <typename T> static boost::array<double,12> _ExtractFullTransformFromChildren(const T pelt) {
02321 boost::array<double,12> t = _matrixIdentity();
02322 daeTArray<daeElementRef> children; pelt->getChildren(children);
02323 for(size_t i = 0; i < children.getCount(); ++i) {
02324 t = _poseMult(t, _getTransform(children[i]));
02325 }
02326 return t;
02327 }
02328
02329
02330 void _decompose(const boost::array<double,12>& tm, Pose& tout, Vector3& vscale)
02331 {
02332 vscale.x = 1; vscale.y = 1; vscale.z = 1;
02333 tout = _poseFromMatrix(tm);
02334 }
02335
02336 virtual void handleError( daeString msg )
02337 {
02338 ROS_ERROR("COLLADA error: %s\n", msg);
02339 }
02340
02341 virtual void handleWarning( daeString msg )
02342 {
02343 ROS_WARN("COLLADA warning: %s\n", msg);
02344 }
02345
02346 inline static double _GetUnitScale(daeElement* pelt)
02347 {
02348 return ((USERDATA*)pelt->getUserData())->scale;
02349 }
02350
02351 domTechniqueRef _ExtractOpenRAVEProfile(const domTechnique_Array& arr)
02352 {
02353 for(size_t i = 0; i < arr.getCount(); ++i) {
02354 if( strcmp(arr[i]->getProfile(), "OpenRAVE") == 0 ) {
02355 return arr[i];
02356 }
02357 }
02358 return domTechniqueRef();
02359 }
02360
02362 boost::shared_ptr<std::string> _ExtractInterfaceType(const domExtra_Array& arr) {
02363 for(size_t i = 0; i < arr.getCount(); ++i) {
02364 if( strcmp(arr[i]->getType(),"interface_type") == 0 ) {
02365 domTechniqueRef tec = _ExtractOpenRAVEProfile(arr[i]->getTechnique_array());
02366 if( !!tec ) {
02367 daeElement* ptype = tec->getChild("interface");
02368 if( !!ptype ) {
02369 return boost::shared_ptr<std::string>(new std::string(ptype->getCharData()));
02370 }
02371 }
02372 }
02373 }
02374 return boost::shared_ptr<std::string>();
02375 }
02376
02377 std::string _ExtractLinkName(domLinkRef pdomlink) {
02378 std::string linkname;
02379 if( !!pdomlink ) {
02380 if( !!pdomlink->getName() ) {
02381 linkname = pdomlink->getName();
02382 }
02383 if( linkname.size() == 0 && !!pdomlink->getID() ) {
02384 linkname = pdomlink->getID();
02385 }
02386 }
02387 return linkname;
02388 }
02389
02390 bool _checkMathML(daeElementRef pelt,const std::string& type)
02391 {
02392 if( pelt->getElementName()==type ) {
02393 return true;
02394 }
02395
02396 std::string name = pelt->getElementName();
02397 std::size_t pos = name.find_last_of(':');
02398 if( pos == std::string::npos ) {
02399 return false;
02400 }
02401 return name.substr(pos+1)==type;
02402 }
02403
02404 boost::shared_ptr<Joint> _getJointFromRef(xsToken targetref, daeElementRef peltref) {
02405 daeElement* peltjoint = daeSidRef(targetref, peltref).resolve().elt;
02406 domJointRef pdomjoint = daeSafeCast<domJoint> (peltjoint);
02407
02408 if (!pdomjoint) {
02409 domInstance_jointRef pdomijoint = daeSafeCast<domInstance_joint> (peltjoint);
02410 if (!!pdomijoint) {
02411 pdomjoint = daeSafeCast<domJoint> (pdomijoint->getUrl().getElement().cast());
02412 }
02413 }
02414
02415 if (!pdomjoint || pdomjoint->typeID() != domJoint::ID() || !pdomjoint->getName()) {
02416 ROS_WARN_STREAM(str(boost::format("could not find collada joint %s!\n")%targetref));
02417 return boost::shared_ptr<Joint>();
02418 }
02419
02420 boost::shared_ptr<Joint> pjoint;
02421 std::string name(pdomjoint->getName());
02422 if (_model->joints_.find(name) == _model->joints_.end()) {
02423 pjoint.reset();
02424 } else {
02425 pjoint = _model->joints_.find(name)->second;
02426 }
02427 if(!pjoint) {
02428 ROS_WARN_STREAM(str(boost::format("could not find openrave joint %s!\n")%pdomjoint->getName()));
02429 }
02430 return pjoint;
02431 }
02432
02436 static void _ExtractKinematicsVisualBindings(domInstance_with_extraRef viscene, domInstance_kinematics_sceneRef kiscene, KinematicsSceneBindings& bindings)
02437 {
02438 domKinematics_sceneRef kscene = daeSafeCast<domKinematics_scene> (kiscene->getUrl().getElement().cast());
02439 if (!kscene) {
02440 return;
02441 }
02442 for (size_t imodel = 0; imodel < kiscene->getBind_kinematics_model_array().getCount(); imodel++) {
02443 domArticulated_systemRef articulated_system;
02444 domBind_kinematics_modelRef kbindmodel = kiscene->getBind_kinematics_model_array()[imodel];
02445 if (!kbindmodel->getNode()) {
02446 ROS_WARN_STREAM("do not support kinematics models without references to nodes\n");
02447 continue;
02448 }
02449
02450
02451 domNodeRef node = daeSafeCast<domNode>(daeSidRef(kbindmodel->getNode(), viscene->getUrl().getElement()).resolve().elt);
02452 if (!node) {
02453 ROS_WARN_STREAM(str(boost::format("bind_kinematics_model does not reference valid node %s\n")%kbindmodel->getNode()));
02454 continue;
02455 }
02456
02457
02458 daeElement* pelt = searchBinding(kbindmodel,kscene);
02459 domInstance_kinematics_modelRef kimodel = daeSafeCast<domInstance_kinematics_model>(pelt);
02460 if (!kimodel) {
02461 if( !pelt ) {
02462 ROS_WARN_STREAM("bind_kinematics_model does not reference element\n");
02463 }
02464 else {
02465 ROS_WARN_STREAM(str(boost::format("bind_kinematics_model cannot find reference to %s:\n")%pelt->getElementName()));
02466 }
02467 continue;
02468 }
02469 bindings.listKinematicsVisualBindings.push_back(std::make_pair(node,kimodel));
02470 }
02471
02472 for (size_t ijoint = 0; ijoint < kiscene->getBind_joint_axis_array().getCount(); ++ijoint) {
02473 domBind_joint_axisRef bindjoint = kiscene->getBind_joint_axis_array()[ijoint];
02474 daeElementRef pjtarget = daeSidRef(bindjoint->getTarget(), viscene->getUrl().getElement()).resolve().elt;
02475 if (!pjtarget) {
02476 ROS_ERROR_STREAM(str(boost::format("Target Node %s NOT found!!!\n")%bindjoint->getTarget()));
02477 continue;
02478 }
02479 daeElement* pelt = searchBinding(bindjoint->getAxis(),kscene);
02480 domAxis_constraintRef pjointaxis = daeSafeCast<domAxis_constraint>(pelt);
02481 if (!pjointaxis) {
02482 continue;
02483 }
02484 bindings.listAxisBindings.push_back(JointAxisBinding(pjtarget, pjointaxis, bindjoint->getValue(), NULL, NULL));
02485 }
02486 }
02487
02488 static void _ExtractPhysicsBindings(domCOLLADA::domSceneRef allscene, KinematicsSceneBindings& bindings)
02489 {
02490 for(size_t iphysics = 0; iphysics < allscene->getInstance_physics_scene_array().getCount(); ++iphysics) {
02491 domPhysics_sceneRef pscene = daeSafeCast<domPhysics_scene>(allscene->getInstance_physics_scene_array()[iphysics]->getUrl().getElement().cast());
02492 for(size_t imodel = 0; imodel < pscene->getInstance_physics_model_array().getCount(); ++imodel) {
02493 domInstance_physics_modelRef ipmodel = pscene->getInstance_physics_model_array()[imodel];
02494 domPhysics_modelRef pmodel = daeSafeCast<domPhysics_model> (ipmodel->getUrl().getElement().cast());
02495 domNodeRef nodephysicsoffset = daeSafeCast<domNode>(ipmodel->getParent().getElement().cast());
02496 for(size_t ibody = 0; ibody < ipmodel->getInstance_rigid_body_array().getCount(); ++ibody) {
02497 LinkBinding lb;
02498 lb.irigidbody = ipmodel->getInstance_rigid_body_array()[ibody];
02499 lb.node = daeSafeCast<domNode>(lb.irigidbody->getTarget().getElement().cast());
02500 lb.rigidbody = daeSafeCast<domRigid_body>(daeSidRef(lb.irigidbody->getBody(),pmodel).resolve().elt);
02501 lb.nodephysicsoffset = nodephysicsoffset;
02502 if( !!lb.rigidbody && !!lb.node ) {
02503 bindings.listLinkBindings.push_back(lb);
02504 }
02505 }
02506 }
02507 }
02508 }
02509
02510
02511 size_t _countChildren(daeElement* pelt) {
02512 size_t c = 1;
02513 daeTArray<daeElementRef> children;
02514 pelt->getChildren(children);
02515 for (size_t i = 0; i < children.getCount(); ++i) {
02516 c += _countChildren(children[i]);
02517 }
02518 return c;
02519 }
02520
02521 void _processUserData(daeElement* pelt, double scale)
02522 {
02523
02524 domAssetRef passet = daeSafeCast<domAsset> (pelt->getChild("asset"));
02525 if (!!passet && !!passet->getUnit()) {
02526 scale = passet->getUnit()->getMeter();
02527 }
02528
02529 _vuserdata.push_back(USERDATA(scale));
02530 pelt->setUserData(&_vuserdata.back());
02531 daeTArray<daeElementRef> children;
02532 pelt->getChildren(children);
02533 for (size_t i = 0; i < children.getCount(); ++i) {
02534 if (children[i] != passet) {
02535 _processUserData(children[i], scale);
02536 }
02537 }
02538 }
02539
02540 USERDATA* _getUserData(daeElement* pelt)
02541 {
02542 BOOST_ASSERT(!!pelt);
02543 void* p = pelt->getUserData();
02544 BOOST_ASSERT(!!p);
02545 return (USERDATA*)p;
02546 }
02547
02548
02549
02550
02551
02552 static Vector3 _poseMult(const Pose& p, const Vector3& v)
02553 {
02554 double ww = 2 * p.rotation.x * p.rotation.x;
02555 double wx = 2 * p.rotation.x * p.rotation.y;
02556 double wy = 2 * p.rotation.x * p.rotation.z;
02557 double wz = 2 * p.rotation.x * p.rotation.w;
02558 double xx = 2 * p.rotation.y * p.rotation.y;
02559 double xy = 2 * p.rotation.y * p.rotation.z;
02560 double xz = 2 * p.rotation.y * p.rotation.w;
02561 double yy = 2 * p.rotation.z * p.rotation.z;
02562 double yz = 2 * p.rotation.z * p.rotation.w;
02563 Vector3 vnew;
02564 vnew.x = (1-xx-yy) * v.x + (wx-yz) * v.y + (wy+xz)*v.z + p.position.x;
02565 vnew.y = (wx+yz) * v.x + (1-ww-yy) * v.y + (xy-wz)*v.z + p.position.y;
02566 vnew.z = (wy-xz) * v.x + (xy+wz) * v.y + (1-ww-xx)*v.z + p.position.z;
02567 return vnew;
02568 }
02569
02570 static Vector3 _poseMult(const boost::array<double,12>& m, const Vector3& v)
02571 {
02572 Vector3 vnew;
02573 vnew.x = m[4*0+0] * v.x + m[4*0+1] * v.y + m[4*0+2] * v.z + m[4*0+3];
02574 vnew.y = m[4*1+0] * v.x + m[4*1+1] * v.y + m[4*1+2] * v.z + m[4*1+3];
02575 vnew.z = m[4*2+0] * v.x + m[4*2+1] * v.y + m[4*2+2] * v.z + m[4*2+3];
02576 return vnew;
02577 }
02578
02579 static boost::array<double,12> _poseMult(const boost::array<double,12>& m0, const boost::array<double,12>& m1)
02580 {
02581 boost::array<double,12> mres;
02582 mres[0*4+0] = m0[0*4+0]*m1[0*4+0]+m0[0*4+1]*m1[1*4+0]+m0[0*4+2]*m1[2*4+0];
02583 mres[0*4+1] = m0[0*4+0]*m1[0*4+1]+m0[0*4+1]*m1[1*4+1]+m0[0*4+2]*m1[2*4+1];
02584 mres[0*4+2] = m0[0*4+0]*m1[0*4+2]+m0[0*4+1]*m1[1*4+2]+m0[0*4+2]*m1[2*4+2];
02585 mres[1*4+0] = m0[1*4+0]*m1[0*4+0]+m0[1*4+1]*m1[1*4+0]+m0[1*4+2]*m1[2*4+0];
02586 mres[1*4+1] = m0[1*4+0]*m1[0*4+1]+m0[1*4+1]*m1[1*4+1]+m0[1*4+2]*m1[2*4+1];
02587 mres[1*4+2] = m0[1*4+0]*m1[0*4+2]+m0[1*4+1]*m1[1*4+2]+m0[1*4+2]*m1[2*4+2];
02588 mres[2*4+0] = m0[2*4+0]*m1[0*4+0]+m0[2*4+1]*m1[1*4+0]+m0[2*4+2]*m1[2*4+0];
02589 mres[2*4+1] = m0[2*4+0]*m1[0*4+1]+m0[2*4+1]*m1[1*4+1]+m0[2*4+2]*m1[2*4+1];
02590 mres[2*4+2] = m0[2*4+0]*m1[0*4+2]+m0[2*4+1]*m1[1*4+2]+m0[2*4+2]*m1[2*4+2];
02591 mres[3] = m1[3] * m0[0] + m1[7] * m0[1] + m1[11] * m0[2] + m0[3];
02592 mres[7] = m1[3] * m0[4] + m1[7] * m0[5] + m1[11] * m0[6] + m0[7];
02593 mres[11] = m1[3] * m0[8] + m1[7] * m0[9] + m1[11] * m0[10] + m0[11];
02594 return mres;
02595 }
02596
02597 static Pose _poseMult(const Pose& p0, const Pose& p1)
02598 {
02599 Pose p;
02600 p.position = _poseMult(p0,p1.position);
02601 p.rotation = _quatMult(p0.rotation,p1.rotation);
02602 return p;
02603 }
02604
02605 static Pose _poseInverse(const Pose& p)
02606 {
02607 Pose pinv;
02608 pinv.rotation.x = -p.rotation.x;
02609 pinv.rotation.y = -p.rotation.y;
02610 pinv.rotation.z = -p.rotation.z;
02611 pinv.rotation.w = p.rotation.w;
02612 Vector3 t = _poseMult(pinv,p.position);
02613 pinv.position.x = -t.x;
02614 pinv.position.y = -t.y;
02615 pinv.position.z = -t.z;
02616 return pinv;
02617 }
02618
02619 static Rotation _quatMult(const Rotation& quat0, const Rotation& quat1)
02620 {
02621 Rotation q;
02622 q.x = quat0.w*quat1.x + quat0.x*quat1.w + quat0.y*quat1.z - quat0.z*quat1.y;
02623 q.y = quat0.w*quat1.y + quat0.y*quat1.w + quat0.z*quat1.x - quat0.x*quat1.z;
02624 q.z = quat0.w*quat1.z + quat0.z*quat1.w + quat0.x*quat1.y - quat0.y*quat1.x;
02625 q.w = quat0.w*quat1.w - quat0.x*quat1.x - quat0.y*quat1.y - quat0.z*quat1.z;
02626 double fnorm = std::sqrt(q.x*q.x+q.y*q.y+q.z*q.z+q.w*q.w);
02627
02628 q.x /= fnorm;
02629 q.y /= fnorm;
02630 q.z /= fnorm;
02631 q.w /= fnorm;
02632 return q;
02633 }
02634
02635 static boost::array<double,12> _matrixFromAxisAngle(const Vector3& axis, double angle)
02636 {
02637 return _matrixFromQuat(_quatFromAxisAngle(axis.x,axis.y,axis.z,angle));
02638 }
02639
02640 static boost::array<double,12> _matrixFromQuat(const Rotation& quat)
02641 {
02642 boost::array<double,12> m;
02643 double qq1 = 2*quat.x*quat.x;
02644 double qq2 = 2*quat.y*quat.y;
02645 double qq3 = 2*quat.z*quat.z;
02646 m[4*0+0] = 1 - qq2 - qq3;
02647 m[4*0+1] = 2*(quat.x*quat.y - quat.w*quat.z);
02648 m[4*0+2] = 2*(quat.x*quat.z + quat.w*quat.y);
02649 m[4*0+3] = 0;
02650 m[4*1+0] = 2*(quat.x*quat.y + quat.w*quat.z);
02651 m[4*1+1] = 1 - qq1 - qq3;
02652 m[4*1+2] = 2*(quat.y*quat.z - quat.w*quat.x);
02653 m[4*1+3] = 0;
02654 m[4*2+0] = 2*(quat.x*quat.z - quat.w*quat.y);
02655 m[4*2+1] = 2*(quat.y*quat.z + quat.w*quat.x);
02656 m[4*2+2] = 1 - qq1 - qq2;
02657 m[4*2+3] = 0;
02658 return m;
02659 }
02660
02661 static Pose _poseFromMatrix(const boost::array<double,12>& m)
02662 {
02663 Pose t;
02664 t.rotation = _quatFromMatrix(m);
02665 t.position.x = m[3];
02666 t.position.y = m[7];
02667 t.position.z = m[11];
02668 return t;
02669 }
02670
02671 static boost::array<double,12> _matrixFromPose(const Pose& t)
02672 {
02673 boost::array<double,12> m = _matrixFromQuat(t.rotation);
02674 m[3] = t.position.x;
02675 m[7] = t.position.y;
02676 m[11] = t.position.z;
02677 return m;
02678 }
02679
02680 static Rotation _quatFromAxisAngle(double x, double y, double z, double angle)
02681 {
02682 Rotation q;
02683 double axislen = std::sqrt(x*x+y*y+z*z);
02684 if( axislen == 0 ) {
02685 return q;
02686 }
02687 angle *= 0.5;
02688 double sang = std::sin(angle)/axislen;
02689 q.w = std::cos(angle);
02690 q.x = x*sang;
02691 q.y = y*sang;
02692 q.z = z*sang;
02693 return q;
02694 }
02695
02696 static Rotation _quatFromMatrix(const boost::array<double,12>& mat)
02697 {
02698 Rotation rot;
02699 double tr = mat[4*0+0] + mat[4*1+1] + mat[4*2+2];
02700 if (tr >= 0) {
02701 rot.w = tr + 1;
02702 rot.x = (mat[4*2+1] - mat[4*1+2]);
02703 rot.y = (mat[4*0+2] - mat[4*2+0]);
02704 rot.z = (mat[4*1+0] - mat[4*0+1]);
02705 }
02706 else {
02707
02708 if (mat[4*1+1] > mat[4*0+0]) {
02709 if (mat[4*2+2] > mat[4*1+1]) {
02710 rot.z = (mat[4*2+2] - (mat[4*0+0] + mat[4*1+1])) + 1;
02711 rot.x = (mat[4*2+0] + mat[4*0+2]);
02712 rot.y = (mat[4*1+2] + mat[4*2+1]);
02713 rot.w = (mat[4*1+0] - mat[4*0+1]);
02714 }
02715 else {
02716 rot.y = (mat[4*1+1] - (mat[4*2+2] + mat[4*0+0])) + 1;
02717 rot.z = (mat[4*1+2] + mat[4*2+1]);
02718 rot.x = (mat[4*0+1] + mat[4*1+0]);
02719 rot.w = (mat[4*0+2] - mat[4*2+0]);
02720 }
02721 }
02722 else if (mat[4*2+2] > mat[4*0+0]) {
02723 rot.z = (mat[4*2+2] - (mat[4*0+0] + mat[4*1+1])) + 1;
02724 rot.x = (mat[4*2+0] + mat[4*0+2]);
02725 rot.y = (mat[4*1+2] + mat[4*2+1]);
02726 rot.w = (mat[4*1+0] - mat[4*0+1]);
02727 }
02728 else {
02729 rot.x = (mat[4*0+0] - (mat[4*1+1] + mat[4*2+2])) + 1;
02730 rot.y = (mat[4*0+1] + mat[4*1+0]);
02731 rot.z = (mat[4*2+0] + mat[4*0+2]);
02732 rot.w = (mat[4*2+1] - mat[4*1+2]);
02733 }
02734 }
02735 double fnorm = std::sqrt(rot.x*rot.x+rot.y*rot.y+rot.z*rot.z+rot.w*rot.w);
02736
02737 rot.x /= fnorm;
02738 rot.y /= fnorm;
02739 rot.z /= fnorm;
02740 rot.w /= fnorm;
02741 return rot;
02742 }
02743
02744 static double _dot3(const Vector3& v0, const Vector3& v1)
02745 {
02746 return v0.x*v1.x + v0.y*v1.y + v0.z*v1.z;
02747 }
02748 static Vector3 _cross3(const Vector3& v0, const Vector3& v1)
02749 {
02750 Vector3 v;
02751 v.x = v0.y * v1.z - v0.z * v1.y;
02752 v.y = v0.z * v1.x - v0.x * v1.z;
02753 v.z = v0.x * v1.y - v0.y * v1.x;
02754 return v;
02755 }
02756 static Vector3 _sub3(const Vector3& v0, const Vector3& v1)
02757 {
02758 Vector3 v;
02759 v.x = v0.x-v1.x;
02760 v.y = v0.y-v1.y;
02761 v.z = v0.z-v1.z;
02762 return v;
02763 }
02764 static Vector3 _add3(const Vector3& v0, const Vector3& v1)
02765 {
02766 Vector3 v;
02767 v.x = v0.x+v1.x;
02768 v.y = v0.y+v1.y;
02769 v.z = v0.z+v1.z;
02770 return v;
02771 }
02772 static Vector3 _normalize3(const Vector3& v0)
02773 {
02774 Vector3 v;
02775 double norm = std::sqrt(v0.x*v0.x+v0.y*v0.y+v0.z*v0.z);
02776 v.x = v0.x/norm;
02777 v.y = v0.y/norm;
02778 v.z = v0.z/norm;
02779 return v;
02780 }
02781
02782 boost::shared_ptr<DAE> _collada;
02783 domCOLLADA* _dom;
02784 std::vector<USERDATA> _vuserdata;
02785 int _nGlobalSensorId, _nGlobalManipulatorId;
02786 std::string _filename;
02787 std::string _resourcedir;
02788 boost::shared_ptr<ModelInterface> _model;
02789 Pose _RootOrigin;
02790 Pose _VisualRootOrigin;
02791 };
02792
02793
02794
02795
02796 boost::shared_ptr<ModelInterface> parseCollada(const std::string &xml_str)
02797 {
02798 boost::shared_ptr<ModelInterface> model(new ModelInterface);
02799
02800 ColladaModelReader reader(model);
02801 if (!reader.InitFromData(xml_str))
02802 model.reset();
02803 return model;
02804 }
02805 }