46 #pragma GCC diagnostic push 47 #pragma GCC diagnostic ignored "-Wdeprecated-declarations" 49 #include <dae/daeErrorHandler.h> 50 #pragma GCC diagnostic pop 52 #include <dom/domCOLLADA.h> 53 #include <dae/domAny.h> 54 #include <dom/domConstants.h> 55 #include <dom/domTriangles.h> 56 #include <dae/daeStandardURIResolver.h> 58 #include <boost/array.hpp> 59 #include <boost/assert.hpp> 60 #include <boost/format.hpp> 61 #include <boost/lexical_cast.hpp> 62 #include <boost/shared_ptr.hpp> 66 #include <urdf_model/model.h> 73 #define typeof __typeof__ 74 #define FOREACH(it, v) for(typeof((v).begin())it = (v).begin(); it != (v).end(); (it)++) 75 #define FOREACHC FOREACH 89 unlink(_filename.c_str());
101 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) {
102 BOOST_ASSERT( !!pkinematicaxis );
103 daeElement* pae = pvisualtrans->getParentElement();
105 visualnode = daeSafeCast<domNode> (pae);
109 pae = pae->getParentElement();
113 ROS_WARN_STREAM(str(boost::format(
"couldn't find parent node of element id %s, sid %s\n")%pkinematicaxis->getID()%pkinematicaxis->getSid()));
144 bool AddAxisInfo(
const domInstance_kinematics_model_Array& arr, domKinematics_axis_infoRef kinematics_axis_info, domMotion_axis_infoRef motion_axis_info)
146 if( !kinematics_axis_info ) {
149 for(
size_t ik = 0; ik < arr.getCount(); ++ik) {
150 daeElement* pelt = daeSidRef(kinematics_axis_info->getAxis(), arr[ik]->getUrl().getElement()).
resolve().elt;
154 FOREACH(itbinding,listAxisBindings) {
155 if( itbinding->pkinematicaxis.cast() == pelt ) {
156 itbinding->kinematics_axis_info = kinematics_axis_info;
157 if( !!motion_axis_info ) {
158 itbinding->motion_axis_info = motion_axis_info;
165 ROS_WARN_STREAM(str(boost::format(
"could not find binding for axis: %s, %s\n")%kinematics_axis_info->getAxis()%pelt->getAttribute(
"sid")));
171 ROS_WARN_STREAM(str(boost::format(
"could not find kinematics axis target: %s\n")%kinematics_axis_info->getAxis()));
183 #if URDFDOM_HEADERS_MAJOR_VERSION < 1 186 std::shared_ptr<void> p;
202 Color diffuseColor, ambientColor;
217 const double GTS_M_ICOSAHEDRON_X = 0.850650808352039932181540497063011072240401406;
218 const double GTS_M_ICOSAHEDRON_Y = 0.525731112119133606025669084847876607285497935;
219 const double GTS_M_ICOSAHEDRON_Z = 0;
220 std::vector<Vector3> tempvertices[2];
221 std::vector<int> tempindices[2];
223 tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_Z, +GTS_M_ICOSAHEDRON_X, -GTS_M_ICOSAHEDRON_Y));
224 tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_X, +GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z));
225 tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z, -GTS_M_ICOSAHEDRON_X));
226 tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z, +GTS_M_ICOSAHEDRON_X));
227 tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_X, -GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z));
228 tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_Z, +GTS_M_ICOSAHEDRON_X, +GTS_M_ICOSAHEDRON_Y));
229 tempvertices[0].push_back(Vector3(-GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z, +GTS_M_ICOSAHEDRON_X));
230 tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_Z, -GTS_M_ICOSAHEDRON_X, -GTS_M_ICOSAHEDRON_Y));
231 tempvertices[0].push_back(Vector3(-GTS_M_ICOSAHEDRON_X, +GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z));
232 tempvertices[0].push_back(Vector3(-GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z, -GTS_M_ICOSAHEDRON_X));
233 tempvertices[0].push_back(Vector3(-GTS_M_ICOSAHEDRON_X, -GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z));
234 tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_Z, -GTS_M_ICOSAHEDRON_X, +GTS_M_ICOSAHEDRON_Y));
236 const int nindices=60;
237 int indices[nindices] = {
262 for(
int i = 0; i < nindices; i += 3 ) {
263 v[0] = tempvertices[0][indices[i]];
264 v[1] = tempvertices[0][indices[i+1]];
265 v[2] = tempvertices[0][indices[i+2]];
266 if( _dot3(v[0], _cross3(_sub3(v[1],v[0]),_sub3(v[2],v[0]))) < 0 ) {
267 std::swap(indices[i], indices[i+1]);
271 tempindices[0].resize(nindices);
272 std::copy(&indices[0],&indices[nindices],tempindices[0].begin());
273 std::vector<Vector3>* curvertices = &tempvertices[0], *newvertices = &tempvertices[1];
274 std::vector<int> *curindices = &tempindices[0], *newindices = &tempindices[1];
275 while(levels-- > 0) {
277 newvertices->resize(0);
278 newvertices->reserve(2*curvertices->size());
279 newvertices->insert(newvertices->end(), curvertices->begin(), curvertices->end());
280 newindices->resize(0);
281 newindices->reserve(4*curindices->size());
283 std::map< uint64_t, int > mapnewinds;
284 std::map< uint64_t, int >::iterator it;
286 for(
size_t i = 0; i < curindices->size(); i += 3) {
288 v[0] = curvertices->at(curindices->at(i));
289 v[1] = curvertices->at(curindices->at(i+1));
290 v[2] = curvertices->at(curindices->at(i+2));
293 for(
int j = 0; j < 3; ++j) {
294 uint64_t key = ((uint64_t)curindices->at(i+j)<<32)|(uint64_t)curindices->at(i + ((j+1)%3));
295 it = mapnewinds.find(key);
297 if( it == mapnewinds.end() ) {
298 inds[j] = mapnewinds[key] = mapnewinds[(key<<32)|(key>>32)] = (
int)newvertices->size();
299 newvertices->push_back(_normalize3(_add3(v[j],v[(j+1)%3 ])));
302 inds[j] = it->second;
306 newindices->push_back(curindices->at(i)); newindices->push_back(inds[0]); newindices->push_back(inds[2]);
307 newindices->push_back(inds[0]); newindices->push_back(curindices->at(i+1)); newindices->push_back(inds[1]);
308 newindices->push_back(inds[2]); newindices->push_back(inds[0]); newindices->push_back(inds[1]);
309 newindices->push_back(inds[2]); newindices->push_back(inds[1]); newindices->push_back(curindices->at(i+2));
312 std::swap(newvertices,curvertices);
313 std::swap(newindices,curindices);
316 realvertices = *curvertices;
317 realindices = *curindices;
322 if( type == GeomTrimesh ) {
328 if( fTessellation < 0.01
f ) {
329 fTessellation = 0.01f;
335 GenerateSphereTriangulation(vertices,indices, 3 + (
int)(logf(fTessellation) / logf(2.0
f)) );
336 double fRadius = vGeomData.x;
346 Vector3 ex = vGeomData;
347 Vector3 v[8] = { Vector3(ex.x, ex.y, ex.z),
348 Vector3(ex.x, ex.y, -ex.z),
349 Vector3(ex.x, -ex.y, ex.z),
350 Vector3(ex.x, -ex.y, -ex.z),
351 Vector3(-ex.x, ex.y, ex.z),
352 Vector3(-ex.x, ex.y, -ex.z),
353 Vector3(-ex.x, -ex.y, ex.z),
354 Vector3(-ex.x, -ex.y, -ex.z) };
355 const int nindices = 36;
356 int startindices[] = {
371 for(
int i = 0; i < nindices; i += 3 ) {
372 Vector3 v1 = v[startindices[i]];
373 Vector3 v2 = v[startindices[i+1]];
374 Vector3 v3 = v[startindices[i+2]];
375 if( _dot3(v1, _sub3(v2,_cross3(v1, _sub3(v3,v1)))) < 0 ) {
376 std::swap(indices[i], indices[i+1]);
381 std::copy(&v[0],&v[8],vertices.begin());
382 indices.resize(nindices);
383 std::copy(&startindices[0],&startindices[nindices],indices.begin());
388 double rad = vGeomData.x, len = vGeomData.y*0.5f;
390 int numverts = (int)(fTessellation*24.0
f) + 3;
391 double dtheta = 2 * M_PI / (double)numverts;
392 vertices.push_back(Vector3(0,0,len));
393 vertices.push_back(Vector3(0,0,-len));
394 vertices.push_back(Vector3(rad,0,len));
395 vertices.push_back(Vector3(rad,0,-len));
397 for(
int i = 0; i < numverts+1; ++i) {
398 double s = rad * std::sin(dtheta * (
double)i);
399 double c = rad * std::cos(dtheta * (
double)i);
401 int off = (int)vertices.size();
402 vertices.push_back(Vector3(c, s, len));
403 vertices.push_back(Vector3(c, s, -len));
405 indices.push_back(0); indices.push_back(off); indices.push_back(off-2);
406 indices.push_back(1); indices.push_back(off-1); indices.push_back(off+1);
407 indices.push_back(off-2); indices.push_back(off); indices.push_back(off-1);
408 indices.push_back(off); indices.push_back(off-1); indices.push_back(off+1);
420 ColladaModelReader(urdf::ModelInterfaceSharedPtr model) : _dom(NULL), _nGlobalSensorId(0), _nGlobalManipulatorId(0), _model(model) {
421 daeErrorHandler::setErrorHandler(
this);
431 ROS_DEBUG_STREAM(str(boost::format(
"init COLLADA reader version: %s, namespace: %s, filename: %s\n")%COLLADA_VERSION%COLLADA_NAMESPACE%filename));
432 _collada.reset(
new DAE);
433 _dom = (domCOLLADA*)_collada->open(filename);
439 size_t maxchildren = _countChildren(_dom);
440 _vuserdata.resize(0);
441 _vuserdata.reserve(maxchildren);
444 _processUserData(_dom, dScale);
445 ROS_DEBUG_STREAM(str(boost::format(
"processed children: %d/%d\n")%_vuserdata.size()%maxchildren));
450 ROS_DEBUG_STREAM(str(boost::format(
"init COLLADA reader version: %s, namespace: %s\n")%COLLADA_VERSION%COLLADA_NAMESPACE));
451 _collada.reset(
new DAE);
452 _dom = (domCOLLADA*)_collada->openFromMemory(
".",pdata.c_str());
457 size_t maxchildren = _countChildren(_dom);
458 _vuserdata.resize(0);
459 _vuserdata.reserve(maxchildren);
462 _processUserData(_dom, dScale);
463 ROS_DEBUG_STREAM(str(boost::format(
"processed children: %d/%d\n")%_vuserdata.size()%maxchildren));
473 std::list< std::pair<domInstance_kinematics_modelRef, boost::shared_ptr<KinematicsSceneBindings> > > listPossibleBodies;
474 domCOLLADA::domSceneRef allscene = _dom->getScene();
480 for (
size_t iscene = 0; iscene < allscene->getInstance_kinematics_scene_array().getCount(); iscene++) {
481 domInstance_kinematics_sceneRef kiscene = allscene->getInstance_kinematics_scene_array()[iscene];
482 domKinematics_sceneRef kscene = daeSafeCast<domKinematics_scene> (kiscene->getUrl().getElement().cast());
487 _ExtractKinematicsVisualBindings(allscene->getInstance_visual_scene(),kiscene,*bindings);
488 _ExtractPhysicsBindings(allscene,*bindings);
489 for(
size_t ias = 0; ias < kscene->getInstance_articulated_system_array().getCount(); ++ias) {
490 if( _ExtractArticulatedSystem(kscene->getInstance_articulated_system_array()[ias], *bindings) ) {
495 for(
size_t ikmodel = 0; ikmodel < kscene->getInstance_kinematics_model_array().getCount(); ++ikmodel) {
496 listPossibleBodies.push_back(std::make_pair(kscene->getInstance_kinematics_model_array()[ikmodel], bindings));
500 FOREACH(it, listPossibleBodies) {
501 if( _ExtractKinematicsModel(it->first, *it->second) ) {
512 std::map<std::string, std::string> parent_link_tree;
516 _model->initTree(parent_link_tree);
520 ROS_ERROR(
"Failed to build tree: %s", e.what());
526 _model->initRoot(parent_link_tree);
530 ROS_ERROR(
"Failed to find root link: %s", e.what());
540 ROS_DEBUG_STREAM(str(boost::format(
"instance articulated system sid %s\n")%ias->getSid()));
541 domArticulated_systemRef articulated_system = daeSafeCast<domArticulated_system> (ias->getUrl().getElement().cast());
542 if( !articulated_system ) {
547 if( !pinterface_type ) {
548 pinterface_type = _ExtractInterfaceType(articulated_system->getExtra_array());
550 if( !!pinterface_type ) {
555 if( _model->name_.size() == 0 && !!ias->getName() ) {
556 _model->name_ = ias->getName();
558 if( _model->name_.size() == 0 && !!ias->getSid()) {
559 _model->name_ = ias->getSid();
561 if( _model->name_.size() == 0 && !!articulated_system->getName() ) {
562 _model->name_ = articulated_system->getName();
564 if( _model->name_.size() == 0 && !!articulated_system->getId()) {
565 _model->name_ = articulated_system->getId();
568 if( !!articulated_system->getMotion() ) {
569 domInstance_articulated_systemRef ias_new = articulated_system->getMotion()->getInstance_articulated_system();
570 if( !!articulated_system->getMotion()->getTechnique_common() ) {
571 for(
size_t i = 0; i < articulated_system->getMotion()->getTechnique_common()->getAxis_info_array().getCount(); ++i) {
572 domMotion_axis_infoRef motion_axis_info = articulated_system->getMotion()->getTechnique_common()->getAxis_info_array()[i];
574 domKinematics_axis_infoRef kinematics_axis_info = daeSafeCast<domKinematics_axis_info>(daeSidRef(motion_axis_info->getAxis(), ias_new->getUrl().getElement()).
resolve().elt);
575 if( !!kinematics_axis_info ) {
577 daeElement* pparent = kinematics_axis_info->getParent();
578 while(!!pparent && pparent->typeID() != domKinematics::ID()) {
579 pparent = pparent->getParent();
581 BOOST_ASSERT(!!pparent);
582 bindings.
AddAxisInfo(daeSafeCast<domKinematics>(pparent)->getInstance_kinematics_model_array(), kinematics_axis_info, motion_axis_info);
585 ROS_WARN_STREAM(str(boost::format(
"failed to find kinematics axis %s\n")%motion_axis_info->getAxis()));
589 if( !_ExtractArticulatedSystem(ias_new,bindings) ) {
594 if( !articulated_system->getKinematics() ) {
595 ROS_WARN_STREAM(str(boost::format(
"collada <kinematics> tag empty? instance_articulated_system=%s\n")%ias->getID()));
599 if( !!articulated_system->getKinematics()->getTechnique_common() ) {
600 for(
size_t i = 0; i < articulated_system->getKinematics()->getTechnique_common()->getAxis_info_array().getCount(); ++i) {
601 bindings.
AddAxisInfo(articulated_system->getKinematics()->getInstance_kinematics_model_array(), articulated_system->getKinematics()->getTechnique_common()->getAxis_info_array()[i], NULL);
605 for(
size_t ik = 0; ik < articulated_system->getKinematics()->getInstance_kinematics_model_array().getCount(); ++ik) {
606 _ExtractKinematicsModel(articulated_system->getKinematics()->getInstance_kinematics_model_array()[ik],bindings);
610 _ExtractRobotAttachedActuators(articulated_system);
611 _ExtractRobotManipulators(articulated_system);
612 _ExtractRobotAttachedSensors(articulated_system);
621 ROS_DEBUG_STREAM(str(boost::format(
"instance kinematics model sid %s\n")%ikm->getSid()));
622 domKinematics_modelRef kmodel = daeSafeCast<domKinematics_model> (ikm->getUrl().getElement().cast());
624 ROS_WARN_STREAM(str(boost::format(
"%s does not reference valid kinematics\n")%ikm->getSid()));
627 domPhysics_modelRef pmodel;
629 if( !pinterface_type ) {
630 pinterface_type = _ExtractInterfaceType(kmodel->getExtra_array());
632 if( !!pinterface_type ) {
633 ROS_DEBUG_STREAM(str(boost::format(
"kinbody interface type: %s")%(*pinterface_type)));
637 domNodeRef pvisualnode;
639 if( it->second == ikm ) {
640 pvisualnode = it->first;
645 ROS_WARN_STREAM(str(boost::format(
"failed to find visual node for instance kinematics model %s\n")%ikm->getSid()));
649 if( _model->name_.size() == 0 && !!ikm->getName() ) {
650 _model->name_ = ikm->getName();
652 if( _model->name_.size() == 0 && !!ikm->getID() ) {
653 _model->name_ = ikm->getID();
656 if (!_ExtractKinematicsModel(kmodel, pvisualnode, pmodel, bindings)) {
657 ROS_WARN_STREAM(str(boost::format(
"failed to load kinbody from kinematics model %s\n")%kmodel->getID()));
666 std::vector<domJointRef> vdomjoints;
667 ROS_DEBUG_STREAM(str(boost::format(
"kinematics model: %s\n")%_model->name_));
673 domKinematics_model_techniqueRef ktec = kmodel->getTechnique_common();
676 for (
size_t ijoint = 0; ijoint < ktec->getJoint_array().getCount(); ++ijoint) {
677 vdomjoints.push_back(ktec->getJoint_array()[ijoint]);
681 for (
size_t ijoint = 0; ijoint < ktec->getInstance_joint_array().getCount(); ++ijoint) {
682 domJointRef pelt = daeSafeCast<domJoint> (ktec->getInstance_joint_array()[ijoint]->getUrl().getElement());
687 vdomjoints.push_back(pelt);
691 ROS_DEBUG_STREAM(str(boost::format(
"Number of root links in the kmodel %d\n")%ktec->getLink_array().getCount()));
692 for (
size_t ilink = 0; ilink < ktec->getLink_array().getCount(); ++ilink) {
693 domLinkRef pdomlink = ktec->getLink_array()[ilink];
694 _RootOrigin = _poseFromMatrix(_ExtractFullTransform(pdomlink));
695 ROS_DEBUG(
"RootOrigin: %s %lf %lf %lf %lf %lf %lf %lf",
697 _RootOrigin.position.x, _RootOrigin.position.y, _RootOrigin.position.z,
698 _RootOrigin.rotation.x, _RootOrigin.rotation.y, _RootOrigin.rotation.z, _RootOrigin.rotation.w);
700 domNodeRef pvisualnode;
702 if(strcmp(it->first->getName() ,pdomlink->getName()) == 0) {
703 pvisualnode = it->first;
708 _VisualRootOrigin = _poseFromMatrix(_getNodeParentTransform(pvisualnode));
709 ROS_DEBUG(
"VisualRootOrigin: %s %lf %lf %lf %lf %lf %lf %lf",
711 _VisualRootOrigin.position.x, _VisualRootOrigin.position.y, _VisualRootOrigin.position.z,
712 _VisualRootOrigin.rotation.x, _VisualRootOrigin.rotation.y, _VisualRootOrigin.rotation.z, _VisualRootOrigin.rotation.w);
714 _ExtractLink(pdomlink, ilink == 0 ? pnode : domNodeRef(), Pose(), Pose(), vdomjoints, bindings);
718 for (
size_t iform = 0; iform < ktec->getFormula_array().getCount(); ++iform) {
719 domFormulaRef pf = ktec->getFormula_array()[iform];
720 if (!pf->getTarget()) {
726 urdf::JointSharedPtr pjoint = _getJointFromRef(pf->getTarget()->getParam()->getValue(),pf);
731 if (!!pf->getTechnique_common()) {
732 daeElementRef peltmath;
733 daeTArray<daeElementRef> children;
734 pf->getTechnique_common()->getChildren(children);
735 for (
size_t ichild = 0; ichild < children.getCount(); ++ichild) {
736 daeElementRef pelt = children[ichild];
737 if (_checkMathML(pelt,std::string(
"math")) ) {
741 ROS_WARN_STREAM(str(boost::format(
"unsupported formula element: %s\n")%pelt->getElementName()));
748 daeElementRef psymboljoint;
749 BOOST_ASSERT(peltmath->getChildren().getCount()>0);
750 daeElementRef papplyelt = peltmath->getChildren()[0];
751 BOOST_ASSERT(_checkMathML(papplyelt,
"apply"));
752 BOOST_ASSERT(papplyelt->getChildren().getCount()>0);
753 if( _checkMathML(papplyelt->getChildren()[0],
"plus") ) {
754 BOOST_ASSERT(papplyelt->getChildren().getCount()==3);
755 daeElementRef pa = papplyelt->getChildren()[1];
756 daeElementRef pb = papplyelt->getChildren()[2];
757 if( !_checkMathML(papplyelt->getChildren()[1],
"apply") ) {
760 if( !_checkMathML(pa,
"csymbol") ) {
761 BOOST_ASSERT(_checkMathML(pa,
"apply"));
762 BOOST_ASSERT(_checkMathML(pa->getChildren()[0],
"times"));
763 if( _checkMathML(pa->getChildren()[1],
"csymbol") ) {
764 psymboljoint = pa->getChildren()[1];
765 BOOST_ASSERT(_checkMathML(pa->getChildren()[2],
"cn"));
766 std::stringstream ss(pa->getChildren()[2]->getCharData());
770 psymboljoint = pa->getChildren()[2];
771 BOOST_ASSERT(_checkMathML(pa->getChildren()[1],
"cn"));
772 std::stringstream ss(pa->getChildren()[1]->getCharData());
779 BOOST_ASSERT(_checkMathML(pb,
"cn"));
781 std::stringstream ss(pb->getCharData());
785 else if( _checkMathML(papplyelt->getChildren()[0],
"minus") ) {
786 BOOST_ASSERT(_checkMathML(papplyelt->getChildren()[1],
"csymbol"));
788 psymboljoint = papplyelt->getChildren()[1];
791 BOOST_ASSERT(_checkMathML(papplyelt->getChildren()[0],
"csymbol"));
792 psymboljoint = papplyelt->getChildren()[0];
794 BOOST_ASSERT(psymboljoint->hasAttribute(
"encoding"));
795 BOOST_ASSERT(psymboljoint->getAttribute(
"encoding")==std::string(
"COLLADA"));
796 urdf::JointSharedPtr pbasejoint = _getJointFromRef(psymboljoint->getCharData().c_str(),pf);
799 pjoint->mimic.reset(
new JointMimic());
800 pjoint->mimic->joint_name = pbasejoint->name;
801 pjoint->mimic->multiplier = a;
802 pjoint->mimic->offset = b;
803 ROS_DEBUG_STREAM(str(boost::format(
"assigning joint %s to mimic %s %f %f\n")%pjoint->name%pbasejoint->name%a%b));
812 urdf::LinkSharedPtr
_ExtractLink(
const domLinkRef pdomlink,
const domNodeRef pdomnode,
const Pose& tParentWorldLink,
const Pose& tParentLink,
const std::vector<domJointRef>& vdomjoints,
const KinematicsSceneBindings& bindings) {
813 const std::list<JointAxisBinding>& listAxisBindings = bindings.
listAxisBindings;
815 std::string linkname = _ExtractLinkName(pdomlink);
816 if( linkname.size() == 0 ) {
817 ROS_WARN_STREAM(
"<link> has no name or id, falling back to <node>!\n");
819 if (!!pdomnode->getName()) {
820 linkname = pdomnode->getName();
822 if( linkname.size() == 0 && !!pdomnode->getID()) {
823 linkname = pdomnode->getID();
828 urdf::LinkSharedPtr plink;
829 _model->getLink(linkname,plink);
831 plink.reset(
new Link());
832 plink->name = linkname;
833 plink->visual.reset(
new Visual());
834 plink->visual->material_name =
"";
835 plink->visual->material.reset(
new Material());
836 plink->visual->material->name =
"Red";
837 plink->visual->material->color.r = 0.0;
838 plink->visual->material->color.g = 1.0;
839 plink->visual->material->color.b = 0.0;
840 plink->visual->material->color.a = 1.0;
841 plink->inertial.reset();
842 _model->links_.insert(std::make_pair(linkname,plink));
845 _getUserData(pdomlink)->p = plink;
847 ROS_DEBUG_STREAM(str(boost::format(
"Node Id %s and Name %s\n")%pdomnode->getId()%pdomnode->getName()));
851 domInstance_rigid_bodyRef irigidbody;
852 domRigid_bodyRef rigidbody;
853 bool bFoundBinding =
false;
855 if( !!pdomnode->getID() && !!itlinkbinding->node->getID() && strcmp(pdomnode->getID(),itlinkbinding->node->getID()) == 0 ) {
856 bFoundBinding =
true;
857 irigidbody = itlinkbinding->irigidbody;
858 rigidbody = itlinkbinding->rigidbody;
862 if( !!rigidbody && !!rigidbody->getTechnique_common() ) {
863 domRigid_body::domTechnique_commonRef rigiddata = rigidbody->getTechnique_common();
864 if( !!rigiddata->getMass() ) {
865 if ( !plink->inertial ) {
866 plink->inertial.reset(
new Inertial());
868 plink->inertial->mass = rigiddata->getMass()->getValue();
870 if( !!rigiddata->getInertia() ) {
871 if ( !plink->inertial ) {
872 plink->inertial.reset(
new Inertial());
874 plink->inertial->ixx = rigiddata->getInertia()->getValue()[0];
875 plink->inertial->iyy = rigiddata->getInertia()->getValue()[1];
876 plink->inertial->izz = rigiddata->getInertia()->getValue()[2];
878 if( !!rigiddata->getMass_frame() ) {
879 if ( !plink->inertial ) {
880 plink->inertial.reset(
new Inertial());
883 Pose tlink = _poseFromMatrix(_ExtractFullTransform(pdomlink));
884 plink->inertial->origin = _poseMult(_poseInverse(_poseMult(_poseInverse(_RootOrigin),
885 _poseMult(tParentWorldLink, tlink))),
886 _poseFromMatrix(_ExtractFullTransform(rigiddata->getMass_frame())));
890 std::list<GEOMPROPERTIES> listGeomProperties;
893 _ExtractGeometry(pdomnode,listGeomProperties,listAxisBindings,Pose());
896 ROS_DEBUG_STREAM(str(boost::format(
"Attachment link elements: %d\n")%pdomlink->getAttachment_full_array().getCount()));
897 Pose tlink = _poseFromMatrix(_ExtractFullTransform(pdomlink));
898 ROS_DEBUG(
"tlink: %s: %lf %lf %lf %lf %lf %lf %lf",
900 tlink.position.x, tlink.position.y, tlink.position.z,
901 tlink.rotation.x, tlink.rotation.y, tlink.rotation.z, tlink.rotation.w);
902 plink->visual->origin = _poseMult(tParentLink, tlink);
907 _ExtractGeometry(pdomnode, listGeomProperties, listAxisBindings,
908 _poseMult(_poseMult(tParentWorldLink,tlink), plink->visual->origin));
910 ROS_DEBUG_STREAM(str(boost::format(
"After ExtractGeometry Attachment link elements: %d\n")%pdomlink->getAttachment_full_array().getCount()));
913 for (
size_t iatt = 0; iatt < pdomlink->getAttachment_full_array().getCount(); ++iatt) {
914 domLink::domAttachment_fullRef pattfull = pdomlink->getAttachment_full_array()[iatt];
917 Pose tatt = _poseFromMatrix(_ExtractFullTransform(pattfull));
920 daeElement* peltjoint = daeSidRef(pattfull->getJoint(), pattfull).
resolve().elt;
921 domJointRef pdomjoint = daeSafeCast<domJoint> (peltjoint);
924 domInstance_jointRef pdomijoint = daeSafeCast<domInstance_joint> (peltjoint);
926 pdomjoint = daeSafeCast<domJoint> (pdomijoint->getUrl().getElement().cast());
930 if (!pdomjoint || pdomjoint->typeID() != domJoint::ID()) {
931 ROS_WARN_STREAM(str(boost::format(
"could not find attached joint %s!\n")%pattfull->getJoint()));
932 return urdf::LinkSharedPtr();
936 if (!pattfull->getLink()) {
937 ROS_WARN_STREAM(str(boost::format(
"joint %s needs to be attached to a valid link\n")%pdomjoint->getID()));
942 daeTArray<domAxis_constraintRef> vdomaxes = pdomjoint->getChildrenByType<domAxis_constraint>();
943 domNodeRef pchildnode;
946 FOREACHC(itaxisbinding,listAxisBindings) {
947 for (
size_t ic = 0; ic < vdomaxes.getCount(); ++ic) {
949 if (vdomaxes[ic] == itaxisbinding->pkinematicaxis) {
950 pchildnode = itaxisbinding->visualnode;
959 ROS_DEBUG_STREAM(str(boost::format(
"joint %s has no visual binding\n")%pdomjoint->getID()));
963 std::vector<urdf::JointSharedPtr > vjoints(vdomaxes.getCount());
964 for (
size_t ic = 0; ic < vdomaxes.getCount(); ++ic) {
965 bool joint_active =
true;
966 FOREACHC(itaxisbinding,listAxisBindings) {
967 if (vdomaxes[ic] == itaxisbinding->pkinematicaxis) {
968 if( !!itaxisbinding->kinematics_axis_info ) {
969 if( !!itaxisbinding->kinematics_axis_info->getActive() ) {
970 joint_active = resolveBool(itaxisbinding->kinematics_axis_info->getActive(),itaxisbinding->kinematics_axis_info);
977 urdf::JointSharedPtr pjoint(
new Joint());
978 pjoint->limits.reset(
new JointLimits());
979 pjoint->limits->velocity = 0.0;
980 pjoint->limits->effort = 0.0;
981 pjoint->parent_link_name = plink->name;
983 if( !!pdomjoint->getName() ) {
984 pjoint->name = pdomjoint->getName();
987 pjoint->name = str(boost::format(
"dummy%d")%_model->joints_.size());
990 if( !joint_active ) {
991 ROS_INFO_STREAM(str(boost::format(
"joint %s is passive, but adding to hierarchy\n")%pjoint->name));
994 domAxis_constraintRef pdomaxis = vdomaxes[ic];
995 if( strcmp(pdomaxis->getElementName(),
"revolute") == 0 ) {
996 pjoint->type = Joint::REVOLUTE;
998 else if( strcmp(pdomaxis->getElementName(),
"prismatic") == 0 ) {
999 pjoint->type = Joint::PRISMATIC;
1002 ROS_WARN_STREAM(str(boost::format(
"unsupported joint type: %s\n")%pdomaxis->getElementName()));
1005 _getUserData(pdomjoint)->p = pjoint;
1006 #if URDFDOM_HEADERS_MAJOR_VERSION < 1 1009 _getUserData(pdomaxis)->p = std::shared_ptr<int>(
new int(_model->joints_.size()));
1011 _model->joints_[pjoint->name] = pjoint;
1012 vjoints[ic] = pjoint;
1015 urdf::LinkSharedPtr pchildlink = _ExtractLink(pattfull->getLink(), pchildnode, _poseMult(_poseMult(tParentWorldLink,tlink), tatt), tatt, vdomjoints, bindings);
1018 ROS_WARN_STREAM(str(boost::format(
"Link has no child: %s\n")%plink->name));
1023 for (
size_t ic = 0; ic < vdomaxes.getCount(); ++ic) {
1024 domKinematics_axis_infoRef kinematics_axis_info;
1025 domMotion_axis_infoRef motion_axis_info;
1026 FOREACHC(itaxisbinding,listAxisBindings) {
1027 bool bfound =
false;
1028 if (vdomaxes[ic] == itaxisbinding->pkinematicaxis) {
1029 kinematics_axis_info = itaxisbinding->kinematics_axis_info;
1030 motion_axis_info = itaxisbinding->motion_axis_info;
1035 domAxis_constraintRef pdomaxis = vdomaxes[ic];
1039 ROS_WARN_STREAM(str(boost::format(
"creating dummy link %s, num joints %d\n")%plink->name%numjoints));
1041 std::stringstream ss;
1043 ss <<
"_dummy" << numjoints;
1044 pchildlink.reset(
new Link());
1045 pchildlink->name = ss.str();
1046 _model->links_.insert(std::make_pair(pchildlink->name,pchildlink));
1049 ROS_DEBUG_STREAM(str(boost::format(
"Joint %s assigned %d \n")%vjoints[ic]->name%ic));
1050 urdf::JointSharedPtr pjoint = vjoints[ic];
1051 pjoint->child_link_name = pchildlink->name;
1053 #define PRINT_POSE(pname, apose) ROS_DEBUG(pname" pos: %f %f %f, rot: %f %f %f %f", \ 1054 apose.position.x, apose.position.y, apose.position.z, \ 1055 apose.rotation.x, apose.rotation.y, apose.rotation.z, apose.rotation.w); 1059 Pose trans_joint_to_child = _poseFromMatrix(_ExtractFullTransform(pattfull->getLink()));
1061 pjoint->parent_to_joint_origin_transform = _poseMult(tatt, trans_joint_to_child);
1063 Vector3 ax(pdomaxis->getAxis()->getValue()[0],
1064 pdomaxis->getAxis()->getValue()[1],
1065 pdomaxis->getAxis()->getValue()[2]);
1066 Pose pinv = _poseInverse(trans_joint_to_child);
1068 ax = pinv.rotation * ax;
1070 pjoint->axis.x = ax.x;
1071 pjoint->axis.y = ax.y;
1072 pjoint->axis.z = ax.z;
1074 ROS_DEBUG(
"joint %s axis: %f %f %f -> %f %f %f\n", pjoint->name.c_str(),
1075 pdomaxis->getAxis()->getValue()[0],
1076 pdomaxis->getAxis()->getValue()[1],
1077 pdomaxis->getAxis()->getValue()[2],
1078 pjoint->axis.x, pjoint->axis.y, pjoint->axis.z);
1081 if (!motion_axis_info) {
1082 ROS_WARN_STREAM(str(boost::format(
"No motion axis info for joint %s\n")%pjoint->name));
1086 if (!!motion_axis_info) {
1087 if (!!motion_axis_info->getSpeed()) {
1088 pjoint->limits->velocity = resolveFloat(motion_axis_info->getSpeed(),motion_axis_info);
1089 ROS_DEBUG(
"... Joint Speed: %f...\n",pjoint->limits->velocity);
1091 if (!!motion_axis_info->getAcceleration()) {
1092 pjoint->limits->effort = resolveFloat(motion_axis_info->getAcceleration(),motion_axis_info);
1093 ROS_DEBUG(
"... Joint effort: %f...\n",pjoint->limits->effort);
1097 bool joint_locked =
false;
1098 bool kinematics_limits =
false;
1100 if (!!kinematics_axis_info) {
1101 if (!!kinematics_axis_info->getLocked()) {
1102 joint_locked = resolveBool(kinematics_axis_info->getLocked(),kinematics_axis_info);
1106 if( pjoint->type == Joint::REVOLUTE || pjoint->type ==Joint::PRISMATIC) {
1108 pjoint->limits->lower = 0;
1109 pjoint->limits->upper = 0;
1112 else if (kinematics_axis_info->getLimits()) {
1113 kinematics_limits =
true;
1114 double fscale = (pjoint->type == Joint::REVOLUTE) ? (M_PI/180.0
f) : _GetUnitScale(kinematics_axis_info);
1115 if( pjoint->type == Joint::REVOLUTE || pjoint->type ==Joint::PRISMATIC) {
1116 pjoint->limits->lower = fscale*(double)(resolveFloat(kinematics_axis_info->getLimits()->getMin(),kinematics_axis_info));
1117 pjoint->limits->upper = fscale*(double)(resolveFloat(kinematics_axis_info->getLimits()->getMax(),kinematics_axis_info));
1118 if ( pjoint->limits->lower == 0 && pjoint->limits->upper == 0 ) {
1119 pjoint->type = Joint::FIXED;
1126 if (!kinematics_axis_info || (!joint_locked && !kinematics_limits)) {
1128 if( !pdomaxis->getLimits() ) {
1129 ROS_DEBUG_STREAM(str(boost::format(
"There are NO LIMITS in joint %s:%d ...\n")%pjoint->name%kinematics_limits));
1130 if( pjoint->type == Joint::REVOLUTE ) {
1131 pjoint->type = Joint::CONTINUOUS;
1132 pjoint->limits->lower = -M_PI;
1133 pjoint->limits->upper = M_PI;
1136 pjoint->limits->lower = -100000;
1137 pjoint->limits->upper = 100000;
1141 ROS_DEBUG_STREAM(str(boost::format(
"There are LIMITS in joint %s ...\n")%pjoint->name));
1142 double fscale = (pjoint->type == Joint::REVOLUTE) ? (M_PI/180.0
f) : _GetUnitScale(pdomaxis);
1143 pjoint->limits->lower = (double)pdomaxis->getLimits()->getMin()->getValue()*fscale;
1144 pjoint->limits->upper = (double)pdomaxis->getLimits()->getMax()->getValue()*fscale;
1145 if ( pjoint->limits->lower == 0 && pjoint->limits->upper == 0 ) {
1146 pjoint->type = Joint::FIXED;
1151 if (pjoint->limits->velocity == 0.0) {
1152 pjoint->limits->velocity = pjoint->type == Joint::PRISMATIC ? 0.01 : 0.5f;
1160 if( pdomlink->getAttachment_start_array().getCount() > 0 ) {
1161 ROS_WARN(
"urdf collada reader does not support attachment_start\n");
1163 if( pdomlink->getAttachment_end_array().getCount() > 0 ) {
1164 ROS_WARN(
"urdf collada reader does not support attachment_end\n");
1167 plink->visual->geometry = _CreateGeometry(plink->name, listGeomProperties);
1174 if( !plink->visual->geometry ) {
1175 plink->visual.reset();
1176 plink->collision.reset();
1179 plink->collision.reset(
new Collision());
1180 plink->collision->geometry = plink->visual->geometry;
1181 plink->collision->origin = plink->visual->origin;
1193 urdf::GeometrySharedPtr
_CreateGeometry(
const std::string& name,
const std::list<GEOMPROPERTIES>& listGeomProperties)
1195 std::vector<std::vector<Vector3> > vertices;
1196 std::vector<std::vector<int> > indices;
1197 std::vector<Color> ambients;
1198 std::vector<Color> diffuses;
1199 unsigned int index, vert_counter;
1200 vertices.resize(listGeomProperties.size());
1201 indices.resize(listGeomProperties.size());
1202 ambients.resize(listGeomProperties.size());
1203 diffuses.resize(listGeomProperties.size());
1207 vertices[index].resize(it->vertices.size());
1208 for(
size_t i = 0; i < it->vertices.size(); ++i) {
1209 vertices[index][i] = _poseMult(it->_t, it->vertices[i]);
1212 indices[index].resize(it->indices.size());
1213 for(
size_t i = 0; i < it->indices.size(); ++i) {
1214 indices[index][i] = it->indices[i];
1216 ambients[index] = it->ambientColor;
1217 diffuses[index] = it->diffuseColor;
1220 ambients[index].r *= 0.5; ambients[index].g *= 0.5; ambients[index].b *= 0.5;
1221 if ( ambients[index].r == 0.0 ) {
1222 ambients[index].r = 0.0001;
1224 if ( ambients[index].g == 0.0 ) {
1225 ambients[index].g = 0.0001;
1227 if ( ambients[index].b == 0.0 ) {
1228 ambients[index].b = 0.0001;
1233 if (vert_counter == 0) {
1234 urdf::MeshSharedPtr ret;
1239 urdf::MeshSharedPtr geometry(
new Mesh());
1240 geometry->type = Geometry::MESH;
1241 geometry->scale.x = 1;
1242 geometry->scale.y = 1;
1243 geometry->scale.z = 1;
1246 std::stringstream daedata;
1247 daedata << str(boost::format(
"<?xml version=\"1.0\" encoding=\"utf-8\"?>\n\ 1248 <COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n\ 1251 <author>Rosen Diankov</author>\n\ 1253 robot_model/urdf temporary collada geometry\n\ 1256 <unit name=\"meter\" meter=\"1.0\"/>\n\ 1257 <up_axis>Y_UP</up_axis>\n\ 1259 <library_materials>\n"));
1260 for(
unsigned int i=0; i < index; i++) {
1261 daedata << str(boost::format(
"\ 1262 <material id=\"blinn2%d\" name=\"blinn2%d\">\n\ 1263 <instance_effect url=\"#blinn2-fx%d\"/>\n\ 1264 </material>\n")%i%i%i);
1267 </library_materials>\n\ 1268 <library_effects>\n";
1269 for(
unsigned int i=0; i < index; i++) {
1270 daedata << str(boost::format(
"\ 1271 <effect id=\"blinn2-fx%d\">\n\ 1273 <technique sid=\"common\">\n\ 1276 <color>%f %f %f %f</color>\n\ 1279 <color>%f %f %f %f</color>\n\ 1282 <color>0 0 0 1</color>\n\ 1286 </profile_COMMON>\n\ 1287 </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);
1289 daedata << str(boost::format(
"\ 1290 </library_effects>\n\ 1291 <library_geometries>\n"));
1293 for(
unsigned int i=0; i < index; i++) {
1294 daedata << str(boost::format(
"\ 1295 <geometry id=\"base2_M1KShape%d\" name=\"base2_M1KShape%d\">\n\ 1297 <source id=\"geo0.positions\">\n\ 1298 <float_array id=\"geo0.positions-array\" count=\"%d\">")%i%i%(vertices[i].size()*3));
1300 daedata << it->x <<
" " << it->y <<
" " << it->z <<
" ";
1302 daedata << str(boost::format(
"\n\ 1304 <technique_common>\n\ 1305 <accessor count=\"%d\" source=\"#geo0.positions-array\" stride=\"3\">\n\ 1306 <param name=\"X\" type=\"float\"/>\n\ 1307 <param name=\"Y\" type=\"float\"/>\n\ 1308 <param name=\"Z\" type=\"float\"/>\n\ 1310 </technique_common>\n\ 1312 <vertices id=\"geo0.vertices\">\n\ 1313 <input semantic=\"POSITION\" source=\"#geo0.positions\"/>\n\ 1315 <triangles count=\"%d\" material=\"lambert2SG\">\n\ 1316 <input offset=\"0\" semantic=\"VERTEX\" source=\"#geo0.vertices\"/>\n\ 1317 <p>")%vertices[i].size()%(indices[i].size()/3));
1320 daedata << *it <<
" ";
1322 daedata << str(boost::format(
"</p>\n\ 1327 daedata << str(boost::format(
"\ 1328 </library_geometries>\n\ 1329 <library_visual_scenes>\n\ 1330 <visual_scene id=\"VisualSceneNode\" name=\"base1d_med\">\n\ 1331 <node id=\"%s\" name=\"%s\" type=\"NODE\">\n")%name%name);
1332 for(
unsigned int i=0; i < index; i++) {
1333 daedata << str(boost::format(
"\ 1334 <instance_geometry url=\"#base2_M1KShape%i\">\n\ 1336 <technique_common>\n\ 1337 <instance_material symbol=\"lambert2SG\" target=\"#blinn2%i\"/>\n\ 1338 </technique_common>\n\ 1340 </instance_geometry>\n")%i%i);
1342 daedata << str(boost::format(
"\ 1345 </library_visual_scenes>\n\ 1347 <instance_visual_scene url=\"#VisualSceneNode\"/>\n\ 1351 #ifdef HAVE_MKSTEMPS 1352 geometry->filename = str(boost::format(
"/tmp/collada_model_reader_%s_XXXXXX.dae")%name);
1353 int fd = mkstemps(&geometry->filename[0],4);
1356 for(
int iter = 0; iter < 1000; ++iter) {
1357 geometry->filename = str(boost::format(
"/tmp/collada_model_reader_%s_%d.dae")%name%rand());
1358 if( !!std::ifstream(geometry->filename.c_str())) {
1359 fd = open(geometry->filename.c_str(),O_WRONLY|O_CREAT|O_EXCL);
1366 ROS_ERROR(
"failed to open geometry dae file %s",geometry->filename.c_str());
1371 std::string daedatastr = daedata.str();
1372 if( (
size_t)write(fd,daedatastr.c_str(),daedatastr.size()) != daedatastr.size() ) {
1373 ROS_ERROR(
"failed to write to geometry dae file %s",geometry->filename.c_str());
1377 geometry->filename = std::string(
"file://") + geometry->filename;
1384 void _ExtractGeometry(
const domNodeRef pdomnode,std::list<GEOMPROPERTIES>& listGeomProperties,
const std::list<JointAxisBinding>& listAxisBindings,
const Pose& tlink)
1390 ROS_DEBUG_STREAM(str(boost::format(
"ExtractGeometry(node,link) of %s\n")%pdomnode->getName()));
1393 for (
size_t i = 0; i < pdomnode->getNode_array().getCount(); i++) {
1395 bool contains=
false;
1398 if ( (pdomnode->getNode_array()[i]) == (it->visualnode)) {
1407 _ExtractGeometry(pdomnode->getNode_array()[i],listGeomProperties, listAxisBindings,tlink);
1414 unsigned int nGeomBefore = listGeomProperties.size();
1417 for (
size_t igeom = 0; igeom < pdomnode->getInstance_geometry_array().getCount(); ++igeom) {
1418 domInstance_geometryRef domigeom = pdomnode->getInstance_geometry_array()[igeom];
1419 domGeometryRef domgeom = daeSafeCast<domGeometry> (domigeom->getUrl().getElement());
1425 std::map<std::string, domMaterialRef> mapmaterials;
1426 if (!!domigeom->getBind_material() && !!domigeom->getBind_material()->getTechnique_common()) {
1427 const domInstance_material_Array& matarray = domigeom->getBind_material()->getTechnique_common()->getInstance_material_array();
1428 for (
size_t imat = 0; imat < matarray.getCount(); ++imat) {
1429 domMaterialRef pmat = daeSafeCast<domMaterial>(matarray[imat]->getTarget().getElement());
1431 mapmaterials[matarray[imat]->getSymbol()] = pmat;
1437 _ExtractGeometry(domgeom, mapmaterials, listGeomProperties);
1440 std::list<GEOMPROPERTIES>::iterator itgeom= listGeomProperties.begin();
1441 for (
unsigned int i=0; i< nGeomBefore; i++) {
1445 boost::array<double,12> tmnodegeom = _poseMult(_matrixFromPose(_poseInverse(tlink)),
1446 _poseMult(_poseMult(_matrixFromPose(_poseInverse(_VisualRootOrigin)),
1447 _getNodeParentTransform(pdomnode)),
1448 _ExtractFullTransform(pdomnode)));
1450 Vector3 vscale(1,1,1);
1451 _decompose(tmnodegeom, tnodegeom, vscale);
1454 << tnodegeom.position.x <<
" " << tnodegeom.position.y <<
" " << tnodegeom.position.z <<
" / " 1455 << tnodegeom.rotation.x <<
" " << tnodegeom.rotation.y <<
" " << tnodegeom.rotation.z <<
" " 1456 << tnodegeom.rotation.w);
1465 for (; itgeom != listGeomProperties.end(); itgeom++) {
1466 itgeom->_t = tnodegeom;
1467 switch (itgeom->type) {
1469 itgeom->vGeomData.x *= vscale.x;
1470 itgeom->vGeomData.y *= vscale.y;
1471 itgeom->vGeomData.z *= vscale.z;
1474 itgeom->vGeomData.x *= std::max(vscale.z, std::max(vscale.x, vscale.y));
1478 itgeom->vGeomData.x *= std::max(vscale.x, vscale.y);
1479 itgeom->vGeomData.y *= vscale.z;
1482 for(
size_t i = 0; i < itgeom->vertices.size(); ++i ) {
1483 itgeom->vertices[i] = _poseMult(tmnodegeom,itgeom->vertices[i]);
1485 itgeom->_t = Pose();
1488 ROS_WARN_STREAM(str(boost::format(
"unknown geometry type: %d\n")%itgeom->type));
1498 if( !!pmat && !!pmat->getInstance_effect() ) {
1499 domEffectRef peffect = daeSafeCast<domEffect>(pmat->getInstance_effect()->getUrl().getElement().cast());
1501 domProfile_common::domTechnique::domPhongRef pphong = daeSafeCast<domProfile_common::domTechnique::domPhong>(peffect->getDescendant(daeElement::matchType(domProfile_common::domTechnique::domPhong::ID())));
1503 if( !!pphong->getAmbient() && !!pphong->getAmbient()->getColor() ) {
1504 domFx_color c = pphong->getAmbient()->getColor()->getValue();
1510 if( !!pphong->getDiffuse() && !!pphong->getDiffuse()->getColor() ) {
1511 domFx_color c = pphong->getDiffuse()->getColor()->getValue();
1527 bool _ExtractGeometry(
const domTrianglesRef triRef,
const domVerticesRef vertsRef,
const std::map<std::string,domMaterialRef>& mapmaterials, std::list<GEOMPROPERTIES>& listGeomProperties)
1534 geom.
type = GeomTrimesh;
1537 if( !!triRef->getMaterial() ) {
1538 std::map<std::string,domMaterialRef>::const_iterator itmat = mapmaterials.find(triRef->getMaterial());
1539 if( itmat != mapmaterials.end() ) {
1540 _FillGeometryColor(itmat->second,geom);
1544 size_t triangleIndexStride = 0, vertexoffset = -1;
1545 domInput_local_offsetRef indexOffsetRef;
1547 for (
unsigned int w=0; w<triRef->getInput_array().getCount(); w++) {
1548 size_t offset = triRef->getInput_array()[w]->getOffset();
1549 daeString str = triRef->getInput_array()[w]->getSemantic();
1550 if (!strcmp(str,
"VERTEX")) {
1551 indexOffsetRef = triRef->getInput_array()[w];
1552 vertexoffset = offset;
1554 if (offset> triangleIndexStride) {
1555 triangleIndexStride = offset;
1558 triangleIndexStride++;
1560 const domList_of_uints& indexArray =triRef->getP()->getValue();
1561 geom.
indices.reserve(triRef->getCount()*3);
1562 geom.
vertices.reserve(triRef->getCount()*3);
1563 for (
size_t i=0; i<vertsRef->getInput_array().getCount(); ++i) {
1564 domInput_localRef localRef = vertsRef->getInput_array()[i];
1565 daeString str = localRef->getSemantic();
1566 if ( strcmp(str,
"POSITION") == 0 ) {
1567 const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
1571 double fUnitScale = _GetUnitScale(node);
1572 const domFloat_arrayRef flArray = node->getFloat_array();
1574 const domList_of_floats& listFloats = flArray->getValue();
1576 int vertexStride = 3;
1577 for(
size_t itri = 0; itri < triRef->getCount(); ++itri) {
1578 if(k+2*triangleIndexStride < indexArray.getCount() ) {
1579 for (
int j=0; j<3; j++) {
1580 int index0 = indexArray.get(k)*vertexStride;
1581 domFloat fl0 = listFloats.get(index0);
1582 domFloat fl1 = listFloats.get(index0+1);
1583 domFloat fl2 = listFloats.get(index0+2);
1584 k+=triangleIndexStride;
1586 geom.
vertices.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale));
1597 if( geom.
indices.size() != 3*triRef->getCount() ) {
1608 bool _ExtractGeometry(
const domTrifansRef triRef,
const domVerticesRef vertsRef,
const std::map<std::string,domMaterialRef>& mapmaterials, std::list<GEOMPROPERTIES>& listGeomProperties)
1615 geom.
type = GeomTrimesh;
1618 if( !!triRef->getMaterial() ) {
1619 std::map<std::string,domMaterialRef>::const_iterator itmat = mapmaterials.find(triRef->getMaterial());
1620 if( itmat != mapmaterials.end() ) {
1621 _FillGeometryColor(itmat->second,geom);
1625 size_t triangleIndexStride = 0, vertexoffset = -1;
1626 domInput_local_offsetRef indexOffsetRef;
1628 for (
unsigned int w=0; w<triRef->getInput_array().getCount(); w++) {
1629 size_t offset = triRef->getInput_array()[w]->getOffset();
1630 daeString str = triRef->getInput_array()[w]->getSemantic();
1631 if (!strcmp(str,
"VERTEX")) {
1632 indexOffsetRef = triRef->getInput_array()[w];
1633 vertexoffset = offset;
1635 if (offset> triangleIndexStride) {
1636 triangleIndexStride = offset;
1639 triangleIndexStride++;
1640 size_t primitivecount = triRef->getCount();
1641 if( primitivecount > triRef->getP_array().getCount() ) {
1643 primitivecount = triRef->getP_array().getCount();
1645 for(
size_t ip = 0; ip < primitivecount; ++ip) {
1646 domList_of_uints indexArray =triRef->getP_array()[ip]->getValue();
1647 for (
size_t i=0; i<vertsRef->getInput_array().getCount(); ++i) {
1648 domInput_localRef localRef = vertsRef->getInput_array()[i];
1649 daeString str = localRef->getSemantic();
1650 if ( strcmp(str,
"POSITION") == 0 ) {
1651 const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
1655 double fUnitScale = _GetUnitScale(node);
1656 const domFloat_arrayRef flArray = node->getFloat_array();
1658 const domList_of_floats& listFloats = flArray->getValue();
1660 int vertexStride = 3;
1661 size_t usedindices = 3*(indexArray.getCount()-2);
1662 if( geom.
indices.capacity() < geom.
indices.size()+usedindices ) {
1665 if( geom.
vertices.capacity() < geom.
vertices.size()+indexArray.getCount() ) {
1668 size_t startoffset = (int)geom.
vertices.size();
1669 while(k < (
int)indexArray.getCount() ) {
1670 int index0 = indexArray.get(k)*vertexStride;
1671 domFloat fl0 = listFloats.get(index0);
1672 domFloat fl1 = listFloats.get(index0+1);
1673 domFloat fl2 = listFloats.get(index0+2);
1674 k+=triangleIndexStride;
1675 geom.
vertices.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale));
1677 for(
size_t ivert = startoffset+2; ivert < geom.
vertices.size(); ++ivert) {
1678 geom.
indices.push_back(startoffset);
1679 geom.
indices.push_back(ivert-1);
1680 geom.
indices.push_back(ivert);
1699 bool _ExtractGeometry(
const domTristripsRef triRef,
const domVerticesRef vertsRef,
const std::map<std::string,domMaterialRef>& mapmaterials, std::list<GEOMPROPERTIES>& listGeomProperties)
1706 geom.
type = GeomTrimesh;
1709 if( !!triRef->getMaterial() ) {
1710 std::map<std::string,domMaterialRef>::const_iterator itmat = mapmaterials.find(triRef->getMaterial());
1711 if( itmat != mapmaterials.end() ) {
1712 _FillGeometryColor(itmat->second,geom);
1715 size_t triangleIndexStride = 0, vertexoffset = -1;
1716 domInput_local_offsetRef indexOffsetRef;
1718 for (
unsigned int w=0; w<triRef->getInput_array().getCount(); w++) {
1719 size_t offset = triRef->getInput_array()[w]->getOffset();
1720 daeString str = triRef->getInput_array()[w]->getSemantic();
1721 if (!strcmp(str,
"VERTEX")) {
1722 indexOffsetRef = triRef->getInput_array()[w];
1723 vertexoffset = offset;
1725 if (offset> triangleIndexStride) {
1726 triangleIndexStride = offset;
1729 triangleIndexStride++;
1730 size_t primitivecount = triRef->getCount();
1731 if( primitivecount > triRef->getP_array().getCount() ) {
1733 primitivecount = triRef->getP_array().getCount();
1735 for(
size_t ip = 0; ip < primitivecount; ++ip) {
1736 domList_of_uints indexArray = triRef->getP_array()[ip]->getValue();
1737 for (
size_t i=0; i<vertsRef->getInput_array().getCount(); ++i) {
1738 domInput_localRef localRef = vertsRef->getInput_array()[i];
1739 daeString str = localRef->getSemantic();
1740 if ( strcmp(str,
"POSITION") == 0 ) {
1741 const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
1745 double fUnitScale = _GetUnitScale(node);
1746 const domFloat_arrayRef flArray = node->getFloat_array();
1748 const domList_of_floats& listFloats = flArray->getValue();
1750 int vertexStride = 3;
1751 size_t usedindices = indexArray.getCount()-2;
1752 if( geom.
indices.capacity() < geom.
indices.size()+usedindices ) {
1755 if( geom.
vertices.capacity() < geom.
vertices.size()+indexArray.getCount() ) {
1759 size_t startoffset = (int)geom.
vertices.size();
1760 while(k < (
int)indexArray.getCount() ) {
1761 int index0 = indexArray.get(k)*vertexStride;
1762 domFloat fl0 = listFloats.get(index0);
1763 domFloat fl1 = listFloats.get(index0+1);
1764 domFloat fl2 = listFloats.get(index0+2);
1765 k+=triangleIndexStride;
1766 geom.
vertices.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale));
1770 for(
size_t ivert = startoffset+2; ivert < geom.
vertices.size(); ++ivert) {
1771 geom.
indices.push_back(ivert-2);
1772 geom.
indices.push_back(bFlip ? ivert : ivert-1);
1773 geom.
indices.push_back(bFlip ? ivert-1 : ivert);
1793 bool _ExtractGeometry(
const domPolylistRef triRef,
const domVerticesRef vertsRef,
const std::map<std::string,domMaterialRef>& mapmaterials, std::list<GEOMPROPERTIES>& listGeomProperties)
1800 geom.
type = GeomTrimesh;
1803 if( !!triRef->getMaterial() ) {
1804 std::map<std::string,domMaterialRef>::const_iterator itmat = mapmaterials.find(triRef->getMaterial());
1805 if( itmat != mapmaterials.end() ) {
1806 _FillGeometryColor(itmat->second,geom);
1810 size_t triangleIndexStride = 0,vertexoffset = -1;
1811 domInput_local_offsetRef indexOffsetRef;
1812 for (
unsigned int w=0; w<triRef->getInput_array().getCount(); w++) {
1813 size_t offset = triRef->getInput_array()[w]->getOffset();
1814 daeString str = triRef->getInput_array()[w]->getSemantic();
1815 if (!strcmp(str,
"VERTEX")) {
1816 indexOffsetRef = triRef->getInput_array()[w];
1817 vertexoffset = offset;
1819 if (offset> triangleIndexStride) {
1820 triangleIndexStride = offset;
1823 triangleIndexStride++;
1824 const domList_of_uints& indexArray =triRef->getP()->getValue();
1825 for (
size_t i=0; i<vertsRef->getInput_array().getCount(); ++i) {
1826 domInput_localRef localRef = vertsRef->getInput_array()[i];
1827 daeString str = localRef->getSemantic();
1828 if ( strcmp(str,
"POSITION") == 0 ) {
1829 const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
1833 double fUnitScale = _GetUnitScale(node);
1834 const domFloat_arrayRef flArray = node->getFloat_array();
1836 const domList_of_floats& listFloats = flArray->getValue();
1837 size_t k=vertexoffset;
1838 int vertexStride = 3;
1839 for(
size_t ipoly = 0; ipoly < triRef->getVcount()->getValue().getCount(); ++ipoly) {
1840 size_t numverts = triRef->getVcount()->getValue()[ipoly];
1841 if(numverts > 0 && k+(numverts-1)*triangleIndexStride < indexArray.getCount() ) {
1842 size_t startoffset = geom.
vertices.size();
1843 for (
size_t j=0; j<numverts; j++) {
1844 int index0 = indexArray.get(k)*vertexStride;
1845 domFloat fl0 = listFloats.get(index0);
1846 domFloat fl1 = listFloats.get(index0+1);
1847 domFloat fl2 = listFloats.get(index0+2);
1848 k+=triangleIndexStride;
1849 geom.
vertices.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale));
1851 for(
size_t ivert = startoffset+2; ivert < geom.
vertices.size(); ++ivert) {
1852 geom.
indices.push_back(startoffset);
1853 geom.
indices.push_back(ivert-1);
1854 geom.
indices.push_back(ivert);
1872 bool _ExtractGeometry(
const domGeometryRef geom,
const std::map<std::string,domMaterialRef>& mapmaterials, std::list<GEOMPROPERTIES>& listGeomProperties)
1877 std::vector<Vector3> vconvexhull;
1878 if (geom->getMesh()) {
1879 const domMeshRef meshRef = geom->getMesh();
1880 for (
size_t tg = 0; tg<meshRef->getTriangles_array().getCount(); tg++) {
1881 _ExtractGeometry(meshRef->getTriangles_array()[tg], meshRef->getVertices(), mapmaterials, listGeomProperties);
1883 for (
size_t tg = 0; tg<meshRef->getTrifans_array().getCount(); tg++) {
1884 _ExtractGeometry(meshRef->getTrifans_array()[tg], meshRef->getVertices(), mapmaterials, listGeomProperties);
1886 for (
size_t tg = 0; tg<meshRef->getTristrips_array().getCount(); tg++) {
1887 _ExtractGeometry(meshRef->getTristrips_array()[tg], meshRef->getVertices(), mapmaterials, listGeomProperties);
1889 for (
size_t tg = 0; tg<meshRef->getPolylist_array().getCount(); tg++) {
1890 _ExtractGeometry(meshRef->getPolylist_array()[tg], meshRef->getVertices(), mapmaterials, listGeomProperties);
1892 if( meshRef->getPolygons_array().getCount()> 0 ) {
1927 else if (geom->getConvex_mesh()) {
1929 const domConvex_meshRef convexRef = geom->getConvex_mesh();
1930 daeElementRef otherElemRef = convexRef->getConvex_hull_of().getElement();
1931 if ( !!otherElemRef ) {
1932 domGeometryRef linkedGeom = *(domGeometryRef*)&otherElemRef;
1936 ROS_WARN(
"convexMesh polyCount = %d\n",(
int)convexRef->getPolygons_array().getCount());
1937 ROS_WARN(
"convexMesh triCount = %d\n",(
int)convexRef->getTriangles_array().getCount());
1941 const domConvex_meshRef convexRef = geom->getConvex_mesh();
1943 daeString urlref2 = convexRef->getConvex_hull_of().getOriginalURI();
1945 daeElementRef otherElemRef = convexRef->getConvex_hull_of().getElement();
1948 for (
size_t i = 0; i < _dom->getLibrary_geometries_array().getCount(); i++) {
1949 domLibrary_geometriesRef libgeom = _dom->getLibrary_geometries_array()[i];
1950 for (
size_t i = 0; i < libgeom->getGeometry_array().getCount(); i++) {
1951 domGeometryRef lib = libgeom->getGeometry_array()[i];
1952 if (!strcmp(lib->getId(),urlref2+1)) {
1954 domMesh *meshElement = lib->getMesh();
1956 const domVerticesRef vertsRef = meshElement->getVertices();
1957 for (
size_t i=0; i<vertsRef->getInput_array().getCount(); i++) {
1958 domInput_localRef localRef = vertsRef->getInput_array()[i];
1959 daeString str = localRef->getSemantic();
1960 if ( strcmp(str,
"POSITION") == 0) {
1961 const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
1965 double fUnitScale = _GetUnitScale(node);
1966 const domFloat_arrayRef flArray = node->getFloat_array();
1968 vconvexhull.reserve(vconvexhull.size()+flArray->getCount());
1969 const domList_of_floats& listFloats = flArray->getValue();
1970 for (
size_t k=0; k+2<flArray->getCount(); k+=3) {
1971 domFloat fl0 = listFloats.get(k);
1972 domFloat fl1 = listFloats.get(k+1);
1973 domFloat fl2 = listFloats.get(k+2);
1974 vconvexhull.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale));
1986 const domVerticesRef vertsRef = convexRef->getVertices();
1987 for (
size_t i=0; i<vertsRef->getInput_array().getCount(); i++) {
1988 domInput_localRef localRef = vertsRef->getInput_array()[i];
1989 daeString str = localRef->getSemantic();
1990 if ( strcmp(str,
"POSITION") == 0 ) {
1991 const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
1995 double fUnitScale = _GetUnitScale(node);
1996 const domFloat_arrayRef flArray = node->getFloat_array();
1998 const domList_of_floats& listFloats = flArray->getValue();
1999 vconvexhull.reserve(vconvexhull.size()+flArray->getCount());
2000 for (
size_t k=0; k+2<flArray->getCount(); k+=3) {
2001 domFloat fl0 = listFloats.get(k);
2002 domFloat fl1 = listFloats.get(k+1);
2003 domFloat fl2 = listFloats.get(k+2);
2004 vconvexhull.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale));
2011 if( vconvexhull.size()> 0 ) {
2014 geom.
type = GeomTrimesh;
2029 for(
size_t ie = 0; ie < as->getExtra_array().getCount(); ++ie) {
2030 domExtraRef pextra = as->getExtra_array()[ie];
2031 if( strcmp(pextra->getType(),
"attach_actuator") == 0 ) {
2033 domTechniqueRef tec = _ExtractOpenRAVEProfile(pextra->getTechnique_array());
2035 urdf::JointSharedPtr pjoint;
2036 daeElementRef domactuator;
2038 daeElementRef bact = tec->getChild(
"bind_actuator");
2039 pjoint = _getJointFromRef(bact->getAttribute(
"joint").c_str(), as);
2040 if (!pjoint)
continue;
2043 daeElementRef iact = tec->getChild(
"instance_actuator");
2045 std::string instance_url = iact->getAttribute(
"url");
2046 domactuator = daeURI(*iact, instance_url).getElement();
2047 if(!domactuator)
continue;
2049 daeElement *nom_torque = domactuator->getChild(
"nominal_torque");
2050 if ( !! nom_torque ) {
2051 if( !! pjoint->limits ) {
2052 pjoint->limits->effort = boost::lexical_cast<
double>(nom_torque->getCharData());
2053 ROS_DEBUG(
"effort limit at joint (%s) is over written by %f",
2054 pjoint->name.c_str(), pjoint->limits->effort);
2065 ROS_DEBUG(
"collada manipulators not supported yet");
2071 ROS_DEBUG(
"collada sensors not supported yet");
2076 return daeStandardURIResolver(*_collada).resolveElement(uri);
2079 static daeElement*
searchBinding(domCommon_sidref_or_paramRef paddr, daeElementRef parent)
2081 if( !!paddr->getSIDREF() ) {
2082 return daeSidRef(paddr->getSIDREF()->getValue(),parent).
resolve().elt;
2084 if (!!paddr->getParam()) {
2085 return searchBinding(paddr->getParam()->getValue(),parent);
2098 daeElement* pelt = NULL;
2099 domKinematics_sceneRef kscene = daeSafeCast<domKinematics_scene>(parent.cast());
2101 pelt = searchBindingArray(ref,kscene->getInstance_articulated_system_array());
2105 return searchBindingArray(ref,kscene->getInstance_kinematics_model_array());
2107 domArticulated_systemRef articulated_system = daeSafeCast<domArticulated_system>(parent.cast());
2108 if( !!articulated_system ) {
2109 if( !!articulated_system->getKinematics() ) {
2110 pelt = searchBindingArray(ref,articulated_system->getKinematics()->getInstance_kinematics_model_array());
2115 if( !!articulated_system->getMotion() ) {
2116 return searchBinding(ref,articulated_system->getMotion()->getInstance_articulated_system());
2121 daeElementRef pbindelt;
2122 const domKinematics_bind_Array* pbindarray = NULL;
2123 const domKinematics_newparam_Array* pnewparamarray = NULL;
2124 domInstance_articulated_systemRef ias = daeSafeCast<domInstance_articulated_system>(parent.cast());
2126 pbindarray = &ias->getBind_array();
2127 pbindelt = ias->getUrl().getElement();
2128 pnewparamarray = &ias->getNewparam_array();
2130 if( !pbindarray || !pbindelt ) {
2131 domInstance_kinematics_modelRef ikm = daeSafeCast<domInstance_kinematics_model>(parent.cast());
2133 pbindarray = &ikm->getBind_array();
2134 pbindelt = ikm->getUrl().getElement();
2135 pnewparamarray = &ikm->getNewparam_array();
2138 if( !!pbindarray && !!pbindelt ) {
2139 for (
size_t ibind = 0; ibind < pbindarray->getCount(); ++ibind) {
2140 domKinematics_bindRef pbind = (*pbindarray)[ibind];
2141 if( !!pbind->getSymbol() && strcmp(pbind->getSymbol(), ref) == 0 ) {
2143 if( !!pbind->getParam() ) {
2144 return daeSidRef(pbind->getParam()->getRef(), pbindelt).
resolve().elt;
2146 else if( !!pbind->getSIDREF() ) {
2147 return daeSidRef(pbind->getSIDREF()->getValue(), pbindelt).
resolve().elt;
2151 for(
size_t inewparam = 0; inewparam < pnewparamarray->getCount(); ++inewparam) {
2152 domKinematics_newparamRef newparam = (*pnewparamarray)[inewparam];
2153 if( !!newparam->getSid() && strcmp(newparam->getSid(), ref) == 0 ) {
2154 if( !!newparam->getSIDREF() ) {
2155 return daeSidRef(newparam->getSIDREF()->getValue(),pbindelt).
resolve().elt;
2157 ROS_WARN_STREAM(str(boost::format(
"newparam sid=%s does not have SIDREF\n")%newparam->getSid()));
2161 ROS_WARN_STREAM(str(boost::format(
"failed to get binding '%s' for element: %s\n")%ref%parent->getElementName()));
2165 static daeElement*
searchBindingArray(daeString ref,
const domInstance_articulated_system_Array& paramArray)
2167 for(
size_t iikm = 0; iikm < paramArray.getCount(); ++iikm) {
2168 daeElement* pelt = searchBinding(ref,paramArray[iikm].cast());
2176 static daeElement*
searchBindingArray(daeString ref,
const domInstance_kinematics_model_Array& paramArray)
2178 for(
size_t iikm = 0; iikm < paramArray.getCount(); ++iikm) {
2179 daeElement* pelt = searchBinding(ref,paramArray[iikm].cast());
2187 template <
typename U>
static xsBoolean
resolveBool(domCommon_bool_or_paramRef paddr,
const U& parent) {
2188 if( !!paddr->getBool() ) {
2189 return paddr->getBool()->getValue();
2191 if( !paddr->getParam() ) {
2195 for(
size_t iparam = 0; iparam < parent->getNewparam_array().getCount(); ++iparam) {
2196 domKinematics_newparamRef pnewparam = parent->getNewparam_array()[iparam];
2197 if( !!pnewparam->getSid() && strcmp(pnewparam->getSid(), paddr->getParam()->getValue()) == 0 ) {
2198 if( !!pnewparam->getBool() ) {
2199 return pnewparam->getBool()->getValue();
2201 else if( !!pnewparam->getSIDREF() ) {
2202 domKinematics_newparam::domBoolRef ptarget = daeSafeCast<domKinematics_newparam::domBool>(daeSidRef(pnewparam->getSIDREF()->getValue(), pnewparam).
resolve().elt);
2204 ROS_WARN(
"failed to resolve %s from %s\n", pnewparam->getSIDREF()->getValue(), paddr->getID());
2207 return ptarget->getValue();
2211 ROS_WARN_STREAM(str(boost::format(
"failed to resolve %s\n")%paddr->getParam()->getValue()));
2214 template <
typename U>
static domFloat
resolveFloat(domCommon_float_or_paramRef paddr,
const U& parent) {
2215 if( !!paddr->getFloat() ) {
2216 return paddr->getFloat()->getValue();
2218 if( !paddr->getParam() ) {
2222 for(
size_t iparam = 0; iparam < parent->getNewparam_array().getCount(); ++iparam) {
2223 domKinematics_newparamRef pnewparam = parent->getNewparam_array()[iparam];
2224 if( !!pnewparam->getSid() && strcmp(pnewparam->getSid(), paddr->getParam()->getValue()) == 0 ) {
2225 if( !!pnewparam->getFloat() ) {
2226 return pnewparam->getFloat()->getValue();
2228 else if( !!pnewparam->getSIDREF() ) {
2229 domKinematics_newparam::domFloatRef ptarget = daeSafeCast<domKinematics_newparam::domFloat>(daeSidRef(pnewparam->getSIDREF()->getValue(), pnewparam).
resolve().elt);
2231 ROS_WARN(
"failed to resolve %s from %s\n", pnewparam->getSIDREF()->getValue(), paddr->getID());
2234 return ptarget->getValue();
2238 ROS_WARN_STREAM(str(boost::format(
"failed to resolve %s\n")%paddr->getParam()->getValue()));
2244 daeElement* pfloat = pcommon->getChild(
"float");
2246 std::stringstream sfloat(pfloat->getCharData());
2250 daeElement* pparam = pcommon->getChild(
"param");
2252 if( pparam->hasAttribute(
"ref") ) {
2256 daeElement* pelt = daeSidRef(pparam->getCharData(),parent).
resolve().elt;
2258 ROS_WARN_STREAM(str(boost::format(
"found param ref: %s from %s\n")%pelt->getCharData()%pparam->getCharData()));
2267 boost::array<double,12> m = {{1,0,0,0,0,1,0,0,0,0,1,0}};
2274 boost::array<double,12> m = _matrixIdentity();
2275 domRotateRef protate = daeSafeCast<domRotate>(pelt);
2277 m = _matrixFromAxisAngle(Vector3(protate->getValue()[0],protate->getValue()[1],protate->getValue()[2]), (
double)(protate->getValue()[3]*(M_PI/180.0)));
2281 domTranslateRef ptrans = daeSafeCast<domTranslate>(pelt);
2283 double scale = _GetUnitScale(pelt);
2284 m[3] = ptrans->getValue()[0]*scale;
2285 m[7] = ptrans->getValue()[1]*scale;
2286 m[11] = ptrans->getValue()[2]*scale;
2290 domMatrixRef pmat = daeSafeCast<domMatrix>(pelt);
2292 double scale = _GetUnitScale(pelt);
2293 for(
int i = 0; i < 3; ++i) {
2294 m[4*i+0] = pmat->getValue()[4*i+0];
2295 m[4*i+1] = pmat->getValue()[4*i+1];
2296 m[4*i+2] = pmat->getValue()[4*i+2];
2297 m[4*i+3] = pmat->getValue()[4*i+3]*scale;
2302 domScaleRef pscale = daeSafeCast<domScale>(pelt);
2304 m[0] = pscale->getValue()[0];
2305 m[4*1+1] = pscale->getValue()[1];
2306 m[4*2+2] = pscale->getValue()[2];
2310 domLookatRef pcamera = daeSafeCast<domLookat>(pelt);
2311 if( pelt->typeID() == domLookat::ID() ) {
2316 domSkewRef pskew = daeSafeCast<domSkew>(pelt);
2327 domNodeRef pnode = daeSafeCast<domNode>(pelt->getParent());
2329 return _matrixIdentity();
2331 return _poseMult(_getNodeParentTransform(pnode), _ExtractFullTransform(pnode));
2336 boost::array<double,12> t = _matrixIdentity();
2337 for(
size_t i = 0; i < pelt->getContents().getCount(); ++i) {
2338 t = _poseMult(t, _getTransform(pelt->getContents()[i]));
2345 boost::array<double,12> t = _matrixIdentity();
2346 daeTArray<daeElementRef> children; pelt->getChildren(children);
2347 for(
size_t i = 0; i < children.getCount(); ++i) {
2348 t = _poseMult(t, _getTransform(children[i]));
2354 void _decompose(
const boost::array<double,12>& tm, Pose& tout, Vector3& vscale)
2356 vscale.x = 1; vscale.y = 1; vscale.z = 1;
2357 tout = _poseFromMatrix(tm);
2367 ROS_WARN(
"COLLADA warning: %s\n", msg);
2372 return ((
USERDATA*)pelt->getUserData())->scale;
2377 for(
size_t i = 0; i < arr.getCount(); ++i) {
2378 if( strcmp(arr[i]->getProfile(),
"OpenRAVE") == 0 ) {
2382 return domTechniqueRef();
2387 for(
size_t i = 0; i < arr.getCount(); ++i) {
2388 if( strcmp(arr[i]->getType(),
"interface_type") == 0 ) {
2389 domTechniqueRef tec = _ExtractOpenRAVEProfile(arr[i]->getTechnique_array());
2391 daeElement* ptype = tec->getChild(
"interface");
2402 std::string linkname;
2404 if( !!pdomlink->getName() ) {
2405 linkname = pdomlink->getName();
2407 if( linkname.size() == 0 && !!pdomlink->getID() ) {
2408 linkname = pdomlink->getID();
2416 if( pelt->getElementName()==type ) {
2420 std::string name = pelt->getElementName();
2421 std::size_t pos = name.find_last_of(
':');
2422 if( pos == std::string::npos ) {
2425 return name.substr(pos+1)==type;
2429 daeElement* peltjoint = daeSidRef(targetref, peltref).resolve().elt;
2430 domJointRef pdomjoint = daeSafeCast<domJoint> (peltjoint);
2433 domInstance_jointRef pdomijoint = daeSafeCast<domInstance_joint> (peltjoint);
2435 pdomjoint = daeSafeCast<domJoint> (pdomijoint->getUrl().getElement().cast());
2439 if (!pdomjoint || pdomjoint->typeID() != domJoint::ID() || !pdomjoint->getName()) {
2440 ROS_WARN_STREAM(str(boost::format(
"could not find collada joint %s!\n")%targetref));
2441 return urdf::JointSharedPtr();
2444 urdf::JointSharedPtr pjoint;
2445 std::string name(pdomjoint->getName());
2446 if (_model->joints_.find(name) == _model->joints_.end()) {
2449 pjoint = _model->joints_.find(name)->second;
2452 ROS_WARN_STREAM(str(boost::format(
"could not find openrave joint %s!\n")%pdomjoint->getName()));
2462 domKinematics_sceneRef kscene = daeSafeCast<domKinematics_scene> (kiscene->getUrl().getElement().cast());
2466 for (
size_t imodel = 0; imodel < kiscene->getBind_kinematics_model_array().getCount(); imodel++) {
2467 domArticulated_systemRef articulated_system;
2468 domBind_kinematics_modelRef kbindmodel = kiscene->getBind_kinematics_model_array()[imodel];
2469 if (!kbindmodel->getNode()) {
2470 ROS_WARN_STREAM(
"do not support kinematics models without references to nodes\n");
2475 domNodeRef node = daeSafeCast<domNode>(daeSidRef(kbindmodel->getNode(), viscene->getUrl().getElement()).
resolve().elt);
2477 ROS_WARN_STREAM(str(boost::format(
"bind_kinematics_model does not reference valid node %s\n")%kbindmodel->getNode()));
2482 daeElement* pelt = searchBinding(kbindmodel,kscene);
2483 domInstance_kinematics_modelRef kimodel = daeSafeCast<domInstance_kinematics_model>(pelt);
2486 ROS_WARN_STREAM(
"bind_kinematics_model does not reference element\n");
2489 ROS_WARN_STREAM(str(boost::format(
"bind_kinematics_model cannot find reference to %s:\n")%pelt->getElementName()));
2496 for (
size_t ijoint = 0; ijoint < kiscene->getBind_joint_axis_array().getCount(); ++ijoint) {
2497 domBind_joint_axisRef bindjoint = kiscene->getBind_joint_axis_array()[ijoint];
2498 daeElementRef pjtarget = daeSidRef(bindjoint->getTarget(), viscene->getUrl().getElement()).
resolve().elt;
2500 ROS_ERROR_STREAM(str(boost::format(
"Target Node %s NOT found!!!\n")%bindjoint->getTarget()));
2503 daeElement* pelt = searchBinding(bindjoint->getAxis(),kscene);
2504 domAxis_constraintRef pjointaxis = daeSafeCast<domAxis_constraint>(pelt);
2514 for(
size_t iphysics = 0; iphysics < allscene->getInstance_physics_scene_array().getCount(); ++iphysics) {
2515 domPhysics_sceneRef pscene = daeSafeCast<domPhysics_scene>(allscene->getInstance_physics_scene_array()[iphysics]->getUrl().getElement().cast());
2516 for(
size_t imodel = 0; imodel < pscene->getInstance_physics_model_array().getCount(); ++imodel) {
2517 domInstance_physics_modelRef ipmodel = pscene->getInstance_physics_model_array()[imodel];
2518 domPhysics_modelRef pmodel = daeSafeCast<domPhysics_model> (ipmodel->getUrl().getElement().cast());
2519 domNodeRef nodephysicsoffset = daeSafeCast<domNode>(ipmodel->getParent().getElement().cast());
2520 for(
size_t ibody = 0; ibody < ipmodel->getInstance_rigid_body_array().getCount(); ++ibody) {
2522 lb.
irigidbody = ipmodel->getInstance_rigid_body_array()[ibody];
2523 lb.
node = daeSafeCast<domNode>(lb.
irigidbody->getTarget().getElement().cast());
2537 daeTArray<daeElementRef> children;
2538 pelt->getChildren(children);
2539 for (
size_t i = 0; i < children.getCount(); ++i) {
2540 c += _countChildren(children[i]);
2548 domAssetRef passet = daeSafeCast<domAsset> (pelt->getChild(
"asset"));
2549 if (!!passet && !!passet->getUnit()) {
2550 scale = passet->getUnit()->getMeter();
2553 _vuserdata.push_back(
USERDATA(scale));
2554 pelt->setUserData(&_vuserdata.back());
2555 daeTArray<daeElementRef> children;
2556 pelt->getChildren(children);
2557 for (
size_t i = 0; i < children.getCount(); ++i) {
2558 if (children[i] != passet) {
2559 _processUserData(children[i], scale);
2566 BOOST_ASSERT(!!pelt);
2567 void* p = pelt->getUserData();
2578 double ww = 2 * p.rotation.x * p.rotation.x;
2579 double wx = 2 * p.rotation.x * p.rotation.y;
2580 double wy = 2 * p.rotation.x * p.rotation.z;
2581 double wz = 2 * p.rotation.x * p.rotation.w;
2582 double xx = 2 * p.rotation.y * p.rotation.y;
2583 double xy = 2 * p.rotation.y * p.rotation.z;
2584 double xz = 2 * p.rotation.y * p.rotation.w;
2585 double yy = 2 * p.rotation.z * p.rotation.z;
2586 double yz = 2 * p.rotation.z * p.rotation.w;
2588 vnew.x = (1-xx-yy) * v.x + (wx-yz) * v.y + (wy+xz)*v.z + p.position.x;
2589 vnew.y = (wx+yz) * v.x + (1-ww-yy) * v.y + (xy-wz)*v.z + p.position.y;
2590 vnew.z = (wy-xz) * v.x + (xy+wz) * v.y + (1-ww-xx)*v.z + p.position.z;
2594 static Vector3
_poseMult(
const boost::array<double,12>& m,
const Vector3& v)
2597 vnew.x = m[4*0+0] * v.x + m[4*0+1] * v.y + m[4*0+2] * v.z + m[4*0+3];
2598 vnew.y = m[4*1+0] * v.x + m[4*1+1] * v.y + m[4*1+2] * v.z + m[4*1+3];
2599 vnew.z = m[4*2+0] * v.x + m[4*2+1] * v.y + m[4*2+2] * v.z + m[4*2+3];
2603 static boost::array<double,12>
_poseMult(
const boost::array<double,12>& m0,
const boost::array<double,12>& m1)
2605 boost::array<double,12> mres;
2606 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];
2607 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];
2608 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];
2609 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];
2610 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];
2611 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];
2612 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];
2613 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];
2614 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];
2615 mres[3] = m1[3] * m0[0] + m1[7] * m0[1] + m1[11] * m0[2] + m0[3];
2616 mres[7] = m1[3] * m0[4] + m1[7] * m0[5] + m1[11] * m0[6] + m0[7];
2617 mres[11] = m1[3] * m0[8] + m1[7] * m0[9] + m1[11] * m0[10] + m0[11];
2624 p.position = _poseMult(p0,p1.position);
2625 p.rotation = _quatMult(p0.rotation,p1.rotation);
2632 pinv.rotation.x = -p.rotation.x;
2633 pinv.rotation.y = -p.rotation.y;
2634 pinv.rotation.z = -p.rotation.z;
2635 pinv.rotation.w = p.rotation.w;
2636 Vector3 t = _poseMult(pinv,p.position);
2637 pinv.position.x = -t.x;
2638 pinv.position.y = -t.y;
2639 pinv.position.z = -t.z;
2643 static Rotation
_quatMult(
const Rotation& quat0,
const Rotation& quat1)
2646 q.x = quat0.w*quat1.x + quat0.x*quat1.w + quat0.y*quat1.z - quat0.z*quat1.y;
2647 q.y = quat0.w*quat1.y + quat0.y*quat1.w + quat0.z*quat1.x - quat0.x*quat1.z;
2648 q.z = quat0.w*quat1.z + quat0.z*quat1.w + quat0.x*quat1.y - quat0.y*quat1.x;
2649 q.w = quat0.w*quat1.w - quat0.x*quat1.x - quat0.y*quat1.y - quat0.z*quat1.z;
2650 double fnorm = std::sqrt(q.x*q.x+q.y*q.y+q.z*q.z+q.w*q.w);
2661 return _matrixFromQuat(_quatFromAxisAngle(axis.x,axis.y,axis.z,angle));
2666 boost::array<double,12> m;
2667 double qq1 = 2*quat.x*quat.x;
2668 double qq2 = 2*quat.y*quat.y;
2669 double qq3 = 2*quat.z*quat.z;
2670 m[4*0+0] = 1 - qq2 - qq3;
2671 m[4*0+1] = 2*(quat.x*quat.y - quat.w*quat.z);
2672 m[4*0+2] = 2*(quat.x*quat.z + quat.w*quat.y);
2674 m[4*1+0] = 2*(quat.x*quat.y + quat.w*quat.z);
2675 m[4*1+1] = 1 - qq1 - qq3;
2676 m[4*1+2] = 2*(quat.y*quat.z - quat.w*quat.x);
2678 m[4*2+0] = 2*(quat.x*quat.z - quat.w*quat.y);
2679 m[4*2+1] = 2*(quat.y*quat.z + quat.w*quat.x);
2680 m[4*2+2] = 1 - qq1 - qq2;
2688 t.rotation = _quatFromMatrix(m);
2689 t.position.x = m[3];
2690 t.position.y = m[7];
2691 t.position.z = m[11];
2697 boost::array<double,12> m = _matrixFromQuat(t.rotation);
2698 m[3] = t.position.x;
2699 m[7] = t.position.y;
2700 m[11] = t.position.z;
2707 double axislen = std::sqrt(x*x+y*y+z*z);
2708 if( axislen == 0 ) {
2712 double sang = std::sin(angle)/axislen;
2713 q.w = std::cos(angle);
2723 double tr = mat[4*0+0] + mat[4*1+1] + mat[4*2+2];
2726 rot.x = (mat[4*2+1] - mat[4*1+2]);
2727 rot.y = (mat[4*0+2] - mat[4*2+0]);
2728 rot.z = (mat[4*1+0] - mat[4*0+1]);
2732 if (mat[4*1+1] > mat[4*0+0]) {
2733 if (mat[4*2+2] > mat[4*1+1]) {
2734 rot.z = (mat[4*2+2] - (mat[4*0+0] + mat[4*1+1])) + 1;
2735 rot.x = (mat[4*2+0] + mat[4*0+2]);
2736 rot.y = (mat[4*1+2] + mat[4*2+1]);
2737 rot.w = (mat[4*1+0] - mat[4*0+1]);
2740 rot.y = (mat[4*1+1] - (mat[4*2+2] + mat[4*0+0])) + 1;
2741 rot.z = (mat[4*1+2] + mat[4*2+1]);
2742 rot.x = (mat[4*0+1] + mat[4*1+0]);
2743 rot.w = (mat[4*0+2] - mat[4*2+0]);
2746 else if (mat[4*2+2] > mat[4*0+0]) {
2747 rot.z = (mat[4*2+2] - (mat[4*0+0] + mat[4*1+1])) + 1;
2748 rot.x = (mat[4*2+0] + mat[4*0+2]);
2749 rot.y = (mat[4*1+2] + mat[4*2+1]);
2750 rot.w = (mat[4*1+0] - mat[4*0+1]);
2753 rot.x = (mat[4*0+0] - (mat[4*1+1] + mat[4*2+2])) + 1;
2754 rot.y = (mat[4*0+1] + mat[4*1+0]);
2755 rot.z = (mat[4*2+0] + mat[4*0+2]);
2756 rot.w = (mat[4*2+1] - mat[4*1+2]);
2759 double fnorm = std::sqrt(rot.x*rot.x+rot.y*rot.y+rot.z*rot.z+rot.w*rot.w);
2768 static double _dot3(
const Vector3& v0,
const Vector3& v1)
2770 return v0.x*v1.x + v0.y*v1.y + v0.z*v1.z;
2772 static Vector3
_cross3(
const Vector3& v0,
const Vector3& v1)
2775 v.x = v0.y * v1.z - v0.z * v1.y;
2776 v.y = v0.z * v1.x - v0.x * v1.z;
2777 v.z = v0.x * v1.y - v0.y * v1.x;
2780 static Vector3
_sub3(
const Vector3& v0,
const Vector3& v1)
2788 static Vector3
_add3(
const Vector3& v0,
const Vector3& v1)
2799 double norm = std::sqrt(v0.x*v0.x+v0.y*v0.y+v0.z*v0.z);
2822 urdf::ModelInterfaceSharedPtr model(
new ModelInterface);
static Pose _poseMult(const Pose &p0, const Pose &p1)
domRigid_bodyRef rigidbody
boost::shared_ptr< std::string > _ExtractInterfaceType(const domExtra_Array &arr)
returns an openrave interface type from the extra array
static void _ExtractPhysicsBindings(domCOLLADA::domSceneRef allscene, KinematicsSceneBindings &bindings)
static boost::array< double, 12 > _ExtractFullTransformFromChildren(const T pelt)
Travel by the transformation array and calls the _getTransform method.
bool InitFromData(const std::string &pdata)
std::string _ExtractLinkName(domLinkRef pdomlink)
urdf::JointSharedPtr _getJointFromRef(xsToken targetref, daeElementRef peltref)
std::vector< USERDATA > _vuserdata
void _ExtractRobotManipulators(const domArticulated_systemRef as)
extract the robot manipulators
void _FillGeometryColor(const domMaterialRef pmat, GEOMPROPERTIES &geom)
domMotion_axis_infoRef motion_axis_info
bool InitFromFile(const std::string &filename)
std::vector< int > indices
discretization value is chosen. Should be transformed by _t before rendering
virtual ~ColladaModelReader()
static Vector3 _poseMult(const Pose &p, const Vector3 &v)
daeElementRef pvisualtrans
bool AddAxisInfo(const domInstance_kinematics_model_Array &arr, domKinematics_axis_infoRef kinematics_axis_info, domMotion_axis_infoRef motion_axis_info)
static boost::array< double, 12 > _poseMult(const boost::array< double, 12 > &m0, const boost::array< double, 12 > &m1)
static xsBoolean resolveBool(domCommon_bool_or_paramRef paddr, const U &parent)
static Pose _poseFromMatrix(const boost::array< double, 12 > &m)
static void GenerateSphereTriangulation(std::vector< Vector3 > realvertices, std::vector< int > realindices, int levels)
static boost::array< double, 12 > _getTransform(daeElementRef pelt)
Gets all transformations applied to the node.
ROSCPP_DECL std::string resolve(const std::string &name, bool remap=true)
GeomType type
the type of geometry primitive
domKinematics_axis_infoRef kinematics_axis_info
static boost::array< double, 12 > _matrixFromQuat(const Rotation &quat)
virtual void handleError(daeString msg)
static domFloat resolveFloat(domCommon_float_or_paramRef paddr, const U &parent)
static Vector3 _normalize3(const Vector3 &v0)
std::list< LinkBinding > listLinkBindings
static daeElement * searchBinding(domCommon_sidref_or_paramRef paddr, daeElementRef parent)
static boost::array< double, 12 > _matrixFromPose(const Pose &t)
bool InitCollisionMesh(double fTessellation=1.0)
static Vector3 _add3(const Vector3 &v0, const Vector3 &v1)
domAxis_constraintRef pkinematicaxis
static bool resolveCommon_float_or_param(daeElementRef pcommon, daeElementRef parent, float &f)
static boost::array< double, 12 > _ExtractFullTransform(const T pelt)
Travel by the transformation array and calls the _getTransform method.
boost::shared_ptr< void > p
custom managed data
domTechniqueRef _ExtractOpenRAVEProfile(const domTechnique_Array &arr)
urdf::ModelInterfaceSharedPtr _model
bool _ExtractGeometry(const domTristripsRef triRef, const domVerticesRef vertsRef, const std::map< std::string, domMaterialRef > &mapmaterials, std::list< GEOMPROPERTIES > &listGeomProperties)
static Vector3 _sub3(const Vector3 &v0, const Vector3 &v1)
static Rotation _quatFromAxisAngle(double x, double y, double z, double angle)
UnlinkFilename(const std::string &filename)
bool _ExtractGeometry(const domPolylistRef triRef, const domVerticesRef vertsRef, const std::map< std::string, domMaterialRef > &mapmaterials, std::list< GEOMPROPERTIES > &listGeomProperties)
bool _ExtractGeometry(const domTrifansRef triRef, const domVerticesRef vertsRef, const std::map< std::string, domMaterialRef > &mapmaterials, std::list< GEOMPROPERTIES > &listGeomProperties)
static boost::array< double, 12 > _matrixIdentity()
void _ExtractRobotAttachedActuators(const domArticulated_systemRef as)
extract the robot actuators
static Vector3 _poseMult(const boost::array< double, 12 > &m, const Vector3 &v)
static Vector3 _cross3(const Vector3 &v0, const Vector3 &v1)
urdf::ModelInterfaceSharedPtr parseCollada(const std::string &xml_string)
Load Model from string.
USERDATA * _getUserData(daeElement *pelt)
static Rotation _quatMult(const Rotation &quat0, const Rotation &quat1)
domNodeRef nodephysicsoffset
static double _GetUnitScale(daeElement *pelt)
virtual ~UnlinkFilename()
void _processUserData(daeElement *pelt, double scale)
Pose _t
local transformation of the geom primitive with respect to the link's coordinate system ...
static double _dot3(const Vector3 &v0, const Vector3 &v1)
domCommon_float_or_paramRef jointvalue
static Rotation _quatFromMatrix(const boost::array< double, 12 > &mat)
#define ROS_WARN_STREAM(args)
#define ROS_DEBUG_STREAM(args)
static boost::array< double, 12 > _matrixFromAxisAngle(const Vector3 &axis, double angle)
daeElementRef _getElementFromUrl(const daeURI &uri)
void _ExtractRobotAttachedSensors(const domArticulated_systemRef as)
Extract Sensors attached to a Robot.
static boost::array< double, 12 > _getNodeParentTransform(const T pelt)
boost::shared_ptr< DAE > _collada
bool _Extract()
the first possible robot in the scene
ColladaModelReader(urdf::ModelInterfaceSharedPtr model)
std::list< JointAxisBinding > listAxisBindings
bindings for links between different spaces
bool _checkMathML(daeElementRef pelt, const std::string &type)
JointAxisBinding(daeElementRef pvisualtrans, domAxis_constraintRef pkinematicaxis, domCommon_float_or_paramRef jointvalue, domKinematics_axis_infoRef kinematics_axis_info, domMotion_axis_infoRef motion_axis_info)
bool _ExtractGeometry(const domTrianglesRef triRef, const domVerticesRef vertsRef, const std::map< std::string, domMaterialRef > &mapmaterials, std::list< GEOMPROPERTIES > &listGeomProperties)
bool _ExtractGeometry(const domGeometryRef geom, const std::map< std::string, domMaterialRef > &mapmaterials, std::list< GEOMPROPERTIES > &listGeomProperties)
static daeElement * searchBindingArray(daeString ref, const domInstance_kinematics_model_Array ¶mArray)
#define ROS_INFO_STREAM(args)
bool _ExtractKinematicsModel(domKinematics_modelRef kmodel, domNodeRef pnode, domPhysics_modelRef pmodel, const KinematicsSceneBindings &bindings)
append the kinematics model to the openrave kinbody
urdf::LinkSharedPtr _ExtractLink(const domLinkRef pdomlink, const domNodeRef pdomnode, const Pose &tParentWorldLink, const Pose &tParentLink, const std::vector< domJointRef > &vdomjoints, const KinematicsSceneBindings &bindings)
Extract Link info and add it to an existing body.
bool _ExtractArticulatedSystem(domInstance_articulated_systemRef ias, KinematicsSceneBindings &bindings)
extracts an articulated system. Note that an articulated system can include other articulated systems...
static daeElement * searchBindingArray(daeString ref, const domInstance_articulated_system_Array ¶mArray)
void _ExtractGeometry(const domNodeRef pdomnode, std::list< GEOMPROPERTIES > &listGeomProperties, const std::list< JointAxisBinding > &listAxisBindings, const Pose &tlink)
Color ambientColor
hints for how to color the meshes
#define PRINT_POSE(pname, apose)
void _decompose(const boost::array< double, 12 > &tm, Pose &tout, Vector3 &vscale)
virtual void handleWarning(daeString msg)
std::vector< Vector3 > vertices
inter-collada bindings for a kinematics scene
#define ROS_ERROR_STREAM(args)
static daeElement * searchBinding(daeString ref, daeElementRef parent)
bool _ExtractKinematicsModel(domInstance_kinematics_modelRef ikm, KinematicsSceneBindings &bindings)
static std::list< boost::shared_ptr< UnlinkFilename > > _listTempFilenames
static Pose _poseInverse(const Pose &p)
size_t _countChildren(daeElement *pelt)
static void _ExtractKinematicsVisualBindings(domInstance_with_extraRef viscene, domInstance_kinematics_sceneRef kiscene, KinematicsSceneBindings &bindings)
go through all kinematics binds to get a kinematics/visual pair
domInstance_rigid_bodyRef irigidbody
std::list< std::pair< domNodeRef, domInstance_kinematics_modelRef > > listKinematicsVisualBindings
urdf::GeometrySharedPtr _CreateGeometry(const std::string &name, const std::list< GEOMPROPERTIES > &listGeomProperties)