diff --git a/collada_urdf/annotate_pr2dae.py b/collada_urdf/annotate_pr2dae.py new file mode 100755 index 0000000..1d62095 --- /dev/null +++ b/collada_urdf/annotate_pr2dae.py @@ -0,0 +1,368 @@ +#!/usr/bin/python +# -*- coding: utf-8 -*- +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +import sys + +if __name__=="__main__": + if len(sys.argv) < 2: + print "Usage:\n\tannotate_pr2dae.py [collada file]\n\nAnnoates the PR2 with the OpenRAVE profile tags.\nThis file will be in existence until URDF can support the extra information providedhere" + sys.exit(0) + + dae = open(sys.argv[1],'r').read() + if dae.find('') < 0: + index = dae.find('') + dae = dae[:index] + """ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +""" + dae[index:] + + if dae.find('') + dae = dae[:index] + """ + + + + -130 129.75 + 0.023 60.0 + 0.25 + 1.73611115315e-05 + 0.05 + + + BaseLaser2D + + + + + -90 89.75 + 0.023 10.0 + 0.25 + 1.73611115315e-05 + 0.025 + + + BaseLaser2D + + + + + 2448 2050 3 + uint8 + 0.05 + 2955 0 1224.5 0 2955 1025.5 + 0 0 0 0 0 + + + BaseCamera + + + + + 640 480 1 + uint8 + 0.04 + 395.71449999999999 0.0 335.86279000000002 0.0 395.71449999999999 245.62557000000001 + 0 0 0 0 0 + + + BaseCamera + + + + + 640 480 1 + uint8 + 0.04 + 395.71449999999999 0.0 335.86279000000002 0.0 395.71449999999999 245.62557000000001 + 0 0 0 0 0 + + + BaseCamera + + + + + + 0.99956000000000012 0.0027200000000000002 -0.029390000000000003 -0.0027700000000000003 0.99999000000000005 -0.0016800000000000001 0.029390000000000003 0.0017600000000000001 0.99957000000000007 + + + 0.99941000000000013 0.0035200000000000001 -0.034260000000000006 -0.0034600000000000004 0.99999000000000005 0.0017800000000000001 0.034270000000000002 -0.0016600000000000002 0.99941000000000013 + + + + 640 480 1 + uint8 + 0.04 + 426.35142000000002 0.0 313.91464000000002 0.0 426.51092999999997 238.27394000000001 + 0 0 0 0 0 + + + BaseCamera + + + + + 640 480 1 + uint8 + 0.04 + 430.5514 0.0 320.85068000000001 0.0 429.22170999999997 240.4314 + 0 0 0 0 0 + + + BaseCamera + + + + + +""" + dae[index:] + + manipulators = [('leftarm',""" + + + + + 0.18 0 0 + 0 1 0 90 + + + + -1 + + + + + + + ikfast_pr2_leftarm + + + + + +"""), + ('leftarm_torso',""" + + + + + 0.18 0 0 + 0 1 0 90 + + + + -1 + + + + + + + + ikfast_pr2_leftarm_torso + + + + + +"""), + ('rightarm',""" + + + + + 0.18 0 0 + 0 1 0 90 + + + + -1 + + + + + + + ikfast_pr2_rightarm + + + + + +"""), + ('rightarm_torso',""" + + + + + 0.18 0 0 + 0 1 0 90 + + + + -1 + + + + + + + + ikfast_pr2_rightarm_torso + + + + + +"""), + ('head',""" + + + + + + + + ikfast_pr2_head + + + + + +"""), + ('head_torso',""" + + + + + + + + + ikfast_pr2_head_torso + + + + + +"""), + ('rightarm_camera',""" + + + + + + + + + +"""), + ('leftarm_camera',""" + + + + + + + + + +""")] + sensors = [('base_laser',""" + + + + + + +"""), + ('tilt_laser',""" + + + + + + +"""), + ('l_forearm_cam_optical_sensor',""" + + + + + + +"""), + ('r_forearm_cam_optical_sensor',""" + + + + + + +"""), + ('narrow_stereo_optical_sensor',""" + + + + + + +"""), + ('wide_stereo_optical_sensor',""" + + + + + + +""")] + for name,xml in manipulators: + if dae.find(''%name) < 0: + index = dae.find('')+9 + dae = dae[:index] + xml + dae[index:] + for name,xml in sensors: + if dae.find(''%name) < 0: + index = dae.find('')+9 + dae = dae[:index] + xml + dae[index:] + open(sys.argv[1],'w').write(dae) diff --git a/collada_urdf/src/collada_urdf.cpp b/collada_urdf/src/collada_urdf.cpp index 1fd163b..2671187 100644 --- a/collada_urdf/src/collada_urdf.cpp +++ b/collada_urdf/src/collada_urdf.cpp @@ -469,34 +469,34 @@ protected: } // write the bindings - string asmsym = str(boost::format("%s.%s")%asmid%_ikmout->ikm->getSid()); - string assym = str(boost::format("%s.%s")%_scene.kscene->getID()%_ikmout->ikm->getSid()); + string asmsym = str(boost::format("%s_%s")%asmid%_ikmout->ikm->getSid()); + string assym = str(boost::format("%s_%s")%_scene.kscene->getID()%_ikmout->ikm->getSid()); FOREACH(it, _ikmout->vkinematicsbindings) { domKinematics_newparamRef abm = daeSafeCast(ias_motion->add(COLLADA_ELEMENT_NEWPARAM)); abm->setSid(asmsym.c_str()); - daeSafeCast(abm->add(COLLADA_ELEMENT_SIDREF))->setValue(it->first.c_str()); + daeSafeCast(abm->add(COLLADA_ELEMENT_SIDREF))->setValue(str(boost::format("%s/%s")%askid%it->first).c_str()); domKinematics_bindRef ab = daeSafeCast(ias->add(COLLADA_ELEMENT_BIND)); ab->setSymbol(assym.c_str()); - daeSafeCast(ab->add(COLLADA_ELEMENT_PARAM))->setRef(asmsym.c_str()); + daeSafeCast(ab->add(COLLADA_ELEMENT_PARAM))->setRef(str(boost::format("%s/%s")%asmid%asmsym).c_str()); _iasout->vkinematicsbindings.push_back(make_pair(string(ab->getSymbol()), it->second)); } for(size_t idof = 0; idof < _ikmout->vaxissids.size(); ++idof) { const axis_sids& kas = _ikmout->vaxissids.at(idof); domKinematics_newparamRef abm = daeSafeCast(ias_motion->add(COLLADA_ELEMENT_NEWPARAM)); - abm->setSid(str(boost::format("%s.%s")%asmid%kas.axissid).c_str()); - daeSafeCast(abm->add(COLLADA_ELEMENT_SIDREF))->setValue(kas.axissid.c_str()); + abm->setSid(str(boost::format("%s_%s")%asmid%kas.axissid).c_str()); + daeSafeCast(abm->add(COLLADA_ELEMENT_SIDREF))->setValue(str(boost::format("%s/%s")%askid%kas.axissid).c_str()); domKinematics_bindRef ab = daeSafeCast(ias->add(COLLADA_ELEMENT_BIND)); - ab->setSymbol(str(boost::format("%s.%s")%assym%kas.axissid).c_str()); - daeSafeCast(ab->add(COLLADA_ELEMENT_PARAM))->setRef(str(boost::format("%s.%s")%asmid%kas.axissid).c_str()); + ab->setSymbol(str(boost::format("%s_%s")%assym%kas.axissid).c_str()); + daeSafeCast(ab->add(COLLADA_ELEMENT_PARAM))->setRef(str(boost::format("%s/%s_%s")%asmid%asmid%kas.axissid).c_str()); string valuesid; if( kas.valuesid.size() > 0 ) { domKinematics_newparamRef abmvalue = daeSafeCast(ias_motion->add(COLLADA_ELEMENT_NEWPARAM)); - abmvalue->setSid(str(boost::format("%s.%s")%asmid%kas.valuesid).c_str()); - daeSafeCast(abmvalue->add(COLLADA_ELEMENT_SIDREF))->setValue(kas.valuesid.c_str()); + abmvalue->setSid(str(boost::format("%s_%s")%asmid%kas.valuesid).c_str()); + daeSafeCast(abmvalue->add(COLLADA_ELEMENT_SIDREF))->setValue(str(boost::format("%s/%s")%askid%kas.valuesid).c_str()); domKinematics_bindRef abvalue = daeSafeCast(ias->add(COLLADA_ELEMENT_BIND)); - valuesid = str(boost::format("%s.%s")%assym%kas.valuesid); + valuesid = str(boost::format("%s_%s")%assym%kas.valuesid); abvalue->setSymbol(valuesid.c_str()); - daeSafeCast(abvalue->add(COLLADA_ELEMENT_PARAM))->setRef(str(boost::format("%s.%s")%asmid%kas.valuesid).c_str()); + daeSafeCast(abvalue->add(COLLADA_ELEMENT_PARAM))->setRef(str(boost::format("%s/%s_%s")%asmid%asmid%kas.valuesid).c_str()); } _iasout->vaxissids.push_back(axis_sids(ab->getSymbol(),valuesid,kas.jointnodesid)); } @@ -514,7 +514,7 @@ protected: string symscope, refscope; if( sidscope.size() > 0 ) { - symscope = sidscope+string("."); + symscope = sidscope+string("_"); refscope = sidscope+string("/"); } string ikmsid = str(boost::format("%s_inst")%kmout->kmodel->getID()); @@ -536,7 +536,7 @@ protected: ref[index] = '.'; index = ref.find("/",index+1); } - string sid = symscope+ikmsid+"."+ref; + string sid = symscope+ikmsid+"_"+ref; kbind->setSid(sid.c_str()); daeSafeCast(kbind->add(COLLADA_ELEMENT_SIDREF))->setValue((refscope+ikmsid+"/"+it->sid).c_str()); double value=0; @@ -721,7 +721,7 @@ protected: pdomlink->setSid(linksid.c_str()); domNodeRef pnode = daeSafeCast(pnodeparent->add(COLLADA_ELEMENT_NODE)); - string nodeid = str(boost::format("v%s.node%d")%strModelUri%linkindex); + string nodeid = str(boost::format("v%s_node%d")%strModelUri%linkindex); pnode->setId( nodeid.c_str() ); string nodesid = str(boost::format("node%d")%linkindex); pnode->setSid(nodesid.c_str()); @@ -742,7 +742,7 @@ protected: if( !!geometry ) { int igeom = 0; - string geomid = str(boost::format("g%s.%s.geom%d")%strModelUri%linksid%igeom); + string geomid = str(boost::format("g%s_%s_geom%d")%strModelUri%linksid%igeom); domGeometryRef pdomgeom = _WriteGeometry(geometry, geomid); domInstance_geometryRef pinstgeom = daeSafeCast(pnode->add(COLLADA_ELEMENT_INSTANCE_GEOMETRY)); pinstgeom->setUrl((string("#")+geomid).c_str()); @@ -752,7 +752,7 @@ protected: domBind_materialRef pmat = daeSafeCast(pinstgeom->add(COLLADA_ELEMENT_BIND_MATERIAL)); domBind_material::domTechnique_commonRef pmattec = daeSafeCast(pmat->add(COLLADA_ELEMENT_TECHNIQUE_COMMON)); domInstance_materialRef pinstmat = daeSafeCast(pmattec->add(COLLADA_ELEMENT_INSTANCE_MATERIAL)); - pinstmat->setTarget(xsAnyURI(*pdomgeom, string("#")+geomid+string(".mat"))); + pinstmat->setTarget(xsAnyURI(*pdomgeom, string("#")+geomid+string("_mat"))); pinstmat->setSymbol("mat0"); } @@ -850,8 +850,8 @@ protected: void _WriteMaterial(const string& geometry_id, boost::shared_ptr material) { - string effid = geometry_id+string(".eff"); - string matid = geometry_id+string(".mat"); + string effid = geometry_id+string("_eff"); + string matid = geometry_id+string("_mat"); domMaterialRef pdommat = daeSafeCast(_materialsLib->add(COLLADA_ELEMENT_MATERIAL)); pdommat->setId(matid.c_str()); domInstance_effectRef pdominsteff = daeSafeCast(pdommat->add(COLLADA_ELEMENT_INSTANCE_EFFECT)); @@ -872,7 +872,7 @@ protected: // domMaterialRef dommaterial = daeSafeCast(_materialsLib->add(COLLADA_ELEMENT_MATERIAL)); - string material_id = geometry_id + string(".mat"); + string material_id = geometry_id + string("_mat"); dommaterial->setId(material_id.c_str()); { // @@ -905,10 +905,10 @@ protected: domAccessorRef pacc; domFloat_arrayRef parray; { - pvertsource->setId(str(boost::format("%s.positions")%pdomgeom->getID()).c_str()); + pvertsource->setId(str(boost::format("%s_positions")%pdomgeom->getID()).c_str()); parray = daeSafeCast(pvertsource->add(COLLADA_ELEMENT_FLOAT_ARRAY)); - parray->setId(str(boost::format("%s.positions-array")%pdomgeom->getID()).c_str()); + parray->setId(str(boost::format("%s_positions-array")%pdomgeom->getID()).c_str()); parray->setDigits(6); // 6 decimal places domSource::domTechnique_commonRef psourcetec = daeSafeCast(pvertsource->add(COLLADA_ELEMENT_TECHNIQUE_COMMON)); diff --git a/colladadom/collada-dom.patch b/colladadom/collada-dom.patch index e28599d..468977a 100644 --- a/colladadom/collada-dom.patch +++ b/colladadom/collada-dom.patch @@ -74,3 +74,27 @@ Index: dom/make/domTest.mk // First see if the whole thing resolves correctly if (daeElement* result = finder(container, s, profile)) return result; +@@ -355,6 +367,23 @@ + if ((!member.empty() || haveArrayIndex1) && result.scalar == NULL) + return daeSidRef::resolveData(); + ++ if( !!result.elt && !result.array && !result.scalar ) { ++ // if newparam, follow its SIDREF (Rosen Diankov) ++ if( strcmp(result.elt->getElementName(),"newparam") == 0) { ++ daeElement* psidref = result.elt->getChild("SIDREF"); ++ if( !!psidref ) { ++ daeSidRef::resolveData newresult; ++ newresult = resolveImpl(daeSidRef(string("./") + psidref->getCharData(),result.elt->getParent(),sidRef.profile)); ++ if( !newresult.elt ) { ++ newresult = resolveImpl(daeSidRef(psidref->getCharData(),result.elt->getParent(),sidRef.profile)); ++ } ++ if( !!newresult.elt ) { ++ return newresult; ++ } ++ } ++ } ++ } ++ + // SID resolution was successful. + return result; + } diff --git a/urdf/src/collada_model_reader.cpp b/urdf/src/collada_model_reader.cpp index 8778559..72f427d 100644 --- a/urdf/src/collada_model_reader.cpp +++ b/urdf/src/collada_model_reader.cpp @@ -1858,7 +1858,7 @@ protected: if( !!pbind->getSymbol() && strcmp(pbind->getSymbol(), ref) == 0 ) { // found a match if( !!pbind->getParam() ) { - return searchBinding(pbind->getParam()->getRef(), pbindelt); + return daeSidRef(pbind->getParam()->getRef(), pbindelt).resolve().elt; } else if( !!pbind->getSIDREF() ) { return daeSidRef(pbind->getSIDREF()->getValue(), pbindelt).resolve().elt; @@ -2197,7 +2197,7 @@ protected: ROS_WARN_STREAM("bind_kinematics_model does not reference element\n"); } else { - ROS_WARN_STREAM(str(boost::format("bind_kinematics_model cannot find reference to %s%s:\n")%pelt->getElementName())); + ROS_WARN_STREAM(str(boost::format("bind_kinematics_model cannot find reference to %s:\n")%pelt->getElementName())); } continue; }