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 #define PRINT_POSE(pname, apose) ROS_DEBUG(pname" pos: %f %f %f, rot: %f %f %f %f", \
01042 apose.position.x, apose.position.y, apose.position.z, \
01043 apose.rotation.x, apose.rotation.y, apose.rotation.z, apose.rotation.w);
01044 {
01045 PRINT_POSE("tatt", tatt);
01046 PRINT_POSE("trans_joint_to_child", tatt);
01047 Pose trans_joint_to_child = _poseFromMatrix(_ExtractFullTransform(pattfull->getLink()));
01048
01049 pjoint->parent_to_joint_origin_transform = _poseMult(tatt, trans_joint_to_child);
01050
01051 Vector3 ax(pdomaxis->getAxis()->getValue()[0],
01052 pdomaxis->getAxis()->getValue()[1],
01053 pdomaxis->getAxis()->getValue()[2]);
01054 Pose pinv = _poseInverse(trans_joint_to_child);
01055
01056 ax = pinv.rotation * ax;
01057
01058 pjoint->axis.x = ax.x;
01059 pjoint->axis.y = ax.y;
01060 pjoint->axis.z = ax.z;
01061
01062 ROS_DEBUG("joint %s axis: %f %f %f -> %f %f %f\n", pjoint->name.c_str(),
01063 pdomaxis->getAxis()->getValue()[0],
01064 pdomaxis->getAxis()->getValue()[1],
01065 pdomaxis->getAxis()->getValue()[2],
01066 pjoint->axis.x, pjoint->axis.y, pjoint->axis.z);
01067 }
01068
01069 if (!motion_axis_info) {
01070 ROS_WARN_STREAM(str(boost::format("No motion axis info for joint %s\n")%pjoint->name));
01071 }
01072
01073
01074 if (!!motion_axis_info) {
01075 if (!!motion_axis_info->getSpeed()) {
01076 pjoint->limits->velocity = resolveFloat(motion_axis_info->getSpeed(),motion_axis_info);
01077 ROS_DEBUG("... Joint Speed: %f...\n",pjoint->limits->velocity);
01078 }
01079 if (!!motion_axis_info->getAcceleration()) {
01080 pjoint->limits->effort = resolveFloat(motion_axis_info->getAcceleration(),motion_axis_info);
01081 ROS_DEBUG("... Joint effort: %f...\n",pjoint->limits->effort);
01082 }
01083 }
01084
01085 bool joint_locked = false;
01086 bool kinematics_limits = false;
01087
01088 if (!!kinematics_axis_info) {
01089 if (!!kinematics_axis_info->getLocked()) {
01090 joint_locked = resolveBool(kinematics_axis_info->getLocked(),kinematics_axis_info);
01091 }
01092
01093 if (joint_locked) {
01094 if( pjoint->type == Joint::REVOLUTE || pjoint->type ==Joint::PRISMATIC) {
01095 ROS_WARN_STREAM("lock joint!!\n");
01096 pjoint->limits->lower = 0;
01097 pjoint->limits->upper = 0;
01098 }
01099 }
01100 else if (kinematics_axis_info->getLimits()) {
01101 kinematics_limits = true;
01102 double fscale = (pjoint->type == Joint::REVOLUTE) ? (M_PI/180.0f) : _GetUnitScale(kinematics_axis_info);
01103 if( pjoint->type == Joint::REVOLUTE || pjoint->type ==Joint::PRISMATIC) {
01104 pjoint->limits->lower = fscale*(double)(resolveFloat(kinematics_axis_info->getLimits()->getMin(),kinematics_axis_info));
01105 pjoint->limits->upper = fscale*(double)(resolveFloat(kinematics_axis_info->getLimits()->getMax(),kinematics_axis_info));
01106 if ( pjoint->limits->lower == 0 && pjoint->limits->upper == 0 ) {
01107 pjoint->type = Joint::FIXED;
01108 }
01109 }
01110 }
01111 }
01112
01113
01114 if (!kinematics_axis_info || (!joint_locked && !kinematics_limits)) {
01115
01116 if( !pdomaxis->getLimits() ) {
01117 ROS_DEBUG_STREAM(str(boost::format("There are NO LIMITS in joint %s:%d ...\n")%pjoint->name%kinematics_limits));
01118 if( pjoint->type == Joint::REVOLUTE ) {
01119 pjoint->type = Joint::CONTINUOUS;
01120 pjoint->limits->lower = -M_PI;
01121 pjoint->limits->upper = M_PI;
01122 }
01123 else {
01124 pjoint->limits->lower = -100000;
01125 pjoint->limits->upper = 100000;
01126 }
01127 }
01128 else {
01129 ROS_DEBUG_STREAM(str(boost::format("There are LIMITS in joint %s ...\n")%pjoint->name));
01130 double fscale = (pjoint->type == Joint::REVOLUTE) ? (M_PI/180.0f) : _GetUnitScale(pdomaxis);
01131 pjoint->limits->lower = (double)pdomaxis->getLimits()->getMin()->getValue()*fscale;
01132 pjoint->limits->upper = (double)pdomaxis->getLimits()->getMax()->getValue()*fscale;
01133 if ( pjoint->limits->lower == 0 && pjoint->limits->upper == 0 ) {
01134 pjoint->type = Joint::FIXED;
01135 }
01136 }
01137 }
01138
01139 if (pjoint->limits->velocity == 0.0) {
01140 pjoint->limits->velocity = pjoint->type == Joint::PRISMATIC ? 0.01 : 0.5f;
01141 }
01142 pchildlink.reset();
01143 ++numjoints;
01144 }
01145 }
01146 }
01147
01148 if( pdomlink->getAttachment_start_array().getCount() > 0 ) {
01149 ROS_WARN("urdf collada reader does not support attachment_start\n");
01150 }
01151 if( pdomlink->getAttachment_end_array().getCount() > 0 ) {
01152 ROS_WARN("urdf collada reader does not support attachment_end\n");
01153 }
01154
01155 plink->visual->geometry = _CreateGeometry(plink->name, listGeomProperties);
01156
01157
01158
01159
01160
01161
01162 if( !plink->visual->geometry ) {
01163 plink->visual.reset();
01164 plink->collision.reset();
01165 } else {
01166
01167 plink->collision.reset(new Collision());
01168 plink->collision->geometry = plink->visual->geometry;
01169 plink->collision->origin = plink->visual->origin;
01170 }
01171
01172
01173
01174
01175
01176
01177
01178 return plink;
01179 }
01180
01181 boost::shared_ptr<Geometry> _CreateGeometry(const std::string& name, const std::list<GEOMPROPERTIES>& listGeomProperties)
01182 {
01183 std::vector<std::vector<Vector3> > vertices;
01184 std::vector<std::vector<int> > indices;
01185 std::vector<Color> ambients;
01186 std::vector<Color> diffuses;
01187 unsigned int index, vert_counter;
01188 vertices.resize(listGeomProperties.size());
01189 indices.resize(listGeomProperties.size());
01190 ambients.resize(listGeomProperties.size());
01191 diffuses.resize(listGeomProperties.size());
01192 index = 0;
01193 vert_counter = 0;
01194 FOREACHC(it, listGeomProperties) {
01195 vertices[index].resize(it->vertices.size());
01196 for(size_t i = 0; i < it->vertices.size(); ++i) {
01197 vertices[index][i] = _poseMult(it->_t, it->vertices[i]);
01198 vert_counter++;
01199 }
01200 indices[index].resize(it->indices.size());
01201 for(size_t i = 0; i < it->indices.size(); ++i) {
01202 indices[index][i] = it->indices[i];
01203 }
01204 ambients[index] = it->ambientColor;
01205 diffuses[index] = it->diffuseColor;
01206
01207
01208 ambients[index].r *= 0.5; ambients[index].g *= 0.5; ambients[index].b *= 0.5;
01209 if ( ambients[index].r == 0.0 ) {
01210 ambients[index].r = 0.0001;
01211 }
01212 if ( ambients[index].g == 0.0 ) {
01213 ambients[index].g = 0.0001;
01214 }
01215 if ( ambients[index].b == 0.0 ) {
01216 ambients[index].b = 0.0001;
01217 }
01218 index++;
01219 }
01220
01221 if (vert_counter == 0) {
01222 boost::shared_ptr<Mesh> ret;
01223 ret.reset();
01224 return ret;
01225 }
01226
01227 boost::shared_ptr<Mesh> geometry(new Mesh());
01228 geometry->type = Geometry::MESH;
01229 geometry->scale.x = 1;
01230 geometry->scale.y = 1;
01231 geometry->scale.z = 1;
01232
01233
01234 std::stringstream daedata;
01235 daedata << str(boost::format("<?xml version=\"1.0\" encoding=\"utf-8\"?>\n\
01236 <COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n\
01237 <asset>\n\
01238 <contributor>\n\
01239 <author>Rosen Diankov</author>\n\
01240 <comments>\n\
01241 robot_model/urdf temporary collada geometry\n\
01242 </comments>\n\
01243 </contributor>\n\
01244 <unit name=\"meter\" meter=\"1.0\"/>\n\
01245 <up_axis>Y_UP</up_axis>\n\
01246 </asset>\n\
01247 <library_materials>\n"));
01248 for(unsigned int i=0; i < index; i++) {
01249 daedata << str(boost::format("\
01250 <material id=\"blinn2%d\" name=\"blinn2%d\">\n\
01251 <instance_effect url=\"#blinn2-fx%d\"/>\n\
01252 </material>\n")%i%i%i);
01253 }
01254 daedata << "\
01255 </library_materials>\n\
01256 <library_effects>\n";
01257 for(unsigned int i=0; i < index; i++) {
01258 daedata << str(boost::format("\
01259 <effect id=\"blinn2-fx%d\">\n\
01260 <profile_COMMON>\n\
01261 <technique sid=\"common\">\n\
01262 <phong>\n\
01263 <ambient>\n\
01264 <color>%f %f %f %f</color>\n\
01265 </ambient>\n\
01266 <diffuse>\n\
01267 <color>%f %f %f %f</color>\n\
01268 </diffuse>\n\
01269 <specular>\n\
01270 <color>0 0 0 1</color>\n\
01271 </specular>\n\
01272 </phong>\n\
01273 </technique>\n\
01274 </profile_COMMON>\n\
01275 </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);
01276 }
01277 daedata << str(boost::format("\
01278 </library_effects>\n\
01279 <library_geometries>\n"));
01280
01281 for(unsigned int i=0; i < index; i++) {
01282 daedata << str(boost::format("\
01283 <geometry id=\"base2_M1KShape%d\" name=\"base2_M1KShape%d\">\n\
01284 <mesh>\n\
01285 <source id=\"geo0.positions\">\n\
01286 <float_array id=\"geo0.positions-array\" count=\"%d\">")%i%i%(vertices[i].size()*3));
01287 FOREACH(it,vertices[i]) {
01288 daedata << it->x << " " << it->y << " " << it->z << " ";
01289 }
01290 daedata << str(boost::format("\n\
01291 </float_array>\n\
01292 <technique_common>\n\
01293 <accessor count=\"%d\" source=\"#geo0.positions-array\" stride=\"3\">\n\
01294 <param name=\"X\" type=\"float\"/>\n\
01295 <param name=\"Y\" type=\"float\"/>\n\
01296 <param name=\"Z\" type=\"float\"/>\n\
01297 </accessor>\n\
01298 </technique_common>\n\
01299 </source>\n\
01300 <vertices id=\"geo0.vertices\">\n\
01301 <input semantic=\"POSITION\" source=\"#geo0.positions\"/>\n\
01302 </vertices>\n\
01303 <triangles count=\"%d\" material=\"lambert2SG\">\n\
01304 <input offset=\"0\" semantic=\"VERTEX\" source=\"#geo0.vertices\"/>\n\
01305 <p>")%vertices[i].size()%(indices[i].size()/3));
01306
01307 FOREACH(it,indices[i]) {
01308 daedata << *it << " ";
01309 }
01310 daedata << str(boost::format("</p>\n\
01311 </triangles>\n\
01312 </mesh>\n\
01313 </geometry>\n"));
01314 }
01315 daedata << str(boost::format("\
01316 </library_geometries>\n\
01317 <library_visual_scenes>\n\
01318 <visual_scene id=\"VisualSceneNode\" name=\"base1d_med\">\n\
01319 <node id=\"%s\" name=\"%s\" type=\"NODE\">\n")%name%name);
01320 for(unsigned int i=0; i < index; i++) {
01321 daedata << str(boost::format("\
01322 <instance_geometry url=\"#base2_M1KShape%i\">\n\
01323 <bind_material>\n\
01324 <technique_common>\n\
01325 <instance_material symbol=\"lambert2SG\" target=\"#blinn2%i\"/>\n\
01326 </technique_common>\n\
01327 </bind_material>\n\
01328 </instance_geometry>\n")%i%i);
01329 }
01330 daedata << str(boost::format("\
01331 </node>\n\
01332 </visual_scene>\n\
01333 </library_visual_scenes>\n\
01334 <scene>\n\
01335 <instance_visual_scene url=\"#VisualSceneNode\"/>\n\
01336 </scene>\n\
01337 </COLLADA>"));
01338
01339 #ifdef HAVE_MKSTEMPS
01340 geometry->filename = str(boost::format("/tmp/collada_model_reader_%s_XXXXXX.dae")%name);
01341 int fd = mkstemps(&geometry->filename[0],4);
01342 #else
01343 int fd = -1;
01344 for(int iter = 0; iter < 1000; ++iter) {
01345 geometry->filename = str(boost::format("/tmp/collada_model_reader_%s_%d.dae")%name%rand());
01346 if( !!std::ifstream(geometry->filename.c_str())) {
01347 fd = open(geometry->filename.c_str(),O_WRONLY|O_CREAT|O_EXCL);
01348 if( fd != -1 ) {
01349 break;
01350 }
01351 }
01352 }
01353 if( fd == -1 ) {
01354 ROS_ERROR("failed to open geometry dae file %s",geometry->filename.c_str());
01355 return geometry;
01356 }
01357 #endif
01358
01359 std::string daedatastr = daedata.str();
01360 if( (size_t)write(fd,daedatastr.c_str(),daedatastr.size()) != daedatastr.size() ) {
01361 ROS_ERROR("failed to write to geometry dae file %s",geometry->filename.c_str());
01362 }
01363 close(fd);
01364 _listTempFilenames.push_back(boost::shared_ptr<UnlinkFilename>(new UnlinkFilename(geometry->filename)));
01365 geometry->filename = std::string("file://") + geometry->filename;
01366 return geometry;
01367 }
01368
01372 void _ExtractGeometry(const domNodeRef pdomnode,std::list<GEOMPROPERTIES>& listGeomProperties, const std::list<JointAxisBinding>& listAxisBindings, const Pose& tlink)
01373 {
01374 if( !pdomnode ) {
01375 return;
01376 }
01377
01378 ROS_DEBUG_STREAM(str(boost::format("ExtractGeometry(node,link) of %s\n")%pdomnode->getName()));
01379
01380
01381 for (size_t i = 0; i < pdomnode->getNode_array().getCount(); i++) {
01382
01383 bool contains=false;
01384 FOREACHC(it,listAxisBindings) {
01385
01386 if ( (pdomnode->getNode_array()[i]) == (it->visualnode)) {
01387 contains=true;
01388 break;
01389 }
01390 }
01391 if (contains) {
01392 continue;
01393 }
01394
01395 _ExtractGeometry(pdomnode->getNode_array()[i],listGeomProperties, listAxisBindings,tlink);
01396
01397
01398
01399
01400 }
01401
01402 unsigned int nGeomBefore = listGeomProperties.size();
01403
01404
01405 for (size_t igeom = 0; igeom < pdomnode->getInstance_geometry_array().getCount(); ++igeom) {
01406 domInstance_geometryRef domigeom = pdomnode->getInstance_geometry_array()[igeom];
01407 domGeometryRef domgeom = daeSafeCast<domGeometry> (domigeom->getUrl().getElement());
01408 if (!domgeom) {
01409 continue;
01410 }
01411
01412
01413 std::map<std::string, domMaterialRef> mapmaterials;
01414 if (!!domigeom->getBind_material() && !!domigeom->getBind_material()->getTechnique_common()) {
01415 const domInstance_material_Array& matarray = domigeom->getBind_material()->getTechnique_common()->getInstance_material_array();
01416 for (size_t imat = 0; imat < matarray.getCount(); ++imat) {
01417 domMaterialRef pmat = daeSafeCast<domMaterial>(matarray[imat]->getTarget().getElement());
01418 if (!!pmat) {
01419 mapmaterials[matarray[imat]->getSymbol()] = pmat;
01420 }
01421 }
01422 }
01423
01424
01425 _ExtractGeometry(domgeom, mapmaterials, listGeomProperties);
01426 }
01427
01428 std::list<GEOMPROPERTIES>::iterator itgeom= listGeomProperties.begin();
01429 for (unsigned int i=0; i< nGeomBefore; i++) {
01430 itgeom++;
01431 }
01432
01433 boost::array<double,12> tmnodegeom = _poseMult(_matrixFromPose(_poseInverse(tlink)),
01434 _poseMult(_poseMult(_matrixFromPose(_poseInverse(_VisualRootOrigin)),
01435 _getNodeParentTransform(pdomnode)),
01436 _ExtractFullTransform(pdomnode)));
01437 Pose tnodegeom;
01438 Vector3 vscale(1,1,1);
01439 _decompose(tmnodegeom, tnodegeom, vscale);
01440
01441 ROS_DEBUG_STREAM("tnodegeom: " << pdomnode->getName()
01442 << tnodegeom.position.x << " " << tnodegeom.position.y << " " << tnodegeom.position.z << " / "
01443 << tnodegeom.rotation.x << " " << tnodegeom.rotation.y << " " << tnodegeom.rotation.z << " "
01444 << tnodegeom.rotation.w);
01445
01446
01447
01448
01449
01450
01451
01452
01453 for (; itgeom != listGeomProperties.end(); itgeom++) {
01454 itgeom->_t = tnodegeom;
01455 switch (itgeom->type) {
01456 case GeomBox:
01457 itgeom->vGeomData.x *= vscale.x;
01458 itgeom->vGeomData.y *= vscale.y;
01459 itgeom->vGeomData.z *= vscale.z;
01460 break;
01461 case GeomSphere: {
01462 itgeom->vGeomData.x *= std::max(vscale.z, std::max(vscale.x, vscale.y));
01463 break;
01464 }
01465 case GeomCylinder:
01466 itgeom->vGeomData.x *= std::max(vscale.x, vscale.y);
01467 itgeom->vGeomData.y *= vscale.z;
01468 break;
01469 case GeomTrimesh:
01470 for(size_t i = 0; i < itgeom->vertices.size(); ++i ) {
01471 itgeom->vertices[i] = _poseMult(tmnodegeom,itgeom->vertices[i]);
01472 }
01473 itgeom->_t = Pose();
01474 break;
01475 default:
01476 ROS_WARN_STREAM(str(boost::format("unknown geometry type: %d\n")%itgeom->type));
01477 }
01478 }
01479 }
01480
01484 void _FillGeometryColor(const domMaterialRef pmat, GEOMPROPERTIES& geom)
01485 {
01486 if( !!pmat && !!pmat->getInstance_effect() ) {
01487 domEffectRef peffect = daeSafeCast<domEffect>(pmat->getInstance_effect()->getUrl().getElement().cast());
01488 if( !!peffect ) {
01489 domProfile_common::domTechnique::domPhongRef pphong = daeSafeCast<domProfile_common::domTechnique::domPhong>(peffect->getDescendant(daeElement::matchType(domProfile_common::domTechnique::domPhong::ID())));
01490 if( !!pphong ) {
01491 if( !!pphong->getAmbient() && !!pphong->getAmbient()->getColor() ) {
01492 domFx_color c = pphong->getAmbient()->getColor()->getValue();
01493 geom.ambientColor.r = c[0];
01494 geom.ambientColor.g = c[1];
01495 geom.ambientColor.b = c[2];
01496 geom.ambientColor.a = c[3];
01497 }
01498 if( !!pphong->getDiffuse() && !!pphong->getDiffuse()->getColor() ) {
01499 domFx_color c = pphong->getDiffuse()->getColor()->getValue();
01500 geom.diffuseColor.r = c[0];
01501 geom.diffuseColor.g = c[1];
01502 geom.diffuseColor.b = c[2];
01503 geom.diffuseColor.a = c[3];
01504 }
01505 }
01506 }
01507 }
01508 }
01509
01515 bool _ExtractGeometry(const domTrianglesRef triRef, const domVerticesRef vertsRef, const std::map<std::string,domMaterialRef>& mapmaterials, std::list<GEOMPROPERTIES>& listGeomProperties)
01516 {
01517 if( !triRef ) {
01518 return false;
01519 }
01520 listGeomProperties.push_back(GEOMPROPERTIES());
01521 GEOMPROPERTIES& geom = listGeomProperties.back();
01522 geom.type = GeomTrimesh;
01523
01524
01525 if( !!triRef->getMaterial() ) {
01526 std::map<std::string,domMaterialRef>::const_iterator itmat = mapmaterials.find(triRef->getMaterial());
01527 if( itmat != mapmaterials.end() ) {
01528 _FillGeometryColor(itmat->second,geom);
01529 }
01530 }
01531
01532 size_t triangleIndexStride = 0, vertexoffset = -1;
01533 domInput_local_offsetRef indexOffsetRef;
01534
01535 for (unsigned int w=0; w<triRef->getInput_array().getCount(); w++) {
01536 size_t offset = triRef->getInput_array()[w]->getOffset();
01537 daeString str = triRef->getInput_array()[w]->getSemantic();
01538 if (!strcmp(str,"VERTEX")) {
01539 indexOffsetRef = triRef->getInput_array()[w];
01540 vertexoffset = offset;
01541 }
01542 if (offset> triangleIndexStride) {
01543 triangleIndexStride = offset;
01544 }
01545 }
01546 triangleIndexStride++;
01547
01548 const domList_of_uints& indexArray =triRef->getP()->getValue();
01549 geom.indices.reserve(triRef->getCount()*3);
01550 geom.vertices.reserve(triRef->getCount()*3);
01551 for (size_t i=0; i<vertsRef->getInput_array().getCount(); ++i) {
01552 domInput_localRef localRef = vertsRef->getInput_array()[i];
01553 daeString str = localRef->getSemantic();
01554 if ( strcmp(str,"POSITION") == 0 ) {
01555 const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
01556 if( !node ) {
01557 continue;
01558 }
01559 double fUnitScale = _GetUnitScale(node);
01560 const domFloat_arrayRef flArray = node->getFloat_array();
01561 if (!!flArray) {
01562 const domList_of_floats& listFloats = flArray->getValue();
01563 int k=vertexoffset;
01564 int vertexStride = 3;
01565 for(size_t itri = 0; itri < triRef->getCount(); ++itri) {
01566 if(k+2*triangleIndexStride < indexArray.getCount() ) {
01567 for (int j=0; j<3; j++) {
01568 int index0 = indexArray.get(k)*vertexStride;
01569 domFloat fl0 = listFloats.get(index0);
01570 domFloat fl1 = listFloats.get(index0+1);
01571 domFloat fl2 = listFloats.get(index0+2);
01572 k+=triangleIndexStride;
01573 geom.indices.push_back(geom.vertices.size());
01574 geom.vertices.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale));
01575 }
01576 }
01577 }
01578 }
01579 else {
01580 ROS_WARN_STREAM("float array not defined!\n");
01581 }
01582 break;
01583 }
01584 }
01585 if( geom.indices.size() != 3*triRef->getCount() ) {
01586 ROS_WARN_STREAM("triangles declares wrong count!\n");
01587 }
01588 geom.InitCollisionMesh();
01589 return true;
01590 }
01591
01596 bool _ExtractGeometry(const domTrifansRef triRef, const domVerticesRef vertsRef, const std::map<std::string,domMaterialRef>& mapmaterials, std::list<GEOMPROPERTIES>& listGeomProperties)
01597 {
01598 if( !triRef ) {
01599 return false;
01600 }
01601 listGeomProperties.push_back(GEOMPROPERTIES());
01602 GEOMPROPERTIES& geom = listGeomProperties.back();
01603 geom.type = GeomTrimesh;
01604
01605
01606 if( !!triRef->getMaterial() ) {
01607 std::map<std::string,domMaterialRef>::const_iterator itmat = mapmaterials.find(triRef->getMaterial());
01608 if( itmat != mapmaterials.end() ) {
01609 _FillGeometryColor(itmat->second,geom);
01610 }
01611 }
01612
01613 size_t triangleIndexStride = 0, vertexoffset = -1;
01614 domInput_local_offsetRef indexOffsetRef;
01615
01616 for (unsigned int w=0; w<triRef->getInput_array().getCount(); w++) {
01617 size_t offset = triRef->getInput_array()[w]->getOffset();
01618 daeString str = triRef->getInput_array()[w]->getSemantic();
01619 if (!strcmp(str,"VERTEX")) {
01620 indexOffsetRef = triRef->getInput_array()[w];
01621 vertexoffset = offset;
01622 }
01623 if (offset> triangleIndexStride) {
01624 triangleIndexStride = offset;
01625 }
01626 }
01627 triangleIndexStride++;
01628 size_t primitivecount = triRef->getCount();
01629 if( primitivecount > triRef->getP_array().getCount() ) {
01630 ROS_WARN_STREAM("trifans has incorrect count\n");
01631 primitivecount = triRef->getP_array().getCount();
01632 }
01633 for(size_t ip = 0; ip < primitivecount; ++ip) {
01634 domList_of_uints indexArray =triRef->getP_array()[ip]->getValue();
01635 for (size_t i=0; i<vertsRef->getInput_array().getCount(); ++i) {
01636 domInput_localRef localRef = vertsRef->getInput_array()[i];
01637 daeString str = localRef->getSemantic();
01638 if ( strcmp(str,"POSITION") == 0 ) {
01639 const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
01640 if( !node ) {
01641 continue;
01642 }
01643 double fUnitScale = _GetUnitScale(node);
01644 const domFloat_arrayRef flArray = node->getFloat_array();
01645 if (!!flArray) {
01646 const domList_of_floats& listFloats = flArray->getValue();
01647 int k=vertexoffset;
01648 int vertexStride = 3;
01649 size_t usedindices = 3*(indexArray.getCount()-2);
01650 if( geom.indices.capacity() < geom.indices.size()+usedindices ) {
01651 geom.indices.reserve(geom.indices.size()+usedindices);
01652 }
01653 if( geom.vertices.capacity() < geom.vertices.size()+indexArray.getCount() ) {
01654 geom.vertices.reserve(geom.vertices.size()+indexArray.getCount());
01655 }
01656 size_t startoffset = (int)geom.vertices.size();
01657 while(k < (int)indexArray.getCount() ) {
01658 int index0 = indexArray.get(k)*vertexStride;
01659 domFloat fl0 = listFloats.get(index0);
01660 domFloat fl1 = listFloats.get(index0+1);
01661 domFloat fl2 = listFloats.get(index0+2);
01662 k+=triangleIndexStride;
01663 geom.vertices.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale));
01664 }
01665 for(size_t ivert = startoffset+2; ivert < geom.vertices.size(); ++ivert) {
01666 geom.indices.push_back(startoffset);
01667 geom.indices.push_back(ivert-1);
01668 geom.indices.push_back(ivert);
01669 }
01670 }
01671 else {
01672 ROS_WARN_STREAM("float array not defined!\n");
01673 }
01674 break;
01675 }
01676 }
01677 }
01678
01679 geom.InitCollisionMesh();
01680 return false;
01681 }
01682
01687 bool _ExtractGeometry(const domTristripsRef triRef, const domVerticesRef vertsRef, const std::map<std::string,domMaterialRef>& mapmaterials, std::list<GEOMPROPERTIES>& listGeomProperties)
01688 {
01689 if( !triRef ) {
01690 return false;
01691 }
01692 listGeomProperties.push_back(GEOMPROPERTIES());
01693 GEOMPROPERTIES& geom = listGeomProperties.back();
01694 geom.type = GeomTrimesh;
01695
01696
01697 if( !!triRef->getMaterial() ) {
01698 std::map<std::string,domMaterialRef>::const_iterator itmat = mapmaterials.find(triRef->getMaterial());
01699 if( itmat != mapmaterials.end() ) {
01700 _FillGeometryColor(itmat->second,geom);
01701 }
01702 }
01703 size_t triangleIndexStride = 0, vertexoffset = -1;
01704 domInput_local_offsetRef indexOffsetRef;
01705
01706 for (unsigned int w=0; w<triRef->getInput_array().getCount(); w++) {
01707 size_t offset = triRef->getInput_array()[w]->getOffset();
01708 daeString str = triRef->getInput_array()[w]->getSemantic();
01709 if (!strcmp(str,"VERTEX")) {
01710 indexOffsetRef = triRef->getInput_array()[w];
01711 vertexoffset = offset;
01712 }
01713 if (offset> triangleIndexStride) {
01714 triangleIndexStride = offset;
01715 }
01716 }
01717 triangleIndexStride++;
01718 size_t primitivecount = triRef->getCount();
01719 if( primitivecount > triRef->getP_array().getCount() ) {
01720 ROS_WARN_STREAM("tristrips has incorrect count\n");
01721 primitivecount = triRef->getP_array().getCount();
01722 }
01723 for(size_t ip = 0; ip < primitivecount; ++ip) {
01724 domList_of_uints indexArray = triRef->getP_array()[ip]->getValue();
01725 for (size_t i=0; i<vertsRef->getInput_array().getCount(); ++i) {
01726 domInput_localRef localRef = vertsRef->getInput_array()[i];
01727 daeString str = localRef->getSemantic();
01728 if ( strcmp(str,"POSITION") == 0 ) {
01729 const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
01730 if( !node ) {
01731 continue;
01732 }
01733 double fUnitScale = _GetUnitScale(node);
01734 const domFloat_arrayRef flArray = node->getFloat_array();
01735 if (!!flArray) {
01736 const domList_of_floats& listFloats = flArray->getValue();
01737 int k=vertexoffset;
01738 int vertexStride = 3;
01739 size_t usedindices = indexArray.getCount()-2;
01740 if( geom.indices.capacity() < geom.indices.size()+usedindices ) {
01741 geom.indices.reserve(geom.indices.size()+usedindices);
01742 }
01743 if( geom.vertices.capacity() < geom.vertices.size()+indexArray.getCount() ) {
01744 geom.vertices.reserve(geom.vertices.size()+indexArray.getCount());
01745 }
01746
01747 size_t startoffset = (int)geom.vertices.size();
01748 while(k < (int)indexArray.getCount() ) {
01749 int index0 = indexArray.get(k)*vertexStride;
01750 domFloat fl0 = listFloats.get(index0);
01751 domFloat fl1 = listFloats.get(index0+1);
01752 domFloat fl2 = listFloats.get(index0+2);
01753 k+=triangleIndexStride;
01754 geom.vertices.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale));
01755 }
01756
01757 bool bFlip = false;
01758 for(size_t ivert = startoffset+2; ivert < geom.vertices.size(); ++ivert) {
01759 geom.indices.push_back(ivert-2);
01760 geom.indices.push_back(bFlip ? ivert : ivert-1);
01761 geom.indices.push_back(bFlip ? ivert-1 : ivert);
01762 bFlip = !bFlip;
01763 }
01764 }
01765 else {
01766 ROS_WARN_STREAM("float array not defined!\n");
01767 }
01768 break;
01769 }
01770 }
01771 }
01772
01773 geom.InitCollisionMesh();
01774 return false;
01775 }
01776
01781 bool _ExtractGeometry(const domPolylistRef triRef, const domVerticesRef vertsRef, const std::map<std::string,domMaterialRef>& mapmaterials, std::list<GEOMPROPERTIES>& listGeomProperties)
01782 {
01783 if( !triRef ) {
01784 return false;
01785 }
01786 listGeomProperties.push_back(GEOMPROPERTIES());
01787 GEOMPROPERTIES& geom = listGeomProperties.back();
01788 geom.type = GeomTrimesh;
01789
01790
01791 if( !!triRef->getMaterial() ) {
01792 std::map<std::string,domMaterialRef>::const_iterator itmat = mapmaterials.find(triRef->getMaterial());
01793 if( itmat != mapmaterials.end() ) {
01794 _FillGeometryColor(itmat->second,geom);
01795 }
01796 }
01797
01798 size_t triangleIndexStride = 0,vertexoffset = -1;
01799 domInput_local_offsetRef indexOffsetRef;
01800 for (unsigned int w=0; w<triRef->getInput_array().getCount(); w++) {
01801 size_t offset = triRef->getInput_array()[w]->getOffset();
01802 daeString str = triRef->getInput_array()[w]->getSemantic();
01803 if (!strcmp(str,"VERTEX")) {
01804 indexOffsetRef = triRef->getInput_array()[w];
01805 vertexoffset = offset;
01806 }
01807 if (offset> triangleIndexStride) {
01808 triangleIndexStride = offset;
01809 }
01810 }
01811 triangleIndexStride++;
01812 const domList_of_uints& indexArray =triRef->getP()->getValue();
01813 for (size_t i=0; i<vertsRef->getInput_array().getCount(); ++i) {
01814 domInput_localRef localRef = vertsRef->getInput_array()[i];
01815 daeString str = localRef->getSemantic();
01816 if ( strcmp(str,"POSITION") == 0 ) {
01817 const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
01818 if( !node ) {
01819 continue;
01820 }
01821 double fUnitScale = _GetUnitScale(node);
01822 const domFloat_arrayRef flArray = node->getFloat_array();
01823 if (!!flArray) {
01824 const domList_of_floats& listFloats = flArray->getValue();
01825 size_t k=vertexoffset;
01826 int vertexStride = 3;
01827 for(size_t ipoly = 0; ipoly < triRef->getVcount()->getValue().getCount(); ++ipoly) {
01828 size_t numverts = triRef->getVcount()->getValue()[ipoly];
01829 if(numverts > 0 && k+(numverts-1)*triangleIndexStride < indexArray.getCount() ) {
01830 size_t startoffset = geom.vertices.size();
01831 for (size_t j=0; j<numverts; j++) {
01832 int index0 = indexArray.get(k)*vertexStride;
01833 domFloat fl0 = listFloats.get(index0);
01834 domFloat fl1 = listFloats.get(index0+1);
01835 domFloat fl2 = listFloats.get(index0+2);
01836 k+=triangleIndexStride;
01837 geom.vertices.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale));
01838 }
01839 for(size_t ivert = startoffset+2; ivert < geom.vertices.size(); ++ivert) {
01840 geom.indices.push_back(startoffset);
01841 geom.indices.push_back(ivert-1);
01842 geom.indices.push_back(ivert);
01843 }
01844 }
01845 }
01846 }
01847 else {
01848 ROS_WARN_STREAM("float array not defined!\n");
01849 }
01850 break;
01851 }
01852 }
01853 geom.InitCollisionMesh();
01854 return false;
01855 }
01856
01860 bool _ExtractGeometry(const domGeometryRef geom, const std::map<std::string,domMaterialRef>& mapmaterials, std::list<GEOMPROPERTIES>& listGeomProperties)
01861 {
01862 if( !geom ) {
01863 return false;
01864 }
01865 std::vector<Vector3> vconvexhull;
01866 if (geom->getMesh()) {
01867 const domMeshRef meshRef = geom->getMesh();
01868 for (size_t tg = 0; tg<meshRef->getTriangles_array().getCount(); tg++) {
01869 _ExtractGeometry(meshRef->getTriangles_array()[tg], meshRef->getVertices(), mapmaterials, listGeomProperties);
01870 }
01871 for (size_t tg = 0; tg<meshRef->getTrifans_array().getCount(); tg++) {
01872 _ExtractGeometry(meshRef->getTrifans_array()[tg], meshRef->getVertices(), mapmaterials, listGeomProperties);
01873 }
01874 for (size_t tg = 0; tg<meshRef->getTristrips_array().getCount(); tg++) {
01875 _ExtractGeometry(meshRef->getTristrips_array()[tg], meshRef->getVertices(), mapmaterials, listGeomProperties);
01876 }
01877 for (size_t tg = 0; tg<meshRef->getPolylist_array().getCount(); tg++) {
01878 _ExtractGeometry(meshRef->getPolylist_array()[tg], meshRef->getVertices(), mapmaterials, listGeomProperties);
01879 }
01880 if( meshRef->getPolygons_array().getCount()> 0 ) {
01881 ROS_WARN_STREAM("openrave does not support collada polygons\n");
01882 }
01883
01884
01885
01886
01887
01888
01889
01890
01891
01892
01893
01894
01895
01896
01897
01898
01899
01900
01901
01902
01903
01904
01905
01906
01907
01908
01909
01910
01911
01912
01913 return true;
01914 }
01915 else if (geom->getConvex_mesh()) {
01916 {
01917 const domConvex_meshRef convexRef = geom->getConvex_mesh();
01918 daeElementRef otherElemRef = convexRef->getConvex_hull_of().getElement();
01919 if ( !!otherElemRef ) {
01920 domGeometryRef linkedGeom = *(domGeometryRef*)&otherElemRef;
01921 ROS_WARN_STREAM( "otherLinked\n");
01922 }
01923 else {
01924 ROS_WARN("convexMesh polyCount = %d\n",(int)convexRef->getPolygons_array().getCount());
01925 ROS_WARN("convexMesh triCount = %d\n",(int)convexRef->getTriangles_array().getCount());
01926 }
01927 }
01928
01929 const domConvex_meshRef convexRef = geom->getConvex_mesh();
01930
01931 daeString urlref2 = convexRef->getConvex_hull_of().getOriginalURI();
01932 if (urlref2) {
01933 daeElementRef otherElemRef = convexRef->getConvex_hull_of().getElement();
01934
01935
01936 for ( size_t i = 0; i < _dom->getLibrary_geometries_array().getCount(); i++) {
01937 domLibrary_geometriesRef libgeom = _dom->getLibrary_geometries_array()[i];
01938 for (size_t i = 0; i < libgeom->getGeometry_array().getCount(); i++) {
01939 domGeometryRef lib = libgeom->getGeometry_array()[i];
01940 if (!strcmp(lib->getId(),urlref2+1)) {
01941
01942 domMesh *meshElement = lib->getMesh();
01943 if (meshElement) {
01944 const domVerticesRef vertsRef = meshElement->getVertices();
01945 for (size_t i=0; i<vertsRef->getInput_array().getCount(); i++) {
01946 domInput_localRef localRef = vertsRef->getInput_array()[i];
01947 daeString str = localRef->getSemantic();
01948 if ( strcmp(str,"POSITION") == 0) {
01949 const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
01950 if( !node ) {
01951 continue;
01952 }
01953 double fUnitScale = _GetUnitScale(node);
01954 const domFloat_arrayRef flArray = node->getFloat_array();
01955 if (!!flArray) {
01956 vconvexhull.reserve(vconvexhull.size()+flArray->getCount());
01957 const domList_of_floats& listFloats = flArray->getValue();
01958 for (size_t k=0; k+2<flArray->getCount(); k+=3) {
01959 domFloat fl0 = listFloats.get(k);
01960 domFloat fl1 = listFloats.get(k+1);
01961 domFloat fl2 = listFloats.get(k+2);
01962 vconvexhull.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale));
01963 }
01964 }
01965 }
01966 }
01967 }
01968 }
01969 }
01970 }
01971 }
01972 else {
01973
01974 const domVerticesRef vertsRef = convexRef->getVertices();
01975 for (size_t i=0; i<vertsRef->getInput_array().getCount(); i++) {
01976 domInput_localRef localRef = vertsRef->getInput_array()[i];
01977 daeString str = localRef->getSemantic();
01978 if ( strcmp(str,"POSITION") == 0 ) {
01979 const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
01980 if( !node ) {
01981 continue;
01982 }
01983 double fUnitScale = _GetUnitScale(node);
01984 const domFloat_arrayRef flArray = node->getFloat_array();
01985 if (!!flArray) {
01986 const domList_of_floats& listFloats = flArray->getValue();
01987 vconvexhull.reserve(vconvexhull.size()+flArray->getCount());
01988 for (size_t k=0; k+2<flArray->getCount(); k+=3) {
01989 domFloat fl0 = listFloats.get(k);
01990 domFloat fl1 = listFloats.get(k+1);
01991 domFloat fl2 = listFloats.get(k+2);
01992 vconvexhull.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale));
01993 }
01994 }
01995 }
01996 }
01997 }
01998
01999 if( vconvexhull.size()> 0 ) {
02000 listGeomProperties.push_back(GEOMPROPERTIES());
02001 GEOMPROPERTIES& geom = listGeomProperties.back();
02002 geom.type = GeomTrimesh;
02003
02004
02005 geom.InitCollisionMesh();
02006 }
02007 return true;
02008 }
02009
02010 return false;
02011 }
02012
02014 void _ExtractRobotAttachedActuators(const domArticulated_systemRef as)
02015 {
02016
02017 for(size_t ie = 0; ie < as->getExtra_array().getCount(); ++ie) {
02018 domExtraRef pextra = as->getExtra_array()[ie];
02019 if( strcmp(pextra->getType(), "attach_actuator") == 0 ) {
02020
02021 domTechniqueRef tec = _ExtractOpenRAVEProfile(pextra->getTechnique_array());
02022 if( !!tec ) {
02023 boost::shared_ptr<Joint> pjoint;
02024 daeElementRef domactuator;
02025 {
02026 daeElementRef bact = tec->getChild("bind_actuator");
02027 pjoint = _getJointFromRef(bact->getAttribute("joint").c_str(), as);
02028 if (!pjoint) continue;
02029 }
02030 {
02031 daeElementRef iact = tec->getChild("instance_actuator");
02032 if(!iact) continue;
02033 std::string instance_url = iact->getAttribute("url");
02034 domactuator = daeURI(*iact, instance_url).getElement();
02035 if(!domactuator) continue;
02036 }
02037 daeElement *nom_torque = domactuator->getChild("nominal_torque");
02038 if ( !! nom_torque ) {
02039 if( !! pjoint->limits ) {
02040 pjoint->limits->effort = boost::lexical_cast<double>(nom_torque->getCharData());
02041 ROS_DEBUG("effort limit at joint (%s) is over written by %f",
02042 pjoint->name.c_str(), pjoint->limits->effort);
02043 }
02044 }
02045 }
02046 }
02047 }
02048 }
02049
02051 void _ExtractRobotManipulators(const domArticulated_systemRef as)
02052 {
02053 ROS_DEBUG("collada manipulators not supported yet");
02054 }
02055
02057 void _ExtractRobotAttachedSensors(const domArticulated_systemRef as)
02058 {
02059 ROS_DEBUG("collada sensors not supported yet");
02060 }
02061
02062 inline daeElementRef _getElementFromUrl(const daeURI &uri)
02063 {
02064 return daeStandardURIResolver(*_collada).resolveElement(uri);
02065 }
02066
02067 static daeElement* searchBinding(domCommon_sidref_or_paramRef paddr, daeElementRef parent)
02068 {
02069 if( !!paddr->getSIDREF() ) {
02070 return daeSidRef(paddr->getSIDREF()->getValue(),parent).resolve().elt;
02071 }
02072 if (!!paddr->getParam()) {
02073 return searchBinding(paddr->getParam()->getValue(),parent);
02074 }
02075 return NULL;
02076 }
02077
02081 static daeElement* searchBinding(daeString ref, daeElementRef parent)
02082 {
02083 if( !parent ) {
02084 return NULL;
02085 }
02086 daeElement* pelt = NULL;
02087 domKinematics_sceneRef kscene = daeSafeCast<domKinematics_scene>(parent.cast());
02088 if( !!kscene ) {
02089 pelt = searchBindingArray(ref,kscene->getInstance_articulated_system_array());
02090 if( !!pelt ) {
02091 return pelt;
02092 }
02093 return searchBindingArray(ref,kscene->getInstance_kinematics_model_array());
02094 }
02095 domArticulated_systemRef articulated_system = daeSafeCast<domArticulated_system>(parent.cast());
02096 if( !!articulated_system ) {
02097 if( !!articulated_system->getKinematics() ) {
02098 pelt = searchBindingArray(ref,articulated_system->getKinematics()->getInstance_kinematics_model_array());
02099 if( !!pelt ) {
02100 return pelt;
02101 }
02102 }
02103 if( !!articulated_system->getMotion() ) {
02104 return searchBinding(ref,articulated_system->getMotion()->getInstance_articulated_system());
02105 }
02106 return NULL;
02107 }
02108
02109 daeElementRef pbindelt;
02110 const domKinematics_bind_Array* pbindarray = NULL;
02111 const domKinematics_newparam_Array* pnewparamarray = NULL;
02112 domInstance_articulated_systemRef ias = daeSafeCast<domInstance_articulated_system>(parent.cast());
02113 if( !!ias ) {
02114 pbindarray = &ias->getBind_array();
02115 pbindelt = ias->getUrl().getElement();
02116 pnewparamarray = &ias->getNewparam_array();
02117 }
02118 if( !pbindarray || !pbindelt ) {
02119 domInstance_kinematics_modelRef ikm = daeSafeCast<domInstance_kinematics_model>(parent.cast());
02120 if( !!ikm ) {
02121 pbindarray = &ikm->getBind_array();
02122 pbindelt = ikm->getUrl().getElement();
02123 pnewparamarray = &ikm->getNewparam_array();
02124 }
02125 }
02126 if( !!pbindarray && !!pbindelt ) {
02127 for (size_t ibind = 0; ibind < pbindarray->getCount(); ++ibind) {
02128 domKinematics_bindRef pbind = (*pbindarray)[ibind];
02129 if( !!pbind->getSymbol() && strcmp(pbind->getSymbol(), ref) == 0 ) {
02130
02131 if( !!pbind->getParam() ) {
02132 return daeSidRef(pbind->getParam()->getRef(), pbindelt).resolve().elt;
02133 }
02134 else if( !!pbind->getSIDREF() ) {
02135 return daeSidRef(pbind->getSIDREF()->getValue(), pbindelt).resolve().elt;
02136 }
02137 }
02138 }
02139 for(size_t inewparam = 0; inewparam < pnewparamarray->getCount(); ++inewparam) {
02140 domKinematics_newparamRef newparam = (*pnewparamarray)[inewparam];
02141 if( !!newparam->getSid() && strcmp(newparam->getSid(), ref) == 0 ) {
02142 if( !!newparam->getSIDREF() ) {
02143 return daeSidRef(newparam->getSIDREF()->getValue(),pbindelt).resolve().elt;
02144 }
02145 ROS_WARN_STREAM(str(boost::format("newparam sid=%s does not have SIDREF\n")%newparam->getSid()));
02146 }
02147 }
02148 }
02149 ROS_WARN_STREAM(str(boost::format("failed to get binding '%s' for element: %s\n")%ref%parent->getElementName()));
02150 return NULL;
02151 }
02152
02153 static daeElement* searchBindingArray(daeString ref, const domInstance_articulated_system_Array& paramArray)
02154 {
02155 for(size_t iikm = 0; iikm < paramArray.getCount(); ++iikm) {
02156 daeElement* pelt = searchBinding(ref,paramArray[iikm].cast());
02157 if( !!pelt ) {
02158 return pelt;
02159 }
02160 }
02161 return NULL;
02162 }
02163
02164 static daeElement* searchBindingArray(daeString ref, const domInstance_kinematics_model_Array& paramArray)
02165 {
02166 for(size_t iikm = 0; iikm < paramArray.getCount(); ++iikm) {
02167 daeElement* pelt = searchBinding(ref,paramArray[iikm].cast());
02168 if( !!pelt ) {
02169 return pelt;
02170 }
02171 }
02172 return NULL;
02173 }
02174
02175 template <typename U> static xsBoolean resolveBool(domCommon_bool_or_paramRef paddr, const U& parent) {
02176 if( !!paddr->getBool() ) {
02177 return paddr->getBool()->getValue();
02178 }
02179 if( !paddr->getParam() ) {
02180 ROS_WARN_STREAM("param not specified, setting to 0\n");
02181 return false;
02182 }
02183 for(size_t iparam = 0; iparam < parent->getNewparam_array().getCount(); ++iparam) {
02184 domKinematics_newparamRef pnewparam = parent->getNewparam_array()[iparam];
02185 if( !!pnewparam->getSid() && strcmp(pnewparam->getSid(), paddr->getParam()->getValue()) == 0 ) {
02186 if( !!pnewparam->getBool() ) {
02187 return pnewparam->getBool()->getValue();
02188 }
02189 else if( !!pnewparam->getSIDREF() ) {
02190 domKinematics_newparam::domBoolRef ptarget = daeSafeCast<domKinematics_newparam::domBool>(daeSidRef(pnewparam->getSIDREF()->getValue(), pnewparam).resolve().elt);
02191 if( !ptarget ) {
02192 ROS_WARN("failed to resolve %s from %s\n", pnewparam->getSIDREF()->getValue(), paddr->getID());
02193 continue;
02194 }
02195 return ptarget->getValue();
02196 }
02197 }
02198 }
02199 ROS_WARN_STREAM(str(boost::format("failed to resolve %s\n")%paddr->getParam()->getValue()));
02200 return false;
02201 }
02202 template <typename U> static domFloat resolveFloat(domCommon_float_or_paramRef paddr, const U& parent) {
02203 if( !!paddr->getFloat() ) {
02204 return paddr->getFloat()->getValue();
02205 }
02206 if( !paddr->getParam() ) {
02207 ROS_WARN_STREAM("param not specified, setting to 0\n");
02208 return 0;
02209 }
02210 for(size_t iparam = 0; iparam < parent->getNewparam_array().getCount(); ++iparam) {
02211 domKinematics_newparamRef pnewparam = parent->getNewparam_array()[iparam];
02212 if( !!pnewparam->getSid() && strcmp(pnewparam->getSid(), paddr->getParam()->getValue()) == 0 ) {
02213 if( !!pnewparam->getFloat() ) {
02214 return pnewparam->getFloat()->getValue();
02215 }
02216 else if( !!pnewparam->getSIDREF() ) {
02217 domKinematics_newparam::domFloatRef ptarget = daeSafeCast<domKinematics_newparam::domFloat>(daeSidRef(pnewparam->getSIDREF()->getValue(), pnewparam).resolve().elt);
02218 if( !ptarget ) {
02219 ROS_WARN("failed to resolve %s from %s\n", pnewparam->getSIDREF()->getValue(), paddr->getID());
02220 continue;
02221 }
02222 return ptarget->getValue();
02223 }
02224 }
02225 }
02226 ROS_WARN_STREAM(str(boost::format("failed to resolve %s\n")%paddr->getParam()->getValue()));
02227 return 0;
02228 }
02229
02230 static bool resolveCommon_float_or_param(daeElementRef pcommon, daeElementRef parent, float& f)
02231 {
02232 daeElement* pfloat = pcommon->getChild("float");
02233 if( !!pfloat ) {
02234 std::stringstream sfloat(pfloat->getCharData());
02235 sfloat >> f;
02236 return !!sfloat;
02237 }
02238 daeElement* pparam = pcommon->getChild("param");
02239 if( !!pparam ) {
02240 if( pparam->hasAttribute("ref") ) {
02241 ROS_WARN_STREAM("cannot process param ref\n");
02242 }
02243 else {
02244 daeElement* pelt = daeSidRef(pparam->getCharData(),parent).resolve().elt;
02245 if( !!pelt ) {
02246 ROS_WARN_STREAM(str(boost::format("found param ref: %s from %s\n")%pelt->getCharData()%pparam->getCharData()));
02247 }
02248 }
02249 }
02250 return false;
02251 }
02252
02253 static boost::array<double,12> _matrixIdentity()
02254 {
02255 boost::array<double,12> m = {{1,0,0,0,0,1,0,0,0,0,1,0}};
02256 return m;
02257 };
02258
02260 static boost::array<double,12> _getTransform(daeElementRef pelt)
02261 {
02262 boost::array<double,12> m = _matrixIdentity();
02263 domRotateRef protate = daeSafeCast<domRotate>(pelt);
02264 if( !!protate ) {
02265 m = _matrixFromAxisAngle(Vector3(protate->getValue()[0],protate->getValue()[1],protate->getValue()[2]), (double)(protate->getValue()[3]*(M_PI/180.0)));
02266 return m;
02267 }
02268
02269 domTranslateRef ptrans = daeSafeCast<domTranslate>(pelt);
02270 if( !!ptrans ) {
02271 double scale = _GetUnitScale(pelt);
02272 m[3] = ptrans->getValue()[0]*scale;
02273 m[7] = ptrans->getValue()[1]*scale;
02274 m[11] = ptrans->getValue()[2]*scale;
02275 return m;
02276 }
02277
02278 domMatrixRef pmat = daeSafeCast<domMatrix>(pelt);
02279 if( !!pmat ) {
02280 double scale = _GetUnitScale(pelt);
02281 for(int i = 0; i < 3; ++i) {
02282 m[4*i+0] = pmat->getValue()[4*i+0];
02283 m[4*i+1] = pmat->getValue()[4*i+1];
02284 m[4*i+2] = pmat->getValue()[4*i+2];
02285 m[4*i+3] = pmat->getValue()[4*i+3]*scale;
02286 }
02287 return m;
02288 }
02289
02290 domScaleRef pscale = daeSafeCast<domScale>(pelt);
02291 if( !!pscale ) {
02292 m[0] = pscale->getValue()[0];
02293 m[4*1+1] = pscale->getValue()[1];
02294 m[4*2+2] = pscale->getValue()[2];
02295 return m;
02296 }
02297
02298 domLookatRef pcamera = daeSafeCast<domLookat>(pelt);
02299 if( pelt->typeID() == domLookat::ID() ) {
02300 ROS_ERROR_STREAM("look at transform not implemented\n");
02301 return m;
02302 }
02303
02304 domSkewRef pskew = daeSafeCast<domSkew>(pelt);
02305 if( !!pskew ) {
02306 ROS_ERROR_STREAM("skew transform not implemented\n");
02307 }
02308
02309 return m;
02310 }
02311
02314 template <typename T> static boost::array<double,12> _getNodeParentTransform(const T pelt) {
02315 domNodeRef pnode = daeSafeCast<domNode>(pelt->getParent());
02316 if( !pnode ) {
02317 return _matrixIdentity();
02318 }
02319 return _poseMult(_getNodeParentTransform(pnode), _ExtractFullTransform(pnode));
02320 }
02321
02323 template <typename T> static boost::array<double,12> _ExtractFullTransform(const T pelt) {
02324 boost::array<double,12> t = _matrixIdentity();
02325 for(size_t i = 0; i < pelt->getContents().getCount(); ++i) {
02326 t = _poseMult(t, _getTransform(pelt->getContents()[i]));
02327 }
02328 return t;
02329 }
02330
02332 template <typename T> static boost::array<double,12> _ExtractFullTransformFromChildren(const T pelt) {
02333 boost::array<double,12> t = _matrixIdentity();
02334 daeTArray<daeElementRef> children; pelt->getChildren(children);
02335 for(size_t i = 0; i < children.getCount(); ++i) {
02336 t = _poseMult(t, _getTransform(children[i]));
02337 }
02338 return t;
02339 }
02340
02341
02342 void _decompose(const boost::array<double,12>& tm, Pose& tout, Vector3& vscale)
02343 {
02344 vscale.x = 1; vscale.y = 1; vscale.z = 1;
02345 tout = _poseFromMatrix(tm);
02346 }
02347
02348 virtual void handleError( daeString msg )
02349 {
02350 ROS_ERROR("COLLADA error: %s\n", msg);
02351 }
02352
02353 virtual void handleWarning( daeString msg )
02354 {
02355 ROS_WARN("COLLADA warning: %s\n", msg);
02356 }
02357
02358 inline static double _GetUnitScale(daeElement* pelt)
02359 {
02360 return ((USERDATA*)pelt->getUserData())->scale;
02361 }
02362
02363 domTechniqueRef _ExtractOpenRAVEProfile(const domTechnique_Array& arr)
02364 {
02365 for(size_t i = 0; i < arr.getCount(); ++i) {
02366 if( strcmp(arr[i]->getProfile(), "OpenRAVE") == 0 ) {
02367 return arr[i];
02368 }
02369 }
02370 return domTechniqueRef();
02371 }
02372
02374 boost::shared_ptr<std::string> _ExtractInterfaceType(const domExtra_Array& arr) {
02375 for(size_t i = 0; i < arr.getCount(); ++i) {
02376 if( strcmp(arr[i]->getType(),"interface_type") == 0 ) {
02377 domTechniqueRef tec = _ExtractOpenRAVEProfile(arr[i]->getTechnique_array());
02378 if( !!tec ) {
02379 daeElement* ptype = tec->getChild("interface");
02380 if( !!ptype ) {
02381 return boost::shared_ptr<std::string>(new std::string(ptype->getCharData()));
02382 }
02383 }
02384 }
02385 }
02386 return boost::shared_ptr<std::string>();
02387 }
02388
02389 std::string _ExtractLinkName(domLinkRef pdomlink) {
02390 std::string linkname;
02391 if( !!pdomlink ) {
02392 if( !!pdomlink->getName() ) {
02393 linkname = pdomlink->getName();
02394 }
02395 if( linkname.size() == 0 && !!pdomlink->getID() ) {
02396 linkname = pdomlink->getID();
02397 }
02398 }
02399 return linkname;
02400 }
02401
02402 bool _checkMathML(daeElementRef pelt,const std::string& type)
02403 {
02404 if( pelt->getElementName()==type ) {
02405 return true;
02406 }
02407
02408 std::string name = pelt->getElementName();
02409 std::size_t pos = name.find_last_of(':');
02410 if( pos == std::string::npos ) {
02411 return false;
02412 }
02413 return name.substr(pos+1)==type;
02414 }
02415
02416 boost::shared_ptr<Joint> _getJointFromRef(xsToken targetref, daeElementRef peltref) {
02417 daeElement* peltjoint = daeSidRef(targetref, peltref).resolve().elt;
02418 domJointRef pdomjoint = daeSafeCast<domJoint> (peltjoint);
02419
02420 if (!pdomjoint) {
02421 domInstance_jointRef pdomijoint = daeSafeCast<domInstance_joint> (peltjoint);
02422 if (!!pdomijoint) {
02423 pdomjoint = daeSafeCast<domJoint> (pdomijoint->getUrl().getElement().cast());
02424 }
02425 }
02426
02427 if (!pdomjoint || pdomjoint->typeID() != domJoint::ID() || !pdomjoint->getName()) {
02428 ROS_WARN_STREAM(str(boost::format("could not find collada joint %s!\n")%targetref));
02429 return boost::shared_ptr<Joint>();
02430 }
02431
02432 boost::shared_ptr<Joint> pjoint;
02433 std::string name(pdomjoint->getName());
02434 if (_model->joints_.find(name) == _model->joints_.end()) {
02435 pjoint.reset();
02436 } else {
02437 pjoint = _model->joints_.find(name)->second;
02438 }
02439 if(!pjoint) {
02440 ROS_WARN_STREAM(str(boost::format("could not find openrave joint %s!\n")%pdomjoint->getName()));
02441 }
02442 return pjoint;
02443 }
02444
02448 static void _ExtractKinematicsVisualBindings(domInstance_with_extraRef viscene, domInstance_kinematics_sceneRef kiscene, KinematicsSceneBindings& bindings)
02449 {
02450 domKinematics_sceneRef kscene = daeSafeCast<domKinematics_scene> (kiscene->getUrl().getElement().cast());
02451 if (!kscene) {
02452 return;
02453 }
02454 for (size_t imodel = 0; imodel < kiscene->getBind_kinematics_model_array().getCount(); imodel++) {
02455 domArticulated_systemRef articulated_system;
02456 domBind_kinematics_modelRef kbindmodel = kiscene->getBind_kinematics_model_array()[imodel];
02457 if (!kbindmodel->getNode()) {
02458 ROS_WARN_STREAM("do not support kinematics models without references to nodes\n");
02459 continue;
02460 }
02461
02462
02463 domNodeRef node = daeSafeCast<domNode>(daeSidRef(kbindmodel->getNode(), viscene->getUrl().getElement()).resolve().elt);
02464 if (!node) {
02465 ROS_WARN_STREAM(str(boost::format("bind_kinematics_model does not reference valid node %s\n")%kbindmodel->getNode()));
02466 continue;
02467 }
02468
02469
02470 daeElement* pelt = searchBinding(kbindmodel,kscene);
02471 domInstance_kinematics_modelRef kimodel = daeSafeCast<domInstance_kinematics_model>(pelt);
02472 if (!kimodel) {
02473 if( !pelt ) {
02474 ROS_WARN_STREAM("bind_kinematics_model does not reference element\n");
02475 }
02476 else {
02477 ROS_WARN_STREAM(str(boost::format("bind_kinematics_model cannot find reference to %s:\n")%pelt->getElementName()));
02478 }
02479 continue;
02480 }
02481 bindings.listKinematicsVisualBindings.push_back(std::make_pair(node,kimodel));
02482 }
02483
02484 for (size_t ijoint = 0; ijoint < kiscene->getBind_joint_axis_array().getCount(); ++ijoint) {
02485 domBind_joint_axisRef bindjoint = kiscene->getBind_joint_axis_array()[ijoint];
02486 daeElementRef pjtarget = daeSidRef(bindjoint->getTarget(), viscene->getUrl().getElement()).resolve().elt;
02487 if (!pjtarget) {
02488 ROS_ERROR_STREAM(str(boost::format("Target Node %s NOT found!!!\n")%bindjoint->getTarget()));
02489 continue;
02490 }
02491 daeElement* pelt = searchBinding(bindjoint->getAxis(),kscene);
02492 domAxis_constraintRef pjointaxis = daeSafeCast<domAxis_constraint>(pelt);
02493 if (!pjointaxis) {
02494 continue;
02495 }
02496 bindings.listAxisBindings.push_back(JointAxisBinding(pjtarget, pjointaxis, bindjoint->getValue(), NULL, NULL));
02497 }
02498 }
02499
02500 static void _ExtractPhysicsBindings(domCOLLADA::domSceneRef allscene, KinematicsSceneBindings& bindings)
02501 {
02502 for(size_t iphysics = 0; iphysics < allscene->getInstance_physics_scene_array().getCount(); ++iphysics) {
02503 domPhysics_sceneRef pscene = daeSafeCast<domPhysics_scene>(allscene->getInstance_physics_scene_array()[iphysics]->getUrl().getElement().cast());
02504 for(size_t imodel = 0; imodel < pscene->getInstance_physics_model_array().getCount(); ++imodel) {
02505 domInstance_physics_modelRef ipmodel = pscene->getInstance_physics_model_array()[imodel];
02506 domPhysics_modelRef pmodel = daeSafeCast<domPhysics_model> (ipmodel->getUrl().getElement().cast());
02507 domNodeRef nodephysicsoffset = daeSafeCast<domNode>(ipmodel->getParent().getElement().cast());
02508 for(size_t ibody = 0; ibody < ipmodel->getInstance_rigid_body_array().getCount(); ++ibody) {
02509 LinkBinding lb;
02510 lb.irigidbody = ipmodel->getInstance_rigid_body_array()[ibody];
02511 lb.node = daeSafeCast<domNode>(lb.irigidbody->getTarget().getElement().cast());
02512 lb.rigidbody = daeSafeCast<domRigid_body>(daeSidRef(lb.irigidbody->getBody(),pmodel).resolve().elt);
02513 lb.nodephysicsoffset = nodephysicsoffset;
02514 if( !!lb.rigidbody && !!lb.node ) {
02515 bindings.listLinkBindings.push_back(lb);
02516 }
02517 }
02518 }
02519 }
02520 }
02521
02522
02523 size_t _countChildren(daeElement* pelt) {
02524 size_t c = 1;
02525 daeTArray<daeElementRef> children;
02526 pelt->getChildren(children);
02527 for (size_t i = 0; i < children.getCount(); ++i) {
02528 c += _countChildren(children[i]);
02529 }
02530 return c;
02531 }
02532
02533 void _processUserData(daeElement* pelt, double scale)
02534 {
02535
02536 domAssetRef passet = daeSafeCast<domAsset> (pelt->getChild("asset"));
02537 if (!!passet && !!passet->getUnit()) {
02538 scale = passet->getUnit()->getMeter();
02539 }
02540
02541 _vuserdata.push_back(USERDATA(scale));
02542 pelt->setUserData(&_vuserdata.back());
02543 daeTArray<daeElementRef> children;
02544 pelt->getChildren(children);
02545 for (size_t i = 0; i < children.getCount(); ++i) {
02546 if (children[i] != passet) {
02547 _processUserData(children[i], scale);
02548 }
02549 }
02550 }
02551
02552 USERDATA* _getUserData(daeElement* pelt)
02553 {
02554 BOOST_ASSERT(!!pelt);
02555 void* p = pelt->getUserData();
02556 BOOST_ASSERT(!!p);
02557 return (USERDATA*)p;
02558 }
02559
02560
02561
02562
02563
02564 static Vector3 _poseMult(const Pose& p, const Vector3& v)
02565 {
02566 double ww = 2 * p.rotation.x * p.rotation.x;
02567 double wx = 2 * p.rotation.x * p.rotation.y;
02568 double wy = 2 * p.rotation.x * p.rotation.z;
02569 double wz = 2 * p.rotation.x * p.rotation.w;
02570 double xx = 2 * p.rotation.y * p.rotation.y;
02571 double xy = 2 * p.rotation.y * p.rotation.z;
02572 double xz = 2 * p.rotation.y * p.rotation.w;
02573 double yy = 2 * p.rotation.z * p.rotation.z;
02574 double yz = 2 * p.rotation.z * p.rotation.w;
02575 Vector3 vnew;
02576 vnew.x = (1-xx-yy) * v.x + (wx-yz) * v.y + (wy+xz)*v.z + p.position.x;
02577 vnew.y = (wx+yz) * v.x + (1-ww-yy) * v.y + (xy-wz)*v.z + p.position.y;
02578 vnew.z = (wy-xz) * v.x + (xy+wz) * v.y + (1-ww-xx)*v.z + p.position.z;
02579 return vnew;
02580 }
02581
02582 static Vector3 _poseMult(const boost::array<double,12>& m, const Vector3& v)
02583 {
02584 Vector3 vnew;
02585 vnew.x = m[4*0+0] * v.x + m[4*0+1] * v.y + m[4*0+2] * v.z + m[4*0+3];
02586 vnew.y = m[4*1+0] * v.x + m[4*1+1] * v.y + m[4*1+2] * v.z + m[4*1+3];
02587 vnew.z = m[4*2+0] * v.x + m[4*2+1] * v.y + m[4*2+2] * v.z + m[4*2+3];
02588 return vnew;
02589 }
02590
02591 static boost::array<double,12> _poseMult(const boost::array<double,12>& m0, const boost::array<double,12>& m1)
02592 {
02593 boost::array<double,12> mres;
02594 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];
02595 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];
02596 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];
02597 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];
02598 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];
02599 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];
02600 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];
02601 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];
02602 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];
02603 mres[3] = m1[3] * m0[0] + m1[7] * m0[1] + m1[11] * m0[2] + m0[3];
02604 mres[7] = m1[3] * m0[4] + m1[7] * m0[5] + m1[11] * m0[6] + m0[7];
02605 mres[11] = m1[3] * m0[8] + m1[7] * m0[9] + m1[11] * m0[10] + m0[11];
02606 return mres;
02607 }
02608
02609 static Pose _poseMult(const Pose& p0, const Pose& p1)
02610 {
02611 Pose p;
02612 p.position = _poseMult(p0,p1.position);
02613 p.rotation = _quatMult(p0.rotation,p1.rotation);
02614 return p;
02615 }
02616
02617 static Pose _poseInverse(const Pose& p)
02618 {
02619 Pose pinv;
02620 pinv.rotation.x = -p.rotation.x;
02621 pinv.rotation.y = -p.rotation.y;
02622 pinv.rotation.z = -p.rotation.z;
02623 pinv.rotation.w = p.rotation.w;
02624 Vector3 t = _poseMult(pinv,p.position);
02625 pinv.position.x = -t.x;
02626 pinv.position.y = -t.y;
02627 pinv.position.z = -t.z;
02628 return pinv;
02629 }
02630
02631 static Rotation _quatMult(const Rotation& quat0, const Rotation& quat1)
02632 {
02633 Rotation q;
02634 q.x = quat0.w*quat1.x + quat0.x*quat1.w + quat0.y*quat1.z - quat0.z*quat1.y;
02635 q.y = quat0.w*quat1.y + quat0.y*quat1.w + quat0.z*quat1.x - quat0.x*quat1.z;
02636 q.z = quat0.w*quat1.z + quat0.z*quat1.w + quat0.x*quat1.y - quat0.y*quat1.x;
02637 q.w = quat0.w*quat1.w - quat0.x*quat1.x - quat0.y*quat1.y - quat0.z*quat1.z;
02638 double fnorm = std::sqrt(q.x*q.x+q.y*q.y+q.z*q.z+q.w*q.w);
02639
02640 q.x /= fnorm;
02641 q.y /= fnorm;
02642 q.z /= fnorm;
02643 q.w /= fnorm;
02644 return q;
02645 }
02646
02647 static boost::array<double,12> _matrixFromAxisAngle(const Vector3& axis, double angle)
02648 {
02649 return _matrixFromQuat(_quatFromAxisAngle(axis.x,axis.y,axis.z,angle));
02650 }
02651
02652 static boost::array<double,12> _matrixFromQuat(const Rotation& quat)
02653 {
02654 boost::array<double,12> m;
02655 double qq1 = 2*quat.x*quat.x;
02656 double qq2 = 2*quat.y*quat.y;
02657 double qq3 = 2*quat.z*quat.z;
02658 m[4*0+0] = 1 - qq2 - qq3;
02659 m[4*0+1] = 2*(quat.x*quat.y - quat.w*quat.z);
02660 m[4*0+2] = 2*(quat.x*quat.z + quat.w*quat.y);
02661 m[4*0+3] = 0;
02662 m[4*1+0] = 2*(quat.x*quat.y + quat.w*quat.z);
02663 m[4*1+1] = 1 - qq1 - qq3;
02664 m[4*1+2] = 2*(quat.y*quat.z - quat.w*quat.x);
02665 m[4*1+3] = 0;
02666 m[4*2+0] = 2*(quat.x*quat.z - quat.w*quat.y);
02667 m[4*2+1] = 2*(quat.y*quat.z + quat.w*quat.x);
02668 m[4*2+2] = 1 - qq1 - qq2;
02669 m[4*2+3] = 0;
02670 return m;
02671 }
02672
02673 static Pose _poseFromMatrix(const boost::array<double,12>& m)
02674 {
02675 Pose t;
02676 t.rotation = _quatFromMatrix(m);
02677 t.position.x = m[3];
02678 t.position.y = m[7];
02679 t.position.z = m[11];
02680 return t;
02681 }
02682
02683 static boost::array<double,12> _matrixFromPose(const Pose& t)
02684 {
02685 boost::array<double,12> m = _matrixFromQuat(t.rotation);
02686 m[3] = t.position.x;
02687 m[7] = t.position.y;
02688 m[11] = t.position.z;
02689 return m;
02690 }
02691
02692 static Rotation _quatFromAxisAngle(double x, double y, double z, double angle)
02693 {
02694 Rotation q;
02695 double axislen = std::sqrt(x*x+y*y+z*z);
02696 if( axislen == 0 ) {
02697 return q;
02698 }
02699 angle *= 0.5;
02700 double sang = std::sin(angle)/axislen;
02701 q.w = std::cos(angle);
02702 q.x = x*sang;
02703 q.y = y*sang;
02704 q.z = z*sang;
02705 return q;
02706 }
02707
02708 static Rotation _quatFromMatrix(const boost::array<double,12>& mat)
02709 {
02710 Rotation rot;
02711 double tr = mat[4*0+0] + mat[4*1+1] + mat[4*2+2];
02712 if (tr >= 0) {
02713 rot.w = tr + 1;
02714 rot.x = (mat[4*2+1] - mat[4*1+2]);
02715 rot.y = (mat[4*0+2] - mat[4*2+0]);
02716 rot.z = (mat[4*1+0] - mat[4*0+1]);
02717 }
02718 else {
02719
02720 if (mat[4*1+1] > mat[4*0+0]) {
02721 if (mat[4*2+2] > mat[4*1+1]) {
02722 rot.z = (mat[4*2+2] - (mat[4*0+0] + mat[4*1+1])) + 1;
02723 rot.x = (mat[4*2+0] + mat[4*0+2]);
02724 rot.y = (mat[4*1+2] + mat[4*2+1]);
02725 rot.w = (mat[4*1+0] - mat[4*0+1]);
02726 }
02727 else {
02728 rot.y = (mat[4*1+1] - (mat[4*2+2] + mat[4*0+0])) + 1;
02729 rot.z = (mat[4*1+2] + mat[4*2+1]);
02730 rot.x = (mat[4*0+1] + mat[4*1+0]);
02731 rot.w = (mat[4*0+2] - mat[4*2+0]);
02732 }
02733 }
02734 else if (mat[4*2+2] > mat[4*0+0]) {
02735 rot.z = (mat[4*2+2] - (mat[4*0+0] + mat[4*1+1])) + 1;
02736 rot.x = (mat[4*2+0] + mat[4*0+2]);
02737 rot.y = (mat[4*1+2] + mat[4*2+1]);
02738 rot.w = (mat[4*1+0] - mat[4*0+1]);
02739 }
02740 else {
02741 rot.x = (mat[4*0+0] - (mat[4*1+1] + mat[4*2+2])) + 1;
02742 rot.y = (mat[4*0+1] + mat[4*1+0]);
02743 rot.z = (mat[4*2+0] + mat[4*0+2]);
02744 rot.w = (mat[4*2+1] - mat[4*1+2]);
02745 }
02746 }
02747 double fnorm = std::sqrt(rot.x*rot.x+rot.y*rot.y+rot.z*rot.z+rot.w*rot.w);
02748
02749 rot.x /= fnorm;
02750 rot.y /= fnorm;
02751 rot.z /= fnorm;
02752 rot.w /= fnorm;
02753 return rot;
02754 }
02755
02756 static double _dot3(const Vector3& v0, const Vector3& v1)
02757 {
02758 return v0.x*v1.x + v0.y*v1.y + v0.z*v1.z;
02759 }
02760 static Vector3 _cross3(const Vector3& v0, const Vector3& v1)
02761 {
02762 Vector3 v;
02763 v.x = v0.y * v1.z - v0.z * v1.y;
02764 v.y = v0.z * v1.x - v0.x * v1.z;
02765 v.z = v0.x * v1.y - v0.y * v1.x;
02766 return v;
02767 }
02768 static Vector3 _sub3(const Vector3& v0, const Vector3& v1)
02769 {
02770 Vector3 v;
02771 v.x = v0.x-v1.x;
02772 v.y = v0.y-v1.y;
02773 v.z = v0.z-v1.z;
02774 return v;
02775 }
02776 static Vector3 _add3(const Vector3& v0, const Vector3& v1)
02777 {
02778 Vector3 v;
02779 v.x = v0.x+v1.x;
02780 v.y = v0.y+v1.y;
02781 v.z = v0.z+v1.z;
02782 return v;
02783 }
02784 static Vector3 _normalize3(const Vector3& v0)
02785 {
02786 Vector3 v;
02787 double norm = std::sqrt(v0.x*v0.x+v0.y*v0.y+v0.z*v0.z);
02788 v.x = v0.x/norm;
02789 v.y = v0.y/norm;
02790 v.z = v0.z/norm;
02791 return v;
02792 }
02793
02794 boost::shared_ptr<DAE> _collada;
02795 domCOLLADA* _dom;
02796 std::vector<USERDATA> _vuserdata;
02797 int _nGlobalSensorId, _nGlobalManipulatorId;
02798 std::string _filename;
02799 std::string _resourcedir;
02800 boost::shared_ptr<ModelInterface> _model;
02801 Pose _RootOrigin;
02802 Pose _VisualRootOrigin;
02803 };
02804
02805
02806
02807
02808 boost::shared_ptr<ModelInterface> parseCollada(const std::string &xml_str)
02809 {
02810 boost::shared_ptr<ModelInterface> model(new ModelInterface);
02811
02812 ColladaModelReader reader(model);
02813 if (!reader.InitFromData(xml_str))
02814 model.reset();
02815 return model;
02816 }
02817 }