17 #include "libopenrave.h"
19 #include <boost/scoped_ptr.hpp>
20 #include <boost/utility.hpp>
21 #include <boost/thread/once.hpp>
27 #include <sys/types.h>
32 #include "plugindatabase.h"
34 #include <boost/lexical_cast.hpp>
36 #define LIBXML_SAX1_ENABLED
37 #include <libxml/globals.h>
38 #include <libxml/xmlerror.h>
39 #include <libxml/parser.h>
40 #include <libxml/parserInternals.h>
41 #include <libxml/tree.h>
43 #include <libxml/debugXML.h>
44 #include <libxml/xmlmemory.h>
54 #if defined(USE_CRLIBM)
60 #ifdef LIBM_ACCURACY_RESULTS_H
61 #include LIBM_ACCURACY_RESULTS_H
65 #ifdef LIBM_EXP_ACCURATE
74 #ifdef LIBM_LOG_ACCURATE
83 #ifdef LIBM_COS_ACCURATE
92 #ifdef LIBM_SIN_ACCURATE
101 #ifdef LIBM_TAN_ACCURATE
110 #ifdef LIBM_LOG2_ACCURATE
119 #ifdef LIBM_LOG10_ACCURATE
128 #ifdef LIBM_ACOS_ACCURATE
137 #ifdef LIBM_ASIN_ACCURATE
146 #ifdef LIBM_ATAN2_ACCURATE
153 dReal absx, absy, val;
154 if ((x == 0)&&(y == 0)) {
157 absy = y < 0 ? -y : y;
158 absx = x < 0 ? -x : x;
159 if (absy - absx == absy) {
161 return y < 0 ? -M_PI_2 : M_PI_2;
163 if (absx - absy == absx) {
167 else val = atan_rn(y/x);
179 #ifdef LIBM_POW_ACCURATE
188 #ifdef LIBM_SQRT_ACCURATE
202 #else // use all standard libm
204 #if OPENRAVE_PRECISION == 0 // floating-point
226 return logf(f)/logf(2.0f);
239 return atan2f(fy,fx);
272 return log(f)/log(2.0f);
304 class RaveGlobal :
private boost::noncopyable,
public boost::enable_shared_from_this<RaveGlobal>,
public UserData
306 typedef std::map<std::string, CreateXMLReaderFn, CaseInsensitiveCompare> READERSMAP;
314 _nGlobalEnvironmentId = 0;
315 _nDataAccessOptions = 0;
317 _mapinterfacenames[PT_Planner] =
"planner";
318 _mapinterfacenames[PT_Robot] =
"robot";
319 _mapinterfacenames[PT_SensorSystem] =
"sensorsystem";
320 _mapinterfacenames[PT_Controller] =
"controller";
321 _mapinterfacenames[PT_Module] =
"module";
322 _mapinterfacenames[PT_IkSolver] =
"iksolver";
323 _mapinterfacenames[PT_KinBody] =
"kinbody";
324 _mapinterfacenames[PT_PhysicsEngine] =
"physicsengine";
325 _mapinterfacenames[PT_Sensor] =
"sensor";
326 _mapinterfacenames[PT_CollisionChecker] =
"collisionchecker";
327 _mapinterfacenames[PT_Trajectory] =
"trajectory";
328 _mapinterfacenames[PT_Viewer] =
"viewer";
329 _mapinterfacenames[PT_SpaceSampler] =
"spacesampler";
330 BOOST_ASSERT(_mapinterfacenames.size()==PT_NumberOfInterfaces);
336 _mapikparameterization[
IKP_Ray4D] =
"Ray4D";
349 FOREACH(it,_mapikparameterization) {
350 std::string name = it->second;
351 std::transform(name.begin(), name.end(), name.begin(), ::tolower);
352 _mapikparameterizationlower[it->first] = name;
373 _crlibm_fpu_state = crlibm_init();
377 std::locale::global(std::locale::classic());
379 catch(
const std::runtime_error& e) {
380 RAVELOG_WARN(
"failed to set to C locale: %s\n",e.what());
383 _nDebugLevel = level;
384 _pdatabase.reset(
new RaveDatabase());
385 if( !_pdatabase->Init(bLoadAllPlugins) ) {
386 RAVELOG_FATAL(
"failed to create the openrave plugin database\n");
389 char* phomedir = getenv(
"OPENRAVE_HOME");
390 if( phomedir == NULL ) {
392 _homedirectory = string(getenv(
"HOME"))+string(
"/.openrave");
394 _homedirectory = string(getenv(
"HOMEDRIVE"))+string(getenv(
"HOMEPATH"))+string(
"\\.openrave");
398 _homedirectory = phomedir;
401 mkdir(_homedirectory.c_str(),S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH | S_IRWXU);
403 CreateDirectory(_homedirectory.c_str(),NULL);
407 const char* delim =
";";
409 const char* delim =
":";
411 _vdbdirectories.clear();
412 char* pOPENRAVE_PLUGINS = getenv(
"OPENRAVE_DATABASE");
413 if( pOPENRAVE_PLUGINS != NULL ) {
416 _vdbdirectories.push_back(_homedirectory);
426 std::map<int, EnvironmentBase*> mapenvironments;
428 boost::mutex::scoped_lock lock(_mutexinternal);
429 mapenvironments = _mapenvironments;
431 FOREACH(itenv,mapenvironments) {
436 mapenvironments.clear();
437 _mapenvironments.clear();
438 _pdefaultsampler.reset();
442 std::list<boost::function<void()> > listDestroyCallbacks;
444 boost::mutex::scoped_lock lock(_mutexinternal);
445 listDestroyCallbacks.swap(_listDestroyCallbacks);
447 FOREACH(itcallback, listDestroyCallbacks) {
450 listDestroyCallbacks.clear();
454 _pdatabase->Destroy();
462 crlibm_exit(_crlibm_fpu_state);
468 boost::mutex::scoped_lock lock(_mutexinternal);
469 _listDestroyCallbacks.push_back(fn);
474 return _homedirectory;
479 FOREACH(itdirectory,_vdbdirectories) {
480 #ifdef HAVE_BOOST_FILESYSTEM
481 std::string fullfilename = boost::filesystem::absolute(boost::filesystem::path(*itdirectory)/filename).string();
483 std::string fullfilename = *itdirectory;
485 fullfilename += filename;
488 if( !!ifstream(fullfilename.c_str()) ) {
501 _nDebugLevel = level;
514 boost::mutex::scoped_lock lock(global->_mutexinternal);
520 boost::shared_ptr<RaveGlobal> global =
_global.lock();
522 boost::mutex::scoped_lock lock(global->_mutexinternal);
540 READERSMAP::iterator it = _mapreaders[type].find(xmltag);
541 if( it == _mapreaders[type].end() ) {
545 return it->second(pinterface,atts);
552 return _mapinterfacenames;
556 return _mapikparameterizationlower;
558 return _mapikparameterization;
563 std::map<InterfaceType,std::string>::const_iterator it = _mapinterfacenames.find(type);
564 if( it == _mapinterfacenames.end() ) {
573 BOOST_ASSERT(!!_pdatabase);
574 boost::mutex::scoped_lock lock(_mutexinternal);
575 _mapenvironments[++_nGlobalEnvironmentId] = penv;
576 return _nGlobalEnvironmentId;
581 boost::mutex::scoped_lock lock(_mutexinternal);
582 FOREACH(it, _mapenvironments) {
583 if( it->second == penv ) {
584 _mapenvironments.erase(it);
592 boost::mutex::scoped_lock lock(_mutexinternal);
593 FOREACH(it,_mapenvironments) {
594 if( it->second == penv.get() ) {
603 boost::mutex::scoped_lock lock(_mutexinternal);
604 std::map<int, EnvironmentBase*>::iterator it = _mapenvironments.find(
id);
605 if( it == _mapenvironments.end() ) {
608 return it->second->shared_from_this();
613 listenvironments.clear();
614 boost::mutex::scoped_lock lock(_mutexinternal);
615 FOREACH(it,_mapenvironments) {
618 listenvironments.push_back(penv);
625 if( !_pdefaultsampler ) {
626 boost::mutex::scoped_lock lock(_mutexinternal);
627 BOOST_ASSERT( _mapenvironments.size() > 0 );
628 _pdefaultsampler =
GetDatabase()->CreateSpaceSampler(_mapenvironments.begin()->second->shared_from_this(),
"MT19937");
630 return _pdefaultsampler;
633 std::string
FindLocalFile(
const std::string& _filename,
const std::string& curdir)
635 #ifndef HAVE_BOOST_FILESYSTEM
638 if( _filename.size() == 0 ) {
639 return std::string();
642 boost::mutex::scoped_lock lock(_mutexinternal);
643 boost::filesystem::path fullfilename;
644 boost::filesystem::path filename(_filename);
646 if( filename.is_complete() ) {
647 fullfilename = filename;
649 else if( curdir.size() > 0 ) {
650 fullfilename = boost::filesystem::absolute(boost::filesystem::path(curdir)) / filename;
653 fullfilename = boost::filesystem::current_path() / filename;
656 if( boost::filesystem::exists(fullfilename) ) {
657 if( !_ValidateFilename(fullfilename,boost::filesystem::path()) ) {
658 RAVELOG_WARN(str(boost::format(
"acess denied to file %s\n")%fullfilename));
659 return std::string();
661 return fullfilename.string();
665 FOREACHC(itdir, _vBoostDataDirs) {
666 fullfilename = *itdir / filename;
667 if( _ValidateFilename(fullfilename,boost::filesystem::path()) ) {
668 return fullfilename.string();
672 RAVELOG_WARN(str(boost::format(
"could not find file %s\n")%filename));
673 return std::string();
679 #ifndef HAVE_BOOST_FILESYSTEM
680 RAVELOG_WARN(
"need to compile with boost::filesystem\n");
683 boost::filesystem::path fullfilename = boost::filesystem::absolute(filename);
684 _CustomNormalizePath(fullfilename);
685 FOREACHC(itpath,_vBoostDataDirs) {
686 std::list<boost::filesystem::path> listfilenameparts;
687 boost::filesystem::path testfilename = fullfilename.parent_path();
688 while( testfilename >= *itpath ) {
689 if( testfilename == *itpath ) {
690 boost::filesystem::path relpath;
691 FOREACH(itpart,listfilenameparts) {
694 relpath /= fullfilename.filename();
695 newfilename=relpath.string();
698 listfilenameparts.push_front(testfilename.filename());
699 testfilename = testfilename.parent_path();
707 boost::mutex::scoped_lock lock(_mutexinternal);
708 _nDataAccessOptions = options;
711 boost::mutex::scoped_lock lock(_mutexinternal);
712 return _nDataAccessOptions;
729 _vdatadirs.resize(0);
733 const char* delim =
";";
735 const char* delim =
":";
737 char* pOPENRAVE_DATA = getenv(
"OPENRAVE_DATA");
738 if( pOPENRAVE_DATA != NULL ) {
742 #ifdef HAVE_BOOST_FILESYSTEM
743 if( !boost::filesystem::is_directory(boost::filesystem::path(installdir)) ) {
746 if(RegOpenKeyEx(HKEY_LOCAL_MACHINE, TEXT(
"Software\\OpenRAVE\\" OPENRAVE_VERSION_STRING), 0, KEY_QUERY_VALUE, &hkey) == ERROR_SUCCESS) {
747 DWORD dwType = REG_SZ;
748 CHAR szInstallRoot[4096];
749 DWORD dwSize =
sizeof(szInstallRoot);
750 RegQueryValueEx(hkey, TEXT(
"InstallRoot"), NULL, &dwType, (PBYTE)szInstallRoot, &dwSize);
752 installdir.assign(szInstallRoot);
754 RAVELOG_VERBOSE(str(boost::format(
"window registry data dir '%s'")%installdir));
759 RAVELOG_WARN(str(boost::format(
"%s doesn't exist")%installdir));
763 boost::filesystem::path datafilename = boost::filesystem::absolute(boost::filesystem::path(installdir));
764 FOREACH(itname, _vdatadirs) {
765 if( datafilename == boost::filesystem::absolute(boost::filesystem::path(*itname)) ) {
771 string datafilename=installdir;
772 FOREACH(itname, _vdatadirs) {
773 if( datafilename == installdir ) {
780 _vdatadirs.push_back(installdir);
782 FOREACHC(itdir,_vdatadirs) {
786 #ifdef HAVE_BOOST_FILESYSTEM
787 _vBoostDataDirs.resize(0);
788 FOREACHC(itfilename,_vdatadirs) {
789 boost::filesystem::path fullfilename = boost::filesystem::absolute(boost::filesystem::path(*itfilename));
790 _CustomNormalizePath(fullfilename);
791 if( fullfilename.filename() ==
"." ) {
793 fullfilename = fullfilename.parent_path();
795 _vBoostDataDirs.push_back(fullfilename);
800 #ifdef HAVE_BOOST_FILESYSTEM
802 void _CustomNormalizePath(boost::filesystem::path& p)
804 #ifndef BOOST_FILESYSTEM_NO_DEPRECATED
807 boost::filesystem::path result;
808 for(boost::filesystem::path::iterator it=p.begin(); it!=p.end(); ++it)
812 if(boost::filesystem::is_symlink(result) ) {
817 else if(result.filename() ==
"..") {
822 result = result.parent_path();
825 else if(*it ==
".") {
837 bool _ValidateFilename(
const boost::filesystem::path& filename,
const boost::filesystem::path& curdir)
839 if( !boost::filesystem::exists(filename) ) {
843 if( _nDataAccessOptions & 1 ) {
845 boost::filesystem::path fullfilename = boost::filesystem::absolute(filename,curdir.empty() ? boost::filesystem::current_path() : curdir);
846 _CustomNormalizePath(fullfilename);
848 FOREACHC(itpath,_vBoostDataDirs) {
849 boost::filesystem::path testfilename = fullfilename.parent_path();
850 while( testfilename >= *itpath ) {
851 if( testfilename == *itpath ) {
855 testfilename = testfilename.parent_path();
871 static boost::shared_ptr<RaveGlobal> _state;
875 boost::shared_ptr<RaveDatabase> _pdatabase;
877 boost::mutex _mutexinternal;
878 std::map<InterfaceType, READERSMAP > _mapreaders;
879 std::map<InterfaceType,string> _mapinterfacenames;
880 std::map<IkParameterizationType,string> _mapikparameterization, _mapikparameterizationlower;
881 std::map<int, EnvironmentBase*> _mapenvironments;
882 std::list<boost::function<void()> > _listDestroyCallbacks;
883 std::string _homedirectory;
884 std::vector<std::string> _vdbdirectories;
885 int _nGlobalEnvironmentId;
888 long long _crlibm_fpu_state;
890 int _nDataAccessOptions;
892 std::vector<string> _vdatadirs;
893 #ifdef HAVE_BOOST_FILESYSTEM
894 std::vector<boost::filesystem::path> _vBoostDataDirs;
901 boost::shared_ptr<RaveGlobal> RaveGlobal::_state;
956 RaveGlobal::_state = boost::dynamic_pointer_cast<
RaveGlobal>(globalstate);
962 boost::shared_ptr<RaveGlobal> state = RaveGlobal::_state;
963 if( !!state && state->_IsInitialized() ) {
1106 return RaveGlobal::instance()->GetDatabase()->RegisterInterface(type, name, interfacehash,envhash,createfn);
1145 spec.
_vgroups[0].name = str(boost::format(
"ikparam_values %d")%iktype);
1146 spec.
_vgroups[0].interpolation = interpolation;
1152 int type = ikparam.
_type;
1167 O << v.x <<
" " << v.y <<
" " << v.z <<
" ";
1172 O << v.x <<
" " << v.y <<
" " << v.z <<
" ";
1177 O << r.
dir.x <<
" " << r.
dir.y <<
" " << r.
dir.z <<
" " << r.
pos.x <<
" " << r.
pos.y <<
" " << r.
pos.z <<
" ";
1182 O << v.x <<
" " << v.y <<
" " << v.z <<
" ";
1187 O << r.
dir.x <<
" " << r.
dir.y <<
" " << r.
dir.z <<
" " << r.
pos.x <<
" " << r.
pos.y <<
" " << r.
pos.z <<
" ";
1192 O << v.x <<
" " << v.y <<
" ";
1197 O << v.x <<
" " << v.y <<
" " << v.z <<
" ";
1202 O << p.first.x <<
" " << p.first.y <<
" " << p.first.z <<
" " << p.second.x <<
" " << p.second.y <<
" " << p.second.z <<
" ";
1207 O << p.second <<
" " << p.first.x <<
" " << p.first.y <<
" " << p.first.z <<
" ";
1212 O << p.second <<
" " << p.first.x <<
" " << p.first.y <<
" " << p.first.z <<
" ";
1217 O << p.second <<
" " << p.first.x <<
" " << p.first.y <<
" " << p.first.z <<
" ";
1222 O << p.second <<
" " << p.first.x <<
" " << p.first.y <<
" " << p.first.z <<
" ";
1227 O << p.second <<
" " << p.first.x <<
" " << p.first.y <<
" " << p.first.z <<
" ";
1232 O << p.second <<
" " << p.first.x <<
" " << p.first.y <<
" " << p.first.z <<
" ";
1241 O << it->first <<
" " << it->second.size() <<
" ";
1242 FOREACHC(itvalue, it->second) {
1243 O << *itvalue <<
" ";
1255 switch(ikparam.
_type) {
1270 I >> v.x >> v.y >> v.z;
1301 I >> angle >> trans.x >> trans.y >> trans.z;
1307 I >> angle >> trans.x >> trans.y >> trans.z;
1313 I >> angle >> trans.x >> trans.y >> trans.z;
1319 I >> angle >> trans.x >> trans.y >> trans.z;
1325 I >> angle >> trans.x >> trans.y >> trans.z;
1331 I >> angle >> trans.x >> trans.y >> trans.z;
1348 size_t numcustom = 0, numvalues=0;
1354 for(
size_t i = 0; i < numcustom; ++i) {
1355 I >> name >> numvalues;
1360 v.resize(numvalues);
1361 for(
size_t j = 0; j < v.size(); ++j) {
1371 int dof =
static_cast<int>(_dof);
1377 else if( affinedofs & DOF_X ) {
1383 else if( affinedofs & DOF_Y ) {
1389 else if( affinedofs & DOF_Z ) {
1406 BOOST_ASSERT(requestedindex >= 0);
1408 if( index == requestedindex && (affinedofs&
DOF_X) ) {
1411 else if( affinedofs & DOF_X ) {
1414 if( index == requestedindex && (affinedofs&
DOF_Y) ) {
1417 else if( affinedofs & DOF_Y ) {
1420 if( index == requestedindex && (affinedofs&
DOF_Z)) {
1423 else if( affinedofs & DOF_Z ) {
1426 if( index <= requestedindex && index+3 > requestedindex && (affinedofs&
DOF_RotationAxis) ) {
1429 if( index <= requestedindex && index+3 > requestedindex && (affinedofs&
DOF_Rotation3D) ) {
1432 if( index <= requestedindex && index+4 > requestedindex && (affinedofs&
DOF_RotationQuat) ) {
1447 BOOST_ASSERT( !(affinedofs & (DOF_Rotation3D|DOF_RotationAxis)) );
1450 if( affinedofs &
DOF_X ) {
1453 if( affinedofs &
DOF_Y ) {
1456 if( affinedofs &
DOF_Z ) {
1459 if( affinedofs & DOF_RotationAxis ) {
1462 else if( affinedofs & DOF_Rotation3D ) {
1465 else if( affinedofs & DOF_RotationQuat ) {
1473 if( affinedofs &
DOF_X ) {
1474 *itvalues++ = t.trans.x;
1476 if( affinedofs &
DOF_Y ) {
1477 *itvalues++ = t.trans.y;
1479 if( affinedofs &
DOF_Z ) {
1480 *itvalues++ = t.trans.z;
1486 dReal fsin =
RaveSqrt(t.rot.y * t.rot.y + t.rot.z * t.rot.z + t.rot.w * t.rot.w);
1487 dReal fangle = 2 * atan2(fsin, t.rot.x);
1489 dReal normalizer = fangle / fsin;
1490 *itvalues++ = normalizer * t.rot.y;
1491 *itvalues++ = normalizer * t.rot.z;
1492 *itvalues++ = normalizer * t.rot.w;
1501 *itvalues++ = t.rot.x;
1502 *itvalues++ = t.rot.y;
1503 *itvalues++ = t.rot.z;
1504 *itvalues++ = t.rot.w;
1510 if( affinedofs &
DOF_X ) {
1511 *itvalues++ = linearvel.x;
1513 if( affinedofs &
DOF_Y ) {
1514 *itvalues++ = linearvel.y;
1516 if( affinedofs &
DOF_Z ) {
1517 *itvalues++ = linearvel.z;
1520 *itvalues++ = axis.dot3(angularvel);
1523 *itvalues++ = angularvel.x;
1524 *itvalues++ = angularvel.y;
1525 *itvalues++ = angularvel.z;
1529 *itvalues++ = qdot.x;
1530 *itvalues++ = qdot.y;
1531 *itvalues++ = qdot.z;
1532 *itvalues++ = qdot.w;
1538 if( affinedofs &
DOF_X ) {
1539 t.trans.x = *itvalues++;
1541 if( affinedofs &
DOF_Y ) {
1542 t.trans.y = *itvalues++;
1544 if( affinedofs &
DOF_Z ) {
1545 t.trans.z = *itvalues++;
1548 dReal angle = *itvalues++;
1551 t.rot.y = vActvAffineRotationAxis.x * fsin;
1552 t.rot.z = vActvAffineRotationAxis.y * fsin;
1553 t.rot.w = vActvAffineRotationAxis.z * fsin;
1557 dReal x = *itvalues++;
1558 dReal y = *itvalues++;
1559 dReal z = *itvalues++;
1562 dReal fnormalizer = sin((
dReal)0.5 * fang) / fang;
1563 t.rot.x = cos((
dReal)0.5 * fang);
1564 t.rot.y = fnormalizer * x;
1565 t.rot.z = fnormalizer * y;
1566 t.rot.w = fnormalizer * z;
1577 t.rot[1] = 0.5 * *itvalues++;
1578 t.rot[2] = 0.5 * *itvalues++;
1579 t.rot[3] = 0.5 * *itvalues++;
1584 t.rot.x = *itvalues++;
1585 t.rot.y = *itvalues++;
1586 t.rot.z = *itvalues++;
1587 t.rot.w = *itvalues++;
1596 if( affinedofs &
DOF_X ) {
1597 linearvel.x = *itvalues++;
1599 if( affinedofs &
DOF_Y ) {
1600 linearvel.y = *itvalues++;
1602 if( affinedofs &
DOF_Z ) {
1603 linearvel.z = *itvalues++;
1606 dReal angle = *itvalues++;
1607 angularvel = axis*angle;
1610 angularvel.x = *itvalues++;
1611 angularvel.y = *itvalues++;
1612 angularvel.z = *itvalues++;
1622 angularvel.x = angularvel2.y * 2;
1623 angularvel.y = angularvel2.z * 2;
1624 angularvel.z = angularvel2.w * 2;
1643 s <<
plink1->GetParent()->GetName() <<
":" <<
plink1->GetName();
1647 s <<
plink2->GetParent()->GetName() <<
":" <<
plink2->GetName();
1649 s <<
") contacts="<<
contacts.size();
1675 indices.push_back(*it+offset);
1683 for(
size_t i = 0; i < mesh.
vertices.size(); ++i) {
1690 indices.push_back(*it+offset);
1704 if( vmin.x > v.x ) {
1707 if( vmin.y > v.y ) {
1710 if( vmin.z > v.z ) {
1713 if( vmax.x < v.x ) {
1716 if( vmax.y < v.y ) {
1719 if( vmax.z < v.z ) {
1733 o << it->x <<
" " << it->y <<
" " << it->z <<
" ";
1758 I >> it->x >> it->y >> it->z;
1773 DummyXMLReader::DummyXMLReader(
const std::string& fieldname,
const std::string& pparentname, boost::shared_ptr<std::ostream> osrecord) : _fieldname(fieldname), _osrecord(osrecord)
1775 _parentname = pparentname;
1777 _parentname += _fieldname;
1782 if( !!_pcurreader ) {
1783 if( _pcurreader->startElement(name, atts) ==
PE_Support ) {
1790 *_osrecord <<
"<" << name <<
" ";
1791 FOREACHC(itatt, atts) {
1792 *_osrecord << itatt->first <<
"=\"" << itatt->second <<
"\" ";
1794 *_osrecord <<
">" << endl;
1798 _pcurreader.reset(
new DummyXMLReader(name, _parentname,_osrecord));
1804 if( !!_pcurreader ) {
1805 if( _pcurreader->endElement(name) ) {
1806 _pcurreader.reset();
1808 *_osrecord <<
"</" << name <<
">" << endl;
1814 if( name == _fieldname ) {
1817 RAVELOG_ERROR(str(boost::format(
"invalid xml tag %s\n")%name));
1823 if( !_pcurreader ) {
1829 _pcurreader->characters(ch);
1836 RAVELOG_WARN(
"OpenRAVE global state not initialized! Need to call RaveInitialize before any OpenRAVE services can be used. For now, initializing with default parameters.\n");
1850 RAVELOG_WARN(
"SensorData XML serialization not implemented\n");
1856 RAVELOG_WARN(
"LaserSensorData XML serialization not implemented\n");
1862 RAVELOG_WARN(
"CameraSensorData XML serialization not implemented\n");
1868 _oldoptions = p->GetCollisionOptions();
1870 if( !_p->SetCollisionOptions(newoptions) ) {
1872 throw openrave_exception(str(boost::format(
"Failed to set collision options %d in checker %s\n")%newoptions%_p->GetXMLId()));
1879 _p->SetCollisionOptions(_oldoptions);
1889 std::vector<uint32_t> sample;
1891 return sample.at(0);
1896 std::vector<dReal> sample;
1898 return float(sample.at(0));
1903 std::vector<dReal> sample;
1905 return double(sample.at(0));
1908 namespace LocalXML {
1914 va_start(args, msg);
1931 if( atts != NULL ) {
1932 for (
int i = 0; (atts[i] != NULL); i+=2) {
1933 listatts.push_back(make_pair(
string((
const char*)atts[i]),
string((
const char*)atts[i+1])));
1934 std::transform(listatts.back().first.begin(), listatts.back().first.end(), listatts.back().first.begin(), ::tolower);
1939 string s = (
const char*)name;
1940 std::transform(s.begin(), s.end(), s.begin(), ::tolower);
1943 pdata->
_pdummy->startElement(s,listatts);
1957 string s = (
const char*)name;
1958 std::transform(s.begin(), s.end(), s.begin(), ::tolower);
1960 if( pdata->
_pdummy->endElement(s) ) {
1965 if( pdata->
_preader->endElement(s) ) {
1967 xmlStopParser(pdata->
_ctxt);
1976 pdata->
_pdummy->characters(
string((
const char*)ch, len));
1979 pdata->
_preader->characters(
string((
const char*)ch, len));
1988 #ifdef LIBXML_SAX1_ENABLED
1989 if (ctxt->sax && ctxt->sax->initialized == XML_SAX2_MAGIC && (ctxt->sax->startElementNs != NULL || ctxt->sax->endElementNs != NULL)) {
1996 ctxt->str_xml = xmlDictLookup(ctxt->dict, BAD_CAST
"xml", 3);
1997 ctxt->str_xmlns = xmlDictLookup(ctxt->dict, BAD_CAST
"xmlns", 5);
1998 ctxt->str_xml_ns = xmlDictLookup(ctxt->dict, XML_XML_NAMESPACE, 36);
1999 if ( ctxt->str_xml==NULL || ctxt->str_xmlns==NULL || ctxt->str_xml_ns == NULL) {
2007 static xmlSAXHandler s_DefaultSAXHandler = { 0};
2009 size = strlen(buffer);
2011 if( !s_DefaultSAXHandler.initialized ) {
2017 s_DefaultSAXHandler.initialized = 1;
2020 xmlSAXHandlerPtr sax = &s_DefaultSAXHandler;
2022 xmlParserCtxtPtr ctxt;
2024 ctxt = xmlCreateMemoryParserCtxt(buffer, size);
2028 if (ctxt->sax != (xmlSAXHandlerPtr) &xmlDefaultSAXHandler) {
2035 ctxt->userData = &reader;
2037 xmlParseDocument(ctxt);
2039 if (ctxt->wellFormed) {
2043 if (ctxt->errNo != 0) {
2053 if (ctxt->myDoc != NULL) {
2054 xmlFreeDoc(ctxt->myDoc);
2057 xmlFreeParserCtxt(ctxt);