/
openrave.h
2951 lines (2641 loc) · 139 KB
/
openrave.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
// -*- coding: utf-8 -*-
// Copyright (C) 2006-2012 Rosen Diankov <rosen.diankov@gmail.com>
//
// This file is part of OpenRAVE.
// OpenRAVE is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
/** \file openrave.h
\brief Defines the public headers that every plugin must include in order to use openrave properly.
*/
#ifndef OPENRAVE_H
#define OPENRAVE_H
#ifndef OPENRAVE_DISABLE_ASSERT_HANDLER
#define BOOST_ENABLE_ASSERT_HANDLER
#endif
#include <cstdio>
#include <stdarg.h>
#include <cstring>
#include <cstdlib>
#include <stdint.h>
#ifdef _MSC_VER
#pragma warning(disable:4251) // needs to have dll-interface to be used by clients of class
#pragma warning(disable:4190) // C-linkage specified, but returns UDT 'boost::shared_ptr<T>' which is incompatible with C
#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
// needed to get typeof working
//#include <boost/typeof/std/string.hpp>
//#include <boost/typeof/std/vector.hpp>
//#include <boost/typeof/std/list.hpp>
//#include <boost/typeof/std/map.hpp>
//#include <boost/typeof/std/set.hpp>
//#include <boost/typeof/std/string.hpp>
#ifndef __PRETTY_FUNCTION__
#define __PRETTY_FUNCTION__ __FUNCDNAME__
#endif
#else
#endif
#include <string>
#include <vector>
#include <list>
#include <map>
#include <set>
#include <string>
#include <exception>
#include <iomanip>
#include <fstream>
#include <sstream>
#include <boost/version.hpp>
#include <boost/function.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/weak_ptr.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/enable_shared_from_this.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/thread/shared_mutex.hpp>
#include <boost/thread/thread.hpp>
#include <boost/thread/recursive_mutex.hpp>
#include <boost/static_assert.hpp>
#include <boost/format.hpp>
#include <boost/array.hpp>
#include <boost/multi_array.hpp>
//#include <boost/cstdint.hpp>
#if defined(__GNUC__)
#define RAVE_DEPRECATED __attribute__((deprecated))
#else
#define RAVE_DEPRECATED
#endif
/// The entire %OpenRAVE library
namespace OpenRAVE {
#include <openrave/config.h>
#include <openrave/interfacehashes.h>
#if OPENRAVE_PRECISION // 1 if double precision
typedef double dReal;
#define g_fEpsilon 1e-15
#else
typedef float dReal;
#define g_fEpsilon 2e-7f
#endif
/// \brief openrave constant for PI, could be replaced by accurate precision number depending on choice of dReal.
static const dReal PI = dReal(3.14159265358979323846);
/// Wrappers of common basic math functions, allows OpenRAVE to control the precision requirements.
/// \ingroup affine_math
//@{
/// \brief exponential
OPENRAVE_API dReal RaveExp(dReal f);
/// \brief logarithm
OPENRAVE_API dReal RaveLog(dReal f);
/// \brief cosine
OPENRAVE_API dReal RaveCos(dReal f);
/// \brief sine
OPENRAVE_API dReal RaveSin(dReal f);
/// \brief tangent
OPENRAVE_API dReal RaveTan(dReal f);
/// \brief base 2 logarithm
OPENRAVE_API dReal RaveLog2(dReal f);
/// \brief base 10 logarithm
OPENRAVE_API dReal RaveLog10(dReal f);
/// \brief arccosine
OPENRAVE_API dReal RaveAcos(dReal f);
/// \brief arcsine
OPENRAVE_API dReal RaveAsin(dReal f);
/// \brief arctangent2 covering entire circle
OPENRAVE_API dReal RaveAtan2(dReal fy, dReal fx);
/// \brief power x^y
OPENRAVE_API dReal RavePow(dReal fx, dReal fy);
/// \brief square-root
OPENRAVE_API dReal RaveSqrt(dReal f);
/// \brief absolute value
OPENRAVE_API dReal RaveFabs(dReal f);
/// \brief ceil
OPENRAVE_API dReal RaveCeil(dReal f);
//@}
/// %OpenRAVE error codes
enum OpenRAVEErrorCode {
ORE_Failed=0,
ORE_InvalidArguments=1, ///< passed in input arguments are not valid
ORE_EnvironmentNotLocked=2,
ORE_CommandNotSupported=3, ///< string command could not be parsed or is not supported
ORE_Assert=4,
ORE_InvalidPlugin=5, ///< shared object is not a valid plugin
ORE_InvalidInterfaceHash=6, ///< interface hashes do not match between plugins
ORE_NotImplemented=7, ///< function is not implemented by the interface.
ORE_InconsistentConstraints=8, ///< returned solutions or trajectories do not follow the constraints of the planner/module. The constraints invalidated here are planning constraints, not programming constraints.
ORE_NotInitialized=9, ///< when object is used without it getting fully initialized
ORE_InvalidState=10, ///< the state of the object is not consistent with its parameters, or cannot be used. This is usually due to a programming error where a vector is not the correct length, etc.
ORE_Timeout=11, ///< process timed out
};
inline const char* GetErrorCodeString(OpenRAVEErrorCode error)
{
switch(error) {
case ORE_Failed: return "Failed";
case ORE_InvalidArguments: return "InvalidArguments";
case ORE_EnvironmentNotLocked: return "EnvironmentNotLocked";
case ORE_CommandNotSupported: return "CommandNotSupported";
case ORE_Assert: return "Assert";
case ORE_InvalidPlugin: return "InvalidPlugin";
case ORE_InvalidInterfaceHash: return "InvalidInterfaceHash";
case ORE_NotImplemented: return "NotImplemented";
case ORE_InconsistentConstraints: return "InconsistentConstraints";
case ORE_NotInitialized: return "NotInitialized";
case ORE_InvalidState: return "InvalidState";
case ORE_Timeout: return "Timeout";
}
// should throw an exception?
return "";
}
/// \brief Exception that all OpenRAVE internal methods throw; the error codes are held in \ref OpenRAVEErrorCode.
class OPENRAVE_API openrave_exception : public std::exception
{
public:
openrave_exception() : std::exception(), _s("unknown exception"), _error(ORE_Failed) {
}
openrave_exception(const std::string& s, OpenRAVEErrorCode error=ORE_Failed) : std::exception() {
_error = error;
_s = "openrave (";
_s += GetErrorCodeString(_error);
_s += "): ";
_s += s;
}
virtual ~openrave_exception() throw() {
}
char const* what() const throw() {
return _s.c_str();
}
const std::string& message() const {
return _s;
}
OpenRAVEErrorCode GetCode() const {
return _error;
}
private:
std::string _s;
OpenRAVEErrorCode _error;
};
class OPENRAVE_LOCAL CaseInsensitiveCompare
{
public:
bool operator() (const std::string & s1, const std::string& s2) const
{
std::string::const_iterator it1=s1.begin();
std::string::const_iterator it2=s2.begin();
//has the end of at least one of the strings been reached?
while ( (it1!=s1.end()) && (it2!=s2.end()) ) {
if(::toupper(*it1) != ::toupper(*it2)) { //letters differ?
// return -1 to indicate 'smaller than', 1 otherwise
return ::toupper(*it1) < ::toupper(*it2);
}
//proceed to the next character in each string
++it1;
++it2;
}
std::size_t size1=s1.size(), size2=s2.size(); // cache lengths
//return -1,0 or 1 according to strings' lengths
if (size1==size2) {
return 0;
}
return size1<size2;
}
};
/// \brief base class for all user data
class OPENRAVE_API UserData
{
public:
virtual ~UserData() {
}
};
typedef boost::shared_ptr<UserData> UserDataPtr;
typedef boost::weak_ptr<UserData> UserDataWeakPtr;
/// \brief user data that can serialize/deserialize itself
class OPENRAVE_API SerializableData : public UserData
{
public:
virtual ~SerializableData() {
}
/// \brief output the data of the object
virtual void Serialize(std::ostream& O, int options=0) const = 0;
/// \brief initialize the object
virtual void Deserialize(std::istream& I) = 0;
};
typedef boost::shared_ptr<SerializableData> SerializableDataPtr;
typedef boost::weak_ptr<SerializableData> SerializableDataWeakPtr;
// terminal attributes
//#define RESET 0
//#define BRIGHT 1
//#define DIM 2
//#define UNDERLINE 3
//#define BLINK 4
//#define REVERSE 7
//#define HIDDEN 8
// terminal colors
//#define BLACK 0
//#define RED 1
//#define GREEN 2
//#define YELLOW 3
//#define BLUE 4
//#define MAGENTA 5
//#define CYAN 6
//#define WHITE 7
/// Change the text color (on either stdout or stderr) with an attr:fg:bg (thanks to Radu Rusu for the code)
inline std::string ChangeTextColor (int attribute, int fg, int bg)
{
char command[13];
sprintf (command, "%c[%d;%d;%dm", 0x1B, attribute, fg + 30, bg + 40);
return command;
}
/// Change the text color (on either stdout or stderr) with an attr:fg (thanks to Radu Rusu for the code)
inline std::string ChangeTextColor (int attribute, int fg)
{
char command[13];
sprintf (command, "%c[%d;%dm", 0x1B, attribute, fg + 30);
return command;
}
/// Reset the text color (on either stdout or stderr) to its original state (thanks to Radu Rusu for the code)
inline std::string ResetTextColor()
{
char command[12];
sprintf (command, "%c[0;38;48m", 0x1B);
return command;
}
inline std::wstring ChangeTextColorW (int attribute, int fg)
{
wchar_t command[13];
swprintf (command, 13, L"%c[%d;%dm", 0x1B, attribute, fg + 30);
return command;
}
inline std::wstring RavePrintTransformString(const wchar_t* fmt)
{
std::vector<int> positions;
std::wstring str = fmt;
wchar_t* p = wcsstr(&str[0], L"%s");
while(p != NULL ) {
positions.push_back((int)(p-&str[0])+1);
p = wcsstr(p+2, L"%s");
}
p = wcsstr(&str[0], L"%S");
while(p != NULL ) {
p[1] = 's';
p = wcsstr(p+2, L"%S");
}
p = wcsstr(&str[0], L"%ls");
while(p != NULL ) {
p[1] = 's';
p[2] = ' ';
p = wcsstr(p+2, L"%ls");
}
for(int i = 0; i < (int)positions.size(); ++i)
str[positions[i]] = 'S';
return str;
}
enum DebugLevel {
Level_Fatal=0,
Level_Error=1,
Level_Warn=2,
Level_Info=3,
Level_Debug=4,
Level_Verbose=5,
Level_OutputMask=0xf,
Level_VerifyPlans=0x80000000, ///< if set, should verify every plan returned. the verification is left up to the planners or the modules calling the planners. See \ref planningutils::ValidateTrajectory
};
#define OPENRAVECOLOR_FATALLEVEL 5 // magenta
#define OPENRAVECOLOR_ERRORLEVEL 1 // red
#define OPENRAVECOLOR_WARNLEVEL 3 // yellow
#define OPENRAVECOLOR_INFOLEVEL 0 // black
#define OPENRAVECOLOR_DEBUGLEVEL 2 // green
#define OPENRAVECOLOR_VERBOSELEVEL 4 // blue
/// \brief Sets the global openrave debug level. A combination of \ref DebugLevel
OPENRAVE_API void RaveSetDebugLevel(int level);
/// Returns the openrave debug level
OPENRAVE_API int RaveGetDebugLevel();
/// extracts only the filename
inline const char* RaveGetSourceFilename(const char* pfilename)
{
if( pfilename == NULL ) {
return "";
}
const char* p0 = strrchr(pfilename,'/');
const char* p1 = strrchr(pfilename,'\\');
const char* p = p0 > p1 ? p0 : p1;
if( p == NULL ) {
return pfilename;
}
return p+1;
}
#ifdef _WIN32
#define DefineRavePrintfW(LEVEL) \
inline int RavePrintfW ## LEVEL(const wchar_t *fmt, ...) \
{ \
/*ChangeTextColor (stdout, 0, OPENRAVECOLOR##LEVEL);*/ \
va_list list; \
va_start(list,fmt); \
int r = vwprintf(OpenRAVE::RavePrintTransformString(fmt).c_str(), list); \
va_end(list); \
/*ResetTextColor (stdout);*/ \
return r; \
}
#define DefineRavePrintfA(LEVEL) \
inline int RavePrintfA ## LEVEL(const std::string& s) \
{ \
if((s.size() == 0)||(s[s.size()-1] != '\n')) { \
printf("%s\n", s.c_str()); \
} \
else { \
printf ("%s", s.c_str()); \
} \
return s.size(); \
} \
\
inline int RavePrintfA ## LEVEL(const char *fmt, ...) \
{ \
/*ChangeTextColor (stdout, 0, OPENRAVECOLOR##LEVEL);*/ \
va_list list; \
va_start(list,fmt); \
int r = vprintf(fmt, list); \
va_end(list); \
/*if( fmt[0] != '\n' ) { printf("\n"); }*/ \
/*ResetTextColor(stdout);*/ \
return r; \
}
inline int RavePrintfA(const std::string& s, uint32_t level)
{
if((s.size() == 0)||(s[s.size()-1] != '\n')) { // automatically add a new line
printf("%s\n", s.c_str());
}
else {
printf ("%s", s.c_str());
}
return s.size();
}
DefineRavePrintfW(_INFOLEVEL)
DefineRavePrintfA(_INFOLEVEL)
#else
#define DefineRavePrintfW(LEVEL) \
inline int RavePrintfW ## LEVEL(const wchar_t *wfmt, ...) \
{ \
va_list list; \
va_start(list,wfmt); \
/* Allocate memory on the stack to avoid heap fragmentation */ \
size_t allocsize = wcstombs(NULL, wfmt, 0)+32; \
char* fmt = (char*)alloca(allocsize); \
strcpy(fmt, ChangeTextColor(0, OPENRAVECOLOR ## LEVEL,8).c_str()); \
snprintf(fmt+strlen(fmt),allocsize-16,"%S",wfmt); \
strcat(fmt, ResetTextColor().c_str()); \
int r = vprintf(fmt, list); \
va_end(list); \
return r; \
}
// In linux, only wprintf will succeed, due to the fwide() call in main, so
// for programmers who want to use regular format strings without
// the L in front, we will take their regular string and widen it
// for them.
inline int RavePrintfA_INFOLEVEL(const std::string& s)
{
if((s.size() == 0)||(s[s.size()-1] != '\n')) { // automatically add a new line
printf("%s\n", s.c_str());
}
else {
printf ("%s", s.c_str());
}
return s.size();
}
inline int RavePrintfA_INFOLEVEL(const char *fmt, ...)
{
va_list list;
va_start(list,fmt);
int r = vprintf(fmt, list);
va_end(list);
//if( fmt[0] != '\n' ) { printf("\n"); }
return r;
}
#define DefineRavePrintfA(LEVEL) \
inline int RavePrintfA ## LEVEL(const std::string& s) \
{ \
if((s.size() == 0)||(s[s.size()-1] != '\n')) { \
printf ("%c[0;%d;%dm%s%c[m\n", 0x1B, OPENRAVECOLOR ## LEVEL + 30,8+40,s.c_str(),0x1B); \
} \
else { \
printf ("%c[0;%d;%dm%s%c[m", 0x1B, OPENRAVECOLOR ## LEVEL + 30,8+40,s.c_str(),0x1B); \
} \
return s.size(); \
} \
\
inline int RavePrintfA ## LEVEL(const char *fmt, ...) \
{ \
va_list list; \
va_start(list,fmt); \
int r = vprintf((ChangeTextColor(0, OPENRAVECOLOR ## LEVEL,8) + std::string(fmt) + ResetTextColor()).c_str(), list); \
va_end(list); \
/*if( fmt[0] != '\n' ) { printf("\n"); } */ \
return r; \
} \
inline int RavePrintfA(const std::string& s, uint32_t level)
{
if( (OpenRAVE::RaveGetDebugLevel()&OpenRAVE::Level_OutputMask)>=level ) {
int color = 0;
switch(level) {
case Level_Fatal: color = OPENRAVECOLOR_FATALLEVEL; break;
case Level_Error: color = OPENRAVECOLOR_ERRORLEVEL; break;
case Level_Warn: color = OPENRAVECOLOR_WARNLEVEL; break;
case Level_Info: // print regular
if((s.size() == 0)||(s[s.size()-1] != '\n')) { // automatically add a new line
printf ("%s\n",s.c_str());
}
else {
printf ("%s",s.c_str());
}
return s.size();
case Level_Debug: color = OPENRAVECOLOR_DEBUGLEVEL; break;
case Level_Verbose: color = OPENRAVECOLOR_VERBOSELEVEL; break;
}
if((s.size() == 0)||(s[s.size()-1] != '\n')) { // automatically add a new line
printf ("%c[0;%d;%dm%s%c[0;38;48m\n", 0x1B, color + 30,8+40,s.c_str(),0x1B);
}
else {
printf ("%c[0;%d;%dm%s%c[0;38;48m", 0x1B, color + 30,8+40,s.c_str(),0x1B);
}
return s.size();
}
return 0;
}
#endif
DefineRavePrintfW(_FATALLEVEL)
DefineRavePrintfW(_ERRORLEVEL)
DefineRavePrintfW(_WARNLEVEL)
//DefineRavePrintfW(_INFOLEVEL)
DefineRavePrintfW(_DEBUGLEVEL)
DefineRavePrintfW(_VERBOSELEVEL)
DefineRavePrintfA(_FATALLEVEL)
DefineRavePrintfA(_ERRORLEVEL)
DefineRavePrintfA(_WARNLEVEL)
//DefineRavePrintfA(_INFOLEVEL)
DefineRavePrintfA(_DEBUGLEVEL)
DefineRavePrintfA(_VERBOSELEVEL)
#define RAVEPRINTHEADER(LEVEL) OpenRAVE::RavePrintfA ## LEVEL("[%s:%d %s] ", OpenRAVE::RaveGetSourceFilename(__FILE__), __LINE__, __FUNCTION__)
// different logging levels. The higher the suffix number, the less important the information is.
// 0 log level logs all the time. OpenRAVE starts up with a log level of 0.
#define RAVELOG_LEVELW(LEVEL,level) int(OpenRAVE::RaveGetDebugLevel()&OpenRAVE::Level_OutputMask)>=int(level)&&(RAVEPRINTHEADER(LEVEL)>0)&&OpenRAVE::RavePrintfW ## LEVEL
#define RAVELOG_LEVELA(LEVEL,level) int(OpenRAVE::RaveGetDebugLevel()&OpenRAVE::Level_OutputMask)>=int(level)&&(RAVEPRINTHEADER(LEVEL)>0)&&OpenRAVE::RavePrintfA ## LEVEL
// define log4cxx equivalents (eventually OpenRAVE will move to log4cxx logging)
#define RAVELOG_FATALW RAVELOG_LEVELW(_FATALLEVEL,OpenRAVE::Level_Fatal)
#define RAVELOG_FATALA RAVELOG_LEVELA(_FATALLEVEL,OpenRAVE::Level_Fatal)
#define RAVELOG_FATAL RAVELOG_FATALA
#define RAVELOG_ERRORW RAVELOG_LEVELW(_ERRORLEVEL,OpenRAVE::Level_Error)
#define RAVELOG_ERRORA RAVELOG_LEVELA(_ERRORLEVEL,OpenRAVE::Level_Error)
#define RAVELOG_ERROR RAVELOG_ERRORA
#define RAVELOG_WARNW RAVELOG_LEVELW(_WARNLEVEL,OpenRAVE::Level_Warn)
#define RAVELOG_WARNA RAVELOG_LEVELA(_WARNLEVEL,OpenRAVE::Level_Warn)
#define RAVELOG_WARN RAVELOG_WARNA
#define RAVELOG_INFOW RAVELOG_LEVELW(_INFOLEVEL,OpenRAVE::Level_Info)
#define RAVELOG_INFOA RAVELOG_LEVELA(_INFOLEVEL,OpenRAVE::Level_Info)
#define RAVELOG_INFO RAVELOG_INFOA
#define RAVELOG_DEBUGW RAVELOG_LEVELW(_DEBUGLEVEL,OpenRAVE::Level_Debug)
#define RAVELOG_DEBUGA RAVELOG_LEVELA(_DEBUGLEVEL,OpenRAVE::Level_Debug)
#define RAVELOG_DEBUG RAVELOG_DEBUGA
#define RAVELOG_VERBOSEW RAVELOG_LEVELW(_VERBOSELEVEL,OpenRAVE::Level_Verbose)
#define RAVELOG_VERBOSEA RAVELOG_LEVELA(_VERBOSELEVEL,OpenRAVE::Level_Verbose)
#define RAVELOG_VERBOSE RAVELOG_VERBOSEA
#define RAVELOG_FATAL_FORMAT(x, params) RAVELOG_FATAL(boost::str(boost::format(x)%params))
#define RAVELOG_ERROR_FORMAT(x, params) RAVELOG_ERROR(boost::str(boost::format(x)%params))
#define RAVELOG_WARN_FORMAT(x, params) RAVELOG_WARN(boost::str(boost::format(x)%params))
#define RAVELOG_INFO_FORMAT(x, params) RAVELOG_INFO(boost::str(boost::format(x)%params))
#define RAVELOG_DEBUG_FORMAT(x, params) RAVELOG_DEBUG(boost::str(boost::format(x)%params))
#define RAVELOG_VERBOSE_FORMAT(x, params) RAVELOG_VERBOSE(boost::str(boost::format(x)%params))
#define IS_DEBUGLEVEL(level) ((OpenRAVE::RaveGetDebugLevel()&OpenRAVE::Level_OutputMask)>=(level))
#define OPENRAVE_EXCEPTION_FORMAT0(s, errorcode) OpenRAVE::openrave_exception(boost::str(boost::format("[%s:%d] %s")%(__PRETTY_FUNCTION__)%(__LINE__)%(s)),errorcode)
/// adds the function name and line number to an openrave exception
#define OPENRAVE_EXCEPTION_FORMAT(s, args, errorcode) OpenRAVE::openrave_exception(boost::str(boost::format("[%s:%d] ")%(__PRETTY_FUNCTION__)%(__LINE__)) + boost::str(boost::format(s)%args),errorcode)
#define OPENRAVE_ASSERT_FORMAT(testexpr, s, args, errorcode) { if( !(testexpr) ) { throw OpenRAVE::openrave_exception(boost::str(boost::format("[%s:%d] (%s) failed ")%(__PRETTY_FUNCTION__)%(__LINE__)%(# testexpr)) + boost::str(boost::format(s)%args),errorcode); } }
#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)%(s)),errorcode); } }
// note that expr1 and expr2 will be evaluated twice if not equal
#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) ")%(__PRETTY_FUNCTION__)%(__LINE__)%(# expr1)%(# op)%(# expr2)%(expr1)%(# op)%(expr2)) + boost::str(boost::format(s)%args),errorcode); } }
#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)%(s)),errorcode); } }
#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); } }
#define OPENRAVE_DUMMY_IMPLEMENTATION { throw OPENRAVE_EXCEPTION_FORMAT0("not implemented",ORE_NotImplemented); }
/// \brief Enumeration of all the interfaces.
enum InterfaceType
{
PT_Planner=1, ///< describes \ref PlannerBase interface
PT_Robot=2, ///< describes \ref RobotBase interface
PT_SensorSystem=3, ///< describes \ref SensorSystemBase interface
PT_Controller=4, ///< describes \ref ControllerBase interface
PT_Module=5, ///< describes \ref ModuleBase interface
PT_ProblemInstance=5, ///< describes \ref ModuleBase interface
PT_IkSolver=6, ///< describes \ref IkSolverBase interface
PT_InverseKinematicsSolver=6, ///< describes \ref IkSolverBase interface
PT_KinBody=7, ///< describes \ref KinBody
PT_PhysicsEngine=8, ///< describes \ref PhysicsEngineBase
PT_Sensor=9, ///< describes \ref SensorBase
PT_CollisionChecker=10, ///< describes \ref CollisionCheckerBase
PT_Trajectory=11, ///< describes \ref TrajectoryBase
PT_Viewer=12, ///< describes \ref ViewerBase
PT_SpaceSampler=13, ///< describes \ref SamplerBase
PT_NumberOfInterfaces=13 ///< number of interfaces, do not forget to update
};
class CollisionReport;
class InterfaceBase;
class IkSolverBase;
class TrajectoryBase;
class ControllerBase;
class PlannerBase;
class RobotBase;
class ModuleBase;
class EnvironmentBase;
class KinBody;
class SensorSystemBase;
class PhysicsEngineBase;
class SensorBase;
class CollisionCheckerBase;
class ViewerBase;
class SpaceSamplerBase;
class IkParameterization;
class ConfigurationSpecification;
class IkReturn;
typedef boost::shared_ptr<CollisionReport> CollisionReportPtr;
typedef boost::shared_ptr<CollisionReport const> CollisionReportConstPtr;
typedef boost::shared_ptr<InterfaceBase> InterfaceBasePtr;
typedef boost::shared_ptr<InterfaceBase const> InterfaceBaseConstPtr;
typedef boost::weak_ptr<InterfaceBase> InterfaceBaseWeakPtr;
typedef boost::shared_ptr<KinBody> KinBodyPtr;
typedef boost::shared_ptr<KinBody const> KinBodyConstPtr;
typedef boost::weak_ptr<KinBody> KinBodyWeakPtr;
typedef boost::shared_ptr<RobotBase> RobotBasePtr;
typedef boost::shared_ptr<RobotBase const> RobotBaseConstPtr;
typedef boost::weak_ptr<RobotBase> RobotBaseWeakPtr;
typedef boost::shared_ptr<CollisionCheckerBase> CollisionCheckerBasePtr;
typedef boost::shared_ptr<CollisionCheckerBase const> CollisionCheckerBaseConstPtr;
typedef boost::weak_ptr<CollisionCheckerBase> CollisionCheckerBaseWeakPtr;
typedef boost::shared_ptr<ControllerBase> ControllerBasePtr;
typedef boost::shared_ptr<ControllerBase const> ControllerBaseConstPtr;
typedef boost::weak_ptr<ControllerBase> ControllerBaseWeakPtr;
typedef boost::shared_ptr<IkSolverBase> IkSolverBasePtr;
typedef boost::shared_ptr<IkSolverBase const> IkSolverBaseConstPtr;
typedef boost::weak_ptr<IkSolverBase> IkSolverBaseWeakPtr;
typedef boost::shared_ptr<PhysicsEngineBase> PhysicsEngineBasePtr;
typedef boost::shared_ptr<PhysicsEngineBase const> PhysicsEngineBaseConstPtr;
typedef boost::weak_ptr<PhysicsEngineBase> PhysicsEngineBaseWeakPtr;
typedef boost::shared_ptr<PlannerBase> PlannerBasePtr;
typedef boost::shared_ptr<PlannerBase const> PlannerBaseConstPtr;
typedef boost::weak_ptr<PlannerBase> PlannerBaseWeakPtr;
typedef boost::shared_ptr<ModuleBase> ModuleBasePtr;
typedef boost::shared_ptr<ModuleBase const> ModuleBaseConstPtr;
typedef boost::weak_ptr<ModuleBase> ModuleBaseWeakPtr;
typedef boost::shared_ptr<SensorBase> SensorBasePtr;
typedef boost::shared_ptr<SensorBase const> SensorBaseConstPtr;
typedef boost::weak_ptr<SensorBase> SensorBaseWeakPtr;
typedef boost::shared_ptr<SensorSystemBase> SensorSystemBasePtr;
typedef boost::shared_ptr<SensorSystemBase const> SensorSystemBaseConstPtr;
typedef boost::weak_ptr<SensorSystemBase> SensorSystemBaseWeakPtr;
typedef boost::shared_ptr<TrajectoryBase> TrajectoryBasePtr;
typedef boost::shared_ptr<TrajectoryBase const> TrajectoryBaseConstPtr;
typedef boost::weak_ptr<TrajectoryBase> TrajectoryBaseWeakPtr;
typedef boost::shared_ptr<ViewerBase> ViewerBasePtr;
typedef boost::shared_ptr<ViewerBase const> ViewerBaseConstPtr;
typedef boost::weak_ptr<ViewerBase> ViewerBaseWeakPtr;
typedef boost::shared_ptr<SpaceSamplerBase> SpaceSamplerBasePtr;
typedef boost::shared_ptr<SpaceSamplerBase const> SpaceSamplerBaseConstPtr;
typedef boost::weak_ptr<SpaceSamplerBase> SpaceSamplerBaseWeakPtr;
typedef boost::shared_ptr<EnvironmentBase> EnvironmentBasePtr;
typedef boost::shared_ptr<EnvironmentBase const> EnvironmentBaseConstPtr;
typedef boost::weak_ptr<EnvironmentBase> EnvironmentBaseWeakPtr;
typedef boost::shared_ptr<IkReturn> IkReturnPtr;
typedef boost::shared_ptr<IkReturn const> IkReturnConstPtr;
typedef boost::weak_ptr<IkReturn> IkReturnWeakPtr;
class BaseXMLReader;
typedef boost::shared_ptr<BaseXMLReader> BaseXMLReaderPtr;
typedef boost::shared_ptr<BaseXMLReader const> BaseXMLReaderConstPtr;
class BaseXMLWriter;
typedef boost::shared_ptr<BaseXMLWriter> BaseXMLWriterPtr;
typedef boost::shared_ptr<BaseXMLWriter const> BaseXMLWriterConstPtr;
///< Cloning Options for interfaces and environments
enum CloningOptions {
Clone_Bodies = 1, ///< clone all the bodies/robots of the environment, exclude attached interfaces like sensors/controllers
Clone_Viewer = 2, ///< clone the viewer type, although figures won't be copied, new viewer does try to match views
Clone_Simulation = 4, ///< clone the physics engine and simulation state (ie, timesteps, gravity)
Clone_RealControllers = 8, ///< if specified, will clone the real controllers of all the robots, otherwise each robot gets ideal controller
Clone_Sensors = 0x0010, ///< if specified, will clone the sensors attached to the robot and added to the environment
Clone_Modules = 0x0020, ///< if specified, will clone the modules attached to the environment
Clone_All = 0xffffffff,
};
/// base class for readable interfaces
class OPENRAVE_API XMLReadable : public UserData
{
public:
XMLReadable(const std::string& xmlid) : __xmlid(xmlid) {
}
virtual ~XMLReadable() {
}
virtual const std::string& GetXMLId() const {
return __xmlid;
}
/// \brief serializes the interface
virtual void Serialize(BaseXMLWriterPtr writer, int options=0) const {
}
private:
std::string __xmlid;
};
typedef boost::shared_ptr<XMLReadable> XMLReadablePtr;
typedef boost::shared_ptr<XMLReadable const> XMLReadableConstPtr;
/// \brief a list of key-value pairs. It is possible for keys to repeat.
typedef std::list<std::pair<std::string,std::string> > AttributesList;
/// \brief base class for all xml readers. XMLReaders are used to process data from xml files.
///
/// Custom readers can be registered through \ref RaveRegisterXMLReader.
class OPENRAVE_API BaseXMLReader : public boost::enable_shared_from_this<BaseXMLReader>
{
public:
enum ProcessElement
{
PE_Pass=0, ///< current tag was not supported, so pass onto another class
PE_Support=1, ///< current tag will be processed by this class
PE_Ignore=2, ///< current tag and all its children should be ignored
};
BaseXMLReader() {
}
virtual ~BaseXMLReader() {
}
/// a readable interface that stores the information processsed for the current tag
/// This pointer is used to the InterfaceBase class registered readers
virtual XMLReadablePtr GetReadable() {
return XMLReadablePtr();
}
/// Gets called in the beginning of each "<type>" expression. In this case, name is "type"
/// \param name of the tag, will be always lower case
/// \param atts string of attributes where the first std::string is the attribute name and second is the value
/// \return true if tag is accepted and this class will process it, otherwise false
virtual ProcessElement startElement(const std::string& name, const AttributesList& atts) = 0;
/// Gets called at the end of each "</type>" expression. In this case, name is "type"
/// \param name of the tag, will be always lower case
/// \return true if XMLReader has finished parsing (one condition is that name==_fieldname) , otherwise false
virtual bool endElement(const std::string& name) = 0;
/// gets called for all data in between tags.
/// \param ch a string to the data
virtual void characters(const std::string& ch) = 0;
/// XML filename/resource used for this class (can be empty)
std::string _filename;
};
typedef boost::function<BaseXMLReaderPtr(InterfaceBasePtr, const AttributesList&)> CreateXMLReaderFn;
/// \brief reads until the tag ends
class OPENRAVE_API DummyXMLReader : public BaseXMLReader
{
public:
DummyXMLReader(const std::string& fieldname, const std::string& parentname, boost::shared_ptr<std::ostream> osrecord = boost::shared_ptr<std::ostream>());
virtual ProcessElement startElement(const std::string& name, const AttributesList& atts);
virtual bool endElement(const std::string& name);
virtual void characters(const std::string& ch);
const std::string& GetFieldName() const {
return _fieldname;
}
virtual boost::shared_ptr<std::ostream> GetStream() const {
return _osrecord;
}
private:
std::string _parentname; /// XML filename
std::string _fieldname;
boost::shared_ptr<std::ostream> _osrecord; ///< used to store the xml data
boost::shared_ptr<BaseXMLReader> _pcurreader;
};
/// \brief base class for writing to XML files.
///
/// OpenRAVE Interfaces accept a BaseXMLWriter instance and call its write methods to write the data.
class OPENRAVE_API BaseXMLWriter : public boost::enable_shared_from_this<BaseXMLWriter>
{
public:
virtual ~BaseXMLWriter() {
}
/// \brief return the format for the data writing, should be all lower capitals.
///
/// Samples formats are 'openrave', 'collada'
virtual const std::string& GetFormat() const = 0;
/// \brief saves character data to the child. Special characters like '<' are automatically converted to fit inside XML.
///
/// \throw openrave_exception throws if this element cannot have character data or the character data was not written
virtual void SetCharData(const std::string& data) = 0;
/// \brief returns a writer for child elements
virtual BaseXMLWriterPtr AddChild(const std::string& xmltag, const AttributesList& atts=AttributesList()) = 0;
};
} // end namespace OpenRAVE
// define the math functions
#if OPENRAVE_PRECISION // 1 if double precision
#define OPENRAVE_MATH_EXP_DOUBLE RaveExp
#define OPENRAVE_MATH_LOG_DOUBLE RaveLog
#define OPENRAVE_MATH_COS_DOUBLE RaveCos
#define OPENRAVE_MATH_SIN_DOUBLE RaveSin
#define OPENRAVE_MATH_TAN_DOUBLE RaveTan
#define OPENRAVE_MATH_LOG2_DOUBLE RaveLog2
#define OPENRAVE_MATH_LOG10_DOUBLE RaveLog10
#define OPENRAVE_MATH_ACOS_DOUBLE RaveAcos
#define OPENRAVE_MATH_ASIN_DOUBLE RaveAsin
#define OPENRAVE_MATH_ATAN2_DOUBLE RaveAtan2
#define OPENRAVE_MATH_POW_DOUBLE RavePow
#define OPENRAVE_MATH_SQRT_DOUBLE RaveSqrt
#define OPENRAVE_MATH_FABS_DOUBLE RaveFabs
#else // 32bit float
#define OPENRAVE_MATH_EXP_FLOAT RaveExp
#define OPENRAVE_MATH_LOG_FLOAT RaveLog
#define OPENRAVE_MATH_COS_FLOAT RaveCos
#define OPENRAVE_MATH_SIN_FLOAT RaveSin
#define OPENRAVE_MATH_TAN_FLOAT RaveTan
#define OPENRAVE_MATH_LOG2_FLOAT RaveLog2
#define OPENRAVE_MATH_LOG10_FLOAT RaveLog10
#define OPENRAVE_MATH_ACOS_FLOAT RaveAcos
#define OPENRAVE_MATH_ASIN_FLOAT RaveAsin
#define OPENRAVE_MATH_ATAN2_FLOAT RaveAtan2
#define OPENRAVE_MATH_POW_FLOAT RavePow
#define OPENRAVE_MATH_SQRT_FLOAT RaveSqrt
#define OPENRAVE_MATH_FABS_FLOAT RaveFabs
#endif
#include <openrave/geometry.h>
#include <openrave/mathextra.h>
namespace OpenRAVE {
using geometry::RaveVector;
using geometry::RaveTransform;
using geometry::RaveTransformMatrix;
typedef RaveVector<dReal> Vector;
typedef RaveTransform<dReal> Transform;
typedef boost::shared_ptr< RaveTransform<dReal> > TransformPtr;
typedef boost::shared_ptr< RaveTransform<dReal> const > TransformConstPtr;
typedef RaveTransformMatrix<dReal> TransformMatrix;
typedef boost::shared_ptr< RaveTransformMatrix<dReal> > TransformMatrixPtr;
typedef boost::shared_ptr< RaveTransformMatrix<dReal> const > TransformMatrixConstPtr;
typedef geometry::obb<dReal> OBB;
typedef geometry::aabb<dReal> AABB;
typedef geometry::ray<dReal> RAY;
// for compatibility
//@{
using mathextra::dot2;
using mathextra::dot3;
using mathextra::dot4;
using mathextra::normalize2;
using mathextra::normalize3;
using mathextra::normalize4;
using mathextra::cross3;
using mathextra::inv3;
using mathextra::inv4;
using mathextra::lengthsqr2;
using mathextra::lengthsqr3;
using mathextra::lengthsqr4;
using mathextra::mult4;
//@}
/// \brief The types of inverse kinematics parameterizations supported.
///
/// The minimum degree of freedoms required is set in the upper 4 bits of each type.
/// The number of values used to represent the parameterization ( >= dof ) is the next 4 bits.
/// The lower bits contain a unique id of the type.
enum IkParameterizationType {
IKP_None=0,
IKP_Transform6D=0x67000001, ///< end effector reaches desired 6D transformation
IKP_Rotation3D=0x34000002, ///< end effector reaches desired 3D rotation
IKP_Translation3D=0x33000003, ///< end effector origin reaches desired 3D translation
IKP_Direction3D=0x23000004, ///< direction on end effector coordinate system reaches desired direction
IKP_Ray4D=0x46000005, ///< ray on end effector coordinate system reaches desired global ray
IKP_Lookat3D=0x23000006, ///< direction on end effector coordinate system points to desired 3D position
IKP_TranslationDirection5D=0x56000007, ///< end effector origin and direction reaches desired 3D translation and direction. Can be thought of as Ray IK where the origin of the ray must coincide.
IKP_TranslationXY2D=0x22000008, ///< 2D translation along XY plane
IKP_TranslationXYOrientation3D=0x33000009, ///< 2D translation along XY plane and 1D rotation around Z axis. The offset of the rotation is measured starting at +X, so at +X is it 0, at +Y it is pi/2.
IKP_TranslationLocalGlobal6D=0x3600000a, ///< local point on end effector origin reaches desired 3D global point
IKP_TranslationXAxisAngle4D=0x4400000b, ///< end effector origin reaches desired 3D translation, manipulator direction makes a specific angle with x-axis like a cone, angle is from 0-pi. Axes defined in the manipulator base link's coordinate system)
IKP_TranslationYAxisAngle4D=0x4400000c, ///< end effector origin reaches desired 3D translation, manipulator direction makes a specific angle with y-axis like a cone, angle is from 0-pi. Axes defined in the manipulator base link's coordinate system)
IKP_TranslationZAxisAngle4D=0x4400000d, ///< end effector origin reaches desired 3D translation, manipulator direction makes a specific angle with z-axis like a cone, angle is from 0-pi. Axes are defined in the manipulator base link's coordinate system.
IKP_TranslationXAxisAngleZNorm4D=0x4400000e, ///< end effector origin reaches desired 3D translation, manipulator direction needs to be orthogonal to z-axis and be rotated at a certain angle starting from the x-axis (defined in the manipulator base link's coordinate system)
IKP_TranslationYAxisAngleXNorm4D=0x4400000f, ///< end effector origin reaches desired 3D translation, manipulator direction needs to be orthogonal to x-axis and be rotated at a certain angle starting from the y-axis (defined in the manipulator base link's coordinate system)
IKP_TranslationZAxisAngleYNorm4D=0x44000010, ///< end effector origin reaches desired 3D translation, manipulator direction needs to be orthogonal to y-axis and be rotated at a certain angle starting from the z-axis (defined in the manipulator base link's coordinate system)
IKP_NumberOfParameterizations=16, ///< number of parameterizations (does not count IKP_None)
IKP_VelocityDataBit = 0x00008000, ///< bit is set if the data represents the time-derivate velocity of an IkParameterization
IKP_Transform6DVelocity = IKP_Transform6D|IKP_VelocityDataBit,
IKP_Rotation3DVelocity = IKP_Rotation3D|IKP_VelocityDataBit,
IKP_Translation3DVelocity = IKP_Translation3D|IKP_VelocityDataBit,
IKP_Direction3DVelocity = IKP_Direction3D|IKP_VelocityDataBit,
IKP_Ray4DVelocity = IKP_Ray4D|IKP_VelocityDataBit,
IKP_Lookat3DVelocity = IKP_Lookat3D|IKP_VelocityDataBit,
IKP_TranslationDirection5DVelocity = IKP_TranslationDirection5D|IKP_VelocityDataBit,
IKP_TranslationXY2DVelocity = IKP_TranslationXY2D|IKP_VelocityDataBit,
IKP_TranslationXYOrientation3DVelocity = IKP_TranslationXYOrientation3D|IKP_VelocityDataBit,
IKP_TranslationLocalGlobal6DVelocity = IKP_TranslationLocalGlobal6D|IKP_VelocityDataBit,
IKP_TranslationXAxisAngle4DVelocity = IKP_TranslationXAxisAngle4D|IKP_VelocityDataBit,
IKP_TranslationYAxisAngle4DVelocity = IKP_TranslationYAxisAngle4D|IKP_VelocityDataBit,
IKP_TranslationZAxisAngle4DVelocity = IKP_TranslationZAxisAngle4D|IKP_VelocityDataBit,
IKP_TranslationXAxisAngleZNorm4DVelocity = IKP_TranslationXAxisAngleZNorm4D|IKP_VelocityDataBit,
IKP_TranslationYAxisAngleXNorm4DVelocity = IKP_TranslationYAxisAngleXNorm4D|IKP_VelocityDataBit,
IKP_TranslationZAxisAngleYNorm4DVelocity = IKP_TranslationZAxisAngleYNorm4D|IKP_VelocityDataBit,
IKP_UniqueIdMask = 0x0000ffff, ///< the mask for the unique ids
IKP_CustomDataBit = 0x00010000, ///< bit is set if the ikparameterization contains custom data, this is only used when serializing the ik parameterizations
};
/// \brief returns a string of the ik parameterization type names
///
/// \param[in] alllowercase If 1, sets all characters to lower case. Otherwise can include upper case in order to match \ref IkParameterizationType definition.
OPENRAVE_API const std::map<IkParameterizationType,std::string>& RaveGetIkParameterizationMap(int alllowercase=0);
/// \brief returns the IkParameterizationType given the unique id detmerined b IKP_UniqueIdMask
OPENRAVE_API IkParameterizationType RaveGetIkTypeFromUniqueId(int uniqueid);
/** \brief A configuration specification references values in the environment that then define a configuration-space which can be searched for.
It is composed of several groups targetting values for individual bodies. It is serialized into XML. The XML syntax is as follows:
\code
<configuration>
<group name="string" offset="#OFF1" dof="#D1" interpolation="string"/>
<group name="string" offset="#OFF2" dof="#D2" interpolation="string"/>
</configuration>
\endcode
*/
class OPENRAVE_API ConfigurationSpecification
{
public:
/// \brief A group referencing the values of one body in the environment
class OPENRAVE_API Group
{
public:
Group() : offset(0), dof(0) {
}
inline bool operator==(const Group& r) const {
return offset==r.offset && dof==r.dof && name==r.name && interpolation==r.interpolation;
}
inline bool operator!=(const Group& r) const {
return offset!=r.offset || dof!=r.dof || name!=r.name || interpolation!=r.interpolation;
}
/// \brief For each data point, the number of values to offset before data for this group starts.
int offset;
/// \brief The number of values in this group.
int dof;
/** \brief semantic information on what part of the environment the group refers to.
Can be composed of multiple workds; the first word is the group type, and the words following narrow the specifics. Common types are:
- \b joint_values - The joint values of a kinbody/robot. The joint names with the name of the body can follow.
- \b joint_velocities - The joint velocities (1/second) of a kinbody/robot. The name of the body with the joint names can follow.
- \b joint_accelerations - The joint accelerations (1/second^2) of a kinbody/robot. The name of the body with the joint names can follow.
- \b joint_jerks - The joint jerks (1/second^3) of a kinbody/robot. The name of the body with the joint names can follow.
- \b joint_snaps - The joint snaps (1/second^4) of a kinbody/robot. The name of the body with the joint names can follow.
- \b joint_crackles - The joint crackles (1/second^5) of a kinbody/robot. The name of the body with the joint names can follow.
- \b joint_pops - The joint pops (1/second^6) of a kinbody/robot. The name of the body with the joint names can follow.
- \b joint_torques - The joint torques (Newton meter) of a kinbody/robot. The name of the body with the joint names can follow.
- \b affine_transform - An affine transformation [quaternion, translation]. The name of the body with selected affine dofs (see \ref DOFAffine) can follow.
- \b affine_velocities - The velocity (1/second) of the affine transformation [rotation axis, translation velocity], the name of the body can follow.
- \b affine_accelerations - The acceleration (1/second^2) of the affine transformation [rotation axis, translation velocity], the name of the body can follow.
- \b affine_jerks - The jerk (1/second^3) of the affine transformation [rotation axis, translation velocity], the name of the body can follow.
- \b ikparam_values - The values of an IkParmeterization. The ikparam type is stored as the second value in name
- \b ikparam_velocities - acceleration of an IkParmeterization. The ikparam type is stored as the second value in name
- \b ikparam_jerks - jerk of an IkParmeterization. The ikparam type is stored as the second value in name
- \b iswaypoint - non-zero if the point represents a major knot point of the trajectory
- \b grabbody - Grabs the body. The configuration values are 1 for grab and 0 for release. The group name format is: bodyname robotname robotlinkindex [relative_grab_pose]. relative_grab_pose is a 7 value (quaterion + translation) pose of the relative location of the body with respect to the grabbed link. Only 1 DOF is accepted.
*/
std::string name;
/** \brief Describes how the data should be interpolated. Common methods are:
- \b previous - the previous waypoint's value is always chosen
- \b next - the next waypoint's value is always chosen