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