| 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 | |
|
| 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 | |
|
| 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 | |
|