openrave.org

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
openrave.h
Go to the documentation of this file.
1 // -*- coding: utf-8 -*-
2 // Copyright (C) 2006-2012 Rosen Diankov <rosen.diankov@gmail.com>
3 //
4 // This file is part of OpenRAVE.
5 // OpenRAVE is free software: you can redistribute it and/or modify
6 // it under the terms of the GNU Lesser General Public License as published by
7 // the Free Software Foundation, either version 3 of the License, or
8 // at your option) any later version.
9 //
10 // This program is distributed in the hope that it will be useful,
11 // but WITHOUT ANY WARRANTY; without even the implied warranty of
12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 // GNU Lesser General Public License for more details.
14 //
15 // You should have received a copy of the GNU Lesser General Public License
16 // along with this program. If not, see <http://www.gnu.org/licenses/>.
20 #ifndef OPENRAVE_H
21 #define OPENRAVE_H
22 
23 #ifndef OPENRAVE_DISABLE_ASSERT_HANDLER
24 #define BOOST_ENABLE_ASSERT_HANDLER
25 #endif
26 
27 #include <cstdio>
28 #include <stdarg.h>
29 #include <cstring>
30 #include <cstdlib>
31 
32 #include <stdint.h>
33 
34 #ifdef _MSC_VER
35 
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
39 
40 // needed to get typeof working
41 //#include <boost/typeof/std/string.hpp>
42 //#include <boost/typeof/std/vector.hpp>
43 //#include <boost/typeof/std/list.hpp>
44 //#include <boost/typeof/std/map.hpp>
45 //#include <boost/typeof/std/set.hpp>
46 //#include <boost/typeof/std/string.hpp>
47 
48 #ifndef __PRETTY_FUNCTION__
49 #define __PRETTY_FUNCTION__ __FUNCDNAME__
50 #endif
51 
52 #else
53 #endif
54 
55 #include <string>
56 #include <vector>
57 #include <list>
58 #include <map>
59 #include <set>
60 #include <string>
61 #include <exception>
62 
63 #include <iomanip>
64 #include <fstream>
65 #include <sstream>
66 
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>
81 //#include <boost/cstdint.hpp>
82 
83 #if defined(__GNUC__)
84 #define RAVE_DEPRECATED __attribute__((deprecated))
85 #else
86 #define RAVE_DEPRECATED
87 #endif
88 
90 namespace OpenRAVE {
91 
92 #include <openrave/config.h>
94 
95 #if OPENRAVE_PRECISION // 1 if double precision
96 typedef double dReal;
97 #define g_fEpsilon 1e-15
98 #else
99 typedef float dReal;
100 #define g_fEpsilon 2e-7f
101 #endif
102 
104 static const dReal PI = dReal(3.14159265358979323846);
105 
108 
109 
136 
138 
153 };
154 
155 inline const char* GetErrorCodeString(OpenRAVEErrorCode error)
156 {
157  switch(error) {
158  case ORE_Failed: return "Failed";
159  case ORE_InvalidArguments: return "InvalidArguments";
160  case ORE_EnvironmentNotLocked: return "EnvironmentNotLocked";
161  case ORE_CommandNotSupported: return "CommandNotSupported";
162  case ORE_Assert: return "Assert";
163  case ORE_InvalidPlugin: return "InvalidPlugin";
164  case ORE_InvalidInterfaceHash: return "InvalidInterfaceHash";
165  case ORE_NotImplemented: return "NotImplemented";
166  case ORE_InconsistentConstraints: return "InconsistentConstraints";
167  case ORE_NotInitialized: return "NotInitialized";
168  case ORE_InvalidState: return "InvalidState";
169  case ORE_Timeout: return "Timeout";
170  }
171  // should throw an exception?
172  return "";
173 }
174 
176 class OPENRAVE_API openrave_exception : public std::exception
177 {
178 public:
179  openrave_exception() : std::exception(), _s("unknown exception"), _error(ORE_Failed) {
180  }
181  openrave_exception(const std::string& s, OpenRAVEErrorCode error=ORE_Failed) : std::exception() {
182  _error = error;
183  _s = "openrave (";
184  _s += GetErrorCodeString(_error);
185  _s += "): ";
186  _s += s;
187  }
188  virtual ~openrave_exception() throw() {
189  }
190  char const* what() const throw() {
191  return _s.c_str();
192  }
193  const std::string& message() const {
194  return _s;
195  }
197  return _error;
198  }
199 private:
200  std::string _s;
201  OpenRAVEErrorCode _error;
202 };
203 
205 {
206 public:
207  bool operator() (const std::string & s1, const std::string& s2) const
208  {
209  std::string::const_iterator it1=s1.begin();
210  std::string::const_iterator it2=s2.begin();
211 
212  //has the end of at least one of the strings been reached?
213  while ( (it1!=s1.end()) && (it2!=s2.end()) ) {
214  if(::toupper(*it1) != ::toupper(*it2)) { //letters differ?
215  // return -1 to indicate 'smaller than', 1 otherwise
216  return ::toupper(*it1) < ::toupper(*it2);
217  }
218  //proceed to the next character in each string
219  ++it1;
220  ++it2;
221  }
222  std::size_t size1=s1.size(), size2=s2.size(); // cache lengths
223  //return -1,0 or 1 according to strings' lengths
224  if (size1==size2) {
225  return 0;
226  }
227  return size1<size2;
228  }
229 };
230 
233 {
234 public:
235  virtual ~UserData() {
236  }
237 };
238 typedef boost::shared_ptr<UserData> UserDataPtr;
239 typedef boost::weak_ptr<UserData> UserDataWeakPtr;
240 
243 {
244 public:
245  virtual ~SerializableData() {
246  }
247 
249  virtual void Serialize(std::ostream& O, int options=0) const = 0;
250 
252  virtual void Deserialize(std::istream& I) = 0;
253 };
254 typedef boost::shared_ptr<SerializableData> SerializableDataPtr;
255 typedef boost::weak_ptr<SerializableData> SerializableDataWeakPtr;
256 
257 // terminal attributes
258 //#define RESET 0
259 //#define BRIGHT 1
260 //#define DIM 2
261 //#define UNDERLINE 3
262 //#define BLINK 4
263 //#define REVERSE 7
264 //#define HIDDEN 8
265 // terminal colors
266 //#define BLACK 0
267 //#define RED 1
268 //#define GREEN 2
269 //#define YELLOW 3
270 //#define BLUE 4
271 //#define MAGENTA 5
272 //#define CYAN 6
273 //#define WHITE 7
274 
276 inline std::string ChangeTextColor (int attribute, int fg, int bg)
277 {
278  char command[13];
279  sprintf (command, "%c[%d;%d;%dm", 0x1B, attribute, fg + 30, bg + 40);
280  return command;
281 }
282 
284 inline std::string ChangeTextColor (int attribute, int fg)
285 {
286  char command[13];
287  sprintf (command, "%c[%d;%dm", 0x1B, attribute, fg + 30);
288  return command;
289 }
290 
292 inline std::string ResetTextColor()
293 {
294  char command[12];
295  sprintf (command, "%c[0;38;48m", 0x1B);
296  return command;
297 }
298 
299 inline std::wstring ChangeTextColorW (int attribute, int fg)
300 {
301  wchar_t command[13];
302  swprintf (command, 13, L"%c[%d;%dm", 0x1B, attribute, fg + 30);
303  return command;
304 }
305 
306 inline std::wstring RavePrintTransformString(const wchar_t* fmt)
307 {
308  std::vector<int> positions;
309  std::wstring str = fmt;
310  wchar_t* p = wcsstr(&str[0], L"%s");
311  while(p != NULL ) {
312  positions.push_back((int)(p-&str[0])+1);
313  p = wcsstr(p+2, L"%s");
314  }
315 
316  p = wcsstr(&str[0], L"%S");
317  while(p != NULL ) {
318  p[1] = 's';
319  p = wcsstr(p+2, L"%S");
320  }
321 
322  p = wcsstr(&str[0], L"%ls");
323  while(p != NULL ) {
324  p[1] = 's';
325  p[2] = ' ';
326  p = wcsstr(p+2, L"%ls");
327  }
328 
329  for(int i = 0; i < (int)positions.size(); ++i)
330  str[positions[i]] = 'S';
331  return str;
332 }
333 
342  Level_VerifyPlans=0x80000000,
343 };
344 
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
351 
353 OPENRAVE_API void RaveSetDebugLevel(int level);
354 
357 
359 inline const char* RaveGetSourceFilename(const char* pfilename)
360 {
361  if( pfilename == NULL ) {
362  return "";
363  }
364  const char* p0 = strrchr(pfilename,'/');
365  const char* p1 = strrchr(pfilename,'\\');
366  const char* p = p0 > p1 ? p0 : p1;
367  if( p == NULL ) {
368  return pfilename;
369  }
370  return p+1;
371 }
372 
373 #ifdef _WIN32
374 
375 #define DefineRavePrintfW(LEVEL) \
376  inline int RavePrintfW ## LEVEL(const wchar_t *fmt, ...) \
377  { \
378  /*ChangeTextColor (stdout, 0, OPENRAVECOLOR##LEVEL);*/ \
379  va_list list; \
380  va_start(list,fmt); \
381  int r = vwprintf(OpenRAVE::RavePrintTransformString(fmt).c_str(), list); \
382  va_end(list); \
383  /*ResetTextColor (stdout);*/ \
384  return r; \
385  }
386 
387 #define DefineRavePrintfA(LEVEL) \
388  inline int RavePrintfA ## LEVEL(const std::string& s) \
389  { \
390  if((s.size() == 0)||(s[s.size()-1] != '\n')) { \
391  printf("%s\n", s.c_str()); \
392  } \
393  else { \
394  printf ("%s", s.c_str()); \
395  } \
396  return s.size(); \
397  } \
398  \
399  inline int RavePrintfA ## LEVEL(const char *fmt, ...) \
400  { \
401  /*ChangeTextColor (stdout, 0, OPENRAVECOLOR##LEVEL);*/ \
402  va_list list; \
403  va_start(list,fmt); \
404  int r = vprintf(fmt, list); \
405  va_end(list); \
406  /*if( fmt[0] != '\n' ) { printf("\n"); }*/ \
407  /*ResetTextColor(stdout);*/ \
408  return r; \
409  }
410 
411 inline int RavePrintfA(const std::string& s, uint32_t level)
412 {
413  if((s.size() == 0)||(s[s.size()-1] != '\n')) { // automatically add a new line
414  printf("%s\n", s.c_str());
415  }
416  else {
417  printf ("%s", s.c_str());
418  }
419  return s.size();
420 }
421 
422 DefineRavePrintfW(_INFOLEVEL)
423 DefineRavePrintfA(_INFOLEVEL)
424 
425 #else
426 
427 #define DefineRavePrintfW(LEVEL) \
428  inline int RavePrintfW ## LEVEL(const wchar_t *wfmt, ...) \
429  { \
430  va_list list; \
431  va_start(list,wfmt); \
432  /* Allocate memory on the stack to avoid heap fragmentation */ \
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); \
439  va_end(list); \
440  return r; \
441  }
442 
443 // In linux, only wprintf will succeed, due to the fwide() call in main, so
444 // for programmers who want to use regular format strings without
445 // the L in front, we will take their regular string and widen it
446 // for them.
447 inline int RavePrintfA_INFOLEVEL(const std::string& s)
448 {
449  if((s.size() == 0)||(s[s.size()-1] != '\n')) { // automatically add a new line
450  printf("%s\n", s.c_str());
451  }
452  else {
453  printf ("%s", s.c_str());
454  }
455  return s.size();
456 }
457 
458 inline int RavePrintfA_INFOLEVEL(const char *fmt, ...)
459 {
460  va_list list;
461  va_start(list,fmt);
462  int r = vprintf(fmt, list);
463  va_end(list);
464  //if( fmt[0] != '\n' ) { printf("\n"); }
465  return r;
466 }
467 
468 #define DefineRavePrintfA(LEVEL) \
469  inline int RavePrintfA ## LEVEL(const std::string& s) \
470  { \
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); \
473  } \
474  else { \
475  printf ("%c[0;%d;%dm%s%c[m", 0x1B, OPENRAVECOLOR ## LEVEL + 30,8+40,s.c_str(),0x1B); \
476  } \
477  return s.size(); \
478  } \
479  \
480  inline int RavePrintfA ## LEVEL(const char *fmt, ...) \
481  { \
482  va_list list; \
483  va_start(list,fmt); \
484  int r = vprintf((ChangeTextColor(0, OPENRAVECOLOR ## LEVEL,8) + std::string(fmt) + ResetTextColor()).c_str(), list); \
485  va_end(list); \
486  /*if( fmt[0] != '\n' ) { printf("\n"); } */ \
487  return r; \
488  } \
489 
490 
491 inline int RavePrintfA(const std::string& s, uint32_t level)
492 {
494  int color = 0;
495  switch(level) {
496  case Level_Fatal: color = OPENRAVECOLOR_FATALLEVEL; break;
497  case Level_Error: color = OPENRAVECOLOR_ERRORLEVEL; break;
498  case Level_Warn: color = OPENRAVECOLOR_WARNLEVEL; break;
499  case Level_Info: // print regular
500  if((s.size() == 0)||(s[s.size()-1] != '\n')) { // automatically add a new line
501  printf ("%s\n",s.c_str());
502  }
503  else {
504  printf ("%s",s.c_str());
505  }
506  return s.size();
507  case Level_Debug: color = OPENRAVECOLOR_DEBUGLEVEL; break;
508  case Level_Verbose: color = OPENRAVECOLOR_VERBOSELEVEL; break;
509  }
510  if((s.size() == 0)||(s[s.size()-1] != '\n')) { // automatically add a new line
511  printf ("%c[0;%d;%dm%s%c[0;38;48m\n", 0x1B, color + 30,8+40,s.c_str(),0x1B);
512  }
513  else {
514  printf ("%c[0;%d;%dm%s%c[0;38;48m", 0x1B, color + 30,8+40,s.c_str(),0x1B);
515  }
516  return s.size();
517  }
518  return 0;
519 }
520 
521 #endif
522 
523 DefineRavePrintfW(_FATALLEVEL)
524 DefineRavePrintfW(_ERRORLEVEL)
525 DefineRavePrintfW(_WARNLEVEL)
526 //DefineRavePrintfW(_INFOLEVEL)
527 DefineRavePrintfW(_DEBUGLEVEL)
528 DefineRavePrintfW(_VERBOSELEVEL)
529 
530 DefineRavePrintfA(_FATALLEVEL)
531 DefineRavePrintfA(_ERRORLEVEL)
532 DefineRavePrintfA(_WARNLEVEL)
533 //DefineRavePrintfA(_INFOLEVEL)
534 DefineRavePrintfA(_DEBUGLEVEL)
535 DefineRavePrintfA(_VERBOSELEVEL)
536 
537 #define RAVEPRINTHEADER(LEVEL) OpenRAVE::RavePrintfA ## LEVEL("[%s:%d] ", OpenRAVE::RaveGetSourceFilename(__FILE__), __LINE__)
538 
539 // different logging levels. The higher the suffix number, the less important the information is.
540 // 0 log level logs all the time. OpenRAVE starts up with a log level of 0.
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
543 
544 // define log4cxx equivalents (eventually OpenRAVE will move to log4cxx logging)
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
563 
564 #define IS_DEBUGLEVEL(level) ((OpenRAVE::RaveGetDebugLevel()&OpenRAVE::Level_OutputMask)>=(level))
565 
566 #define OPENRAVE_EXCEPTION_FORMAT0(s, errorcode) OpenRAVE::openrave_exception(boost::str(boost::format("[%s:%d] " s)%(__PRETTY_FUNCTION__)%(__LINE__)),errorcode)
567 
569 #define OPENRAVE_EXCEPTION_FORMAT(s, args,errorcode) OpenRAVE::openrave_exception(boost::str(boost::format("[%s:%d] " s)%(__PRETTY_FUNCTION__)%(__LINE__)%args),errorcode)
570 
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); } }
572 
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); } }
574 
575 // note that expr1 and expr2 will be evaluated twice if not equal
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); } }
577 
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); } }
579 
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); } }
581 
582 #define OPENRAVE_DUMMY_IMPLEMENTATION { throw OPENRAVE_EXCEPTION_FORMAT0("not implemented",ORE_NotImplemented); }
583 
585 enum InterfaceType
586 {
587  PT_Planner=1,
588  PT_Robot=2,
589  PT_SensorSystem=3,
590  PT_Controller=4,
591  PT_Module=5,
592  PT_ProblemInstance=5,
593  PT_IkSolver=6,
594  PT_InverseKinematicsSolver=6,
595  PT_KinBody=7,
596  PT_PhysicsEngine=8,
597  PT_Sensor=9,
598  PT_CollisionChecker=10,
599  PT_Trajectory=11,
600  PT_Viewer=12,
601  PT_SpaceSampler=13,
602  PT_NumberOfInterfaces=13
603 };
604 
605 class CollisionReport;
606 class InterfaceBase;
607 class IkSolverBase;
608 class TrajectoryBase;
609 class ControllerBase;
610 class PlannerBase;
611 class RobotBase;
612 class ModuleBase;
613 class EnvironmentBase;
614 class KinBody;
615 class SensorSystemBase;
616 class PhysicsEngineBase;
617 class SensorBase;
618 class CollisionCheckerBase;
619 class ViewerBase;
620 class SpaceSamplerBase;
621 class IkParameterization;
622 class ConfigurationSpecification;
623 class IkReturn;
624 
625 typedef boost::shared_ptr<CollisionReport> CollisionReportPtr;
626 typedef boost::shared_ptr<CollisionReport const> CollisionReportConstPtr;
627 typedef boost::shared_ptr<InterfaceBase> InterfaceBasePtr;
628 typedef boost::shared_ptr<InterfaceBase const> InterfaceBaseConstPtr;
629 typedef boost::weak_ptr<InterfaceBase> InterfaceBaseWeakPtr;
630 typedef boost::shared_ptr<KinBody> KinBodyPtr;
631 typedef boost::shared_ptr<KinBody const> KinBodyConstPtr;
632 typedef boost::weak_ptr<KinBody> KinBodyWeakPtr;
633 typedef boost::shared_ptr<RobotBase> RobotBasePtr;
634 typedef boost::shared_ptr<RobotBase const> RobotBaseConstPtr;
635 typedef boost::weak_ptr<RobotBase> RobotBaseWeakPtr;
636 typedef boost::shared_ptr<CollisionCheckerBase> CollisionCheckerBasePtr;
637 typedef boost::shared_ptr<CollisionCheckerBase const> CollisionCheckerBaseConstPtr;
638 typedef boost::weak_ptr<CollisionCheckerBase> CollisionCheckerBaseWeakPtr;
639 typedef boost::shared_ptr<ControllerBase> ControllerBasePtr;
640 typedef boost::shared_ptr<ControllerBase const> ControllerBaseConstPtr;
641 typedef boost::weak_ptr<ControllerBase> ControllerBaseWeakPtr;
642 typedef boost::shared_ptr<IkSolverBase> IkSolverBasePtr;
643 typedef boost::shared_ptr<IkSolverBase const> IkSolverBaseConstPtr;
644 typedef boost::weak_ptr<IkSolverBase> IkSolverBaseWeakPtr;
645 typedef boost::shared_ptr<PhysicsEngineBase> PhysicsEngineBasePtr;
646 typedef boost::shared_ptr<PhysicsEngineBase const> PhysicsEngineBaseConstPtr;
647 typedef boost::weak_ptr<PhysicsEngineBase> PhysicsEngineBaseWeakPtr;
648 typedef boost::shared_ptr<PlannerBase> PlannerBasePtr;
649 typedef boost::shared_ptr<PlannerBase const> PlannerBaseConstPtr;
650 typedef boost::weak_ptr<PlannerBase> PlannerBaseWeakPtr;
651 typedef boost::shared_ptr<ModuleBase> ModuleBasePtr;
652 typedef boost::shared_ptr<ModuleBase const> ModuleBaseConstPtr;
653 typedef boost::weak_ptr<ModuleBase> ModuleBaseWeakPtr;
654 typedef boost::shared_ptr<SensorBase> SensorBasePtr;
655 typedef boost::shared_ptr<SensorBase const> SensorBaseConstPtr;
656 typedef boost::weak_ptr<SensorBase> SensorBaseWeakPtr;
657 typedef boost::shared_ptr<SensorSystemBase> SensorSystemBasePtr;
658 typedef boost::shared_ptr<SensorSystemBase const> SensorSystemBaseConstPtr;
659 typedef boost::weak_ptr<SensorSystemBase> SensorSystemBaseWeakPtr;
660 typedef boost::shared_ptr<TrajectoryBase> TrajectoryBasePtr;
661 typedef boost::shared_ptr<TrajectoryBase const> TrajectoryBaseConstPtr;
662 typedef boost::weak_ptr<TrajectoryBase> TrajectoryBaseWeakPtr;
663 typedef boost::shared_ptr<ViewerBase> ViewerBasePtr;
664 typedef boost::shared_ptr<ViewerBase const> ViewerBaseConstPtr;
665 typedef boost::weak_ptr<ViewerBase> ViewerBaseWeakPtr;
666 typedef boost::shared_ptr<SpaceSamplerBase> SpaceSamplerBasePtr;
667 typedef boost::shared_ptr<SpaceSamplerBase const> SpaceSamplerBaseConstPtr;
668 typedef boost::weak_ptr<SpaceSamplerBase> SpaceSamplerBaseWeakPtr;
669 typedef boost::shared_ptr<EnvironmentBase> EnvironmentBasePtr;
670 typedef boost::shared_ptr<EnvironmentBase const> EnvironmentBaseConstPtr;
671 typedef boost::weak_ptr<EnvironmentBase> EnvironmentBaseWeakPtr;
672 
673 typedef boost::shared_ptr<IkReturn> IkReturnPtr;
674 typedef boost::weak_ptr<IkReturn> IkReturnWeakPtr;
675 
677 typedef boost::shared_ptr<BaseXMLReader> BaseXMLReaderPtr;
678 typedef boost::shared_ptr<BaseXMLReader const> BaseXMLReaderConstPtr;
680 typedef boost::shared_ptr<BaseXMLWriter> BaseXMLWriterPtr;
681 typedef boost::shared_ptr<BaseXMLWriter const> BaseXMLWriterConstPtr;
682 
690 };
691 
694 {
695 public:
696  XMLReadable(const std::string& xmlid) : __xmlid(xmlid) {
697  }
698  virtual ~XMLReadable() {
699  }
700  virtual const std::string& GetXMLId() const {
701  return __xmlid;
702  }
704  virtual void Serialize(BaseXMLWriterPtr writer, int options=0) const {
705  }
706 private:
707  std::string __xmlid;
708 };
709 
710 typedef boost::shared_ptr<XMLReadable> XMLReadablePtr;
711 typedef boost::shared_ptr<XMLReadable const> XMLReadableConstPtr;
712 
714 typedef std::list<std::pair<std::string,std::string> > AttributesList;
715 
719 class OPENRAVE_API BaseXMLReader : public boost::enable_shared_from_this<BaseXMLReader>
720 {
721 public:
723  {
724  PE_Pass=0,
725  PE_Support=1,
726  PE_Ignore=2,
727  };
729  }
730  virtual ~BaseXMLReader() {
731  }
732 
736  return XMLReadablePtr();
737  }
738 
743  virtual ProcessElement startElement(const std::string& name, const AttributesList& atts) = 0;
744 
748  virtual bool endElement(const std::string& name) = 0;
749 
752  virtual void characters(const std::string& ch) = 0;
753 
755  std::string _filename;
756 };
757 
758 typedef boost::function<BaseXMLReaderPtr(InterfaceBasePtr, const AttributesList&)> CreateXMLReaderFn;
759 
762 {
763 public:
764  DummyXMLReader(const std::string& fieldname, const std::string& parentname, boost::shared_ptr<std::ostream> osrecord = boost::shared_ptr<std::ostream>());
765  virtual ProcessElement startElement(const std::string& name, const AttributesList& atts);
766  virtual bool endElement(const std::string& name);
767  virtual void characters(const std::string& ch);
768  const std::string& GetFieldName() const {
769  return _fieldname;
770  }
771  virtual boost::shared_ptr<std::ostream> GetStream() const {
772  return _osrecord;
773  }
774 private:
775  std::string _parentname;
776  std::string _fieldname;
777  boost::shared_ptr<std::ostream> _osrecord;
778  boost::shared_ptr<BaseXMLReader> _pcurreader;
779 };
780 
784 class OPENRAVE_API BaseXMLWriter : public boost::enable_shared_from_this<BaseXMLWriter>
785 {
786 public:
787  virtual ~BaseXMLWriter() {
788  }
792  virtual const std::string& GetFormat() const = 0;
793 
797  virtual void SetCharData(const std::string& data) = 0;
798 
800  virtual BaseXMLWriterPtr AddChild(const std::string& xmltag, const AttributesList& atts=AttributesList()) = 0;
801 };
802 
803 } // end namespace OpenRAVE
804 
805 // define the math functions
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
820 #else // 32bit float
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
834 #endif
835 
836 #include <openrave/geometry.h>
837 #include <openrave/mathextra.h>
838 
839 namespace OpenRAVE {
840 using geometry::RaveVector;
841 using geometry::RaveTransform;
842 using geometry::RaveTransformMatrix;
843 typedef RaveVector<dReal> Vector;
844 typedef RaveTransform<dReal> Transform;
845 typedef boost::shared_ptr< RaveTransform<dReal> > TransformPtr;
846 typedef boost::shared_ptr< RaveTransform<dReal> const > TransformConstPtr;
847 typedef RaveTransformMatrix<dReal> TransformMatrix;
848 typedef boost::shared_ptr< RaveTransformMatrix<dReal> > TransformMatrixPtr;
849 typedef boost::shared_ptr< RaveTransformMatrix<dReal> const > TransformMatrixConstPtr;
853 
854 // for compatibility
856 using mathextra::dot2;
857 using mathextra::dot3;
858 using mathextra::dot4;
862 using mathextra::cross3;
863 using mathextra::inv3;
864 using mathextra::inv4;
868 using mathextra::mult4;
870 
878  IKP_Transform6D=0x67000001,
879  IKP_Rotation3D=0x34000002,
880  IKP_Translation3D=0x33000003,
881  IKP_Direction3D=0x23000004,
882  IKP_Ray4D=0x46000005,
883  IKP_Lookat3D=0x23000006,
885  IKP_TranslationXY2D=0x22000008,
888 
892 
896 
898 
899  IKP_VelocityDataBit = 0x00008000,
916 
917  IKP_UniqueIdMask = 0x0000ffff,
918  IKP_CustomDataBit = 0x00010000,
919 };
920 
924 OPENRAVE_API const std::map<IkParameterizationType,std::string>& RaveGetIkParameterizationMap(int alllowercase=0);
925 
928 
941 {
942 public:
943 
946  {
947 public:
948  Group() : offset(0), dof(0) {
949  }
950 
951  inline bool operator==(const Group& r) const {
952  return offset==r.offset && dof==r.dof && name==r.name && interpolation==r.interpolation;
953  }
954  inline bool operator!=(const Group& r) const {
955  return offset!=r.offset || dof!=r.dof || name!=r.name || interpolation!=r.interpolation;
956  }
957 
959  int offset;
961  int dof;
978  std::string name;
989  std::string interpolation;
990  };
991 
992  class Reader : public BaseXMLReader
993  {
994 public:
996  virtual ProcessElement startElement(const std::string& name, const AttributesList& atts);
997  virtual bool endElement(const std::string& name);
998  virtual void characters(const std::string& ch);
999 protected:
1001  std::stringstream _ss;
1003  };
1004 
1008 
1010  }
1011 
1013  virtual int GetDOF() const;
1014 
1019  virtual bool IsValid() const;
1020 
1025  virtual void Validate() const;
1026 
1027  virtual bool operator==(const ConfigurationSpecification& r) const;
1028  virtual bool operator!=(const ConfigurationSpecification& r) const;
1029 
1034  virtual const Group& GetGroupFromName(const std::string& name) const;
1035 
1040  virtual Group& GetGroupFromName(const std::string& name);
1041 
1047  virtual std::vector<Group>::const_iterator FindCompatibleGroup(const Group& g, bool exactmatch=false) const;
1048 
1054  virtual std::vector<Group>::const_iterator FindCompatibleGroup(const std::string& name, bool exactmatch=false) const;
1055 
1063  virtual std::vector<Group>::const_iterator FindTimeDerivativeGroup(const Group& g, bool exactmatch=false) const;
1064 
1072  virtual std::vector<Group>::const_iterator FindTimeDerivativeGroup(const std::string& name, bool exactmatch=false) const;
1073 
1082  virtual void AddDerivativeGroups(int deriv, bool adddeltatime=false);
1083 
1085  inline void AddVelocityGroups(bool adddeltatime) RAVE_DEPRECATED {
1086  AddDerivativeGroups(1,adddeltatime);
1087  }
1088 
1095  virtual ConfigurationSpecification ConvertToVelocitySpecification() const;
1096 
1100  virtual ConfigurationSpecification GetTimeDerivativeSpecification(int timederivative) const;
1101 
1104  virtual void ResetGroupOffsets();
1105 
1107  virtual int AddDeltaTimeGroup();
1108 
1115  virtual int AddGroup(const std::string& name, int dof, const std::string& interpolation = "");
1116 
1122  virtual ConfigurationSpecification& operator+= (const ConfigurationSpecification& r);
1123 
1129  virtual ConfigurationSpecification operator+ (const ConfigurationSpecification& r) const;
1130 
1139  virtual bool ExtractTransform(Transform& t, std::vector<dReal>::const_iterator itdata, KinBodyConstPtr pbody, int timederivative=0) const;
1140 
1149  virtual bool ExtractIkParameterization(IkParameterization& ikparam, std::vector<dReal>::const_iterator itdata, int timederivative=0) const;
1150 
1160  virtual bool ExtractAffineValues(std::vector<dReal>::iterator itvalues, std::vector<dReal>::const_iterator itdata, KinBodyConstPtr pbody, int affinedofs, int timederivative=0) const;
1161 
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;
1172 
1176  virtual bool ExtractDeltaTime(dReal& deltatime, std::vector<dReal>::const_iterator itdata) const;
1177 
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;
1188 
1195  virtual bool InsertDeltaTime(std::vector<dReal>::iterator itdata, dReal deltatime) const;
1196 
1204  virtual int AddGroup(const Group& g);
1205 
1206  typedef boost::function<void (const std::vector<dReal>&)> SetConfigurationStateFn;
1207  typedef boost::function<void (std::vector<dReal>&)> GetConfigurationStateFn;
1208 
1210  virtual boost::shared_ptr<SetConfigurationStateFn> GetSetFn(EnvironmentBasePtr penv) const;
1211 
1213  virtual boost::shared_ptr<GetConfigurationStateFn> GetGetFn(EnvironmentBasePtr penv) const;
1214 
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);
1229 
1240  static void ConvertData(std::vector<dReal>::iterator ittargetdata, const ConfigurationSpecification& targetspec, std::vector<dReal>::const_iterator itsourcedata, const ConfigurationSpecification& sourcespec, size_t numpoints, EnvironmentBaseConstPtr penv, bool filluninitialized = true);
1241 
1247  static std::string GetInterpolationDerivative(const std::string& interpolation, int deriv=1);
1248 
1249  std::vector<Group> _vgroups;
1250 };
1251 
1252 OPENRAVE_API std::ostream& operator<<(std::ostream& O, const ConfigurationSpecification &spec);
1253 OPENRAVE_API std::istream& operator>>(std::istream& I, ConfigurationSpecification& spec);
1254 
1255 typedef boost::shared_ptr<ConfigurationSpecification> ConfigurationSpecificationPtr;
1256 typedef boost::shared_ptr<ConfigurationSpecification const> ConfigurationSpecificationConstPtr;
1257 
1258 template <typename T>
1259 inline T NormalizeCircularAnglePrivate(T theta, T min, T max)
1260 {
1261  if (theta < min) {
1262  T range = max-min;
1263  theta += range;
1264  while (theta < min) {
1265  theta += range;
1266  }
1267  }
1268  else if (theta > max) {
1269  T range = max-min;
1270  theta -= range;
1271  while (theta > max) {
1272  theta -= range;
1273  }
1274  }
1275  return theta;
1276 }
1277 
1278 
1285 {
1286 public:
1290  static const IkParameterizationType Type_Transform6D RAVE_DEPRECATED = IKP_Transform6D;
1293  static const IkParameterizationType Type_Direction3D RAVE_DEPRECATED = IKP_Direction3D;
1296  static const IkParameterizationType Type_TranslationDirection5D RAVE_DEPRECATED = IKP_TranslationDirection5D;
1297  static const IkParameterizationType Type_TranslationXY2D RAVE_DEPRECATED = IKP_TranslationXY2D;
1298  static const IkParameterizationType Type_TranslationXYOrientation3D RAVE_DEPRECATED = IKP_TranslationXYOrientation3D;
1299  static const IkParameterizationType Type_TranslationLocalGlobal6D RAVE_DEPRECATED = IKP_TranslationLocalGlobal6D;
1300  static const IkParameterizationType Type_NumberOfParameterizations RAVE_DEPRECATED = IKP_NumberOfParameterizations;
1301 
1303  }
1306  SetTransform6D(t);
1307  }
1310  SetRay4D(r);
1311  }
1314  _type=type;
1315  switch(_type) {
1316  case IKP_Transform6D: SetTransform6D(t); break;
1317  case IKP_Rotation3D: SetRotation3D(t.rot); break;
1318  case IKP_Translation3D: SetTranslation3D(t.trans); break;
1319  case IKP_Lookat3D: SetLookat3D(t.trans); break;
1320  default:
1321  throw openrave_exception(str(boost::format("IkParameterization constructor does not support type 0x%x")%_type));
1322  }
1323  }
1324 
1326  return _type;
1327  }
1328 
1330  inline const std::string& GetName() const;
1331 
1333  static int GetDOF(IkParameterizationType type) {
1334  return (type>>28)&0xf;
1335  }
1337  inline int GetDOF() const {
1338  return (_type>>28)&0xf;
1339  }
1340 
1343  return (type>>24)&0xf;
1344  }
1346  inline int GetNumberOfValues() const {
1347  return (_type>>24)&0xf;
1348  }
1349 
1350  inline void SetTransform6D(const Transform& t) {
1351  _type = IKP_Transform6D; _transform = t;
1352  }
1353  inline void SetRotation3D(const Vector& quaternion) {
1354  _type = IKP_Rotation3D; _transform.rot = quaternion;
1355  }
1356  inline void SetTranslation3D(const Vector& trans) {
1357  _type = IKP_Translation3D; _transform.trans = trans;
1358  }
1359  inline void SetDirection3D(const Vector& dir) {
1360  _type = IKP_Direction3D; _transform.rot = dir;
1361  }
1362  inline void SetRay4D(const RAY& ray) {
1363  _type = IKP_Ray4D; _transform.trans = ray.pos; _transform.rot = ray.dir;
1364  }
1365  inline void SetLookat3D(const Vector& trans) {
1366  _type = IKP_Lookat3D; _transform.trans = trans;
1367  }
1369  inline void SetLookat3D(const RAY& ray) {
1370  _type = IKP_Lookat3D; _transform.trans = ray.pos; _transform.rot = ray.dir;
1371  }
1372  inline void SetTranslationDirection5D(const RAY& ray) {
1373  _type = IKP_TranslationDirection5D; _transform.trans = ray.pos; _transform.rot = ray.dir;
1374  }
1375  inline void SetTranslationXY2D(const Vector& trans) {
1376  _type = IKP_TranslationXY2D; _transform.trans.x = trans.x; _transform.trans.y = trans.y; _transform.trans.z = 0; _transform.trans.w = 0;
1377  }
1378  inline void SetTranslationXYOrientation3D(const Vector& trans) {
1379  _type = IKP_TranslationXYOrientation3D; _transform.trans.x = trans.x; _transform.trans.y = trans.y; _transform.trans.z = trans.z; _transform.trans.w = 0;
1380  }
1381  inline void SetTranslationLocalGlobal6D(const Vector& localtrans, const Vector& trans) {
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;
1383  }
1384  inline void SetTranslationXAxisAngle4D(const Vector& trans, dReal angle) {
1386  _transform.trans = trans;
1387  _transform.rot.x = angle;
1388  }
1389  inline void SetTranslationYAxisAngle4D(const Vector& trans, dReal angle) {
1391  _transform.trans = trans;
1392  _transform.rot.x = angle;
1393  }
1394  inline void SetTranslationZAxisAngle4D(const Vector& trans, dReal angle) {
1396  _transform.trans = trans;
1397  _transform.rot.x = angle;
1398  }
1399 
1400  inline void SetTranslationXAxisAngleZNorm4D(const Vector& trans, dReal angle) {
1402  _transform.trans = trans;
1403  _transform.rot.x = angle;
1404  }
1405  inline void SetTranslationYAxisAngleXNorm4D(const Vector& trans, dReal angle) {
1407  _transform.trans = trans;
1408  _transform.rot.x = angle;
1409  }
1410  inline void SetTranslationZAxisAngleYNorm4D(const Vector& trans, dReal angle) {
1412  _transform.trans = trans;
1413  _transform.rot.x = angle;
1414  }
1415 
1416  inline const Transform& GetTransform6D() const {
1417  return _transform;
1418  }
1419  inline const Vector& GetRotation3D() const {
1420  return _transform.rot;
1421  }
1422  inline const Vector& GetTranslation3D() const {
1423  return _transform.trans;
1424  }
1425  inline const Vector& GetDirection3D() const {
1426  return _transform.rot;
1427  }
1428  inline const RAY GetRay4D() const {
1429  return RAY(_transform.trans,_transform.rot);
1430  }
1431  inline const Vector& GetLookat3D() const {
1432  return _transform.trans;
1433  }
1434  inline const Vector& GetLookat3DDirection() const {
1435  return _transform.rot;
1436  }
1437  inline const RAY GetTranslationDirection5D() const {
1438  return RAY(_transform.trans,_transform.rot);
1439  }
1440  inline const Vector& GetTranslationXY2D() const {
1441  return _transform.trans;
1442  }
1443  inline const Vector& GetTranslationXYOrientation3D() const {
1444  return _transform.trans;
1445  }
1446  inline std::pair<Vector,Vector> GetTranslationLocalGlobal6D() const {
1447  return std::make_pair(_transform.rot,_transform.trans);
1448  }
1449  inline std::pair<Vector,dReal> GetTranslationXAxisAngle4D() const {
1450  return std::make_pair(_transform.trans,_transform.rot.x);
1451  }
1452  inline std::pair<Vector,dReal> GetTranslationYAxisAngle4D() const {
1453  return std::make_pair(_transform.trans,_transform.rot.x);
1454  }
1455  inline std::pair<Vector,dReal> GetTranslationZAxisAngle4D() const {
1456  return std::make_pair(_transform.trans,_transform.rot.x);
1457  }
1458  inline std::pair<Vector,dReal> GetTranslationXAxisAngleZNorm4D() const {
1459  return std::make_pair(_transform.trans,_transform.rot.x);
1460  }
1461  inline std::pair<Vector,dReal> GetTranslationYAxisAngleXNorm4D() const {
1462  return std::make_pair(_transform.trans,_transform.rot.x);
1463  }
1464  inline std::pair<Vector,dReal> GetTranslationZAxisAngleYNorm4D() const {
1465  return std::make_pair(_transform.trans,_transform.rot.x);
1466  }
1467 
1469  inline dReal ComputeDistanceSqr(const IkParameterization& ikparam) const
1470  {
1471  const dReal anglemult = 0.4; // this is a hack that should be removed....
1472  BOOST_ASSERT(_type==ikparam.GetType());
1473  switch(_type) {
1474  case IKP_Transform6D: {
1475  Transform t0 = GetTransform6D(), t1 = ikparam.GetTransform6D();
1476  dReal fcos = RaveFabs(t0.rot.dot(t1.rot));
1477  dReal facos = fcos >= 1 ? 0 : RaveAcos(fcos);
1478  return (t0.trans-t1.trans).lengthsqr3() + anglemult*facos*facos;
1479  }
1480  case IKP_Rotation3D: {
1481  dReal fcos = RaveFabs(GetRotation3D().dot(ikparam.GetRotation3D()));
1482  dReal facos = fcos >= 1 ? 0 : RaveAcos(fcos);
1483  return facos*facos;
1484  }
1485  case IKP_Translation3D:
1486  return (GetTranslation3D()-ikparam.GetTranslation3D()).lengthsqr3();
1487  case IKP_Direction3D: {
1488  dReal fcos = GetDirection3D().dot(ikparam.GetDirection3D());
1489  dReal facos = fcos >= 1 ? 0 : RaveAcos(fcos);
1490  return facos*facos;
1491  }
1492  case IKP_Ray4D: {
1493  Vector pos0 = GetRay4D().pos - GetRay4D().dir*GetRay4D().dir.dot(GetRay4D().pos);
1494  Vector pos1 = ikparam.GetRay4D().pos - ikparam.GetRay4D().dir*ikparam.GetRay4D().dir.dot(ikparam.GetRay4D().pos);
1495  dReal fcos = GetRay4D().dir.dot(ikparam.GetRay4D().dir);
1496  dReal facos = fcos >= 1 ? 0 : RaveAcos(fcos);
1497  return (pos0-pos1).lengthsqr3() + anglemult*facos*facos;
1498  }
1499  case IKP_Lookat3D: {
1500  Vector v = GetLookat3D()-ikparam.GetLookat3D();
1501  dReal s = v.dot3(ikparam.GetLookat3DDirection());
1502  if( s >= -1 ) { // ikparam's lookat is always 1 beyond the origin, this is just the convention for testing...
1503  v -= s*ikparam.GetLookat3DDirection();
1504  }
1505  return v.lengthsqr3();
1506  }
1508  dReal fcos = GetTranslationDirection5D().dir.dot(ikparam.GetTranslationDirection5D().dir);
1509  dReal facos = fcos >= 1 ? 0 : RaveAcos(fcos);
1510  return (GetTranslationDirection5D().pos-ikparam.GetTranslationDirection5D().pos).lengthsqr3() + anglemult*facos*facos;
1511  }
1512  case IKP_TranslationXY2D: {
1513  return (GetTranslationXY2D()-ikparam.GetTranslationXY2D()).lengthsqr2();
1514  }
1516  Vector v0 = GetTranslationXYOrientation3D();
1517  Vector v1 = ikparam.GetTranslationXYOrientation3D();
1518  dReal anglediff = v0.z-v1.z;
1519  if (anglediff < dReal(-PI)) {
1520  anglediff += dReal(2*PI);
1521  while (anglediff < dReal(-PI))
1522  anglediff += dReal(2*PI);
1523  }
1524  else if (anglediff > dReal(PI)) {
1525  anglediff -= dReal(2*PI);
1526  while (anglediff > dReal(PI))
1527  anglediff -= dReal(2*PI);
1528  }
1529  return (v0-v1).lengthsqr2() + anglemult*anglediff*anglediff;
1530  }
1532  std::pair<Vector,Vector> p0 = GetTranslationLocalGlobal6D(), p1 = ikparam.GetTranslationLocalGlobal6D();
1533  return (p0.first-p1.first).lengthsqr3() + (p0.second-p1.second).lengthsqr3();
1534  }
1536  std::pair<Vector,dReal> p0 = GetTranslationXAxisAngle4D(), p1 = ikparam.GetTranslationXAxisAngle4D();
1537  // dot product with axis is always in [0,pi]
1538  dReal angle0 = RaveFabs(NormalizeCircularAnglePrivate(p0.second, -PI, PI));
1539  dReal angle1 = RaveFabs(NormalizeCircularAnglePrivate(p1.second, -PI, PI));
1540  return (p0.first-p1.first).lengthsqr3() + (angle0-angle1)*(angle0-angle1);
1541  }
1543  std::pair<Vector,dReal> p0 = GetTranslationYAxisAngle4D(), p1 = ikparam.GetTranslationYAxisAngle4D();
1544  // dot product with axis is always in [0,pi]
1545  dReal angle0 = RaveFabs(NormalizeCircularAnglePrivate(p0.second, -PI, PI));
1546  dReal angle1 = RaveFabs(NormalizeCircularAnglePrivate(p1.second, -PI, PI));
1547  return (p0.first-p1.first).lengthsqr3() + (angle0-angle1)*(angle0-angle1);
1548  }
1550  std::pair<Vector,dReal> p0 = GetTranslationZAxisAngle4D(), p1 = ikparam.GetTranslationZAxisAngle4D();
1551  // dot product with axis is always in [0,pi]
1552  dReal angle0 = RaveFabs(NormalizeCircularAnglePrivate(p0.second, -PI, PI));
1553  dReal angle1 = RaveFabs(NormalizeCircularAnglePrivate(p1.second, -PI, PI));
1554  return (p0.first-p1.first).lengthsqr3() + (angle0-angle1)*(angle0-angle1);
1555  }
1557  std::pair<Vector,dReal> p0 = GetTranslationXAxisAngleZNorm4D(), p1 = ikparam.GetTranslationXAxisAngleZNorm4D();
1558  dReal anglediff = NormalizeCircularAnglePrivate(p0.second-p1.second, -PI, PI);
1559  return (p0.first-p1.first).lengthsqr3() + anglediff*anglediff;
1560  }
1562  std::pair<Vector,dReal> p0 = GetTranslationYAxisAngleXNorm4D(), p1 = ikparam.GetTranslationYAxisAngleXNorm4D();
1563  dReal anglediff = NormalizeCircularAnglePrivate(p0.second-p1.second, -PI, PI);
1564  return (p0.first-p1.first).lengthsqr3() + anglediff*anglediff;
1565  }
1567  std::pair<Vector,dReal> p0 = GetTranslationZAxisAngleYNorm4D(), p1 = ikparam.GetTranslationZAxisAngleYNorm4D();
1568  dReal anglediff = NormalizeCircularAnglePrivate(p0.second-p1.second, -PI, PI);
1569  return (p0.first-p1.first).lengthsqr3() + anglediff*anglediff;
1570  }
1571  default:
1572  BOOST_ASSERT(0);
1573  }
1574  return 1e30;
1575  }
1576 
1582  inline void GetValues(std::vector<dReal>::iterator itvalues) const
1583  {
1584  switch(_type & ~IKP_VelocityDataBit) {
1585  case IKP_Transform6D:
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;
1593  break;
1594  case IKP_Rotation3D:
1595  *itvalues++ = _transform.rot.x;
1596  *itvalues++ = _transform.rot.y;
1597  *itvalues++ = _transform.rot.z;
1598  *itvalues++ = _transform.rot.w;
1599  break;
1600  case IKP_Translation3D:
1601  *itvalues++ = _transform.trans.x;
1602  *itvalues++ = _transform.trans.y;
1603  *itvalues++ = _transform.trans.z;
1604  break;
1605  case IKP_Direction3D:
1606  *itvalues++ = _transform.rot.x;
1607  *itvalues++ = _transform.rot.y;
1608  *itvalues++ = _transform.rot.z;
1609  break;
1610  case IKP_Ray4D:
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;
1617  break;
1618  case IKP_Lookat3D:
1619  *itvalues++ = _transform.trans.x;
1620  *itvalues++ = _transform.trans.y;
1621  *itvalues++ = _transform.trans.z;
1622  break;
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;
1630  break;
1631  case IKP_TranslationXY2D:
1632  *itvalues++ = _transform.trans.x;
1633  *itvalues++ = _transform.trans.y;
1634  break;
1636  *itvalues++ = _transform.trans.x;
1637  *itvalues++ = _transform.trans.y;
1638  *itvalues++ = _transform.trans.z;
1639  break;
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;
1647  break;
1654  *itvalues++ = _transform.rot.x;
1655  *itvalues++ = _transform.trans.x;
1656  *itvalues++ = _transform.trans.y;
1657  *itvalues++ = _transform.trans.z;
1658  break;
1659  default:
1660  throw OPENRAVE_EXCEPTION_FORMAT("does not support parameterization 0x%x", _type,ORE_InvalidArguments);
1661  }
1662  }
1663 
1667  inline void SetValues(std::vector<dReal>::const_iterator itvalues, IkParameterizationType iktype)
1668  {
1669  _type = iktype;
1670  switch(_type & ~IKP_VelocityDataBit) {
1671  case IKP_Transform6D:
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++;
1679  break;
1680  case IKP_Rotation3D:
1681  _transform.rot.x = *itvalues++;
1682  _transform.rot.y = *itvalues++;
1683  _transform.rot.z = *itvalues++;
1684  _transform.rot.w = *itvalues++;
1685  break;
1686  case IKP_Translation3D:
1687  _transform.trans.x = *itvalues++;
1688  _transform.trans.y = *itvalues++;
1689  _transform.trans.z = *itvalues++;
1690  break;
1691  case IKP_Direction3D:
1692  _transform.rot.x = *itvalues++;
1693  _transform.rot.y = *itvalues++;
1694  _transform.rot.z = *itvalues++;
1695  break;
1696  case IKP_Ray4D:
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++;
1703  break;
1704  case IKP_Lookat3D:
1705  _transform.trans.x = *itvalues++;
1706  _transform.trans.y = *itvalues++;
1707  _transform.trans.z = *itvalues++;
1708  break;
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++;
1716  break;
1717  case IKP_TranslationXY2D:
1718  _transform.trans.x = *itvalues++;
1719  _transform.trans.y = *itvalues++;
1720  break;
1722  _transform.trans.x = *itvalues++;
1723  _transform.trans.y = *itvalues++;
1724  _transform.trans.z = *itvalues++;
1725  break;
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++;
1733  break;
1740  _transform.rot.x = *itvalues++;
1741  _transform.trans.x = *itvalues++;
1742  _transform.trans.y = *itvalues++;
1743  _transform.trans.z = *itvalues++;
1744  break;
1745  default:
1746  throw OPENRAVE_EXCEPTION_FORMAT("does not support parameterization 0x%x", _type,ORE_InvalidArguments);
1747  }
1748  }
1749 
1750  inline void Set(std::vector<dReal>::const_iterator itvalues, IkParameterizationType iktype) {
1751  SetValues(itvalues,iktype);
1752  }
1753 
1769  inline void SetCustomValues(const std::string& name, const std::vector<dReal>& values)
1770  {
1771  OPENRAVE_ASSERT_OP_FORMAT0( name.size(), >, 0, "name is empty", ORE_InvalidArguments );
1772  OPENRAVE_ASSERT_OP_FORMAT0(std::count_if(name.begin(), name.end(), _IsValidCharInName), ==, (int)name.size(), "name has invalid characters",ORE_InvalidArguments);
1773  _mapCustomData[name] = values;
1774  }
1775 
1777  inline void SetCustomValue(const std::string& name, dReal value)
1778  {
1779  OPENRAVE_ASSERT_OP_FORMAT0( name.size(), >, 0, "name is empty", ORE_InvalidArguments );
1780  OPENRAVE_ASSERT_OP_FORMAT0(std::count_if(name.begin(), name.end(), _IsValidCharInName), ==, (int)name.size(), "name has invalid characters",ORE_InvalidArguments);
1781  _mapCustomData[name].resize(1);
1782  _mapCustomData[name][0] = value;
1783  }
1784 
1786  inline bool GetCustomValues(const std::string& name, std::vector<dReal>& values) const
1787  {
1788  std::map<std::string, std::vector<dReal> >::const_iterator it = _mapCustomData.find(name);
1789  if( it == _mapCustomData.end() ) {
1790  return false;
1791  }
1792  values = it->second;
1793  return true;
1794  }
1795 
1797  inline const std::map<std::string, std::vector<dReal> >& GetCustomDataMap() const
1798  {
1799  return _mapCustomData;
1800  }
1801 
1806  inline size_t ClearCustomValues(const std::string& name=std::string())
1807  {
1808  if( name.size() > 0 ) {
1809  return _mapCustomData.erase(name) > 0;
1810  }
1811  else {
1812  size_t num = _mapCustomData.size();
1813  _mapCustomData.clear();
1814  return num;
1815  }
1816  }
1817 
1818  static ConfigurationSpecification GetConfigurationSpecification(IkParameterizationType iktype, const std::string& interpolation="");
1819  inline ConfigurationSpecification GetConfigurationSpecification(const std::string& interpolation="") const
1820  {
1821  return GetConfigurationSpecification(GetType(),interpolation);
1822  }
1823 
1826  switch(GetType()) {
1827  case IKP_Transform6D:
1828  _transform = t * _transform;
1829  break;
1831  _transform.trans = t.rotate(_transform.trans);
1832  _transform.rot = quatMultiply(t.rot,_transform.rot);
1833  break;
1834  case IKP_Rotation3D:
1836  _transform.rot = quatMultiply(t.rot,_transform.rot);
1837  break;
1838  case IKP_Translation3D:
1839  _transform.trans = t * _transform.trans;
1840  break;
1842  _transform.trans = t.rotate(_transform.trans);
1843  break;
1844  case IKP_Direction3D:
1846  _transform.rot = t.rotate(_transform.rot);
1847  break;
1848  case IKP_Ray4D:
1849  _transform.trans = t * _transform.trans;
1850  _transform.rot = t.rotate(_transform.rot);
1851  break;
1852  case IKP_Ray4DVelocity:
1853  _transform.trans = t.rotate(_transform.trans);
1854  _transform.rot = t.rotate(_transform.rot);
1855  break;
1856  case IKP_Lookat3D:
1857  SetLookat3D(RAY(t*GetLookat3D(),t.rotate(GetLookat3DDirection())));
1858  break;
1860  _transform.trans = t * _transform.trans;
1861  _transform.rot = t.rotate(_transform.rot);
1862  break;
1864  _transform.trans = t.rotate(_transform.trans);
1865  _transform.rot = t.rotate(_transform.rot);
1866  break;
1867  case IKP_TranslationXY2D:
1868  SetTranslationXY2D(t*GetTranslationXY2D());
1869  break;
1871  _transform.trans = t.rotate(_transform.trans);
1872  break;
1874  Vector v = GetTranslationXYOrientation3D();
1875  Vector voldtrans(v.x,v.y,0);
1876  Vector vnewtrans = t*voldtrans;
1877  dReal zangle = -normalizeAxisRotation(Vector(0,0,1),t.rot).first;
1878  SetTranslationXYOrientation3D(Vector(vnewtrans.y,vnewtrans.y,v.z+zangle));
1879  break;
1880  }
1882  Vector v = GetTranslationXYOrientation3D();
1883  Vector voldtrans(v.x,v.y,0);
1884  _transform.trans = t.rotate(voldtrans);
1885  _transform.trans.z = quatRotate(t.rot,Vector(0,0,v.z)).z;
1886  break;
1887  }
1889  _transform.trans = t*_transform.trans;
1890  break;
1892  _transform.trans = t.rotate(_transform.trans);
1893  break;
1895  _transform.trans = t*_transform.trans;
1896  // do not support rotations
1897  break;
1898  }
1900  _transform.trans = t.rotate(_transform.trans);
1901  // do not support rotations
1902  break;
1903  }
1905  _transform.trans = t*_transform.trans;
1906  // do not support rotations
1907  break;
1908  }
1910  _transform.trans = t.rotate(_transform.trans);
1911  // do not support rotations
1912  break;
1913  }
1915  _transform.trans = t*_transform.trans;
1916  // do not support rotations
1917  break;
1918  }
1920  _transform.trans = t.rotate(_transform.trans);
1921  // do not support rotations
1922  break;
1923  }
1924 
1926  _transform.trans = t*_transform.trans;
1927  // only support rotation along z-axis
1928  _transform.rot.x -= normalizeAxisRotation(Vector(0,0,1),t.rot).first;
1929  break;
1930  }
1932  _transform.trans = t.rotate(_transform.trans);
1933  // only support rotation along z-axis
1934  _transform.rot.x = quatRotate(t.rot,Vector(0,0,_transform.rot.x)).z;
1935  break;
1936  }
1938  _transform.trans = t*_transform.trans;
1939  // only support rotation along x-axis
1940  _transform.rot.x -= normalizeAxisRotation(Vector(1,0,0),t.rot).first;
1941  break;
1942  }
1944  _transform.trans = t.rotate(_transform.trans);
1945  // only support rotation along x-axis
1946  _transform.rot.x = quatRotate(t.rot,Vector(_transform.rot.x,0,0)).x;
1947  break;
1948  }
1950  _transform.trans = t*_transform.trans;
1951  // only support rotation along y-axis
1952  _transform.rot.x -= normalizeAxisRotation(Vector(0,1,0),t.rot).first;
1953  break;
1954  }
1956  _transform.trans = t.rotate(_transform.trans);
1957  // only support rotation along y-axis
1958  _transform.rot.x = quatRotate(t.rot,Vector(0,_transform.rot.x,0)).y;
1959  break;
1960  }
1961  default:
1962  throw openrave_exception(str(boost::format("parameterization 0x%x does not support left-transform")%GetType()));
1963  }
1964  for(std::map<std::string, std::vector<dReal> >::iterator it = _mapCustomData.begin(); it != _mapCustomData.end(); ++it) {
1965  _MultiplyTransform(t, it->first, it->second);
1966  }
1967  return *this;
1968  }
1969 
1982  switch(GetType()) {
1983  case IKP_Transform6D:
1984  _transform *= t;
1985  break;
1986 // case IKP_Transform6DVelocity:
1987 // _transform.trans = t.rotate(_transform.trans);
1988 // _transform.rot = quatMultiply(t.rot,_transform.rot);
1989 // break;
1990  case IKP_Rotation3D:
1991 // case IKP_Rotation3DVelocity:
1992  _transform.rot = quatMultiply(_transform.rot,t.rot);
1993  break;
1994  case IKP_Translation3D:
1995  _transform.trans = _transform.trans + t.trans;
1996  break;
1997 // case IKP_Translation3DVelocity:
1998 // _transform.trans = t.rotate(_transform.trans);
1999 // break;
2000  case IKP_Direction3D:
2001 // case IKP_Direction3DVelocity:
2002  _transform.rot = quatRotate(quatMultiply(quatRotateDirection(Vector(0,0,1),_transform.rot), t.rot), Vector(0,0,1));
2003  break;
2004 // case IKP_Ray4D:
2005 // _transform.trans = t * _transform.trans;
2006 // _transform.rot = t.rotate(_transform.rot);
2007 // break;
2008 // case IKP_Ray4DVelocity:
2009 // _transform.trans = t.rotate(_transform.trans);
2010 // _transform.rot = t.rotate(_transform.rot);
2011 // break;
2012  case IKP_Lookat3D:
2013  SetLookat3D(GetLookat3D() + t.trans);
2014  break;
2016  Vector qorig = quatRotateDirection(Vector(0,0,1),_transform.rot);
2017  Vector q = quatMultiply(qorig, t.rot);
2018  _transform.trans += quatRotate(qorig,t.trans);
2019  _transform.rot = quatRotate(q, Vector(0,0,1));
2020  break;
2021  }
2022 // case IKP_TranslationDirection5DVelocity:
2023 // _transform.trans = t.rotate(_transform.trans);
2024 // _transform.rot = t.rotate(_transform.rot);
2025 // break;
2026  case IKP_TranslationXY2D:
2027  SetTranslationXY2D(GetTranslationXY2D() + t.trans);
2028  break;
2029 // case IKP_TranslationXY2DVelocity:
2030 // _transform.trans = t.rotate(_transform.trans);
2031 // break;
2033  Vector v = GetTranslationXYOrientation3D();
2034  Vector voldtrans(v.x,v.y,0);
2035  Vector q = quatFromAxisAngle(Vector(0,0,1),v.z);
2036  Vector vnewtrans = voldtrans + quatRotate(q,t.trans);
2037  dReal zangle = -normalizeAxisRotation(Vector(0,0,1),t.rot).first;
2038  SetTranslationXYOrientation3D(Vector(vnewtrans.y,vnewtrans.y,v.z+zangle));
2039  break;
2040  }
2041 // case IKP_TranslationXYOrientation3DVelocity: {
2042 // Vector v = GetTranslationXYOrientation3D();
2043 // Vector voldtrans(v.x,v.y,0);
2044 // _transform.trans = t.rotate(voldtrans);
2045 // _transform.trans.z = quatRotate(t.rot,Vector(0,0,v.z)).z;
2046 // break;
2047 // }
2049  _transform.trans = _transform.trans + t.trans;
2050  break;
2051 // case IKP_TranslationLocalGlobal6DVelocity:
2052 // _transform.trans = t.rotate(_transform.trans);
2053 // break;
2054 // case IKP_TranslationXAxisAngle4D: {
2055 // _transform.trans = t*_transform.trans;
2056 // // do not support rotations
2057 // break;
2058 // }
2059 // case IKP_TranslationXAxisAngle4DVelocity: {
2060 // _transform.trans = t.rotate(_transform.trans);
2061 // // do not support rotations
2062 // break;
2063 // }
2064 // case IKP_TranslationYAxisAngle4D: {
2065 // _transform.trans = t*_transform.trans;
2066 // // do not support rotations
2067 // break;
2068 // }
2069 // case IKP_TranslationYAxisAngle4DVelocity: {
2070 // _transform.trans = t.rotate(_transform.trans);
2071 // // do not support rotations
2072 // break;
2073 // }
2074 // case IKP_TranslationZAxisAngle4D: {
2075 // _transform.trans = t*_transform.trans;
2076 // // do not support rotations
2077 // break;
2078 // }
2079 // case IKP_TranslationZAxisAngle4DVelocity: {
2080 // _transform.trans = t.rotate(_transform.trans);
2081 // // do not support rotations
2082 // break;
2083 // }
2085  Vector q = quatFromAxisAngle(Vector(0,0,1),_transform.rot.x);
2086  _transform.trans += quatRotate(q,t.trans);
2087  // only support rotation along z-axis
2088  _transform.rot.x -= normalizeAxisRotation(Vector(0,0,1),t.rot).first;
2089  break;
2090  }
2091 // case IKP_TranslationXAxisAngleZNorm4DVelocity: {
2092 // _transform.trans = t.rotate(_transform.trans);
2093 // // only support rotation along z-axis
2094 // _transform.rot.x = quatRotate(t.rot,Vector(0,0,_transform.rot.x)).z;
2095 // break;
2096 // }
2098  Vector q = quatFromAxisAngle(Vector(1,0,0),_transform.rot.x);
2099  _transform.trans += quatRotate(q,t.trans);
2100  // only support rotation along x-axis
2101  _transform.rot.x -= normalizeAxisRotation(Vector(1,0,0),t.rot).first;
2102  break;
2103  }
2104 // case IKP_TranslationYAxisAngleXNorm4DVelocity: {
2105 // _transform.trans = t.rotate(_transform.trans);
2106 // // only support rotation along x-axis
2107 // _transform.rot.x = quatRotate(t.rot,Vector(_transform.rot.x,0,0)).x;
2108 // break;
2109 // }
2111  Vector q = quatFromAxisAngle(Vector(0,1,0),_transform.rot.x);
2112  _transform.trans += quatRotate(q,t.trans);
2113  // only support rotation along y-axis
2114  _transform.rot.x -= normalizeAxisRotation(Vector(0,1,0),t.rot).first;
2115  break;
2116  }
2117 // case IKP_TranslationZAxisAngleYNorm4DVelocity: {
2118 // _transform.trans = t.rotate(_transform.trans);
2119 // // only support rotation along y-axis
2120 // _transform.rot.x = quatRotate(t.rot,Vector(0,_transform.rot.x,0)).y;
2121 // break;
2122 // }
2123  default:
2124  throw openrave_exception(str(boost::format("parameterization 0x%x does not support right-transforms")%GetType()));
2125  }
2126  for(std::map<std::string, std::vector<dReal> >::iterator it = _mapCustomData.begin(); it != _mapCustomData.end(); ++it) {
2127  _MultiplyTransformRight(t, it->first, it->second);
2128  }
2129  return *this;
2130  }
2131 
2132  inline IkParameterization operator*(const Transform& t) const {
2133  IkParameterization iknew(*this);
2134  iknew.MultiplyTransformRight(t);
2135  return iknew;
2136  }
2137 
2138 protected:
2139  inline static bool _IsValidCharInName(char c) {
2140  return c < 0 || c >= 33;
2141  }
2142  inline static void _MultiplyTransform(const Transform& t, const std::string& name, std::vector<dReal>& values)
2143  {
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);
2150  }
2151  else {
2152  transformtype = name.substr(startoffset+11,endoffset-startoffset-11);
2153  }
2154  if( transformtype == "direction" ) {
2155  Vector v(values.at(0),values.at(1), values.at(2));
2156  v = t.rotate(v);
2157  values.at(0) = v[0]; values.at(1) = v[1]; values.at(2) = v[2];
2158  }
2159  else if( transformtype == "point" ) {
2160  Vector v(values.at(0),values.at(1), values.at(2));
2161  v = t*v;
2162  values.at(0) = v[0]; values.at(1) = v[1]; values.at(2) = v[2];
2163  }
2164  else if( transformtype == "quat" ) {
2165  Vector v(values.at(0),values.at(1), values.at(2),values.at(3));
2166  v = quatMultiply(t.rot,v);
2167  values.at(0) = v[0]; values.at(1) = v[1]; values.at(2) = v[2]; values.at(3) = v[3];
2168  }
2169  else if( transformtype == "ikparam" ) {
2170  IkParameterizationType newiktype = RaveGetIkTypeFromUniqueId(static_cast<int>(values.at(0)+0.5));
2171  IkParameterization newikparam;
2172  OPENRAVE_ASSERT_OP_FORMAT0(IkParameterization::GetNumberOfValues(newiktype)+1, ==, (int)values.size(),"expected values not equal",ORE_InvalidState);
2173  newikparam.SetValues(values.begin()+1,newiktype);
2174  newikparam.MultiplyTransform(t);
2175  newikparam.GetValues(values.begin()+1);
2176  }
2177  else {
2178  throw OPENRAVE_EXCEPTION_FORMAT("IkParameterization custom data '%s' does not have a valid transform",name,ORE_InvalidState);
2179  }
2180  }
2181  }
2182 
2183  inline static void _MultiplyTransformRight(const Transform& t, const std::string& name, std::vector<dReal>& values)
2184  {
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);
2191  }
2192  else {
2193  transformtype = name.substr(startoffset+11,endoffset-startoffset-11);
2194  }
2195  if( transformtype == "direction" ) {
2196  Vector v(values.at(0),values.at(1), values.at(2));
2197  v = quatRotate(quatMultiply(quatRotateDirection(Vector(0,0,1),v), t.rot), Vector(0,0,1));
2198  values.at(0) = v[0]; values.at(1) = v[1]; values.at(2) = v[2];
2199  }
2200  else if( transformtype == "point" ) {
2201  Vector v(values.at(0),values.at(1), values.at(2));
2202  v += t.trans;
2203  values.at(0) = v[0]; values.at(1) = v[1]; values.at(2) = v[2];
2204  }
2205  else if( transformtype == "quat" ) {
2206  Vector v(values.at(0),values.at(1), values.at(2),values.at(3));
2207  v = quatMultiply(v,t.rot);
2208  values.at(0) = v[0]; values.at(1) = v[1]; values.at(2) = v[2]; values.at(3) = v[3];
2209  }
2210  else if( transformtype == "ikparam" ) {
2211  IkParameterizationType newiktype = RaveGetIkTypeFromUniqueId(static_cast<int>(values.at(0)+0.5));
2212  IkParameterization newikparam;
2213  OPENRAVE_ASSERT_OP_FORMAT0(IkParameterization::GetNumberOfValues(newiktype)+1, ==, (int)values.size(),"expected values not equal",ORE_InvalidState);
2214  newikparam.SetValues(values.begin()+1,newiktype);
2215  newikparam.MultiplyTransformRight(t);
2216  newikparam.GetValues(values.begin()+1);
2217  }
2218  else {
2219  throw OPENRAVE_EXCEPTION_FORMAT("IkParameterization custom data '%s' does not have a valid transform",name,ORE_InvalidState);
2220  }
2221  }
2222  }
2223 
2226  std::map<std::string, std::vector<dReal> > _mapCustomData;
2227 
2228  friend IkParameterization operator* (const Transform &t, const IkParameterization &ikparam);
2229  friend OPENRAVE_API std::ostream& operator<<(std::ostream& O, const IkParameterization &ikparam);
2230  friend OPENRAVE_API std::istream& operator>>(std::istream& I, IkParameterization& ikparam);
2231 };
2232 
2234 {
2235  IkParameterization local;
2236  switch(ikparam.GetType()) {
2237  case IKP_Transform6D:
2238  local.SetTransform6D(t * ikparam.GetTransform6D());
2239  break;
2240  case IKP_Rotation3D:
2241  local.SetRotation3D(quatMultiply(t.rot,ikparam.GetRotation3D()));
2242  break;
2243  case IKP_Translation3D:
2244  local.SetTranslation3D(t*ikparam.GetTranslation3D());
2245  break;
2246  case IKP_Direction3D:
2247  local.SetDirection3D(t.rotate(ikparam.GetDirection3D()));
2248  break;
2249  case IKP_Ray4D:
2250  local.SetRay4D(RAY(t*ikparam.GetRay4D().pos,t.rotate(ikparam.GetRay4D().dir)));
2251  break;
2252  case IKP_Lookat3D:
2253  local.SetLookat3D(RAY(t*ikparam.GetLookat3D(),t.rotate(ikparam.GetLookat3DDirection())));
2254  break;
2257  break;
2258  case IKP_TranslationXY2D:
2259  local.SetTranslationXY2D(t*ikparam.GetTranslationXY2D());
2260  break;
2262  Vector v = ikparam.GetTranslationXYOrientation3D();
2263  Vector voldtrans(v.x,v.y,0);
2264  Vector vnewtrans = t*voldtrans;
2265  dReal zangle = -normalizeAxisRotation(Vector(0,0,1),t.rot).first;
2266  local.SetTranslationXYOrientation3D(Vector(vnewtrans.y,vnewtrans.y,v.z+zangle));
2267  break;
2268  }
2270  local.SetTranslationLocalGlobal6D(ikparam.GetTranslationLocalGlobal6D().first, t*ikparam.GetTranslationLocalGlobal6D().second);
2271  break;
2273  std::pair<Vector,dReal> p = ikparam.GetTranslationXAxisAngle4D();
2274  // don't change the angle since don't know the exact direction it is pointing at
2275  local.SetTranslationXAxisAngle4D(t*p.first,p.second);
2276  break;
2277  }
2279  std::pair<Vector,dReal> p = ikparam.GetTranslationYAxisAngle4D();
2280  // don't change the angle since don't know the exact direction it is pointing at
2281  local.SetTranslationYAxisAngle4D(t*p.first,p.second);
2282  break;
2283  }
2285  std::pair<Vector,dReal> p = ikparam.GetTranslationZAxisAngle4D();
2286  // don't change the angle since don't know the exact direction it is pointing at
2287  local.SetTranslationZAxisAngle4D(t*p.first,p.second);
2288  break;
2289  }
2291  std::pair<Vector,dReal> p = ikparam.GetTranslationXAxisAngleZNorm4D();
2292  // don't change the angle since don't know the exact direction it is pointing at
2293  local.SetTranslationXAxisAngleZNorm4D(t*p.first,p.second);
2294  break;
2295  }
2297  std::pair<Vector,dReal> p = ikparam.GetTranslationYAxisAngleXNorm4D();
2298  // don't change the angle since don't know the exact direction it is pointing at
2299  local.SetTranslationYAxisAngleXNorm4D(t*p.first,p.second);
2300  break;
2301  }
2303  std::pair<Vector,dReal> p = ikparam.GetTranslationZAxisAngleYNorm4D();
2304  // don't change the angle since don't know the exact direction it is pointing at
2305  local.SetTranslationZAxisAngleYNorm4D(t*p.first,p.second);
2306  break;
2307  }
2308  default:
2309  // internal MultiplyTransform supports more types
2310  return IkParameterization(ikparam).MultiplyTransform(t);
2311  }
2312  local._mapCustomData = ikparam._mapCustomData;
2313  for(std::map<std::string, std::vector<dReal> >::iterator it = local._mapCustomData.begin(); it != local._mapCustomData.end(); ++it) {
2314  IkParameterization::_MultiplyTransform(t, it->first,it->second);
2315  }
2316  return local;
2317 }
2318 
2319 OPENRAVE_API std::ostream& operator<<(std::ostream& O, const IkParameterization &ikparam);
2320 OPENRAVE_API std::istream& operator>>(std::istream& I, IkParameterization& ikparam);
2321 
2324 {
2325 public:
2326  std::vector<Vector> vertices;
2327  std::vector<int> indices;
2328 
2329  void ApplyTransform(const Transform& t);
2330  void ApplyTransform(const TransformMatrix& t);
2331 
2333  void Append(const TriMesh& mesh);
2334  void Append(const TriMesh& mesh, const Transform& trans);
2335 
2336  AABB ComputeAABB() const;
2337  void serialize(std::ostream& o, int options=0) const;
2338 
2339  friend OPENRAVE_API std::ostream& operator<<(std::ostream& O, const TriMesh &trimesh);
2340  friend OPENRAVE_API std::istream& operator>>(std::istream& I, TriMesh& trimesh);
2341 };
2342 
2343 OPENRAVE_API std::ostream& operator<<(std::ostream& O, const TriMesh& trimesh);
2344 OPENRAVE_API std::istream& operator>>(std::istream& I, TriMesh& trimesh);
2345 
2348 {
2350  DOF_X = 1,
2351  DOF_Y = 2,
2352  DOF_Z = 4,
2354 
2355  // DOF_RotationX fields are mutually exclusive
2358 
2362 };
2363 
2370 OPENRAVE_API int RaveGetIndexFromAffineDOF(int affinedofs, DOFAffine dof);
2371 
2378 OPENRAVE_API DOFAffine RaveGetAffineDOFFromIndex(int affinedofs, int index);
2379 
2383 OPENRAVE_API int RaveGetAffineDOF(int affinedofs);
2384 
2392 OPENRAVE_API void RaveGetAffineDOFValuesFromTransform(std::vector<dReal>::iterator itvalues, const Transform& t, int affinedofs, const Vector& axis=Vector(0,0,1));
2393 
2403 OPENRAVE_API void RaveGetAffineDOFValuesFromVelocity(std::vector<dReal>::iterator itvalues, const Vector& linearvel, const Vector& angularvel, const Vector& quatrotation, int affinedofs, const Vector& axis=Vector(0,0,1));
2404 
2414 OPENRAVE_API void RaveGetTransformFromAffineDOFValues(Transform& t, std::vector<dReal>::const_iterator itvalues, int affinedofs, const Vector& axis=Vector(0,0,1), bool normalize=true);
2415 
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));
2427 
2428 OPENRAVE_API ConfigurationSpecification RaveGetAffineConfigurationSpecification(int affinedofs,KinBodyConstPtr pbody=KinBodyConstPtr(),const std::string& interpolation="");
2429 
2430 }
2431 
2432 #include <openrave/plugininfo.h>
2433 #include <openrave/interface.h>
2434 #include <openrave/spacesampler.h>
2435 #include <openrave/kinbody.h>
2436 #include <openrave/trajectory.h>
2437 #include <openrave/module.h>
2438 #include <openrave/collisionchecker.h>
2439 #include <openrave/sensor.h>
2440 #include <openrave/robot.h>
2441 #include <openrave/iksolver.h>
2442 #include <openrave/planner.h>
2443 #include <openrave/controller.h>
2444 #include <openrave/physicsengine.h>
2445 #include <openrave/sensorsystem.h>
2446 #include <openrave/viewer.h>
2447 #include <openrave/environment.h>
2448 
2449 namespace OpenRAVE {
2450 
2453 
2454 
2456 inline const char* RaveGetInterfaceHash(InterfaceType type)
2457 {
2458  switch(type) {
2459  case PT_Planner: return OPENRAVE_PLANNER_HASH;
2460  case PT_Robot: return OPENRAVE_ROBOT_HASH;
2461  case PT_SensorSystem: return OPENRAVE_SENSORSYSTEM_HASH;
2462  case PT_Controller: return OPENRAVE_CONTROLLER_HASH;
2463  case PT_Module: return OPENRAVE_MODULE_HASH;
2464  case PT_InverseKinematicsSolver: return OPENRAVE_IKSOLVER_HASH;
2465  case PT_KinBody: return OPENRAVE_KINBODY_HASH;
2466  case PT_PhysicsEngine: return OPENRAVE_PHYSICSENGINE_HASH;
2467  case PT_Sensor: return OPENRAVE_SENSOR_HASH;
2468  case PT_CollisionChecker: return OPENRAVE_COLLISIONCHECKER_HASH;
2469  case PT_Trajectory: return OPENRAVE_TRAJECTORY_HASH;
2470  case PT_Viewer: return OPENRAVE_VIEWER_HASH;
2471  case PT_SpaceSampler: return OPENRAVE_SPACESAMPLER_HASH;
2472  default:
2473  throw openrave_exception("failed to find openrave interface type",ORE_InvalidArguments);
2474  return NULL;
2475  }
2476 }
2477 
2481 template <typename T>
2482 inline boost::shared_ptr<T> RaveInterfaceCast(InterfaceBasePtr pinterface)
2483 {
2484  if( !!pinterface ) {
2485  if( pinterface->GetInterfaceType() == T::GetInterfaceTypeStatic() ) {
2486  return boost::static_pointer_cast<T>(pinterface);
2487  }
2488  // encode special cases
2489  if((pinterface->GetInterfaceType() == PT_Robot)&&(T::GetInterfaceTypeStatic() == PT_KinBody)) {
2490  return boost::static_pointer_cast<T>(pinterface);
2491  }
2492  }
2493  return boost::shared_ptr<T>();
2494 }
2495 
2499 template <typename T>
2500 inline boost::shared_ptr<T const> RaveInterfaceConstCast(InterfaceBaseConstPtr pinterface)
2501 {
2502  if( !!pinterface ) {
2503  if( pinterface->GetInterfaceType() == T::GetInterfaceTypeStatic() ) {
2504  return boost::static_pointer_cast<T const>(pinterface);
2505  }
2506  // encode special cases
2507  if((pinterface->GetInterfaceType() == PT_Robot)&&(T::GetInterfaceTypeStatic() == PT_KinBody)) {
2508  return boost::static_pointer_cast<T const>(pinterface);
2509  }
2510  }
2511  return boost::shared_ptr<T>();
2512 }
2513 
2515 OPENRAVE_API const std::map<InterfaceType,std::string>& RaveGetInterfaceNamesMap();
2516 OPENRAVE_API const std::string& RaveGetInterfaceName(InterfaceType type);
2517 
2521 OPENRAVE_API std::string RaveGetHomeDirectory();
2522 
2528 OPENRAVE_API std::string RaveFindDatabaseFile(const std::string& filename, bool bRead=true);
2529 
2538 OPENRAVE_API int RaveInitialize(bool bLoadAllPlugins=true, int level = Level_Info);
2539 
2547 
2551 
2556 OPENRAVE_API void RaveDestroy();
2557 
2564 OPENRAVE_API void RaveAddCallbackForDestroy(const boost::function<void()>& fn);
2565 
2569 OPENRAVE_API void RaveGetPluginInfo(std::list< std::pair<std::string, PLUGININFO> >& plugins);
2570 
2572 OPENRAVE_API void RaveGetLoadedInterfaces(std::map<InterfaceType, std::vector<std::string> >& interfacenames);
2573 
2578 
2583 OPENRAVE_API bool RaveLoadPlugin(const std::string& libraryname);
2584 
2586 OPENRAVE_API bool RaveHasInterface(InterfaceType type, const std::string& interfacename);
2587 
2588 OPENRAVE_API InterfaceBasePtr RaveCreateInterface(EnvironmentBasePtr penv, InterfaceType type,const std::string& interfacename);
2589 OPENRAVE_API RobotBasePtr RaveCreateRobot(EnvironmentBasePtr penv, const std::string& name="");
2590 OPENRAVE_API PlannerBasePtr RaveCreatePlanner(EnvironmentBasePtr penv, const std::string& name);
2593 OPENRAVE_API ModuleBasePtr RaveCreateModule(EnvironmentBasePtr penv, const std::string& name);
2594 OPENRAVE_API ModuleBasePtr RaveCreateProblem(EnvironmentBasePtr penv, const std::string& name);
2596 OPENRAVE_API IkSolverBasePtr RaveCreateIkSolver(EnvironmentBasePtr penv, const std::string& name);
2598 OPENRAVE_API SensorBasePtr RaveCreateSensor(EnvironmentBasePtr penv, const std::string& name);
2600 OPENRAVE_API ViewerBasePtr RaveCreateViewer(EnvironmentBasePtr penv, const std::string& name);
2602 OPENRAVE_API KinBodyPtr RaveCreateKinBody(EnvironmentBasePtr penv, const std::string& name="");
2604 OPENRAVE_API TrajectoryBasePtr RaveCreateTrajectory(EnvironmentBasePtr penv, const std::string& name="");
2605 
2608 
2610 template <typename T>
2611 inline boost::shared_ptr<T> RaveClone(boost::shared_ptr<T const> preference, int cloningoptions)
2612 {
2613  InterfaceBasePtr pcloned = RaveCreateInterface(preference->GetEnv(), preference->GetInterfaceType(), preference->GetXMLId());
2614  OPENRAVE_ASSERT_FORMAT(!!pcloned, "Failed to clone interface=%s id=%s", RaveGetInterfaceName(preference->GetInterfaceType())%preference->GetXMLId(), ORE_InvalidArguments);
2615  boost::shared_ptr<T> pclonedcast = boost::dynamic_pointer_cast<T>(pcloned);
2616  OPENRAVE_ASSERT_FORMAT(!!pclonedcast, "Interface created but failed to cast interface=%s id=%s", RaveGetInterfaceName(preference->GetInterfaceType())%preference->GetXMLId(), ORE_InvalidArguments);
2617  pclonedcast->Clone(preference,cloningoptions);
2618  return pclonedcast;
2619 }
2620 
2631 OPENRAVE_API UserDataPtr RaveRegisterInterface(InterfaceType type, const std::string& name, const char* interfacehash, const char* envhash, const boost::function<InterfaceBasePtr(EnvironmentBasePtr, std::istream&)>& createfn);
2632 
2641 OPENRAVE_API UserDataPtr RaveRegisterXMLReader(InterfaceType type, const std::string& xmltag, const CreateXMLReaderFn& fn);
2642 
2645 
2649 
2651 OPENRAVE_API void RaveGetEnvironments(std::list<EnvironmentBasePtr>& listenvironments);
2652 
2656 OPENRAVE_API BaseXMLReaderPtr RaveCallXMLReader(InterfaceType type, const std::string& xmltag, InterfaceBasePtr pinterface, const AttributesList& atts);
2657 
2666 OPENRAVE_API std::string RaveFindLocalFile(const std::string& filename, const std::string& curdir="");
2667 
2672 OPENRAVE_API void RaveSetDataAccess(int accessoptions);
2673 
2678 
2680 
2682 OPENRAVE_API void RaveInitRandomGeneration(uint32_t seed);
2684 OPENRAVE_API uint32_t RaveRandomInt();
2689 
2691 bool RaveParseDirectories(const char* pdirs, std::vector<std::string>& vdirs) RAVE_DEPRECATED;
2692 
2693 inline bool RaveParseDirectories(const char* pdirs, std::vector<std::string>& vdirs)
2694 {
2695  vdirs.resize(0);
2696  if( !pdirs ) {
2697  return false;
2698  }
2699  // search for all directories separated by ':'
2700  std::string tmp = pdirs;
2701  std::string::size_type pos = 0, newpos=0;
2702  while( pos < tmp.size() ) {
2703 #ifdef _WIN32
2704  newpos = tmp.find(';', pos);
2705 #else
2706  newpos = tmp.find(':', pos);
2707 #endif
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 ) {
2711  break;
2712  }
2713  pos = newpos+1;
2714  }
2715  return true;
2716 }
2717 
2720 typedef InterfaceBasePtr (*PluginExportFn_OpenRAVECreateInterface)(InterfaceType type, const std::string& name, const char* pluginhash, const char* envhash, EnvironmentBasePtr penv);
2721 
2724 typedef bool (*PluginExportFn_OpenRAVEGetPluginAttributes)(PLUGININFO* pinfo, int size, const char* infohash);
2725 
2729 
2731 typedef InterfaceBasePtr (*PluginExportFn_CreateInterface)(InterfaceType type, const std::string& name, const char* pluginhash, EnvironmentBasePtr penv) RAVE_DEPRECATED;
2732 
2735 
2736 // define inline functions
2737 const std::string& IkParameterization::GetName() const
2738 {
2739  std::map<IkParameterizationType,std::string>::const_iterator it = RaveGetIkParameterizationMap().find(_type);
2740  if( it != RaveGetIkParameterizationMap().end() ) {
2741  return it->second;
2742  }
2743  throw openrave_exception(str(boost::format("IkParameterization iktype 0x%x not supported")));
2744 }
2745 
2746 } // end namespace OpenRAVE
2747 
2748 #if !defined(OPENRAVE_DISABLE_ASSERT_HANDLER) && defined(BOOST_ENABLE_ASSERT_HANDLER)
2749 
2750 namespace boost
2751 {
2752 inline void assertion_failed(char const * expr, char const * function, char const * file, long line)
2753 {
2754  throw OpenRAVE::openrave_exception(boost::str(boost::format("[%s:%d] -> %s, expr: %s")%file%line%function%expr),OpenRAVE::ORE_Assert);
2755 }
2756 
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)
2759 {
2760  throw OpenRAVE::openrave_exception(boost::str(boost::format("[%s:%d] -> %s, expr: %s, msg: %s")%file%line%function%expr%msg),OpenRAVE::ORE_Assert);
2761 }
2762 #endif
2763 
2764 }
2765 #endif
2766 
2770 
2771 // register for typeof (MSVC only)
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)
2776 
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)
2796 BOOST_TYPEOF_REGISTER_TEMPLATE(OpenRAVE::RaveTransform, 1)
2797 BOOST_TYPEOF_REGISTER_TEMPLATE(OpenRAVE::RaveTransformMatrix, 1)
2798 BOOST_TYPEOF_REGISTER_TEMPLATE(OpenRAVE::OrientedBox, 1)
2799 
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)
2809 
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)
2815 
2816 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::TrajectoryBase)
2817 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::TrajectoryBase::TPOINT)
2818 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::TrajectoryBase::TSEGMENT)
2819 
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)
2825 
2826 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::EnvironmentBase)
2827 BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::EnvironmentBase::BODYSTATE)
2828 
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)
2833 #endif
2834 
2835 
2836 #endif