collisionchecker.h   collisionchecker.h 
skipping to change at line 19 skipping to change at line 19
// //
// This program is distributed in the hope that it will be useful, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details. // GNU Lesser General Public License for more details.
// //
// You should have received a copy of the GNU Lesser General Public License // 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/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
/** \file collisionchecker.h /** \file collisionchecker.h
\brief Collision checking related definitions. \brief Collision checking related definitions.
Automatically included with \ref openrave.h
*/ */
#ifndef OPENRAVE_COLLISIONCHECKER_H #ifndef OPENRAVE_COLLISIONCHECKER_H
#define OPENRAVE_COLLISIONCHECKER_H #define OPENRAVE_COLLISIONCHECKER_H
namespace OpenRAVE { namespace OpenRAVE {
/// options for collision checker /// options for collision checker
enum CollisionOptions enum CollisionOptions
{ {
CO_Distance = 1, ///< Compute distance measurements, this is usually sl ow and not all checkers support it. CO_Distance = 1, ///< Compute distance measurements, this is usually sl ow and not all checkers support it.
 End of changes. 1 change blocks. 
0 lines changed or deleted 2 lines changed or added


 config.h   config.h 
skipping to change at line 40 skipping to change at line 40
#define OPENRAVE_API OPENRAVE_HELPER_DLL_IMPORT #define OPENRAVE_API OPENRAVE_HELPER_DLL_IMPORT
#endif // OPENRAVE_DLL_EXPORTS #endif // OPENRAVE_DLL_EXPORTS
#define OPENRAVE_LOCAL OPENRAVE_HELPER_DLL_LOCAL #define OPENRAVE_LOCAL OPENRAVE_HELPER_DLL_LOCAL
#else // OPENRAVE_DLL is not defined: this means OpenRAVE is a static lib. #else // OPENRAVE_DLL is not defined: this means OpenRAVE is a static lib.
#define OPENRAVE_API #define OPENRAVE_API
#define OPENRAVE_LOCAL #define OPENRAVE_LOCAL
#endif // OPENRAVE_DLL #endif // OPENRAVE_DLL
#define OPENRAVE_VERSION_MAJOR 0 #define OPENRAVE_VERSION_MAJOR 0
#define OPENRAVE_VERSION_MINOR 6 #define OPENRAVE_VERSION_MINOR 6
#define OPENRAVE_VERSION_PATCH 2 #define OPENRAVE_VERSION_PATCH 4
#define OPENRAVE_VERSION_COMBINED(major, minor, patch) (((major) << 16) | ( (minor) << 8) | (patch)) #define OPENRAVE_VERSION_COMBINED(major, minor, patch) (((major) << 16) | ( (minor) << 8) | (patch))
#define OPENRAVE_VERSION OPENRAVE_VERSION_COMBINED(OPENRAVE_VERSION_MAJOR, OPENRAVE_VERSION_MINOR, OPENRAVE_VERSION_PATCH) #define OPENRAVE_VERSION OPENRAVE_VERSION_COMBINED(OPENRAVE_VERSION_MAJOR, OPENRAVE_VERSION_MINOR, OPENRAVE_VERSION_PATCH)
#define OPENRAVE_VERSION_EXTRACT_MAJOR(version) (((version)>>16)&0xff) #define OPENRAVE_VERSION_EXTRACT_MAJOR(version) (((version)>>16)&0xff)
#define OPENRAVE_VERSION_EXTRACT_MINOR(version) (((version)>>8)&0xff) #define OPENRAVE_VERSION_EXTRACT_MINOR(version) (((version)>>8)&0xff)
#define OPENRAVE_VERSION_EXTRACT_PATCH(version) (((version))&0xff) #define OPENRAVE_VERSION_EXTRACT_PATCH(version) (((version))&0xff)
#define OPENRAVE_VERSION_STRING "0.6.2" #define OPENRAVE_VERSION_STRING "0.6.4"
#define OPENRAVE_VERSION_STRING_FORMAT(version) boost::str(boost::format("% s.%s.%s")%(OPENRAVE_VERSION_EXTRACT_MAJOR(version))%(OPENRAVE_VERSION_EXTRA CT_MINOR(version))%(OPENRAVE_VERSION_EXTRACT_PATCH(version))) #define OPENRAVE_VERSION_STRING_FORMAT(version) boost::str(boost::format("% s.%s.%s")%(OPENRAVE_VERSION_EXTRACT_MAJOR(version))%(OPENRAVE_VERSION_EXTRA CT_MINOR(version))%(OPENRAVE_VERSION_EXTRACT_PATCH(version)))
#define OPENRAVE_VERSION_GE(major1, minor1, patch1, major2, minor2, patch2) (OPENRAVE_VERSION_COMBINED(major1, minor1, patch1) >= OPENRAVE_VERSION_COM BINED(major2, minor2, patch2)) #define OPENRAVE_VERSION_GE(major1, minor1, patch1, major2, minor2, patch2) (OPENRAVE_VERSION_COMBINED(major1, minor1, patch1) >= OPENRAVE_VERSION_COM BINED(major2, minor2, patch2))
#define OPENRAVE_VERSION_MINIMUM(major, minor, patch) OPENRAVE_VERSION_GE(O PENRAVE_VERSION_MAJOR, OPENRAVE_VERSION_MINOR, OPENRAVE_VERSION_PATCH, majo r, minor, patch) #define OPENRAVE_VERSION_MINIMUM(major, minor, patch) OPENRAVE_VERSION_GE(O PENRAVE_VERSION_MAJOR, OPENRAVE_VERSION_MINOR, OPENRAVE_VERSION_PATCH, majo r, minor, patch)
// if 0, single precision // if 0, single precision
// if 1, double precision // if 1, double precision
#define OPENRAVE_PRECISION 1 #define OPENRAVE_PRECISION 1
#define OPENRAVE_PLUGINS_INSTALL_DIR "/home/andrey/upstream-tracker/testing #define OPENRAVE_PLUGINS_INSTALL_DIR "/home/andrey/upstream-tracker/testing
/openrave/0.6.2/share/openrave-0.6/plugins" /openrave/0.6.4/share/openrave-0.6/plugins"
#define OPENRAVE_DATA_INSTALL_DIR "/home/andrey/upstream-tracker/testing/op #define OPENRAVE_DATA_INSTALL_DIR "/home/andrey/upstream-tracker/testing/op
enrave/0.6.2/share/openrave-0.6" enrave/0.6.4/share/openrave-0.6"
#define OPENRAVE_PYTHON_INSTALL_DIR "/home/andrey/upstream-tracker/testing/ #define OPENRAVE_PYTHON_INSTALL_DIR "/home/andrey/upstream-tracker/testing/
openrave/0.6.2/lib/python2.7/site-packages" openrave/0.6.4/lib/python2.7/site-packages"
#endif #endif
 End of changes. 3 change blocks. 
8 lines changed or deleted 8 lines changed or added


 controller.h   controller.h 
skipping to change at line 19 skipping to change at line 19
// //
// This program is distributed in the hope that it will be useful, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details. // GNU Lesser General Public License for more details.
// //
// You should have received a copy of the GNU Lesser General Public License // 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/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
/** \file controller.h /** \file controller.h
\brief Controller related definitions. \brief Controller related definitions.
Automatically included with \ref openrave.h
*/ */
#ifndef OPENRAVE_CONTROLLER_H #ifndef OPENRAVE_CONTROLLER_H
#define OPENRAVE_CONTROLLER_H #define OPENRAVE_CONTROLLER_H
namespace OpenRAVE { namespace OpenRAVE {
/** \brief <b>[interface]</b> Abstract base class to encapsulate a local co ntroller. <b>If not specified, method is not multi-thread safe.</b> See \re f arch_controller. /** \brief <b>[interface]</b> Abstract base class to encapsulate a local co ntroller. <b>If not specified, method is not multi-thread safe.</b> See \re f arch_controller.
\ingroup interfaces \ingroup interfaces
*/ */
class OPENRAVE_API ControllerBase : public InterfaceBase class OPENRAVE_API ControllerBase : public InterfaceBase
 End of changes. 1 change blocks. 
0 lines changed or deleted 2 lines changed or added


 environment.h   environment.h 
skipping to change at line 19 skipping to change at line 19
// //
// This program is distributed in the hope that it will be useful, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details. // GNU Lesser General Public License for more details.
// //
// You should have received a copy of the GNU Lesser General Public License // 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/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
/** \file environment.h /** \file environment.h
\brief Definition of the EnvironmentBase interface used for managing al l objects in an environment. \brief Definition of the EnvironmentBase interface used for managing al l objects in an environment.
Automatically included with \ref openrave.h
*/ */
#ifndef OPENRAVE_ENVIRONMENTBASE_H #ifndef OPENRAVE_ENVIRONMENTBASE_H
#define OPENRAVE_ENVIRONMENTBASE_H #define OPENRAVE_ENVIRONMENTBASE_H
namespace OpenRAVE { namespace OpenRAVE {
typedef boost::recursive_try_mutex EnvironmentMutex; typedef boost::recursive_try_mutex EnvironmentMutex;
/** \brief Maintains a world state, which serves as the gateway to all func tions offered through %OpenRAVE. See \ref arch_environment. /** \brief Maintains a world state, which serves as the gateway to all func tions offered through %OpenRAVE. See \ref arch_environment.
*/ */
 End of changes. 1 change blocks. 
0 lines changed or deleted 2 lines changed or added


 geometry.h   geometry.h 
// -*- coding: utf-8 -*- // -*- coding: utf-8 -*-
// Copyright (C) 2006-2011 Rosen Diankov <rosen.diankov@gmail.com> // Copyright (C) 2006-2012 Rosen Diankov <rosen.diankov@gmail.com>
// //
// This file is part of OpenRAVE. // This file is part of OpenRAVE.
// OpenRAVE is free software: you can redistribute it and/or modify // 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 // 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 // the Free Software Foundation, either version 3 of the License, or
// at your option) any later version. // at your option) any later version.
// //
// This program is distributed in the hope that it will be useful, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details. // GNU Lesser General Public License for more details.
// //
// You should have received a copy of the GNU Lesser General Public License // 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/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
/** \file geometry.h /** \file geometry.h
\brief Basic gemoetric primitives and affine math functions on them. \brief Basic gemoetric primitives and affine math functions on them.
This file can be used stand-alone without \ref openrave.h .
*/ */
#ifndef OPENRAVE_GEOMETRY_H #ifndef OPENRAVE_GEOMETRY_H
#define OPENRAVE_GEOMETRY_H #define OPENRAVE_GEOMETRY_H
#include <cmath> #include <cmath>
#include <iostream> #include <vector>
#include <string>
#include <limits> #include <limits>
#include <utility> // for std::pair #include <utility> // for std::pair
#include <cstring>
#include <cstdlib> #include <cstdlib>
#ifndef RAVE_DEPRECATED #ifndef RAVE_DEPRECATED
#define RAVE_DEPRECATED #define RAVE_DEPRECATED
#endif #endif
#ifdef BOOST_ASSERT #ifdef BOOST_ASSERT
#define MATH_ASSERT BOOST_ASSERT #define MATH_ASSERT BOOST_ASSERT
#else #else
#include <cassert> #include <cassert>
#define MATH_ASSERT assert #define MATH_ASSERT assert
skipping to change at line 134 skipping to change at line 136
} }
/// note, it only copes 3 values! /// note, it only copes 3 values!
template<typename U> RaveVector(const U* pf) { template<typename U> RaveVector(const U* pf) {
MATH_ASSERT(pf != NULL); x = (T)pf[0]; y = (T)pf[1]; z = (T)pf[2]; w = 0; MATH_ASSERT(pf != NULL); x = (T)pf[0]; y = (T)pf[1]; z = (T)pf[2]; w = 0;
} }
T operator[] (int i) const { T operator[] (int i) const {
return (&x)[i]; return (&x)[i];
} }
T& operator[] (int i) { T& operator[] (int i) {
return (&x)[i]; return (&x)[i];
} }
// casting operators
operator T* () {
return &x;
}
operator const T* () const { return (const T*)&x; }
template <typename U> template <typename U>
RaveVector<T>& operator=(const RaveVector<U>&r) { RaveVector<T>& operator=(const RaveVector<U>&r) {
x = (T)r.x; y = (T)r.y; z = (T)r.z; w = (T)r.w; return *this; x = (T)r.x; y = (T)r.y; z = (T)r.z; w = (T)r.w; return *this;
} }
// SCALAR FUNCTIONS // SCALAR FUNCTIONS
template <typename U> inline T dot(const RaveVector<U> &v) const { template <typename U> inline T dot(const RaveVector<U> &v) const {
return x*v.x + y*v.y + z*v.z + w*v.w; return x*v.x + y*v.y + z*v.z + w*v.w;
} }
template <typename U> inline T dot3(const RaveVector<U> &v) const { template <typename U> inline T dot3(const RaveVector<U> &v) const {
return x*v.x + y*v.y + z*v.z; return x*v.x + y*v.y + z*v.z;
} }
inline RaveVector<T>& normalize() { inline RaveVector<T>& normalize() {
return normalize4(); return normalize4();
} }
inline RaveVector<T>& normalize4() { inline RaveVector<T>& normalize4() {
T f = x*x+y*y+z*z+w*w; T f = x*x+y*y+z*z+w*w;
if(( f < T(1)-std::numeric_limits<dReal>::epsilon()) ||( f > T(1)+s td::numeric_limits<dReal>::epsilon()) ) { if(( f < T(1)-std::numeric_limits<T>::epsilon()) ||( f > T(1)+std:: numeric_limits<T>::epsilon()) ) {
MATH_ASSERT( f > 0 ); MATH_ASSERT( f > 0 );
// yes it is faster to multiply by (1/f), but with 4 divides we gain precision (which is more important in robotics) // yes it is faster to multiply by (1/f), but with 4 divides we gain precision (which is more important in robotics)
f = MATH_SQRT(f); f = MATH_SQRT(f);
x /= f; y /= f; z /= f; w /= f; x /= f; y /= f; z /= f; w /= f;
} }
return *this; return *this;
} }
inline RaveVector<T>& normalize3() { inline RaveVector<T>& normalize3() {
T f = x*x+y*y+z*z; T f = x*x+y*y+z*z;
if(( f < T(1)-std::numeric_limits<dReal>::epsilon()) ||( f > T(1)+s td::numeric_limits<dReal>::epsilon()) ) { if(( f < T(1)-std::numeric_limits<T>::epsilon()) ||( f > T(1)+std:: numeric_limits<T>::epsilon()) ) {
MATH_ASSERT( f > 0 ); MATH_ASSERT( f > 0 );
f = MATH_SQRT(f); f = MATH_SQRT(f);
x /= f; y /= f; z /= f; x /= f; y /= f; z /= f;
} }
return *this; return *this;
} }
inline T lengthsqr2() const { inline T lengthsqr2() const {
return x*x + y*y; return x*x + y*y;
} }
skipping to change at line 419 skipping to change at line 415
*/ */
template <typename T> template <typename T>
class RaveTransformMatrix class RaveTransformMatrix
{ {
public: public:
inline RaveTransformMatrix() { inline RaveTransformMatrix() {
identity(); m[3] = m[7] = m[11] = 0; identity(); m[3] = m[7] = m[11] = 0;
} }
template <typename U> RaveTransformMatrix(const RaveTransformMatrix<U>& t) { template <typename U> RaveTransformMatrix(const RaveTransformMatrix<U>& t) {
// don't memcpy! // don't memcpy!
m[0] = t.m[0]; m[1] = t.m[1]; m[2] = t.m[2]; m[3] = t.m[3]; m[0] = T(t.m[0]); m[1] = T(t.m[1]); m[2] = T(t.m[2]); m[3] = T(t.m[
m[4] = t.m[4]; m[5] = t.m[5]; m[6] = t.m[6]; m[7] = t.m[7]; 3]);
m[8] = t.m[8]; m[9] = t.m[9]; m[10] = t.m[10]; m[11] = t.m[11]; m[4] = T(t.m[4]); m[5] = T(t.m[5]); m[6] = T(t.m[6]); m[7] = T(t.m[
7]);
m[8] = T(t.m[8]); m[9] = T(t.m[9]); m[10] = T(t.m[10]); m[11] = T(t
.m[11]);
trans = t.trans; trans = t.trans;
} }
inline RaveTransformMatrix(const RaveTransform<T>&t); inline RaveTransformMatrix(const RaveTransform<T>&t);
inline void identity() { inline void identity() {
m[0] = 1; m[1] = 0; m[2] = 0; m[0] = 1; m[1] = 0; m[2] = 0;
m[4] = 0; m[5] = 1; m[6] = 0; m[4] = 0; m[5] = 1; m[6] = 0;
m[8] = 0; m[9] = 0; m[10] = 1; m[8] = 0; m[9] = 0; m[10] = 1;
trans.x = trans.y = trans.z = 0; trans.x = trans.y = trans.z = 0;
} }
skipping to change at line 797 skipping to change at line 793
return rot; return rot;
} }
/// \brief Converts a quaternion to a 3x3 matrix /// \brief Converts a quaternion to a 3x3 matrix
/// ///
/// \ingroup affine_math /// \ingroup affine_math
/// \param[in] quat quaternion, (s,vx,vy,vz) /// \param[in] quat quaternion, (s,vx,vy,vz)
template <typename T> inline RaveTransformMatrix<T> matrixFromQuat(const Ra veVector<T>& quat) template <typename T> inline RaveTransformMatrix<T> matrixFromQuat(const Ra veVector<T>& quat)
{ {
// should normalize the quaternion first // should normalize the quaternion first
dReal length2 = quat.lengthsqr4(); T length2 = quat.lengthsqr4();
BOOST_ASSERT(length2 > 0.99 && length2 < 1.01); // make sure it is at l MATH_ASSERT(length2 > 0.99 && length2 < 1.01); // make sure it is at le
east close ast close
dReal ilength2 = 2/length2; T ilength2 = 2/length2;
RaveTransformMatrix<T> t; RaveTransformMatrix<T> t;
T qq1 = ilength2*quat[1]*quat[1]; T qq1 = ilength2*quat[1]*quat[1];
T qq2 = ilength2*quat[2]*quat[2]; T qq2 = ilength2*quat[2]*quat[2];
T qq3 = ilength2*quat[3]*quat[3]; T qq3 = ilength2*quat[3]*quat[3];
t.m[4*0+0] = 1 - qq2 - qq3; t.m[4*0+0] = 1 - qq2 - qq3;
t.m[4*0+1] = ilength2*(quat[1]*quat[2] - quat[0]*quat[3]); t.m[4*0+1] = ilength2*(quat[1]*quat[2] - quat[0]*quat[3]);
t.m[4*0+2] = ilength2*(quat[1]*quat[3] + quat[0]*quat[2]); t.m[4*0+2] = ilength2*(quat[1]*quat[3] + quat[0]*quat[2]);
t.m[4*0+3] = 0; t.m[4*0+3] = 0;
t.m[4*1+0] = ilength2*(quat[1]*quat[2] + quat[0]*quat[3]); t.m[4*1+0] = ilength2*(quat[1]*quat[2] + quat[0]*quat[3]);
t.m[4*1+1] = 1 - qq1 - qq3; t.m[4*1+1] = 1 - qq1 - qq3;
skipping to change at line 827 skipping to change at line 823
} }
/// \brief Converts a quaternion to a 3x3 matrix /// \brief Converts a quaternion to a 3x3 matrix
/// ///
/// \ingroup affine_math /// \ingroup affine_math
/// \param[out] rotation /// \param[out] rotation
/// \param[in] quat quaternion, (s,vx,vy,vz) /// \param[in] quat quaternion, (s,vx,vy,vz)
template <typename T> void matrixFromQuat(RaveTransformMatrix<T>& rotation, const RaveVector<T>& quat) template <typename T> void matrixFromQuat(RaveTransformMatrix<T>& rotation, const RaveVector<T>& quat)
{ {
// should normalize the quaternion first // should normalize the quaternion first
dReal length2 = quat.lengthsqr4(); T length2 = quat.lengthsqr4();
BOOST_ASSERT(length2 > 0.99 && length2 < 1.01); // make sure it is at l MATH_ASSERT(length2 > 0.99 && length2 < 1.01); // make sure it is at le
east close ast close
dReal ilength2 = 2/length2; T ilength2 = 2/length2;
T qq1 = ilength2*quat[1]*quat[1]; T qq1 = ilength2*quat[1]*quat[1];
T qq2 = ilength2*quat[2]*quat[2]; T qq2 = ilength2*quat[2]*quat[2];
T qq3 = ilength2*quat[3]*quat[3]; T qq3 = ilength2*quat[3]*quat[3];
rotation.m[4*0+0] = 1 - qq2 - qq3; rotation.m[4*0+0] = 1 - qq2 - qq3;
rotation.m[4*0+1] = ilength2*(quat[1]*quat[2] - quat[0]*quat[3]); rotation.m[4*0+1] = ilength2*(quat[1]*quat[2] - quat[0]*quat[3]);
rotation.m[4*0+2] = ilength2*(quat[1]*quat[3] + quat[0]*quat[2]); rotation.m[4*0+2] = ilength2*(quat[1]*quat[3] + quat[0]*quat[2]);
rotation.m[4*0+3] = 0; rotation.m[4*0+3] = 0;
rotation.m[4*1+0] = ilength2*(quat[1]*quat[2] + quat[0]*quat[3]); rotation.m[4*1+0] = ilength2*(quat[1]*quat[2] + quat[0]*quat[3]);
rotation.m[4*1+1] = 1 - qq1 - qq3; rotation.m[4*1+1] = 1 - qq1 - qq3;
rotation.m[4*1+2] = ilength2*(quat[2]*quat[3] - quat[0]*quat[1]); rotation.m[4*1+2] = ilength2*(quat[2]*quat[3] - quat[0]*quat[1]);
skipping to change at line 878 skipping to change at line 874
/// \ingroup affine_math /// \ingroup affine_math
/// \param quat0 quaternion, (s,vx,vy,vz) /// \param quat0 quaternion, (s,vx,vy,vz)
/// \param quat1 quaternion, (s,vx,vy,vz) /// \param quat1 quaternion, (s,vx,vy,vz)
template <typename T> template <typename T>
inline RaveVector<T> quatMultiply(const RaveVector<T>& quat0, const RaveVec tor<T>& quat1) inline RaveVector<T> quatMultiply(const RaveVector<T>& quat0, const RaveVec tor<T>& quat1)
{ {
RaveVector<T> q(quat0.x*quat1.x - quat0.y*quat1.y - quat0.z*quat1.z - q uat0.w*quat1.w, RaveVector<T> q(quat0.x*quat1.x - quat0.y*quat1.y - quat0.z*quat1.z - q uat0.w*quat1.w,
quat0.x*quat1.y + quat0.y*quat1.x + quat0.z*quat1.w - q uat0.w*quat1.z, quat0.x*quat1.y + quat0.y*quat1.x + quat0.z*quat1.w - q uat0.w*quat1.z,
quat0.x*quat1.z + quat0.z*quat1.x + quat0.w*quat1.y - q uat0.y*quat1.w, quat0.x*quat1.z + quat0.z*quat1.x + quat0.w*quat1.y - q uat0.y*quat1.w,
quat0.x*quat1.w + quat0.w*quat1.x + quat0.y*quat1.z - q uat0.z*quat1.y); quat0.x*quat1.w + quat0.w*quat1.x + quat0.y*quat1.z - q uat0.z*quat1.y);
T fnorm = q.lengthsqr4(); // normalize the quaternion // do not normalize since some quaternion math (like derivatives) do no
MATH_ASSERT( fnorm > 0.99f && fnorm < 1.01f ); // catches multi-threadi t correspond to unit quaternions
ng errors
q.normalize4();
return q; return q;
} }
/// \brief Inverted a quaternion rotation. /// \brief Inverted a quaternion rotation.
/// ///
/// \ingroup affine_math /// \ingroup affine_math
/// \param quat quaternion, (s,vx,vy,vz) /// \param quat quaternion, (s,vx,vy,vz)
template <typename T> template <typename T>
inline RaveVector<T> quatInverse(const RaveVector<T>& quat) inline RaveVector<T> quatInverse(const RaveVector<T>& quat)
{ {
skipping to change at line 1008 skipping to change at line 1002
/// \brief Find the rotation theta around axis such that rot(axis,theta) * q is closest to the identity rotation /// \brief Find the rotation theta around axis such that rot(axis,theta) * q is closest to the identity rotation
/// ///
/// \ingroup affine_math /// \ingroup affine_math
/// \param[in] axis axis to minimize rotation about /// \param[in] axis axis to minimize rotation about
/// \param[in] quat input /// \param[in] quat input
/// \return The angle that minimizes the rotation along with the normalized rotation rot(axis,theta)*q /// \return The angle that minimizes the rotation along with the normalized rotation rot(axis,theta)*q
template<typename T> template<typename T>
std::pair<T, RaveVector<T> > normalizeAxisRotation(const RaveVector<T>& axi s, const RaveVector<T>& quat) std::pair<T, RaveVector<T> > normalizeAxisRotation(const RaveVector<T>& axi s, const RaveVector<T>& quat)
{ {
T axislen = MATH_SQRT(axis.lengthsqr3()); T axislen = MATH_SQRT(axis.lengthsqr3());
dReal angle = MATH_ATAN2(-quat.w*axis.z-quat.z*axis.y-quat.y*axis.x,qua T angle = MATH_ATAN2(-quat.w*axis.z-quat.z*axis.y-quat.y*axis.x,quat.x*
t.x*axislen); axislen);
dReal sinangle2 = MATH_SIN(angle)/axislen, cosangle2 = MATH_COS(angle); T sinangle2 = MATH_SIN(angle)/axislen, cosangle2 = MATH_COS(angle);
RaveVector<T> normalizingquat = RaveVector<T>(cosangle2,axis.x*sinangle 2,axis.y*sinangle2,axis.z*sinangle2); RaveVector<T> normalizingquat = RaveVector<T>(cosangle2,axis.x*sinangle 2,axis.y*sinangle2,axis.z*sinangle2);
return std::make_pair(2*angle,quatMultiply(normalizingquat,quat)); return std::make_pair(2*angle,quatMultiply(normalizingquat,quat));
} }
/// \brief Converts a quaternion into the axis-angle representation. /// \brief Converts a quaternion into the axis-angle representation.
/// ///
/// \ingroup affine_math /// \ingroup affine_math
/// \param quat quaternion, (s,vx,vy,vz) /// \param quat quaternion, (s,vx,vy,vz)
template<typename T> template<typename T>
RaveVector<T> axisAngleFromQuat(const RaveVector<T>& quat) RaveVector<T> axisAngleFromQuat(const RaveVector<T>& quat)
 End of changes. 13 change blocks. 
30 lines changed or deleted 27 lines changed or added


 iksolver.h   iksolver.h 
skipping to change at line 19 skipping to change at line 19
// //
// This program is distributed in the hope that it will be useful, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details. // GNU Lesser General Public License for more details.
// //
// You should have received a copy of the GNU Lesser General Public License // 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/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
/** \file iksolver.h /** \file iksolver.h
\brief Inverse kinematics related definitions. \brief Inverse kinematics related definitions.
Automatically included with \ref openrave.h
*/ */
#ifndef OPENRAVE_IKSOLVER_H #ifndef OPENRAVE_IKSOLVER_H
#define OPENRAVE_IKSOLVER_H #define OPENRAVE_IKSOLVER_H
namespace OpenRAVE { namespace OpenRAVE {
/// \brief Return value for the ik filter that can be optionally set on an ik solver. /// \brief Return value for the ik filter that can be optionally set on an ik solver.
enum IkFilterReturn enum IkFilterReturn
{ {
IKFR_Success = 0, ///< the ik solution is good IKFR_Success = 0, ///< the ik solution is good
IKFR_Reject = 1, ///< reject the ik solution IKFR_Reject = 1, ///< reject the ik solution
IKFR_Quit = 2, ///< the ik solution is rejected and the ik call itself should quit with failure IKFR_Quit = 2, ///< the ik solution is rejected and the ik call itself should quit with failure
}; };
/// \brief Controls what information gets validated when searching for an i nverse kinematics solution. /// \brief Controls what information gets validated when searching for an i nverse kinematics solution.
enum IkFilterOptions enum IkFilterOptions
{ {
IKFO_CheckEnvCollisions=1, ///< will check environment collisions with the robot (not checked by default) IKFO_CheckEnvCollisions=1, ///< will check environment collisions with the robot (not checked by default)
IKFO_IgnoreSelfCollisions=2, ///< will not check the self-collision of the robot (checked by default) IKFO_IgnoreSelfCollisions=2, ///< will not check the self-collision of the robot (checked by default)
IKFO_IgnoreJointLimits=4, ///< will not check the joint limits of the r obot (checked by default) IKFO_IgnoreJointLimits=4, ///< will not check the joint limits of the r obot (checked by default). This has the side effect of only returning solut ions within 360 degrees for revolute joints, even if they have a range > 36 0.
IKFO_IgnoreCustomFilters=8, ///< will not use the custom filter, even i f one is set IKFO_IgnoreCustomFilters=8, ///< will not use the custom filter, even i f one is set
IKFO_IgnoreEndEffectorCollisions=16, ///< will not check collision with the environment and the end effector links and bodies attached to the end effector links. The end effector links are defined by \ref RobotBase::Manip ulator::GetChildLinks. Use this option when \ref RobotBase::Manipulator::Ch eckEndEffectorCollision has already been called, or it is ok for the end ef fector to collide given the IK constraints. Self-collisions between the mov ing links and end effector are still checked. IKFO_IgnoreEndEffectorCollisions=16, ///< will not check collision with the environment and the end effector links and bodies attached to the end effector links. The end effector links are defined by \ref RobotBase::Manip ulator::GetChildLinks. Use this option when \ref RobotBase::Manipulator::Ch eckEndEffectorCollision has already been called, or it is ok for the end ef fector to collide given the IK constraints. Self-collisions between the mov ing links and end effector are still checked.
}; };
/** \brief <b>[interface]</b> Base class for all Inverse Kinematic solvers. <b>If not specified, method is not multi-thread safe.</b> See \ref arch_ik solver. /** \brief <b>[interface]</b> Base class for all Inverse Kinematic solvers. <b>If not specified, method is not multi-thread safe.</b> See \ref arch_ik solver.
\ingroup interfaces \ingroup interfaces
*/ */
class OPENRAVE_API IkSolverBase : public InterfaceBase class OPENRAVE_API IkSolverBase : public InterfaceBase
{ {
public: public:
 End of changes. 2 change blocks. 
1 lines changed or deleted 3 lines changed or added


 interface.h   interface.h 
skipping to change at line 19 skipping to change at line 19
// //
// This program is distributed in the hope that it will be useful, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details. // GNU Lesser General Public License for more details.
// //
// You should have received a copy of the GNU Lesser General Public License // 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/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
/** \file interface.h /** \file interface.h
\brief Base interface definition that all exported interfaces derive f rom. \brief Base interface definition that all exported interfaces derive f rom.
Automatically included with \ref openrave.h
*/ */
#ifndef OPENRAVE_INTERFACE_BASE #ifndef OPENRAVE_INTERFACE_BASE
#define OPENRAVE_INTERFACE_BASE #define OPENRAVE_INTERFACE_BASE
namespace OpenRAVE { namespace OpenRAVE {
/// serialization options for interfaces /// serialization options for interfaces
enum SerializationOptions enum SerializationOptions
{ {
SO_Kinematics = 0x01, ///< kinematics information SO_Kinematics = 0x01, ///< kinematics information
 End of changes. 1 change blocks. 
0 lines changed or deleted 2 lines changed or added


 kinbody.h   kinbody.h 
skipping to change at line 19 skipping to change at line 19
// //
// This program is distributed in the hope that it will be useful, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details. // GNU Lesser General Public License for more details.
// //
// You should have received a copy of the GNU Lesser General Public License // 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/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
/** \file kinbody.h /** \file kinbody.h
\brief Kinematics body related definitions. \brief Kinematics body related definitions.
Automatically included with \ref openrave.h
*/ */
#ifndef OPENRAVE_KINBODY_H #ifndef OPENRAVE_KINBODY_H
#define OPENRAVE_KINBODY_H #define OPENRAVE_KINBODY_H
/// declare function parser class from fparser library /// declare function parser class from fparser library
template<typename Value_t> class FunctionParserBase; template<typename Value_t> class FunctionParserBase;
namespace OpenRAVE { namespace OpenRAVE {
/** \brief <b>[interface]</b> A kinematic body of links and joints. <b>If n ot specified, method is not multi-thread safe.</b> See \ref arch_kinbody. /** \brief <b>[interface]</b> A kinematic body of links and joints. <b>If n ot specified, method is not multi-thread safe.</b> See \ref arch_kinbody.
 End of changes. 1 change blocks. 
0 lines changed or deleted 2 lines changed or added


 module.h   module.h 
skipping to change at line 19 skipping to change at line 19
// //
// This program is distributed in the hope that it will be useful, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details. // GNU Lesser General Public License for more details.
// //
// You should have received a copy of the GNU Lesser General Public License // 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/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
/** \file module.h /** \file module.h
\brief Modules containing useful routines and algorithms. \brief Modules containing useful routines and algorithms.
Automatically included with \ref openrave.h
*/ */
#ifndef OPENRAVE_COMMAND_PROBLEM_INSTANCE_H #ifndef OPENRAVE_COMMAND_PROBLEM_INSTANCE_H
#define OPENRAVE_COMMAND_PROBLEM_INSTANCE_H #define OPENRAVE_COMMAND_PROBLEM_INSTANCE_H
namespace OpenRAVE { namespace OpenRAVE {
/** \brief <b>[interface]</b> A loadable module of user code meant to solve a specific domain. <b>If not specified, method is not multi-thread safe.</ b> See \ref arch_module. /** \brief <b>[interface]</b> A loadable module of user code meant to solve a specific domain. <b>If not specified, method is not multi-thread safe.</ b> See \ref arch_module.
\ingroup interfaces \ingroup interfaces
*/ */
class OPENRAVE_API ModuleBase : public InterfaceBase class OPENRAVE_API ModuleBase : public InterfaceBase
 End of changes. 1 change blocks. 
0 lines changed or deleted 2 lines changed or added


 openrave.h   openrave.h 
skipping to change at line 23 skipping to change at line 23
// GNU Lesser General Public License for more details. // GNU Lesser General Public License for more details.
// //
// You should have received a copy of the GNU Lesser General Public License // 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/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
/** \file openrave.h /** \file openrave.h
\brief Defines the public headers that every plugin must include in or der to use openrave properly. \brief Defines the public headers that every plugin must include in or der to use openrave properly.
*/ */
#ifndef OPENRAVE_H #ifndef OPENRAVE_H
#define OPENRAVE_H #define OPENRAVE_H
#ifndef RAVE_DISABLE_ASSERT_HANDLER #ifndef OPENRAVE_DISABLE_ASSERT_HANDLER
#define BOOST_ENABLE_ASSERT_HANDLER #define BOOST_ENABLE_ASSERT_HANDLER
#endif #endif
#include <cstdio> #include <cstdio>
#include <stdarg.h> #include <stdarg.h>
#include <cstring> #include <cstring>
#include <cstdlib> #include <cstdlib>
#include <stdint.h> #include <stdint.h>
skipping to change at line 102 skipping to change at line 102
#if OPENRAVE_PRECISION // 1 if double precision #if OPENRAVE_PRECISION // 1 if double precision
typedef double dReal; typedef double dReal;
#define g_fEpsilon 1e-15 #define g_fEpsilon 1e-15
#else #else
typedef float dReal; typedef float dReal;
#define g_fEpsilon 2e-7f #define g_fEpsilon 2e-7f
#endif #endif
/// \brief openrave constant for PI, could be replaced by accurate precisio n number depending on choice of dReal. /// \brief openrave constant for PI, could be replaced by accurate precisio n number depending on choice of dReal.
static const dReal PI = (dReal)3.14159265358979323846; static const dReal PI = dReal(3.14159265358979323846);
/// Wrappers of common basic math functions, allows OpenRAVE to control the precision requirements. /// Wrappers of common basic math functions, allows OpenRAVE to control the precision requirements.
/// \ingroup affine_math /// \ingroup affine_math
//@{ //@{
/// \brief exponential /// \brief exponential
OPENRAVE_API dReal RaveExp(dReal f); OPENRAVE_API dReal RaveExp(dReal f);
/// \brief logarithm /// \brief logarithm
OPENRAVE_API dReal RaveLog(dReal f); OPENRAVE_API dReal RaveLog(dReal f);
/// \brief cosine /// \brief cosine
skipping to change at line 140 skipping to change at line 140
/// \brief square-root /// \brief square-root
OPENRAVE_API dReal RaveSqrt(dReal f); OPENRAVE_API dReal RaveSqrt(dReal f);
/// \brief absolute value /// \brief absolute value
OPENRAVE_API dReal RaveFabs(dReal f); OPENRAVE_API dReal RaveFabs(dReal f);
//@} //@}
/// %OpenRAVE error codes /// %OpenRAVE error codes
enum OpenRAVEErrorCode { enum OpenRAVEErrorCode {
ORE_Failed=0, ORE_Failed=0,
ORE_InvalidArguments=1, ORE_InvalidArguments=1, ///< passed in input arguments are not valid
ORE_EnvironmentNotLocked=2, ORE_EnvironmentNotLocked=2,
ORE_CommandNotSupported=3, ///< string command could not be parsed or i s not supported ORE_CommandNotSupported=3, ///< string command could not be parsed or i s not supported
ORE_Assert=4, ORE_Assert=4,
ORE_InvalidPlugin=5, ///< shared object is not a valid plugin ORE_InvalidPlugin=5, ///< shared object is not a valid plugin
ORE_InvalidInterfaceHash=6, ///< interface hashes do not match between plugins ORE_InvalidInterfaceHash=6, ///< interface hashes do not match between plugins
ORE_NotImplemented=7, ///< function is not implemented by the interface . ORE_NotImplemented=7, ///< function is not implemented by the interface .
ORE_InconsistentConstraints=8, ///< return solutions or trajectories do ORE_InconsistentConstraints=8, ///< returned solutions or trajectories
not follow the constraints of the planner/module do not follow the constraints of the planner/module. The constraints invali
dated 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 wit
h its parameters, or cannot be used. This is usually due to a programming e
rror where a vector is not the correct length, etc.
}; };
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";
}
// should throw an exception?
return "";
}
/// \brief Exception that all OpenRAVE internal methods throw; the error co des are held in \ref OpenRAVEErrorCode. /// \brief Exception that all OpenRAVE internal methods throw; the error co des are held in \ref OpenRAVEErrorCode.
class OPENRAVE_API openrave_exception : public std::exception class OPENRAVE_API openrave_exception : public std::exception
{ {
public: public:
openrave_exception() : std::exception(), _s("unknown exception"), _erro r(ORE_Failed) { openrave_exception() : std::exception(), _s("unknown exception"), _erro r(ORE_Failed) {
} }
openrave_exception(const std::string& s, OpenRAVEErrorCode error=ORE_Fa iled) : std::exception() { openrave_exception(const std::string& s, OpenRAVEErrorCode error=ORE_Fa iled) : std::exception() {
_error = error; _error = error;
_s = "openrave ("; _s = "openrave (";
switch(_error) { _s += GetErrorCodeString(_error);
case ORE_Failed: _s += "Failed"; break;
case ORE_InvalidArguments: _s += "InvalidArguments"; break;
case ORE_EnvironmentNotLocked: _s += "EnvironmentNotLocked"; break;
case ORE_CommandNotSupported: _s += "CommandNotSupported"; break;
case ORE_Assert: _s += "Assert"; break;
case ORE_InvalidPlugin: _s += "InvalidPlugin"; break;
case ORE_InvalidInterfaceHash: _s += "InvalidInterfaceHash"; break;
case ORE_NotImplemented: _s += "NotImplemented"; break;
default:
_s += boost::str(boost::format("%8.8x")%static_cast<int>(_error
));
break;
}
_s += "): "; _s += "): ";
_s += s; _s += s;
} }
virtual ~openrave_exception() throw() { virtual ~openrave_exception() throw() {
} }
char const* what() const throw() { char const* what() const throw() {
return _s.c_str(); return _s.c_str();
} }
const std::string& message() const { const std::string& message() const {
return _s; return _s;
skipping to change at line 228 skipping to change at line 237
/// \brief base class for all user data /// \brief base class for all user data
class OPENRAVE_API UserData class OPENRAVE_API UserData
{ {
public: public:
virtual ~UserData() { virtual ~UserData() {
} }
}; };
typedef boost::shared_ptr<UserData> UserDataPtr; typedef boost::shared_ptr<UserData> UserDataPtr;
typedef boost::weak_ptr<UserData> UserDataWeakPtr; 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 // terminal attributes
//#define RESET 0 //#define RESET 0
//#define BRIGHT 1 //#define BRIGHT 1
//#define DIM 2 //#define DIM 2
//#define UNDERLINE 3 //#define UNDERLINE 3
//#define BLINK 4 //#define BLINK 4
//#define REVERSE 7 //#define REVERSE 7
//#define HIDDEN 8 //#define HIDDEN 8
// terminal colors // terminal colors
//#define BLACK 0 //#define BLACK 0
skipping to change at line 511 skipping to change at line 536
DefineRavePrintfA(_ERRORLEVEL) DefineRavePrintfA(_ERRORLEVEL)
DefineRavePrintfA(_WARNLEVEL) DefineRavePrintfA(_WARNLEVEL)
//DefineRavePrintfA(_INFOLEVEL) //DefineRavePrintfA(_INFOLEVEL)
DefineRavePrintfA(_DEBUGLEVEL) DefineRavePrintfA(_DEBUGLEVEL)
DefineRavePrintfA(_VERBOSELEVEL) DefineRavePrintfA(_VERBOSELEVEL)
#define RAVEPRINTHEADER(LEVEL) OpenRAVE::RavePrintfA ## LEVEL("[%s:%d] ", O penRAVE::RaveGetSourceFilename(__FILE__), __LINE__) #define RAVEPRINTHEADER(LEVEL) OpenRAVE::RavePrintfA ## LEVEL("[%s:%d] ", O penRAVE::RaveGetSourceFilename(__FILE__), __LINE__)
// different logging levels. The higher the suffix number, the less importa nt the information is. // different logging levels. The higher the suffix number, the less importa nt the information is.
// 0 log level logs all the time. OpenRAVE starts up with a log level of 0. // 0 log level logs all the time. OpenRAVE starts up with a log level of 0.
#define RAVELOG_LEVELW(LEVEL,level) (OpenRAVE::RaveGetDebugLevel()&OpenRAVE #define RAVELOG_LEVELW(LEVEL,level) int(OpenRAVE::RaveGetDebugLevel()&OpenR
::Level_OutputMask)>=(level)&&(RAVEPRINTHEADER(LEVEL)>0)&&OpenRAVE::RavePri AVE::Level_OutputMask)>=int(level)&&(RAVEPRINTHEADER(LEVEL)>0)&&OpenRAVE::R
ntfW ## LEVEL avePrintfW ## LEVEL
#define RAVELOG_LEVELA(LEVEL,level) (OpenRAVE::RaveGetDebugLevel()&OpenRAVE #define RAVELOG_LEVELA(LEVEL,level) int(OpenRAVE::RaveGetDebugLevel()&OpenR
::Level_OutputMask)>=(level)&&(RAVEPRINTHEADER(LEVEL)>0)&&OpenRAVE::RavePri AVE::Level_OutputMask)>=int(level)&&(RAVEPRINTHEADER(LEVEL)>0)&&OpenRAVE::R
ntfA ## LEVEL avePrintfA ## LEVEL
// define log4cxx equivalents (eventually OpenRAVE will move to log4cxx log ging) // define log4cxx equivalents (eventually OpenRAVE will move to log4cxx log ging)
#define RAVELOG_FATALW RAVELOG_LEVELW(_FATALLEVEL,OpenRAVE::Level_Fatal) #define RAVELOG_FATALW RAVELOG_LEVELW(_FATALLEVEL,OpenRAVE::Level_Fatal)
#define RAVELOG_FATALA RAVELOG_LEVELA(_FATALLEVEL,OpenRAVE::Level_Fatal) #define RAVELOG_FATALA RAVELOG_LEVELA(_FATALLEVEL,OpenRAVE::Level_Fatal)
#define RAVELOG_FATAL RAVELOG_FATALA #define RAVELOG_FATAL RAVELOG_FATALA
#define RAVELOG_ERRORW RAVELOG_LEVELW(_ERRORLEVEL,OpenRAVE::Level_Error) #define RAVELOG_ERRORW RAVELOG_LEVELW(_ERRORLEVEL,OpenRAVE::Level_Error)
#define RAVELOG_ERRORA RAVELOG_LEVELA(_ERRORLEVEL,OpenRAVE::Level_Error) #define RAVELOG_ERRORA RAVELOG_LEVELA(_ERRORLEVEL,OpenRAVE::Level_Error)
#define RAVELOG_ERROR RAVELOG_ERRORA #define RAVELOG_ERROR RAVELOG_ERRORA
#define RAVELOG_WARNW RAVELOG_LEVELW(_WARNLEVEL,OpenRAVE::Level_Warn) #define RAVELOG_WARNW RAVELOG_LEVELW(_WARNLEVEL,OpenRAVE::Level_Warn)
#define RAVELOG_WARNA RAVELOG_LEVELA(_WARNLEVEL,OpenRAVE::Level_Warn) #define RAVELOG_WARNA RAVELOG_LEVELA(_WARNLEVEL,OpenRAVE::Level_Warn)
skipping to change at line 541 skipping to change at line 566
#define RAVELOG_VERBOSEA RAVELOG_LEVELA(_VERBOSELEVEL,OpenRAVE::Level_Verbo se) #define RAVELOG_VERBOSEA RAVELOG_LEVELA(_VERBOSELEVEL,OpenRAVE::Level_Verbo se)
#define RAVELOG_VERBOSE RAVELOG_VERBOSEA #define RAVELOG_VERBOSE RAVELOG_VERBOSEA
#define IS_DEBUGLEVEL(level) ((OpenRAVE::RaveGetDebugLevel()&OpenRAVE::Leve l_OutputMask)>=(level)) #define IS_DEBUGLEVEL(level) ((OpenRAVE::RaveGetDebugLevel()&OpenRAVE::Leve l_OutputMask)>=(level))
#define OPENRAVE_EXCEPTION_FORMAT0(s, errorcode) OpenRAVE::openrave_excepti on(boost::str(boost::format("[%s:%d] " s)%(__PRETTY_FUNCTION__)%(__LINE__)) ,errorcode) #define OPENRAVE_EXCEPTION_FORMAT0(s, errorcode) OpenRAVE::openrave_excepti on(boost::str(boost::format("[%s:%d] " s)%(__PRETTY_FUNCTION__)%(__LINE__)) ,errorcode)
/// adds the function name and line number to an openrave exception /// adds the function name and line number to an openrave exception
#define OPENRAVE_EXCEPTION_FORMAT(s, args,errorcode) OpenRAVE::openrave_exc eption(boost::str(boost::format("[%s:%d] " s)%(__PRETTY_FUNCTION__)%(__LINE __)%args),errorcode) #define OPENRAVE_EXCEPTION_FORMAT(s, args,errorcode) OpenRAVE::openrave_exc eption(boost::str(boost::format("[%s:%d] " s)%(__PRETTY_FUNCTION__)%(__LINE __)%args),errorcode)
#define OPENRAVE_ASSERT_FORMAT(testexpr, s, args, errorcode) { if( !(testex
pr) ) { throw OpenRAVE::openrave_exception(boost::str(boost::format("[%s:%d
] (%s) failed " s)%(__PRETTY_FUNCTION__)%(__LINE__)%(# testexpr)%args),erro
rcode); } }
#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); }
}
// 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(boo
st::format("[%s:%d] %s %s %s, (eval %s %s %s) " s)%(__PRETTY_FUNCTION__)%(_
_LINE__)%(# expr1)%(# op)%(# expr2)%(expr1)%(# op)%(expr2)%args),errorcode)
; } }
#define OPENRAVE_ASSERT_OP_FORMAT0(expr1,op,expr2,s, errorcode) { if( !((ex
pr1) op (expr2)) ) { throw OpenRAVE::openrave_exception(boost::str(boost::f
ormat("[%s:%d] %s %s %s, (eval %s %s %s) " s)%(__PRETTY_FUNCTION__)%(__LINE
__)%(# expr1)%(# op)%(# expr2)%(expr1)%(# op)%(expr2)),errorcode); } }
#define OPENRAVE_ASSERT_OP(expr1,op,expr2) { if( !((expr1) op (expr2)) ) {
throw OpenRAVE::openrave_exception(boost::str(boost::format("[%s:%d] %s!=%s
, (eval %s!=%s) ")%(__PRETTY_FUNCTION__)%(__LINE__)%(# expr1)%(# op)%(# exp
r2)%(expr1)%(# op)%(expr2)),ORE_Assert); } }
#define OPENRAVE_DUMMY_IMPLEMENTATION { throw OPENRAVE_EXCEPTION_FORMAT0("n ot implemented",ORE_NotImplemented); } #define OPENRAVE_DUMMY_IMPLEMENTATION { throw OPENRAVE_EXCEPTION_FORMAT0("n ot implemented",ORE_NotImplemented); }
/// \brief Enumeration of all the interfaces. /// \brief Enumeration of all the interfaces.
enum InterfaceType enum InterfaceType
{ {
PT_Planner=1, ///< describes \ref PlannerBase interface PT_Planner=1, ///< describes \ref PlannerBase interface
PT_Robot=2, ///< describes \ref RobotBase interface PT_Robot=2, ///< describes \ref RobotBase interface
PT_SensorSystem=3, ///< describes \ref SensorSystemBase interface PT_SensorSystem=3, ///< describes \ref SensorSystemBase interface
PT_Controller=4, ///< describes \ref ControllerBase interface PT_Controller=4, ///< describes \ref ControllerBase interface
PT_Module=5, ///< describes \ref ModuleBase interface PT_Module=5, ///< describes \ref ModuleBase interface
skipping to change at line 981 skipping to change at line 1017
virtual int AddDeltaTimeGroup(); virtual int AddDeltaTimeGroup();
/** \brief Adds a new group to the specification and returns its new of fset. /** \brief Adds a new group to the specification and returns its new of fset.
If the new group's semantic name does not exist in the current spec ification, adds it and returns the new offset. If the new group's semantic name does not exist in the current spec ification, adds it and returns the new offset.
If the new group's semantic name exists in the current specificatio n and it exactly matches, then function returns the old group's index. If t he semantic names match, but parameters do not match, then an openrave_exce ption is thrown. If the new group's semantic name exists in the current specificatio n and it exactly matches, then function returns the old group's index. If t he semantic names match, but parameters do not match, then an openrave_exce ption is thrown.
This method is not responsible for merging semantic information This method is not responsible for merging semantic information
*/ */
virtual int AddGroup(const std::string& name, int dof, const std::strin g& interpolation = ""); virtual int AddGroup(const std::string& name, int dof, const std::strin g& interpolation = "");
/** \brief Adds a new group to the specification and returns its new of
fset.
\param g the group whose name, dof, and interpolation are extracted
.
If the new group's semantic name does not exist in the current spec
ification, adds it and returns the new offset.
If the new group's semantic name exists in the current specificatio
n and it exactly matches, then function returns the old group's index. If t
he semantic names match, but parameters do not match, then an openrave_exce
ption is thrown.
This method is not responsible for merging semantic information
*/
virtual int AddGroup(const Group& g);
/** \brief Merges all the information from the input group into this gr oup /** \brief Merges all the information from the input group into this gr oup
For groups that are merged, the interpolation is overwritten if the source group has an empty string. For groups that are merged, the interpolation is overwritten if the source group has an empty string.
\throw openrave_exception throws if groups do not contain enough in formation to be merged or interpolations do not match. \throw openrave_exception throws if groups do not contain enough in formation to be merged or interpolations do not match.
*/ */
virtual ConfigurationSpecification& operator+= (const ConfigurationSpec ification& r); virtual ConfigurationSpecification& operator+= (const ConfigurationSpec ification& r);
/** \brief Return a new specification that holds the merged information from the current and input specification and the input parameter.. /** \brief Return a new specification that holds the merged information from the current and input specification and the input parameter..
For groups that are merged, the interpolation either has to match f or both groups, or one of the groups needs an empty interpolation. For groups that are merged, the interpolation either has to match f or both groups, or one of the groups needs an empty interpolation.
skipping to change at line 1068 skipping to change at line 1095
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; 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;
/** \brief sets the deltatime field of the data if one exists /** \brief sets the deltatime field of the data if one exists
\param[inout] itdata data in the format of this configuration speci fication. \param[inout] itdata data in the format of this configuration speci fication.
\param[in] deltatime the delta time of the time stamp (from previou s point) \param[in] deltatime the delta time of the time stamp (from previou s point)
\return true if at least one group was found for inserting \return true if at least one group was found for inserting
*/ */
virtual bool InsertDeltaTime(std::vector<dReal>::iterator itdata, dReal deltatime) const; virtual bool InsertDeltaTime(std::vector<dReal>::iterator itdata, dReal deltatime) const;
/** \brief Adds a new group to the specification and returns its new of
fset.
\param g the group whose name, dof, and interpolation are extracted
.
If the new group's semantic name does not exist in the current spec
ification, adds it and returns the new offset.
If the new group's semantic name exists in the current specificatio
n and it exactly matches, then function returns the old group's index. If t
he semantic names match, but parameters do not match, then an openrave_exce
ption is thrown.
This method is not responsible for merging semantic information
*/
virtual int AddGroup(const Group& g);
/** \brief given two compatible groups, convers data represented in the source group to data represented in the target group /** \brief given two compatible groups, convers data represented in the source group to data represented in the target group
\param ittargetdata iterator pointing to start of target group data that should be overwritten \param ittargetdata iterator pointing to start of target group data that should be overwritten
\param targetstride the number of elements that to go from the next target point. Necessary if numpoints > 1. \param targetstride the number of elements that to go from the next target point. Necessary if numpoints > 1.
\param gtarget the target configuration group \param gtarget the target configuration group
\param itsourcedata iterator pointing to start of source group data that should be read \param itsourcedata iterator pointing to start of source group data that should be read
\param sourcestride the number of elements that to go from the next source point. Necessary if numpoints > 1. \param sourcestride the number of elements that to go from the next source point. Necessary if numpoints > 1.
\param gsource the source configuration group \param gsource the source configuration group
\param numpoints the number of points to convert. The target and so urce strides are gtarget.dof and gsource.dof \param numpoints the number of points to convert. The target and so urce strides are gtarget.dof and gsource.dof
\param penv [optional] The environment which might be needed to fil l in unknown data. Assumes environment is locked. \param penv [optional] The environment which might be needed to fil l in unknown data. Assumes environment is locked.
skipping to change at line 1109 skipping to change at line 1145
std::vector<Group> _vgroups; std::vector<Group> _vgroups;
}; };
OPENRAVE_API std::ostream& operator<<(std::ostream& O, const ConfigurationS pecification &spec); OPENRAVE_API std::ostream& operator<<(std::ostream& O, const ConfigurationS pecification &spec);
OPENRAVE_API std::istream& operator>>(std::istream& I, ConfigurationSpecifi cation& spec); OPENRAVE_API std::istream& operator>>(std::istream& I, ConfigurationSpecifi cation& spec);
typedef boost::shared_ptr<ConfigurationSpecification> ConfigurationSpecific ationPtr; typedef boost::shared_ptr<ConfigurationSpecification> ConfigurationSpecific ationPtr;
typedef boost::shared_ptr<ConfigurationSpecification const> ConfigurationSp ecificationConstPtr; typedef boost::shared_ptr<ConfigurationSpecification const> ConfigurationSp ecificationConstPtr;
template <typename T>
inline T NormalizeCircularAnglePrivate(T theta, T min, T max)
{
if (theta < min) {
T range = max-min;
theta += range;
while (theta < min) {
theta += range;
}
}
else if (theta > max) {
T range = max-min;
theta -= range;
while (theta > max) {
theta -= range;
}
}
return theta;
}
/** \brief Parameterization of basic primitives for querying inverse-kinema tics solutions. /** \brief Parameterization of basic primitives for querying inverse-kinema tics solutions.
Holds the parameterization of a geometric primitive useful for autonomo us manipulation scenarios like: Holds the parameterization of a geometric primitive useful for autonomo us manipulation scenarios like:
6D pose, 3D translation, 3D rotation, 3D look at direction, and ray loo k at direction. 6D pose, 3D translation, 3D rotation, 3D look at direction, and ray loo k at direction.
*/ */
class OPENRAVE_API IkParameterization class OPENRAVE_API IkParameterization
{ {
public: public:
/// \deprecated (11/10/12) /// \deprecated (11/10/12)
typedef IkParameterizationType Type RAVE_DEPRECATED; typedef IkParameterizationType Type RAVE_DEPRECATED;
skipping to change at line 1411 skipping to change at line 1467
anglediff -= dReal(2*PI); anglediff -= dReal(2*PI);
} }
return (v0-v1).lengthsqr2() + anglemult*anglediff*anglediff; return (v0-v1).lengthsqr2() + anglemult*anglediff*anglediff;
} }
case IKP_TranslationLocalGlobal6D: { case IKP_TranslationLocalGlobal6D: {
std::pair<Vector,Vector> p0 = GetTranslationLocalGlobal6D(), p1 = ikparam.GetTranslationLocalGlobal6D(); std::pair<Vector,Vector> p0 = GetTranslationLocalGlobal6D(), p1 = ikparam.GetTranslationLocalGlobal6D();
return (p0.first-p1.first).lengthsqr3() + (p0.second-p1.second) .lengthsqr3(); return (p0.first-p1.first).lengthsqr3() + (p0.second-p1.second) .lengthsqr3();
} }
case IKP_TranslationXAxisAngle4D: { case IKP_TranslationXAxisAngle4D: {
std::pair<Vector,dReal> p0 = GetTranslationXAxisAngle4D(), p1 = ikparam.GetTranslationXAxisAngle4D(); std::pair<Vector,dReal> p0 = GetTranslationXAxisAngle4D(), p1 = ikparam.GetTranslationXAxisAngle4D();
return (p0.first-p1.first).lengthsqr3() + (p0.second-p1.second) // dot product with axis is always in [0,pi]
*(p0.second-p1.second); dReal angle0 = RaveFabs(NormalizeCircularAnglePrivate(p0.second
, -PI, PI));
dReal angle1 = RaveFabs(NormalizeCircularAnglePrivate(p1.second
, -PI, PI));
return (p0.first-p1.first).lengthsqr3() + (angle0-angle1)*(angl
e0-angle1);
} }
case IKP_TranslationYAxisAngle4D: { case IKP_TranslationYAxisAngle4D: {
std::pair<Vector,dReal> p0 = GetTranslationYAxisAngle4D(), p1 = ikparam.GetTranslationYAxisAngle4D(); std::pair<Vector,dReal> p0 = GetTranslationYAxisAngle4D(), p1 = ikparam.GetTranslationYAxisAngle4D();
return (p0.first-p1.first).lengthsqr3() + (p0.second-p1.second) // dot product with axis is always in [0,pi]
*(p0.second-p1.second); dReal angle0 = RaveFabs(NormalizeCircularAnglePrivate(p0.second
, -PI, PI));
dReal angle1 = RaveFabs(NormalizeCircularAnglePrivate(p1.second
, -PI, PI));
return (p0.first-p1.first).lengthsqr3() + (angle0-angle1)*(angl
e0-angle1);
} }
case IKP_TranslationZAxisAngle4D: { case IKP_TranslationZAxisAngle4D: {
std::pair<Vector,dReal> p0 = GetTranslationZAxisAngle4D(), p1 = ikparam.GetTranslationZAxisAngle4D(); std::pair<Vector,dReal> p0 = GetTranslationZAxisAngle4D(), p1 = ikparam.GetTranslationZAxisAngle4D();
return (p0.first-p1.first).lengthsqr3() + (p0.second-p1.second) // dot product with axis is always in [0,pi]
*(p0.second-p1.second); dReal angle0 = RaveFabs(NormalizeCircularAnglePrivate(p0.second
, -PI, PI));
dReal angle1 = RaveFabs(NormalizeCircularAnglePrivate(p1.second
, -PI, PI));
return (p0.first-p1.first).lengthsqr3() + (angle0-angle1)*(angl
e0-angle1);
} }
case IKP_TranslationXAxisAngleZNorm4D: { case IKP_TranslationXAxisAngleZNorm4D: {
std::pair<Vector,dReal> p0 = GetTranslationXAxisAngleZNorm4D(), p1 = ikparam.GetTranslationXAxisAngleZNorm4D(); std::pair<Vector,dReal> p0 = GetTranslationXAxisAngleZNorm4D(), p1 = ikparam.GetTranslationXAxisAngleZNorm4D();
return (p0.first-p1.first).lengthsqr3() + (p0.second-p1.second) dReal anglediff = NormalizeCircularAnglePrivate(p0.second-p1.se
*(p0.second-p1.second); cond, -PI, PI);
return (p0.first-p1.first).lengthsqr3() + anglediff*anglediff;
} }
case IKP_TranslationYAxisAngleXNorm4D: { case IKP_TranslationYAxisAngleXNorm4D: {
std::pair<Vector,dReal> p0 = GetTranslationYAxisAngleXNorm4D(), p1 = ikparam.GetTranslationYAxisAngleXNorm4D(); std::pair<Vector,dReal> p0 = GetTranslationYAxisAngleXNorm4D(), p1 = ikparam.GetTranslationYAxisAngleXNorm4D();
return (p0.first-p1.first).lengthsqr3() + (p0.second-p1.second) dReal anglediff = NormalizeCircularAnglePrivate(p0.second-p1.se
*(p0.second-p1.second); cond, -PI, PI);
return (p0.first-p1.first).lengthsqr3() + anglediff*anglediff;
} }
case IKP_TranslationZAxisAngleYNorm4D: { case IKP_TranslationZAxisAngleYNorm4D: {
std::pair<Vector,dReal> p0 = GetTranslationZAxisAngleYNorm4D(), p1 = ikparam.GetTranslationZAxisAngleYNorm4D(); std::pair<Vector,dReal> p0 = GetTranslationZAxisAngleYNorm4D(), p1 = ikparam.GetTranslationZAxisAngleYNorm4D();
return (p0.first-p1.first).lengthsqr3() + (p0.second-p1.second) dReal anglediff = NormalizeCircularAnglePrivate(p0.second-p1.se
*(p0.second-p1.second); cond, -PI, PI);
return (p0.first-p1.first).lengthsqr3() + anglediff*anglediff;
} }
default: default:
BOOST_ASSERT(0); BOOST_ASSERT(0);
} }
return 1e30; return 1e30;
} }
/// \brief fills the iterator with the serialized values of the ikparam eterization. /// \brief fills the iterator with the serialized values of the ikparam eterization.
/// ///
/// the container the iterator points to needs to have \ref GetNumberOf Values() available. /// the container the iterator points to needs to have \ref GetNumberOf Values() available.
skipping to change at line 1936 skipping to change at line 2004
OPENRAVE_API IkSolverBasePtr RaveCreateIkSolver(EnvironmentBasePtr penv, co nst std::string& name); OPENRAVE_API IkSolverBasePtr RaveCreateIkSolver(EnvironmentBasePtr penv, co nst std::string& name);
OPENRAVE_API PhysicsEngineBasePtr RaveCreatePhysicsEngine(EnvironmentBasePt r penv, const std::string& name); OPENRAVE_API PhysicsEngineBasePtr RaveCreatePhysicsEngine(EnvironmentBasePt r penv, const std::string& name);
OPENRAVE_API SensorBasePtr RaveCreateSensor(EnvironmentBasePtr penv, const std::string& name); OPENRAVE_API SensorBasePtr RaveCreateSensor(EnvironmentBasePtr penv, const std::string& name);
OPENRAVE_API CollisionCheckerBasePtr RaveCreateCollisionChecker(Environment BasePtr penv, const std::string& name); OPENRAVE_API CollisionCheckerBasePtr RaveCreateCollisionChecker(Environment BasePtr penv, const std::string& name);
OPENRAVE_API ViewerBasePtr RaveCreateViewer(EnvironmentBasePtr penv, const std::string& name); OPENRAVE_API ViewerBasePtr RaveCreateViewer(EnvironmentBasePtr penv, const std::string& name);
OPENRAVE_API SpaceSamplerBasePtr RaveCreateSpaceSampler(EnvironmentBasePtr penv, const std::string& name); OPENRAVE_API SpaceSamplerBasePtr RaveCreateSpaceSampler(EnvironmentBasePtr penv, const std::string& name);
OPENRAVE_API KinBodyPtr RaveCreateKinBody(EnvironmentBasePtr penv, const st d::string& name=""); OPENRAVE_API KinBodyPtr RaveCreateKinBody(EnvironmentBasePtr penv, const st d::string& name="");
/// \brief Return an empty trajectory instance. /// \brief Return an empty trajectory instance.
OPENRAVE_API TrajectoryBasePtr RaveCreateTrajectory(EnvironmentBasePtr penv , const std::string& name=""); OPENRAVE_API TrajectoryBasePtr RaveCreateTrajectory(EnvironmentBasePtr penv , const std::string& name="");
/// \deprecated (11/10/01)
OPENRAVE_API TrajectoryBasePtr RaveCreateTrajectory(EnvironmentBasePtr penv , int dof) RAVE_DEPRECATED; OPENRAVE_API TrajectoryBasePtr RaveCreateTrajectory(EnvironmentBasePtr penv , int dof) RAVE_DEPRECATED;
/// \brief returned a fully cloned interface
template <typename T>
inline boost::shared_ptr<T> RaveClone(boost::shared_ptr<T const> preference
, int cloningoptions)
{
InterfaceBasePtr pcloned = RaveCreateInterface(preference->GetEnv(), pr
eference->GetInterfaceType(), preference->GetXMLId());
OPENRAVE_ASSERT_FORMAT(!!pcloned, "Failed to clone interface=%s id=%s",
RaveGetInterfaceName(preference->GetInterfaceType())%preference->GetXMLId(
), ORE_InvalidArguments);
boost::shared_ptr<T> pclonedcast = boost::dynamic_pointer_cast<T>(pclon
ed);
OPENRAVE_ASSERT_FORMAT(!!pclonedcast, "Interface created but failed to
cast interface=%s id=%s", RaveGetInterfaceName(preference->GetInterfaceType
())%preference->GetXMLId(), ORE_InvalidArguments);
pclonedcast->Clone(preference,cloningoptions);
return pclonedcast;
}
/** \brief Registers a function to create an interface, this allows the int erface to be created by other modules. /** \brief Registers a function to create an interface, this allows the int erface to be created by other modules.
\param type interface type \param type interface type
\param name interface name \param name interface name
\param interfacehash the hash of the interface being created (use the g lobal defines OPENRAVE_X_HASH) \param interfacehash the hash of the interface being created (use the g lobal defines OPENRAVE_X_HASH)
\param envhash the hash of the environment (use the global define OPENR AVE_ENVIRONMENT_HASH) \param envhash the hash of the environment (use the global define OPENR AVE_ENVIRONMENT_HASH)
\param createfn functions to create the interface it takes two paramete rs: the environment and an istream to the rest of the interface creation ar guments. \param createfn functions to create the interface it takes two paramete rs: the environment and an istream to the rest of the interface creation ar guments.
\return a handle if function is successfully registered. By destroying the handle, the interface will be automatically unregistered. \return a handle if function is successfully registered. By destroying the handle, the interface will be automatically unregistered.
\throw openrave_exception Will throw with ORE_InvalidInterfaceHash if h ashes do not match \throw openrave_exception Will throw with ORE_InvalidInterfaceHash if h ashes do not match
*/ */
skipping to change at line 1986 skipping to change at line 2067
/// \deprecated (11/06/03), use \ref SpaceSamplerBase /// \deprecated (11/06/03), use \ref SpaceSamplerBase
OPENRAVE_API void RaveInitRandomGeneration(uint32_t seed); OPENRAVE_API void RaveInitRandomGeneration(uint32_t seed);
/// \deprecated (11/06/03), use \ref SpaceSamplerBase /// \deprecated (11/06/03), use \ref SpaceSamplerBase
OPENRAVE_API uint32_t RaveRandomInt(); OPENRAVE_API uint32_t RaveRandomInt();
/// \deprecated (11/06/03), use \ref SpaceSamplerBase /// \deprecated (11/06/03), use \ref SpaceSamplerBase
OPENRAVE_API float RaveRandomFloat(IntervalType interval=IT_Closed); OPENRAVE_API float RaveRandomFloat(IntervalType interval=IT_Closed);
/// \deprecated (11/06/03), use \ref SpaceSamplerBase /// \deprecated (11/06/03), use \ref SpaceSamplerBase
OPENRAVE_API double RaveRandomDouble(IntervalType interval=IT_Closed); OPENRAVE_API double RaveRandomDouble(IntervalType interval=IT_Closed);
/// \brief separates the directories from a string and returns them in a ve /// \deprecated (12/02/06) see \ref OpenRAVE::utils::TokenizeString
ctor bool RaveParseDirectories(const char* pdirs, std::vector<std::string>& vdir
s) RAVE_DEPRECATED;
inline bool RaveParseDirectories(const char* pdirs, std::vector<std::string >& vdirs) inline bool RaveParseDirectories(const char* pdirs, std::vector<std::string >& vdirs)
{ {
vdirs.resize(0); vdirs.resize(0);
if( !pdirs ) { if( !pdirs ) {
return false; return false;
} }
// search for all directories separated by ':' // search for all directories separated by ':'
std::string tmp = pdirs; std::string tmp = pdirs;
std::string::size_type pos = 0, newpos=0; std::string::size_type pos = 0, newpos=0;
while( pos < tmp.size() ) { while( pos < tmp.size() ) {
skipping to change at line 2042 skipping to change at line 2125
{ {
std::map<IkParameterizationType,std::string>::const_iterator it = RaveG etIkParameterizationMap().find(_type); std::map<IkParameterizationType,std::string>::const_iterator it = RaveG etIkParameterizationMap().find(_type);
if( it != RaveGetIkParameterizationMap().end() ) { if( it != RaveGetIkParameterizationMap().end() ) {
return it->second; return it->second;
} }
throw openrave_exception(str(boost::format("IkParameterization iktype 0 x%x not supported"))); throw openrave_exception(str(boost::format("IkParameterization iktype 0 x%x not supported")));
} }
} // end namespace OpenRAVE } // end namespace OpenRAVE
#if !defined(RAVE_DISABLE_ASSERT_HANDLER) && defined(BOOST_ENABLE_ASSERT_HA NDLER) #if !defined(OPENRAVE_DISABLE_ASSERT_HANDLER) && defined(BOOST_ENABLE_ASSER T_HANDLER)
/// Modifications controlling %boost library behavior. /// Modifications controlling %boost library behavior.
namespace boost namespace boost
{ {
inline void assertion_failed(char const * expr, char const * function, char const * file, long line) inline void assertion_failed(char const * expr, char const * function, char const * file, long line)
{ {
throw OpenRAVE::openrave_exception(boost::str(boost::format("[%s:%d] -> %s, expr: %s")%file%line%function%expr),OpenRAVE::ORE_Assert); throw OpenRAVE::openrave_exception(boost::str(boost::format("[%s:%d] -> %s, expr: %s")%file%line%function%expr),OpenRAVE::ORE_Assert);
} }
} }
#endif #endif
 End of changes. 22 change blocks. 
55 lines changed or deleted 170 lines changed or added


 physicsengine.h   physicsengine.h 
skipping to change at line 19 skipping to change at line 19
// //
// This program is distributed in the hope that it will be useful, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details. // GNU Lesser General Public License for more details.
// //
// You should have received a copy of the GNU Lesser General Public License // 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/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
/** \file physicsengine.h /** \file physicsengine.h
\brief Physics engine related definitions. \brief Physics engine related definitions.
Automatically included with \ref openrave.h
*/ */
#ifndef OPENRAVE_PHYSICSENGINE_H #ifndef OPENRAVE_PHYSICSENGINE_H
#define OPENRAVE_PHYSICSENGINE_H #define OPENRAVE_PHYSICSENGINE_H
namespace OpenRAVE { namespace OpenRAVE {
/// basic options for physics engine /// basic options for physics engine
enum PhysicsEngineOptions enum PhysicsEngineOptions
{ {
PEO_SelfCollisions = 1, ///< if set, physics engine will use contact fo rces from self-collisions PEO_SelfCollisions = 1, ///< if set, physics engine will use contact fo rces from self-collisions
 End of changes. 1 change blocks. 
0 lines changed or deleted 2 lines changed or added


 planner.h   planner.h 
skipping to change at line 19 skipping to change at line 19
// //
// This program is distributed in the hope that it will be useful, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details. // GNU Lesser General Public License for more details.
// //
// You should have received a copy of the GNU Lesser General Public License // 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/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
/** \file planner.h /** \file planner.h
\brief Planning related defintions. \brief Planning related defintions.
Automatically included with \ref openrave.h
*/ */
#ifndef OPENRAVE_PLANNER_H #ifndef OPENRAVE_PLANNER_H
#define OPENRAVE_PLANNER_H #define OPENRAVE_PLANNER_H
namespace OpenRAVE { namespace OpenRAVE {
/// \brief the status of the PlanPath method. Used when PlanPath can be cal led multiple times to resume planning. /// \brief the status of the PlanPath method. Used when PlanPath can be cal led multiple times to resume planning.
enum PlannerStatus enum PlannerStatus
{ {
PS_Failed = 0, ///< planner failed PS_Failed = 0, ///< planner failed
 End of changes. 1 change blocks. 
0 lines changed or deleted 2 lines changed or added


 planningutils.h   planningutils.h 
skipping to change at line 20 skipping to change at line 20
// This program is distributed in the hope that it will be useful, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details. // GNU Lesser General Public License for more details.
// //
// You should have received a copy of the GNU Lesser General Public License // 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/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
/** \file planningutils.h /** \file planningutils.h
\brief Planning related utilities likes samplers, distance metrics, etc . \brief Planning related utilities likes samplers, distance metrics, etc .
This file is optional and not automatically included with openrave.h
*/ */
#ifndef OPENRAVE_PLANNINGUTILS_H #ifndef OPENRAVE_PLANNINGUTILS_H
#define OPENRAVE_PLANNINGUTILS_H #define OPENRAVE_PLANNINGUTILS_H
#include <openrave/openrave.h> #include <openrave/openrave.h>
namespace OpenRAVE { namespace OpenRAVE {
namespace planningutils { namespace planningutils {
skipping to change at line 55 skipping to change at line 57
*/ */
OPENRAVE_API void VerifyTrajectory(PlannerBase::PlannerParametersConstPtr p arameters, TrajectoryBaseConstPtr trajectory, dReal samplingstep=0.002); OPENRAVE_API void VerifyTrajectory(PlannerBase::PlannerParametersConstPtr p arameters, TrajectoryBaseConstPtr trajectory, dReal samplingstep=0.002);
/** \brief Smooth the trajectory points consisting of active dofs of the ro bot while avoiding collisions. <b>[multi-thread safe]</b> /** \brief Smooth the trajectory points consisting of active dofs of the ro bot while avoiding collisions. <b>[multi-thread safe]</b>
Only initial and goal configurations are preserved. Only initial and goal configurations are preserved.
\param traj the trajectory that initially contains the input points, it is modified to contain the new re-timed data. \param traj the trajectory that initially contains the input points, it is modified to contain the new re-timed data.
\param robot use the robot's active dofs to initialize the trajectory s pace \param robot use the robot's active dofs to initialize the trajectory s pace
\param plannername the name of the planner to use to smooth. If empty, will use the default trajectory re-timer. \param plannername the name of the planner to use to smooth. If empty, will use the default trajectory re-timer.
\param hastimestamps if true, use the already initialized timestamps of the trajectory \param hastimestamps if true, use the already initialized timestamps of the trajectory
\param plannerparameters XML string to be appended to PlannerBase::Plan nerParameters::_sExtraParameters passed in to the planner.
*/ */
OPENRAVE_API void SmoothActiveDOFTrajectory(TrajectoryBasePtr traj, RobotBa sePtr robot, bool hastimestamps=false, dReal fmaxvelmult=1, const std::stri ng& plannername=""); OPENRAVE_API void SmoothActiveDOFTrajectory(TrajectoryBasePtr traj, RobotBa sePtr robot, bool hastimestamps=false, dReal fmaxvelmult=1, const std::stri ng& plannername="", const std::string& plannerparameters="");
/** \brief Smooth the trajectory points consisting of affine dofs while avo iding collisions. <b>[multi-thread safe]</b> /** \brief Smooth the trajectory points consisting of affine dofs while avo iding collisions. <b>[multi-thread safe]</b>
Only initial and goal configurations are preserved. Only initial and goal configurations are preserved.
\param traj the trajectory that initially contains the input points, it is modified to contain the new re-timed data. \param traj the trajectory that initially contains the input points, it is modified to contain the new re-timed data.
\param maxvelocities the max velocities of each dof \param maxvelocities the max velocities of each dof
\param maxaccelerations the max acceleration of each dof \param maxaccelerations the max acceleration of each dof
\param plannername the name of the planner to use to smooth. If empty, will use the default trajectory re-timer. \param plannername the name of the planner to use to smooth. If empty, will use the default trajectory re-timer.
\param hastimestamps if true, use the already initialized timestamps of the trajectory \param hastimestamps if true, use the already initialized timestamps of the trajectory
\param plannerparameters XML string to be appended to PlannerBase::Plan nerParameters::_sExtraParameters passed in to the planner.
*/ */
OPENRAVE_API void SmoothAffineTrajectory(TrajectoryBasePtr traj, const std: :vector<dReal>& maxvelocities, const std::vector<dReal>& maxaccelerations, bool hastimestamps=false, const std::string& plannername=""); OPENRAVE_API void SmoothAffineTrajectory(TrajectoryBasePtr traj, const std: :vector<dReal>& maxvelocities, const std::vector<dReal>& maxaccelerations, bool hastimestamps=false, const std::string& plannername="", const std::str ing& plannerparameters="");
/** \brief Retime the trajectory points consisting of active dofs. <b>[mult i-thread safe]</b> /** \brief Retime the trajectory points consisting of active dofs. <b>[mult i-thread safe]</b>
Collision is not checked. Every waypoint in the trajectory is guarantee d to be hit. Collision is not checked. Every waypoint in the trajectory is guarantee d to be hit.
\param traj the trajectory that initially contains the input points, it is modified to contain the new re-timed data. \param traj the trajectory that initially contains the input points, it is modified to contain the new re-timed data.
\param robot use the robot's active dofs to initialize the trajectory s pace \param robot use the robot's active dofs to initialize the trajectory s pace
\param plannername the name of the planner to use to retime. If empty, will use the default trajectory re-timer. \param plannername the name of the planner to use to retime. If empty, will use the default trajectory re-timer.
\param hastimestamps if true, use the already initialized timestamps of the trajectory \param hastimestamps if true, use the already initialized timestamps of the trajectory
\param plannerparameters XML string to be appended to PlannerBase::Plan nerParameters::_sExtraParameters passed in to the planner.
*/ */
OPENRAVE_API void RetimeActiveDOFTrajectory(TrajectoryBasePtr traj, RobotBa sePtr robot, bool hastimestamps=false, dReal fmaxvelmult=1, const std::stri ng& plannername=""); OPENRAVE_API void RetimeActiveDOFTrajectory(TrajectoryBasePtr traj, RobotBa sePtr robot, bool hastimestamps=false, dReal fmaxvelmult=1, const std::stri ng& plannername="", const std::string& plannerparameters="");
/** \brief Retime the trajectory points consisting of affine dofs while avo iding collisions. <b>[multi-thread safe]</b> /** \brief Retime the trajectory points consisting of affine dofs while avo iding collisions. <b>[multi-thread safe]</b>
Collision is not checked. Every waypoint in the trajectory is guarantee d to be hit. Collision is not checked. Every waypoint in the trajectory is guarantee d to be hit.
\param traj the trajectory that initially contains the input points, it is modified to contain the new re-timed data. \param traj the trajectory that initially contains the input points, it is modified to contain the new re-timed data.
\param maxvelocities the max velocities of each dof \param maxvelocities the max velocities of each dof
\param maxaccelerations the max acceleration of each dof \param maxaccelerations the max acceleration of each dof
\param plannername the name of the planner to use to retime. If empty, will use the default trajectory re-timer. \param plannername the name of the planner to use to retime. If empty, will use the default trajectory re-timer.
\param hastimestamps if true, use the already initialized timestamps of the trajectory \param hastimestamps if true, use the already initialized timestamps of the trajectory
\param plannerparameters XML string to be appended to PlannerBase::Plan nerParameters::_sExtraParameters passed in to the planner.
*/ */
OPENRAVE_API void RetimeAffineTrajectory(TrajectoryBasePtr traj, const std: :vector<dReal>& maxvelocities, const std::vector<dReal>& maxaccelerations, bool hastimestamps=false, const std::string& plannername=""); OPENRAVE_API void RetimeAffineTrajectory(TrajectoryBasePtr traj, const std: :vector<dReal>& maxvelocities, const std::vector<dReal>& maxaccelerations, bool hastimestamps=false, const std::string& plannername="", const std::str ing& plannerparameters="");
/** \brief Inserts a waypoint into a trajectory at the index specified, and retimes the segment before and after the trajectory. <b>[multi-thread safe ]</b> /** \brief Inserts a waypoint into a trajectory at the index specified, and retimes the segment before and after the trajectory. <b>[multi-thread safe ]</b>
Collision is not checked on the modified segments of the trajectory. Collision is not checked on the modified segments of the trajectory.
\param index The index where to start modifying the trajectory. \param index The index where to start modifying the trajectory.
\param dofvalues the configuration to insert into the trajectcory (acti ve dof values of the robot) \param dofvalues the configuration to insert into the trajectcory (acti ve dof values of the robot)
\param dofvelocities the velocities that the inserted point should star t with \param dofvelocities the velocities that the inserted point should star t with
\param traj the trajectory that initially contains the input points, it is modified to contain the new re-timed data. \param traj the trajectory that initially contains the input points, it is modified to contain the new re-timed data.
\param robot use the robot's active dofs to initialize the trajectory s pace \param robot use the robot's active dofs to initialize the trajectory s pace
\param plannername the name of the planner to use to retime. If empty, will use the default trajectory re-timer. \param plannername the name of the planner to use to retime. If empty, will use the default trajectory re-timer.
skipping to change at line 123 skipping to change at line 129
/// trajectories's data can be set at a time. /// trajectories's data can be set at a time.
/// \throw openrave_exception throws an exception if the trajectory data is incompatible and cannot be merged. /// \throw openrave_exception throws an exception if the trajectory data is incompatible and cannot be merged.
OPENRAVE_API TrajectoryBasePtr MergeTrajectories(const std::list<Trajectory BaseConstPtr>& listtrajectories); OPENRAVE_API TrajectoryBasePtr MergeTrajectories(const std::list<Trajectory BaseConstPtr>& listtrajectories);
/** \brief sets the planner parameters structure from a configuration speci fication /** \brief sets the planner parameters structure from a configuration speci fication
Attempt to set default values for all parameters Attempt to set default values for all parameters
*/ */
OPENRAVE_API void SetPlannerParametersFromSpecification(PlannerBase::Planne rParametersPtr parameters, const ConfigurationSpecification& spec); OPENRAVE_API void SetPlannerParametersFromSpecification(PlannerBase::Planne rParametersPtr parameters, const ConfigurationSpecification& spec);
/** \brief represents the DH parameters for one joint
T = Z_1 X_1 Z_2 X_2 ... X_n Z_n
where
Z_i = [cos(theta) -sin(theta) 0 0; sin(theta) cos(theta) 0 0; 0 0 1 d]
X_i = [1 0 0 a; 0 cos(alpha) -sin(alpha) 0; 0 sin(alpha) cos(alpha) 0]
http://en.wikipedia.org/wiki/Denavit%E2%80%93Hartenberg_parameters
*/
class DHParameter
{
public:
KinBody::JointConstPtr joint; ///< pointer to joint
int parentindex; ///< index into dh parameter array for getting cooreai
nte system of parent joint. If -1, no parent.
Transform transform; ///< the computed coordinate system of this joint,
this can be automatically computed from DH parameters
dReal d; ///< distance along previous z
dReal a; ///< orthogonal distance from previous z axis to current z
dReal theta; ///< rotation of previous x around previous z to current x
dReal alpha; ///< rotation of previous z to current z
};
/** \brief returns the Denavit-Hartenberg parameters of the kinematics stru
cture of the body.
If the robot has joints that cannot be represented by DH, will throw an
exception.
Passive joints are ignored. Joints are ordered by hierarchy dependency.
By convention N joints give N-1 DH parameters, but GetDHParameters retu
rns N parameters. The reason is because the first parameter is used to defi
ne the coordinate system of the first axis relative to the robot origin.
\note The coordinate systems computed from the DH parameters do not mat
ch the OpenRAVE link coordinate systems.
\param vparameters One set of parameters are returned for each joint. \
see DHParameter.
\param tstart the initial transform in the body coordinate system to th
e first joint
*/
OPENRAVE_API void GetDHParameters(std::vector<DHParameter>& vparameters, Ki
nBodyConstPtr pbody);
/// \brief Line collision /// \brief Line collision
class OPENRAVE_API LineCollisionConstraint class OPENRAVE_API LineCollisionConstraint
{ {
public: public:
LineCollisionConstraint(); LineCollisionConstraint();
bool Check(PlannerBase::PlannerParametersWeakPtr _params, KinBodyPtr ro bot, const std::vector<dReal>& pQ0, const std::vector<dReal>& pQ1, Interval Type interval, PlannerBase::ConfigurationListPtr pvCheckedConfigurations); bool Check(PlannerBase::PlannerParametersWeakPtr _params, KinBodyPtr ro bot, const std::vector<dReal>& pQ0, const std::vector<dReal>& pQ1, Interval Type interval, PlannerBase::ConfigurationListPtr pvCheckedConfigurations);
protected: protected:
std::vector<dReal> _vtempconfig, dQ; std::vector<dReal> _vtempconfig, dQ;
CollisionReportPtr _report; CollisionReportPtr _report;
skipping to change at line 198 skipping to change at line 237
RobotBasePtr _probot; RobotBasePtr _probot;
RobotBase::ManipulatorConstPtr _pmanip; RobotBase::ManipulatorConstPtr _pmanip;
int _nummaxsamples, _nummaxtries; int _nummaxsamples, _nummaxtries;
std::list<SampleInfo> _listsamples; std::list<SampleInfo> _listsamples;
SpaceSamplerBasePtr _pindexsampler; SpaceSamplerBasePtr _pindexsampler;
dReal _fsampleprob, _fjittermaxdist; dReal _fsampleprob, _fjittermaxdist;
CollisionReportPtr _report; CollisionReportPtr _report;
std::vector< std::vector<dReal> > _viksolutions; std::vector< std::vector<dReal> > _viksolutions;
std::list<int> _listreturnedsamples; std::list<int> _listreturnedsamples;
std::vector<dReal> _vfreestart; std::vector<dReal> _vfreestart;
int _tempikindex; ///< if _viksolutions.size() > 0, points to the origi nal ik index of those solutions
}; };
typedef boost::shared_ptr<ManipulatorIKGoalSampler> ManipulatorIKGoalSample rPtr; typedef boost::shared_ptr<ManipulatorIKGoalSampler> ManipulatorIKGoalSample rPtr;
} // planningutils } // planningutils
} // OpenRAVE } // OpenRAVE
#endif #endif
 End of changes. 11 change blocks. 
4 lines changed or deleted 54 lines changed or added


 plugin.h   plugin.h 
skipping to change at line 19 skipping to change at line 19
// //
// This program is distributed in the hope that it will be useful, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details. // GNU Lesser General Public License for more details.
// //
// You should have received a copy of the GNU Lesser General Public License // 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/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
/** \file plugin.h /** \file plugin.h
\brief Provides helper functions for creating plugins. Defines all the necessary functions to export. \brief Provides helper functions for creating plugins. Defines all the necessary functions to export.
This file is optional and not automatically included with openrave.h
*/ */
#ifndef OPENRAVE_PLUGIN_H #ifndef OPENRAVE_PLUGIN_H
#define OPENRAVE_PLUGIN_H #define OPENRAVE_PLUGIN_H
#include <openrave/openrave.h> #include <openrave/openrave.h>
#include <boost/format.hpp> #include <boost/format.hpp>
// export symbol prefix for plugin functions // export symbol prefix for plugin functions
#define OPENRAVE_PLUGIN_API extern "C" OPENRAVE_HELPER_DLL_EXPORT #define OPENRAVE_PLUGIN_API extern "C" OPENRAVE_HELPER_DLL_EXPORT
 End of changes. 1 change blocks. 
0 lines changed or deleted 2 lines changed or added


 plugininfo.h   plugininfo.h 
skipping to change at line 19 skipping to change at line 19
// //
// This program is distributed in the hope that it will be useful, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details. // GNU Lesser General Public License for more details.
// //
// You should have received a copy of the GNU Lesser General Public License // 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/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
/** \file plugininfo.h /** \file plugininfo.h
\brief Holds the plugin information structure. \brief Holds the plugin information structure.
Automatically included with \ref openrave.h
*/ */
#ifndef OPENRAVE_PLUGIN_INFO #ifndef OPENRAVE_PLUGIN_INFO
#define OPENRAVE_PLUGIN_INFO #define OPENRAVE_PLUGIN_INFO
namespace OpenRAVE { namespace OpenRAVE {
/** \brief Holds all the %OpenRAVE-specific information provided by a plugi n. /** \brief Holds all the %OpenRAVE-specific information provided by a plugi n.
\ingroup plugin_exports \ingroup plugin_exports
PLUGININFO has a hash computed for it to validate its size and type bef ore having a plugin fill it. PLUGININFO has a hash computed for it to validate its size and type bef ore having a plugin fill it.
 End of changes. 1 change blocks. 
0 lines changed or deleted 2 lines changed or added


 robot.h   robot.h 
skipping to change at line 19 skipping to change at line 19
// //
// This program is distributed in the hope that it will be useful, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details. // GNU Lesser General Public License for more details.
// //
// You should have received a copy of the GNU Lesser General Public License // 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/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
/** \file robot.h /** \file robot.h
\brief Base robot and manipulator description. \brief Base robot and manipulator description.
Automatically included with \ref openrave.h
*/ */
#ifndef OPENRAVE_ROBOT_H #ifndef OPENRAVE_ROBOT_H
#define OPENRAVE_ROBOT_H #define OPENRAVE_ROBOT_H
namespace OpenRAVE { namespace OpenRAVE {
/** \brief <b>[interface]</b> A robot is a kinematic body that has attached manipulators, sensors, and controllers. <b>If not specified, method is not multi-thread safe.</b> See \ref arch_robot. /** \brief <b>[interface]</b> A robot is a kinematic body that has attached manipulators, sensors, and controllers. <b>If not specified, method is not multi-thread safe.</b> See \ref arch_robot.
\ingroup interfaces \ingroup interfaces
*/ */
class OPENRAVE_API RobotBase : public KinBody class OPENRAVE_API RobotBase : public KinBody
skipping to change at line 345 skipping to change at line 347
}; };
typedef boost::shared_ptr<RobotBase::AttachedSensor> AttachedSensorPtr; typedef boost::shared_ptr<RobotBase::AttachedSensor> AttachedSensorPtr;
typedef boost::shared_ptr<RobotBase::AttachedSensor const> AttachedSens orConstPtr; typedef boost::shared_ptr<RobotBase::AttachedSensor const> AttachedSens orConstPtr;
/// \brief The information of a currently grabbed body. /// \brief The information of a currently grabbed body.
class OPENRAVE_API Grabbed class OPENRAVE_API Grabbed
{ {
public: public:
KinBodyWeakPtr pbody; ///< the grabbed body KinBodyWeakPtr pbody; ///< the grabbed body
LinkPtr plinkrobot; ///< robot link that is grabbing the bo dy LinkPtr plinkrobot; ///< robot link that is grabbing the bo dy
std::vector<LinkConstPtr> vCollidingLinks, vNonCollidingLinks; ///< robot links that already collide with the body std::vector<LinkConstPtr> vCollidingLinks, vNonCollidingLinks; ///< vCollidingLinks: robot links that already collide with the body. T his will always include plinkrobot and any other body's first link attached to plinkrobot (or static versions)
Transform troot; ///< root transform (of first link of body ) relative to plinkrobot's transform. In other words, pbody->GetTransform() == plinkrobot->GetTransform()*troot Transform troot; ///< root transform (of first link of body ) relative to plinkrobot's transform. In other words, pbody->GetTransform() == plinkrobot->GetTransform()*troot
}; };
/// \brief Helper class derived from KinBodyStateSaver to additionaly s ave robot information. /// \brief Helper class derived from KinBodyStateSaver to additionaly s ave robot information.
class OPENRAVE_API RobotStateSaver : public KinBodyStateSaver class OPENRAVE_API RobotStateSaver : public KinBodyStateSaver
{ {
public: public:
RobotStateSaver(RobotBasePtr probot, int options = Save_LinkTransfo rmation|Save_LinkEnable|Save_ActiveDOF|Save_ActiveManipulator); RobotStateSaver(RobotBasePtr probot, int options = Save_LinkTransfo rmation|Save_LinkEnable|Save_ActiveDOF|Save_ActiveManipulator);
virtual ~RobotStateSaver(); virtual ~RobotStateSaver();
virtual void Restore(); virtual void Restore();
 End of changes. 2 change blocks. 
1 lines changed or deleted 3 lines changed or added


 sensor.h   sensor.h 
skipping to change at line 19 skipping to change at line 19
// //
// This program is distributed in the hope that it will be useful, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details. // GNU Lesser General Public License for more details.
// //
// You should have received a copy of the GNU Lesser General Public License // 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/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
/** \file sensor.h /** \file sensor.h
\brief Sensor and sensing related defintions. \brief Sensor and sensing related defintions.
Automatically included with \ref openrave.h
*/ */
#ifndef OPENRAVE_SENSOR_H #ifndef OPENRAVE_SENSOR_H
#define OPENRAVE_SENSOR_H #define OPENRAVE_SENSOR_H
namespace OpenRAVE { namespace OpenRAVE {
/** \brief <b>[interface]</b> A sensor measures physical properties from th e environment. <b>If not specified, method is not multi-thread safe.</b> Se e \ref arch_sensor. /** \brief <b>[interface]</b> A sensor measures physical properties from th e environment. <b>If not specified, method is not multi-thread safe.</b> Se e \ref arch_sensor.
\ingroup interfaces \ingroup interfaces
*/ */
class OPENRAVE_API SensorBase : public InterfaceBase class OPENRAVE_API SensorBase : public InterfaceBase
 End of changes. 1 change blocks. 
0 lines changed or deleted 2 lines changed or added


 sensorsystem.h   sensorsystem.h 
skipping to change at line 19 skipping to change at line 19
// //
// This program is distributed in the hope that it will be useful, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details. // GNU Lesser General Public License for more details.
// //
// You should have received a copy of the GNU Lesser General Public License // 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/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
/** \file sensorsystem.h /** \file sensorsystem.h
\brief Sensor systems used to define how environment information is org anized and how bodies are managed. \brief Sensor systems used to define how environment information is org anized and how bodies are managed.
Automatically included with \ref openrave.h
*/ */
#ifndef OPENRAVE_SENSORSYSTEM_H #ifndef OPENRAVE_SENSORSYSTEM_H
#define OPENRAVE_SENSORSYSTEM_H #define OPENRAVE_SENSORSYSTEM_H
namespace OpenRAVE { namespace OpenRAVE {
/** \brief <b>[interface]</b> Used to manage the creation and destruction o f bodies. See \ref arch_sensorsystem. /** \brief <b>[interface]</b> Used to manage the creation and destruction o f bodies. See \ref arch_sensorsystem.
\ingroup interfaces \ingroup interfaces
*/ */
class OPENRAVE_API SensorSystemBase : public InterfaceBase class OPENRAVE_API SensorSystemBase : public InterfaceBase
 End of changes. 1 change blocks. 
0 lines changed or deleted 2 lines changed or added


 spacesampler.h   spacesampler.h 
skipping to change at line 19 skipping to change at line 19
// //
// This program is distributed in the hope that it will be useful, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details. // GNU Lesser General Public License for more details.
// //
// You should have received a copy of the GNU Lesser General Public License // 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/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
/** \file spacesampler.h /** \file spacesampler.h
\brief Sampling definitions. \brief Sampling definitions.
Automatically included with \ref openrave.h
*/ */
#ifndef OPENRAVE_SPACESAMPLER_H #ifndef OPENRAVE_SPACESAMPLER_H
#define OPENRAVE_SPACESAMPLER_H #define OPENRAVE_SPACESAMPLER_H
namespace OpenRAVE { namespace OpenRAVE {
/// \brief Specifies the boundary conditions of intervals for sampling /// \brief Specifies the boundary conditions of intervals for sampling
enum IntervalType { enum IntervalType {
IT_Open=0, ///< (a,b) IT_Open=0, ///< (a,b)
IT_OpenStart=1, ///< (a,b] IT_OpenStart=1, ///< (a,b]
 End of changes. 1 change blocks. 
0 lines changed or deleted 2 lines changed or added


 trajectory.h   trajectory.h 
skipping to change at line 19 skipping to change at line 19
// //
// This program is distributed in the hope that it will be useful, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details. // GNU Lesser General Public License for more details.
// //
// You should have received a copy of the GNU Lesser General Public License // 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/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
/** \file trajectory.h /** \file trajectory.h
\brief Definition of \ref OpenRAVE::TrajectoryBase \brief Definition of \ref OpenRAVE::TrajectoryBase
*/
Automatically included with \ref openrave.h
*/
#ifndef OPENRAVE_TRAJECTORY_H #ifndef OPENRAVE_TRAJECTORY_H
#define OPENRAVE_TRAJECTORY_H #define OPENRAVE_TRAJECTORY_H
namespace OpenRAVE { namespace OpenRAVE {
/** \brief <b>[interface]</b> Encapsulate a time-parameterized trajectories of robot configurations. <b>If not specified, method is not multi-thread s afe.</b> \arch_trajectory /** \brief <b>[interface]</b> Encapsulate a time-parameterized trajectories of robot configurations. <b>If not specified, method is not multi-thread s afe.</b> \arch_trajectory
\ingroup interfaces \ingroup interfaces
*/ */
class OPENRAVE_API TrajectoryBase : public InterfaceBase class OPENRAVE_API TrajectoryBase : public InterfaceBase
{ {
 End of changes. 2 change blocks. 
1 lines changed or deleted 2 lines changed or added


 viewer.h   viewer.h 
skipping to change at line 19 skipping to change at line 19
// //
// This program is distributed in the hope that it will be useful, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details. // GNU Lesser General Public License for more details.
// //
// You should have received a copy of the GNU Lesser General Public License // 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/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
/** \file viewer.h /** \file viewer.h
\brief Graphical interface functions. \brief Graphical interface functions.
Automatically included with \ref openrave.h
*/ */
#ifndef OPENRAVE_VIEWER_H #ifndef OPENRAVE_VIEWER_H
#define OPENRAVE_VIEWER_H #define OPENRAVE_VIEWER_H
namespace OpenRAVE { namespace OpenRAVE {
/** \brief Handle holding the plot from the viewers. The plot will continue to be drawn as long as a reference to this handle is held. /** \brief Handle holding the plot from the viewers. The plot will continue to be drawn as long as a reference to this handle is held.
Designed to be multi-thread safe and destruction and modification of th e viewer plot can be done at any time. The viewers Designed to be multi-thread safe and destruction and modification of th e viewer plot can be done at any time. The viewers
internally handle synchronization and threading issues. internally handle synchronization and threading issues.
 End of changes. 1 change blocks. 
0 lines changed or deleted 2 lines changed or added

This html diff was produced by rfcdiff 1.41. The latest version is available from http://tools.ietf.org/tools/rfcdiff/