23 #ifndef OPENRAVE_DISABLE_ASSERT_HANDLER
24 #define BOOST_ENABLE_ASSERT_HANDLER
36 #pragma warning(disable:4251) // needs to have dll-interface to be used by clients of class
37 #pragma warning(disable:4190) // C-linkage specified, but returns UDT 'boost::shared_ptr<T>' which is incompatible with C
38 #pragma warning(disable:4819) //The file contains a character that cannot be represented in the current code page (932). Save the file in Unicode format to prevent data loss using native typeof
48 #ifndef __PRETTY_FUNCTION__
49 #define __PRETTY_FUNCTION__ __FUNCDNAME__
67 #include <boost/version.hpp>
68 #include <boost/function.hpp>
69 #include <boost/shared_ptr.hpp>
70 #include <boost/weak_ptr.hpp>
71 #include <boost/tuple/tuple.hpp>
72 #include <boost/enable_shared_from_this.hpp>
73 #include <boost/thread/mutex.hpp>
74 #include <boost/thread/shared_mutex.hpp>
75 #include <boost/thread/thread.hpp>
76 #include <boost/thread/recursive_mutex.hpp>
77 #include <boost/static_assert.hpp>
78 #include <boost/format.hpp>
79 #include <boost/array.hpp>
80 #include <boost/multi_array.hpp>
84 #define RAVE_DEPRECATED __attribute__((deprecated))
86 #define RAVE_DEPRECATED
95 #if OPENRAVE_PRECISION // 1 if double precision
97 #define g_fEpsilon 1e-15
100 #define g_fEpsilon 2e-7f
190 char const*
what()
const throw() {
207 bool operator() (
const std::string & s1,
const std::string& s2)
const
209 std::string::const_iterator it1=s1.begin();
210 std::string::const_iterator it2=s2.begin();
213 while ( (it1!=s1.end()) && (it2!=s2.end()) ) {
214 if(::toupper(*it1) != ::toupper(*it2)) {
216 return ::toupper(*it1) < ::toupper(*it2);
222 std::size_t size1=s1.size(), size2=s2.size();
249 virtual void Serialize(std::ostream& O,
int options=0)
const = 0;
252 virtual void Deserialize(std::istream& I) = 0;
279 sprintf (command,
"%c[%d;%d;%dm", 0x1B, attribute, fg + 30, bg + 40);
287 sprintf (command,
"%c[%d;%dm", 0x1B, attribute, fg + 30);
295 sprintf (command,
"%c[0;38;48m", 0x1B);
302 swprintf (command, 13, L
"%c[%d;%dm", 0x1B, attribute, fg + 30);
308 std::vector<int> positions;
309 std::wstring str = fmt;
310 wchar_t* p = wcsstr(&str[0], L
"%s");
312 positions.push_back((
int)(p-&str[0])+1);
313 p = wcsstr(p+2, L
"%s");
316 p = wcsstr(&str[0], L
"%S");
319 p = wcsstr(p+2, L
"%S");
322 p = wcsstr(&str[0], L
"%ls");
326 p = wcsstr(p+2, L
"%ls");
329 for(
int i = 0; i < (int)positions.size(); ++i)
330 str[positions[i]] =
'S';
345 #define OPENRAVECOLOR_FATALLEVEL 5 // magenta
346 #define OPENRAVECOLOR_ERRORLEVEL 1 // red
347 #define OPENRAVECOLOR_WARNLEVEL 3 // yellow
348 #define OPENRAVECOLOR_INFOLEVEL 0 // black
349 #define OPENRAVECOLOR_DEBUGLEVEL 2 // green
350 #define OPENRAVECOLOR_VERBOSELEVEL 4 // blue
361 if( pfilename == NULL ) {
364 const char* p0 = strrchr(pfilename,
'/');
365 const char* p1 = strrchr(pfilename,
'\\');
366 const char* p = p0 > p1 ? p0 : p1;
375 #define DefineRavePrintfW(LEVEL) \
376 inline int RavePrintfW ## LEVEL(const wchar_t *fmt, ...) \
380 va_start(list,fmt); \
381 int r = vwprintf(OpenRAVE::RavePrintTransformString(fmt).c_str(), list); \
387 #define DefineRavePrintfA(LEVEL) \
388 inline int RavePrintfA ## LEVEL(const std::string& s) \
390 if((s.size() == 0)||(s[s.size()-1] != '\n')) { \
391 printf("%s\n", s.c_str()); \
394 printf ("%s", s.c_str()); \
399 inline int RavePrintfA ## LEVEL(const char *fmt, ...) \
403 va_start(list,fmt); \
404 int r = vprintf(fmt, list); \
411 inline int RavePrintfA(
const std::string& s, uint32_t level)
413 if((s.size() == 0)||(s[s.size()-1] !=
'\n')) {
414 printf(
"%s\n", s.c_str());
417 printf (
"%s", s.c_str());
427 #define DefineRavePrintfW(LEVEL) \
428 inline int RavePrintfW ## LEVEL(const wchar_t *wfmt, ...) \
431 va_start(list,wfmt); \
433 size_t allocsize = wcstombs(NULL, wfmt, 0)+32; \
434 char* fmt = (char*)alloca(allocsize); \
435 strcpy(fmt, ChangeTextColor(0, OPENRAVECOLOR ## LEVEL,8).c_str()); \
436 snprintf(fmt+strlen(fmt),allocsize-16,"%S",wfmt); \
437 strcat(fmt, ResetTextColor().c_str()); \
438 int r = vprintf(fmt, list); \
449 if((s.size() == 0)||(s[s.size()-1] !=
'\n')) {
450 printf(
"%s\n", s.c_str());
453 printf (
"%s", s.c_str());
462 int r = vprintf(fmt, list);
468 #define DefineRavePrintfA(LEVEL) \
469 inline int RavePrintfA ## LEVEL(const std::string& s) \
471 if((s.size() == 0)||(s[s.size()-1] != '\n')) { \
472 printf ("%c[0;%d;%dm%s%c[m\n", 0x1B, OPENRAVECOLOR ## LEVEL + 30,8+40,s.c_str(),0x1B); \
475 printf ("%c[0;%d;%dm%s%c[m", 0x1B, OPENRAVECOLOR ## LEVEL + 30,8+40,s.c_str(),0x1B); \
480 inline int RavePrintfA ## LEVEL(const char *fmt, ...) \
483 va_start(list,fmt); \
484 int r = vprintf((ChangeTextColor(0, OPENRAVECOLOR ## LEVEL,8) + std::string(fmt) + ResetTextColor()).c_str(), list); \
500 if((s.size() == 0)||(s[s.size()-1] !=
'\n')) {
501 printf (
"%s\n",s.c_str());
504 printf (
"%s",s.c_str());
510 if((s.size() == 0)||(s[s.size()-1] !=
'\n')) {
511 printf (
"%c[0;%d;%dm%s%c[0;38;48m\n", 0x1B, color + 30,8+40,s.c_str(),0x1B);
514 printf (
"%c[0;%d;%dm%s%c[0;38;48m", 0x1B, color + 30,8+40,s.c_str(),0x1B);
537 #define RAVEPRINTHEADER(LEVEL) OpenRAVE::RavePrintfA ## LEVEL("[%s:%d] ", OpenRAVE::RaveGetSourceFilename(__FILE__), __LINE__)
541 #define RAVELOG_LEVELW(LEVEL,level) int(OpenRAVE::RaveGetDebugLevel()&OpenRAVE::Level_OutputMask)>=int(level)&&(RAVEPRINTHEADER(LEVEL)>0)&&OpenRAVE::RavePrintfW ## LEVEL
542 #define RAVELOG_LEVELA(LEVEL,level) int(OpenRAVE::RaveGetDebugLevel()&OpenRAVE::Level_OutputMask)>=int(level)&&(RAVEPRINTHEADER(LEVEL)>0)&&OpenRAVE::RavePrintfA ## LEVEL
545 #define RAVELOG_FATALW RAVELOG_LEVELW(_FATALLEVEL,OpenRAVE::Level_Fatal)
546 #define RAVELOG_FATALA RAVELOG_LEVELA(_FATALLEVEL,OpenRAVE::Level_Fatal)
547 #define RAVELOG_FATAL RAVELOG_FATALA
548 #define RAVELOG_ERRORW RAVELOG_LEVELW(_ERRORLEVEL,OpenRAVE::Level_Error)
549 #define RAVELOG_ERRORA RAVELOG_LEVELA(_ERRORLEVEL,OpenRAVE::Level_Error)
550 #define RAVELOG_ERROR RAVELOG_ERRORA
551 #define RAVELOG_WARNW RAVELOG_LEVELW(_WARNLEVEL,OpenRAVE::Level_Warn)
552 #define RAVELOG_WARNA RAVELOG_LEVELA(_WARNLEVEL,OpenRAVE::Level_Warn)
553 #define RAVELOG_WARN RAVELOG_WARNA
554 #define RAVELOG_INFOW RAVELOG_LEVELW(_INFOLEVEL,OpenRAVE::Level_Info)
555 #define RAVELOG_INFOA RAVELOG_LEVELA(_INFOLEVEL,OpenRAVE::Level_Info)
556 #define RAVELOG_INFO RAVELOG_INFOA
557 #define RAVELOG_DEBUGW RAVELOG_LEVELW(_DEBUGLEVEL,OpenRAVE::Level_Debug)
558 #define RAVELOG_DEBUGA RAVELOG_LEVELA(_DEBUGLEVEL,OpenRAVE::Level_Debug)
559 #define RAVELOG_DEBUG RAVELOG_DEBUGA
560 #define RAVELOG_VERBOSEW RAVELOG_LEVELW(_VERBOSELEVEL,OpenRAVE::Level_Verbose)
561 #define RAVELOG_VERBOSEA RAVELOG_LEVELA(_VERBOSELEVEL,OpenRAVE::Level_Verbose)
562 #define RAVELOG_VERBOSE RAVELOG_VERBOSEA
564 #define IS_DEBUGLEVEL(level) ((OpenRAVE::RaveGetDebugLevel()&OpenRAVE::Level_OutputMask)>=(level))
566 #define OPENRAVE_EXCEPTION_FORMAT0(s, errorcode) OpenRAVE::openrave_exception(boost::str(boost::format("[%s:%d] " s)%(__PRETTY_FUNCTION__)%(__LINE__)),errorcode)
569 #define OPENRAVE_EXCEPTION_FORMAT(s, args,errorcode) OpenRAVE::openrave_exception(boost::str(boost::format("[%s:%d] " s)%(__PRETTY_FUNCTION__)%(__LINE__)%args),errorcode)
571 #define OPENRAVE_ASSERT_FORMAT(testexpr, s, args, errorcode) { if( !(testexpr) ) { throw OpenRAVE::openrave_exception(boost::str(boost::format("[%s:%d] (%s) failed " s)%(__PRETTY_FUNCTION__)%(__LINE__)%(# testexpr)%args),errorcode); } }
573 #define OPENRAVE_ASSERT_FORMAT0(testexpr, s, errorcode) { if( !(testexpr) ) { throw OpenRAVE::openrave_exception(boost::str(boost::format("[%s:%d] (%s) failed " s)%(__PRETTY_FUNCTION__)%(__LINE__)%(# testexpr)),errorcode); } }
576 #define OPENRAVE_ASSERT_OP_FORMAT(expr1,op,expr2,s, args, errorcode) { if( !((expr1) op (expr2)) ) { throw OpenRAVE::openrave_exception(boost::str(boost::format("[%s:%d] %s %s %s, (eval %s %s %s) " s)%(__PRETTY_FUNCTION__)%(__LINE__)%(# expr1)%(# op)%(# expr2)%(expr1)%(# op)%(expr2)%args),errorcode); } }
578 #define OPENRAVE_ASSERT_OP_FORMAT0(expr1,op,expr2,s, errorcode) { if( !((expr1) op (expr2)) ) { throw OpenRAVE::openrave_exception(boost::str(boost::format("[%s:%d] %s %s %s, (eval %s %s %s) " s)%(__PRETTY_FUNCTION__)%(__LINE__)%(# expr1)%(# op)%(# expr2)%(expr1)%(# op)%(expr2)),errorcode); } }
580 #define OPENRAVE_ASSERT_OP(expr1,op,expr2) { if( !((expr1) op (expr2)) ) { throw OpenRAVE::openrave_exception(boost::str(boost::format("[%s:%d] %s %s %s, (eval %s %s %s) ")%(__PRETTY_FUNCTION__)%(__LINE__)%(# expr1)%(# op)%(# expr2)%(expr1)%(# op)%(expr2)),ORE_Assert); } }
582 #define OPENRAVE_DUMMY_IMPLEMENTATION { throw OPENRAVE_EXCEPTION_FORMAT0("not implemented",ORE_NotImplemented); }
592 PT_ProblemInstance=5,
594 PT_InverseKinematicsSolver=6,
598 PT_CollisionChecker=10,
602 PT_NumberOfInterfaces=13
605 class CollisionReport;
608 class TrajectoryBase;
609 class ControllerBase;
613 class EnvironmentBase;
615 class SensorSystemBase;
616 class PhysicsEngineBase;
618 class CollisionCheckerBase;
620 class SpaceSamplerBase;
621 class IkParameterization;
622 class ConfigurationSpecification;
743 virtual ProcessElement startElement(
const std::string& name,
const AttributesList& atts) = 0;
748 virtual bool endElement(
const std::string& name) = 0;
752 virtual void characters(
const std::string& ch) = 0;
758 typedef boost::function<BaseXMLReaderPtr(InterfaceBasePtr, const AttributesList&)>
CreateXMLReaderFn;
764 DummyXMLReader(
const std::string& fieldname,
const std::string& parentname, boost::shared_ptr<std::ostream> osrecord = boost::shared_ptr<std::ostream>());
766 virtual bool endElement(
const std::string& name);
767 virtual void characters(
const std::string& ch);
771 virtual boost::shared_ptr<std::ostream>
GetStream()
const {
775 std::string _parentname;
776 std::string _fieldname;
777 boost::shared_ptr<std::ostream> _osrecord;
778 boost::shared_ptr<BaseXMLReader> _pcurreader;
792 virtual const std::string& GetFormat()
const = 0;
797 virtual void SetCharData(
const std::string& data) = 0;
806 #if OPENRAVE_PRECISION // 1 if double precision
807 #define OPENRAVE_MATH_EXP_DOUBLE RaveExp
808 #define OPENRAVE_MATH_LOG_DOUBLE RaveLog
809 #define OPENRAVE_MATH_COS_DOUBLE RaveCos
810 #define OPENRAVE_MATH_SIN_DOUBLE RaveSin
811 #define OPENRAVE_MATH_TAN_DOUBLE RaveTan
812 #define OPENRAVE_MATH_LOG2_DOUBLE RaveLog2
813 #define OPENRAVE_MATH_LOG10_DOUBLE RaveLog10
814 #define OPENRAVE_MATH_ACOS_DOUBLE RaveAcos
815 #define OPENRAVE_MATH_ASIN_DOUBLE RaveAsin
816 #define OPENRAVE_MATH_ATAN2_DOUBLE RaveAtan2
817 #define OPENRAVE_MATH_POW_DOUBLE RavePow
818 #define OPENRAVE_MATH_SQRT_DOUBLE RaveSqrt
819 #define OPENRAVE_MATH_FABS_DOUBLE RaveFabs
821 #define OPENRAVE_MATH_EXP_FLOAT RaveExp
822 #define OPENRAVE_MATH_LOG_FLOAT RaveLog
823 #define OPENRAVE_MATH_COS_FLOAT RaveCos
824 #define OPENRAVE_MATH_SIN_FLOAT RaveSin
825 #define OPENRAVE_MATH_TAN_FLOAT RaveTan
826 #define OPENRAVE_MATH_LOG2_FLOAT RaveLog2
827 #define OPENRAVE_MATH_LOG10_FLOAT RaveLog10
828 #define OPENRAVE_MATH_ACOS_FLOAT RaveAcos
829 #define OPENRAVE_MATH_ASIN_FLOAT RaveAsin
830 #define OPENRAVE_MATH_ATAN2_FLOAT RaveAtan2
831 #define OPENRAVE_MATH_POW_FLOAT RavePow
832 #define OPENRAVE_MATH_SQRT_FLOAT RaveSqrt
833 #define OPENRAVE_MATH_FABS_FLOAT RaveFabs
840 using geometry::RaveVector;
841 using geometry::RaveTransform;
842 using geometry::RaveTransformMatrix;
997 virtual bool endElement(
const std::string& name);
998 virtual void characters(
const std::string& ch);
1013 virtual int GetDOF()
const;
1019 virtual bool IsValid()
const;
1025 virtual void Validate()
const;
1034 virtual const Group& GetGroupFromName(
const std::string& name)
const;
1040 virtual Group& GetGroupFromName(
const std::string& name);
1047 virtual std::vector<Group>::const_iterator FindCompatibleGroup(
const Group& g,
bool exactmatch=
false)
const;
1054 virtual std::vector<Group>::const_iterator FindCompatibleGroup(
const std::string& name,
bool exactmatch=
false)
const;
1063 virtual std::vector<Group>::const_iterator FindTimeDerivativeGroup(
const Group& g,
bool exactmatch=
false)
const;
1072 virtual std::vector<Group>::const_iterator FindTimeDerivativeGroup(
const std::string& name,
bool exactmatch=
false)
const;
1082 virtual void AddDerivativeGroups(
int deriv,
bool adddeltatime=
false);
1086 AddDerivativeGroups(1,adddeltatime);
1104 virtual void ResetGroupOffsets();
1107 virtual int AddDeltaTimeGroup();
1115 virtual int AddGroup(
const std::string& name,
int dof,
const std::string& interpolation =
"");
1139 virtual bool ExtractTransform(
Transform& t, std::vector<dReal>::const_iterator itdata,
KinBodyConstPtr pbody,
int timederivative=0)
const;
1149 virtual bool ExtractIkParameterization(
IkParameterization& ikparam, std::vector<dReal>::const_iterator itdata,
int timederivative=0)
const;
1160 virtual bool ExtractAffineValues(std::vector<dReal>::iterator itvalues, std::vector<dReal>::const_iterator itdata,
KinBodyConstPtr pbody,
int affinedofs,
int timederivative=0)
const;
1171 virtual bool ExtractJointValues(std::vector<dReal>::iterator itvalues, std::vector<dReal>::const_iterator itdata,
KinBodyConstPtr pbody,
const std::vector<int>& indices,
int timederivative=0)
const;
1176 virtual bool ExtractDeltaTime(
dReal& deltatime, std::vector<dReal>::const_iterator itdata)
const;
1187 virtual bool InsertJointValues(std::vector<dReal>::iterator itdata, std::vector<dReal>::const_iterator itvalues,
KinBodyConstPtr pbody,
const std::vector<int>& indices,
int timederivative=0)
const;
1195 virtual bool InsertDeltaTime(std::vector<dReal>::iterator itdata,
dReal deltatime)
const;
1204 virtual int AddGroup(
const Group& g);
1210 virtual boost::shared_ptr<SetConfigurationStateFn> GetSetFn(
EnvironmentBasePtr penv)
const;
1213 virtual boost::shared_ptr<GetConfigurationStateFn> GetGetFn(
EnvironmentBasePtr penv)
const;
1228 static void ConvertGroupData(std::vector<dReal>::iterator ittargetdata,
size_t targetstride,
const Group& gtarget, std::vector<dReal>::const_iterator itsourcedata,
size_t sourcestride,
const Group& gsource,
size_t numpoints,
EnvironmentBaseConstPtr penv,
bool filluninitialized =
true);
1247 static std::string GetInterpolationDerivative(
const std::string& interpolation,
int deriv=1);
1258 template <
typename T>
1264 while (theta < min) {
1268 else if (theta > max) {
1271 while (theta > max) {
1321 throw openrave_exception(str(boost::format(
"IkParameterization constructor does not support type 0x%x")%_type));
1330 inline const std::string& GetName()
const;
1334 return (type>>28)&0xf;
1338 return (_type>>28)&0xf;
1343 return (type>>24)&0xf;
1347 return (_type>>24)&0xf;
1363 _type =
IKP_Ray4D; _transform.trans = ray.
pos; _transform.rot = ray.
dir;
1376 _type =
IKP_TranslationXY2D; _transform.trans.x = trans.x; _transform.trans.y = trans.y; _transform.trans.z = 0; _transform.trans.w = 0;
1379 _type =
IKP_TranslationXYOrientation3D; _transform.trans.x = trans.x; _transform.trans.y = trans.y; _transform.trans.z = trans.z; _transform.trans.w = 0;
1382 _type =
IKP_TranslationLocalGlobal6D; _transform.rot.x = localtrans.x; _transform.rot.y = localtrans.y; _transform.rot.z = localtrans.z; _transform.rot.w = 0; _transform.trans.x = trans.x; _transform.trans.y = trans.y; _transform.trans.z = trans.z; _transform.trans.w = 0;
1386 _transform.trans = trans;
1387 _transform.rot.x = angle;
1391 _transform.trans = trans;
1392 _transform.rot.x = angle;
1396 _transform.trans = trans;
1397 _transform.rot.x = angle;
1402 _transform.trans = trans;
1403 _transform.rot.x = angle;
1407 _transform.trans = trans;
1408 _transform.rot.x = angle;
1412 _transform.trans = trans;
1413 _transform.rot.x = angle;
1420 return _transform.rot;
1423 return _transform.trans;
1426 return _transform.rot;
1429 return RAY(_transform.trans,_transform.rot);
1432 return _transform.trans;
1435 return _transform.rot;
1438 return RAY(_transform.trans,_transform.rot);
1441 return _transform.trans;
1444 return _transform.trans;
1447 return std::make_pair(_transform.rot,_transform.trans);
1450 return std::make_pair(_transform.trans,_transform.rot.x);
1453 return std::make_pair(_transform.trans,_transform.rot.x);
1456 return std::make_pair(_transform.trans,_transform.rot.x);
1459 return std::make_pair(_transform.trans,_transform.rot.x);
1462 return std::make_pair(_transform.trans,_transform.rot.x);
1465 return std::make_pair(_transform.trans,_transform.rot.x);
1471 const dReal anglemult = 0.4;
1472 BOOST_ASSERT(_type==ikparam.
GetType());
1478 return (t0.trans-t1.trans).lengthsqr3() + anglemult*facos*facos;
1493 Vector pos0 = GetRay4D().pos - GetRay4D().dir*GetRay4D().dir.dot(GetRay4D().pos);
1497 return (pos0-pos1).lengthsqr3() + anglemult*facos*facos;
1505 return v.lengthsqr3();
1516 Vector v0 = GetTranslationXYOrientation3D();
1518 dReal anglediff = v0.z-v1.z;
1521 while (anglediff <
dReal(-
PI))
1524 else if (anglediff >
dReal(
PI)) {
1529 return (v0-v1).lengthsqr2() + anglemult*anglediff*anglediff;
1533 return (p0.first-p1.first).lengthsqr3() + (p0.second-p1.second).
lengthsqr3();
1540 return (p0.first-p1.first).lengthsqr3() + (angle0-angle1)*(angle0-angle1);
1547 return (p0.first-p1.first).lengthsqr3() + (angle0-angle1)*(angle0-angle1);
1554 return (p0.first-p1.first).lengthsqr3() + (angle0-angle1)*(angle0-angle1);
1559 return (p0.first-p1.first).lengthsqr3() + anglediff*anglediff;
1564 return (p0.first-p1.first).lengthsqr3() + anglediff*anglediff;
1569 return (p0.first-p1.first).lengthsqr3() + anglediff*anglediff;
1582 inline void GetValues(std::vector<dReal>::iterator itvalues)
const
1586 *itvalues++ = _transform.rot.x;
1587 *itvalues++ = _transform.rot.y;
1588 *itvalues++ = _transform.rot.z;
1589 *itvalues++ = _transform.rot.w;
1590 *itvalues++ = _transform.trans.x;
1591 *itvalues++ = _transform.trans.y;
1592 *itvalues++ = _transform.trans.z;
1595 *itvalues++ = _transform.rot.x;
1596 *itvalues++ = _transform.rot.y;
1597 *itvalues++ = _transform.rot.z;
1598 *itvalues++ = _transform.rot.w;
1601 *itvalues++ = _transform.trans.x;
1602 *itvalues++ = _transform.trans.y;
1603 *itvalues++ = _transform.trans.z;
1606 *itvalues++ = _transform.rot.x;
1607 *itvalues++ = _transform.rot.y;
1608 *itvalues++ = _transform.rot.z;
1611 *itvalues++ = _transform.rot.x;
1612 *itvalues++ = _transform.rot.y;
1613 *itvalues++ = _transform.rot.z;
1614 *itvalues++ = _transform.trans.x;
1615 *itvalues++ = _transform.trans.y;
1616 *itvalues++ = _transform.trans.z;
1619 *itvalues++ = _transform.trans.x;
1620 *itvalues++ = _transform.trans.y;
1621 *itvalues++ = _transform.trans.z;
1624 *itvalues++ = _transform.rot.x;
1625 *itvalues++ = _transform.rot.y;
1626 *itvalues++ = _transform.rot.z;
1627 *itvalues++ = _transform.trans.x;
1628 *itvalues++ = _transform.trans.y;
1629 *itvalues++ = _transform.trans.z;
1632 *itvalues++ = _transform.trans.x;
1633 *itvalues++ = _transform.trans.y;
1636 *itvalues++ = _transform.trans.x;
1637 *itvalues++ = _transform.trans.y;
1638 *itvalues++ = _transform.trans.z;
1641 *itvalues++ = _transform.rot.x;
1642 *itvalues++ = _transform.rot.y;
1643 *itvalues++ = _transform.rot.z;
1644 *itvalues++ = _transform.trans.x;
1645 *itvalues++ = _transform.trans.y;
1646 *itvalues++ = _transform.trans.z;
1654 *itvalues++ = _transform.rot.x;
1655 *itvalues++ = _transform.trans.x;
1656 *itvalues++ = _transform.trans.y;
1657 *itvalues++ = _transform.trans.z;
1672 _transform.rot.x = *itvalues++;
1673 _transform.rot.y = *itvalues++;
1674 _transform.rot.z = *itvalues++;
1675 _transform.rot.w = *itvalues++;
1676 _transform.trans.x = *itvalues++;
1677 _transform.trans.y = *itvalues++;
1678 _transform.trans.z = *itvalues++;
1681 _transform.rot.x = *itvalues++;
1682 _transform.rot.y = *itvalues++;
1683 _transform.rot.z = *itvalues++;
1684 _transform.rot.w = *itvalues++;
1687 _transform.trans.x = *itvalues++;
1688 _transform.trans.y = *itvalues++;
1689 _transform.trans.z = *itvalues++;
1692 _transform.rot.x = *itvalues++;
1693 _transform.rot.y = *itvalues++;
1694 _transform.rot.z = *itvalues++;
1697 _transform.rot.x = *itvalues++;
1698 _transform.rot.y = *itvalues++;
1699 _transform.rot.z = *itvalues++;
1700 _transform.trans.x = *itvalues++;
1701 _transform.trans.y = *itvalues++;
1702 _transform.trans.z = *itvalues++;
1705 _transform.trans.x = *itvalues++;
1706 _transform.trans.y = *itvalues++;
1707 _transform.trans.z = *itvalues++;
1710 _transform.rot.x = *itvalues++;
1711 _transform.rot.y = *itvalues++;
1712 _transform.rot.z = *itvalues++;
1713 _transform.trans.x = *itvalues++;
1714 _transform.trans.y = *itvalues++;
1715 _transform.trans.z = *itvalues++;
1718 _transform.trans.x = *itvalues++;
1719 _transform.trans.y = *itvalues++;
1722 _transform.trans.x = *itvalues++;
1723 _transform.trans.y = *itvalues++;
1724 _transform.trans.z = *itvalues++;
1727 _transform.rot.x = *itvalues++;
1728 _transform.rot.y = *itvalues++;
1729 _transform.rot.z = *itvalues++;
1730 _transform.trans.x = *itvalues++;
1731 _transform.trans.y = *itvalues++;
1732 _transform.trans.z = *itvalues++;
1740 _transform.rot.x = *itvalues++;
1741 _transform.trans.x = *itvalues++;
1742 _transform.trans.y = *itvalues++;
1743 _transform.trans.z = *itvalues++;
1751 SetValues(itvalues,iktype);
1769 inline void SetCustomValues(
const std::string& name,
const std::vector<dReal>& values)
1773 _mapCustomData[name] = values;
1777 inline void SetCustomValue(
const std::string& name,
dReal value)
1781 _mapCustomData[name].resize(1);
1782 _mapCustomData[name][0] = value;
1786 inline bool GetCustomValues(
const std::string& name, std::vector<dReal>& values)
const
1788 std::map<std::string, std::vector<dReal> >::const_iterator it = _mapCustomData.find(name);
1789 if( it == _mapCustomData.end() ) {
1792 values = it->second;
1797 inline const std::map<std::string, std::vector<dReal> >& GetCustomDataMap()
const
1799 return _mapCustomData;
1806 inline size_t ClearCustomValues(
const std::string& name=std::string())
1808 if( name.size() > 0 ) {
1809 return _mapCustomData.erase(name) > 0;
1812 size_t num = _mapCustomData.size();
1813 _mapCustomData.clear();
1821 return GetConfigurationSpecification(GetType(),interpolation);
1828 _transform = t * _transform;
1831 _transform.trans = t.rotate(_transform.trans);
1839 _transform.trans = t * _transform.trans;
1842 _transform.trans = t.rotate(_transform.trans);
1846 _transform.rot = t.rotate(_transform.rot);
1849 _transform.trans = t * _transform.trans;
1850 _transform.rot = t.rotate(_transform.rot);
1853 _transform.trans = t.rotate(_transform.trans);
1854 _transform.rot = t.rotate(_transform.rot);
1857 SetLookat3D(
RAY(t*GetLookat3D(),t.rotate(GetLookat3DDirection())));
1860 _transform.trans = t * _transform.trans;
1861 _transform.rot = t.rotate(_transform.rot);
1864 _transform.trans = t.rotate(_transform.trans);
1865 _transform.rot = t.rotate(_transform.rot);
1868 SetTranslationXY2D(t*GetTranslationXY2D());
1871 _transform.trans = t.rotate(_transform.trans);
1874 Vector v = GetTranslationXYOrientation3D();
1875 Vector voldtrans(v.x,v.y,0);
1876 Vector vnewtrans = t*voldtrans;
1878 SetTranslationXYOrientation3D(
Vector(vnewtrans.y,vnewtrans.y,v.z+zangle));
1882 Vector v = GetTranslationXYOrientation3D();
1883 Vector voldtrans(v.x,v.y,0);
1884 _transform.trans = t.rotate(voldtrans);
1889 _transform.trans = t*_transform.trans;
1892 _transform.trans = t.rotate(_transform.trans);
1895 _transform.trans = t*_transform.trans;
1900 _transform.trans = t.rotate(_transform.trans);
1905 _transform.trans = t*_transform.trans;
1910 _transform.trans = t.rotate(_transform.trans);
1915 _transform.trans = t*_transform.trans;
1920 _transform.trans = t.rotate(_transform.trans);
1926 _transform.trans = t*_transform.trans;
1932 _transform.trans = t.rotate(_transform.trans);
1938 _transform.trans = t*_transform.trans;
1944 _transform.trans = t.rotate(_transform.trans);
1950 _transform.trans = t*_transform.trans;
1956 _transform.trans = t.rotate(_transform.trans);
1962 throw openrave_exception(str(boost::format(
"parameterization 0x%x does not support left-transform")%GetType()));
1964 for(std::map<std::string, std::vector<dReal> >::iterator it = _mapCustomData.begin(); it != _mapCustomData.end(); ++it) {
1965 _MultiplyTransform(t, it->first, it->second);
1995 _transform.trans = _transform.trans + t.trans;
2013 SetLookat3D(GetLookat3D() + t.trans);
2018 _transform.trans +=
quatRotate(qorig,t.trans);
2027 SetTranslationXY2D(GetTranslationXY2D() + t.trans);
2033 Vector v = GetTranslationXYOrientation3D();
2034 Vector voldtrans(v.x,v.y,0);
2038 SetTranslationXYOrientation3D(
Vector(vnewtrans.y,vnewtrans.y,v.z+zangle));
2049 _transform.trans = _transform.trans + t.trans;
2124 throw openrave_exception(str(boost::format(
"parameterization 0x%x does not support right-transforms")%GetType()));
2126 for(std::map<std::string, std::vector<dReal> >::iterator it = _mapCustomData.begin(); it != _mapCustomData.end(); ++it) {
2127 _MultiplyTransformRight(t, it->first, it->second);
2140 return c < 0 || c >= 33;
2142 inline static void _MultiplyTransform(
const Transform& t,
const std::string& name, std::vector<dReal>& values)
2144 size_t startoffset = name.find(
"_transform=");
2145 if( startoffset != std::string::npos ) {
2146 size_t endoffset = name.find(
"_", startoffset+11);
2147 std::string transformtype;
2148 if( endoffset == std::string::npos ) {
2149 transformtype = name.substr(startoffset+11);
2152 transformtype = name.substr(startoffset+11,endoffset-startoffset-11);
2154 if( transformtype ==
"direction" ) {
2155 Vector v(values.at(0),values.at(1), values.at(2));
2157 values.at(0) = v[0]; values.at(1) = v[1]; values.at(2) = v[2];
2159 else if( transformtype ==
"point" ) {
2160 Vector v(values.at(0),values.at(1), values.at(2));
2162 values.at(0) = v[0]; values.at(1) = v[1]; values.at(2) = v[2];
2164 else if( transformtype ==
"quat" ) {
2165 Vector v(values.at(0),values.at(1), values.at(2),values.at(3));
2167 values.at(0) = v[0]; values.at(1) = v[1]; values.at(2) = v[2]; values.at(3) = v[3];
2169 else if( transformtype ==
"ikparam" ) {
2173 newikparam.SetValues(values.begin()+1,newiktype);
2174 newikparam.MultiplyTransform(t);
2175 newikparam.GetValues(values.begin()+1);
2183 inline static void _MultiplyTransformRight(
const Transform& t,
const std::string& name, std::vector<dReal>& values)
2185 size_t startoffset = name.find(
"_transform=");
2186 if( startoffset != std::string::npos ) {
2187 size_t endoffset = name.find(
"_", startoffset+11);
2188 std::string transformtype;
2189 if( endoffset == std::string::npos ) {
2190 transformtype = name.substr(startoffset+11);
2193 transformtype = name.substr(startoffset+11,endoffset-startoffset-11);
2195 if( transformtype ==
"direction" ) {
2196 Vector v(values.at(0),values.at(1), values.at(2));
2198 values.at(0) = v[0]; values.at(1) = v[1]; values.at(2) = v[2];
2200 else if( transformtype ==
"point" ) {
2201 Vector v(values.at(0),values.at(1), values.at(2));
2203 values.at(0) = v[0]; values.at(1) = v[1]; values.at(2) = v[2];
2205 else if( transformtype ==
"quat" ) {
2206 Vector v(values.at(0),values.at(1), values.at(2),values.at(3));
2208 values.at(0) = v[0]; values.at(1) = v[1]; values.at(2) = v[2]; values.at(3) = v[3];
2210 else if( transformtype ==
"ikparam" ) {
2214 newikparam.SetValues(values.begin()+1,newiktype);
2215 newikparam.MultiplyTransformRight(t);
2216 newikparam.GetValues(values.begin()+1);
2263 Vector voldtrans(v.x,v.y,0);
2264 Vector vnewtrans = t*voldtrans;
2329 void ApplyTransform(
const Transform& t);
2333 void Append(
const TriMesh& mesh);
2336 AABB ComputeAABB()
const;
2337 void serialize(std::ostream& o,
int options=0)
const;
2426 OPENRAVE_API void RaveGetVelocityFromAffineDOFVelocities(
Vector& linearvel,
Vector& angularvel, std::vector<dReal>::const_iterator itvalues,
int affinedofs,
const Vector& axis=
Vector(0,0,1),
const Vector& quatrotation =
Vector(1,0,0,0));
2449 namespace OpenRAVE {
2481 template <
typename T>
2484 if( !!pinterface ) {
2485 if( pinterface->GetInterfaceType() == T::GetInterfaceTypeStatic() ) {
2486 return boost::static_pointer_cast<T>(pinterface);
2489 if((pinterface->GetInterfaceType() == PT_Robot)&&(T::GetInterfaceTypeStatic() == PT_KinBody)) {
2490 return boost::static_pointer_cast<T>(pinterface);
2493 return boost::shared_ptr<T>();
2499 template <
typename T>
2502 if( !!pinterface ) {
2503 if( pinterface->GetInterfaceType() == T::GetInterfaceTypeStatic() ) {
2504 return boost::static_pointer_cast<T
const>(pinterface);
2507 if((pinterface->GetInterfaceType() == PT_Robot)&&(T::GetInterfaceTypeStatic() == PT_KinBody)) {
2508 return boost::static_pointer_cast<T
const>(pinterface);
2511 return boost::shared_ptr<T>();
2610 template <
typename T>
2611 inline boost::shared_ptr<T>
RaveClone(boost::shared_ptr<T const> preference,
int cloningoptions)
2615 boost::shared_ptr<T> pclonedcast = boost::dynamic_pointer_cast<T>(pcloned);
2617 pclonedcast->Clone(preference,cloningoptions);
2700 std::string tmp = pdirs;
2701 std::string::size_type pos = 0, newpos=0;
2702 while( pos < tmp.size() ) {
2704 newpos = tmp.find(
';', pos);
2706 newpos = tmp.find(
':', pos);
2708 std::string::size_type n = newpos == std::string::npos ? tmp.size()-pos : (newpos-pos);
2709 vdirs.push_back(tmp.substr(pos, n));
2710 if( newpos == std::string::npos ) {
2743 throw openrave_exception(str(boost::format(
"IkParameterization iktype 0x%x not supported")));
2748 #if !defined(OPENRAVE_DISABLE_ASSERT_HANDLER) && defined(BOOST_ENABLE_ASSERT_HANDLER)
2752 inline void assertion_failed(
char const * expr,
char const *
function,
char const * file,
long line)
2757 #if BOOST_VERSION>104600
2758 inline void assertion_failed_msg(
char const * expr,
char const * msg,
char const *
function,
char const * file,
long line)
2772 #ifdef RAVE_REGISTER_BOOST
2773 #include BOOST_TYPEOF_INCREMENT_REGISTRATION_GROUP()
2774 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::InterfaceType)
2775 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::UserData)
2777 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::ModuleBase)
2778 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::ControllerBase)
2779 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::PlannerBase)
2780 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::PlannerBase::PlannerParameters)
2781 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::IkSolverBase)
2782 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::SensorBase)
2783 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::SensorBase::SensorData)
2784 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::SensorSystemBase)
2785 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::SimpleSensorSystem)
2786 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::SimpleSensorSystem::XMLData)
2787 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::IkSolverBase)
2788 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::ViewerBase)
2789 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::SpaceSamplerBase)
2790 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::GraphHandle)
2791 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::IkParameterization)
2792 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::ConfigurationSpecification)
2793 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::ConfigurationSpecification::Group)
2794 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::ConfigurationSpecification::Reader)
2795 BOOST_TYPEOF_REGISTER_TEMPLATE(OpenRAVE::RaveVector, 1)
2798 BOOST_TYPEOF_REGISTER_TEMPLATE(OpenRAVE::OrientedBox, 1)
2800 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::KinBody)
2801 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::KinBody::Joint)
2802 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::KinBody::Joint::MIMIC)
2803 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::KinBody::Link)
2804 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::KinBody::Link::GEOMPROPERTIES)
2805 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::TriMesh)
2806 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::KinBody::KinBodyStateSaver)
2807 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::KinBody::BodyState)
2808 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::KinBody::ManageData)
2810 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::RobotBase)
2811 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::RobotBase::Manipulator)
2812 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::RobotBase::AttachedSensor)
2813 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::RobotBase::GRABBED)
2814 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::RobotBase::RobotStateSaver)
2816 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::TrajectoryBase)
2817 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::TrajectoryBase::TPOINT)
2818 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::TrajectoryBase::TSEGMENT)
2820 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::PLUGININFO)
2821 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::XMLReadable)
2822 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::InterfaceBase)
2823 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::BaseXMLReader)
2824 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::BaseXMLWriter)
2826 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::EnvironmentBase)
2827 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::EnvironmentBase::BODYSTATE)
2829 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::
RAY)
2830 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::
AABB)
2831 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::
OBB)
2832 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::TRIANGLE)