46 #pragma GCC diagnostic push 47 #pragma GCC diagnostic ignored "-Wdeprecated-declarations" 49 #include <dae/daeDocument.h> 50 #include <dae/daeErrorHandler.h> 51 #include <dae/domAny.h> 52 #include <dom/domCOLLADA.h> 53 #include <dom/domConstants.h> 54 #include <dom/domElements.h> 55 #include <dom/domTriangles.h> 56 #include <dom/domTypes.h> 57 #pragma GCC diagnostic pop 62 #include <urdf_model/pose.h> 66 #include <boost/date_time/posix_time/posix_time.hpp> 67 #include <boost/date_time/posix_time/posix_time_io.hpp> 68 #include <boost/format.hpp> 69 #include <boost/array.hpp> 70 #include <boost/shared_ptr.hpp> 71 #include <boost/scoped_ptr.hpp> 73 #if defined(ASSIMP_UNIFIED_HEADER_NAMES) 74 #include <assimp/scene.h> 75 #include <assimp/LogStream.hpp> 76 #include <assimp/DefaultLogger.hpp> 77 #include <assimp/IOStream.hpp> 78 #include <assimp/IOSystem.hpp> 79 #include <assimp/Importer.hpp> 80 #include <assimp/postprocess.h> 84 #include <aiPostProcess.h> 85 #include <DefaultLogger.h> 93 #define FOREACH(it, v) for(decltype((v).begin()) it = (v).begin(); it != (v).end(); (it)++) 94 #define FOREACHC FOREACH 110 , pos_(res.data.
get())
118 size_t Read(
void* buffer,
size_t size,
size_t count)
120 size_t to_read = size * count;
121 if (pos_ + to_read > res_.data.get() + res_.size)
123 to_read = res_.size - (pos_ - res_.data.get());
126 memcpy(buffer, pos_, to_read);
132 size_t Write(
const void* buffer,
size_t size,
size_t count) {
136 aiReturn
Seek(
size_t offset, aiOrigin origin)
138 uint8_t* new_pos = 0;
142 new_pos = res_.data.get() + offset;
145 new_pos = pos_ + offset;
148 new_pos = res_.data.get() + res_.size - offset;
154 if (new_pos < res_.data.get() || new_pos > res_.data.get() + res_.size)
156 return aiReturn_FAILURE;
160 return aiReturn_SUCCESS;
165 return pos_ - res_.data.get();
181 namespace mathextra {
188 #define distinctRoots 0 // roots r0 < r1 < r2 189 #define singleRoot 1 // root r0 190 #define floatRoot01 2 // roots r0 = r1 < r2 191 #define floatRoot12 4 // roots r0 < r1 = r2 192 #define tripleRoot 6 // roots r0 = r1 = r2 195 template <
typename T,
typename S>
198 T a, b, c,
d, e,
f, ell, q;
209 if ( fabs(c) >= g_fEpsilon ) {
210 ell = (T)sqrt(b*b+c*c);
218 mat[0*3+0] = (S)1; mat[0*3+1] = (S)0; mat[0*3+2] = (T)0;
219 mat[1*3+0] = (S)0; mat[1*3+1] = b; mat[1*3+2] = c;
220 mat[2*3+0] = (S)0; mat[2*3+1] = c; mat[2*3+2] = -b;
227 mat[0*3+0] = (S)1; mat[0*3+1] = (S)0; mat[0*3+2] = (S)0;
228 mat[1*3+0] = (S)0; mat[1*3+1] = (S)1; mat[1*3+2] = (S)0;
229 mat[2*3+0] = (S)0; mat[2*3+1] = (S)0; mat[2*3+2] = (S)1;
233 int CubicRoots (
double c0,
double c1,
double c2,
double *r0,
double *r1,
double *r2)
238 double discr, temp, pval, pdval, b0, b1;
243 if ( discr >= 0.0 ) {
244 discr = (double)sqrt(discr);
246 pval = temp*(temp*(temp-c2)+c1)-c0;
249 (*r0) = (c2-discr)/3 - 1;
251 for (i = 0; i < maxiter && fabs(pval) >
g_fEpsilon; i++) {
252 pval = (*r0)*((*r0)*((*r0)-c2)+c1)-c0;
253 pdval = (*r0)*(3*(*r0)-2*c2)+c1;
260 b0 = (*r0)*((*r0)-c2)+c1;
262 if ( discr < -g_fEpsilon )
272 discr = sqrt(fabs(discr));
273 (*r1) = 0.5f*(-b1-discr);
274 (*r2) = 0.5f*(-b1+discr);
276 if ( fabs((*r0)-(*r1)) <= g_fEpsilon )
281 if ( fabs((*r1)-(*r2)) <= g_fEpsilon )
293 for (i = 0; i < maxiter && fabs(pval) >
g_fEpsilon; i++) {
294 pval = (*r2)*((*r2)*((*r2)-c2)+c1)-c0;
295 pdval = (*r2)*(3*(*r2)-2*c2)+c1;
302 b0 = (*r2)*((*r2)-c2)+c1;
304 if ( discr < -g_fEpsilon )
315 discr = sqrt(fabs(discr));
316 (*r0) = 0.5f*(-b1-discr);
317 (*r1) = 0.5f*(-b1+discr);
319 if ( fabs((*r0)-(*r1)) <= g_fEpsilon )
324 if ( fabs((*r1)-(*r2)) <= g_fEpsilon )
337 for (i = 0; i < maxiter && fabs(pval) >
g_fEpsilon; i++) {
338 pval = (*r0)*((*r0)*((*r0)-c2)+c1)-c0;
339 pdval = (*r0)*(3*(*r0)-2*c2)+c1;
353 for (
int i0 = 0; i0 < 3; i0++)
355 const int iMaxIter = 32;
357 for (iIter = 0; iIter < iMaxIter; iIter++)
360 for (i1 = i0; i1 <= 1; i1++)
362 T fSum = fabs(afDiag[i1]) +
364 if ( fabs(afSubDiag[i1]) + fSum == fSum )
370 T fTmp0 = (afDiag[i0+1]-afDiag[i0])/(2.0
f*afSubDiag[i0]);
371 T fTmp1 = sqrt(fTmp0*fTmp0+1.0
f);
373 fTmp0 = afDiag[i1]-afDiag[i0]+afSubDiag[i0]/(fTmp0-fTmp1);
375 fTmp0 = afDiag[i1]-afDiag[i0]+afSubDiag[i0]/(fTmp0+fTmp1);
379 for (
int i2 = i1-1; i2 >= i0; i2--)
381 T fTmp3 = fSin*afSubDiag[i2];
382 T fTmp4 = fCos*afSubDiag[i2];
383 if ( fabs(fTmp3) >= fabs(fTmp0) )
386 fTmp1 = sqrt(fCos*fCos+1.0
f);
387 afSubDiag[i2+1] = fTmp3*fTmp1;
394 fTmp1 = sqrt(fSin*fSin+1.0
f);
395 afSubDiag[i2+1] = fTmp0*fTmp1;
399 fTmp0 = afDiag[i2+1]-fTmp2;
400 fTmp1 = (afDiag[i2]-fTmp0)*fSin+2.0
f*fTmp4*fCos;
402 afDiag[i2+1] = fTmp0+fTmp2;
403 fTmp0 = fCos*fTmp1-fTmp4;
405 for (
int iRow = 0; iRow < 3; iRow++)
407 fTmp3 = m_aafEntry[iRow*3+i2+1];
408 m_aafEntry[iRow*3+i2+1] = fSin*m_aafEntry[iRow*3+i2] +
410 m_aafEntry[iRow*3+i2] = fCos*m_aafEntry[iRow*3+i2] -
415 afSubDiag[i0] = fTmp0;
416 afSubDiag[i1] = 0.0f;
419 if ( iIter == iMaxIter )
431 return _QLAlgorithm3<float>(m_aafEntry, afDiag, afSubDiag);
434 bool QLAlgorithm3 (
double* m_aafEntry,
double* afDiag,
double* afSubDiag)
436 return _QLAlgorithm3<double>(m_aafEntry, afDiag, afSubDiag);
443 memcpy(fevecs, fmat,
sizeof(
double)*9);
448 double fDet = fevecs[0*3+0] * (fevecs[1*3+1] * fevecs[2*3+2] - fevecs[1*3+2] * fevecs[2*3+1]) +
449 fevecs[0*3+1] * (fevecs[1*3+2] * fevecs[2*3+0] - fevecs[1*3+0] * fevecs[2*3+2]) +
450 fevecs[0*3+2] * (fevecs[1*3+0] * fevecs[2*3+1] - fevecs[1*3+1] * fevecs[2*3+0]);
453 fevecs[0*3+2] = -fevecs[0*3+2];
454 fevecs[1*3+2] = -fevecs[1*3+2];
455 fevecs[2*3+2] = -fevecs[2*3+2];
482 res = retriever_.get(file);
498 Assimp::IOStream*
Open(
const char* file,
const char* mode)
500 ROS_ASSERT(mode == std::string(
"r") || mode == std::string(
"rb"));
506 res = retriever_.get(file);
515 void Close(Assimp::IOStream* stream) {
526 Triangle(
const urdf::Vector3 &_p1,
const urdf::Vector3 &_p2,
const urdf::Vector3 &_p3) :
527 p1(_p1), p2(_p2), p3(_p3)
530 urdf::Vector3 p1, p2, p3;
532 void clear() { p1.clear(); p2.clear(); p3.clear(); };
586 axis_sids(
const string& axissid,
const string& valuesid,
const string& jointnodesid) : axissid(axissid), valuesid(valuesid), jointnodesid(jointnodesid) {
593 domInstance_kinematics_modelRef
ikm;
601 domInstance_articulated_systemRef
ias;
609 domInstance_physics_modelRef
ipm;
615 std::string
uri, kinematicsgeometryhash;
622 daeErrorHandler::setErrorHandler(
this);
635 const char* documentName =
"urdf_snapshot";
636 daeInt error = _collada.getDatabase()->insertDocument(documentName, &_doc );
637 if (error != DAE_OK || _doc == NULL) {
640 _dom = daeSafeCast<domCOLLADA>(_doc->getDomRoot());
641 _dom->setAttribute(
"xmlns:math",
"http://www.w3.org/1998/Math/MathML");
644 domAssetRef asset = daeSafeCast<domAsset>( _dom->add( COLLADA_ELEMENT_ASSET ) );
647 boost::posix_time::time_facet* facet =
new boost::posix_time::time_facet(
"%Y-%m-%dT%H:%M:%s");
648 std::stringstream ss;
649 ss.imbue(std::locale(ss.getloc(), facet));
650 ss << boost::posix_time::second_clock::local_time();
652 domAsset::domCreatedRef created = daeSafeCast<domAsset::domCreated>( asset->add( COLLADA_ELEMENT_CREATED ) );
653 created->setValue(ss.str().c_str());
654 domAsset::domModifiedRef modified = daeSafeCast<domAsset::domModified>( asset->add( COLLADA_ELEMENT_MODIFIED ) );
655 modified->setValue(ss.str().c_str());
657 domAsset::domContributorRef contrib = daeSafeCast<domAsset::domContributor>( asset->add( COLLADA_TYPE_CONTRIBUTOR ) );
658 domAsset::domContributor::domAuthoring_toolRef authoringtool = daeSafeCast<domAsset::domContributor::domAuthoring_tool>( contrib->add( COLLADA_ELEMENT_AUTHORING_TOOL ) );
659 authoringtool->setValue(
"URDF Collada Writer");
661 domAsset::domUnitRef units = daeSafeCast<domAsset::domUnit>( asset->add( COLLADA_ELEMENT_UNIT ) );
663 units->setName(
"meter");
665 domAsset::domUp_axisRef zup = daeSafeCast<domAsset::domUp_axis>( asset->add( COLLADA_ELEMENT_UP_AXIS ) );
666 zup->setValue(UP_AXIS_Z_UP);
669 _globalscene = _dom->getScene();
670 if( !_globalscene ) {
671 _globalscene = daeSafeCast<domCOLLADA::domScene>( _dom->add( COLLADA_ELEMENT_SCENE ) );
674 _visualScenesLib = daeSafeCast<domLibrary_visual_scenes>(_dom->add(COLLADA_ELEMENT_LIBRARY_VISUAL_SCENES));
675 _visualScenesLib->setId(
"vscenes");
676 _geometriesLib = daeSafeCast<domLibrary_geometries>(_dom->add(COLLADA_ELEMENT_LIBRARY_GEOMETRIES));
677 _geometriesLib->setId(
"geometries");
678 _effectsLib = daeSafeCast<domLibrary_effects>(_dom->add(COLLADA_ELEMENT_LIBRARY_EFFECTS));
679 _effectsLib->setId(
"effects");
680 _materialsLib = daeSafeCast<domLibrary_materials>(_dom->add(COLLADA_ELEMENT_LIBRARY_MATERIALS));
681 _materialsLib->setId(
"materials");
682 _kinematicsModelsLib = daeSafeCast<domLibrary_kinematics_models>(_dom->add(COLLADA_ELEMENT_LIBRARY_KINEMATICS_MODELS));
683 _kinematicsModelsLib->setId(
"kmodels");
684 _articulatedSystemsLib = daeSafeCast<domLibrary_articulated_systems>(_dom->add(COLLADA_ELEMENT_LIBRARY_ARTICULATED_SYSTEMS));
685 _articulatedSystemsLib->setId(
"asystems");
686 _kinematicsScenesLib = daeSafeCast<domLibrary_kinematics_scenes>(_dom->add(COLLADA_ELEMENT_LIBRARY_KINEMATICS_SCENES));
687 _kinematicsScenesLib->setId(
"kscenes");
688 _physicsScenesLib = daeSafeCast<domLibrary_physics_scenes>(_dom->add(COLLADA_ELEMENT_LIBRARY_PHYSICS_SCENES));
689 _physicsScenesLib->setId(
"pscenes");
690 _physicsModelsLib = daeSafeCast<domLibrary_physics_models>(_dom->add(COLLADA_ELEMENT_LIBRARY_PHYSICS_MODELS));
691 _physicsModelsLib->setId(
"pmodels");
692 domExtraRef pextra_library_sensors = daeSafeCast<domExtra>(_dom->add(COLLADA_ELEMENT_EXTRA));
693 pextra_library_sensors->setId(
"sensors");
694 pextra_library_sensors->setType(
"library_sensors");
695 _sensorsLib = daeSafeCast<domTechnique>(pextra_library_sensors->add(COLLADA_ELEMENT_TECHNIQUE));
696 _sensorsLib->setProfile(
"OpenRAVE");
701 _WriteBindingsInstance_kinematics_scene();
705 ROS_ERROR(
"Error converting: %s", ex.what());
712 daeString uri = _doc->getDocumentURI()->getURI();
713 _collada.writeTo(uri, file);
725 std::cerr <<
"COLLADA DOM warning: " << msg << std::endl;
731 _scene.vscene = daeSafeCast<domVisual_scene>(_visualScenesLib->add(COLLADA_ELEMENT_VISUAL_SCENE));
732 _scene.vscene->setId(
"vscene");
733 _scene.vscene->setName(
"URDF Visual Scene");
736 _scene.kscene = daeSafeCast<domKinematics_scene>(_kinematicsScenesLib->add(COLLADA_ELEMENT_KINEMATICS_SCENE));
737 _scene.kscene->setId(
"kscene");
738 _scene.kscene->setName(
"URDF Kinematics Scene");
741 _scene.pscene = daeSafeCast<domPhysics_scene>(_physicsScenesLib->add(COLLADA_ELEMENT_PHYSICS_SCENE));
742 _scene.pscene->setId(
"pscene");
743 _scene.pscene->setName(
"URDF Physics Scene");
746 _scene.viscene = daeSafeCast<domInstance_with_extra>(_globalscene->add( COLLADA_ELEMENT_INSTANCE_VISUAL_SCENE ));
747 _scene.viscene->setUrl( (
string(
"#") +
string(_scene.vscene->getID())).c_str() );
750 _scene.kiscene = daeSafeCast<domInstance_kinematics_scene>(_globalscene->add( COLLADA_ELEMENT_INSTANCE_KINEMATICS_SCENE ));
751 _scene.kiscene->setUrl( (
string(
"#") +
string(_scene.kscene->getID())).c_str() );
754 _scene.piscene = daeSafeCast<domInstance_with_extra>(_globalscene->add( COLLADA_ELEMENT_INSTANCE_PHYSICS_SCENE ));
755 _scene.piscene->setUrl( (
string(
"#") +
string(_scene.pscene->getID())).c_str() );
759 domPhysics_scene::domTechnique_commonRef common = daeSafeCast<domPhysics_scene::domTechnique_common>(_scene.pscene->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
761 domTargetable_float3Ref g = daeSafeCast<domTargetable_float3>(common->add(COLLADA_ELEMENT_GRAVITY));
762 g->getValue().set3 (0,0,0);
768 ROS_DEBUG_STREAM(str(boost::format(
"writing robot as instance_articulated_system (%d) %s\n")%
id%_robot.getName()));
769 string asid = _ComputeId(str(boost::format(
"robot%d")%
id));
770 string askid = _ComputeId(str(boost::format(
"%s_kinematics")%asid));
771 string asmid = _ComputeId(str(boost::format(
"%s_motion")%asid));
772 string iassid = _ComputeId(str(boost::format(
"%s_inst")%asmid));
774 domInstance_articulated_systemRef ias = daeSafeCast<domInstance_articulated_system>(_scene.kscene->add(COLLADA_ELEMENT_INSTANCE_ARTICULATED_SYSTEM));
775 ias->setSid(iassid.c_str());
776 ias->setUrl((
string(
"#")+asmid).c_str());
777 ias->setName(_robot.getName().c_str());
783 domArticulated_systemRef articulated_system_motion = daeSafeCast<domArticulated_system>(_articulatedSystemsLib->add(COLLADA_ELEMENT_ARTICULATED_SYSTEM));
784 articulated_system_motion->setId(asmid.c_str());
785 domMotionRef motion = daeSafeCast<domMotion>(articulated_system_motion->add(COLLADA_ELEMENT_MOTION));
786 domMotion_techniqueRef mt = daeSafeCast<domMotion_technique>(motion->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
787 domInstance_articulated_systemRef ias_motion = daeSafeCast<domInstance_articulated_system>(motion->add(COLLADA_ELEMENT_INSTANCE_ARTICULATED_SYSTEM));
788 ias_motion->setUrl(str(boost::format(
"#%s")%askid).c_str());
791 domArticulated_systemRef articulated_system_kinematics = daeSafeCast<domArticulated_system>(_articulatedSystemsLib->add(COLLADA_ELEMENT_ARTICULATED_SYSTEM));
792 articulated_system_kinematics->setId(askid.c_str());
793 domKinematicsRef kinematics = daeSafeCast<domKinematics>(articulated_system_kinematics->add(COLLADA_ELEMENT_KINEMATICS));
794 domKinematics_techniqueRef kt = daeSafeCast<domKinematics_technique>(kinematics->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
796 _WriteInstance_kinematics_model(kinematics,askid,
id);
798 for(
size_t idof = 0; idof < _ikmout->vaxissids.size(); ++idof) {
799 string axis_infosid = _ComputeId(str(boost::format(
"axis_info_inst%d")%idof));
800 urdf::JointConstSharedPtr pjoint = _ikmout->kmout->vaxissids.at(idof).pjoint;
801 BOOST_ASSERT(_mapjointindices[pjoint] == (
int)idof);
805 domKinematics_axis_infoRef kai = daeSafeCast<domKinematics_axis_info>(kt->add(COLLADA_ELEMENT_AXIS_INFO));
806 kai->setAxis(str(boost::format(
"%s/%s")%_ikmout->kmout->kmodel->getID()%_ikmout->kmout->vaxissids.at(idof).sid).c_str());
807 kai->setSid(axis_infosid.c_str());
808 bool bactive = !pjoint->mimic;
809 double flower=0, fupper=0;
810 if( pjoint->type != urdf::Joint::CONTINUOUS ) {
811 if( !!pjoint->limits ) {
812 flower = pjoint->limits->lower;
813 fupper = pjoint->limits->upper;
815 if( !!pjoint->safety ) {
816 flower = pjoint->safety->soft_lower_limit;
817 fupper = pjoint->safety->soft_upper_limit;
819 if( flower == fupper ) {
823 if( pjoint->type != urdf::Joint::PRISMATIC ) {
826 domKinematics_limitsRef plimits = daeSafeCast<domKinematics_limits>(kai->add(COLLADA_ELEMENT_LIMITS));
827 daeSafeCast<domCommon_float_or_param::domFloat>(plimits->add(COLLADA_ELEMENT_MIN)->add(COLLADA_ELEMENT_FLOAT))->setValue(flower*fmult);
828 daeSafeCast<domCommon_float_or_param::domFloat>(plimits->add(COLLADA_ELEMENT_MAX)->add(COLLADA_ELEMENT_FLOAT))->setValue(fupper*fmult);
831 domCommon_bool_or_paramRef active = daeSafeCast<domCommon_bool_or_param>(kai->add(COLLADA_ELEMENT_ACTIVE));
832 daeSafeCast<domCommon_bool_or_param::domBool>(active->add(COLLADA_ELEMENT_BOOL))->setValue(bactive);
833 domCommon_bool_or_paramRef locked = daeSafeCast<domCommon_bool_or_param>(kai->add(COLLADA_ELEMENT_LOCKED));
834 daeSafeCast<domCommon_bool_or_param::domBool>(locked->add(COLLADA_ELEMENT_BOOL))->setValue(
false);
837 domMotion_axis_infoRef mai = daeSafeCast<domMotion_axis_info>(mt->add(COLLADA_ELEMENT_AXIS_INFO));
838 mai->setAxis(str(boost::format(
"%s/%s")%askid%axis_infosid).c_str());
839 if( !!pjoint->limits ) {
840 domCommon_float_or_paramRef speed = daeSafeCast<domCommon_float_or_param>(mai->add(COLLADA_ELEMENT_SPEED));
841 daeSafeCast<domCommon_float_or_param::domFloat>(speed->add(COLLADA_ELEMENT_FLOAT))->setValue(pjoint->limits->velocity);
842 domCommon_float_or_paramRef accel = daeSafeCast<domCommon_float_or_param>(mai->add(COLLADA_ELEMENT_ACCELERATION));
843 daeSafeCast<domCommon_float_or_param::domFloat>(accel->add(COLLADA_ELEMENT_FLOAT))->setValue(pjoint->limits->effort);
848 string asmsym = _ComputeId(str(boost::format(
"%s_%s")%asmid%_ikmout->ikm->getSid()));
849 string assym = _ComputeId(str(boost::format(
"%s_%s")%_scene.kscene->getID()%_ikmout->ikm->getSid()));
850 FOREACH(it, _ikmout->vkinematicsbindings) {
851 domKinematics_newparamRef abm = daeSafeCast<domKinematics_newparam>(ias_motion->add(COLLADA_ELEMENT_NEWPARAM));
852 abm->setSid(asmsym.c_str());
853 daeSafeCast<domKinematics_newparam::domSIDREF>(abm->add(COLLADA_ELEMENT_SIDREF))->setValue(str(boost::format(
"%s/%s")%askid%it->first).c_str());
854 domKinematics_bindRef ab = daeSafeCast<domKinematics_bind>(ias->add(COLLADA_ELEMENT_BIND));
855 ab->setSymbol(assym.c_str());
856 daeSafeCast<domKinematics_param>(ab->add(COLLADA_ELEMENT_PARAM))->setRef(str(boost::format(
"%s/%s")%asmid%asmsym).c_str());
857 _iasout->vkinematicsbindings.push_back(make_pair(
string(ab->getSymbol()), it->second));
859 for(
size_t idof = 0; idof < _ikmout->vaxissids.size(); ++idof) {
860 const axis_sids& kas = _ikmout->vaxissids.at(idof);
861 domKinematics_newparamRef abm = daeSafeCast<domKinematics_newparam>(ias_motion->add(COLLADA_ELEMENT_NEWPARAM));
862 abm->setSid(_ComputeId(str(boost::format(
"%s_%s")%asmid%kas.
axissid)).c_str());
863 daeSafeCast<domKinematics_newparam::domSIDREF>(abm->add(COLLADA_ELEMENT_SIDREF))->setValue(str(boost::format(
"%s/%s")%askid%kas.
axissid).c_str());
864 domKinematics_bindRef ab = daeSafeCast<domKinematics_bind>(ias->add(COLLADA_ELEMENT_BIND));
865 ab->setSymbol(str(boost::format(
"%s_%s")%assym%kas.
axissid).c_str());
866 daeSafeCast<domKinematics_param>(ab->add(COLLADA_ELEMENT_PARAM))->setRef(str(boost::format(
"%s/%s_%s")%asmid%asmid%kas.
axissid).c_str());
869 domKinematics_newparamRef abmvalue = daeSafeCast<domKinematics_newparam>(ias_motion->add(COLLADA_ELEMENT_NEWPARAM));
870 abmvalue->setSid(_ComputeId(str(boost::format(
"%s_%s")%asmid%kas.
valuesid)).c_str());
871 daeSafeCast<domKinematics_newparam::domSIDREF>(abmvalue->add(COLLADA_ELEMENT_SIDREF))->setValue(str(boost::format(
"%s/%s")%askid%kas.
valuesid).c_str());
872 domKinematics_bindRef abvalue = daeSafeCast<domKinematics_bind>(ias->add(COLLADA_ELEMENT_BIND));
873 valuesid = _ComputeId(str(boost::format(
"%s_%s")%assym%kas.
valuesid));
874 abvalue->setSymbol(valuesid.c_str());
875 daeSafeCast<domKinematics_param>(abvalue->add(COLLADA_ELEMENT_PARAM))->setRef(str(boost::format(
"%s/%s_%s")%asmid%asmid%kas.
valuesid).c_str());
886 ROS_DEBUG_STREAM(str(boost::format(
"writing instance_kinematics_model %s\n")%_robot.getName()));
890 _ikmout->kmout = kmout;
891 _ikmout->ikm = daeSafeCast<domInstance_kinematics_model>(parent->add(COLLADA_ELEMENT_INSTANCE_KINEMATICS_MODEL));
893 string symscope, refscope;
894 if( sidscope.size() > 0 ) {
895 symscope = sidscope+string(
"_");
896 refscope = sidscope+string(
"/");
898 string ikmsid = _ComputeId(str(boost::format(
"%s_inst")%kmout->kmodel->getID()));
899 _ikmout->ikm->setUrl(str(boost::format(
"#%s")%kmout->kmodel->getID()).c_str());
900 _ikmout->ikm->setSid(ikmsid.c_str());
902 domKinematics_newparamRef kbind = daeSafeCast<domKinematics_newparam>(_ikmout->ikm->add(COLLADA_ELEMENT_NEWPARAM));
903 kbind->setSid(_ComputeId(symscope+ikmsid).c_str());
904 daeSafeCast<domKinematics_newparam::domSIDREF>(kbind->add(COLLADA_ELEMENT_SIDREF))->setValue((refscope+ikmsid).c_str());
905 _ikmout->vkinematicsbindings.push_back(make_pair(
string(kbind->getSid()), str(boost::format(
"visual%d/node%d")%
id%_maplinkindices[_robot.getRoot()])));
907 _ikmout->vaxissids.reserve(kmout->vaxissids.size());
910 domKinematics_newparamRef kbind = daeSafeCast<domKinematics_newparam>(_ikmout->ikm->add(COLLADA_ELEMENT_NEWPARAM));
911 string ref = it->sid;
912 size_t index = ref.find(
"/");
913 while(index != string::npos) {
915 index = ref.find(
"/",index+1);
917 string sid = _ComputeId(symscope+ikmsid+
"_"+ref);
918 kbind->setSid(sid.c_str());
919 daeSafeCast<domKinematics_newparam::domSIDREF>(kbind->add(COLLADA_ELEMENT_SIDREF))->setValue((refscope+ikmsid+
"/"+it->sid).c_str());
921 double flower=0, fupper=0;
922 if( !!it->pjoint->limits ) {
923 flower = it->pjoint->limits->lower;
924 fupper = it->pjoint->limits->upper;
926 if( flower > 0 || fupper < 0 ) {
927 value = 0.5*(flower+fupper);
929 domKinematics_newparamRef pvalueparam = daeSafeCast<domKinematics_newparam>(_ikmout->ikm->add(COLLADA_ELEMENT_NEWPARAM));
930 pvalueparam->setSid((sid+
string(
"_value")).c_str());
931 daeSafeCast<domKinematics_newparam::domFloat>(pvalueparam->add(COLLADA_ELEMENT_FLOAT))->setValue(value);
932 _ikmout->vaxissids.push_back(
axis_sids(sid,pvalueparam->getSid(),kmout->vaxissids.at(i).jointnodesid));
939 domKinematics_modelRef kmodel = daeSafeCast<domKinematics_model>(_kinematicsModelsLib->add(COLLADA_ELEMENT_KINEMATICS_MODEL));
940 string kmodelid = _ComputeKinematics_modelId(
id);
941 kmodel->setId(kmodelid.c_str());
942 kmodel->setName(_robot.getName().c_str());
944 domKinematics_model_techniqueRef ktec = daeSafeCast<domKinematics_model_technique>(kmodel->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
947 domNodeRef pnoderoot = daeSafeCast<domNode>(_scene.vscene->add(COLLADA_ELEMENT_NODE));
948 string bodyid = _ComputeId(str(boost::format(
"visual%d")%
id));
949 pnoderoot->setId(bodyid.c_str());
950 pnoderoot->setSid(bodyid.c_str());
951 pnoderoot->setName(_robot.getName().c_str());
954 _mapjointindices.clear();
957 _mapjointindices[itj->second] = index++;
959 _maplinkindices.clear();
962 _maplinkindices[itj->second] = index++;
964 _mapmaterialindices.clear();
967 _mapmaterialindices[itj->second] = index++;
971 vector<domJointRef> vdomjoints(_robot.joints_.size());
973 kmout->kmodel = kmodel;
974 kmout->vaxissids.resize(_robot.joints_.size());
975 kmout->vlinksids.resize(_robot.links_.size());
978 urdf::JointSharedPtr pjoint = itjoint->second;
979 int index = _mapjointindices[itjoint->second];
980 domJointRef pdomjoint = daeSafeCast<domJoint>(ktec->add(COLLADA_ELEMENT_JOINT));
981 string jointid = _ComputeId(pjoint->name);
982 pdomjoint->setSid(jointid.c_str() );
983 pdomjoint->setName(pjoint->name.c_str());
984 domAxis_constraintRef axis;
985 if( !!pjoint->limits ) {
986 lmin = pjoint->limits->lower;
987 lmax = pjoint->limits->upper;
994 switch(pjoint->type) {
995 case urdf::Joint::REVOLUTE:
996 case urdf::Joint::CONTINUOUS:
997 axis = daeSafeCast<domAxis_constraint>(pdomjoint->add(COLLADA_ELEMENT_REVOLUTE));
1002 case urdf::Joint::PRISMATIC:
1003 axis = daeSafeCast<domAxis_constraint>(pdomjoint->add(COLLADA_ELEMENT_PRISMATIC));
1005 case urdf::Joint::FIXED:
1006 axis = daeSafeCast<domAxis_constraint>(pdomjoint->add(COLLADA_ELEMENT_REVOLUTE));
1012 ROS_WARN_STREAM(str(boost::format(
"unsupported joint type specified %d")%(
int)pjoint->type));
1021 string axisid = _ComputeId(str(boost::format(
"axis%d")%ia));
1022 axis->setSid(axisid.c_str());
1023 kmout->vaxissids.at(index).pjoint = pjoint;
1024 kmout->vaxissids.at(index).sid = jointid+string(
"/")+axisid;
1025 kmout->vaxissids.at(index).iaxis = ia;
1026 domAxisRef paxis = daeSafeCast<domAxis>(axis->add(COLLADA_ELEMENT_AXIS));
1027 paxis->getValue().setCount(3);
1028 paxis->getValue()[0] = pjoint->axis.x;
1029 paxis->getValue()[1] = pjoint->axis.y;
1030 paxis->getValue()[2] = pjoint->axis.z;
1031 if( pjoint->type != urdf::Joint::CONTINUOUS ) {
1032 domJoint_limitsRef plimits = daeSafeCast<domJoint_limits>(axis->add(COLLADA_TYPE_LIMITS));
1033 daeSafeCast<domMinmax>(plimits->add(COLLADA_ELEMENT_MIN))->getValue() = lmin;
1034 daeSafeCast<domMinmax>(plimits->add(COLLADA_ELEMENT_MAX))->getValue() = lmax;
1036 vdomjoints.at(index) = pdomjoint;
1039 LINKOUTPUT childinfo = _WriteLink(_robot.getRoot(), ktec, pnoderoot, kmodel->getID());
1041 kmout->vlinksids.at(itused->first) = itused->second;
1044 kmout->vaxissids.at(itprocessed->first).jointnodesid = itprocessed->second;
1049 FOREACHC(itjoint, _robot.joints_) {
1050 string jointsid = _ComputeId(itjoint->second->name);
1051 urdf::JointSharedPtr pjoint = itjoint->second;
1052 if( !pjoint->mimic ) {
1056 domFormulaRef pf = daeSafeCast<domFormula>(ktec->add(COLLADA_ELEMENT_FORMULA));
1057 string formulaid = _ComputeId(str(boost::format(
"%s_formula")%jointsid));
1058 pf->setSid(formulaid.c_str());
1059 domCommon_float_or_paramRef ptarget = daeSafeCast<domCommon_float_or_param>(pf->add(COLLADA_ELEMENT_TARGET));
1060 string targetjointid = str(boost::format(
"%s/%s")%kmodel->getID()%jointsid);
1061 daeSafeCast<domCommon_param>(ptarget->add(COLLADA_TYPE_PARAM))->setValue(targetjointid.c_str());
1063 domTechniqueRef pftec = daeSafeCast<domTechnique>(pf->add(COLLADA_ELEMENT_TECHNIQUE));
1064 pftec->setProfile(
"OpenRAVE");
1067 daeElementRef poselt = pftec->add(
"equation");
1068 poselt->setAttribute(
"type",
"position");
1071 daeElementRef pmath_math = poselt->add(
"math");
1072 daeElementRef pmath_apply = pmath_math->add(
"apply");
1074 daeElementRef pmath_plus = pmath_apply->add(
"plus");
1075 daeElementRef pmath_apply1 = pmath_apply->add(
"apply");
1077 daeElementRef pmath_times = pmath_apply1->add(
"times");
1078 daeElementRef pmath_const0 = pmath_apply1->add(
"cn");
1079 pmath_const0->setCharData(str(boost::format(
"%f")%pjoint->mimic->multiplier));
1080 daeElementRef pmath_symb = pmath_apply1->add(
"csymbol");
1081 pmath_symb->setAttribute(
"encoding",
"COLLADA");
1082 pmath_symb->setCharData(str(boost::format(
"%s/%s")%kmodel->getID()%_ComputeId(pjoint->mimic->joint_name)));
1084 daeElementRef pmath_const1 = pmath_apply->add(
"cn");
1085 pmath_const1->setCharData(str(boost::format(
"%f")%pjoint->mimic->offset));
1090 daeElementRef derivelt = pftec->add(
"equation");
1091 derivelt->setAttribute(
"type",
"first_partial");
1092 derivelt->setAttribute(
"target",str(boost::format(
"%s/%s")%kmodel->getID()%_ComputeId(pjoint->mimic->joint_name)).c_str());
1093 daeElementRef pmath_const0 = derivelt->add(
"cn");
1094 pmath_const0->setCharData(str(boost::format(
"%f")%pjoint->mimic->multiplier));
1098 daeElementRef derivelt = pftec->add(
"equation");
1099 derivelt->setAttribute(
"type",
"second_partial");
1100 derivelt->setAttribute(
"target",str(boost::format(
"%s/%s")%kmodel->getID()%_ComputeId(pjoint->mimic->joint_name)).c_str());
1101 daeElementRef pmath_const0 = derivelt->add(
"cn");
1102 pmath_const0->setCharData(str(boost::format(
"%f")%pjoint->mimic->multiplier));
1106 domFormula_techniqueRef pfcommontec = daeSafeCast<domFormula_technique>(pf->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
1109 daeElementRef pmath_math = pfcommontec->add(
"math");
1110 daeElementRef pmath_apply = pmath_math->add(
"apply");
1112 daeElementRef pmath_plus = pmath_apply->add(
"plus");
1113 daeElementRef pmath_apply1 = pmath_apply->add(
"apply");
1115 daeElementRef pmath_times = pmath_apply1->add(
"times");
1116 daeElementRef pmath_const0 = pmath_apply1->add(
"cn");
1117 pmath_const0->setCharData(str(boost::format(
"%f")%pjoint->mimic->multiplier));
1118 daeElementRef pmath_symb = pmath_apply1->add(
"csymbol");
1119 pmath_symb->setAttribute(
"encoding",
"COLLADA");
1120 pmath_symb->setCharData(str(boost::format(
"%s/%s")%kmodel->getID()%_ComputeId(pjoint->mimic->joint_name)));
1122 daeElementRef pmath_const1 = pmath_apply->add(
"cn");
1123 pmath_const1->setCharData(str(boost::format(
"%f")%pjoint->mimic->offset));
1137 virtual LINKOUTPUT _WriteLink(urdf::LinkConstSharedPtr plink, daeElementRef pkinparent, domNodeRef pnodeparent,
const string& strModelUri)
1140 int linkindex = _maplinkindices[plink];
1141 string linksid = _ComputeId(plink->name);
1142 domLinkRef pdomlink = daeSafeCast<domLink>(pkinparent->add(COLLADA_ELEMENT_LINK));
1143 pdomlink->setName(plink->name.c_str());
1144 pdomlink->setSid(linksid.c_str());
1146 domNodeRef pnode = daeSafeCast<domNode>(pnodeparent->add(COLLADA_ELEMENT_NODE));
1147 string nodeid = _ComputeId(str(boost::format(
"v%s_node%d")%strModelUri%linkindex));
1148 pnode->setId( nodeid.c_str() );
1149 string nodesid = _ComputeId(str(boost::format(
"node%d")%linkindex));
1150 pnode->setSid(nodesid.c_str());
1151 pnode->setName(plink->name.c_str());
1153 urdf::GeometrySharedPtr geometry;
1154 urdf::MaterialSharedPtr material;
1155 urdf::Pose geometry_origin;
1156 if( !!plink->visual ) {
1157 geometry = plink->visual->geometry;
1158 material = plink->visual->material;
1159 geometry_origin = plink->visual->origin;
1161 else if( !!plink->collision ) {
1162 geometry = plink->collision->geometry;
1163 geometry_origin = plink->collision->origin;
1166 urdf::Pose geometry_origin_inv = _poseInverse(geometry_origin);
1169 bool write_visual =
false;
1170 if ( !!plink->visual ) {
1171 if (plink->visual_array.size() > 1) {
1173 for (std::vector<urdf::VisualSharedPtr >::const_iterator it = plink->visual_array.begin();
1174 it != plink->visual_array.end(); it++) {
1176 string geomid = _ComputeId(str(boost::format(
"g%s_%s_geom%d")%strModelUri%linksid%igeom));
1178 domGeometryRef pdomgeom;
1179 if ( it != plink->visual_array.begin() ) {
1180 urdf::Pose org_trans = _poseMult(geometry_origin_inv, (*it)->origin);
1181 pdomgeom = _WriteGeometry((*it)->geometry, geomid, &org_trans);
1184 pdomgeom = _WriteGeometry((*it)->geometry, geomid);
1186 domInstance_geometryRef pinstgeom = daeSafeCast<domInstance_geometry>(pnode->add(COLLADA_ELEMENT_INSTANCE_GEOMETRY));
1187 pinstgeom->setUrl((
string(
"#") + geomid).c_str());
1189 _WriteMaterial(pdomgeom->getID(), (*it)->material);
1190 domBind_materialRef pmat = daeSafeCast<domBind_material>(pinstgeom->add(COLLADA_ELEMENT_BIND_MATERIAL));
1191 domBind_material::domTechnique_commonRef pmattec = daeSafeCast<domBind_material::domTechnique_common>(pmat->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
1192 domInstance_materialRef pinstmat = daeSafeCast<domInstance_material>(pmattec->add(COLLADA_ELEMENT_INSTANCE_MATERIAL));
1193 pinstmat->setTarget(xsAnyURI(*pdomgeom,
string(
"#")+geomid+
string(
"_mat")));
1194 pinstmat->setSymbol(
"mat0");
1195 write_visual =
true;
1199 if (!write_visual) {
1202 string geomid = _ComputeId(str(boost::format(
"g%s_%s_geom%d")%strModelUri%linksid%igeom));
1203 domGeometryRef pdomgeom = _WriteGeometry(geometry, geomid);
1204 domInstance_geometryRef pinstgeom = daeSafeCast<domInstance_geometry>(pnode->add(COLLADA_ELEMENT_INSTANCE_GEOMETRY));
1205 pinstgeom->setUrl((
string(
"#")+geomid).c_str());
1208 _WriteMaterial(pdomgeom->getID(), material);
1209 domBind_materialRef pmat = daeSafeCast<domBind_material>(pinstgeom->add(COLLADA_ELEMENT_BIND_MATERIAL));
1210 domBind_material::domTechnique_commonRef pmattec = daeSafeCast<domBind_material::domTechnique_common>(pmat->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
1211 domInstance_materialRef pinstmat = daeSafeCast<domInstance_material>(pmattec->add(COLLADA_ELEMENT_INSTANCE_MATERIAL));
1212 pinstmat->setTarget(xsAnyURI(*pdomgeom,
string(
"#")+geomid+
string(
"_mat")));
1213 pinstmat->setSymbol(
"mat0");
1217 _WriteTransformation(pnode, geometry_origin);
1220 FOREACHC(itjoint, plink->child_joints) {
1221 urdf::JointSharedPtr pjoint = *itjoint;
1222 int index = _mapjointindices[pjoint];
1225 domLink::domAttachment_fullRef attachment_full = daeSafeCast<domLink::domAttachment_full>(pdomlink->add(COLLADA_ELEMENT_ATTACHMENT_FULL));
1226 string jointid = str(boost::format(
"%s/%s")%strModelUri%_ComputeId(pjoint->name));
1227 attachment_full->setJoint(jointid.c_str());
1229 LINKOUTPUT childinfo = _WriteLink(_robot.getLink(pjoint->child_link_name), attachment_full, pnode, strModelUri);
1231 out.
listusedlinks.push_back(make_pair(itused->first,linksid+
string(
"/")+itused->second));
1234 out.
listprocesseddofs.push_back(make_pair(itprocessed->first,nodesid+
string(
"/")+itprocessed->second));
1237 out.
_maplinkposes[itlinkpos->first] = _poseMult(pjoint->parent_to_joint_origin_transform,itlinkpos->second);
1240 string jointnodesid = _ComputeId(str(boost::format(
"node_%s_axis0")%pjoint->name));
1241 switch(pjoint->type) {
1242 case urdf::Joint::REVOLUTE:
1243 case urdf::Joint::CONTINUOUS:
1244 case urdf::Joint::FIXED: {
1245 domRotateRef protate = daeSafeCast<domRotate>(childinfo.
pnode->add(COLLADA_ELEMENT_ROTATE,0));
1246 protate->setSid(jointnodesid.c_str());
1247 protate->getValue().setCount(4);
1248 protate->getValue()[0] = pjoint->axis.x;
1249 protate->getValue()[1] = pjoint->axis.y;
1250 protate->getValue()[2] = pjoint->axis.z;
1251 protate->getValue()[3] = 0;
1254 case urdf::Joint::PRISMATIC: {
1255 domTranslateRef ptrans = daeSafeCast<domTranslate>(childinfo.
pnode->add(COLLADA_ELEMENT_TRANSLATE,0));
1256 ptrans->setSid(jointnodesid.c_str());
1257 ptrans->getValue().setCount(3);
1258 ptrans->getValue()[0] = 0;
1259 ptrans->getValue()[1] = 0;
1260 ptrans->getValue()[2] = 0;
1264 ROS_WARN_STREAM(str(boost::format(
"unsupported joint type specified %d")%(
int)pjoint->type));
1268 _WriteTransformation(attachment_full, pjoint->parent_to_joint_origin_transform);
1269 _WriteTransformation(childinfo.
pnode, pjoint->parent_to_joint_origin_transform);
1270 _WriteTransformation(childinfo.
pnode, geometry_origin_inv);
1271 out.
listprocesseddofs.push_back(make_pair(index,
string(childinfo.
pnode->getSid())+
string(
"/")+jointnodesid));
1277 out.
plink = pdomlink;
1282 domGeometryRef
_WriteGeometry(urdf::GeometrySharedPtr geometry,
const std::string& geometry_id, urdf::Pose *org_trans = NULL)
1284 domGeometryRef cgeometry = daeSafeCast<domGeometry>(_geometriesLib->add(COLLADA_ELEMENT_GEOMETRY));
1285 cgeometry->setId(geometry_id.c_str());
1286 switch (geometry->type) {
1287 case urdf::Geometry::MESH: {
1288 urdf::Mesh* urdf_mesh = (urdf::Mesh*) geometry.get();
1289 cgeometry->setName(urdf_mesh->filename.c_str());
1290 _loadMesh(urdf_mesh->filename, cgeometry, urdf_mesh->scale, org_trans);
1293 case urdf::Geometry::SPHERE: {
1294 shapes::Sphere sphere(static_cast<urdf::Sphere*>(geometry.get())->radius);
1296 _loadVertices(mesh.get(), cgeometry);
1299 case urdf::Geometry::BOX: {
1300 shapes::Box box(static_cast<urdf::Box*>(geometry.get())->dim.x,
1301 static_cast<urdf::Box*>(geometry.get())->dim.y,
1302 static_cast<urdf::Box*>(geometry.get())->dim.z);
1304 _loadVertices(mesh.get(), cgeometry);
1307 case urdf::Geometry::CYLINDER: {
1309 static_cast<urdf::Cylinder*>(geometry.get())->length);
1311 _loadVertices(mesh.get(), cgeometry);
1315 throw ColladaUrdfException(str(boost::format(
"undefined geometry type %d, name %s")%(
int)geometry->type%geometry_id));
1323 string effid = geometry_id+string(
"_eff");
1324 string matid = geometry_id+string(
"_mat");
1325 domMaterialRef pdommat = daeSafeCast<domMaterial>(_materialsLib->add(COLLADA_ELEMENT_MATERIAL));
1326 pdommat->setId(matid.c_str());
1327 domInstance_effectRef pdominsteff = daeSafeCast<domInstance_effect>(pdommat->add(COLLADA_ELEMENT_INSTANCE_EFFECT));
1328 pdominsteff->setUrl((
string(
"#")+effid).c_str());
1330 urdf::Color ambient, diffuse;
1331 ambient.init(
"0.1 0.1 0.1 0");
1332 diffuse.init(
"1 1 1 0");
1335 ambient.r = diffuse.r = material->color.r;
1336 ambient.g = diffuse.g = material->color.g;
1337 ambient.b = diffuse.b = material->color.b;
1338 ambient.a = diffuse.a = material->color.a;
1341 domEffectRef effect = _WriteEffect(geometry_id, ambient, diffuse);
1344 domMaterialRef dommaterial = daeSafeCast<domMaterial>(_materialsLib->add(COLLADA_ELEMENT_MATERIAL));
1345 string material_id = geometry_id + string(
"_mat");
1346 dommaterial->setId(material_id.c_str());
1349 domInstance_effectRef instance_effect = daeSafeCast<domInstance_effect>(dommaterial->add(COLLADA_ELEMENT_INSTANCE_EFFECT));
1350 string effect_id(effect->getId());
1351 instance_effect->setUrl((
string(
"#") + effect_id).c_str());
1355 domEffectRef pdomeff = _WriteEffect(effid, ambient, diffuse);
1362 ipmout->pmout = pmout;
1363 ipmout->ipm = daeSafeCast<domInstance_physics_model>(parent->add(COLLADA_ELEMENT_INSTANCE_PHYSICS_MODEL));
1364 string bodyid = _ComputeId(str(boost::format(
"visual%d")%
id));
1365 ipmout->ipm->setParent(xsAnyURI(*ipmout->ipm,
string(
"#")+bodyid));
1366 string symscope, refscope;
1367 if( sidscope.size() > 0 ) {
1368 symscope = sidscope+string(
"_");
1369 refscope = sidscope+string(
"/");
1371 string ipmsid = str(boost::format(
"%s_inst")%pmout->pmodel->getID());
1372 ipmout->ipm->setUrl(str(boost::format(
"#%s")%pmout->pmodel->getID()).c_str());
1373 ipmout->ipm->setSid(ipmsid.c_str());
1375 string kmodelid = _ComputeKinematics_modelId(
id);
1376 for(
size_t i = 0; i < pmout->vrigidbodysids.size(); ++i) {
1377 domInstance_rigid_bodyRef pirb = daeSafeCast<domInstance_rigid_body>(ipmout->ipm->add(COLLADA_ELEMENT_INSTANCE_RIGID_BODY));
1378 pirb->setBody(pmout->vrigidbodysids[i].c_str());
1379 pirb->setTarget(xsAnyURI(*pirb,str(boost::format(
"#v%s_node%d")%kmodelid%i)));
1388 pmout->pmodel = daeSafeCast<domPhysics_model>(_physicsModelsLib->add(COLLADA_ELEMENT_PHYSICS_MODEL));
1389 string pmodelid = str(boost::format(
"pmodel%d")%
id);
1390 pmout->pmodel->setId(pmodelid.c_str());
1391 pmout->pmodel->setName(_robot.getName().c_str());
1393 domRigid_bodyRef rigid_body = daeSafeCast<domRigid_body>(pmout->pmodel->add(COLLADA_ELEMENT_RIGID_BODY));
1394 string rigidsid = str(boost::format(
"rigid%d")%_maplinkindices[itlink->second]);
1395 pmout->vrigidbodysids.push_back(rigidsid);
1396 rigid_body->setSid(rigidsid.c_str());
1397 rigid_body->setName(itlink->second->name.c_str());
1398 domRigid_body::domTechnique_commonRef ptec = daeSafeCast<domRigid_body::domTechnique_common>(rigid_body->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
1399 urdf::InertialSharedPtr inertial = itlink->second->inertial;
1401 daeSafeCast<domRigid_body::domTechnique_common::domDynamic>(ptec->add(COLLADA_ELEMENT_DYNAMIC))->setValue(xsBoolean(
true));
1402 domTargetable_floatRef mass = daeSafeCast<domTargetable_float>(ptec->add(COLLADA_ELEMENT_MASS));
1403 mass->setValue(inertial->mass);
1404 double fCovariance[9] = { inertial->ixx, inertial->ixy, inertial->ixz, inertial->ixy, inertial->iyy, inertial->iyz, inertial->ixz, inertial->iyz, inertial->izz};
1405 double eigenvalues[3], eigenvectors[9];
1407 boost::array<double,12> minertiaframe;
1408 for(
int j = 0; j < 3; ++j) {
1409 minertiaframe[4*0+j] = eigenvectors[3*j];
1410 minertiaframe[4*1+j] = eigenvectors[3*j+1];
1411 minertiaframe[4*2+j] = eigenvectors[3*j+2];
1413 urdf::Pose tinertiaframe;
1414 tinertiaframe.rotation = _quatFromMatrix(minertiaframe);
1415 tinertiaframe = _poseMult(inertial->origin,tinertiaframe);
1417 domTargetable_float3Ref pinertia = daeSafeCast<domTargetable_float3>(ptec->add(COLLADA_ELEMENT_INERTIA));
1418 pinertia->getValue().setCount(3);
1419 pinertia->getValue()[0] = eigenvalues[0];
1420 pinertia->getValue()[1] = eigenvalues[1];
1421 pinertia->getValue()[2] = eigenvalues[2];
1422 urdf::Pose posemassframe = _poseMult(maplinkposes.find(itlink->second)->second,tinertiaframe);
1423 _WriteTransformation(ptec->add(COLLADA_ELEMENT_MASS_FRAME), posemassframe);
1443 std::vector<char> buffer;
1447 Assimp::Importer importer;
1450 const aiScene* scene = importer.ReadFileFromMemory(reinterpret_cast<const void*>(&buffer[0]), buffer.size(),
1451 aiProcess_Triangulate |
1452 aiProcess_JoinIdenticalVertices |
1453 aiProcess_SortByPType |
1454 aiProcess_OptimizeGraph |
1455 aiProcess_OptimizeMeshes,
"stl");
1459 domMeshRef pdommesh = daeSafeCast<domMesh>(pdomgeom->add(COLLADA_ELEMENT_MESH));
1460 domSourceRef pvertsource = daeSafeCast<domSource>(pdommesh->add(COLLADA_ELEMENT_SOURCE));
1461 domAccessorRef pacc;
1462 domFloat_arrayRef parray;
1464 pvertsource->setId(str(boost::format(
"%s_positions")%pdomgeom->getID()).c_str());
1466 parray = daeSafeCast<domFloat_array>(pvertsource->add(COLLADA_ELEMENT_FLOAT_ARRAY));
1467 parray->setId(str(boost::format(
"%s_positions-array")%pdomgeom->getID()).c_str());
1468 parray->setDigits(6);
1470 domSource::domTechnique_commonRef psourcetec = daeSafeCast<domSource::domTechnique_common>(pvertsource->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
1471 pacc = daeSafeCast<domAccessor>(psourcetec->add(COLLADA_ELEMENT_ACCESSOR));
1472 pacc->setSource(xsAnyURI(*parray, std::string(
"#")+
string(parray->getID())));
1475 domParamRef px = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM));
1476 px->setName(
"X"); px->setType(
"float");
1477 domParamRef py = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM));
1478 py->setName(
"Y"); py->setType(
"float");
1479 domParamRef pz = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM));
1480 pz->setName(
"Z"); pz->setType(
"float");
1482 domVerticesRef pverts = daeSafeCast<domVertices>(pdommesh->add(COLLADA_ELEMENT_VERTICES));
1484 pverts->setId(
"vertices");
1485 domInput_localRef pvertinput = daeSafeCast<domInput_local>(pverts->add(COLLADA_ELEMENT_INPUT));
1486 pvertinput->setSemantic(
"POSITION");
1487 pvertinput->setSource(domUrifragment(*pvertsource, std::string(
"#")+std::string(pvertsource->getID())));
1489 _buildAiMesh(scene,scene->mRootNode,pdommesh,parray, pdomgeom->getID(), urdf::Vector3(1,1,1));
1490 pacc->setCount(parray->getCount());
1493 void _loadMesh(std::string
const& filename, domGeometryRef pdomgeom,
const urdf::Vector3& scale, urdf::Pose *org_trans)
1495 const aiScene* scene = _importer.ReadFile(filename, aiProcess_SortByPType|aiProcess_Triangulate);
1497 ROS_WARN(
"failed to load resource %s",filename.c_str());
1500 if( !scene->mRootNode ) {
1501 ROS_WARN(
"resource %s has no data",filename.c_str());
1504 if (!scene->HasMeshes()) {
1505 ROS_WARN_STREAM(str(boost::format(
"No meshes found in file %s")%filename));
1508 domMeshRef pdommesh = daeSafeCast<domMesh>(pdomgeom->add(COLLADA_ELEMENT_MESH));
1509 domSourceRef pvertsource = daeSafeCast<domSource>(pdommesh->add(COLLADA_ELEMENT_SOURCE));
1510 domAccessorRef pacc;
1511 domFloat_arrayRef parray;
1513 pvertsource->setId(str(boost::format(
"%s_positions")%pdomgeom->getID()).c_str());
1515 parray = daeSafeCast<domFloat_array>(pvertsource->add(COLLADA_ELEMENT_FLOAT_ARRAY));
1516 parray->setId(str(boost::format(
"%s_positions-array")%pdomgeom->getID()).c_str());
1517 parray->setDigits(6);
1519 domSource::domTechnique_commonRef psourcetec = daeSafeCast<domSource::domTechnique_common>(pvertsource->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
1520 pacc = daeSafeCast<domAccessor>(psourcetec->add(COLLADA_ELEMENT_ACCESSOR));
1521 pacc->setSource(xsAnyURI(*parray,
string(
"#")+
string(parray->getID())));
1524 domParamRef px = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM));
1525 px->setName(
"X"); px->setType(
"float");
1526 domParamRef py = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM));
1527 py->setName(
"Y"); py->setType(
"float");
1528 domParamRef pz = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM));
1529 pz->setName(
"Z"); pz->setType(
"float");
1531 domVerticesRef pverts = daeSafeCast<domVertices>(pdommesh->add(COLLADA_ELEMENT_VERTICES));
1533 pverts->setId(
"vertices");
1534 domInput_localRef pvertinput = daeSafeCast<domInput_local>(pverts->add(COLLADA_ELEMENT_INPUT));
1535 pvertinput->setSemantic(
"POSITION");
1536 pvertinput->setSource(domUrifragment(*pvertsource,
string(
"#")+
string(pvertsource->getID())));
1538 _buildAiMesh(scene,scene->mRootNode,pdommesh,parray, pdomgeom->getID(),scale,org_trans);
1539 pacc->setCount(parray->getCount());
1542 void _buildAiMesh(
const aiScene* scene, aiNode* node, domMeshRef pdommesh, domFloat_arrayRef parray,
const string& geomid,
const urdf::Vector3& scale, urdf::Pose *org_trans = NULL)
1547 aiMatrix4x4 transform = node->mTransformation;
1548 aiNode *pnode = node->mParent;
1552 if (pnode->mParent != NULL) {
1553 transform = pnode->mTransformation * transform;
1555 pnode = pnode->mParent;
1559 uint32_t vertexOffset = parray->getCount();
1560 uint32_t nTotalVertices=0;
1561 for (uint32_t i = 0; i < node->mNumMeshes; i++) {
1562 aiMesh* input_mesh = scene->mMeshes[node->mMeshes[i]];
1563 nTotalVertices += input_mesh->mNumVertices;
1566 parray->getValue().grow(parray->getCount()+nTotalVertices*3);
1567 parray->setCount(parray->getCount()+nTotalVertices);
1569 for (uint32_t i = 0; i < node->mNumMeshes; i++) {
1570 aiMesh* input_mesh = scene->mMeshes[node->mMeshes[i]];
1571 for (uint32_t j = 0; j < input_mesh->mNumVertices; j++) {
1572 aiVector3D p = input_mesh->mVertices[j];
1579 urdf::Vector3 nv = _poseMult(*org_trans, vv);
1580 parray->getValue().append(nv.x);
1581 parray->getValue().append(nv.y);
1582 parray->getValue().append(nv.z);
1585 parray->getValue().append(p.x*scale.x);
1586 parray->getValue().append(p.y*scale.y);
1587 parray->getValue().append(p.z*scale.z);
1594 vector<int> triangleindices, otherindices;
1595 int nNumOtherPrimitives = 0;
1596 for (uint32_t i = 0; i < node->mNumMeshes; i++) {
1597 aiMesh* input_mesh = scene->mMeshes[node->mMeshes[i]];
1598 uint32_t indexCount = 0, otherIndexCount = 0;
1599 for (uint32_t j = 0; j < input_mesh->mNumFaces; j++) {
1600 aiFace& face = input_mesh->mFaces[j];
1601 if( face.mNumIndices == 3 ) {
1602 indexCount += face.mNumIndices;
1605 otherIndexCount += face.mNumIndices;
1608 triangleindices.reserve(triangleindices.size()+indexCount);
1609 otherindices.reserve(otherindices.size()+otherIndexCount);
1610 for (uint32_t j = 0; j < input_mesh->mNumFaces; j++) {
1611 aiFace& face = input_mesh->mFaces[j];
1612 if( face.mNumIndices == 3 ) {
1613 triangleindices.push_back(vertexOffset+face.mIndices[0]);
1614 triangleindices.push_back(vertexOffset+face.mIndices[1]);
1615 triangleindices.push_back(vertexOffset+face.mIndices[2]);
1618 for (uint32_t k = 0; k < face.mNumIndices; ++k) {
1619 otherindices.push_back(face.mIndices[k]+vertexOffset);
1621 nNumOtherPrimitives++;
1624 vertexOffset += input_mesh->mNumVertices;
1627 if( triangleindices.size() > 0 ) {
1628 domTrianglesRef ptris = daeSafeCast<domTriangles>(pdommesh->add(COLLADA_ELEMENT_TRIANGLES));
1629 ptris->setCount(triangleindices.size()/3);
1630 ptris->setMaterial(
"mat0");
1631 domInput_local_offsetRef pvertoffset = daeSafeCast<domInput_local_offset>(ptris->add(COLLADA_ELEMENT_INPUT));
1632 pvertoffset->setSemantic(
"VERTEX");
1633 pvertoffset->setOffset(0);
1634 pvertoffset->setSource(domUrifragment(*pdommesh->getVertices(), str(boost::format(
"#%s/vertices")%geomid)));
1635 domPRef pindices = daeSafeCast<domP>(ptris->add(COLLADA_ELEMENT_P));
1636 pindices->getValue().setCount(triangleindices.size());
1637 for(
size_t ind = 0; ind < triangleindices.size(); ++ind) {
1638 pindices->getValue()[ind] = triangleindices[ind];
1642 if( nNumOtherPrimitives > 0 ) {
1643 domPolylistRef ptris = daeSafeCast<domPolylist>(pdommesh->add(COLLADA_ELEMENT_POLYLIST));
1644 ptris->setCount(nNumOtherPrimitives);
1645 ptris->setMaterial(
"mat0");
1646 domInput_local_offsetRef pvertoffset = daeSafeCast<domInput_local_offset>(ptris->add(COLLADA_ELEMENT_INPUT));
1647 pvertoffset->setSemantic(
"VERTEX");
1648 pvertoffset->setSource(domUrifragment(*pdommesh->getVertices(), str(boost::format(
"#%s/vertices")%geomid)));
1649 domPRef pindices = daeSafeCast<domP>(ptris->add(COLLADA_ELEMENT_P));
1650 pindices->getValue().setCount(otherindices.size());
1651 for(
size_t ind = 0; ind < otherindices.size(); ++ind) {
1652 pindices->getValue()[ind] = otherindices[ind];
1655 domPolylist::domVcountRef pcount = daeSafeCast<domPolylist::domVcount>(ptris->add(COLLADA_ELEMENT_VCOUNT));
1656 pcount->getValue().setCount(nNumOtherPrimitives);
1657 uint32_t offset = 0;
1658 for (uint32_t i = 0; i < node->mNumMeshes; i++) {
1659 aiMesh* input_mesh = scene->mMeshes[node->mMeshes[i]];
1660 for (uint32_t j = 0; j < input_mesh->mNumFaces; j++) {
1661 aiFace& face = input_mesh->mFaces[j];
1662 if( face.mNumIndices > 3 ) {
1663 pcount->getValue()[offset++] = face.mNumIndices;
1670 for (uint32_t i=0; i < node->mNumChildren; ++i) {
1671 _buildAiMesh(scene, node->mChildren[i], pdommesh,parray,geomid,scale,org_trans);
1676 domEffectRef
_WriteEffect(std::string
const& effect_id, urdf::Color
const& color_ambient, urdf::Color
const& color_diffuse)
1679 domEffectRef effect = daeSafeCast<domEffect>(_effectsLib->add(COLLADA_ELEMENT_EFFECT));
1680 effect->setId(effect_id.c_str());
1683 domProfile_commonRef profile = daeSafeCast<domProfile_common>(effect->add(COLLADA_ELEMENT_PROFILE_COMMON));
1686 domProfile_common::domTechniqueRef technique = daeSafeCast<domProfile_common::domTechnique>(profile->add(COLLADA_ELEMENT_TECHNIQUE));
1689 domProfile_common::domTechnique::domPhongRef phong = daeSafeCast<domProfile_common::domTechnique::domPhong>(technique->add(COLLADA_ELEMENT_PHONG));
1692 domFx_common_color_or_textureRef ambient = daeSafeCast<domFx_common_color_or_texture>(phong->add(COLLADA_ELEMENT_AMBIENT));
1695 domFx_common_color_or_texture::domColorRef ambient_color = daeSafeCast<domFx_common_color_or_texture::domColor>(ambient->add(COLLADA_ELEMENT_COLOR));
1696 ambient_color->getValue().setCount(4);
1697 ambient_color->getValue()[0] = color_ambient.r;
1698 ambient_color->getValue()[1] = color_ambient.g;
1699 ambient_color->getValue()[2] = color_ambient.b;
1700 ambient_color->getValue()[3] = color_ambient.a;
1706 domFx_common_color_or_textureRef diffuse = daeSafeCast<domFx_common_color_or_texture>(phong->add(COLLADA_ELEMENT_DIFFUSE));
1709 domFx_common_color_or_texture::domColorRef diffuse_color = daeSafeCast<domFx_common_color_or_texture::domColor>(diffuse->add(COLLADA_ELEMENT_COLOR));
1710 diffuse_color->getValue().setCount(4);
1711 diffuse_color->getValue()[0] = color_diffuse.r;
1712 diffuse_color->getValue()[1] = color_diffuse.g;
1713 diffuse_color->getValue()[2] = color_diffuse.b;
1714 diffuse_color->getValue()[3] = color_diffuse.a;
1735 domRotateRef prot = daeSafeCast<domRotate>(pelt->add(COLLADA_ELEMENT_ROTATE,0));
1736 domTranslateRef ptrans = daeSafeCast<domTranslate>(pelt->add(COLLADA_ELEMENT_TRANSLATE,0));
1737 ptrans->getValue().setCount(3);
1738 ptrans->getValue()[0] = t.position.x;
1739 ptrans->getValue()[1] = t.position.y;
1740 ptrans->getValue()[2] = t.position.z;
1742 prot->getValue().setCount(4);
1744 double sinang = t.rotation.x*t.rotation.x+t.rotation.y*t.rotation.y+t.rotation.z*t.rotation.z;
1745 if( std::fabs(sinang) < 1e-10 ) {
1746 prot->getValue()[0] = 1;
1747 prot->getValue()[1] = 0;
1748 prot->getValue()[2] = 0;
1749 prot->getValue()[3] = 0;
1752 urdf::Rotation quat;
1753 if( t.rotation.w < 0 ) {
1754 quat.x = -t.rotation.x;
1755 quat.y = -t.rotation.y;
1756 quat.z = -t.rotation.z;
1757 quat.w = -t.rotation.w;
1762 sinang = std::sqrt(sinang);
1763 prot->getValue()[0] = quat.x/sinang;
1764 prot->getValue()[1] = quat.y/sinang;
1765 prot->getValue()[2] = quat.z/sinang;
1773 FOREACHC(it, _iasout->vkinematicsbindings) {
1774 domBind_kinematics_modelRef pmodelbind = daeSafeCast<domBind_kinematics_model>(_scene.kiscene->add(COLLADA_ELEMENT_BIND_KINEMATICS_MODEL));
1775 pmodelbind->setNode(it->second.c_str());
1776 daeSafeCast<domCommon_param>(pmodelbind->add(COLLADA_ELEMENT_PARAM))->setValue(it->first.c_str());
1779 domBind_joint_axisRef pjointbind = daeSafeCast<domBind_joint_axis>(_scene.kiscene->add(COLLADA_ELEMENT_BIND_JOINT_AXIS));
1780 pjointbind->setTarget(it->jointnodesid.c_str());
1781 daeSafeCast<domCommon_param>(pjointbind->add(COLLADA_ELEMENT_AXIS)->add(COLLADA_TYPE_PARAM))->setValue(it->axissid.c_str());
1782 daeSafeCast<domCommon_param>(pjointbind->add(COLLADA_ELEMENT_VALUE)->add(COLLADA_TYPE_PARAM))->setValue(it->valuesid.c_str());
1787 static urdf::Vector3
_poseMult(
const urdf::Pose& p,
const urdf::Vector3& v)
1789 double ww = 2 * p.rotation.x * p.rotation.x;
1790 double wx = 2 * p.rotation.x * p.rotation.y;
1791 double wy = 2 * p.rotation.x * p.rotation.z;
1792 double wz = 2 * p.rotation.x * p.rotation.w;
1793 double xx = 2 * p.rotation.y * p.rotation.y;
1794 double xy = 2 * p.rotation.y * p.rotation.z;
1795 double xz = 2 * p.rotation.y * p.rotation.w;
1796 double yy = 2 * p.rotation.z * p.rotation.z;
1797 double yz = 2 * p.rotation.z * p.rotation.w;
1799 vnew.x = (1-xx-yy) * v.x + (wx-yz) * v.y + (wy+xz)*v.z + p.position.x;
1800 vnew.y = (wx+yz) * v.x + (1-ww-yy) * v.y + (xy-wz)*v.z + p.position.y;
1801 vnew.z = (wy-xz) * v.x + (xy+wz) * v.y + (1-ww-xx)*v.z + p.position.z;
1808 pinv.rotation.x = -p.rotation.x;
1809 pinv.rotation.y = -p.rotation.y;
1810 pinv.rotation.z = -p.rotation.z;
1811 pinv.rotation.w = p.rotation.w;
1812 urdf::Vector3 t = _poseMult(pinv,p.position);
1813 pinv.position.x = -t.x;
1814 pinv.position.y = -t.y;
1815 pinv.position.z = -t.z;
1819 static urdf::Rotation
_quatMult(
const urdf::Rotation& quat0,
const urdf::Rotation& quat1)
1822 q.x = quat0.w*quat1.x + quat0.x*quat1.w + quat0.y*quat1.z - quat0.z*quat1.y;
1823 q.y = quat0.w*quat1.y + quat0.y*quat1.w + quat0.z*quat1.x - quat0.x*quat1.z;
1824 q.z = quat0.w*quat1.z + quat0.z*quat1.w + quat0.x*quat1.y - quat0.y*quat1.x;
1825 q.w = quat0.w*quat1.w - quat0.x*quat1.x - quat0.y*quat1.y - quat0.z*quat1.z;
1826 double fnorm = std::sqrt(q.x*q.x+q.y*q.y+q.z*q.z+q.w*q.w);
1835 static urdf::Pose
_poseMult(
const urdf::Pose& p0,
const urdf::Pose& p1)
1838 p.position = _poseMult(p0,p1.position);
1839 p.rotation = _quatMult(p0.rotation,p1.rotation);
1846 double tr = mat[4*0+0] + mat[4*1+1] + mat[4*2+2];
1849 rot.x = (mat[4*2+1] - mat[4*1+2]);
1850 rot.y = (mat[4*0+2] - mat[4*2+0]);
1851 rot.z = (mat[4*1+0] - mat[4*0+1]);
1855 if (mat[4*1+1] > mat[4*0+0]) {
1856 if (mat[4*2+2] > mat[4*1+1]) {
1857 rot.z = (mat[4*2+2] - (mat[4*0+0] + mat[4*1+1])) + 1;
1858 rot.x = (mat[4*2+0] + mat[4*0+2]);
1859 rot.y = (mat[4*1+2] + mat[4*2+1]);
1860 rot.w = (mat[4*1+0] - mat[4*0+1]);
1863 rot.y = (mat[4*1+1] - (mat[4*2+2] + mat[4*0+0])) + 1;
1864 rot.z = (mat[4*1+2] + mat[4*2+1]);
1865 rot.x = (mat[4*0+1] + mat[4*1+0]);
1866 rot.w = (mat[4*0+2] - mat[4*2+0]);
1869 else if (mat[4*2+2] > mat[4*0+0]) {
1870 rot.z = (mat[4*2+2] - (mat[4*0+0] + mat[4*1+1])) + 1;
1871 rot.x = (mat[4*2+0] + mat[4*0+2]);
1872 rot.y = (mat[4*1+2] + mat[4*2+1]);
1873 rot.w = (mat[4*1+0] - mat[4*0+1]);
1876 rot.x = (mat[4*0+0] - (mat[4*1+1] + mat[4*2+2])) + 1;
1877 rot.y = (mat[4*0+1] + mat[4*1+0]);
1878 rot.z = (mat[4*2+0] + mat[4*0+2]);
1879 rot.w = (mat[4*2+1] - mat[4*1+2]);
1882 double fnorm = std::sqrt(rot.x*rot.x+rot.y*rot.y+rot.z*rot.z+rot.w*rot.w);
1893 return _ComputeId(str(boost::format(
"kmodel%d")%
id));
1899 std::string newname = name;
1900 for(
size_t i = 0; i < newname.size(); ++i) {
1901 if( newname[i] ==
'/' || newname[i] ==
' ' || newname[i] ==
'.' ) {
1937 ColladaUrdfException::ColladaUrdfException(std::string
const& what)
1938 :
std::runtime_error(what)
1945 std::cerr << std::endl <<
"Error converting document" << std::endl;
domInstance_with_extraRef piscene
static urdf::Vector3 _poseMult(const urdf::Pose &p, const urdf::Vector3 &v)
std::map< urdf::MaterialConstSharedPtr, int > _mapmaterialindices
axis_sids(const string &axissid, const string &valuesid, const string &jointnodesid)
std::map< urdf::JointConstSharedPtr, int > _mapjointindices
boost::shared_ptr< instance_articulated_system_output > _iasout
ColladaWriter(const urdf::Model &robot, int writeoptions)
void _WriteBindingsInstance_kinematics_scene()
domGeometryRef _WriteGeometry(urdf::GeometrySharedPtr geometry, const std::string &geometry_id, urdf::Pose *org_trans=NULL)
static urdf::Pose _poseMult(const urdf::Pose &p0, const urdf::Pose &p1)
domLibrary_effectsRef _effectsLib
domLibrary_kinematics_modelsRef _kinematicsModelsLib
static std::string _ComputeId(const std::string &name)
computes a collada-compliant sid from the urdf name
static urdf::Rotation _quatMult(const urdf::Rotation &quat0, const urdf::Rotation &quat1)
virtual boost::shared_ptr< kinematics_model_output > WriteKinematics_model(int id)
char getOsSeparator() const
Assimp::IOStream * Open(const char *file, const char *mode)
domInstance_kinematics_sceneRef kiscene
domCOLLADA::domSceneRef _globalscene
std::vector< std::string > vrigidbodysids
same ordering as the physics indices
list< pair< int, string > > listprocesseddofs
std::vector< axis_sids > vaxissids
boost::shared_ptr< kinematics_model_output > kmout
Implements writing urdf::Model objects to a COLLADA DOM.
boost::shared_ptr< kinematics_model_output > kmout
boost::shared_ptr< instance_physics_model_output > _WriteInstance_physics_model(int id, daeElementRef parent, const string &sidscope, const MAPLINKPOSES &maplinkposes)
domLibrary_articulated_systemsRef _articulatedSystemsLib
void _loadMesh(std::string const &filename, domGeometryRef pdomgeom, const urdf::Vector3 &scale, urdf::Pose *org_trans)
ResourceIOStream(const resource_retriever::MemoryResource &res)
domInstance_with_extraRef viscene
domVisual_sceneRef vscene
std::vector< std::pair< std::string, std::string > > vkinematicsbindings
domPhysics_sceneRef pscene
ResourceIOStream is copied from rviz (BSD, Willow Garage)
domLibrary_kinematics_scenesRef _kinematicsScenesLib
domTechniqueRef _sensorsLib
custom sensors library
boost::shared_ptr< instance_kinematics_model_output > _ikmout
std::map< urdf::LinkConstSharedPtr, urdf::Pose > MAPLINKPOSES
domLibrary_visual_scenesRef _visualScenesLib
resource_retriever::MemoryResource res_
static std::string _ComputeKinematics_modelId(int id)
domLibrary_materialsRef _materialsLib
Triangle(const urdf::Vector3 &_p1, const urdf::Vector3 &_p2, const urdf::Vector3 &_p3)
std::vector< axis_output > vaxissids
domInstance_articulated_systemRef ias
list< pair< int, string > > listusedlinks
boost::shared_ptr< physics_model_output > pmout
ResourceIOSystem is copied from rviz (BSD, Willow Garage)
domLibrary_physics_modelsRef _physicsModelsLib
domLibrary_geometriesRef _geometriesLib
boost::shared_ptr< physics_model_output > pmout
virtual void handleError(daeString msg)
static double to_degrees(double radians)
domLibrary_physics_scenesRef _physicsScenesLib
urdf::JointConstSharedPtr pjoint
bool WriteUrdfModelToColladaFile(urdf::Model const &robot_model, std::string const &file)
MAPLINKPOSES _maplinkposes
domEffectRef _WriteEffect(std::string const &effect_id, urdf::Color const &color_ambient, urdf::Color const &color_diffuse)
#define ROS_WARN_STREAM(args)
const urdf::Model & _robot
#define ROS_DEBUG_STREAM(args)
bool writeTo(string const &file)
std::vector< std::string > vlinksids
Assimp::Importer _importer
domPhysics_modelRef pmodel
boost::shared_ptr< physics_model_output > WritePhysics_model(int id, const MAPLINKPOSES &maplinkposes)
std::vector< std::string > vlinksids
void _WriteRobot(int id=0)
Write kinematic body in a given scene.
size_t Read(void *buffer, size_t size, size_t count)
domKinematics_sceneRef kscene
MAPLINKPOSES _maplinkposes
void _WriteTransformation(daeElementRef pelt, const urdf::Pose &t)
Write transformation.
std::map< urdf::LinkConstSharedPtr, int > _maplinkindices
void writeSTLBinary(const Mesh *mesh, std::vector< char > &buffer)
virtual void handleWarning(daeString msg)
std::vector< axis_sids > vaxissids
static urdf::Pose _poseInverse(const urdf::Pose &p)
static urdf::Rotation _quatFromMatrix(const boost::array< double, 12 > &mat)
virtual void _WriteInstance_kinematics_model(daeElementRef parent, const string &sidscope, int id)
Write kinematic body in a given scene.
domInstance_kinematics_modelRef ikm
domInstance_physics_modelRef ipm
std::vector< std::pair< std::string, std::string > > vkinematicsbindings
void _WriteMaterial(const string &geometry_id, urdf::MaterialSharedPtr material)
void _buildAiMesh(const aiScene *scene, aiNode *node, domMeshRef pdommesh, domFloat_arrayRef parray, const string &geomid, const urdf::Vector3 &scale, urdf::Pose *org_trans=NULL)
Mesh * createMeshFromShape(const Shape *shape)
resource_retriever::Retriever retriever_
void _loadVertices(const shapes::Mesh *mesh, domGeometryRef pdomgeom)
size_t Write(const void *buffer, size_t size, size_t count)
aiReturn Seek(size_t offset, aiOrigin origin)
bool Exists(const char *file) const
domKinematics_modelRef kmodel
void Close(Assimp::IOStream *stream)
virtual LINKOUTPUT _WriteLink(urdf::LinkConstSharedPtr plink, daeElementRef pkinparent, domNodeRef pnodeparent, const string &strModelUri)
Write link of a kinematic body.