collisionchecker.h   collisionchecker.h 
skipping to change at line 28 skipping to change at line 28
\brief Collision checking related definitions. \brief Collision checking related definitions.
*/ */
#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 CO_Distance = 1, ///< Compute distance measurements, this is usually sl
ow ow and not all checkers support it.
CO_UseTolerance = 2, CO_UseTolerance = 2, ///< not used
CO_Contacts = 4, ///< Collision returns contact points CO_Contacts = 4, ///< Return the contact points of the collision in the
CO_RayAnyHit = 8, ///< when performing collision with rays, if this is \ref CollisionReport. Note that this takes longer to compute.
set, algorithm just returns any hit instead of the closest (can be faster) CO_RayAnyHit = 8, ///< When performing collision with rays, if this is
CO_ActiveDOFs = 16, ///< if set and the target object is a robot, then set, algorithm just returns any hit instead of the closest (can be faster)
only the links controlled by the currently set active DOFs will be checked
for collisions. This allows planners to reduce redundant collision checks. /** Allows planners to greatly reduce redundant collision checks.
If set and the target object is a robot, then only the links contro
lled by the currently set active DOFs and their attached bodies will be che
cked for collisions.
The things that **will not be** checked for collision are:
- links that do not remove with respect to each other as a result o
f moving the active dofs.
*/
CO_ActiveDOFs = 16,
}; };
/// \brief action to perform whenever a collision is detected between objec ts /// \brief action to perform whenever a collision is detected between objec ts
enum CollisionAction enum CollisionAction
{ {
CA_DefaultAction = 0, ///< let the physics/collision engine resolve the action CA_DefaultAction = 0, ///< let the physics/collision engine resolve the action
CA_Ignore = 1, ///< do nothing CA_Ignore = 1, ///< do nothing
}; };
/// \brief Holds information about a particular collision that occured. /// \brief Holds information about a particular collision that occured.
skipping to change at line 83 skipping to change at line 90
int numWithinTol; ///< number of objects within tolerance of this objec t, filled if CO_UseTolerance option is set int numWithinTol; ///< number of objects within tolerance of this objec t, filled if CO_UseTolerance option is set
std::vector<CONTACT> contacts; ///< the convention is that the normal w ill be "out" of plink1's surface. Filled if CO_UseContacts option is set. std::vector<CONTACT> contacts; ///< the convention is that the normal w ill be "out" of plink1's surface. Filled if CO_UseContacts option is set.
virtual void Reset(int coloptions = 0); virtual void Reset(int coloptions = 0);
virtual std::string __str__() const; virtual std::string __str__() const;
}; };
typedef CollisionReport COLLISIONREPORT RAVE_DEPRECATED; typedef CollisionReport COLLISIONREPORT RAVE_DEPRECATED;
/** \brief <b>[interface]</b> Responsible for all collision checking querie s of the environment. See \ref arch_collisionchecker. /** \brief <b>[interface]</b> Responsible for all collision checking querie s of the environment. <b>If not specified, method is not multi-thread safe. </b> See \ref arch_collisionchecker.
\ingroup interfaces \ingroup interfaces
*/ */
class OPENRAVE_API CollisionCheckerBase : public InterfaceBase class OPENRAVE_API CollisionCheckerBase : public InterfaceBase
{ {
public: public:
CollisionCheckerBase(EnvironmentBasePtr penv) : InterfaceBase(PT_Collis ionChecker, penv) { CollisionCheckerBase(EnvironmentBasePtr penv) : InterfaceBase(PT_Collis ionChecker, penv) {
} }
virtual ~CollisionCheckerBase() { virtual ~CollisionCheckerBase() {
} }
skipping to change at line 182 skipping to change at line 189
/// \brief Checks self collision only with the links of the passed in b ody. /// \brief Checks self collision only with the links of the passed in b ody.
/// ///
/// Links that are joined together are ignored. /// Links that are joined together are ignored.
virtual bool CheckSelfCollision(KinBodyConstPtr pbody, CollisionReportP tr report = CollisionReportPtr()) = 0; virtual bool CheckSelfCollision(KinBodyConstPtr pbody, CollisionReportP tr report = CollisionReportPtr()) = 0;
virtual void SetCollisionData(KinBodyPtr pbody, UserDataPtr data) { virtual void SetCollisionData(KinBodyPtr pbody, UserDataPtr data) {
pbody->SetCollisionData(data); pbody->SetCollisionData(data);
} }
inline CollisionCheckerBasePtr shared_collisionchecker() {
return boost::static_pointer_cast<CollisionCheckerBase>(shared_from
_this());
}
inline CollisionCheckerBaseConstPtr shared_collisionchecker_const() con
st {
return boost::static_pointer_cast<CollisionCheckerBase const>(share
d_from_this());
}
private: private:
virtual const char* GetHash() const { virtual const char* GetHash() const {
return OPENRAVE_COLLISIONCHECKER_HASH; return OPENRAVE_COLLISIONCHECKER_HASH;
} }
#ifdef RAVE_PRIVATE #ifdef RAVE_PRIVATE
#ifdef _MSC_VER #ifdef _MSC_VER
friend class Environment; friend class Environment;
#else #else
friend class ::Environment; friend class ::Environment;
 End of changes. 3 change blocks. 
10 lines changed or deleted 29 lines changed or added


 config.h   config.h 
skipping to change at line 39 skipping to change at line 39
#else #else
#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 4 #define OPENRAVE_VERSION_MINOR 5
#define OPENRAVE_VERSION_PATCH 2 #define OPENRAVE_VERSION_PATCH 0
#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.4.2" #define OPENRAVE_VERSION_STRING "0.5.0"
#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.4.2/share/openrave-0.4/plugins" /openrave/0.5.0/share/openrave-0.5/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.4.2/share/openrave-0.4" enrave/0.5.0/share/openrave-0.5"
#define OPENRAVE_PYTHON_INSTALL_DIR "/home/andrey/upstream-tracker/testing/ #define OPENRAVE_PYTHON_INSTALL_DIR "/home/andrey/upstream-tracker/testing/
openrave/0.4.2/lib/python2.7/site-packages" openrave/0.5.0/lib/python2.7/site-packages"
#endif #endif
 End of changes. 3 change blocks. 
9 lines changed or deleted 9 lines changed or added


 controller.h   controller.h 
skipping to change at line 25 skipping to change at line 25
// 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.
*/ */
#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>Methods not multi-thread safe.</b> See \ref 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
{ {
public: public:
ControllerBase(EnvironmentBasePtr penv) : InterfaceBase(PT_Controller, penv) { ControllerBase(EnvironmentBasePtr penv) : InterfaceBase(PT_Controller, penv) {
} }
virtual ~ControllerBase() { virtual ~ControllerBase() {
} }
 End of changes. 1 change blocks. 
1 lines changed or deleted 1 lines changed or added


 environment.h   environment.h 
skipping to change at line 57 skipping to change at line 57
__pUserData = data; __pUserData = data;
} }
/// \brief return the user custom data /// \brief return the user custom data
virtual UserDataPtr GetUserData() const { virtual UserDataPtr GetUserData() const {
return __pUserData; return __pUserData;
} }
/// \brief Returns the OpenRAVE global state, used for initializing plu gins /// \brief Returns the OpenRAVE global state, used for initializing plu gins
virtual UserDataPtr GlobalState() = 0; virtual UserDataPtr GlobalState() = 0;
/// \deprecated (10/09/23) see \ref RaveCreateInterface
virtual InterfaceBasePtr CreateInterface(InterfaceType type,const std::
string& interfacename) RAVE_DEPRECATED =0;
/// \deprecated (10/09/23) see \ref RaveCreateRobot
virtual RobotBasePtr CreateRobot(const std::string& name="") RAVE_DEPRE
CATED =0;
/// \deprecated (10/09/23) see \ref RaveCreatePlanner
virtual PlannerBasePtr CreatePlanner(const std::string& name) RAVE_DEPR
ECATED =0;
/// \deprecated (10/09/23) see \ref RaveCreateSensorSystem
virtual SensorSystemBasePtr CreateSensorSystem(const std::string& name)
RAVE_DEPRECATED =0;
/// \deprecated (10/09/23) see \ref RaveCreateController
virtual ControllerBasePtr CreateController(const std::string& name) RAV
E_DEPRECATED =0;
/// \deprecated (10/09/23) see \ref RaveCreateProblem
virtual ModuleBasePtr CreateProblem(const std::string& name) RAVE_DEPRE
CATED =0;
/// \deprecated (10/09/23) see \ref RaveCreateIkSolver
virtual IkSolverBasePtr CreateIkSolver(const std::string& name) RAVE_DE
PRECATED =0;
/// \deprecated (10/09/23) see \ref RaveCreatePhysicsEngine
virtual PhysicsEngineBasePtr CreatePhysicsEngine(const std::string& nam
e) RAVE_DEPRECATED =0;
/// \deprecated (10/09/23) see \ref RaveCreateSensor
virtual SensorBasePtr CreateSensor(const std::string& name) RAVE_DEPREC
ATED =0;
/// \deprecated (10/09/23) see \ref RaveCreateCollisionChecker
virtual CollisionCheckerBasePtr CreateCollisionChecker(const std::strin
g& name) RAVE_DEPRECATED =0;
/// \deprecated (10/09/23) see \ref RaveCreateViewer
virtual ViewerBasePtr CreateViewer(const std::string& name) RAVE_DEPREC
ATED =0;
/// \deprecated (10/09/23) see \ref RaveCreateKinBody
virtual KinBodyPtr CreateKinBody(const std::string& name="") RAVE_DEPRE
CATED = 0;
/// \deprecated (10/09/23) see \ref RaveCreateTrajectory
virtual TrajectoryBasePtr CreateTrajectory(int nDOF) RAVE_DEPRECATED =
0;
/// \deprecated (10/09/23) see \ref RaveLoadPlugin
virtual void GetPluginInfo(std::list< std::pair<std::string, PLUGININFO
> >& plugins) RAVE_DEPRECATED =0;
/// \deprecated (10/09/23) see \ref RaveLoadPlugin
virtual void GetLoadedInterfaces(std::map<InterfaceType, std::vector<st
d::string> >& interfacenames) const RAVE_DEPRECATED = 0;
/// \deprecated (10/09/23) see \ref RaveLoadPlugin
virtual bool LoadPlugin(const std::string& name) RAVE_DEPRECATED = 0;
/// \deprecated (10/09/23) see \ref RaveReloadPlugins
virtual void ReloadPlugins() RAVE_DEPRECATED = 0;
/// \deprecated (10/09/23) see \ref RaveHasInterface
virtual bool HasInterface(InterfaceType type, const std::string& interf
acename) const RAVE_DEPRECATED = 0;
/// \brief Environment will own the interface until EnvironmentBase::De stroy is called. /// \brief Environment will own the interface until EnvironmentBase::De stroy is called.
virtual void OwnInterface(InterfaceBasePtr pinterface) = 0; virtual void OwnInterface(InterfaceBasePtr pinterface) = 0;
/// \brief Remove ownership of the interface. /// \brief Remove ownership of the interface.
virtual void DisownInterface(InterfaceBasePtr pinterface) = 0; virtual void DisownInterface(InterfaceBasePtr pinterface) = 0;
/// \brief Create and return a clone of the current environment. /// \brief Create and return a clone of the current environment.
/// ///
/// Clones do not share any memory or resource between each other. /// Clones do not share any memory or resource between each other.
/// or their parent making them ideal for performing separte planning e xperiments while keeping /// or their parent making them ideal for performing separte planning e xperiments while keeping
skipping to change at line 340 skipping to change at line 303
\param filename the name of the resource file, its extension determ ines the format of the file. Complex meshes and articulated meshes are all triangulated appropriately. See \ref supported_formats. \param filename the name of the resource file, its extension determ ines the format of the file. Complex meshes and articulated meshes are all triangulated appropriately. See \ref supported_formats.
\param options Options to control the parsing process. \param options Options to control the parsing process.
*/ */
virtual boost::shared_ptr<KinBody::Link::TRIMESH> ReadTrimeshURI(boost: :shared_ptr<KinBody::Link::TRIMESH> ptrimesh, const std::string& filename, const AttributesList& atts = AttributesList()) = 0; virtual boost::shared_ptr<KinBody::Link::TRIMESH> ReadTrimeshURI(boost: :shared_ptr<KinBody::Link::TRIMESH> ptrimesh, const std::string& filename, const AttributesList& atts = AttributesList()) = 0;
virtual boost::shared_ptr<KinBody::Link::TRIMESH> ReadTrimeshFile(boost ::shared_ptr<KinBody::Link::TRIMESH> ptrimesh, const std::string& filename, const AttributesList& atts = AttributesList()) { virtual boost::shared_ptr<KinBody::Link::TRIMESH> ReadTrimeshFile(boost ::shared_ptr<KinBody::Link::TRIMESH> ptrimesh, const std::string& filename, const AttributesList& atts = AttributesList()) {
return ReadTrimeshURI(ptrimesh,filename,atts); return ReadTrimeshURI(ptrimesh,filename,atts);
} }
/// \deprecated (10/09/30) see \ref RaveRegisterXMLReader /// \deprecated (10/09/30) see \ref RaveRegisterXMLReader
virtual boost::shared_ptr<void> RegisterXMLReader(InterfaceType type, c onst std::string& xmltag, const CreateXMLReaderFn& fn) RAVE_DEPRECATED = 0; virtual UserDataPtr RegisterXMLReader(InterfaceType type, const std::st ring& xmltag, const CreateXMLReaderFn& fn) RAVE_DEPRECATED = 0;
/// \brief Parses a file for OpenRAVE XML formatted data. /// \brief Parses a file for OpenRAVE XML formatted data.
virtual bool ParseXMLFile(BaseXMLReaderPtr preader, const std::string& filename) RAVE_DEPRECATED = 0; virtual bool ParseXMLFile(BaseXMLReaderPtr preader, const std::string& filename) RAVE_DEPRECATED = 0;
/** \brief Parses a data file for XML data. /** \brief Parses a data file for XML data.
\param pdata The data of the buffer \param pdata The data of the buffer
\param len the number of bytes valid in pdata \param len the number of bytes valid in pdata
*/ */
virtual bool ParseXMLData(BaseXMLReaderPtr preader, const std::string& data) RAVE_DEPRECATED = 0; virtual bool ParseXMLData(BaseXMLReaderPtr preader, const std::string& data) RAVE_DEPRECATED = 0;
skipping to change at line 398 skipping to change at line 361
/// The function removes currently loaded bodies, robots, sensors, prob lems from the actively used /// The function removes currently loaded bodies, robots, sensors, prob lems from the actively used
/// interfaces of the environment. This does not destroy the interface, but it does remove all /// interfaces of the environment. This does not destroy the interface, but it does remove all
/// references managed. Some interfaces like problems have destroy meth ods that are called to signal /// references managed. Some interfaces like problems have destroy meth ods that are called to signal
/// unloading. Note that the active interfaces are different from the o wned interfaces. /// unloading. Note that the active interfaces are different from the o wned interfaces.
/// \param[in] obj interface to remove /// \param[in] obj interface to remove
/// \return true if the interface was successfully removed from the env ironment. /// \return true if the interface was successfully removed from the env ironment.
virtual bool Remove(InterfaceBasePtr obj) = 0; virtual bool Remove(InterfaceBasePtr obj) = 0;
/// \brief Query a body from its name. <b>[multi-thread safe]</b> /// \brief Query a body from its name. <b>[multi-thread safe]</b>
/// \return first KinBody (including robots) that matches with name /// \return first KinBody (including robots) that matches with name
virtual KinBodyPtr GetKinBody(const std::string& name)=0; virtual KinBodyPtr GetKinBody(const std::string& name) const =0;
/// \brief Query a sensor from its name. <b>[multi-thread safe]</b> /// \brief Query a sensor from its name. <b>[multi-thread safe]</b>
/// \return first sensor that matches with name, note that sensors atta ched to robots have the robot name as a prefix. /// \return first sensor that matches with name, note that sensors atta ched to robots have the robot name as a prefix.
virtual SensorBasePtr GetSensor(const std::string& name)=0; virtual SensorBasePtr GetSensor(const std::string& name) const =0;
/// \brief Query a robot from its name. <b>[multi-thread safe]</b> /// \brief Query a robot from its name. <b>[multi-thread safe]</b>
/// \return first Robot that matches the name /// \return first Robot that matches the name
virtual RobotBasePtr GetRobot(const std::string& name)=0; virtual RobotBasePtr GetRobot(const std::string& name) const =0;
/// \brief Get all bodies loaded in the environment (including robots). <b>[multi-thread safe]</b> /// \brief Get all bodies loaded in the environment (including robots). <b>[multi-thread safe]</b>
/// \param[out] bodies filled with all the bodies /// \param[out] bodies filled with all the bodies
virtual void GetBodies(std::vector<KinBodyPtr>& bodies) const = 0; virtual void GetBodies(std::vector<KinBodyPtr>& bodies) const = 0;
/// \brief Fill an array with all robots loaded in the environment. <b> [multi-thread safe]</b> /// \brief Fill an array with all robots loaded in the environment. <b> [multi-thread safe]</b>
virtual void GetRobots(std::vector<RobotBasePtr>& robots) const = 0; virtual void GetRobots(std::vector<RobotBasePtr>& robots) const = 0;
/// \brief adds a viewer to the environment /// \brief adds a viewer to the environment
/// ///
 End of changes. 5 change blocks. 
57 lines changed or deleted 4 lines changed or added


 geometry.h   geometry.h 
skipping to change at line 440 skipping to change at line 440
trans.x = trans.y = trans.z = 0; trans.x = trans.y = trans.z = 0;
} }
inline void rotfrommat(T m_00, T m_01, T m_02, T m_10, T m_11, T m_12, T m_20, T m_21, T m_22) { inline void rotfrommat(T m_00, T m_01, T m_02, T m_10, T m_11, T m_12, T m_20, T m_21, T m_22) {
m[0] = m_00; m[1] = m_01; m[2] = m_02; m[3] = 0; m[0] = m_00; m[1] = m_01; m[2] = m_02; m[3] = 0;
m[4] = m_10; m[5] = m_11; m[6] = m_12; m[7] = 0; m[4] = m_10; m[5] = m_11; m[6] = m_12; m[7] = 0;
m[8] = m_20; m[9] = m_21; m[10] = m_22; m[11] = 0; m[8] = m_20; m[9] = m_21; m[10] = m_22; m[11] = 0;
} }
inline T rot(int i, int j) const { inline T rot(int i, int j) const {
MATH_ASSERT( i >= 0 && i < 3 && j >= 0 && j < 3); MATH_ASSERT( i >= 0 && j >= 0 && i < 3 && j < 3);
return m[4*i+j]; return m[4*i+j];
} }
inline T& rot(int i, int j) { inline T& rot(int i, int j) {
MATH_ASSERT( i >= 0 && i < 3 && j >= 0 && j < 3); MATH_ASSERT( i >= 0 && j >= 0 && i < 3 && j < 3);
return m[4*i+j]; return m[4*i+j];
} }
template <typename U> template <typename U>
inline RaveVector<T> operator* (const RaveVector<U>&r) const { inline RaveVector<T> operator* (const RaveVector<U>&r) const {
RaveVector<T> v; RaveVector<T> v;
v[0] = r[0] * m[0] + r[1] * m[1] + r[2] * m[2] + trans.x; v[0] = r[0] * m[0] + r[1] * m[1] + r[2] * m[2] + trans.x;
v[1] = r[0] * m[4] + r[1] * m[5] + r[2] * m[6] + trans.y; v[1] = r[0] * m[4] + r[1] * m[5] + r[2] * m[6] + trans.y;
v[2] = r[0] * m[8] + r[1] * m[9] + r[2] * m[10] + trans.z; v[2] = r[0] * m[8] + r[1] * m[9] + r[2] * m[10] + trans.z;
return v; return v;
skipping to change at line 1529 skipping to change at line 1529
/// \return true if triangles collide. /// \return true if triangles collide.
template <typename T> template <typename T>
inline bool TriTriCollision(const RaveVector<T>& u1, const RaveVector<T>& u 2, const RaveVector<T>& u3, const RaveVector<T>& v1, const RaveVector<T>& v 2, const RaveVector<T>& v3, RaveVector<T>& contactpos, RaveVector<T>& conta ctnorm) inline bool TriTriCollision(const RaveVector<T>& u1, const RaveVector<T>& u 2, const RaveVector<T>& u3, const RaveVector<T>& v1, const RaveVector<T>& v 2, const RaveVector<T>& v3, RaveVector<T>& contactpos, RaveVector<T>& conta ctnorm)
{ {
// triangle triangle collision test - by Rosen Diankov // triangle triangle collision test - by Rosen Diankov
// first see if the faces intersect the planes // first see if the faces intersect the planes
// for the face to be intersecting the plane, one of its // for the face to be intersecting the plane, one of its
// vertices must be on the opposite side of the plane // vertices must be on the opposite side of the plane
char b = 0; char b = 0;
RaveVector<T> u12 = u2 - u1, u23 = u3 - u2, u31 = u1 - u3; RaveVector<T> u12 = u2 - u1, u31 = u1 - u3;
RaveVector<T> v12 = v2 - v1, v23 = v3 - v2, v31 = v1 - v3; RaveVector<T> v12 = v2 - v1, v23 = v3 - v2, v31 = v1 - v3;
RaveVector<T> vedges[3] = { v12, v23, v31}; RaveVector<T> vedges[3] = { v12, v23, v31};
RaveVector<T> unorm, vnorm; RaveVector<T> unorm, vnorm;
unorm = u31.cross(u12); unorm = u31.cross(u12);
unorm.w = -unorm.dot3(u1); unorm.w = -unorm.dot3(u1);
vnorm = v31.cross(v12); vnorm = v31.cross(v12);
vnorm.w = -vnorm.dot3(v1); vnorm.w = -vnorm.dot3(v1);
if( vnorm.dot3(u1) + vnorm.w > 0 ) { if( vnorm.dot3(u1) + vnorm.w > 0 ) {
b |= 1; b |= 1;
} }
 End of changes. 3 change blocks. 
3 lines changed or deleted 3 lines changed or added


 iksolver.h   iksolver.h 
skipping to change at line 39 skipping to change at line 39
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)
IKFO_IgnoreCustomFilter=8, ///< will not use the custom filter, even if IKFO_IgnoreCustomFilters=8, ///< will not use the custom filter, even i
one is set 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.
}; };
/** \brief <b>[interface]</b> Base class for all Inverse Kinematic solvers. See \ref arch_iksolver. /** \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:
/** Inverse kinematics filter callback function. /** Inverse kinematics filter callback function.
The filter is of the form <tt>return = filterfn(solution, manipulat or, param)</tt>. The filter is of the form <tt>return = filterfn(solution, manipulat or, param)</tt>.
The solution is guaranteed to be set on the robot's joint values be fore this function is called. The solution is guaranteed to be set on the robot's joint values be fore this function is called.
If modifying the robot state, should restore it before this functio n returns. If modifying the robot state, should restore it before this functio n returns.
\param solution The current solution of the manipulator. Can be mod ified by this function, but note that it will not go through previous check s again. \param solution The current solution of the manipulator. Can be mod ified by this function, but note that it will not go through previous check s again.
\param manipulator The current manipulator that the ik is being sol ved for. \param manipulator The current manipulator that the ik is being sol ved for.
\param param The paramterization that IK was called with. This is i n the manipulator base link's coordinate system (which is not necessarily t he world coordinate system). \param param The paramterization that IK was called with. This is i n the manipulator base link's coordinate system (which is not necessarily t he world coordinate system).
\return \ref IkFilterReturn controlling the behavior of the ik sear ch process. \return \ref IkFilterReturn controlling the behavior of the ik sear ch process.
*/ */
typedef boost::function<IkFilterReturn(std::vector<dReal>&, RobotBase:: ManipulatorPtr, const IkParameterization&)> IkFilterCallbackFn; typedef boost::function<IkFilterReturn(std::vector<dReal>&, RobotBase:: ManipulatorConstPtr, const IkParameterization&)> IkFilterCallbackFn;
IkSolverBase(EnvironmentBasePtr penv) : InterfaceBase(PT_InverseKinemat icsSolver, penv) { IkSolverBase(EnvironmentBasePtr penv) : InterfaceBase(PT_InverseKinemat icsSolver, penv) {
} }
virtual ~IkSolverBase() { virtual ~IkSolverBase() {
} }
/// return the static interface type this class points to (used for saf e casting) /// return the static interface type this class points to (used for saf e casting)
static inline InterfaceType GetInterfaceTypeStatic() { static inline InterfaceType GetInterfaceTypeStatic() {
return PT_InverseKinematicsSolver; return PT_InverseKinematicsSolver;
} }
/// sets the IkSolverBase attached to a specific robot and sets IkSolve /// brief Sets the IkSolverBase attached to a specific robot and sets I
rBase specific options kSolverBase specific options.
///
/// For example, some ik solvers might have different ways of computing optimal solutions. /// For example, some ik solvers might have different ways of computing optimal solutions.
/// \param pmanip The manipulator the IK solver is attached to /// \param pmanip The manipulator the IK solver is attached to
virtual bool Init(RobotBase::ManipulatorPtr pmanip) = 0; virtual bool Init(RobotBase::ManipulatorPtr pmanip) = 0;
virtual RobotBase::ManipulatorPtr GetManipulator() const = 0; virtual RobotBase::ManipulatorPtr GetManipulator() const = 0;
/// \brief Sets an ik solution filter that is called for every ik solut /** \brief Sets an ik solution filter that is called for every ik solut
ion. ion.
///
/// \param filterfn - an optional filter function to be called, see \re Multiple filters can be set at once, each filter will be called acc
f IkFilterCallbackFn. ording to its priority; higher values get called first. The default impleme
/// \exception openrave_exception Throw if filters are not supported. ntation of IkSolverBase manages the filters internally. Users implementing
virtual void SetCustomFilter(const IkFilterCallbackFn& filterfn) OPENRA their own IkSolverBase should call \ref _CallFilters to run the internally
VE_DUMMY_IMPLEMENTATION; managed filters.
\param filterfn - an optional filter function to be called, see \re
f IkFilterCallbackFn.
\param priority - The priority of the filter that controls the orde
r in which filters get called. Higher priority filters get called first. If
not certain what to set, use 0.
\return a managed handle to the filter. If this handle is released,
then the fitler will be removed. Release operation is <b>[multi-thread saf
e]</b>.
*/
virtual UserDataPtr RegisterCustomFilter(int priority, const IkFilterCa
llbackFn& filterfn);
/// \deprecated (11/09/21)
virtual void SetCustomFilter(const IkFilterCallbackFn& filterfn) RAVE_D
EPRECATED
{
RAVELOG_WARN("IkSolverBase::SetCustomFilter is deprecated, have to
use handle=AddCustomFilter. This call will will leak memory\n");
if( __listRegisteredFilters.size() > 0 ) {
RAVELOG_WARN("IkSolverBase::SetCustomFilter is deprecated, dele
ting all current filters!\n");
}
new UserDataPtr(RegisterCustomFilter(0,filterfn));
}
/// \brief Number of free parameters defining the null solution space. /// \brief Number of free parameters defining the null solution space.
/// ///
/// Each parameter is always in the range of [0,1]. /// Each parameter is always in the range of [0,1].
virtual int GetNumFreeParameters() const = 0; virtual int GetNumFreeParameters() const = 0;
/// \brief gets the free parameters from the current robot configuratio /** \brief gets the free parameters from the current robot configuratio
n n
///
/// \param[out] vFreeParameters is filled with GetNumFreeParameters() p \param[out] vFreeParameters is filled with GetNumFreeParameters() p
arameters in [0,1] range arameters in [0,1] range
/// \return true if succeeded \return true if succeeded
*/
virtual bool GetFreeParameters(std::vector<dReal>& vFreeParameters) con st = 0; virtual bool GetFreeParameters(std::vector<dReal>& vFreeParameters) con st = 0;
/// Return a joint configuration for the given end effector transform. /** \brief Return a joint configuration for the given end effector tran
Robot is checked for self-collisions. sform.
/// \param[in] param the pose the end effector has to achieve. Note tha
t the end effector pose \param[in] param the pose the end effector has to achieve. Note tha
/// takes into account the grasp coordinate fram t the end effector pose
e for the RobotBase::Manipulator takes into account the grasp coordinate frame for the RobotBase::Ma
/// \param[in] q0 Return a solution nearest to the given configuration nipulator
q0 in terms of the joint distance. \param[in] q0 Return a solution nearest to the given configuration
/// If q0 is NULL, returns the first solution found q0 in terms of the joint distance. If q0 is NULL, returns the first solutio
/// \param[in] filteroptions A bitmask of \ref IkFilterOptions values c n found
ontrolling what is checked for each ik solution. \param[in] filteroptions A bitmask of \ref IkFilterOptions values c
/// \param[out] solution [optional] Holds the IK solution ontrolling what is checked for each ik solution.
/// \return true if solution is found \param[out] solution [optional] Holds the IK solution
\return true if solution is found
*/
virtual bool Solve(const IkParameterization& param, const std::vector<d Real>& q0, int filteroptions, boost::shared_ptr< std::vector<dReal> > solut ion) = 0; virtual bool Solve(const IkParameterization& param, const std::vector<d Real>& q0, int filteroptions, boost::shared_ptr< std::vector<dReal> > solut ion) = 0;
/// Return all joint configurations for the given end effector transfor /** \brief Return all joint configurations for the given end effector t
m. Robot is checked for self-collisions. ransform.
/// \param[in] param the pose the end effector has to achieve. Note tha
t the end effector pose \param[in] param the pose the end effector has to achieve. Note that
/// takes into account the grasp coordinate fram the end effector pose
e for the RobotBase::Manipulator takes into account the grasp coordinate frame for the RobotBase::Man
/// \param[in] filteroptions A bitmask of \ref IkFilterOptions values c ipulator
ontrolling what is checked for each ik solution. \param[in] filteroptions A bitmask of \ref IkFilterOptions values co
/// \param[out] solutions All solutions within a reasonable discretizat ntrolling what is checked for each ik solution.
ion level of the free parameters. \param[out] solutions All solutions within a reasonable discretizati
/// \return true if at least one solution is found on level of the free parameters.
\return true if at least one solution is found
*/
virtual bool Solve(const IkParameterization& param, int filteroptions, std::vector< std::vector<dReal> >& solutions) = 0; virtual bool Solve(const IkParameterization& param, int filteroptions, std::vector< std::vector<dReal> >& solutions) = 0;
/// Return a joint configuration for the given end effector transform. /** Return a joint configuration for the given end effector transform.
Robot is checked for self-collisions.
/// Can specify the free parameters in [0,1] range. If NULL, the regula Can specify the free parameters in [0,1] range. If NULL, the regula
r equivalent Solve is called r equivalent Solve is called
/// \param[in] param the pose the end effector has to achieve. Note tha \param[in] param the pose the end effector has to achieve. Note tha
t the end effector pose t the end effector pose takes into account the grasp coordinate frame for t
/// takes into account the grasp coordinate fram he RobotBase::Manipulator
e for the RobotBase::Manipulator \param[in] q0 Return a solution nearest to the given configuration
/// \param[in] q0 Return a solution nearest to the given configuration q0 in terms of the joint distance. If q0 is empty, returns the first soluti
q0 in terms of the joint distance. on found
/// If q0 is empty, returns the first solution found \param[in] vFreeParameters The free parameters of the null space of
/// \param[in] vFreeParameters The free parameters of the null space of the IK solutions. Always in range of [0,1]
the IK solutions. Always in range of [0,1] \param[in] filteroptions A bitmask of \ref IkFilterOptions values c
/// \param[in] filteroptions A bitmask of \ref IkFilterOptions values c ontrolling what is checked for each ik solution.
ontrolling what is checked for each ik solution. \param[out] solution Holds the IK solution, must be of size RobotBa
/// \param[out] solution Holds the IK solution, must be of size RobotBa se::Manipulator::_vecarmjoints
se::Manipulator::_vecarmjoints \return true if solution is found
/// \return true if solution is found */
virtual bool Solve(const IkParameterization& param, const std::vector<d Real>& q0, const std::vector<dReal>& vFreeParameters, int filteroptions, bo ost::shared_ptr< std::vector<dReal> > solution) = 0; virtual bool Solve(const IkParameterization& param, const std::vector<d Real>& q0, const std::vector<dReal>& vFreeParameters, int filteroptions, bo ost::shared_ptr< std::vector<dReal> > solution) = 0;
/// Return all joint configurations for the given end effector transfor /** \brief Return all joint configurations for the given end effector t
m. Robot is checked for self-collisions. ransform.
/// Can specify the free parameters in [0,1] range. If NULL, the regula
r equivalent Solve is called Can specify the free parameters in [0,1] range. If NULL, the regula
/// \param[in] param the pose the end effector has to achieve. Note tha r equivalent Solve is called
t the end effector pose \param[in] param the pose the end effector has to achieve. Note tha
/// takes into account the grasp coordinate fram t the end effector pose
e for the RobotBase::Manipulator takes into account the grasp coordinate frame for the RobotBase::Ma
/// \param[in] vFreeParameters The free parameters of the null space of nipulator
the IK solutions. Always in range of [0,1] \param[in] vFreeParameters The free parameters of the null space of
/// \param[in] filteroptions A bitmask of \ref IkFilterOptions values c the IK solutions. Always in range of [0,1]
ontrolling what is checked for each ik solution. \param[in] filteroptions A bitmask of \ref IkFilterOptions values c
/// \param[out] solutions All solutions within a reasonable discretizat ontrolling what is checked for each ik solution.
ion level of the free parameters. \param[out] solutions All solutions within a reasonable discretizat
/// \return true at least one solution is found ion level of the free parameters.
\return true at least one solution is found
*/
virtual bool Solve(const IkParameterization& param, const std::vector<d Real>& vFreeParameters, int filteroptions, std::vector< std::vector<dReal> >& solutions) = 0; virtual bool Solve(const IkParameterization& param, const std::vector<d Real>& vFreeParameters, int filteroptions, std::vector< std::vector<dReal> >& solutions) = 0;
/// \brief returns true if the solver supports a particular ik paramete rization as input. /// \brief returns true if the solver supports a particular ik paramete rization as input.
virtual bool Supports(IkParameterization::Type iktype) const OPENRAVE_D virtual bool Supports(IkParameterizationType iktype) const OPENRAVE_DUM
UMMY_IMPLEMENTATION; MY_IMPLEMENTATION;
protected:
inline IkSolverBasePtr shared_iksolver() {
return boost::static_pointer_cast<IkSolverBase>(shared_from_this())
;
}
inline IkSolverBaseConstPtr shared_iksolver_const() const {
return boost::static_pointer_cast<IkSolverBase const>(shared_from_t
his());
}
/// \brief calls the registered filters in their priority order and ret
urns the value of the last called filter.
virtual IkFilterReturn _CallFilters(std::vector<dReal>& solution, Robot
Base::ManipulatorPtr manipulator, const IkParameterization& param);
private: private:
virtual const char* GetHash() const { virtual const char* GetHash() const {
return OPENRAVE_IKSOLVER_HASH; return OPENRAVE_IKSOLVER_HASH;
} }
std::list<UserDataWeakPtr> __listRegisteredFilters; ///< internally man
aged filters
friend class CustomIkSolverFilterData;
}; };
} // end namespace OpenRAVE } // end namespace OpenRAVE
#endif #endif
 End of changes. 12 change blocks. 
79 lines changed or deleted 138 lines changed or added


 interface.h   interface.h 
skipping to change at line 82 skipping to change at line 82
} }
inline XMLReadablePtr GetReadableInterface(const std::string& xmltag) c onst inline XMLReadablePtr GetReadableInterface(const std::string& xmltag) c onst
{ {
READERSMAP::const_iterator it = __mapReadableInterfaces.find(xmltag ); READERSMAP::const_iterator it = __mapReadableInterfaces.find(xmltag );
return it != __mapReadableInterfaces.end() ? it->second : XMLReadab lePtr(); return it != __mapReadableInterfaces.end() ? it->second : XMLReadab lePtr();
} }
/// \brief Documentation of the interface in reStructuredText format. S ee \ref writing_plugins_doc. /// \brief Documentation of the interface in reStructuredText format. S ee \ref writing_plugins_doc.
virtual const std::string& GetDescription() const { virtual const std::string& GetDescription() const {
return __description; return __description;
}; }
virtual void SetDescription(const std::string& description) {
__description = description;
}
/// \brief set user data /// \brief set user data
virtual void SetUserData(UserDataPtr data) { virtual void SetUserData(UserDataPtr data) {
__pUserData = data; __pUserData = data;
} }
/// \deprecated /// \deprecated
virtual void SetUserData(boost::shared_ptr<void> data) RAVE_DEPRECATED { virtual void SetUserData(boost::shared_ptr<void> data) RAVE_DEPRECATED {
__pUserData = boost::static_pointer_cast<UserData>(data); __pUserData = boost::static_pointer_cast<UserData>(data);
} }
/// \brief return the user custom data /// \brief return the user custom data
skipping to change at line 184 skipping to change at line 188
READERSMAP __mapReadableInterfaces; ///< pointers to extra interfaces t hat are included with this object READERSMAP __mapReadableInterfaces; ///< pointers to extra interfaces t hat are included with this object
typedef std::map<std::string, boost::shared_ptr<InterfaceCommand>, Case InsensitiveCompare> CMDMAP; typedef std::map<std::string, boost::shared_ptr<InterfaceCommand>, Case InsensitiveCompare> CMDMAP;
CMDMAP __mapCommands; ///< all registered commands CMDMAP __mapCommands; ///< all registered commands
#ifdef RAVE_PRIVATE #ifdef RAVE_PRIVATE
#ifdef _MSC_VER #ifdef _MSC_VER
friend class Environment; friend class Environment;
friend class OpenRAVEXMLParser::InterfaceXMLReader; friend class OpenRAVEXMLParser::InterfaceXMLReader;
friend class ColladaReader; friend class ColladaReader;
friend class XFileReader;
#else #else
friend class ::Environment; friend class ::Environment;
friend class ::OpenRAVEXMLParser::InterfaceXMLReader; friend class ::OpenRAVEXMLParser::InterfaceXMLReader;
friend class ::ColladaReader; friend class ::ColladaReader;
friend class ::XFileReader;
#endif #endif
#endif #endif
friend class RaveDatabase; friend class RaveDatabase;
}; };
} // end namespace OpenRAVE } // end namespace OpenRAVE
#endif #endif
 End of changes. 3 change blocks. 
1 lines changed or deleted 7 lines changed or added


 interfacehashes.h   interfacehashes.h 
#define OPENRAVE_COLLISIONCHECKER_HASH "f0e1d66efd6b80e851a12a937f60eca6" #define OPENRAVE_COLLISIONCHECKER_HASH "690784eb9e41b068a893dbf1761c01d1"
#define OPENRAVE_ROBOT_HASH "bee29611dc861971dc759376933d69cf" #define OPENRAVE_ROBOT_HASH "daecf7d25afb90fef5dd9987f5c98f5b"
#define OPENRAVE_PLANNER_HASH "7b8864032c6bf5c572e0535c9c164fd4" #define OPENRAVE_PLANNER_HASH "6fc740f9b2bbfde93d78032cbf1b9121"
#define OPENRAVE_KINBODY_HASH "2d1499ecb721b3af313da4c26b836ce2" #define OPENRAVE_KINBODY_HASH "39cb185f844b5607190cd761199c7ae2"
#define OPENRAVE_SENSORSYSTEM_HASH "43920009ad109091d7e59157a5af3a24" #define OPENRAVE_SENSORSYSTEM_HASH "83208394c99a021fbdcd55ac2810e65f"
#define OPENRAVE_CONTROLLER_HASH "3ff86056459c72cbdb34af28b98f0992" #define OPENRAVE_CONTROLLER_HASH "4b94816a0f156bc4a3e98f79f3898683"
#define OPENRAVE_MODULE_HASH "91a6adfcb28019207938af3fbbf759b3" #define OPENRAVE_MODULE_HASH "fec1b591cb5ef03912009f9abdcd037d"
#define OPENRAVE_IKSOLVER_HASH "721e42b775ddd137a81e53f69519d1d9" #define OPENRAVE_IKSOLVER_HASH "9fd7944dbecc4eed8cceb4f6d5a618ce"
#define OPENRAVE_PHYSICSENGINE_HASH "31b1aa40bf4c5b63aa57c5552bf62d81" #define OPENRAVE_PHYSICSENGINE_HASH "257dd698a5b7a78b8bd4d05a3d322c0b"
#define OPENRAVE_SENSOR_HASH "c7227d59d84d6ad880ea5ae166cc087a" #define OPENRAVE_SENSOR_HASH "1425016f0176ff3811962fbefd63c31e"
#define OPENRAVE_TRAJECTORY_HASH "260138467755d029a252ef128993907e" #define OPENRAVE_TRAJECTORY_HASH "c386a85687d6dea075d3d9ed1c68b808"
#define OPENRAVE_VIEWER_HASH "5770b43a6028630f89678e29a65ee275" #define OPENRAVE_VIEWER_HASH "c58fb4ff10585995bc49e53f3175b473"
#define OPENRAVE_SPACESAMPLER_HASH "8d3866c955b9587aaf42e277edd2da2a" #define OPENRAVE_SPACESAMPLER_HASH "3b8faec230636cab285670c095c95496"
#define OPENRAVE_ENVIRONMENT_HASH "f29fda952d7a5cef7b8b46a7e4f70a27" #define OPENRAVE_ENVIRONMENT_HASH "bee66712a5dc2ac11644ab32817ee06a"
#define OPENRAVE_PLUGININFO_HASH "202a84af5eb50775aa5da86956de73fe" #define OPENRAVE_PLUGININFO_HASH "8d45441947223c660c8f8ff9723bf58e"
 End of changes. 1 change blocks. 
lines changed or deleted lines changed or added


 kinbody.h   kinbody.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 kinbody.h /** \file kinbody.h
\brief Kinematics body related definitions. \brief Kinematics body related definitions.
*/ */
#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>Meth ods 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.
\ingroup interfaces \ingroup interfaces
*/ */
class OPENRAVE_API KinBody : public InterfaceBase class OPENRAVE_API KinBody : public InterfaceBase
{ {
public: public:
/// \brief A set of properties for the kinbody. These properties are us ed to describe a set of variables used in KinBody. /// \brief A set of properties for the kinbody. These properties are us ed to describe a set of variables used in KinBody.
enum KinBodyProperty { enum KinBodyProperty {
Prop_Joints=0x1, ///< all properties of all joints Prop_Joints=0x1, ///< all properties of all joints
Prop_JointLimits=0x2|Prop_Joints, ///< regular limits Prop_JointLimits=0x2|Prop_Joints, ///< regular limits
Prop_JointOffset=0x4|Prop_Joints, Prop_JointOffset=0x4|Prop_Joints,
Prop_JointProperties=0x8|Prop_Joints, ///< max velocity, max ac celeration, resolution, max torque Prop_JointProperties=0x8|Prop_Joints, ///< max velocity, max ac celeration, resolution, max torque
Prop_Links=0x10, ///< all properties of all links Prop_Links=0x10, ///< all properties of all links
Prop_Name=0x20, ///< name changed Prop_Name=0x20, ///< name changed
Prop_LinkDraw=0x40, ///< toggle link geometries rendering Prop_LinkDraw=0x40, ///< toggle link geometries rendering
Prop_LinkGeometry=0x80|Prop_Links, ///< the geometry of the lin k changed Prop_LinkGeometry=0x80|Prop_Links, ///< the geometry of the lin k changed
Prop_JointMimic=0x100|Prop_Joints, ///< joint mimic equations Prop_JointMimic=0x100|Prop_Joints, ///< joint mimic equations
Prop_JointVelocityLimits=0x200|Prop_Joints, ///< velocity limit s Prop_JointAccelerationVelocityTorqueLimits=0x200|Prop_Joints, / //< velocity + acceleration + torque
Prop_LinkStatic=0x400|Prop_Links, ///< static property of link changed Prop_LinkStatic=0x400|Prop_Links, ///< static property of link changed
// robot only // robot only
Prop_RobotManipulators = 0x00010000, ///< [robot only] all prop erties of all manipulators Prop_RobotManipulators = 0x00010000, ///< [robot only] all prop erties of all manipulators
Prop_Manipulators = 0x00010000, Prop_Manipulators = 0x00010000,
Prop_RobotSensors = 0x00020000, ///< [robot only] all propertie s of all sensors Prop_RobotSensors = 0x00020000, ///< [robot only] all propertie s of all sensors
Prop_Sensors = 0x00020000, Prop_Sensors = 0x00020000,
Prop_RobotSensorPlacement = 0x00040000, ///< [robot only] relat ive sensor placement of sensors Prop_RobotSensorPlacement = 0x00040000, ///< [robot only] relat ive sensor placement of sensors
Prop_SensorPlacement = 0x00040000, Prop_SensorPlacement = 0x00040000,
Prop_RobotActiveDOFs = 0x00080000, ///< [robot only] active dof s changed Prop_RobotActiveDOFs = 0x00080000, ///< [robot only] active dof s changed
Prop_RobotManipulatorTool = 0x00100000, ///< [robot only] the tool
coordinate system changed
Prop_RobotManipulatorName = 0x00200000, ///< [robot only] the manip
ulator name
}; };
/// \brief A rigid body holding all its collision and rendering data. /// \brief A rigid body holding all its collision and rendering data.
class OPENRAVE_API Link : public boost::enable_shared_from_this<Link> class OPENRAVE_API Link : public boost::enable_shared_from_this<Link>
{ {
public: public:
Link(KinBodyPtr parent); ///< pass in a ODE world Link(KinBodyPtr parent); ///< pass in a ODE world
virtual ~Link(); virtual ~Link();
/// \brief User data for trimesh geometries. Vertices are defined i n counter-clockwise order for outward pointing faces. /// \brief User data for trimesh geometries. Vertices are defined i n counter-clockwise order for outward pointing faces.
skipping to change at line 113 skipping to change at line 115
/// \brief Local transformation of the geom primitive with resp ect to the link's coordinate system. /// \brief Local transformation of the geom primitive with resp ect to the link's coordinate system.
inline const Transform& GetTransform() const { inline const Transform& GetTransform() const {
return _t; return _t;
} }
inline GeomType GetType() const { inline GeomType GetType() const {
return _type; return _type;
} }
inline const Vector& GetRenderScale() const { inline const Vector& GetRenderScale() const {
return vRenderScale; return vRenderScale;
} }
/// \brief render resource file, should be transformed by _t be
fore rendering
///
/// If the value is "__norenderif__:x", then the viewer should
not render the object if it supports *.x files where"x" is the file extensi
on.
inline const std::string& GetRenderFilename() const { inline const std::string& GetRenderFilename() const {
return _renderfilename; return _renderfilename;
} }
inline float GetTransparency() const { inline float GetTransparency() const {
return ftransparency; return ftransparency;
} }
inline bool IsDraw() const { /// \deprecated (12/1/12)
return _bDraw; inline bool IsDraw() const RAVE_DEPRECATED {
return _bVisible;
}
inline bool IsVisible() const {
return _bVisible;
} }
inline bool IsModifiable() const { inline bool IsModifiable() const {
return _bModifiable; return _bModifiable;
} }
inline dReal GetSphereRadius() const { inline dReal GetSphereRadius() const {
return vGeomData.x; return vGeomData.x;
} }
inline dReal GetCylinderRadius() const { inline dReal GetCylinderRadius() const {
return vGeomData.x; return vGeomData.x;
skipping to change at line 159 skipping to change at line 169
inline const TRIMESH& GetCollisionMesh() const { inline const TRIMESH& GetCollisionMesh() const {
return collisionmesh; return collisionmesh;
} }
/// \brief returns an axis aligned bounding box given that the geometry is transformed by trans /// \brief returns an axis aligned bounding box given that the geometry is transformed by trans
virtual AABB ComputeAABB(const Transform& trans) const; virtual AABB ComputeAABB(const Transform& trans) const;
virtual void serialize(std::ostream& o, int options) const; virtual void serialize(std::ostream& o, int options) const;
/// \brief sets a new collision mesh and notifies every registe red callback about it /// \brief sets a new collision mesh and notifies every registe red callback about it
virtual void SetCollisionMesh(const TRIMESH& mesh); virtual void SetCollisionMesh(const TRIMESH& mesh);
/// \brief sets a drawing and notifies every registered callbac /// \brief sets visible flag. if changed, notifies every regist
k about it ered callback about it.
virtual void SetDraw(bool bDraw); ///
/// \return true if changed
virtual bool SetVisible(bool visible);
/// \deprecated (12/1/12)
inline void SetDraw(bool bDraw) RAVE_DEPRECATED {
SetVisible(bDraw);
}
/// \brief set transparency level (0 is opaque) /// \brief set transparency level (0 is opaque)
virtual void SetTransparency(float f); virtual void SetTransparency(float f);
/// \brief override diffuse color of geometry material /// \brief override diffuse color of geometry material
virtual void SetDiffuseColor(const RaveVector<float>& color); virtual void SetDiffuseColor(const RaveVector<float>& color);
/// \brief override ambient color of geometry material /// \brief override ambient color of geometry material
virtual void SetAmbientColor(const RaveVector<float>& color); virtual void SetAmbientColor(const RaveVector<float>& color);
/// \brief validates the contact normal on the surface of the g eometry and makes sure the normal faces "outside" of the shape. /// \brief validates the contact normal on the surface of the g eometry and makes sure the normal faces "outside" of the shape.
/// ///
/// \param position the position of the contact point specified in the link's coordinate system /// \param position the position of the contact point specified in the link's coordinate system
skipping to change at line 184 skipping to change at line 200
/// \brief sets a new render filename for the geometry. This do es not change the collision /// \brief sets a new render filename for the geometry. This do es not change the collision
virtual void SetRenderFilename(const std::string& renderfilenam e); virtual void SetRenderFilename(const std::string& renderfilenam e);
protected: protected:
/// triangulates the geometry object and initializes collisionm esh. GeomTrimesh types must already be triangulated /// triangulates the geometry object and initializes collisionm esh. GeomTrimesh types must already be triangulated
/// \param fTessellation to control how fine the triangles need to be. 1.0f is the default value /// \param fTessellation to control how fine the triangles need to be. 1.0f is the default value
bool InitCollisionMesh(float fTessellation=1); bool InitCollisionMesh(float fTessellation=1);
boost::weak_ptr<Link> _parent; boost::weak_ptr<Link> _parent;
Transform _t; ///< see \ref GetTransform Transform _t; ///< see \ref GetTransform
Vector vGeomData; ///< for boxes, first 3 values ar Vector vGeomData; ///< for boxes, first 3 values are extents
e extents
///< for sphere it is radius ///< for sphere it is radius
///< for cylinder, first 2 values are radius and height ///< for cylinder, first 2 values are radius and height
///< for trimesh, none ///< for trimesh, none
RaveVector<float> diffuseColor, ambientColor; ///< RaveVector<float> diffuseColor, ambientColor; ///< hints for ho
hints for how to color the meshes w to color the meshes
TRIMESH collisionmesh; ///< see \ref GetCollisionMe TRIMESH collisionmesh; ///< see \ref GetCollisionMesh
sh GeomType _type; ///< the type of geometry primitive
GeomType _type; ///< the type of geometry p std::string _renderfilename; ///< \see ref GetRenderFilename
rimitive Vector vRenderScale; ///< render scale of the object (x,y,z)
std::string _renderfilename; ///< render resource float ftransparency; ///< value from 0-1 for the transparency o
file, should be transformed by _t before rendering f the rendered object, 0 is opaque
Vector vRenderScale; ///< render scale of the objec
t (x,y,z)
float ftransparency; ///< value from 0-1 for the tr
ansparency of the rendered object, 0 is opaque
bool _bDraw; ///< if true, object is drawn bool _bVisible; ///< if true, geometry is visible as part of th
as part of the 3d model (default is true) e 3d model (default is true)
bool _bModifiable; ///< if true, object geometry ca bool _bModifiable; ///< if true, object geometry can be dynamic
n be dynamically modified (default is true) ally modified (default is true)
#ifdef RAVE_PRIVATE #ifdef RAVE_PRIVATE
#ifdef _MSC_VER #ifdef _MSC_VER
friend class ColladaReader; friend class ColladaReader;
friend class OpenRAVEXMLParser::LinkXMLReader; friend class OpenRAVEXMLParser::LinkXMLReader;
friend class OpenRAVEXMLParser::KinBodyXMLReader; friend class OpenRAVEXMLParser::KinBodyXMLReader;
friend class XFileReader;
#else #else
friend class ::ColladaReader; friend class ::ColladaReader;
friend class ::OpenRAVEXMLParser::LinkXMLReader; friend class ::OpenRAVEXMLParser::LinkXMLReader;
friend class ::OpenRAVEXMLParser::KinBodyXMLReader; friend class ::OpenRAVEXMLParser::KinBodyXMLReader;
friend class ::XFileReader;
#endif #endif
#endif #endif
friend class KinBody; friend class KinBody;
friend class KinBody::Link; friend class KinBody::Link;
}; };
inline const std::string& GetName() const { inline const std::string& GetName() const {
return _name; return _name;
} }
/// \brief Indicates a static body that does not move with respect to the root link. /// \brief Indicates a static body that does not move with respect to the root link.
/// ///
//// Static should be used when an object has infinite mass and //// Static should be used when an object has infinite mass and
///< shouldn't be affected by physics (including gravity). Collisio n still works. ///< shouldn't be affected by physics (including gravity). Collisio n still works.
inline bool IsStatic() const { inline bool IsStatic() const {
return _bStatic; return _bStatic;
} }
/// \brief Enables a Link. An enabled link takes part in collision
detection and physics simulations
virtual void Enable(bool enable);
/// \brief returns true if the link is enabled. \see Enable /// \brief returns true if the link is enabled. \see Enable
virtual bool IsEnabled() const; virtual bool IsEnabled() const;
/// \brief Enables a Link. An enabled link takes part in collision /// \brief Sets all the geometries of the link as visible or non vi
detection and physics simulations sible.
virtual void Enable(bool enable); ///
/// \return true if changed
virtual bool SetVisible(bool visible);
/// \return true if any geometry of the link is visible.
virtual bool IsVisible() const;
/// \brief parent body that link belong to. /// \brief parent body that link belong to.
inline KinBodyPtr GetParent() const { inline KinBodyPtr GetParent() const {
return KinBodyPtr(_parent); return KinBodyPtr(_parent);
} }
/// \brief unique index into parent KinBody::GetLinks vector /// \brief unique index into parent KinBody::GetLinks vector
inline int GetIndex() const { inline int GetIndex() const {
return _index; return _index;
} }
skipping to change at line 266 skipping to change at line 292
/// to compute this link's transformation from its parent. /// to compute this link's transformation from its parent.
/// \param[out] filled with the parent links /// \param[out] filled with the parent links
virtual void GetParentLinks(std::vector< boost::shared_ptr<Link> >& vParentLinks) const; virtual void GetParentLinks(std::vector< boost::shared_ptr<Link> >& vParentLinks) const;
/// \brief Tests if a link is a direct parent. /// \brief Tests if a link is a direct parent.
/// ///
/// \see GetParentLinks /// \see GetParentLinks
/// \param link The link to test if it is one of the parents of thi s link. /// \param link The link to test if it is one of the parents of thi s link.
virtual bool IsParentLink(boost::shared_ptr<Link const> plink) cons t; virtual bool IsParentLink(boost::shared_ptr<Link const> plink) cons t;
/// \return center of mass offset in the link's local coordinate fr /// \brief return center of mass offset in the link's local coordin
ame ate frame
inline Vector GetLocalCOM() const {
return _tMassFrame.trans;
}
/// \brief return center of mass of the link in the global coordina
te system
inline Vector GetGlobalCOM() const {
return _t*_tMassFrame.trans;
}
inline Vector GetCOMOffset() const { inline Vector GetCOMOffset() const {
return _transMass.trans; return _tMassFrame.trans;
} }
inline const TransformMatrix& GetInertia() const {
return _transMass; /// \brief return inertia in link's local coordinate frame. The COM
is GetLocalCOM()
virtual TransformMatrix GetLocalInertia() const;
// \brief return inertia in the global coordinate frame. The COM is
GetCOM()
virtual TransformMatrix GetGlobalInertia() const;
/// \deprecated (12/1/20)
inline TransformMatrix GetInertia() const RAVE_DEPRECATED {
RAVELOG_WARN("KinBody::Link::GetInertia is deprecated, use KinB
ody::Link::GetGlobalInertia\n");
return GetLocalInertia();
}
/// \brief return the mass frame in the link's local coordinate sys
tem that holds the center of mass and principal axes for inertia.
inline const Transform& GetLocalMassFrame() const {
return _tMassFrame;
}
/// \brief return the mass frame in the global coordinate system th
at holds the center of mass and principal axes for inertia.
inline Transform GetGlobalMassFrame() const {
return _t*_tMassFrame;
}
/// \brief return the principal moments of inertia inside the mass
frame
inline const Vector& GetPrincipalMomentsOfInertia() const {
return _vinertiamoments;
} }
inline dReal GetMass() const { inline dReal GetMass() const {
return _mass; return _mass;
} }
/// \brief sets a link to be static. /// \brief sets a link to be static.
/// ///
/// Because this can affect the kinematics, it requires the body's internal structures to be recomputed /// Because this can affect the kinematics, it requires the body's internal structures to be recomputed
virtual void SetStatic(bool bStatic); virtual void SetStatic(bool bStatic);
skipping to change at line 342 skipping to change at line 401
/// \param vattachedlinks the array to insert all links attached to linkindex with the link itself. /// \param vattachedlinks the array to insert all links attached to linkindex with the link itself.
virtual void GetRigidlyAttachedLinks(std::vector<boost::shared_ptr< Link> >& vattachedlinks) const; virtual void GetRigidlyAttachedLinks(std::vector<boost::shared_ptr< Link> >& vattachedlinks) const;
virtual void serialize(std::ostream& o, int options) const; virtual void serialize(std::ostream& o, int options) const;
protected: protected:
/// \brief Updates the cached information due to changes in the col lision data. /// \brief Updates the cached information due to changes in the col lision data.
virtual void _Update(); virtual void _Update();
Transform _t; ///< \see GetTransform Transform _t; ///< \see GetTransform
TransformMatrix _transMass; ///< the 3x3 inertia and center Transform _tMassFrame; ///< the frame for inertia and center of mas
of mass of the link in the link's coordinate system s of the link in the link's coordinate system
dReal _mass; dReal _mass; ///< mass of link
Vector _vinertiamoments; ///< inertia along the axes of _tMassFrame
TRIMESH collision; ///< triangles for collision checking, t riangles are always the triangulation TRIMESH collision; ///< triangles for collision checking, t riangles are always the triangulation
///< of the body when it is at the ident ity transformation ///< of the body when it is at the ident ity transformation
std::string _name; ///< optional link name std::string _name; ///< optional link name
std::list<GEOMPROPERTIES> _listGeomProperties; ///< \see Ge tGeometries std::list<GEOMPROPERTIES> _listGeomProperties; ///< \see Ge tGeometries
bool _bStatic; ///< \see IsStatic bool _bStatic; ///< \see IsStatic
bool _bIsEnabled; ///< \see IsEnabled bool _bIsEnabled; ///< \see IsEnabled
private: private:
skipping to change at line 368 skipping to change at line 428
KinBodyWeakPtr _parent; ///< \see GetParent KinBodyWeakPtr _parent; ///< \see GetParent
std::vector<int> _vParentLinks; ///< \see GetParentLinks, I sParentLink std::vector<int> _vParentLinks; ///< \see GetParentLinks, I sParentLink
std::vector<int> _vRigidlyAttachedLinks; ///< \see IsRigidl yAttached, GetRigidlyAttachedLinks std::vector<int> _vRigidlyAttachedLinks; ///< \see IsRigidl yAttached, GetRigidlyAttachedLinks
//@} //@}
#ifdef RAVE_PRIVATE #ifdef RAVE_PRIVATE
#ifdef _MSC_VER #ifdef _MSC_VER
friend class ColladaReader; friend class ColladaReader;
friend class OpenRAVEXMLParser::LinkXMLReader; friend class OpenRAVEXMLParser::LinkXMLReader;
friend class OpenRAVEXMLParser::KinBodyXMLReader; friend class OpenRAVEXMLParser::KinBodyXMLReader;
friend class OpenRAVEXMLParser::RobotXMLReader; friend class OpenRAVEXMLParser::RobotXMLReader;
friend class XFileReader;
#else #else
friend class ::ColladaReader; friend class ::ColladaReader;
friend class ::OpenRAVEXMLParser::LinkXMLReader; friend class ::OpenRAVEXMLParser::LinkXMLReader;
friend class ::OpenRAVEXMLParser::KinBodyXMLReader; friend class ::OpenRAVEXMLParser::KinBodyXMLReader;
friend class ::OpenRAVEXMLParser::RobotXMLReader; friend class ::OpenRAVEXMLParser::RobotXMLReader;
friend class ::XFileReader;
#endif #endif
#endif #endif
friend class KinBody; friend class KinBody;
}; };
typedef boost::shared_ptr<KinBody::Link> LinkPtr; typedef boost::shared_ptr<KinBody::Link> LinkPtr;
typedef boost::shared_ptr<KinBody::Link const> LinkConstPtr; typedef boost::shared_ptr<KinBody::Link const> LinkConstPtr;
typedef boost::weak_ptr<KinBody::Link> LinkWeakPtr; typedef boost::weak_ptr<KinBody::Link> LinkWeakPtr;
/// \brief Information about a joint that controls the relationship bet ween two links. /// \brief Information about a joint that controls the relationship bet ween two links.
class OPENRAVE_API Joint : public boost::enable_shared_from_this<Joint> class OPENRAVE_API Joint : public boost::enable_shared_from_this<Joint>
skipping to change at line 532 skipping to change at line 594
/// \brief \see GetLimits /// \brief \see GetLimits
virtual void SetLimits(const std::vector<dReal>& lower, const std:: vector<dReal>& upper); virtual void SetLimits(const std::vector<dReal>& lower, const std:: vector<dReal>& upper);
/// \deprecated (11/1/1) /// \deprecated (11/1/1)
virtual void SetJointLimits(const std::vector<dReal>& lower, const std::vector<dReal>& upper) RAVE_DEPRECATED { virtual void SetJointLimits(const std::vector<dReal>& lower, const std::vector<dReal>& upper) RAVE_DEPRECATED {
SetLimits(lower,upper); SetLimits(lower,upper);
} }
/** \brief Returns the max velocities of the joint /** \brief Returns the max velocities of the joint
\param[out] vlower the lower velocity limits \param[out] the max velocity
\param[out] vupper the lower velocity limits
\param[in] bAppend if true will append to the end of the vector instead of erasing it \param[in] bAppend if true will append to the end of the vector instead of erasing it
*/ */
virtual void GetVelocityLimits(std::vector<dReal>& vmax, bool bAppe
nd=false) const;
virtual void GetVelocityLimits(std::vector<dReal>& vlower, std::vec tor<dReal>& vupper, bool bAppend=false) const; virtual void GetVelocityLimits(std::vector<dReal>& vlower, std::vec tor<dReal>& vupper, bool bAppend=false) const;
/// \brief \see GetVelocityLimits /// \brief \see GetVelocityLimits
virtual void SetVelocityLimits(const std::vector<dReal>& vmaxvel); virtual void SetVelocityLimits(const std::vector<dReal>& vmax);
/** \brief Returns the max accelerations of the joint
\param[out] the max acceleration
\param[in] bAppend if true will append to the end of the vector
instead of erasing it
*/
virtual void GetAccelerationLimits(std::vector<dReal>& vmax, bool b
Append=false) const;
/// \brief \see GetAccelerationLimits
virtual void SetAccelerationLimits(const std::vector<dReal>& vmax);
/** \brief Returns the max torques of the joint
\param[out] the max torque
\param[in] bAppend if true will append to the end of the vector
instead of erasing it
*/
virtual void GetTorqueLimits(std::vector<dReal>& vmax, bool bAppend
=false) const;
/// \brief \see GetTorqueLimits
virtual void SetTorqueLimits(const std::vector<dReal>& vmax);
/// \brief The weight associated with a joint's axis for computing a distance in the robot configuration space. /// \brief The weight associated with a joint's axis for computing a distance in the robot configuration space.
virtual dReal GetWeight(int axis=0) const; virtual dReal GetWeight(int axis=0) const;
/// \brief \see GetWeight /// \brief \see GetWeight
virtual void SetWeights(const std::vector<dReal>& weights); virtual void SetWeights(const std::vector<dReal>& weights);
/// \brief Return internal offset parameter that determines the bra nch the angle centers on /// \brief Return internal offset parameter that determines the bra nch the angle centers on
/// ///
/// Wrap offsets are needed for rotation joints since the range is limited to 2*pi. /// Wrap offsets are needed for rotation joints since the range is limited to 2*pi.
/// This allows the wrap offset to be set so the joint can function in [-pi+offset,pi+offset].. /// This allows the wrap offset to be set so the joint can function in [-pi+offset,pi+offset]..
skipping to change at line 707 skipping to change at line 790
\param plink0 the first attaching link, all axes and anchors ar e defined in its coordinate system \param plink0 the first attaching link, all axes and anchors ar e defined in its coordinate system
\param plink1 the second attaching link \param plink1 the second attaching link
\param vanchor the anchor of the rotation axes \param vanchor the anchor of the rotation axes
\param vaxes the axes in plink0's coordinate system of the join ts \param vaxes the axes in plink0's coordinate system of the join ts
\param vinitialvalues the current values of the robot used to s et the 0 offset of the robot \param vinitialvalues the current values of the robot used to s et the 0 offset of the robot
*/ */
virtual void _ComputeInternalInformation(LinkPtr plink0, LinkPtr pl ink1, const Vector& vanchor, const std::vector<Vector>& vaxes, const std::v ector<dReal>& vcurrentvalues); virtual void _ComputeInternalInformation(LinkPtr plink0, LinkPtr pl ink1, const Vector& vanchor, const std::vector<Vector>& vaxes, const std::v ector<dReal>& vcurrentvalues);
std::string _name; ///< \see GetName std::string _name; ///< \see GetName
boost::array<bool,3> _bIsCircular; ///< \see IsCircular boost::array<bool,3> _bIsCircular; ///< \see IsCircular
boost::array<int,3> _dofbranches; ///< the branch that identified j
oints are on. +1 means one loop around the identification. For revolute joi
nts, the actual joint value incremented by 2*pi*branch. Branches are import
ant for maintaining joint ranges greater than 2*pi. For circular joints, th
e branches can be ignored or not.
private: private:
/// Sensitive variables that should not be modified. /// Sensitive variables that should not be modified.
/// @name Private Joint Variables /// @name Private Joint Variables
//@{ //@{
int dofindex; ///< the degree of freedom index in the body's DOF array, does not index in KinBody::_vecjoints! int dofindex; ///< the degree of freedom index in the body's DOF array, does not index in KinBody::_vecjoints!
int jointindex; ///< the joint index into KinBody:: _vecjoints int jointindex; ///< the joint index into KinBody:: _vecjoints
JointType _type; JointType _type;
bool _bActive; ///< if true, should belong to the D OF of the body, unless it is a mimic joint (_ComputeInternalInformation dec ides this) bool _bActive; ///< if true, should belong to the D OF of the body, unless it is a mimic joint (_ComputeInternalInformation dec ides this)
KinBodyWeakPtr _parent; ///< body that joint belong t o KinBodyWeakPtr _parent; ///< body that joint belong t o
skipping to change at line 730 skipping to change at line 815
Transform _tinvRight, _tinvLeft; ///< the inverse transform ations of tRight and tLeft Transform _tinvRight, _tinvLeft; ///< the inverse transform ations of tRight and tLeft
bool _bInitialized; bool _bInitialized;
//@} //@}
#ifdef RAVE_PRIVATE #ifdef RAVE_PRIVATE
#ifdef _MSC_VER #ifdef _MSC_VER
friend class ColladaReader; friend class ColladaReader;
friend class ColladaWriter; friend class ColladaWriter;
friend class OpenRAVEXMLParser::JointXMLReader; friend class OpenRAVEXMLParser::JointXMLReader;
friend class OpenRAVEXMLParser::KinBodyXMLReader; friend class OpenRAVEXMLParser::KinBodyXMLReader;
friend class OpenRAVEXMLParser::RobotXMLReader; friend class OpenRAVEXMLParser::RobotXMLReader;
friend class XFileReader;
#else #else
friend class ::ColladaReader; friend class ::ColladaReader;
friend class ::ColladaWriter; friend class ::ColladaWriter;
friend class ::OpenRAVEXMLParser::JointXMLReader; friend class ::OpenRAVEXMLParser::JointXMLReader;
friend class ::OpenRAVEXMLParser::KinBodyXMLReader; friend class ::OpenRAVEXMLParser::KinBodyXMLReader;
friend class ::OpenRAVEXMLParser::RobotXMLReader; friend class ::OpenRAVEXMLParser::RobotXMLReader;
friend class ::XFileReader;
#endif #endif
#endif #endif
friend class KinBody; friend class KinBody;
}; };
typedef boost::shared_ptr<KinBody::Joint> JointPtr; typedef boost::shared_ptr<KinBody::Joint> JointPtr;
typedef boost::shared_ptr<KinBody::Joint const> JointConstPtr; typedef boost::shared_ptr<KinBody::Joint const> JointConstPtr;
typedef boost::weak_ptr<KinBody::Joint> JointWeakPtr; typedef boost::weak_ptr<KinBody::Joint> JointWeakPtr;
/// \brief Stores the state of the current body that is published in a thread safe way from the environment without requiring locking the environm ent. /// \brief Stores the state of the current body that is published in a thread safe way from the environment without requiring locking the environm ent.
class BodyState class BodyState
{ {
public: public:
BodyState() : environmentid(0){ BodyState() : environmentid(0){
} }
virtual ~BodyState() { virtual ~BodyState() {
} }
KinBodyPtr pbody; KinBodyPtr pbody;
std::vector<RaveTransform<dReal> > vectrans; std::vector<RaveTransform<dReal> > vectrans;
std::vector<dReal> jointvalues; std::vector<dReal> jointvalues;
UserDataPtr pguidata, puserdata; UserDataPtr pviewerdata, puserdata;
std::string strname; ///< name of the body std::string strname; ///< name of the body
int environmentid; int environmentid;
}; };
typedef boost::shared_ptr<KinBody::BodyState> BodyStatePtr; typedef boost::shared_ptr<KinBody::BodyState> BodyStatePtr;
typedef boost::shared_ptr<KinBody::BodyState const> BodyStateConstPtr; typedef boost::shared_ptr<KinBody::BodyState const> BodyStateConstPtr;
/// \brief Access point of the sensor system that manages the body. /// \brief Access point of the sensor system that manages the body.
class OPENRAVE_API ManageData : public boost::enable_shared_from_this<M anageData> class OPENRAVE_API ManageData : public boost::enable_shared_from_this<M anageData>
{ {
public: public:
skipping to change at line 824 skipping to change at line 911
}; };
/// \brief Helper class to save and restore the entire kinbody state. /// \brief Helper class to save and restore the entire kinbody state.
/// ///
/// Options can be passed to the constructor in order to choose which p arameters to save (see \ref SaveParameters) /// Options can be passed to the constructor in order to choose which p arameters to save (see \ref SaveParameters)
class OPENRAVE_API KinBodyStateSaver class OPENRAVE_API KinBodyStateSaver
{ {
public: public:
KinBodyStateSaver(KinBodyPtr pbody, int options = Save_LinkTransfor mation|Save_LinkEnable); KinBodyStateSaver(KinBodyPtr pbody, int options = Save_LinkTransfor mation|Save_LinkEnable);
virtual ~KinBodyStateSaver(); virtual ~KinBodyStateSaver();
inline KinBodyPtr GetBody() const {
return _pbody;
}
virtual void Restore();
protected: protected:
int _options; ///< saved options int _options; ///< saved options
std::vector<Transform> _vLinkTransforms; std::vector<Transform> _vLinkTransforms;
std::vector<uint8_t> _vEnabledLinks; std::vector<uint8_t> _vEnabledLinks;
std::vector<std::pair<Vector,Vector> > _vLinkVelocities; std::vector<std::pair<Vector,Vector> > _vLinkVelocities;
std::vector<int> _vdofbranches;
KinBodyPtr _pbody; KinBodyPtr _pbody;
private:
virtual void _RestoreKinBody();
}; };
typedef boost::shared_ptr<KinBodyStateSaver> KinBodyStateSaverPtr;
virtual ~KinBody(); virtual ~KinBody();
/// return the static interface type this class points to (used for saf e casting) /// return the static interface type this class points to (used for saf e casting)
static inline InterfaceType GetInterfaceTypeStatic() { static inline InterfaceType GetInterfaceTypeStatic() {
return PT_KinBody; return PT_KinBody;
} }
virtual void Destroy(); virtual void Destroy();
/// \deprecated (11/02/18) \see EnvironmentBase::ReadKinBodyXMLFile /// \deprecated (11/02/18) \see EnvironmentBase::ReadKinBodyXMLFile
virtual bool InitFromFile(const std::string& filename, const Attributes List& atts = AttributesList()) RAVE_DEPRECATED; virtual bool InitFromFile(const std::string& filename, const Attributes List& atts = AttributesList()) RAVE_DEPRECATED;
/// \deprecated (11/02/18) \see EnvironmentBase::ReadKinBodyXMLData /// \deprecated (11/02/18) \see EnvironmentBase::ReadKinBodyXMLData
virtual bool InitFromData(const std::string& data, const AttributesList & atts = AttributesList()) RAVE_DEPRECATED; virtual bool InitFromData(const std::string& data, const AttributesList & atts = AttributesList()) RAVE_DEPRECATED;
/// \brief Create a kinbody with one link composed of an array of align ed bounding boxes. /// \brief Create a kinbody with one link composed of an array of align ed bounding boxes.
/// ///
/// \param vaabbs the array of aligned bounding boxes that will compris /// \param boxes the array of aligned bounding boxes that will comprise
e of the body of the body
/// \param bDraw if true, the boxes will be rendered in the scene /// \param visible if true, the boxes will be rendered in the scene
virtual bool InitFromBoxes(const std::vector<AABB>& boxes, bool draw); virtual bool InitFromBoxes(const std::vector<AABB>& boxes, bool visible
);
/// \brief Create a kinbody with one link composed of an array of orien ted bounding boxes. /// \brief Create a kinbody with one link composed of an array of orien ted bounding boxes.
/// ///
/// \param vobbs the array of oriented bounding boxes that will compris /// \param boxes the array of oriented bounding boxes that will compris
e of the body e of the body
/// \param bDraw if true, the boxes will be rendered in the scene /// \param visible if true, the boxes will be rendered in the scene
virtual bool InitFromBoxes(const std::vector<OBB>& boxes, bool draw); virtual bool InitFromBoxes(const std::vector<OBB>& boxes, bool visible)
;
/// \brief Create a kinbody with one link composed of an array of spher es /// \brief Create a kinbody with one link composed of an array of spher es
/// ///
/// \param vspheres the XYZ position of the spheres with the W coordina /// \param spheres the XYZ position of the spheres with the W coordinat
te representing the individual radius e representing the individual radius
virtual bool InitFromSpheres(const std::vector<Vector>& vspheres, bool /// \param visible if true, the boxes will be rendered in the scene
draw); virtual bool InitFromSpheres(const std::vector<Vector>& spheres, bool v
isible);
/// \brief Create a kinbody with one link composed of a triangle mesh s urface /// \brief Create a kinbody with one link composed of a triangle mesh s urface
/// ///
/// \param trimesh the triangle mesh /// \param trimesh the triangle mesh
/// \param bDraw if true, will be rendered in the scene /// \param visible if true, will be rendered in the scene
virtual bool InitFromTrimesh(const Link::TRIMESH& trimesh, bool draw); virtual bool InitFromTrimesh(const Link::TRIMESH& trimesh, bool visible
);
/// \brief Create a kinbody with one link composed of a list of geometr
ies
///
/// \param geometries In order to save memory, the geometries in this l
ist are transferred to the link. After function completes, the size should
be 0.
/// \param visible if true, will be rendered in the scene
bool InitFromGeometries(std::list<KinBody::Link::GEOMPROPERTIES>& geome
tries, bool visible);
/// \brief Unique name of the robot. /// \brief Unique name of the robot.
virtual const std::string& GetName() const { virtual const std::string& GetName() const {
return _name; return _name;
} }
/// \brief Set the name of the robot, notifies the environment and chec ks for uniqueness. /// \brief Set the name of the robot, notifies the environment and chec ks for uniqueness.
virtual void SetName(const std::string& name); virtual void SetName(const std::string& name);
/// Methods for accessing basic information about joints /// Methods for accessing basic information about joints
skipping to change at line 891 skipping to change at line 994
/// \brief Number controllable degrees of freedom of the body. /// \brief Number controllable degrees of freedom of the body.
/// ///
/// Only uses _vecjoints and last joint for computation, so can work be fore _ComputeInternalInformation is called. /// Only uses _vecjoints and last joint for computation, so can work be fore _ComputeInternalInformation is called.
virtual int GetDOF() const; virtual int GetDOF() const;
/// \brief Returns all the joint values as organized by the DOF indices . /// \brief Returns all the joint values as organized by the DOF indices .
virtual void GetDOFValues(std::vector<dReal>& v) const; virtual void GetDOFValues(std::vector<dReal>& v) const;
/// \brief Returns all the joint velocities as organized by the DOF ind ices. /// \brief Returns all the joint velocities as organized by the DOF ind ices.
virtual void GetDOFVelocities(std::vector<dReal>& v) const; virtual void GetDOFVelocities(std::vector<dReal>& v) const;
/// \brief Returns all the joint limits as organized by the DOF indices . /// \brief Returns all the joint limits as organized by the DOF indices .
virtual void GetDOFLimits(std::vector<dReal>& vLowerLimit, std::vector< dReal>& vUpperLimit) const; virtual void GetDOFLimits(std::vector<dReal>& lowerlimit, std::vector<d Real>& upperlimit) const;
/// \brief Returns all the joint velocity limits as organized by the DO F indices. /// \brief Returns all the joint velocity limits as organized by the DO F indices.
virtual void GetDOFVelocityLimits(std::vector<dReal>& vLowerLimit, std: virtual void GetDOFVelocityLimits(std::vector<dReal>& lowerlimit, std::
:vector<dReal>& vUpperLimit) const; vector<dReal>& upperlimit) const;
/// \brief Returns the max velocity for each DOF
virtual void GetDOFVelocityLimits(std::vector<dReal>& maxvelocities) co
nst;
/// \brief Returns the max acceleration for each DOF
virtual void GetDOFAccelerationLimits(std::vector<dReal>& maxaccelerati
ons) const;
/// \brief Returns the max torque for each DOF
virtual void GetDOFTorqueLimits(std::vector<dReal>& maxaccelerations) c
onst;
/// \deprecated (11/05/26) /// \deprecated (11/05/26)
virtual void GetDOFMaxVel(std::vector<dReal>& v) const RAVE_DEPRECATED { virtual void GetDOFMaxVel(std::vector<dReal>& v) const RAVE_DEPRECATED {
std::vector<dReal> dummy; GetDOFVelocityLimits(dummy,v); GetDOFVelocityLimits(v);
}
virtual void GetDOFMaxAccel(std::vector<dReal>& v) const RAVE_DEPRECATE
D {
GetDOFAccelerationLimits(v);
} }
virtual void GetDOFMaxAccel(std::vector<dReal>& v) const;
virtual void GetDOFMaxTorque(std::vector<dReal>& v) const; virtual void GetDOFMaxTorque(std::vector<dReal>& v) const;
virtual void GetDOFResolutions(std::vector<dReal>& v) const; virtual void GetDOFResolutions(std::vector<dReal>& v) const;
virtual void GetDOFWeights(std::vector<dReal>& v) const; virtual void GetDOFWeights(std::vector<dReal>& v) const;
/// \deprecated Returns all the joint values in the order of GetJoints( /// \brief \see GetDOFVelocityLimits
) (use GetDOFValues instead) (10/07/10) virtual void SetDOFVelocityLimits(const std::vector<dReal>& maxlimits);
virtual void GetJointValues(std::vector<dReal>& v) const RAVE_DEPRECATE
D { /// \brief \see GetDOFAccelerationLimits
GetDOFValues(v); virtual void SetDOFAccelerationLimits(const std::vector<dReal>& maxlimi
} ts);
virtual void GetJointVelocities(std::vector<dReal>& v) const RAVE_DEPRE
CATED { /// \brief \see GetDOFTorqueLimits
GetDOFVelocities(v); virtual void SetDOFTorqueLimits(const std::vector<dReal>& maxlimits);
}
virtual void GetJointLimits(std::vector<dReal>& vLowerLimit, std::vecto /// \brief \see GetDOFWeights
r<dReal>& vUpperLimit) const RAVE_DEPRECATED { virtual void SetDOFWeights(const std::vector<dReal>& weights);
GetDOFLimits(vLowerLimit,vUpperLimit);
}
virtual void GetJointMaxVel(std::vector<dReal>& v) const RAVE_DEPRECATE
D {
std::vector<dReal> dummy; GetDOFVelocityLimits(dummy,v);
}
virtual void GetJointMaxAccel(std::vector<dReal>& v) const RAVE_DEPRECA
TED {
GetDOFMaxAccel(v);
}
virtual void GetJointMaxTorque(std::vector<dReal>& v) const RAVE_DEPREC
ATED {
GetDOFMaxTorque(v);
}
virtual void GetJointResolutions(std::vector<dReal>& v) const RAVE_DEPR
ECATED {
GetDOFResolutions(v);
}
virtual void GetJointWeights(std::vector<dReal>& v) const RAVE_DEPRECAT
ED {
GetDOFWeights(v);
}
/// \brief Returns the joints making up the controllable degrees of fre edom of the body. /// \brief Returns the joints making up the controllable degrees of fre edom of the body.
const std::vector<JointPtr>& GetJoints() const { const std::vector<JointPtr>& GetJoints() const {
return _vecjoints; return _vecjoints;
} }
/** \brief Returns the passive joints, order does not matter. /** \brief Returns the passive joints, order does not matter.
A passive joint is not directly controlled by the body's degrees of freedom so it has no A passive joint is not directly controlled by the body's degrees of freedom so it has no
joint index and no dof index. Passive joints allows mimic joints to be hidden from the users. joint index and no dof index. Passive joints allows mimic joints to be hidden from the users.
skipping to change at line 1037 skipping to change at line 1135
/// return a pointer to the link with the given name /// return a pointer to the link with the given name
virtual LinkPtr GetLink(const std::string& name) const; virtual LinkPtr GetLink(const std::string& name) const;
/// Updates the bounding box and any other parameters that could have c hanged by a simulation step /// Updates the bounding box and any other parameters that could have c hanged by a simulation step
virtual void SimulationStep(dReal fElapsedTime); virtual void SimulationStep(dReal fElapsedTime);
/// \brief get the transformations of all the links at once /// \brief get the transformations of all the links at once
virtual void GetLinkTransformations(std::vector<Transform>& transforms) const; virtual void GetLinkTransformations(std::vector<Transform>& transforms) const;
/// \brief get the transformations of all the links and the dof branche
s at once.
///
/// Knowing the dof branches allows the robot to recover the full state
of the joints with SetLinkTransformations
virtual void GetLinkTransformations(std::vector<Transform>& transforms,
std::vector<int>& dofbranches) const;
/// \deprecated (11/05/26) /// \deprecated (11/05/26)
virtual void GetBodyTransformations(std::vector<Transform>& transforms) const RAVE_DEPRECATED { virtual void GetBodyTransformations(std::vector<Transform>& transforms) const RAVE_DEPRECATED {
GetLinkTransformations(transforms); GetLinkTransformations(transforms);
} }
/// queries the transfromation of the first link of the body /// queries the transfromation of the first link of the body
virtual Transform GetTransform() const; virtual Transform GetTransform() const;
/// \brief Set the velocity of the base link, rest of links are set to a consistent velocity so entire robot moves correctly. /// \brief Set the velocity of the base link, rest of links are set to a consistent velocity so entire robot moves correctly.
/// \param linearvel linear velocity /// \param linearvel linear velocity
skipping to change at line 1088 skipping to change at line 1191
\param transform 変換行列 \param transform 変換行列
*/ */
virtual void SetTransform(const Transform& transform); virtual void SetTransform(const Transform& transform);
/// \brief Return an axis-aligned bounding box of the entire object in the world coordinate system. /// \brief Return an axis-aligned bounding box of the entire object in the world coordinate system.
virtual AABB ComputeAABB() const; virtual AABB ComputeAABB() const;
/// \brief Return the center of mass of entire robot in the world coord inate system. /// \brief Return the center of mass of entire robot in the world coord inate system.
virtual Vector GetCenterOfMass() const; virtual Vector GetCenterOfMass() const;
/// \brief Enables or disables the bodies. /// \brief Enables or disables all the links.
virtual void Enable(bool enable); virtual void Enable(bool enable);
/// \deprecated (10/09/08) /// \deprecated (10/09/08)
virtual void EnableLink(LinkPtr plink, bool bEnable) RAVE_DEPRECATED { virtual void EnableLink(LinkPtr plink, bool bEnable) RAVE_DEPRECATED {
plink->Enable(bEnable); plink->Enable(bEnable);
} }
/// \return true if any link of the KinBody is enabled /// \return true if any link of the KinBody is enabled
virtual bool IsEnabled() const; virtual bool IsEnabled() const;
/// \brief Sets all the links as visible or not visible.
///
/// \return true if changed
virtual bool SetVisible(bool visible);
/// \return true if any link of the KinBody is visible.
virtual bool IsVisible() const;
/// \brief Sets the joint values of the robot. /// \brief Sets the joint values of the robot.
/// ///
/// \param values the values to set the joint angles (ordered by the do f indices) /// \param values the values to set the joint angles (ordered by the do f indices)
/// \praam checklimits if true, will excplicitly check the joint limits before setting the values. /// \praam checklimits if true, will excplicitly check the joint limits before setting the values.
virtual void SetDOFValues(const std::vector<dReal>& values, bool checkl imits = false); virtual void SetDOFValues(const std::vector<dReal>& values, bool checkl imits = false);
virtual void SetJointValues(const std::vector<dReal>& values, bool chec klimits = false) { virtual void SetJointValues(const std::vector<dReal>& values, bool chec klimits = false) {
SetDOFValues(values,checklimits); SetDOFValues(values,checklimits);
} }
skipping to change at line 1124 skipping to change at line 1235
virtual void SetDOFValues(const std::vector<dReal>& values, const Trans form& transform, bool checklimits = false); virtual void SetDOFValues(const std::vector<dReal>& values, const Trans form& transform, bool checklimits = false);
virtual void SetJointValues(const std::vector<dReal>& values, const Tra nsform& transform, bool checklimits = false) virtual void SetJointValues(const std::vector<dReal>& values, const Tra nsform& transform, bool checklimits = false)
{ {
SetDOFValues(values,transform,checklimits); SetDOFValues(values,transform,checklimits);
} }
/// \brief sets the transformations of all the links at once /// \brief sets the transformations of all the links at once
virtual void SetLinkTransformations(const std::vector<Transform>& trans forms); virtual void SetLinkTransformations(const std::vector<Transform>& trans forms);
/// \brief sets the transformations of all the links and dof branches a
t once.
///
/// Using dof branches allows the full joint state to be recovered
virtual void SetLinkTransformations(const std::vector<Transform>& trans
forms, const std::vector<int>& dofbranches);
/// \deprecated (11/05/26) /// \deprecated (11/05/26)
virtual void SetBodyTransformations(const std::vector<Transform>& trans forms) RAVE_DEPRECATED { virtual void SetBodyTransformations(const std::vector<Transform>& trans forms) RAVE_DEPRECATED {
SetLinkTransformations(transforms); SetLinkTransformations(transforms);
} }
/// \brief sets the link velocities /// \brief sets the link velocities
virtual void SetLinkVelocities(const std::vector<std::pair<Vector,Vecto r> >& velocities); virtual void SetLinkVelocities(const std::vector<std::pair<Vector,Vecto r> >& velocities);
/// \brief Computes the translation jacobian with respect to a world po sition. /// \brief Computes the translation jacobian with respect to a world po sition.
/// ///
skipping to change at line 1189 skipping to change at line 1305
/** \brief Returns a nonzero value if the joint effects the link transf ormation. /** \brief Returns a nonzero value if the joint effects the link transf ormation.
In closed loops, all joints on all paths to the root link are count ed as affecting the link. In closed loops, all joints on all paths to the root link are count ed as affecting the link.
If a mimic joint affects the link, then all the joints used in the mimic joint's computation affect the link. If a mimic joint affects the link, then all the joints used in the mimic joint's computation affect the link.
If negative, the partial derivative of the Jacobian should be negat ed. If negative, the partial derivative of the Jacobian should be negat ed.
\param jointindex index of the joint \param jointindex index of the joint
\param linkindex index of the link \param linkindex index of the link
*/ */
virtual int8_t DoesAffect(int jointindex, int linkindex) const; virtual int8_t DoesAffect(int jointindex, int linkindex) const;
/// \brief GUI data to let the viewer store specific graphic handles fo /// \see SetViewerData
r the object. virtual UserDataPtr GetViewerData() const {
virtual void SetGuiData(UserDataPtr data) { return _pViewerData;
_pGuiData = data;
} }
/// \deprecated (11/09/21)
/// \see SetGuiData virtual UserDataPtr GetGuiData() const RAVE_DEPRECATED {
virtual UserDataPtr GetGuiData() const { return GetViewerData();
return _pGuiData;
} }
/// \brief specifies the type of adjacent link information to receive /// \brief specifies the type of adjacent link information to receive
enum AdjacentOptions enum AdjacentOptions
{ {
AO_Enabled = 1, ///< return only enabled link pairs AO_Enabled = 1, ///< return only enabled link pairs
AO_ActiveDOFs = 2, ///< return only link pairs that have an act ive in its path AO_ActiveDOFs = 2, ///< return only link pairs that have an act ive in its path
}; };
/// \brief return all possible link pairs that could get in collision. /// \brief return all possible link pairs that could get in collision.
skipping to change at line 1243 skipping to change at line 1358
virtual void Clone(InterfaceBaseConstPtr preference, int cloningoptions ); virtual void Clone(InterfaceBaseConstPtr preference, int cloningoptions );
/// \brief Register a callback with the interface. /// \brief Register a callback with the interface.
/// ///
/// Everytime a static property of the interface changes, all /// Everytime a static property of the interface changes, all
/// registered callbacks are called to update the users of the changes. Note that the callbacks will /// registered callbacks are called to update the users of the changes. Note that the callbacks will
/// block the thread that made the parameter change. /// block the thread that made the parameter change.
/// \param callback /// \param callback
/// \param properties a mask of the \ref KinBodyProperty values that th e callback should be called for when they change /// \param properties a mask of the \ref KinBodyProperty values that th e callback should be called for when they change
virtual boost::shared_ptr<void> RegisterChangeCallback(int properties, const boost::function<void()>& callback); virtual UserDataPtr RegisterChangeCallback(int properties, const boost: :function<void()>& callback);
virtual void serialize(std::ostream& o, int options) const; virtual void serialize(std::ostream& o, int options) const;
/// \brief A md5 hash unique to the particular kinematic and geometric structure of a KinBody. /// \brief A md5 hash unique to the particular kinematic and geometric structure of a KinBody.
/// ///
/// This 32 byte string can be used to check if two bodies have the sam e kinematic structure and can be used /// This 32 byte string can be used to check if two bodies have the sam e kinematic structure and can be used
/// to index into tables when looking for body-specific models. OpenRAV E stores all /// to index into tables when looking for body-specific models. OpenRAV E stores all
/// such models in the OPENRAVE_HOME directory (usually ~/.openrave), i ndexed by the particular robot/body hashes. /// such models in the OPENRAVE_HOME directory (usually ~/.openrave), i ndexed by the particular robot/body hashes.
/// \return md5 hash string of kinematics/geometry /// \return md5 hash string of kinematics/geometry
virtual const std::string& GetKinematicsGeometryHash() const; virtual const std::string& GetKinematicsGeometryHash() const;
skipping to change at line 1270 skipping to change at line 1385
/// \deprecated (10/11/18) /// \deprecated (10/11/18)
virtual void GetVelocity(Vector& linearvel, Vector& angularvel) const R AVE_DEPRECATED; virtual void GetVelocity(Vector& linearvel, Vector& angularvel) const R AVE_DEPRECATED;
/// \brief Sets the joint offsets so that the current configuration bec omes the new zero state of the robot. /// \brief Sets the joint offsets so that the current configuration bec omes the new zero state of the robot.
/// ///
/// When this function returns, the returned DOF values should be all z ero for controllable joints. /// When this function returns, the returned DOF values should be all z ero for controllable joints.
/// Mimic equations will use the new offsetted values when computing th eir joints. /// Mimic equations will use the new offsetted values when computing th eir joints.
/// This is primarily used for calibrating a robot's zero position /// This is primarily used for calibrating a robot's zero position
virtual void SetZeroConfiguration(); virtual void SetZeroConfiguration();
/// Functions dealing with configuration specifications
/// @name Configuration Specification API
//@{
/// \brief return the configuration specification of the joint values a
nd transform
virtual const ConfigurationSpecification& GetConfigurationSpecification
() const;
/// \brief return the configuration specification of the specified join
t indices.
///
/// Note that the return type is by-value, so should not be used in ite
ration
virtual ConfigurationSpecification GetConfigurationSpecificationIndices
(const std::vector<int>& indices) const;
/// \brief sets joint values and transform of the body using configurat
ion values as specified by \ref GetConfigurationSpecification()
///
/// \param itvalues the iterator to the vector containing the dof value
s. Must have GetConfigurationSpecification().GetDOF() values!
virtual void SetConfigurationValues(std::vector<dReal>::const_iterator
itvalues, bool checklimits = false);
/// \brief returns the configuration values as specified by \ref GetCon
figurationSpecification()
virtual void GetConfigurationValues(std::vector<dReal>& v) const;
//@}
protected: protected:
/// \brief constructors declared protected so that user always goes thr ough environment to create bodies /// \brief constructors declared protected so that user always goes thr ough environment to create bodies
KinBody(InterfaceType type, EnvironmentBasePtr penv); KinBody(InterfaceType type, EnvironmentBasePtr penv);
inline KinBodyPtr shared_kinbody() { inline KinBodyPtr shared_kinbody() {
return boost::static_pointer_cast<KinBody>(shared_from_this()); return boost::static_pointer_cast<KinBody>(shared_from_this());
} }
inline KinBodyConstPtr shared_kinbody_const() const { inline KinBodyConstPtr shared_kinbody_const() const {
return boost::static_pointer_cast<KinBody const>(shared_from_this() ); return boost::static_pointer_cast<KinBody const>(shared_from_this() );
} }
/// \brief custom data managed by the current active physics engine, sh ould be set only by PhysicsEngineBase /// \brief custom data managed by the current active physics engine, sh ould be set only by PhysicsEngineBase
virtual void SetPhysicsData(UserDataPtr pdata) { virtual void SetPhysicsData(UserDataPtr pdata) {
_pPhysicsData = pdata; _pPhysicsData = pdata;
} }
/// \brief custom data managed by the current active collision checker, should be set only by CollisionCheckerBase /// \brief custom data managed by the current active collision checker, should be set only by CollisionCheckerBase
virtual void SetCollisionData(UserDataPtr pdata) { virtual void SetCollisionData(UserDataPtr pdata) {
_pCollisionData = pdata; _pCollisionData = pdata;
} }
/// \brief custom data managed by the current active viewer, should be
set only by ViewerBase
virtual void SetViewerData(UserDataPtr pdata) {
_pViewerData = pdata;
}
virtual void SetManageData(ManageDataPtr pdata) { virtual void SetManageData(ManageDataPtr pdata) {
_pManageData = pdata; _pManageData = pdata;
} }
/** \brief Final post-processing stage before a kinematics body can be used. /** \brief Final post-processing stage before a kinematics body can be used.
This method is called after the body is finished being initialized with data and before being added to the environment. Also builds the hashes . Builds the internal hierarchy and kinematic body hash. This method is called after the body is finished being initialized with data and before being added to the environment. Also builds the hashes . Builds the internal hierarchy and kinematic body hash.
Avoids making specific calls on the collision checker (like CheckCo llision) or physics engine (like simulating velocities/torques) since this information can change depending on the attached plugin. Avoids making specific calls on the collision checker (like CheckCo llision) or physics engine (like simulating velocities/torques) since this information can change depending on the attached plugin.
*/ */
skipping to change at line 1340 skipping to change at line 1481
std::vector<int> _vDOFIndices; ///< cached start joint indices, indexed by dof indices std::vector<int> _vDOFIndices; ///< cached start joint indices, indexed by dof indices
std::vector<std::pair<int16_t,int16_t> > _vAllPairsShortestPaths; ///< all-pairs shortest paths through the link hierarchy. The first value descri bes the parent link index, and the second value is an index into _vecjoints or _vPassiveJoints. If the second value is greater or equal to _vecjoints .size() then it indexes into _vPassiveJoints. std::vector<std::pair<int16_t,int16_t> > _vAllPairsShortestPaths; ///< all-pairs shortest paths through the link hierarchy. The first value descri bes the parent link index, and the second value is an index into _vecjoints or _vPassiveJoints. If the second value is greater or equal to _vecjoints .size() then it indexes into _vPassiveJoints.
std::vector<int8_t> _vJointsAffectingLinks; ///< joint x link: (jointin dex*_veclinks.size()+linkindex). entry is non-zero if the joint affects the link in the forward kinematics. If negative, the partial derivative of ds/ dtheta should be negated. std::vector<int8_t> _vJointsAffectingLinks; ///< joint x link: (jointin dex*_veclinks.size()+linkindex). entry is non-zero if the joint affects the link in the forward kinematics. If negative, the partial derivative of ds/ dtheta should be negated.
std::vector< std::vector< std::pair<LinkPtr,JointPtr> > > _vClosedLoops ; ///< \see GetClosedLoops std::vector< std::vector< std::pair<LinkPtr,JointPtr> > > _vClosedLoops ; ///< \see GetClosedLoops
std::vector< std::vector< std::pair<int16_t,int16_t> > > _vClosedLoopIn dices; ///< \see GetClosedLoops std::vector< std::vector< std::pair<int16_t,int16_t> > > _vClosedLoopIn dices; ///< \see GetClosedLoops
std::vector<JointPtr> _vPassiveJoints; ///< \see GetPassiveJoints() std::vector<JointPtr> _vPassiveJoints; ///< \see GetPassiveJoints()
std::set<int> _setAdjacentLinks; ///< a set of which links are connecte d to which if link i and j are connected then std::set<int> _setAdjacentLinks; ///< a set of which links are connecte d to which if link i and j are connected then
///< i|(j<<16) will be in the set wher e i<j. ///< i|(j<<16) will be in the set wher e i<j.
std::vector< std::pair<std::string, std::string> > _vForcedAdjacentLink s; ///< internally stores forced adjacent links std::vector< std::pair<std::string, std::string> > _vForcedAdjacentLink s; ///< internally stores forced adjacent links
std::list<KinBodyWeakPtr> _listAttachedBodies; ///< list of bodies that are directly attached to this body (can have duplicates) std::list<KinBodyWeakPtr> _listAttachedBodies; ///< list of bodies that are directly attached to this body (can have duplicates)
std::list<std::pair<int,boost::function<void()> > > _listRegisteredCall backs; ///< callbacks to call when particular properties of the body change . std::list<UserDataWeakPtr> _listRegisteredCallbacks; ///< callbacks to call when particular properties of the body change.
mutable boost::array<std::set<int>, 4> _setNonAdjacentLinks; ///< conta ins cached versions of the non-adjacent links depending on values in Adjace ntOptions. Declared as mutable since data is cached. mutable boost::array<std::set<int>, 4> _setNonAdjacentLinks; ///< conta ins cached versions of the non-adjacent links depending on values in Adjace ntOptions. Declared as mutable since data is cached.
mutable int _nNonAdjacentLinkCache; ///< specifies what information is currently valid in the AdjacentOptions. Declared as mutable since data is cached. If 0x80000000 (ie < 0), then everything needs to be recomputed incl uding _setNonAdjacentLinks[0]. mutable int _nNonAdjacentLinkCache; ///< specifies what information is currently valid in the AdjacentOptions. Declared as mutable since data is cached. If 0x80000000 (ie < 0), then everything needs to be recomputed incl uding _setNonAdjacentLinks[0].
std::vector<Transform> _vInitialLinkTransformations; ///< the initial t ransformations of each link specifying at least one pose where the robot is collision free std::vector<Transform> _vInitialLinkTransformations; ///< the initial t ransformations of each link specifying at least one pose where the robot is collision free
ConfigurationSpecification _spec;
int _environmentid; ///< \see GetEnvironmentId int _environmentid; ///< \see GetEnvironmentId
mutable int _nUpdateStampId; ///< \see GetUpdateStamp mutable int _nUpdateStampId; ///< \see GetUpdateStamp
int _nParametersChanged; ///< set of parameters that changed and need c allbacks int _nParametersChanged; ///< set of parameters that changed and need c allbacks
UserDataPtr _pGuiData; ///< \see SetGuiData UserDataPtr _pViewerData; ///< \see SetViewerData
UserDataPtr _pPhysicsData; ///< \see SetPhysicsData UserDataPtr _pPhysicsData; ///< \see SetPhysicsData
UserDataPtr _pCollisionData; ///< \see SetCollisionData UserDataPtr _pCollisionData; ///< \see SetCollisionData
ManageDataPtr _pManageData; ManageDataPtr _pManageData;
uint32_t _nHierarchyComputed; ///< true if the joint heirarchy and othe r cached information is computed uint32_t _nHierarchyComputed; ///< true if the joint heirarchy and othe r cached information is computed
bool _bMakeJoinedLinksAdjacent; bool _bMakeJoinedLinksAdjacent;
private: private:
mutable std::string __hashkinematics; mutable std::string __hashkinematics;
mutable std::vector<dReal> _vTempJoints; mutable std::vector<dReal> _vTempJoints;
virtual const char* GetHash() const { virtual const char* GetHash() const {
return OPENRAVE_KINBODY_HASH; return OPENRAVE_KINBODY_HASH;
} }
static void __erase_iterator(KinBodyWeakPtr pweakbody, std::list<std::p
air<int,boost::function<void()> > >::iterator* pit);
#ifdef RAVE_PRIVATE #ifdef RAVE_PRIVATE
#ifdef _MSC_VER #ifdef _MSC_VER
friend class Environment; friend class Environment;
friend class ColladaReader; friend class ColladaReader;
friend class ColladaWriter; friend class ColladaWriter;
friend class OpenRAVEXMLParser::KinBodyXMLReader; friend class OpenRAVEXMLParser::KinBodyXMLReader;
friend class OpenRAVEXMLParser::JointXMLReader; friend class OpenRAVEXMLParser::JointXMLReader;
friend class XFileReader;
#else #else
friend class ::Environment; friend class ::Environment;
friend class ::ColladaReader; friend class ::ColladaReader;
friend class ::ColladaWriter; friend class ::ColladaWriter;
friend class ::OpenRAVEXMLParser::KinBodyXMLReader; friend class ::OpenRAVEXMLParser::KinBodyXMLReader;
friend class ::OpenRAVEXMLParser::JointXMLReader; friend class ::OpenRAVEXMLParser::JointXMLReader;
friend class ::XFileReader;
#endif #endif
#endif #endif
friend class PhysicsEngineBase; friend class PhysicsEngineBase;
friend class CollisionCheckerBase; friend class CollisionCheckerBase;
friend class ViewerBase;
friend class SensorSystemBase; friend class SensorSystemBase;
friend class RaveDatabase; friend class RaveDatabase;
friend class ChangeCallbackData;
}; };
OPENRAVE_API std::ostream& operator<<(std::ostream& O, const KinBody::Link: :TRIMESH& trimesh); OPENRAVE_API std::ostream& operator<<(std::ostream& O, const KinBody::Link: :TRIMESH& trimesh);
OPENRAVE_API std::istream& operator>>(std::istream& I, KinBody::Link::TRIME SH& trimesh); OPENRAVE_API std::istream& operator>>(std::istream& I, KinBody::Link::TRIME SH& trimesh);
} // end namespace OpenRAVE } // end namespace OpenRAVE
#endif #endif
 End of changes. 57 change blocks. 
111 lines changed or deleted 289 lines changed or added


 mathextra.h   mathextra.h 
skipping to change at line 382 skipping to change at line 382
template <typename T> template <typename T>
inline T* _multtrans4(T* pfres, const T* pf1, const T* pf2) inline T* _multtrans4(T* pfres, const T* pf1, const T* pf2)
{ {
T* pfres2; T* pfres2;
if( pfres == pf1 ) pfres2 = (T*)alloca(16 * sizeof(T)); if( pfres == pf1 ) pfres2 = (T*)alloca(16 * sizeof(T));
else pfres2 = pfres; else pfres2 = pfres;
for(int i = 0; i < 4; ++i) { for(int i = 0; i < 4; ++i) {
for(int j = 0; j < 4; ++j) { for(int j = 0; j < 4; ++j) {
pfres[4*i+j] = pf1[i] * pf2[j] + pf1[i+4] * pf2[j+4] + pf1[i+8] * pf2[j+8] + pf1[i+12] * pf2[j+12]; pfres2[4*i+j] = pf1[i] * pf2[j] + pf1[i+4] * pf2[j+4] + pf1[i+8 ] * pf2[j+8] + pf1[i+12] * pf2[j+12];
} }
} }
if( pfres2 != pfres ) memcpy(pfres, pfres2, 16*sizeof(T));
return pfres; return pfres;
} }
/// \brief Compute the determinant of a 3x3 matrix whose row stride stride elements. /// \brief Compute the determinant of a 3x3 matrix whose row stride stride elements.
template <typename T> inline T matrixdet3(const T* pf, int stride) template <typename T> inline T matrixdet3(const T* pf, int stride)
{ {
return pf[0*stride+2] * (pf[1*stride + 0] * pf[2*stride + 1] - pf[1*str ide + 1] * pf[2*stride + 0]) + return pf[0*stride+2] * (pf[1*stride + 0] * pf[2*stride + 1] - pf[1*str ide + 1] * pf[2*stride + 0]) +
pf[1*stride+2] * (pf[0*stride + 1] * pf[2*stride + 0] - pf[0*str ide + 0] * pf[2*stride + 1]) + pf[1*stride+2] * (pf[0*stride + 1] * pf[2*stride + 0] - pf[0*str ide + 0] * pf[2*stride + 1]) +
pf[2*stride+2] * (pf[0*stride + 0] * pf[1*stride + 1] - pf[0*str ide + 1] * pf[1*stride + 0]); pf[2*stride+2] * (pf[0*stride + 0] * pf[1*stride + 1] - pf[0*str ide + 1] * pf[1*stride + 0]);
} }
 End of changes. 2 change blocks. 
1 lines changed or deleted 3 lines changed or added


 module.h   module.h 
skipping to change at line 25 skipping to change at line 25
// 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.
*/ */
#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. 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
{ {
public: public:
ModuleBase(EnvironmentBasePtr penv) : InterfaceBase(PT_Module, penv) { ModuleBase(EnvironmentBasePtr penv) : InterfaceBase(PT_Module, penv) {
} }
virtual ~ModuleBase() { virtual ~ModuleBase() {
} }
 End of changes. 1 change blocks. 
1 lines changed or deleted 1 lines changed or added


 openrave.h   openrave.h 
skipping to change at line 226 skipping to change at line 226
}; };
/// \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;
// 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
skipping to change at line 580 skipping to change at line 581
class ModuleBase; class ModuleBase;
class EnvironmentBase; class EnvironmentBase;
class KinBody; class KinBody;
class SensorSystemBase; class SensorSystemBase;
class PhysicsEngineBase; class PhysicsEngineBase;
class SensorBase; class SensorBase;
class CollisionCheckerBase; class CollisionCheckerBase;
class ViewerBase; class ViewerBase;
class SpaceSamplerBase; class SpaceSamplerBase;
class IkParameterization; class IkParameterization;
class ConfigurationSpecification;
typedef boost::shared_ptr<CollisionReport> CollisionReportPtr; typedef boost::shared_ptr<CollisionReport> CollisionReportPtr;
typedef boost::shared_ptr<CollisionReport const> CollisionReportConstPtr; typedef boost::shared_ptr<CollisionReport const> CollisionReportConstPtr;
typedef boost::shared_ptr<InterfaceBase> InterfaceBasePtr; typedef boost::shared_ptr<InterfaceBase> InterfaceBasePtr;
typedef boost::shared_ptr<InterfaceBase const> InterfaceBaseConstPtr; typedef boost::shared_ptr<InterfaceBase const> InterfaceBaseConstPtr;
typedef boost::weak_ptr<InterfaceBase> InterfaceBaseWeakPtr; typedef boost::weak_ptr<InterfaceBase> InterfaceBaseWeakPtr;
typedef boost::shared_ptr<KinBody> KinBodyPtr; typedef boost::shared_ptr<KinBody> KinBodyPtr;
typedef boost::shared_ptr<KinBody const> KinBodyConstPtr; typedef boost::shared_ptr<KinBody const> KinBodyConstPtr;
typedef boost::weak_ptr<KinBody> KinBodyWeakPtr; typedef boost::weak_ptr<KinBody> KinBodyWeakPtr;
typedef boost::shared_ptr<RobotBase> RobotBasePtr; typedef boost::shared_ptr<RobotBase> RobotBasePtr;
skipping to change at line 776 skipping to change at line 778
using mathextra::normalize4; using mathextra::normalize4;
using mathextra::cross3; using mathextra::cross3;
using mathextra::inv3; using mathextra::inv3;
using mathextra::inv4; using mathextra::inv4;
using mathextra::lengthsqr2; using mathextra::lengthsqr2;
using mathextra::lengthsqr3; using mathextra::lengthsqr3;
using mathextra::lengthsqr4; using mathextra::lengthsqr4;
using mathextra::mult4; using mathextra::mult4;
//@} //@}
/// \brief The types of inverse kinematics parameterizations supported.
///
/// The minimum degree of freedoms required is set in the upper 4 bits of e
ach type.
/// The number of values used to represent the parameterization ( >= dof )
is the next 4 bits.
/// The lower bits contain a unique id of the type.
enum IkParameterizationType {
IKP_None=0,
IKP_Transform6D=0x67000001, ///< end effector reaches desired 6D tr
ansformation
IKP_Rotation3D=0x34000002, ///< end effector reaches desired 3D rot
ation
IKP_Translation3D=0x33000003, ///< end effector origin reaches desi
red 3D translation
IKP_Direction3D=0x23000004, ///< direction on end effector coordina
te system reaches desired direction
IKP_Ray4D=0x46000005, ///< ray on end effector coordinate system re
aches desired global ray
IKP_Lookat3D=0x23000006, ///< direction on end effector coordinate
system points to desired 3D position
IKP_TranslationDirection5D=0x56000007, ///< end effector origin and
direction reaches desired 3D translation and direction. Can be thought of
as Ray IK where the origin of the ray must coincide.
IKP_TranslationXY2D=0x22000008, ///< 2D translation along XY plane
IKP_TranslationXYOrientation3D=0x33000009, ///< 2D translation alon
g XY plane and 1D rotation around Z axis. The offset of the rotation is mea
sured starting at +X, so at +X is it 0, at +Y it is pi/2.
IKP_TranslationLocalGlobal6D=0x3600000a, ///< local point on end ef
fector origin reaches desired 3D global point
IKP_TranslationXAxisAngle4D=0x4400000b, ///< end effector origin reache
s desired 3D translation, manipulator direction makes a specific angle with
x-axis like a cone, angle is from 0-pi. Axes defined in the manipulator b
ase link's coordinate system)
IKP_TranslationYAxisAngle4D=0x4400000c, ///< end effector origin reache
s desired 3D translation, manipulator direction makes a specific angle with
y-axis like a cone, angle is from 0-pi. Axes defined in the manipulator b
ase link's coordinate system)
IKP_TranslationZAxisAngle4D=0x4400000d, ///< end effector origin reache
s desired 3D translation, manipulator direction makes a specific angle with
z-axis like a cone, angle is from 0-pi. Axes are defined in the manipulato
r base link's coordinate system.
IKP_TranslationXAxisAngleZNorm4D=0x4400000e, ///< end effector origin r
eaches desired 3D translation, manipulator direction needs to be orthogonal
to z-axis and be rotated at a certain angle starting from the x-axis (defi
ned in the manipulator base link's coordinate system)
IKP_TranslationYAxisAngleXNorm4D=0x4400000f, ///< end effector origin r
eaches desired 3D translation, manipulator direction needs to be orthogonal
to x-axis and be rotated at a certain angle starting from the y-axis (defi
ned in the manipulator base link's coordinate system)
IKP_TranslationZAxisAngleYNorm4D=0x44000010, ///< end effector origin r
eaches desired 3D translation, manipulator direction needs to be orthogonal
to y-axis and be rotated at a certain angle starting from the z-axis (defi
ned in the manipulator base link's coordinate system)
IKP_NumberOfParameterizations=16, ///< number of parameterizations
(does not count IKP_None)
};
/** \brief A configuration specification references values in the environme
nt that then define a configuration-space which can be searched for.
It is composed of several groups targetting values for individual bodie
s. It is serialized into XML. The XML syntax is as follows:
\code
<configuration>
<group name="string" offset="#OFF1" dof="#D1" interpolation="string"/>
<group name="string" offset="#OFF2" dof="#D2" interpolation="string"/>
</configuration>
\endcode
*/
class OPENRAVE_API ConfigurationSpecification
{
public:
/// \brief A group referencing the values of one body in the environmen
t
class OPENRAVE_API Group
{
public:
Group() : offset(0), dof(0) {
}
inline bool operator==(const Group& r) const {
return offset==r.offset && dof==r.dof && name==r.name && interp
olation==r.interpolation;
}
inline bool operator!=(const Group& r) const {
return offset!=r.offset || dof!=r.dof || name!=r.name || interp
olation!=r.interpolation;
}
/// \brief For each data point, the number of values to offset befo
re data for this group starts.
int offset;
/// \brief The number of values in this group.
int dof;
/** \brief semantic information on what part of the environment the
group refers to.
Can be composed of multiple workds; the first word is the group
type, and the words following narrow the specifics. Common types are:
- \b joint_values - The joint values of a kinbody/robot. The jo
int names with the name of the body can follow.
- \b joint_velocities - The joint velocities (1/second) of a ki
nbody/robot. The name of the body with the joint names can follow.
- \b joint_accelerations - The joint accelerations (1/second^2)
of a kinbody/robot. The name of the body with the joint names can follow.
- \b joint_torques - The joint torques (Newton meter) of a kinb
ody/robot. The name of the body with the joint names can follow.
- \b affine_transform - An affine transformation [quaternion, t
ranslation]. The name of the body with selected affine dofs (see \ref DOFAf
fine) can follow.
- \b affine_velocities - The velocity (1/second) of the affine
transformation [rotation axis, translation velocity], the name of the body
can follow.
- \b affine_accelerations - The velocity (1/second^2) of the af
fine transformation [rotation axis, translation velocity], the name of the
body can follow.
- \b ikparam_values - The values of an IkParmeterization. The i
kparam type is stored as the second value in name
- \b ikparam_velocities - velocity of an IkParmeterization. The
ikparam type is stored as the second value in name
- \b iswaypoint - non-zero if the point represents a major knot
point of the trajectory
- \b grab - Used to grab bodies given a robot's links. The name
of the robot name along with the link indices to grab can follow.
*/
std::string name;
/** \brief Describes how the data should be interpolated. Common me
thods are:
- \b previous - the previous waypoint's value is always chosen
- \b next - the next waypoint's value is always chosen
- \b linear - linear interpolation (default)
- \b quadratic - position is piecewise-quadratic, velocity is p
iecewise-linear, acceleration is one of -amax, 0, or amax
- \b cubic - 3 degree polynomial
- \b quadric - 4 degree polynomial
- \b quintic - 5 degree polynomial
*/
std::string interpolation;
};
class Reader : public BaseXMLReader
{
public:
Reader(ConfigurationSpecification& spec);
virtual ProcessElement startElement(const std::string& name, const
AttributesList& atts);
virtual bool endElement(const std::string& name);
virtual void characters(const std::string& ch);
protected:
ConfigurationSpecification& _spec;
std::stringstream _ss;
BaseXMLReaderPtr _preader;
};
virtual ~ConfigurationSpecification() {
}
/// \brief return the dimension of the configuraiton space (degrees of
freedom)
virtual int GetDOF() const;
/// \brief check if the groups form a continguous space
virtual bool IsValid() const;
virtual bool operator==(const ConfigurationSpecification& r) const;
virtual bool operator!=(const ConfigurationSpecification& r) const;
/// \brief return the group whose name begins with a particular string.
///
/// If multiple groups exist that begin with the same string, then the
shortest one is returned.
/// \throw openrave_exception if a group is not found
virtual const Group& GetGroupFromName(const std::string& name) const;
/// \brief return the group whose name begins with a particular string.
///
/// If multiple groups exist that begin with the same string, then the
shortest one is returned.
/// \throw openrave_exception if a group is not found
virtual Group& GetGroupFromName(const std::string& name);
/// \brief finds the most compatible group to the given group
///
/// \param g the group to query, only the Group::name and Group::dof va
lues are used
/// \param exactmatch if true, will only return groups whose name exact
ly matches with g.name
/// \return an iterator part of _vgroups that represents the most compa
tible group. If no group is found, will return _vgroups.end()
virtual std::vector<Group>::const_iterator FindCompatibleGroup(const Gr
oup& g, bool exactmatch=false) const;
/// \brief finds the most compatible group to the given group
///
/// \param name the name of the group to query
/// \param exactmatch if true, will only return groups whose name exact
ly matches with g.name
/// \return an iterator part of _vgroups that represents the most compa
tible group. If no group is found, will return _vgroups.end()
virtual std::vector<Group>::const_iterator FindCompatibleGroup(const st
d::string& name, bool exactmatch=false) const;
/** \brief Return the most compatible group that represents the time-de
rivative data of the group.
For example given a 'joint_values' group, this will return the clos
est 'joint_velocities' group.
\param g the group to query, only the Group::name and Group::dof va
lues are used
\param exactmatch if true, will only return groups whose name exact
ly matches with g.name
\return an iterator part of _vgroups that represents the most compa
tible group. If no group is found, will return _vgroups.end()
*/
virtual std::vector<Group>::const_iterator FindTimeDerivativeGroup(cons
t Group& g, bool exactmatch=false) const;
/** \brief Return the most compatible group that represents the time-de
rivative data of the group.
For example given a 'joint_values' group, this will return the clos
est 'joint_velocities' group.
\param name the name of the group to query
\param exactmatch if true, will only return groups whose name exact
ly matches with g.name
\return an iterator part of _vgroups that represents the most compa
tible group. If no group is found, will return _vgroups.end()
*/
virtual std::vector<Group>::const_iterator FindTimeDerivativeGroup(cons
t std::string& name, bool exactmatch=false) const;
/** \brief adds a velocity group for every position group.
If velocities groups already exist, they are checked for and/or mod
ified. Note that the configuration space
might be re-ordered as a result of this function call.
\param adddeltatime If true will add the 'deltatime' tag, which is
necessary for trajectory sampling
*/
virtual void AddVelocityGroups(bool adddeltatime);
/// \brief converts all the groups to the corresponding velocity groups
and returns the specification
///
/// The velocity configuration space will have a one-to-one corresponde
nce with the
virtual ConfigurationSpecification ConvertToVelocitySpecification() con
st;
/// \brief returns a new specification of just particular time-derivati
ve groups.
///
/// \param timederivative the time derivative to query groups from. 0 i
s positions/joint values, 1 is velocities, 2 is accelerations, etc
virtual ConfigurationSpecification GetTimeDerivativeSpecification(int t
imederivative) const;
/** \brief set the offsets of each group in order to get a contiguous c
onfiguration space
*/
virtual void ResetGroupOffsets();
/// \brief adds the deltatime tag to the end if one doesn't exist and r
eturns the index into the configuration space
virtual int AddDeltaTimeGroup();
/// \brief adds a new group to the specification and returns its new of
fset.
///
/// If the new group's semantic name exists in the current specificatio
n and it exactly matches, then succeeds. If the match
/// isn't exact, then an openrave_exception is throw.
virtual int AddGroup(const std::string& name, int dof, const std::strin
g& interpolation = "");
/// \brief merges all the information from the input group into this gr
oup
///
/// For groups that are merged, the interpolation method is not changed
.
/// \throw openrave_exception throws if groups do not contain enough in
formation to be merged
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..
///
/// For groups that are merged, the interpolation method is not changed
.
/// \throw openrave_exception throws if groups do not contain enough in
formation to be merged
virtual ConfigurationSpecification operator+ (const ConfigurationSpecif
ication& r) const;
/** \brief extracts an affine transform given the start of a configurat
ion space point
Looks for 'affine_transform' groups. If pbody is not initialized, w
ill choose the first affine_transform found.
\param[inout] t the transform holding the default values, which wil
l be overwritten with the new values.
\param[in] itdata data in the format of this configuration specific
ation.
\return true if at least one group was found for extracting
*/
virtual bool ExtractTransform(Transform& t, std::vector<dReal>::const_i
terator itdata, KinBodyConstPtr pbody) const;
/** \brief extracts an ikparameterization given the start of a configur
ation space point
Looks for 'ikparam' groups.
\param[inout] ikparam filled with ikparameterization (if found)
\param[in] itdata data in the format of this configuration specific
ation
\return true if at least one group was found for extracting
*/
virtual bool ExtractIkParameterization(IkParameterization& ikparam, std
::vector<dReal>::const_iterator itdata, int timederivative=0) const;
/** \brief extracts the affine values
Looks for 'affine_X' groups. If pbody is not initialized, will choo
se the first affine_X found.
\param[inout] itvalues iterator to vector that holds the default va
lues and will be overwritten with the new values. must be initialized
\param[in] itdata data in the format of this configuration specific
ation.
\param[in] affinedofs the format of the affine dofs requested
\param[in] timederivative the time derivative of the data to extrac
t
\return true if at least one group was found for extracting
*/
virtual bool ExtractAffineValues(std::vector<dReal>::iterator itvalues,
std::vector<dReal>::const_iterator itdata, KinBodyConstPtr pbody, int affi
nedofs, int timederivative=0) const;
/** \brief extracts a body's joint values given the start of a configur
ation space point
Looks for 'joint_X' groups. If pbody is not initialized, will choos
e the first joint_X found.
\param[inout] itvalues iterator to vector that holds the default va
lues and will be overwritten with the new values. must be initialized
\param[in] itdata data in the format of this configuration specific
ation.
\param[in] indices the set of DOF indices of the body to extract an
d write into itvalues.
\param[in] timederivative the time derivative of the data to extrac
t
\return true if at least one group was found for extracting
*/
virtual bool ExtractJointValues(std::vector<dReal>::iterator itvalues,
std::vector<dReal>::const_iterator itdata, KinBodyConstPtr pbody, const std
::vector<int>& indices, int timederivative=0) const;
/// \brief extracts the delta time from the configuration if one exists
///
/// \return true if deltatime exists in the current configuration, othe
rwise false
virtual bool ExtractDeltaTime(dReal& deltatime, std::vector<dReal>::con
st_iterator itdata) const;
/** \brief inserts a set of joint values into a configuration space poi
nt
Looks for 'joint_X' groups. If pbody is not initialized, will use t
he first joint_X found.
\param[inout] itdata data in the format of this configuration speci
fication.
\param[in] itvalues iterator to joint values to write
\param[in] indices the set of DOF indices that itvalues represents.
\param[in] timederivative the time derivative of the data to insert
\return true if at least one group was found for inserting
*/
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
\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)
\return true if at least one group was found for inserting
*/
virtual bool InsertDeltaTime(std::vector<dReal>::iterator itdata, dReal
deltatime);
/** \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 targetstride the number of elements that to go from the next
target point. Necessary if numpoints > 1.
\param gtarget the target configuration group
\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 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 penv [optional] The environment which might be needed to fil
l in unknown data. Assumes environment is locked.
\param filluninitialized If there exists target groups that cannot
be initialized, then will set default values using the current environment.
For example, the current joint values of the body will be used.
\throw openrave_exception throw f groups are incompatible
*/
static void ConvertGroupData(std::vector<dReal>::iterator ittargetdata,
size_t targetstride, const Group& gtarget, std::vector<dReal>::const_itera
tor itsourcedata, size_t sourcestride, const Group& gsource, size_t numpoin
ts, EnvironmentBaseConstPtr penv, bool filluninitialized = true);
/** \brief Converts from one specification to another.
\param ittargetdata iterator pointing to start of target group data
that should be overwritten
\param targetspec the target configuration specification
\param itsourcedata iterator pointing to start of source group data
that should be read
\param sourcespec the source configuration specification
\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 filluninitialized If there exists target groups that cannot
be initialized, then will set default values using the current environment.
For example, the current joint values of the body will be used.
*/
static void ConvertData(std::vector<dReal>::iterator ittargetdata, cons
t ConfigurationSpecification& targetspec, std::vector<dReal>::const_iterato
r itsourcedata, const ConfigurationSpecification& sourcespec, size_t numpoi
nts, EnvironmentBaseConstPtr penv, bool filluninitialized = true);
std::vector<Group> _vgroups;
};
OPENRAVE_API std::ostream& operator<<(std::ostream& O, const ConfigurationS
pecification &spec);
OPENRAVE_API std::istream& operator>>(std::istream& I, ConfigurationSpecifi
cation& spec);
typedef boost::shared_ptr<ConfigurationSpecification> ConfigurationSpecific
ationPtr;
typedef boost::shared_ptr<ConfigurationSpecification const> ConfigurationSp
ecificationConstPtr;
/** \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:
/// \brief The types of inverse kinematics parameterizations supported. /// \deprecated (11/10/12)
/// typedef IkParameterizationType Type RAVE_DEPRECATED;
/// The minimum degree of freedoms required is set in the upper 4 bits static const IkParameterizationType Type_None RAVE_DEPRECATED = IKP_Non
of each type. e;
/// The number of values used to represent the parameterization ( >= do static const IkParameterizationType Type_Transform6D RAVE_DEPRECATED =
f ) is the next 4 bits. IKP_Transform6D;
/// The lower bits contain a unique id of the type. static const IkParameterizationType Type_Rotation3D RAVE_DEPRECATED =IK
enum Type { P_Rotation3D;
Type_None=0, static const IkParameterizationType Type_Translation3D RAVE_DEPRECATED
Type_Transform6D=0x67000001, ///< end effector reaches desired =IKP_Translation3D;
6D transformation static const IkParameterizationType Type_Direction3D RAVE_DEPRECATED =
Type_Rotation3D=0x34000002, ///< end effector reaches desired 3 IKP_Direction3D;
D rotation static const IkParameterizationType Type_Ray4D RAVE_DEPRECATED = IKP_Ra
Type_Translation3D=0x33000003, ///< end effector origin reaches y4D;
desired 3D translation static const IkParameterizationType Type_Lookat3D RAVE_DEPRECATED = IKP
Type_Direction3D=0x23000004, ///< direction on end effector coo _Lookat3D;
rdinate system reaches desired direction static const IkParameterizationType Type_TranslationDirection5D RAVE_DE
Type_Ray4D=0x46000005, ///< ray on end effector coordinate syst PRECATED = IKP_TranslationDirection5D;
em reaches desired global ray static const IkParameterizationType Type_TranslationXY2D RAVE_DEPRECATE
Type_Lookat3D=0x23000006, ///< direction on end effector coordi D = IKP_TranslationXY2D;
nate system points to desired 3D position static const IkParameterizationType Type_TranslationXYOrientation3D RAV
Type_TranslationDirection5D=0x56000007, ///< end effector origi E_DEPRECATED = IKP_TranslationXYOrientation3D;
n and direction reaches desired 3D translation and direction. Can be though static const IkParameterizationType Type_TranslationLocalGlobal6D RAVE_
t of as Ray IK where the origin of the ray must coincide. DEPRECATED = IKP_TranslationLocalGlobal6D;
Type_TranslationXY2D=0x22000008, ///< 2D translation along XY p static const IkParameterizationType Type_NumberOfParameterizations RAVE
lane _DEPRECATED = IKP_NumberOfParameterizations;
Type_TranslationXYOrientation3D=0x33000009, ///< 2D translation
along XY plane and 1D rotation around Z axis. The offset of the rotation i
s measured starting at +X, so at +X is it 0, at +Y it is pi/2.
Type_TranslationLocalGlobal6D=0x3600000a, ///< local point on e
nd effector origin reaches desired 3D global point
Type_NumberOfParameterizations=10, ///< number of parameterizat
ions (does not count Type_None)
};
IkParameterization() : _type(Type_None) { IkParameterization() : _type(IKP_None) {
} }
/// \brief sets a 6D transform parameterization /// \brief sets a 6D transform parameterization
IkParameterization(const Transform &t) { IkParameterization(const Transform &t) {
SetTransform6D(t); SetTransform6D(t);
} }
/// \brief sets a ray parameterization /// \brief sets a ray parameterization
IkParameterization(const RAY &r) { IkParameterization(const RAY &r) {
SetRay4D(r); SetRay4D(r);
} }
/// \brief set a custom parameterization using a transform as the sourc e of the data. Not all types are supported with this method. /// \brief set a custom parameterization using a transform as the sourc e of the data. Not all types are supported with this method.
IkParameterization(const Transform &t, Type type) { IkParameterization(const Transform &t, IkParameterizationType type) {
_type=type; _type=type;
switch(_type) { switch(_type) {
case Type_Transform6D: SetTransform6D(t); break; case IKP_Transform6D: SetTransform6D(t); break;
case Type_Rotation3D: SetRotation3D(t.rot); break; case IKP_Rotation3D: SetRotation3D(t.rot); break;
case Type_Translation3D: SetTranslation3D(t.trans); break; case IKP_Translation3D: SetTranslation3D(t.trans); break;
case Type_Lookat3D: SetLookat3D(t.trans); break; case IKP_Lookat3D: SetLookat3D(t.trans); break;
default: default:
throw openrave_exception(str(boost::format("IkParameterization constructor does not support type 0x%x")%_type)); throw openrave_exception(str(boost::format("IkParameterization constructor does not support type 0x%x")%_type));
} }
} }
inline Type GetType() const { inline IkParameterizationType GetType() const {
return _type; return _type;
} }
inline const std::string& GetName() const; inline const std::string& GetName() const;
/// \brief Returns the minimum degree of freedoms required for the IK t ype. /// \brief Returns the minimum degree of freedoms required for the IK t ype.
static int GetDOF(Type type) { static int GetDOF(IkParameterizationType type) {
return (type>>28)&0xf; return (type>>28)&0xf;
} }
/// \brief Returns the minimum degree of freedoms required for the IK t ype. /// \brief Returns the minimum degree of freedoms required for the IK t ype.
inline int GetDOF() const { inline int GetDOF() const {
return (_type>>28)&0xf; return (_type>>28)&0xf;
} }
/// \brief Returns the number of values used to represent the parameter ization ( >= dof ). The number of values serialized is this number plus 1 f or the iktype. /// \brief Returns the number of values used to represent the parameter ization ( >= dof ). The number of values serialized is this number plus 1 f or the iktype.
static int GetNumberOfValues(Type type) { static int GetNumberOfValues(IkParameterizationType type) {
return (type>>24)&0xf; return (type>>24)&0xf;
} }
/// \brief Returns the number of values used to represent the parameter ization ( >= dof ). The number of values serialized is this number plus 1 f or the iktype. /// \brief Returns the number of values used to represent the parameter ization ( >= dof ). The number of values serialized is this number plus 1 f or the iktype.
inline int GetNumberOfValues() const { inline int GetNumberOfValues() const {
return (_type>>24)&0xf; return (_type>>24)&0xf;
} }
inline void SetTransform6D(const Transform& t) { inline void SetTransform6D(const Transform& t) {
_type = Type_Transform6D; _transform = t; _type = IKP_Transform6D; _transform = t;
} }
inline void SetRotation3D(const Vector& quaternion) { inline void SetRotation3D(const Vector& quaternion) {
_type = Type_Rotation3D; _transform.rot = quaternion; _type = IKP_Rotation3D; _transform.rot = quaternion;
} }
inline void SetTranslation3D(const Vector& trans) { inline void SetTranslation3D(const Vector& trans) {
_type = Type_Translation3D; _transform.trans = trans; _type = IKP_Translation3D; _transform.trans = trans;
} }
inline void SetDirection3D(const Vector& dir) { inline void SetDirection3D(const Vector& dir) {
_type = Type_Direction3D; _transform.rot = dir; _type = IKP_Direction3D; _transform.rot = dir;
} }
inline void SetRay4D(const RAY& ray) { inline void SetRay4D(const RAY& ray) {
_type = Type_Ray4D; _transform.trans = ray.pos; _transform.rot = ra y.dir; _type = IKP_Ray4D; _transform.trans = ray.pos; _transform.rot = ray .dir;
} }
inline void SetLookat3D(const Vector& trans) { inline void SetLookat3D(const Vector& trans) {
_type = Type_Lookat3D; _transform.trans = trans; _type = IKP_Lookat3D; _transform.trans = trans;
} }
/// \brief the ray direction is not used for IK, however it is needed i n order to compute the error /// \brief the ray direction is not used for IK, however it is needed i n order to compute the error
inline void SetLookat3D(const RAY& ray) { inline void SetLookat3D(const RAY& ray) {
_type = Type_Lookat3D; _transform.trans = ray.pos; _transform.rot = ray.dir; _type = IKP_Lookat3D; _transform.trans = ray.pos; _transform.rot = ray.dir;
} }
inline void SetTranslationDirection5D(const RAY& ray) { inline void SetTranslationDirection5D(const RAY& ray) {
_type = Type_TranslationDirection5D; _transform.trans = ray.pos; _t ransform.rot = ray.dir; _type = IKP_TranslationDirection5D; _transform.trans = ray.pos; _tr ansform.rot = ray.dir;
} }
inline void SetTranslationXY2D(const Vector& trans) { inline void SetTranslationXY2D(const Vector& trans) {
_type = Type_TranslationXY2D; _transform.trans.x = trans.x; _transf orm.trans.y = trans.y; _transform.trans.z = 0; _transform.trans.w = 0; _type = IKP_TranslationXY2D; _transform.trans.x = trans.x; _transfo rm.trans.y = trans.y; _transform.trans.z = 0; _transform.trans.w = 0;
} }
inline void SetTranslationXYOrientation3D(const Vector& trans) { inline void SetTranslationXYOrientation3D(const Vector& trans) {
_type = Type_TranslationXYOrientation3D; _transform.trans.x = trans .x; _transform.trans.y = trans.y; _transform.trans.z = trans.z; _transform. trans.w = 0; _type = IKP_TranslationXYOrientation3D; _transform.trans.x = trans. x; _transform.trans.y = trans.y; _transform.trans.z = trans.z; _transform.t rans.w = 0;
} }
inline void SetTranslationLocalGlobal6D(const Vector& localtrans, const Vector& trans) { inline void SetTranslationLocalGlobal6D(const Vector& localtrans, const Vector& trans) {
_type = Type_TranslationLocalGlobal6D; _transform.rot.x = localtran _type = IKP_TranslationLocalGlobal6D; _transform.rot.x = localtrans
s.x; _transform.rot.y = localtrans.y; _transform.rot.z = localtrans.z; _tra .x; _transform.rot.y = localtrans.y; _transform.rot.z = localtrans.z; _tran
nsform.rot.w = 0; _transform.trans.x = trans.x; _transform.trans.y = trans. sform.rot.w = 0; _transform.trans.x = trans.x; _transform.trans.y = trans.y
y; _transform.trans.z = trans.z; _transform.trans.w = 0; ; _transform.trans.z = trans.z; _transform.trans.w = 0;
}
inline void SetTranslationXAxisAngle4D(const Vector& trans, dReal angle
) {
_type = IKP_TranslationXAxisAngle4D;
_transform.trans = trans;
_transform.rot.x = angle;
}
inline void SetTranslationYAxisAngle4D(const Vector& trans, dReal angle
) {
_type = IKP_TranslationYAxisAngle4D;
_transform.trans = trans;
_transform.rot.x = angle;
}
inline void SetTranslationZAxisAngle4D(const Vector& trans, dReal angle
) {
_type = IKP_TranslationZAxisAngle4D;
_transform.trans = trans;
_transform.rot.x = angle;
}
inline void SetTranslationXAxisAngleZNorm4D(const Vector& trans, dReal
angle) {
_type = IKP_TranslationXAxisAngleZNorm4D;
_transform.trans = trans;
_transform.rot.x = angle;
}
inline void SetTranslationYAxisAngleXNorm4D(const Vector& trans, dReal
angle) {
_type = IKP_TranslationYAxisAngleXNorm4D;
_transform.trans = trans;
_transform.rot.x = angle;
}
inline void SetTranslationZAxisAngleYNorm4D(const Vector& trans, dReal
angle) {
_type = IKP_TranslationZAxisAngleYNorm4D;
_transform.trans = trans;
_transform.rot.x = angle;
} }
inline const Transform& GetTransform6D() const { inline const Transform& GetTransform6D() const {
return _transform; return _transform;
} }
inline const Vector& GetRotation3D() const { inline const Vector& GetRotation3D() const {
return _transform.rot; return _transform.rot;
} }
inline const Vector& GetTranslation3D() const { inline const Vector& GetTranslation3D() const {
return _transform.trans; return _transform.trans;
skipping to change at line 918 skipping to change at line 1250
} }
inline const Vector& GetTranslationXY2D() const { inline const Vector& GetTranslationXY2D() const {
return _transform.trans; return _transform.trans;
} }
inline const Vector& GetTranslationXYOrientation3D() const { inline const Vector& GetTranslationXYOrientation3D() const {
return _transform.trans; return _transform.trans;
} }
inline std::pair<Vector,Vector> GetTranslationLocalGlobal6D() const { inline std::pair<Vector,Vector> GetTranslationLocalGlobal6D() const {
return std::make_pair(_transform.rot,_transform.trans); return std::make_pair(_transform.rot,_transform.trans);
} }
inline std::pair<Vector,dReal> GetTranslationXAxisAngle4D() const {
return std::make_pair(_transform.trans,_transform.rot.x);
}
inline std::pair<Vector,dReal> GetTranslationYAxisAngle4D() const {
return std::make_pair(_transform.trans,_transform.rot.x);
}
inline std::pair<Vector,dReal> GetTranslationZAxisAngle4D() const {
return std::make_pair(_transform.trans,_transform.rot.x);
}
inline std::pair<Vector,dReal> GetTranslationXAxisAngleZNorm4D() const
{
return std::make_pair(_transform.trans,_transform.rot.x);
}
inline std::pair<Vector,dReal> GetTranslationYAxisAngleXNorm4D() const
{
return std::make_pair(_transform.trans,_transform.rot.x);
}
inline std::pair<Vector,dReal> GetTranslationZAxisAngleYNorm4D() const
{
return std::make_pair(_transform.trans,_transform.rot.x);
}
/// \deprecated (11/02/15) /// \deprecated (11/02/15)
//@{ //@{
inline void SetTransform(const Transform& t) RAVE_DEPRECATED { inline void SetTransform(const Transform& t) RAVE_DEPRECATED {
SetTransform6D(t); SetTransform6D(t);
} }
inline void SetRotation(const Vector& quaternion) RAVE_DEPRECATED { inline void SetRotation(const Vector& quaternion) RAVE_DEPRECATED {
SetRotation3D(quaternion); SetRotation3D(quaternion);
} }
inline void SetTranslation(const Vector& trans) RAVE_DEPRECATED { inline void SetTranslation(const Vector& trans) RAVE_DEPRECATED {
skipping to change at line 971 skipping to change at line 1321
return RAY(_transform.trans,_transform.rot); return RAY(_transform.trans,_transform.rot);
} }
//@} //@}
/// \brief Computes the distance squared between two IK parmaeterizatio ns. /// \brief Computes the distance squared between two IK parmaeterizatio ns.
inline dReal ComputeDistanceSqr(const IkParameterization& ikparam) cons t inline dReal ComputeDistanceSqr(const IkParameterization& ikparam) cons t
{ {
const dReal anglemult = 0.4; // this is a hack that should be r emoved.... const dReal anglemult = 0.4; // this is a hack that should be r emoved....
BOOST_ASSERT(_type==ikparam.GetType()); BOOST_ASSERT(_type==ikparam.GetType());
switch(_type) { switch(_type) {
case IkParameterization::Type_Transform6D: { case IKP_Transform6D: {
Transform t0 = GetTransform6D(), t1 = ikparam.GetTransform6D(); Transform t0 = GetTransform6D(), t1 = ikparam.GetTransform6D();
dReal fcos = RaveFabs(t0.rot.dot(t1.rot)); dReal fcos = RaveFabs(t0.rot.dot(t1.rot));
dReal facos = fcos >= 1 ? 0 : RaveAcos(fcos); dReal facos = fcos >= 1 ? 0 : RaveAcos(fcos);
return (t0.trans-t1.trans).lengthsqr3() + anglemult*facos*facos ; return (t0.trans-t1.trans).lengthsqr3() + anglemult*facos*facos ;
} }
case IkParameterization::Type_Rotation3D: { case IKP_Rotation3D: {
dReal fcos = RaveFabs(GetRotation3D().dot(ikparam.GetRotation3D ())); dReal fcos = RaveFabs(GetRotation3D().dot(ikparam.GetRotation3D ()));
dReal facos = fcos >= 1 ? 0 : RaveAcos(fcos); dReal facos = fcos >= 1 ? 0 : RaveAcos(fcos);
return facos*facos; return facos*facos;
} }
case IkParameterization::Type_Translation3D: case IKP_Translation3D:
return (GetTranslation3D()-ikparam.GetTranslation3D()).lengthsq r3(); return (GetTranslation3D()-ikparam.GetTranslation3D()).lengthsq r3();
case IkParameterization::Type_Direction3D: { case IKP_Direction3D: {
dReal fcos = GetDirection3D().dot(ikparam.GetDirection3D()); dReal fcos = GetDirection3D().dot(ikparam.GetDirection3D());
dReal facos = fcos >= 1 ? 0 : RaveAcos(fcos); dReal facos = fcos >= 1 ? 0 : RaveAcos(fcos);
return facos*facos; return facos*facos;
} }
case IkParameterization::Type_Ray4D: { case IKP_Ray4D: {
Vector pos0 = GetRay4D().pos - GetRay4D().dir*GetRay4D().dir.do t(GetRay4D().pos); Vector pos0 = GetRay4D().pos - GetRay4D().dir*GetRay4D().dir.do t(GetRay4D().pos);
Vector pos1 = ikparam.GetRay4D().pos - ikparam.GetRay4D().dir*i kparam.GetRay4D().dir.dot(ikparam.GetRay4D().pos); Vector pos1 = ikparam.GetRay4D().pos - ikparam.GetRay4D().dir*i kparam.GetRay4D().dir.dot(ikparam.GetRay4D().pos);
dReal fcos = GetRay4D().dir.dot(ikparam.GetRay4D().dir); dReal fcos = GetRay4D().dir.dot(ikparam.GetRay4D().dir);
dReal facos = fcos >= 1 ? 0 : RaveAcos(fcos); dReal facos = fcos >= 1 ? 0 : RaveAcos(fcos);
return (pos0-pos1).lengthsqr3() + anglemult*facos*facos; return (pos0-pos1).lengthsqr3() + anglemult*facos*facos;
} }
case IkParameterization::Type_Lookat3D: { case IKP_Lookat3D: {
Vector v = GetLookat3D()-ikparam.GetLookat3D(); Vector v = GetLookat3D()-ikparam.GetLookat3D();
dReal s = v.dot3(ikparam.GetLookat3DDirection()); dReal s = v.dot3(ikparam.GetLookat3DDirection());
if( s >= -1 ) { // ikparam's lookat is always 1 beyond the origin, this is just the convention for testing... if( s >= -1 ) { // ikparam's lookat is always 1 beyond the origin, this is just the convention for testing...
v -= s*ikparam.GetLookat3DDirection(); v -= s*ikparam.GetLookat3DDirection();
} }
return v.lengthsqr3(); return v.lengthsqr3();
} }
case IkParameterization::Type_TranslationDirection5D: { case IKP_TranslationDirection5D: {
dReal fcos = GetTranslationDirection5D().dir.dot(ikparam.GetTra nslationDirection5D().dir); dReal fcos = GetTranslationDirection5D().dir.dot(ikparam.GetTra nslationDirection5D().dir);
dReal facos = fcos >= 1 ? 0 : RaveAcos(fcos); dReal facos = fcos >= 1 ? 0 : RaveAcos(fcos);
return (GetTranslationDirection5D().pos-ikparam.GetTranslationD irection5D().pos).lengthsqr3() + anglemult*facos*facos; return (GetTranslationDirection5D().pos-ikparam.GetTranslationD irection5D().pos).lengthsqr3() + anglemult*facos*facos;
} }
case IkParameterization::Type_TranslationXY2D: { case IKP_TranslationXY2D: {
return (GetTranslationXY2D()-ikparam.GetTranslationXY2D()).leng thsqr2(); return (GetTranslationXY2D()-ikparam.GetTranslationXY2D()).leng thsqr2();
} }
case IkParameterization::Type_TranslationXYOrientation3D: { case IKP_TranslationXYOrientation3D: {
Vector v0 = GetTranslationXYOrientation3D(); Vector v0 = GetTranslationXYOrientation3D();
Vector v1 = ikparam.GetTranslationXYOrientation3D(); Vector v1 = ikparam.GetTranslationXYOrientation3D();
dReal anglediff = v0.z-v1.z; dReal anglediff = v0.z-v1.z;
if (anglediff < dReal(-PI)) { if (anglediff < dReal(-PI)) {
anglediff += dReal(2*PI); anglediff += dReal(2*PI);
while (anglediff < dReal(-PI)) while (anglediff < dReal(-PI))
anglediff += dReal(2*PI); anglediff += dReal(2*PI);
} }
else if (anglediff > dReal(PI)) { else if (anglediff > dReal(PI)) {
anglediff -= dReal(2*PI); anglediff -= dReal(2*PI);
while (anglediff > dReal(PI)) while (anglediff > dReal(PI))
anglediff -= dReal(2*PI); anglediff -= dReal(2*PI);
} }
return (v0-v1).lengthsqr2() + anglemult*anglediff*anglediff; return (v0-v1).lengthsqr2() + anglemult*anglediff*anglediff;
} }
case IkParameterization::Type_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: {
std::pair<Vector,dReal> p0 = GetTranslationXAxisAngle4D(), p1 =
ikparam.GetTranslationXAxisAngle4D();
return (p0.first-p1.first).lengthsqr3() + (p0.second-p1.second)
*(p0.second-p1.second);
}
case IKP_TranslationYAxisAngle4D: {
std::pair<Vector,dReal> p0 = GetTranslationYAxisAngle4D(), p1 =
ikparam.GetTranslationYAxisAngle4D();
return (p0.first-p1.first).lengthsqr3() + (p0.second-p1.second)
*(p0.second-p1.second);
}
case IKP_TranslationZAxisAngle4D: {
std::pair<Vector,dReal> p0 = GetTranslationZAxisAngle4D(), p1 =
ikparam.GetTranslationZAxisAngle4D();
return (p0.first-p1.first).lengthsqr3() + (p0.second-p1.second)
*(p0.second-p1.second);
}
case IKP_TranslationXAxisAngleZNorm4D: {
std::pair<Vector,dReal> p0 = GetTranslationXAxisAngleZNorm4D(),
p1 = ikparam.GetTranslationXAxisAngleZNorm4D();
return (p0.first-p1.first).lengthsqr3() + (p0.second-p1.second)
*(p0.second-p1.second);
}
case IKP_TranslationYAxisAngleXNorm4D: {
std::pair<Vector,dReal> p0 = GetTranslationYAxisAngleXNorm4D(),
p1 = ikparam.GetTranslationYAxisAngleXNorm4D();
return (p0.first-p1.first).lengthsqr3() + (p0.second-p1.second)
*(p0.second-p1.second);
}
case IKP_TranslationZAxisAngleYNorm4D: {
std::pair<Vector,dReal> p0 = GetTranslationZAxisAngleYNorm4D(),
p1 = ikparam.GetTranslationZAxisAngleYNorm4D();
return (p0.first-p1.first).lengthsqr3() + (p0.second-p1.second)
*(p0.second-p1.second);
}
default: default:
BOOST_ASSERT(0); BOOST_ASSERT(0);
} }
return 1e30; return 1e30;
} }
/// \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.
inline void GetValues(std::vector<dReal>::iterator itvalues) const
{
switch(_type) {
case IKP_Transform6D:
*itvalues++ = _transform.rot.x;
*itvalues++ = _transform.rot.y;
*itvalues++ = _transform.rot.z;
*itvalues++ = _transform.rot.w;
*itvalues++ = _transform.trans.x;
*itvalues++ = _transform.trans.y;
*itvalues++ = _transform.trans.z;
break;
case IKP_Rotation3D:
*itvalues++ = _transform.rot.x;
*itvalues++ = _transform.rot.y;
*itvalues++ = _transform.rot.z;
*itvalues++ = _transform.rot.w;
break;
case IKP_Translation3D:
*itvalues++ = _transform.trans.x;
*itvalues++ = _transform.trans.y;
*itvalues++ = _transform.trans.z;
break;
case IKP_Direction3D:
*itvalues++ = _transform.rot.x;
*itvalues++ = _transform.rot.y;
*itvalues++ = _transform.rot.z;
break;
case IKP_Ray4D:
*itvalues++ = _transform.rot.x;
*itvalues++ = _transform.rot.y;
*itvalues++ = _transform.rot.z;
*itvalues++ = _transform.trans.x;
*itvalues++ = _transform.trans.y;
*itvalues++ = _transform.trans.z;
break;
case IKP_Lookat3D:
*itvalues++ = _transform.trans.x;
*itvalues++ = _transform.trans.y;
*itvalues++ = _transform.trans.z;
break;
case IKP_TranslationDirection5D:
*itvalues++ = _transform.rot.x;
*itvalues++ = _transform.rot.y;
*itvalues++ = _transform.rot.z;
*itvalues++ = _transform.trans.x;
*itvalues++ = _transform.trans.y;
*itvalues++ = _transform.trans.z;
break;
case IKP_TranslationXY2D:
*itvalues++ = _transform.trans.x;
*itvalues++ = _transform.trans.y;
break;
case IKP_TranslationXYOrientation3D:
*itvalues++ = _transform.trans.x;
*itvalues++ = _transform.trans.y;
*itvalues++ = _transform.trans.z;
break;
case IKP_TranslationLocalGlobal6D:
*itvalues++ = _transform.rot.x;
*itvalues++ = _transform.rot.y;
*itvalues++ = _transform.rot.z;
*itvalues++ = _transform.trans.x;
*itvalues++ = _transform.trans.y;
*itvalues++ = _transform.trans.z;
break;
case IKP_TranslationXAxisAngle4D:
case IKP_TranslationYAxisAngle4D:
case IKP_TranslationZAxisAngle4D:
case IKP_TranslationXAxisAngleZNorm4D:
case IKP_TranslationYAxisAngleXNorm4D:
case IKP_TranslationZAxisAngleYNorm4D:
*itvalues++ = _transform.rot.x;
*itvalues++ = _transform.trans.x;
*itvalues++ = _transform.trans.y;
*itvalues++ = _transform.trans.z;
break;
default:
throw OPENRAVE_EXCEPTION_FORMAT("does not support parameterizat
ion 0x%x", _type,ORE_InvalidArguments);
}
}
inline void Set(std::vector<dReal>::const_iterator itvalues, IkParamete
rizationType iktype)
{
_type = iktype;
switch(_type) {
case IKP_Transform6D:
_transform.rot.x = *itvalues++;
_transform.rot.y = *itvalues++;
_transform.rot.z = *itvalues++;
_transform.rot.w = *itvalues++;
_transform.trans.x = *itvalues++;
_transform.trans.y = *itvalues++;
_transform.trans.z = *itvalues++;
break;
case IKP_Rotation3D:
_transform.rot.x = *itvalues++;
_transform.rot.y = *itvalues++;
_transform.rot.z = *itvalues++;
_transform.rot.w = *itvalues++;
break;
case IKP_Translation3D:
_transform.trans.x = *itvalues++;
_transform.trans.y = *itvalues++;
_transform.trans.z = *itvalues++;
break;
case IKP_Direction3D:
_transform.rot.x = *itvalues++;
_transform.rot.y = *itvalues++;
_transform.rot.z = *itvalues++;
break;
case IKP_Ray4D:
_transform.rot.x = *itvalues++;
_transform.rot.y = *itvalues++;
_transform.rot.z = *itvalues++;
_transform.trans.x = *itvalues++;
_transform.trans.y = *itvalues++;
_transform.trans.z = *itvalues++;
break;
case IKP_Lookat3D:
_transform.trans.x = *itvalues++;
_transform.trans.y = *itvalues++;
_transform.trans.z = *itvalues++;
break;
case IKP_TranslationDirection5D:
_transform.rot.x = *itvalues++;
_transform.rot.y = *itvalues++;
_transform.rot.z = *itvalues++;
_transform.trans.x = *itvalues++;
_transform.trans.y = *itvalues++;
_transform.trans.z = *itvalues++;
break;
case IKP_TranslationXY2D:
_transform.trans.x = *itvalues++;
_transform.trans.y = *itvalues++;
break;
case IKP_TranslationXYOrientation3D:
_transform.trans.x = *itvalues++;
_transform.trans.y = *itvalues++;
_transform.trans.z = *itvalues++;
break;
case IKP_TranslationLocalGlobal6D:
_transform.rot.x = *itvalues++;
_transform.rot.y = *itvalues++;
_transform.rot.z = *itvalues++;
_transform.trans.x = *itvalues++;
_transform.trans.y = *itvalues++;
_transform.trans.z = *itvalues++;
break;
case IKP_TranslationXAxisAngle4D:
case IKP_TranslationYAxisAngle4D:
case IKP_TranslationZAxisAngle4D:
case IKP_TranslationXAxisAngleZNorm4D:
case IKP_TranslationYAxisAngleXNorm4D:
case IKP_TranslationZAxisAngleYNorm4D:
_transform.rot.x = *itvalues++;
_transform.trans.x = *itvalues++;
_transform.trans.y = *itvalues++;
_transform.trans.z = *itvalues++;
break;
default:
throw OPENRAVE_EXCEPTION_FORMAT("does not support parameterizat
ion 0x%x", _type,ORE_InvalidArguments);
}
}
static ConfigurationSpecification GetConfigurationSpecification(IkParam
eterizationType iktype);
inline ConfigurationSpecification GetConfigurationSpecification() const
{
return GetConfigurationSpecification(GetType());
}
protected: protected:
Transform _transform; Transform _transform;
Type _type; IkParameterizationType _type;
friend IkParameterization operator* (const Transform &t, const IkParame terization &ikparam); friend IkParameterization operator* (const Transform &t, const IkParame terization &ikparam);
friend std::ostream& operator<<(std::ostream& O, const IkParameterizati friend OPENRAVE_API std::ostream& operator<<(std::ostream& O, const IkP
on &ikparam); arameterization &ikparam);
friend std::istream& operator>>(std::istream& I, IkParameterization& ik friend OPENRAVE_API std::istream& operator>>(std::istream& I, IkParamet
param); erization& ikparam);
}; };
inline IkParameterization operator* (const Transform &t, const IkParameteri zation &ikparam) inline IkParameterization operator* (const Transform &t, const IkParameteri zation &ikparam)
{ {
IkParameterization local; IkParameterization local;
switch(ikparam.GetType()) { switch(ikparam.GetType()) {
case IkParameterization::Type_Transform6D: case IKP_Transform6D:
local.SetTransform6D(t * ikparam.GetTransform6D()); local.SetTransform6D(t * ikparam.GetTransform6D());
break; break;
case IkParameterization::Type_Rotation3D: case IKP_Rotation3D:
local.SetRotation3D(quatMultiply(quatInverse(t.rot),ikparam.GetRota tion3D())); local.SetRotation3D(quatMultiply(quatInverse(t.rot),ikparam.GetRota tion3D()));
break; break;
case IkParameterization::Type_Translation3D: case IKP_Translation3D:
local.SetTranslation3D(t*ikparam.GetTranslation3D()); local.SetTranslation3D(t*ikparam.GetTranslation3D());
break; break;
case IkParameterization::Type_Direction3D: case IKP_Direction3D:
local.SetDirection3D(t.rotate(ikparam.GetDirection3D())); local.SetDirection3D(t.rotate(ikparam.GetDirection3D()));
break; break;
case IkParameterization::Type_Ray4D: case IKP_Ray4D:
local.SetRay4D(RAY(t*ikparam.GetRay4D().pos,t.rotate(ikparam.GetRay 4D().dir))); local.SetRay4D(RAY(t*ikparam.GetRay4D().pos,t.rotate(ikparam.GetRay 4D().dir)));
break; break;
case IkParameterization::Type_Lookat3D: case IKP_Lookat3D:
local.SetLookat3D(RAY(t*ikparam.GetLookat3D(),t.rotate(ikparam.GetL ookat3DDirection()))); local.SetLookat3D(RAY(t*ikparam.GetLookat3D(),t.rotate(ikparam.GetL ookat3DDirection())));
break; break;
case IkParameterization::Type_TranslationDirection5D: case IKP_TranslationDirection5D:
local.SetTranslationDirection5D(RAY(t*ikparam.GetTranslationDirecti on5D().pos,t.rotate(ikparam.GetTranslationDirection5D().dir))); local.SetTranslationDirection5D(RAY(t*ikparam.GetTranslationDirecti on5D().pos,t.rotate(ikparam.GetTranslationDirection5D().dir)));
break; break;
case IkParameterization::Type_TranslationXY2D: case IKP_TranslationXY2D:
local.SetTranslationXY2D(t*ikparam.GetTranslationXY2D()); local.SetTranslationXY2D(t*ikparam.GetTranslationXY2D());
break; break;
case IkParameterization::Type_TranslationXYOrientation3D: { case IKP_TranslationXYOrientation3D: {
Vector v = ikparam.GetTranslationXYOrientation3D(); Vector v = ikparam.GetTranslationXYOrientation3D();
Vector voldtrans(v.x,v.y,0); Vector voldtrans(v.x,v.y,0);
Vector vnewtrans = t*voldtrans; Vector vnewtrans = t*voldtrans;
dReal zangle = -normalizeAxisRotation(Vector(0,0,1),t.rot).first; dReal zangle = -normalizeAxisRotation(Vector(0,0,1),t.rot).first;
local.SetTranslationXYOrientation3D(Vector(vnewtrans.y,vnewtrans.y, v.z+zangle)); local.SetTranslationXYOrientation3D(Vector(vnewtrans.y,vnewtrans.y, v.z+zangle));
break; break;
} }
case IkParameterization::Type_TranslationLocalGlobal6D: case IKP_TranslationLocalGlobal6D:
local.SetTranslationLocalGlobal6D(ikparam.GetTranslationLocalGlobal 6D().first, t*ikparam.GetTranslationLocalGlobal6D().second); local.SetTranslationLocalGlobal6D(ikparam.GetTranslationLocalGlobal 6D().first, t*ikparam.GetTranslationLocalGlobal6D().second);
break; break;
default: case IKP_TranslationXAxisAngle4D: {
throw openrave_exception(str(boost::format("does not support parame std::pair<Vector,dReal> p = ikparam.GetTranslationXAxisAngle4D();
terization %d")%ikparam.GetType())); // don't change the angle since don't know the exact direction it i
} s pointing at
return local; local.SetTranslationXAxisAngle4D(t*p.first,p.second);
}
inline std::ostream& operator<<(std::ostream& O, const IkParameterization &
ikparam)
{
O << ikparam._type << " ";
switch(ikparam._type) {
case IkParameterization::Type_Transform6D:
O << ikparam.GetTransform6D();
break;
case IkParameterization::Type_Rotation3D:
O << ikparam.GetRotation3D();
break;
case IkParameterization::Type_Translation3D: {
Vector v = ikparam.GetTranslation3D();
O << v.x << " " << v.y << " " << v.z << " ";
break;
}
case IkParameterization::Type_Direction3D: {
Vector v = ikparam.GetDirection3D();
O << v.x << " " << v.y << " " << v.z << " ";
break; break;
} }
case IkParameterization::Type_Ray4D: { case IKP_TranslationYAxisAngle4D: {
O << ikparam.GetRay4D(); std::pair<Vector,dReal> p = ikparam.GetTranslationYAxisAngle4D();
// don't change the angle since don't know the exact direction it i
s pointing at
local.SetTranslationYAxisAngle4D(t*p.first,p.second);
break; break;
} }
case IkParameterization::Type_Lookat3D: { case IKP_TranslationZAxisAngle4D: {
Vector v = ikparam.GetLookat3D(); std::pair<Vector,dReal> p = ikparam.GetTranslationZAxisAngle4D();
O << v.x << " " << v.y << " " << v.z << " "; // don't change the angle since don't know the exact direction it i
s pointing at
local.SetTranslationZAxisAngle4D(t*p.first,p.second);
break; break;
} }
case IkParameterization::Type_TranslationDirection5D: case IKP_TranslationXAxisAngleZNorm4D: {
O << ikparam.GetTranslationDirection5D(); std::pair<Vector,dReal> p = ikparam.GetTranslationXAxisAngleZNorm4D
break; ();
case IkParameterization::Type_TranslationXY2D: { // don't change the angle since don't know the exact direction it i
Vector v = ikparam.GetTranslationXY2D(); s pointing at
O << v.x << " " << v.y << " "; local.SetTranslationXAxisAngleZNorm4D(t*p.first,p.second);
break; break;
} }
case IkParameterization::Type_TranslationXYOrientation3D: { case IKP_TranslationYAxisAngleXNorm4D: {
Vector v = ikparam.GetTranslationXYOrientation3D(); std::pair<Vector,dReal> p = ikparam.GetTranslationYAxisAngleXNorm4D
O << v.x << " " << v.y << " " << v.z << " "; ();
// don't change the angle since don't know the exact direction it i
s pointing at
local.SetTranslationYAxisAngleXNorm4D(t*p.first,p.second);
break; break;
} }
case IkParameterization::Type_TranslationLocalGlobal6D: { case IKP_TranslationZAxisAngleYNorm4D: {
std::pair<Vector,Vector> p = ikparam.GetTranslationLocalGlobal6D(); std::pair<Vector,dReal> p = ikparam.GetTranslationZAxisAngleYNorm4D
O << p.first.x << " " << p.first.y << " " << p.first.z << " " << p. ();
second.x << " " << p.second.y << " " << p.second.z << " "; // don't change the angle since don't know the exact direction it i
s pointing at
local.SetTranslationZAxisAngleYNorm4D(t*p.first,p.second);
break; break;
} }
default: default:
throw openrave_exception(str(boost::format("does not support parame terization %d")%ikparam.GetType())); throw openrave_exception(str(boost::format("does not support parame terization %d")%ikparam.GetType()));
} }
return O; return local;
} }
inline std::istream& operator>>(std::istream& I, IkParameterization& ikpara OPENRAVE_API std::ostream& operator<<(std::ostream& O, const IkParameteriza
m) tion &ikparam);
OPENRAVE_API std::istream& operator>>(std::istream& I, IkParameterization&
ikparam);
/// \brief Selects which DOFs of the affine transformation to include in th
e active configuration.
enum DOFAffine
{ {
int type=IkParameterization::Type_None; DOF_NoTransform = 0,
I >> type; DOF_X = 1, ///< can move in the x direction
ikparam._type = static_cast<IkParameterization::Type>(type); DOF_Y = 2, ///< can move in the y direction
switch(ikparam._type) { DOF_Z = 4, ///< can move in the z direction
case IkParameterization::Type_Transform6D: { Transform t; I >> t; ikpar DOF_XYZ=DOF_X|DOF_Y|DOF_Z, ///< moves in xyz direction
am.SetTransform6D(t); break; }
case IkParameterization::Type_Rotation3D: { Vector v; I >> v; ikparam.S // DOF_RotationX fields are mutually exclusive
etRotation3D(v); break; } DOF_RotationAxis = 8, ///< can rotate around an axis (1 dof)
case IkParameterization::Type_Translation3D: { Vector v; I >> v.x >> v. DOF_Rotation3D = 16, ///< can rotate freely (3 dof), the parameteri
y >> v.z; ikparam.SetTranslation3D(v); break; } zation is
case IkParameterization::Type_Direction3D: { Vector v; I >> v.x >> v.y ///< theta * v, where v is the rotation axis a
>> v.z; ikparam.SetDirection3D(v); break; } nd theta is the angle about that axis
case IkParameterization::Type_Ray4D: { RAY r; I >> r; ikparam.SetRay4D( DOF_RotationQuat = 32, ///< can rotate freely (4 dof), parameteriza
r); break; } tion is a quaternion. In order for limits to work correctly, the quaternion
case IkParameterization::Type_Lookat3D: { Vector v; I >> v.x >> v.y >> is in the space of _vRotationQuatLimitStart. _vRotationQuatLimitStart is a
v.z; ikparam.SetLookat3D(v); break; } lways left-multiplied before setting the transform!
case IkParameterization::Type_TranslationDirection5D: { RAY r; I >> r; DOF_RotationMask=(DOF_RotationAxis|DOF_Rotation3D|DOF_RotationQuat), //
ikparam.SetTranslationDirection5D(r); break; } /< mask for all bits representing 3D rotations
case IkParameterization::Type_TranslationXY2D: { Vector v; I >> v.y >> DOF_Transform = (DOF_XYZ|DOF_RotationQuat), ///< translate and rotate f
v.y; ikparam.SetTranslationXY2D(v); break; } reely in 3D space
case IkParameterization::Type_TranslationXYOrientation3D: { Vector v; I };
>> v.y >> v.y >> v.z; ikparam.SetTranslationXYOrientation3D(v); break; }
case IkParameterization::Type_TranslationLocalGlobal6D: { Vector localt /** \brief Given a mask of affine dofs and a dof inside that mask, returns
rans, trans; I >> localtrans.x >> localtrans.y >> localtrans.z >> trans.x > the index where the value could be found.
> trans.y >> trans.z; ikparam.SetTranslationLocalGlobal6D(localtrans,trans)
; break; } \param affinedofs a mask of \ref DOFAffine values
default: throw openrave_exception(str(boost::format("does not support p \param dof a set of values inside affinedofs, the index of the first va
arameterization %d")%ikparam.GetType())); lue is returned
} \throw openrave_exception throws if dof is not present in affinedofs
return I; */
} OPENRAVE_API int RaveGetIndexFromAffineDOF(int affinedofs, DOFAffine dof);
/** \brief Given a mask of affine dofs and an index into the array, returns
the affine dof that is being referenced
\param affinedofs a mask of \ref DOFAffine values
\param index an index into the affine dof array
\throw openrave_exception throws if dof if index is out of bounds
*/
OPENRAVE_API DOFAffine RaveGetAffineDOFFromIndex(int affinedofs, int index)
;
/// \brief Returns the degrees of freedom needed to represent all the value
s in the affine dof mask.
///
/// \throw openrave_exception throws if
OPENRAVE_API int RaveGetAffineDOF(int affinedofs);
/** \brief Converts the transformation matrix into the specified affine val
ues format.
\param[out] itvalues an iterator to the vector to write the values to.
Will write exactly \ref RaveGetAffineDOF(affinedofs) values.
\param[in] the affine transformation to convert
\param[in] affinedofs the affine format to output values in
\param[in] vActvAffineRotationAxis optional rotation axis if affinedofs
specified \ref DOF_RotationAxis
*/
OPENRAVE_API void RaveGetAffineDOFValuesFromTransform(std::vector<dReal>::i
terator itvalues, const Transform& t, int affinedofs, const Vector& vActvAf
fineRotationAxis=Vector(0,0,1));
/** \brief Converts affine dof values into a transform.
Note that depending on what the dof values holds, only a part of the tr
ansform will be updated.
\param[out] t the output transform
\param[in] itvalues the start iterator of the affine dof values
\param[in] affinedofs the affine dof mask
\param[in] vActvAffineRotationAxis optional rotation axis if affinedofs
specified \ref DOF_RotationAxis
*/
OPENRAVE_API void RaveGetTransformFromAffineDOFValues(Transform& t, std::ve
ctor<dReal>::const_iterator itvalues, int affinedofs, const Vector& vActvAf
fineRotationAxis=Vector(0,0,1));
OPENRAVE_API ConfigurationSpecification RaveGetAffineConfigurationSpecifica
tion(int affinedofs,KinBodyConstPtr pbody=KinBodyConstPtr());
} }
#include <openrave/plugininfo.h> #include <openrave/plugininfo.h>
#include <openrave/interface.h> #include <openrave/interface.h>
#include <openrave/spacesampler.h> #include <openrave/spacesampler.h>
#include <openrave/kinbody.h> #include <openrave/kinbody.h>
#include <openrave/trajectory.h> #include <openrave/trajectory.h>
#include <openrave/module.h> #include <openrave/module.h>
#include <openrave/collisionchecker.h> #include <openrave/collisionchecker.h>
skipping to change at line 1254 skipping to change at line 1827
return boost::static_pointer_cast<T const>(pinterface); return boost::static_pointer_cast<T const>(pinterface);
} }
} }
return boost::shared_ptr<T>(); return boost::shared_ptr<T>();
} }
/// \brief returns a lower case string of the interface type /// \brief returns a lower case string of the interface type
OPENRAVE_API const std::map<InterfaceType,std::string>& RaveGetInterfaceNam esMap(); OPENRAVE_API const std::map<InterfaceType,std::string>& RaveGetInterfaceNam esMap();
OPENRAVE_API const std::string& RaveGetInterfaceName(InterfaceType type); OPENRAVE_API const std::string& RaveGetInterfaceName(InterfaceType type);
/// \brief returns a string of the ik parameterization type names (can incl /// \brief returns a string of the ik parameterization type names (can incl
ude upper case in order to match IkParameterization::Type) ude upper case in order to match \ref IkParameterizationType)
OPENRAVE_API const std::map<IkParameterization::Type,std::string>& RaveGetI OPENRAVE_API const std::map<IkParameterizationType,std::string>& RaveGetIkP
kParameterizationMap(); arameterizationMap();
/// \brief Returns the openrave home directory where settings, cache, and o ther files are stored. /// \brief Returns the openrave home directory where settings, cache, and o ther files are stored.
/// ///
/// On Linux/Unix systems, this is usually $HOME/.openrave, on Windows this is $HOMEPATH/.openrave /// On Linux/Unix systems, this is usually $HOME/.openrave, on Windows this is $HOMEPATH/.openrave
OPENRAVE_API std::string RaveGetHomeDirectory(); OPENRAVE_API std::string RaveGetHomeDirectory();
/// \brief Searches for a filename in the database and returns a full path/ URL to it /// \brief Searches for a filename in the database and returns a full path/ URL to it
/// ///
/// \param filename the relative filename in the database /// \param filename the relative filename in the database
/// \param bRead if true will only return a file if it exists. If false, wi ll return the filename of the first valid database directory. /// \param bRead if true will only return a file if it exists. If false, wi ll return the filename of the first valid database directory.
skipping to change at line 1333 skipping to change at line 1906
OPENRAVE_API ModuleBasePtr RaveCreateModule(EnvironmentBasePtr penv, const std::string& name); OPENRAVE_API ModuleBasePtr RaveCreateModule(EnvironmentBasePtr penv, const std::string& name);
OPENRAVE_API ModuleBasePtr RaveCreateProblem(EnvironmentBasePtr penv, const std::string& name); OPENRAVE_API ModuleBasePtr RaveCreateProblem(EnvironmentBasePtr penv, const std::string& name);
OPENRAVE_API ModuleBasePtr RaveCreateProblemInstance(EnvironmentBasePtr pen v, const std::string& name); OPENRAVE_API ModuleBasePtr RaveCreateProblemInstance(EnvironmentBasePtr pen v, const std::string& name);
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 initialized to nDOF degrees
of freedom. Will be deprecated soon
OPENRAVE_API TrajectoryBasePtr RaveCreateTrajectory(EnvironmentBasePtr penv
, int nDOF);
/// \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="");
OPENRAVE_API TrajectoryBasePtr RaveCreateTrajectory(EnvironmentBasePtr penv
, int dof) RAVE_DEPRECATED;
/** \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
*/ */
OPENRAVE_API boost::shared_ptr<void> RaveRegisterInterface(InterfaceType ty pe, const std::string& name, const char* interfacehash, const char* envhash , const boost::function<InterfaceBasePtr(EnvironmentBasePtr, std::istream&) >& createfn); OPENRAVE_API UserDataPtr RaveRegisterInterface(InterfaceType type, const st d::string& name, const char* interfacehash, const char* envhash, const boos t::function<InterfaceBasePtr(EnvironmentBasePtr, std::istream&)>& createfn) ;
/** \brief Registers a custom xml reader for a particular interface. /** \brief Registers a custom xml reader for a particular interface.
Once registered, anytime an interface is created through XML and Once registered, anytime an interface is created through XML and
the xmltag is seen, the function CreateXMLReaderFn will be called to ge t a reader for that tag the xmltag is seen, the function CreateXMLReaderFn will be called to ge t a reader for that tag
\param xmltag the tag specified in xmltag is seen in the interface, the the custom reader will be created. \param xmltag the tag specified in xmltag is seen in the interface, the the custom reader will be created.
\param fn CreateXMLReaderFn(pinterface,atts) - passed in the pointer to the interface where the tag was seen along with the list of attributes \param fn CreateXMLReaderFn(pinterface,atts) - passed in the pointer to the interface where the tag was seen along with the list of attributes
\return a pointer holding the registration, releasing the pointer will unregister the XML reader \return a pointer holding the registration, releasing the pointer will unregister the XML reader
*/ */
OPENRAVE_API boost::shared_ptr<void> RaveRegisterXMLReader(InterfaceType ty pe, const std::string& xmltag, const CreateXMLReaderFn& fn); OPENRAVE_API UserDataPtr RaveRegisterXMLReader(InterfaceType type, const st d::string& xmltag, const CreateXMLReaderFn& fn);
/// \brief return the environment's unique id, returns 0 if environment cou ld not be found or not registered /// \brief return the environment's unique id, returns 0 if environment cou ld not be found or not registered
OPENRAVE_API int RaveGetEnvironmentId(EnvironmentBasePtr penv); OPENRAVE_API int RaveGetEnvironmentId(EnvironmentBasePtr penv);
/// \brief get the environment from its unique id /// \brief get the environment from its unique id
/// \param id the unique environment id returned by \ref RaveGetEnvironment Id /// \param id the unique environment id returned by \ref RaveGetEnvironment Id
OPENRAVE_API EnvironmentBasePtr RaveGetEnvironment(int id); OPENRAVE_API EnvironmentBasePtr RaveGetEnvironment(int id);
/// \brief Return all the created OpenRAVE environments. /// \brief Return all the created OpenRAVE environments.
OPENRAVE_API void RaveGetEnvironments(std::list<EnvironmentBasePtr>& listen vironments); OPENRAVE_API void RaveGetEnvironments(std::list<EnvironmentBasePtr>& listen vironments);
skipping to change at line 1433 skipping to change at line 2006
/// \deprecated /// \deprecated
typedef InterfaceBasePtr (*PluginExportFn_CreateInterface)(InterfaceType ty pe, const std::string& name, const char* pluginhash, EnvironmentBasePtr pen v); typedef InterfaceBasePtr (*PluginExportFn_CreateInterface)(InterfaceType ty pe, const std::string& name, const char* pluginhash, EnvironmentBasePtr pen v);
/// \deprecated /// \deprecated
typedef bool (*PluginExportFn_GetPluginAttributes)(PLUGININFO* pinfo, int s ize); typedef bool (*PluginExportFn_GetPluginAttributes)(PLUGININFO* pinfo, int s ize);
// define inline functions // define inline functions
const std::string& IkParameterization::GetName() const const std::string& IkParameterization::GetName() const
{ {
std::map<IkParameterization::Type,std::string>::const_iterator it = Rav eGetIkParameterizationMap().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(RAVE_DISABLE_ASSERT_HANDLER) && defined(BOOST_ENABLE_ASSERT_HA NDLER)
/// Modifications controlling %boost library behavior. /// Modifications controlling %boost library behavior.
skipping to change at line 1478 skipping to change at line 2051
BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::SensorBase) BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::SensorBase)
BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::SensorBase::SensorData) BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::SensorBase::SensorData)
BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::SensorSystemBase) BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::SensorSystemBase)
BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::SimpleSensorSystem) BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::SimpleSensorSystem)
BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::SimpleSensorSystem::XMLData) BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::SimpleSensorSystem::XMLData)
BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::IkSolverBase) BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::IkSolverBase)
BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::ViewerBase) BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::ViewerBase)
BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::SpaceSamplerBase) BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::SpaceSamplerBase)
BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::GraphHandle) BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::GraphHandle)
BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::IkParameterization) BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::IkParameterization)
BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::ConfigurationSpecification)
BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::ConfigurationSpecification::Group)
BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::ConfigurationSpecification::Reader)
BOOST_TYPEOF_REGISTER_TEMPLATE(OpenRAVE::RaveVector, 1) BOOST_TYPEOF_REGISTER_TEMPLATE(OpenRAVE::RaveVector, 1)
BOOST_TYPEOF_REGISTER_TEMPLATE(OpenRAVE::RaveTransform, 1) BOOST_TYPEOF_REGISTER_TEMPLATE(OpenRAVE::RaveTransform, 1)
BOOST_TYPEOF_REGISTER_TEMPLATE(OpenRAVE::RaveTransformMatrix, 1) BOOST_TYPEOF_REGISTER_TEMPLATE(OpenRAVE::RaveTransformMatrix, 1)
BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::KinBody) BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::KinBody)
BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::KinBody::Joint) BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::KinBody::Joint)
BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::KinBody::Joint::MIMIC) BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::KinBody::Joint::MIMIC)
BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::KinBody::Link) BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::KinBody::Link)
BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::KinBody::Link::GEOMPROPERTIES) BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::KinBody::Link::GEOMPROPERTIES)
BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::KinBody::Link::TRIMESH) BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::KinBody::Link::TRIMESH)
 End of changes. 62 change blocks. 
171 lines changed or deleted 943 lines changed or added


 physicsengine.h   physicsengine.h 
skipping to change at line 103 skipping to change at line 103
/// \param link the link to add a torque to /// \param link the link to add a torque to
/// \param torque torque vector /// \param torque torque vector
/// \param bAdd if true, torque is added to previous torques, otherwise it is set /// \param bAdd if true, torque is added to previous torques, otherwise it is set
virtual bool SetBodyTorque(KinBody::LinkPtr link, const Vector& torque, bool bAdd) OPENRAVE_DUMMY_IMPLEMENTATION; virtual bool SetBodyTorque(KinBody::LinkPtr link, const Vector& torque, bool bAdd) OPENRAVE_DUMMY_IMPLEMENTATION;
/// adds torque to a joint /// adds torque to a joint
/// \param pjoint - the joint the torque is added to /// \param pjoint - the joint the torque is added to
/// \param pTorques - the torques added to the joint. Pointer because t he joint dof can be greater than 1. /// \param pTorques - the torques added to the joint. Pointer because t he joint dof can be greater than 1.
virtual bool AddJointTorque(KinBody::JointPtr pjoint, const std::vector <dReal>& pTorques) OPENRAVE_DUMMY_IMPLEMENTATION; virtual bool AddJointTorque(KinBody::JointPtr pjoint, const std::vector <dReal>& pTorques) OPENRAVE_DUMMY_IMPLEMENTATION;
/// \param[in] link the link /// \param[in] link a constant pointer to a link
/// \param[out] force current force on the COM of the link
/// \param[out] torque current torque on the COM of the link
virtual bool GetLinkForceTorque(KinBody::LinkConstPtr link, Vector& for
ce, Vector& torque) OPENRAVE_DUMMY_IMPLEMENTATION;
/// \param[in] joint
/// \param[out] force current accumulated force on the COM of the link /// \param[out] force current accumulated force on the COM of the link
/// \param[out] torque current accumulated torque on the COM of the lin k /// \param[out] torque current accumulated torque on the COM of the lin k
virtual bool GetLinkForceTorque(KinBody::LinkConstPtr link, Vector& for ce, Vector& torque) OPENRAVE_DUMMY_IMPLEMENTATION; //virtual bool GetJointForceTorque(KinBody::JointConstPtr joint, KinBod y::LinkConstPtr link, Vector& force, Vector& torque) OPENRAVE_DUMMY_IMPLEME NTATION;
/// set the gravity direction /// set the gravity direction
virtual void SetGravity(const Vector& gravity) OPENRAVE_DUMMY_IMPLEMENT ATION; virtual void SetGravity(const Vector& gravity) OPENRAVE_DUMMY_IMPLEMENT ATION;
virtual Vector GetGravity() OPENRAVE_DUMMY_IMPLEMENTATION; virtual Vector GetGravity() OPENRAVE_DUMMY_IMPLEMENTATION;
/// dynamically simulate system for fTimeElapsed seconds /// dynamically simulate system for fTimeElapsed seconds
/// add torques to the joints of the body. Torques disappear after one timestep of simulation /// add torques to the joints of the body. Torques disappear after one timestep of simulation
virtual void SimulateStep(dReal fTimeElapsed)=0; virtual void SimulateStep(dReal fTimeElapsed)=0;
/// \deprecated (10/11/18) /// \deprecated (10/11/18)
 End of changes. 2 change blocks. 
2 lines changed or deleted 8 lines changed or added


 planner.h   planner.h 
skipping to change at line 25 skipping to change at line 25
// 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.
*/ */
#ifndef OPENRAVE_PLANNER_H #ifndef OPENRAVE_PLANNER_H
#define OPENRAVE_PLANNER_H #define OPENRAVE_PLANNER_H
namespace OpenRAVE { namespace OpenRAVE {
/** \brief <b>[interface]</b> Planner interface that generates trajectories /// \brief the status of the PlanPath method. Used when PlanPath can be cal
for the robot to follow around the environment. See \ref arch_planner. led multiple times to resume planning.
enum PlannerStatus
{
PS_Failed = 0, ///< planner failed
PS_HasSolution = 1, ///< planner succeeded
PS_Interrupted = 2, ///< planning was interrupted, but can be resumed b
y calling PlanPath again
PS_InterruptedWithSolution = 3, /// planning was interrupted, but a val
id path/solution was returned. Can call PlanPath again to refine results
};
/// \brief action to send to the planner while it is planning. This is usua
lly done by the user-specified planner callback function
enum PlannerAction
{
PA_None=0, ///< no action
PA_Interrupt=1, ///< interrupt the planner and return to user
PA_ReturnWithAnySolution=2, ///< return quickly with any path
};
/** \brief <b>[interface]</b> Planner interface that generates trajectories
for target objects to follow through the environment. <b>If not specified,
method is not multi-thread safe.</b> See \ref arch_planner.
\ingroup interfaces \ingroup interfaces
*/ */
class OPENRAVE_API PlannerBase : public InterfaceBase class OPENRAVE_API PlannerBase : public InterfaceBase
{ {
public: public:
typedef std::list< std::vector<dReal> > ConfigurationList; typedef std::list< std::vector<dReal> > ConfigurationList;
typedef boost::shared_ptr< PlannerBase::ConfigurationList > Configurati onListPtr; typedef boost::shared_ptr< PlannerBase::ConfigurationList > Configurati onListPtr;
/** \brief Describes a common and serializable interface for planning p arameters. /** \brief Describes a common and serializable interface for planning p arameters.
skipping to change at line 51 skipping to change at line 68
Also allows the parameters and descriptions to be serialized to reS tructuredText for documentation purposes. Also allows the parameters and descriptions to be serialized to reS tructuredText for documentation purposes.
*/ */
class OPENRAVE_API PlannerParameters : public BaseXMLReader, public XML Readable class OPENRAVE_API PlannerParameters : public BaseXMLReader, public XML Readable
{ {
public: public:
PlannerParameters(); PlannerParameters();
virtual ~PlannerParameters() { virtual ~PlannerParameters() {
} }
/// tries to copy data from one set of parameters to another in the /** \brief Attemps to copy data from one set of parameters to anoth
safest manner. er in the safest manner.
/// First serializes the data of the right hand into a string, then
initializes the current parameters via >> First serializes the data of the right hand into a string, then
/// pointers to functions are copied directly initializes the current parameters via >>
pointers to functions are copied directly
*/
virtual PlannerParameters& operator=(const PlannerParameters& r); virtual PlannerParameters& operator=(const PlannerParameters& r);
virtual void copy(boost::shared_ptr<PlannerParameters const> r); virtual void copy(boost::shared_ptr<PlannerParameters const> r);
/// sets up the planner parameters to use the active joints of the robot /// sets up the planner parameters to use the active joints of the robot
virtual void SetRobotActiveJoints(RobotBasePtr robot); virtual void SetRobotActiveJoints(RobotBasePtr robot);
/// \brief the configuration specification in which the planner wor
ks in. This specification is passed to the trajecotry creation modules.
ConfigurationSpecification _configurationspecification;
/// \brief Cost function on the state pace (optional). /// \brief Cost function on the state pace (optional).
/// ///
/// cost = _costfn(config) /// cost = _costfn(config)
/// \param cost the cost of being in the current state /// \param cost the cost of being in the current state
typedef boost::function<dReal(const std::vector<dReal>&)> CostFn; typedef boost::function<dReal(const std::vector<dReal>&)> CostFn;
CostFn _costfn; CostFn _costfn;
/// \brief Goal heuristic function.(optional) /** \brief Goal heuristic function.(optional)
///
/// distance = _goalfn(config) distance = _goalfn(config)
///
/// Goal is complete when returns 0 Goal is complete when returns 0
/// \param distance - distance to closest goal \param distance - distance to closest goal
*/
typedef boost::function<dReal(const std::vector<dReal>&)> GoalFn; typedef boost::function<dReal(const std::vector<dReal>&)> GoalFn;
GoalFn _goalfn; GoalFn _goalfn;
/// \brief optional, Distance metric between configuration spaces, /// \brief Distance metric between configuration spaces (optional)
two configurations are considered the same when this returns 0: distmetric( ///
config1,config2) /// distmetric(config1,config2)
///
/// Two configurations are considered the same when function return
s 0.
typedef boost::function<dReal(const std::vector<dReal>&, const std: :vector<dReal>&)> DistMetricFn; typedef boost::function<dReal(const std::vector<dReal>&, const std: :vector<dReal>&)> DistMetricFn;
DistMetricFn _distmetricfn; DistMetricFn _distmetricfn;
/** \brief Checks that all the constraints are satisfied between tw o configurations. /** \brief Checks that all the constraints are satisfied between tw o configurations.
The simplest and most fundamental constraint is line-collision checking. The robot goes from q0 to q1. The simplest and most fundamental constraint is line-collision checking. The robot goes from q0 to q1.
success = _checkpathconstraints(q0,q1,interval,configurations) success = _checkpathconstraints(q0,q1,interval,configurations)
When called, q0 is guaranteed to be set on the robot. When called, q0 is guaranteed to be set on the robot.
skipping to change at line 99 skipping to change at line 126
Because this function can internally use neighstatefn, need to make sure that Q0->Q1 is going from initial to goal direction. Because this function can internally use neighstatefn, need to make sure that Q0->Q1 is going from initial to goal direction.
\param q0 is the configuration the robot is coming from (curren tly set). \param q0 is the configuration the robot is coming from (curren tly set).
\param q1 is the configuration the robot should move to. \param q1 is the configuration the robot should move to.
\param interval Specifies whether to check the end points of th e interval for constraints \param interval Specifies whether to check the end points of th e interval for constraints
\param configurations Optional argument that will hold the inte rmediate configuraitons checked between q0 and q1 configurations. The appen ded configurations will be all valid and in free space. They are appended a fter the items already stored on the list. \param configurations Optional argument that will hold the inte rmediate configuraitons checked between q0 and q1 configurations. The appen ded configurations will be all valid and in free space. They are appended a fter the items already stored on the list.
*/ */
typedef boost::function<bool (const std::vector<dReal>&, const std: :vector<dReal>&, IntervalType, PlannerBase::ConfigurationListPtr)> CheckPat hConstraintFn; typedef boost::function<bool (const std::vector<dReal>&, const std: :vector<dReal>&, IntervalType, PlannerBase::ConfigurationListPtr)> CheckPat hConstraintFn;
CheckPathConstraintFn _checkpathconstraintsfn; CheckPathConstraintFn _checkpathconstraintsfn;
/// \brief Samples a random configuration (mandatory) /** \brief Samples a random configuration (mandatory)
///
/// The dimension of the returned sample is the dimension of the co The dimension of the returned sample is the dimension of the co
nfiguration space. nfiguration space.
/// success = samplefn(newsample) success = samplefn(newsample)
*/
typedef boost::function<bool (std::vector<dReal>&)> SampleFn; typedef boost::function<bool (std::vector<dReal>&)> SampleFn;
SampleFn _samplefn; SampleFn _samplefn;
/// \brief Samples a valid goal configuration (optional). /** \brief Samples a valid goal configuration (optional).
///
/// If valid, the function should be called If valid, the function should be called
/// at every iteration. Any type of sampling probabilities and cond at every iteration. Any type of sampling probabilities and cond
itions can be encoded inside the function. itions can be encoded inside the function.
/// The dimension of the returned sample is the dimension of the co The dimension of the returned sample is the dimension of the co
nfiguration space. nfiguration space.
/// success = samplegoalfn(newsample) success = samplegoalfn(newsample)
*/
typedef boost::function<bool (std::vector<dReal>&)> SampleGoalFn; typedef boost::function<bool (std::vector<dReal>&)> SampleGoalFn;
SampleGoalFn _samplegoalfn; SampleGoalFn _samplegoalfn;
/// \brief Samples a valid initial configuration (optional). /** \brief Samples a valid initial configuration (optional).
///
/// If valid, the function should be called If valid, the function should be called
/// at every iteration. Any type of sampling probabilities and cond at every iteration. Any type of sampling probabilities and cond
itions can be encoded inside the function. itions can be encoded inside the function.
/// The dimension of the returned sample is the dimension of the co The dimension of the returned sample is the dimension of the co
nfiguration space. nfiguration space.
/// success = sampleinitialfn(newsample) success = sampleinitialfn(newsample)
*/
typedef boost::function<bool (std::vector<dReal>&)> SampleInitialFn ; typedef boost::function<bool (std::vector<dReal>&)> SampleInitialFn ;
SampleInitialFn _sampleinitialfn; SampleInitialFn _sampleinitialfn;
/** \brief Returns a random configuration around a neighborhood (op tional). /** \brief Returns a random configuration around a neighborhood (op tional).
_sampleneighfn(newsample,pCurSample,fRadius) _sampleneighfn(newsample,pCurSample,fRadius)
\param pCurSample - the neighborhood to sample around \param pCurSample - the neighborhood to sample around
\param fRadius - specifies the max distance of sampling. The h igher the value, the farther the samples will go \param fRadius - specifies the max distance of sampling. The h igher the value, the farther the samples will go
The distance metric can be arbitrary, but is usually PlannerParameters::pdistmetric. The distance metric can be arbitrary, but is usually PlannerParameters::pdistmetric.
skipping to change at line 173 skipping to change at line 203
typedef boost::function<bool (std::vector<dReal>&,const std::vector <dReal>&, int)> NeighStateFn; typedef boost::function<bool (std::vector<dReal>&,const std::vector <dReal>&, int)> NeighStateFn;
NeighStateFn _neighstatefn; NeighStateFn _neighstatefn;
/// to specify multiple initial or goal configurations, put them in to the vector in series /// to specify multiple initial or goal configurations, put them in to the vector in series
/// (note: not all planners support multiple goals) /// (note: not all planners support multiple goals)
std::vector<dReal> vinitialconfig, vgoalconfig; std::vector<dReal> vinitialconfig, vgoalconfig;
/// \brief the absolute limits of the configuration space. /// \brief the absolute limits of the configuration space.
std::vector<dReal> _vConfigLowerLimit, _vConfigUpperLimit; std::vector<dReal> _vConfigLowerLimit, _vConfigUpperLimit;
/// \brief the absolute velocity limits of each DOF of the configur
ation space.
std::vector<dReal> _vConfigVelocityLimit;
/// \brief the absolute acceleration limits of each DOF of the conf
iguration space.
std::vector<dReal> _vConfigAccelerationLimit;
/// \brief the discretization resolution of each dimension of the c onfiguration space /// \brief the discretization resolution of each dimension of the c onfiguration space
std::vector<dReal> _vConfigResolution; std::vector<dReal> _vConfigResolution;
/** \brief a discretization between the path that connects two conf igurations /** \brief a discretization between the path that connects two conf igurations
This length represents how dense the samples get distributed ac ross the configuration space. This length represents how dense the samples get distributed ac ross the configuration space.
It represents the maximum distance between neighbors when addin g new configuraitons. It represents the maximum distance between neighbors when addin g new configuraitons.
If 0 or less, planner chooses best step length. If 0 or less, planner chooses best step length.
*/ */
dReal _fStepLength; dReal _fStepLength;
/// \brief maximum number of iterations before the planner gives up . If 0 or less, planner chooses best iterations. /// \brief maximum number of iterations before the planner gives up . If 0 or less, planner chooses best iterations.
int _nMaxIterations; int _nMaxIterations;
/// \brief Specifies the planner that will perform the post-process ing path smoothing before returning. /// \brief Specifies the planner that will perform the post-process ing path smoothing before returning.
/// ///
/// If empty, will not path smooth the returned trajectories (used to measure algorithm time) /// If empty, will not path smooth the returned trajectories (used to measure algorithm time)
std::string _sPathOptimizationPlanner; std::string _sPostProcessingPlanner;
/// \brief The serialized planner parameters to pass to the path op timizer. /// \brief The serialized planner parameters to pass to the path op timizer.
/// ///
/// For example: std::stringstream(_sPathOptimizationParameters) >> /// For example: std::stringstream(_sPostProcessingParameters) >> _
_parameters; parameters;
std::string _sPathOptimizationParameters; std::string _sPostProcessingParameters;
/// \brief Extra parameters data that does not fit within this plan ner parameters structure, but is still important not to lose all the inform ation. /// \brief Extra parameters data that does not fit within this plan ner parameters structure, but is still important not to lose all the inform ation.
std::string _sExtraParameters; std::string _sExtraParameters;
/// \brief Return the degrees of freedom of the planning configurat ion space /// \brief Return the degrees of freedom of the planning configurat ion space
virtual int GetDOF() const { virtual int GetDOF() const {
return (int)_vConfigLowerLimit.size(); return (int)_vConfigLowerLimit.size();
} }
protected: protected:
inline boost::shared_ptr<PlannerBase::PlannerParameters> shared_par ameters() { inline boost::shared_ptr<PlannerBase::PlannerParameters> shared_par ameters() {
return boost::static_pointer_cast<PlannerBase::PlannerParameter s>(shared_from_this()); return boost::static_pointer_cast<PlannerBase::PlannerParameter s>(shared_from_this());
} }
inline boost::shared_ptr<PlannerBase::PlannerParameters const > sha red_parameters_const() const { inline boost::shared_ptr<PlannerBase::PlannerParameters const > sha red_parameters_const() const {
return boost::static_pointer_cast<PlannerBase::PlannerParameter s const>(shared_from_this()); return boost::static_pointer_cast<PlannerBase::PlannerParameter s const>(shared_from_this());
} }
/// output the planner parameters in a string (in XML format) /// \brief output the planner parameters in a string (in XML format )
/// don't use PlannerParameters as a tag! /// don't use PlannerParameters as a tag!
virtual bool serialize(std::ostream& O) const; virtual bool serialize(std::ostream& O) const;
//@{ XML parsing functions, parses the default parameters //@{ XML parsing functions, parses the default parameters
virtual ProcessElement startElement(const std::string& name, const AttributesList& atts); virtual ProcessElement startElement(const std::string& name, const AttributesList& atts);
virtual bool endElement(const std::string& name); virtual bool endElement(const std::string& name);
virtual void characters(const std::string& ch); virtual void characters(const std::string& ch);
std::stringstream _ss; ///< holds the data read by characte rs std::stringstream _ss; ///< holds the data read by characte rs
boost::shared_ptr<std::stringstream> _sslocal; boost::shared_ptr<std::stringstream> _sslocal;
/// all the top-level XML parameter tags (lower case) that are hand led by this parameter structure, should be registered in the constructor /// all the top-level XML parameter tags (lower case) that are hand led by this parameter structure, should be registered in the constructor
std::vector<std::string> _vXMLParameters; std::vector<std::string> _vXMLParameters;
//@} //@}
private: private:
/// disallow copy constructors since it gets complicated with virtu /// prevent copy constructors since it gets complicated with virtua
alization l functions
PlannerParameters(const PlannerParameters &r) : XMLReadable("") { PlannerParameters(const PlannerParameters &r);
BOOST_ASSERT(0);
}
BaseXMLReaderPtr __pcurreader; ///< temporary reader BaseXMLReaderPtr __pcurreader; ///< temporary reader
std::string __processingtag; std::string __processingtag;
int _plannerparametersdepth; int _plannerparametersdepth;
/// outputs the data and surrounds it with \verbatim <PlannerParame ters> \endverbatim tags /// outputs the data and surrounds it with \verbatim <PlannerParame ters> \endverbatim tags
friend OPENRAVE_API std::ostream& operator<<(std::ostream& O, const PlannerParameters& v); friend OPENRAVE_API std::ostream& operator<<(std::ostream& O, const PlannerParameters& v);
/// expects \verbatim <PlannerParameters> \endverbatim to be the fi rst token. Parses stream until \verbatim </PlannerParameters> \endverbatim reached /// expects \verbatim <PlannerParameters> \endverbatim to be the fi rst token. Parses stream until \verbatim </PlannerParameters> \endverbatim reached
friend OPENRAVE_API std::istream& operator>>(std::istream& I, Plann erParameters& v); friend OPENRAVE_API std::istream& operator>>(std::istream& I, Plann erParameters& v);
}; };
typedef boost::shared_ptr<PlannerBase::PlannerParameters> PlannerParame tersPtr; typedef boost::shared_ptr<PlannerBase::PlannerParameters> PlannerParame tersPtr;
typedef boost::shared_ptr<PlannerBase::PlannerParameters const> Planner ParametersConstPtr; typedef boost::shared_ptr<PlannerBase::PlannerParameters const> Planner ParametersConstPtr;
typedef boost::weak_ptr<PlannerBase::PlannerParameters> PlannerParamete rsWeakPtr; typedef boost::weak_ptr<PlannerBase::PlannerParameters> PlannerParamete rsWeakPtr;
PlannerBase(EnvironmentBasePtr penv) : InterfaceBase(PT_Planner, penv) /// \brief Planner progress information passed to each callback functio
{ n
} class OPENRAVE_API PlannerProgress
{
public:
PlannerProgress();
int _iteration;
};
PlannerBase(EnvironmentBasePtr penv);
virtual ~PlannerBase() { virtual ~PlannerBase() {
} }
/// \return the static interface type this class points to (used for sa fe casting) /// \return the static interface type this class points to (used for sa fe casting)
static inline InterfaceType GetInterfaceTypeStatic() { static inline InterfaceType GetInterfaceTypeStatic() {
return PT_Planner; return PT_Planner;
} }
/// \brief Setup scene, robot, and properties of the plan, and reset al /** \brief Setup scene, robot, and properties of the plan, and reset al
l internal structures. l internal structures.
/// \param probot The robot will be planning for.
/// \param pparams The parameters of the planner, any class derived fro
m PlannerParameters can be passed. The planner should copy these parameters
for future instead of storing the pointer.
virtual bool InitPlan(RobotBasePtr probot, PlannerParametersConstPtr pp
arams) = 0;
/** \brief Setup scene, robot, and properties of the plan, and reset al \param robot main robot to be used for planning
l structures with pparams. \param params The parameters of the planner, any class derived from
PlannerParameters can be passed. The planner should copy these parameters
for future instead of storing the pointer.
*/
virtual bool InitPlan(RobotBasePtr robot, PlannerParametersConstPtr par
ams) = 0;
\param robot The robot will be planning for. Although the configura /** \brief Setup scene, robot, and properties of the plan, and reset al
tion space of the planner and the robot can be independent, l structures with pparams.
the planner uses the robot to check for environment and self-collis
ions.
In order to speed up computations further, planners can use the CO_
ActiveDOFs collision checker option, which only focuses
collision on the currently moving links in the robot.
Even if the configuration space of the planner is different from th
e robot, the robot active DOFs must be set correctly (or at least have all
dofs active)!
\param robot main robot to be used for planning
\param isParameters The serialized form of the parameters. By defau lt, this exists to allow third parties to \param isParameters The serialized form of the parameters. By defau lt, this exists to allow third parties to
pass information to planners without excplicitly knowning the forma t/internal structures used pass information to planners without excplicitly knowning the forma t/internal structures used
\return true if plan is initialized successfully and initial condit ions are satisfied. \return true if plan is initialized successfully and initial condit ions are satisfied.
*/ */
virtual bool InitPlan(RobotBasePtr robot, std::istream& isParameters); virtual bool InitPlan(RobotBasePtr robot, std::istream& isParameters);
/** \brief Executes the main planner trying to solve for the goal condi tion. /** \brief Executes the main planner trying to solve for the goal condi tion.
Fill ptraj with the trajectory of the planned path that the robot n eeds to execute Fill ptraj with the trajectory of the planned path that the robot n eeds to execute
\param ptraj The output trajectory the robot has to follow in order to successfully complete the plan. If this planner is a path optimizer, th e trajectory can be used as an input for generating a smoother path. The tr ajectory is for the configuration degrees of freedom defined by the planner parameters. \param ptraj The output trajectory the robot has to follow in order to successfully complete the plan. If this planner is a path optimizer, th e trajectory can be used as an input for generating a smoother path. The tr ajectory is for the configuration degrees of freedom defined by the planner parameters.
\param pOutStream If specified, planner will output any other speci \return the status that the planner returned in.
al data
\return true if planner is successful
*/ */
virtual bool PlanPath(TrajectoryBasePtr ptraj, boost::shared_ptr<std::o virtual PlannerStatus PlanPath(TrajectoryBasePtr ptraj) = 0;
stream> pOutStream = boost::shared_ptr<std::ostream>()) = 0;
/// \deprecated (11/10/03)
virtual PlannerStatus PlanPath(TrajectoryBasePtr ptraj, boost::shared_p
tr<std::ostream> pOutStream) RAVE_DEPRECATED {
if( !!pOutStream ) {
RAVELOG_WARN("planner does not support pOutputStream anymore, p
lease find another method to return information like using SendCommand or w
riting the data into the returned trajectory\n");
}
return PlanPath(ptraj);
}
/// \brief return the internal parameters of the planner /// \brief return the internal parameters of the planner
virtual PlannerParametersConstPtr GetParameters() const = 0; virtual PlannerParametersConstPtr GetParameters() const = 0;
/** \brief Callback function during planner execute
\param progress planner progress information
*/
typedef boost::function<PlannerAction(const PlannerProgress&)> PlanCall
backFn;
/** \brief register a function that is called periodically during the p
lan loop.
Allows the calling process to control the behavior of the planner f
rom a high-level perspective
*/
virtual UserDataPtr RegisterPlanCallback(const PlanCallbackFn& callback
fn);
protected: protected:
inline PlannerBasePtr shared_planner() {
return boost::static_pointer_cast<PlannerBase>(shared_from_this());
}
inline PlannerBaseConstPtr shared_planner_const() const {
return boost::static_pointer_cast<PlannerBase const>(shared_from_th
is());
}
/** \brief Calls a planner to optimizes the trajectory path. /** \brief Calls a planner to optimizes the trajectory path.
The PlannerParameters structure passed into the optimization planne r is The PlannerParameters structure passed into the optimization planne r is
constructed with the same freespace constraints as this planner. constructed with the same freespace constraints as this planner.
This function should always be called in PlanPath to post-process t he trajectory. This function should always be called in PlanPath to post-process t he trajectory.
\param probot the robot this trajectory is meant for, also uses the robot for checking collisions. \param probot the robot this trajectory is meant for, also uses the robot for checking collisions.
\param ptraj Initial trajectory to be smoothed is inputted. If opti mization path succeeds, final trajectory output is set in this variable. Th e trajectory is for the configuration degrees of freedom defined by the pla nner parameters. \param ptraj Initial trajectory to be smoothed is inputted. If opti mization path succeeds, final trajectory output is set in this variable. Th e trajectory is for the configuration degrees of freedom defined by the pla nner parameters.
*/ */
virtual bool _OptimizePath(RobotBasePtr probot, TrajectoryBasePtr ptraj virtual PlannerStatus _ProcessPostPlanners(RobotBasePtr probot, Traject
); oryBasePtr ptraj);
virtual bool _OptimizePath(RobotBasePtr probot, TrajectoryBasePtr ptraj
) RAVE_DEPRECATED {
return !!(_ProcessPostPlanners(probot,ptraj) & PS_HasSolution);
}
/// \brief Calls the registered callbacks in order and returns immediat
ely when an action other than PA_None is returned.
///
/// \param progress planner progress information
virtual PlannerAction _CallCallbacks(const PlannerProgress& progress);
private: private:
virtual const char* GetHash() const { virtual const char* GetHash() const {
return OPENRAVE_PLANNER_HASH; return OPENRAVE_PLANNER_HASH;
} }
std::list<UserDataWeakPtr> __listRegisteredCallbacks; ///< internally m
anaged callbacks
friend class CustomPlannerCallbackData;
}; };
OPENRAVE_API std::ostream& operator<<(std::ostream& O, const PlannerBase::P lannerParameters& v); OPENRAVE_API std::ostream& operator<<(std::ostream& O, const PlannerBase::P lannerParameters& v);
OPENRAVE_API std::istream& operator>>(std::istream& I, PlannerBase::Planner Parameters& v); OPENRAVE_API std::istream& operator>>(std::istream& I, PlannerBase::Planner Parameters& v);
} // end namespace OpenRAVE } // end namespace OpenRAVE
#endif #endif
 End of changes. 24 change blocks. 
77 lines changed or deleted 165 lines changed or added


 planningutils.h   planningutils.h 
skipping to change at line 21 skipping to change at line 21
// 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 .
*/ */
#ifndef OPENRAVE_PLANNINGUTIL_H #ifndef OPENRAVE_PLANNINGUTILS_H
#define OPENRAVE_PLANNINGUTIL_H #define OPENRAVE_PLANNINGUTILS_H
#include <openrave/openrave.h> #include <openrave/openrave.h>
namespace OpenRAVE { namespace OpenRAVE {
namespace planningutils { namespace planningutils {
/// \brief Jitters the active joint angles of the robot until it escapes co llision. /// \brief Jitters the active joint angles of the robot until it escapes co llision.
/// ///
/// Return 0 if jitter failed and robot is in collision, -1 if robot origin ally not in collision, 1 if jitter succeeded. /// Return 0 if jitter failed and robot is in collision, -1 if robot origin ally not in collision, 1 if jitter succeeded.
OPENRAVE_API int JitterActiveDOF(RobotBasePtr robot,int nMaxIterations=5000 ,dReal fRand=0.03f,const PlannerBase::PlannerParameters::NeighStateFn& neig hstatefn = PlannerBase::PlannerParameters::NeighStateFn()); OPENRAVE_API int JitterActiveDOF(RobotBasePtr robot,int nMaxIterations=5000 ,dReal fRand=0.03f,const PlannerBase::PlannerParameters::NeighStateFn& neig hstatefn = PlannerBase::PlannerParameters::NeighStateFn());
/// \brief Jitters the transform of a body until it escapes collision. /// \brief Jitters the transform of a body until it escapes collision.
OPENRAVE_API bool JitterTransform(KinBodyPtr pbody, float fJitter, int nMax Iterations=1000); OPENRAVE_API bool JitterTransform(KinBodyPtr pbody, float fJitter, int nMax Iterations=1000);
/** \brief validates a trajectory with respect to the planning constraints. /** \brief validates a trajectory with respect to the planning constraints. <b>[multi-thread safe]</b>
checks internal data structures and verifies that all trajectory via po ints do not violate joint position, velocity, and acceleration limits. checks internal data structures and verifies that all trajectory via po ints do not violate joint position, velocity, and acceleration limits.
\param trajectory trajectory of points to be checked \param trajectory trajectory of points to be checked
\param parameters the planner parameters passed to the planner that ret urned the trajectory \param parameters the planner parameters passed to the planner that ret urned the trajectory
\param samplingstep If == 0, then will only test the supports points in trajectory->GetPoints(). If > 0, then will sample the trajectory at this t ime interval. \param samplingstep If == 0, then will only test the supports points in trajectory->GetPoints(). If > 0, then will sample the trajectory at this t ime interval.
\throw openrave_exception If the trajectory is invalid, will throw ORE_ InconsistentConstraints. \throw openrave_exception If the trajectory is invalid, will throw ORE_ InconsistentConstraints.
*/ */
OPENRAVE_API void VerifyTrajectory(PlannerBase::PlannerParametersConstPtr p OPENRAVE_API void VerifyTrajectory(PlannerBase::PlannerParametersConstPtr p
arameters, TrajectoryBaseConstPtr trajectory, dReal samplingstep=0); 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>
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 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 hastimestamps if true, use the already initialized timestamps of
the trajectory
*/
OPENRAVE_API void SmoothActiveDOFTrajectory(TrajectoryBasePtr traj, RobotBa
sePtr robot, bool hastimestamps=false, dReal fmaxvelmult=1, const std::stri
ng& plannername="");
/** \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.
\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 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 hastimestamps if true, use the already initialized timestamps of
the trajectory
*/
OPENRAVE_API void SmoothAffineTrajectory(TrajectoryBasePtr traj, const std:
:vector<dReal>& maxvelocities, const std::vector<dReal>& maxaccelerations,
bool hastimestamps=false, const std::string& plannername="");
/** \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.
\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 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
*/
OPENRAVE_API void RetimeActiveDOFTrajectory(TrajectoryBasePtr traj, RobotBa
sePtr robot, bool hastimestamps=false, dReal fmaxvelmult=1, const std::stri
ng& plannername="");
/** \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.
\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 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 hastimestamps if true, use the already initialized timestamps of
the trajectory
*/
OPENRAVE_API void RetimeAffineTrajectory(TrajectoryBasePtr traj, const std:
:vector<dReal>& maxvelocities, const std::vector<dReal>& maxaccelerations,
bool hastimestamps=false, const std::string& plannername="");
/// \brief convert the trajectory and all its points to a new specification
OPENRAVE_API void ConvertTrajectorySpecification(TrajectoryBasePtr traj, co
nst ConfigurationSpecification& spec);
/// \brief reverses the order of the trajectory waypoints and times.
///
/// Velocities are just negated and the new trajectory is not guaranteed to
be executable or valid
OPENRAVE_API TrajectoryBasePtr ReverseTrajectory(TrajectoryBaseConstPtr tra
j);
/// \brief merges the contents of multiple trajectories into one so that ev
erything can be played simultaneously.
///
/// Each trajectory needs to have a 'deltatime' group for timestamps. The t
rajectories cannot share common configuration data because only one
/// 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.
OPENRAVE_API TrajectoryBasePtr MergeTrajectories(const std::list<Trajectory
BaseConstPtr>& listtrajectories);
/// \brief Line collision /// \brief Line collision
class OPENRAVE_API LineCollisionConstraint class OPENRAVE_API LineCollisionConstraint
{ {
public: public:
LineCollisionConstraint(); LineCollisionConstraint();
bool Check(PlannerBase::PlannerParametersWeakPtr _params, RobotBasePtr robot, const std::vector<dReal>& pQ0, const std::vector<dReal>& pQ1, Interv alType 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;
}; };
/// \brief simple distance metric based on joint weights /// \brief simple distance metric based on joint weights
class OPENRAVE_API SimpleDistanceMetric class OPENRAVE_API SimpleDistanceMetric
{ {
public: public:
skipping to change at line 85 skipping to change at line 142
SimpleNeighborhoodSampler(SpaceSamplerBasePtr psampler, const boost::fu nction<dReal(const std::vector<dReal>&, const std::vector<dReal>&)>&distmet ricfn); SimpleNeighborhoodSampler(SpaceSamplerBasePtr psampler, const boost::fu nction<dReal(const std::vector<dReal>&, const std::vector<dReal>&)>&distmet ricfn);
bool Sample(std::vector<dReal>& vNewSample, const std::vector<dReal>& v CurSample, dReal fRadius); bool Sample(std::vector<dReal>& vNewSample, const std::vector<dReal>& v CurSample, dReal fRadius);
bool Sample(std::vector<dReal>& samples); bool Sample(std::vector<dReal>& samples);
protected: protected:
SpaceSamplerBasePtr _psampler; SpaceSamplerBasePtr _psampler;
boost::function<dReal(const std::vector<dReal>&, const std::vector<dRea l>&)> _distmetricfn; boost::function<dReal(const std::vector<dReal>&, const std::vector<dRea l>&)> _distmetricfn;
}; };
/// \brief Samples numsamples of solutions and each solution to vsolutions /// \brief Samples numsamples of solutions and each solution to vsolutions
///
/// \param nummaxsamples the max samples to query from a particular workspa
ce goal. This does not necessarily mean every goal will have this many samp
les.
/// \param nummaxtries number of attemps to return a goal per Sample call.
/// \param fsampleprob The probability to attempt to sample a goal
class OPENRAVE_API ManipulatorIKGoalSampler class OPENRAVE_API ManipulatorIKGoalSampler
{ {
public: public:
ManipulatorIKGoalSampler(RobotBase::ManipulatorConstPtr pmanip, const s ManipulatorIKGoalSampler(RobotBase::ManipulatorConstPtr pmanip, const s
td::list<IkParameterization>&listparameterizations, int nummaxsamples=20, i td::list<IkParameterization>&listparameterizations, int nummaxsamples=20, i
nt nummaxtries=10, dReal fsampleprob=0.05f); nt nummaxtries=10, dReal fsampleprob=1);
virtual ~ManipulatorIKGoalSampler() {
}
//void SetCheckPathConstraintsFn(const PlannerBase::PlannerParameters:: CheckPathConstraintFn& checkfn) //void SetCheckPathConstraintsFn(const PlannerBase::PlannerParameters:: CheckPathConstraintFn& checkfn)
bool Sample(std::vector<dReal>& vgoal); virtual bool Sample(std::vector<dReal>& vgoal);
int GetIkParameterizationIndex(int index); virtual int GetIkParameterizationIndex(int index);
virtual void SetSamplingProb(dReal fsampleprob);
/// \brief set a jitter distance for the goal
///
/// \param maxdist If > 0, allows jittering of the goal IK if they caus
e the robot to be in collision and no IK solutions to be found
virtual void SetJitter(dReal maxdist);
protected: protected:
struct SampleInfo struct SampleInfo
{ {
IkParameterization _ikparam; IkParameterization _ikparam;
int _numleft; int _numleft;
SpaceSamplerBasePtr _psampler; SpaceSamplerBasePtr _psampler;
int _orgindex; int _orgindex;
}; };
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; 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;
}; };
typedef boost::shared_ptr<ManipulatorIKGoalSampler> ManipulatorIKGoalSample
rPtr;
} // planningutils } // planningutils
} // OpenRAVE } // OpenRAVE
#endif #endif
 End of changes. 9 change blocks. 
12 lines changed or deleted 122 lines changed or added


 robot.h   robot.h 
// -*- coding: utf-8 -*- // -*- coding: utf-8 -*-
// Copyright (C) 2006-2010 Rosen Diankov (rosen.diankov@gmail.com) // Copyright (C) 2006-2011 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 robot.h /** \file robot.h
\brief Base robot and manipulator description. \brief Base robot and manipulator description.
*/ */
#ifndef OPENRAVE_ROBOT_H
#ifndef RAVE_ROBOT_H #define OPENRAVE_ROBOT_H
#define RAVE_ROBOT_H
namespace OpenRAVE { namespace OpenRAVE {
/** \brief <b>[interface]</b> A robot is a kinematic body that has attached manipulators, sensors, and controllers. <b>Methods 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
{ {
public: public:
/// \brief Defines a chain of joints for an arm and set of joints for a gripper. Simplifies operating with them. /// \brief Defines a chain of joints for an arm and set of joints for a gripper. Simplifies operating with them.
class OPENRAVE_API Manipulator : public boost::enable_shared_from_this< Manipulator> class OPENRAVE_API Manipulator : public boost::enable_shared_from_this< Manipulator>
{ {
Manipulator(RobotBasePtr probot); Manipulator(RobotBasePtr probot);
Manipulator(const Manipulator &r); Manipulator(const Manipulator &r);
skipping to change at line 61 skipping to change at line 60
virtual const std::string& GetName() const { virtual const std::string& GetName() const {
return _name; return _name;
} }
virtual RobotBasePtr GetRobot() const { virtual RobotBasePtr GetRobot() const {
return RobotBasePtr(_probot); return RobotBasePtr(_probot);
} }
/// \brief Sets the ik solver and initializes it with the current m anipulator. /// \brief Sets the ik solver and initializes it with the current m anipulator.
/// ///
/// Due to complications with translation,rotation,direction,and ra y ik, /// Due to complications with translation,rotation,direction,and ra y ik,
/// the ik solver should take into account the grasp transform (_tG rasp) internally. /// the ik solver should take into account the grasp transform (_tL ocalTool) internally.
/// The actual ik primitives are transformed into the base frame on ly. /// The actual ik primitives are transformed into the base frame on ly.
virtual bool SetIkSolver(IkSolverBasePtr iksolver); virtual bool SetIkSolver(IkSolverBasePtr iksolver);
/// \brief Returns the currently set ik solver /// \brief Returns the currently set ik solver
virtual IkSolverBasePtr GetIkSolver() const { virtual IkSolverBasePtr GetIkSolver() const {
return _pIkSolver; return _pIkSolver;
} }
/// \deprecated (11/02/08) use GetIkSolver()->GetNumFreeParameters( ) /// \deprecated (11/02/08) use GetIkSolver()->GetNumFreeParameters( )
virtual int GetNumFreeParameters() const RAVE_DEPRECATED; virtual int GetNumFreeParameters() const RAVE_DEPRECATED;
skipping to change at line 87 skipping to change at line 86
virtual LinkPtr GetBase() const { virtual LinkPtr GetBase() const {
return _pBase; return _pBase;
} }
/// \brief the end effector link (used to define workspace distance ) /// \brief the end effector link (used to define workspace distance )
virtual LinkPtr GetEndEffector() const { virtual LinkPtr GetEndEffector() const {
return _pEndEffector; return _pEndEffector;
} }
/// \brief Return transform with respect to end effector defining t he grasp coordinate system /// \brief Return transform with respect to end effector defining t he grasp coordinate system
virtual Transform GetGraspTransform() const { virtual Transform GetLocalToolTransform() const {
return _tGrasp; return _tLocalTool;
}
/// \brief Sets the local tool transform with respect to the end ef
fector.
///
/// Because this call will change manipulator hash, it resets the l
oaded IK and sets the Prop_RobotManipulatorTool message.
virtual void SetLocalToolTransform(const Transform& t);
/// \brief new name for manipulator
///
/// \throw openrave_exception if name is already used in another ma
nipulator
virtual void SetName(const std::string& name);
/// \deprecated (11/10/15) use GetLocalToolTransform
virtual Transform GetGraspTransform() const RAVE_DEPRECATED {
return GetLocalToolTransform();
} }
/// \brief Gripper indices of the joints that the manipulator cont rols. /// \brief Gripper indices of the joints that the manipulator cont rols.
virtual const std::vector<int>& GetGripperIndices() const { virtual const std::vector<int>& GetGripperIndices() const {
return __vgripperdofindices; return __vgripperdofindices;
} }
/// \brief Return the indices of the DOFs of the arm (used for IK, etc). /// \brief Return the indices of the DOFs of the arm (used for IK, etc).
/// ///
/// Usually the DOF indices from pBase to pEndEffector /// Usually the DOF indices from pBase to pEndEffector
virtual const std::vector<int>& GetArmIndices() const { virtual const std::vector<int>& GetArmIndices() const {
return __varmdofindices; return __varmdofindices;
} }
/// \brief return the normal direction to move joints to 'close' th e hand /// \brief return the normal direction to move joints to 'close' th e hand
virtual const std::vector<dReal>& GetClosingDirection() const { virtual const std::vector<dReal>& GetClosingDirection() const {
return _vClosingDirection; return _vClosingDirection;
} }
/// \brief direction of palm/head/manipulator used for approaching. defined inside the manipulator/grasp coordinate system /// \brief direction of palm/head/manipulator used for approaching. defined inside the manipulator/grasp coordinate system
virtual Vector GetDirection() const { virtual Vector GetLocalToolDirection() const {
return _vdirection; return _vdirection;
} }
/// \deprecated (11/10/15) use GetLocalToolDirection
virtual Vector GetDirection() const {
return GetLocalToolDirection();
}
/// \brief Find a close solution to the current robot's joint value s. /// \brief Find a close solution to the current robot's joint value s.
/// ///
/// The function is a wrapper around the IkSolver interface. /// The function is a wrapper around the IkSolver interface.
/// Note that the solution returned is not guaranteed to be the clo sest solution. In order to compute that, will have to /// Note that the solution returned is not guaranteed to be the clo sest solution. In order to compute that, will have to
/// compute all the ik solutions using FindIKSolutions. /// compute all the ik solutions using FindIKSolutions.
/// \param param The transformation of the end-effector in the glob al coord system /// \param param The transformation of the end-effector in the glob al coord system
/// \param solution Will be of size GetArmIndices().size() and cont ain the best solution /// \param solution Will be of size GetArmIndices().size() and cont ain the best solution
/// \param[in] filteroptions A bitmask of \ref IkFilterOptions valu es controlling what is checked for each ik solution. /// \param[in] filteroptions A bitmask of \ref IkFilterOptions valu es controlling what is checked for each ik solution.
virtual bool FindIKSolution(const IkParameterization& param, std::v ector<dReal>& solution, int filteroptions) const; virtual bool FindIKSolution(const IkParameterization& param, std::v ector<dReal>& solution, int filteroptions) const;
virtual bool FindIKSolution(const IkParameterization& param, const std::vector<dReal>& vFreeParameters, std::vector<dReal>& solution, int filt eroptions) const; virtual bool FindIKSolution(const IkParameterization& param, const std::vector<dReal>& vFreeParameters, std::vector<dReal>& solution, int filt eroptions) const;
skipping to change at line 148 skipping to change at line 167
... move robot ... move robot
std::vector<dReal> sol; std::vector<dReal> sol;
if( FindIKSolution(ikparam,sol, filteroptions) ) { if( FindIKSolution(ikparam,sol, filteroptions) ) {
manip->GetRobot()->SetActiveDOFs(manip->GetArmIndices()); manip->GetRobot()->SetActiveDOFs(manip->GetArmIndices());
manip->GetRobot()->SetActiveDOFValues(sol); manip->GetRobot()->SetActiveDOFValues(sol);
BOOST_ASSERT( dist(manip->GetIkParameterization(iktype), ik param) <= epsilon ); BOOST_ASSERT( dist(manip->GetIkParameterization(iktype), ik param) <= epsilon );
} }
\endcode \endcode
\param iktype the type of parameterization to request \param iktype the type of parameterization to request
*/ */
virtual IkParameterization GetIkParameterization(IkParameterization ::Type iktype) const; virtual IkParameterization GetIkParameterization(IkParameterization Type iktype) const;
/** \brief returns a full parameterization of a given IK type for t he current manipulator position using an existing IkParameterization as the seed. /** \brief returns a full parameterization of a given IK type for t he current manipulator position using an existing IkParameterization as the seed.
Ideally pluging the returned ik parameterization into FindIkSol ution should return the a manipulator configuration Ideally pluging the returned ik parameterization into FindIkSol ution should return the a manipulator configuration
such that a new call to GetIkParameterization returns the same values. such that a new call to GetIkParameterization returns the same values.
\param ikparam Some IK types like Lookat3D and TranslationLocal Global6D set constraints in the global coordinate system of the manipulator . Because these values are not stored in manipulator itself, they have to b e passed in through an existing IkParameterization. \param ikparam Some IK types like Lookat3D and TranslationLocal Global6D set constraints in the global coordinate system of the manipulator . Because these values are not stored in manipulator itself, they have to b e passed in through an existing IkParameterization.
*/ */
virtual IkParameterization GetIkParameterization(const IkParameteri zation& ikparam) const; virtual IkParameterization GetIkParameterization(const IkParameteri zation& ikparam) const;
/// \brief Get all child joints of the manipulator starting at the pEndEffector link /// \brief Get all child joints of the manipulator starting at the pEndEffector link
skipping to change at line 174 skipping to change at line 193
/// \brief returns true if a link is part of the child links of the manipulator. /// \brief returns true if a link is part of the child links of the manipulator.
/// ///
/// The child links do not include the arm links. /// The child links do not include the arm links.
virtual bool IsChildLink(LinkConstPtr plink) const; virtual bool IsChildLink(LinkConstPtr plink) const;
/// \brief Get all child links of the manipulator starting at pEndE ffector link. /// \brief Get all child links of the manipulator starting at pEndE ffector link.
/// ///
/// The child links do not include the arm links. /// The child links do not include the arm links.
virtual void GetChildLinks(std::vector<LinkPtr>& vlinks) const; virtual void GetChildLinks(std::vector<LinkPtr>& vlinks) const;
/// \brief Get all links that are independent of the arm and grippe /** \brief Get all links that are independent of the arm and grippe
r joints r joints
///
/// conditioned that the base and end effector links are static. In other words, returns all links not on the path from the base
/// In other words, returns all links not on the path from the base to the end effector and not children of the end effector. The base and all
to the end effector and not children of the end effector. links rigidly attached to it are also returned.
*/
virtual void GetIndependentLinks(std::vector<LinkPtr>& vlinks) cons t; virtual void GetIndependentLinks(std::vector<LinkPtr>& vlinks) cons t;
/** \brief Checks collision with only the gripper given its end-eff ector transform. Ignores disabled links. /** \brief Checks collision with only the gripper given its end-eff ector transform. Ignores disabled links.
\param tEE the end effector transform \param tEE the end effector transform
\param[out] report [optional] collision report \param[out] report [optional] collision report
\return true if a collision occurred \return true if a collision occurred
*/ */
virtual bool CheckEndEffectorCollision(const Transform& tEE, Collis ionReportPtr report = CollisionReportPtr()) const; virtual bool CheckEndEffectorCollision(const Transform& tEE, Collis ionReportPtr report = CollisionReportPtr()) const;
/** \brief Checks collision with the environment with all the indep endent links of the robot. Ignores disabled links. /** \brief Checks collision with the environment with all the indep endent links of the robot. Ignores disabled links.
\param[out] report [optional] collision report \param[out] report [optional] collision report
\return true if a collision occurred \return true if a collision occurred
*/ */
virtual bool CheckIndependentCollision(CollisionReportPtr report = CollisionReportPtr()) const; virtual bool CheckIndependentCollision(CollisionReportPtr report = CollisionReportPtr()) const;
/** \brief Checks collision with a target body and all the independ
ent links of the robot. Ignores disabled links.
\param[in] the body to check the independent links with
\param[out] report [optional] collision report
\return true if a collision occurred
*/
//virtual bool CheckIndependentCollision(KinBodyConstPtr body, Coll
isionReportPtr report = CollisionReportPtr()) const;
/// \brief return true if the body is being grabbed by any link on this manipulator /// \brief return true if the body is being grabbed by any link on this manipulator
virtual bool IsGrabbing(KinBodyConstPtr body) const; virtual bool IsGrabbing(KinBodyConstPtr body) const;
/// \brief computes the jacobian of the manipulator arm indices fro m the current manipulator frame origin. /// \brief computes the jacobian of the manipulator arm indices fro m the current manipulator frame origin.
virtual void CalculateJacobian(boost::multi_array<dReal,2>& mjacobi an) const; virtual void CalculateJacobian(boost::multi_array<dReal,2>& mjacobi an) const;
/// \brief computes the quaternion jacobian of the manipulator arm indices from the current manipulator frame rotation. /// \brief computes the quaternion jacobian of the manipulator arm indices from the current manipulator frame rotation.
virtual void CalculateRotationJacobian(boost::multi_array<dReal,2>& mjacobian) const; virtual void CalculateRotationJacobian(boost::multi_array<dReal,2>& mjacobian) const;
/// \brief computes the angule axis jacobian of the manipulator arm indices. /// \brief computes the angule axis jacobian of the manipulator arm indices.
skipping to change at line 219 skipping to change at line 246
/// \brief Return hash of just the manipulator definition. /// \brief Return hash of just the manipulator definition.
virtual const std::string& GetStructureHash() const; virtual const std::string& GetStructureHash() const;
/// \brief Return hash of all kinematics information that involves solving the inverse kinematics equations. /// \brief Return hash of all kinematics information that involves solving the inverse kinematics equations.
/// ///
/// This includes joint axes, joint positions, and final grasp tran sform. Hash is used to cache the solvers. /// This includes joint axes, joint positions, and final grasp tran sform. Hash is used to cache the solvers.
virtual const std::string& GetKinematicsStructureHash() const; virtual const std::string& GetKinematicsStructureHash() const;
protected: protected:
std::string _name; std::string _name;
LinkPtr _pBase, _pEndEffector; LinkPtr _pBase, _pEndEffector;
Transform _tGrasp; Transform _tLocalTool;
std::vector<dReal> _vClosingDirection; std::vector<dReal> _vClosingDirection;
Vector _vdirection; Vector _vdirection;
IkSolverBasePtr _pIkSolver; IkSolverBasePtr _pIkSolver;
std::string _strIkSolver; std::string _strIkSolver;
std::vector<std::string> _vgripperjointnames; ///< names of the gripper joints std::vector<std::string> _vgripperjointnames; ///< names of the gripper joints
private: private:
RobotBaseWeakPtr _probot; RobotBaseWeakPtr _probot;
std::vector<int> __vgripperdofindices, __varmdofindices; std::vector<int> __vgripperdofindices, __varmdofindices;
mutable std::string __hashstructure, __hashkinematicsstructure; mutable std::string __hashstructure, __hashkinematicsstructure;
#ifdef RAVE_PRIVATE #ifdef RAVE_PRIVATE
#ifdef _MSC_VER #ifdef _MSC_VER
friend class ColladaReader; friend class ColladaReader;
friend class OpenRAVEXMLParser::ManipulatorXMLReader; friend class OpenRAVEXMLParser::ManipulatorXMLReader;
friend class OpenRAVEXMLParser::RobotXMLReader; friend class OpenRAVEXMLParser::RobotXMLReader;
friend class XFileReader;
#else #else
friend class ::ColladaReader; friend class ::ColladaReader;
friend class ::OpenRAVEXMLParser::ManipulatorXMLReader; friend class ::OpenRAVEXMLParser::ManipulatorXMLReader;
friend class ::OpenRAVEXMLParser::RobotXMLReader; friend class ::OpenRAVEXMLParser::RobotXMLReader;
friend class ::XFileReader;
#endif #endif
#endif #endif
friend class RobotBase; friend class RobotBase;
}; };
typedef boost::shared_ptr<RobotBase::Manipulator> ManipulatorPtr; typedef boost::shared_ptr<RobotBase::Manipulator> ManipulatorPtr;
typedef boost::shared_ptr<RobotBase::Manipulator const> ManipulatorCons tPtr; typedef boost::shared_ptr<RobotBase::Manipulator const> ManipulatorCons tPtr;
typedef boost::weak_ptr<RobotBase::Manipulator> ManipulatorWeakPtr; typedef boost::weak_ptr<RobotBase::Manipulator> ManipulatorWeakPtr;
/// \brief Attaches a sensor to a link on the robot. /// \brief Attaches a sensor to a link on the robot.
class OPENRAVE_API AttachedSensor : public boost::enable_shared_from_th is<AttachedSensor> class OPENRAVE_API AttachedSensor : public boost::enable_shared_from_th is<AttachedSensor>
skipping to change at line 297 skipping to change at line 326
LinkWeakPtr pattachedlink; ///< the robot link that the sen sor is attached to LinkWeakPtr pattachedlink; ///< the robot link that the sen sor is attached to
Transform trelative; ///< relative transform of the sensor with respect to the attached link Transform trelative; ///< relative transform of the sensor with respect to the attached link
SensorBase::SensorDataPtr pdata; ///< pointer to a prealloc ated data using psensor->CreateSensorData() SensorBase::SensorDataPtr pdata; ///< pointer to a prealloc ated data using psensor->CreateSensorData()
std::string _name; ///< name of the attached sensor std::string _name; ///< name of the attached sensor
mutable std::string __hashstructure; mutable std::string __hashstructure;
#ifdef RAVE_PRIVATE #ifdef RAVE_PRIVATE
#ifdef _MSC_VER #ifdef _MSC_VER
friend class ColladaReader; friend class ColladaReader;
friend class OpenRAVEXMLParser::AttachedSensorXMLReader; friend class OpenRAVEXMLParser::AttachedSensorXMLReader;
friend class OpenRAVEXMLParser::RobotXMLReader; friend class OpenRAVEXMLParser::RobotXMLReader;
friend class XFileReader;
#else #else
friend class ::ColladaReader; friend class ::ColladaReader;
friend class ::OpenRAVEXMLParser::AttachedSensorXMLReader; friend class ::OpenRAVEXMLParser::AttachedSensorXMLReader;
friend class ::OpenRAVEXMLParser::RobotXMLReader; friend class ::OpenRAVEXMLParser::RobotXMLReader;
friend class ::XFileReader;
#endif #endif
#endif #endif
friend class RobotBase; friend class RobotBase;
}; };
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
{ {
skipping to change at line 324 skipping to change at line 355
std::vector<LinkConstPtr> vCollidingLinks, vNonCollidingLinks; ///< robot links that already collide with the body std::vector<LinkConstPtr> vCollidingLinks, vNonCollidingLinks; ///< robot links that already collide with the body
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();
protected: protected:
RobotBasePtr _probot; RobotBasePtr _probot;
std::vector<int> vactivedofs; std::vector<int> vactivedofs;
int affinedofs; int affinedofs;
Vector rotationaxis; Vector rotationaxis;
int nActiveManip; int nActiveManip;
std::vector<Grabbed> _vGrabbedBodies; std::vector<Grabbed> _vGrabbedBodies;
private:
virtual void _RestoreRobot();
}; };
typedef boost::shared_ptr<RobotStateSaver> RobotStateSaverPtr;
virtual ~RobotBase(); virtual ~RobotBase();
/// \brief Return the static interface type this class points to (used for safe casting). /// \brief Return the static interface type this class points to (used for safe casting).
static inline InterfaceType GetInterfaceTypeStatic() { static inline InterfaceType GetInterfaceTypeStatic() {
return PT_Robot; return PT_Robot;
} }
virtual void Destroy(); virtual void Destroy();
/// \deprecated (11/02/18) \see EnvironmentBase::ReadRobotXMLFile /// \deprecated (11/02/18) \see EnvironmentBase::ReadRobotXMLFile
virtual bool InitFromFile(const std::string& filename, const Attributes List& atts = AttributesList()) RAVE_DEPRECATED; virtual bool InitFromFile(const std::string& filename, const Attributes List& atts = AttributesList()) RAVE_DEPRECATED;
/// \deprecated (11/02/18) \see EnvironmentBase::ReadRobotXMLData /// \deprecated (11/02/18) \see EnvironmentBase::ReadRobotXMLData
virtual bool InitFromData(const std::string& data, const AttributesList & atts = AttributesList()) RAVE_DEPRECATED; virtual bool InitFromData(const std::string& data, const AttributesList & atts = AttributesList()) RAVE_DEPRECATED;
/// \brief Returns the manipulators of the robot /// \brief Returns the manipulators of the robot
virtual std::vector<ManipulatorPtr>& GetManipulators() { virtual std::vector<ManipulatorPtr>& GetManipulators() {
return _vecManipulators; return _vecManipulators;
} }
virtual bool SetMotion(TrajectoryBaseConstPtr ptraj) {
return false;
}
virtual std::vector<AttachedSensorPtr>& GetAttachedSensors() { virtual std::vector<AttachedSensorPtr>& GetAttachedSensors() {
return _vecSensors; return _vecSensors;
} }
virtual void SetDOFValues(const std::vector<dReal>& vJointValues, bool bCheckLimits = false); virtual void SetDOFValues(const std::vector<dReal>& vJointValues, bool bCheckLimits = false);
virtual void SetDOFValues(const std::vector<dReal>& vJointValues, const Transform& transbase, bool bCheckLimits = false); virtual void SetDOFValues(const std::vector<dReal>& vJointValues, const Transform& transbase, bool bCheckLimits = false);
virtual void SetLinkTransformations(const std::vector<Transform>& vbodi virtual void SetLinkTransformations(const std::vector<Transform>& trans
es); forms);
virtual void SetLinkTransformations(const std::vector<Transform>& trans
forms, const std::vector<int>& dofbranches);
/// Transforms the robot and updates the attached sensors and grabbed b odies. /// Transforms the robot and updates the attached sensors and grabbed b odies.
virtual void SetTransform(const Transform& trans); virtual void SetTransform(const Transform& trans);
/** Methods using the active degrees of freedoms of the robot. Active D OFs are a way for the /** Methods using the active degrees of freedoms of the robot. Active D OFs are a way for the
user to specify degrees of freedom of interest for a current execut ion block. All planners user to specify degrees of freedom of interest for a current execut ion block. All planners
by default use the robot's active DOF and active manipultor. For ev ery Get* method, there is by default use the robot's active DOF and active manipultor. For ev ery Get* method, there is
a corresponding GetActive* method rather than the methods when sett ing joints. The active a corresponding GetActive* method rather than the methods when sett ing joints. The active
DOFs also include affine transfomrations of the robot's base. Affin e transformation DOFs can DOFs also include affine transfomrations of the robot's base. Affin e transformation DOFs can
be found after the joint DOFs in this order: X, Y, Z, Rotation wher e rotation can be around be found after the joint DOFs in this order: X, Y, Z, Rotation wher e rotation can be around
a specified axis a full 3D rotation. Usually the affine transforam tion is with respect to a specified axis a full 3D rotation. Usually the affine transforam tion is with respect to
the first link in the body the first link in the body
@name Affine DOFs @name Affine DOFs
@{ @{
*/ */
/// \brief Selects which DOFs of the affine transformation to include i
n the active configuration.
enum DOFAffine
{
DOF_NoTransform = 0,
DOF_X = 1, ///< robot can move in the x direction
DOF_Y = 2, ///< robot can move in the y direction
DOF_Z = 4, ///< robot can move in the z direction
// DOF_RotationX fields are mutually exclusive /// \deprecated (11/10/04), use OpenRAVE:: global variables
DOF_RotationAxis = 8, ///< robot can rotate around an axis (1 d static const DOFAffine DOF_NoTransform RAVE_DEPRECATED = OpenRAVE::DOF_
of) NoTransform;
DOF_Rotation3D = 16, ///< robot can rotate freely (3 dof), the static const DOFAffine DOF_X RAVE_DEPRECATED = OpenRAVE::DOF_X;
parameterization is static const DOFAffine DOF_Y RAVE_DEPRECATED = OpenRAVE::DOF_Y;
///< theta * v, where v is the rotation ax static const DOFAffine DOF_Z RAVE_DEPRECATED = OpenRAVE::DOF_Z;
is and theta is the angle about that axis static const DOFAffine DOF_RotationAxis RAVE_DEPRECATED = OpenRAVE::DOF
DOF_RotationQuat = 32, ///< robot can rotate freely (4 dof), pa _RotationAxis;
rameterization is a quaternion. In order for limits to work correctly, the static const DOFAffine DOF_Rotation3D RAVE_DEPRECATED = OpenRAVE::DOF_R
quaternion is in the space of _vRotationQuatLimitStart. _vRotationQuatLimit otation3D;
Start is always left-multiplied before setting the transform! static const DOFAffine DOF_RotationQuat RAVE_DEPRECATED = OpenRAVE::DOF
}; _RotationQuat;
/** \brief Set the joint indices and affine transformation dofs that th e planner should use. If \ref DOF_RotationAxis is specified, the previously set axis is used. /** \brief Set the joint indices and affine transformation dofs that th e planner should use. If \ref DOF_RotationAxis is specified, the previously set axis is used.
\param dofindices the indices of the original degrees of freedom to use. \param dofindices the indices of the original degrees of freedom to use.
\param affine A bitmask of \ref DOFAffine values \param affine A bitmask of \ref DOFAffine values
*/ */
virtual void SetActiveDOFs(const std::vector<int>& dofindices, int affi ne = DOF_NoTransform); virtual void SetActiveDOFs(const std::vector<int>& dofindices, int affi ne = OpenRAVE::DOF_NoTransform);
/** \brief Set the joint indices and affine transformation dofs that th e planner should use. If \ref DOF_RotationAxis is specified, then rotationa xis is set as the new axis. /** \brief Set the joint indices and affine transformation dofs that th e planner should use. If \ref DOF_RotationAxis is specified, then rotationa xis is set as the new axis.
\param dofindices the indices of the original degrees of freedom to use. \param dofindices the indices of the original degrees of freedom to use.
\param affine A bitmask of \ref DOFAffine values \param affine A bitmask of \ref DOFAffine values
\param rotationaxis if \ref DOF_RotationAxis is specified, pRotatio nAxis is used as the new axis \param rotationaxis if \ref DOF_RotationAxis is specified, pRotatio nAxis is used as the new axis
*/ */
virtual void SetActiveDOFs(const std::vector<int>& dofindices, int affi ne, const Vector& rotationaxis); virtual void SetActiveDOFs(const std::vector<int>& dofindices, int affi ne, const Vector& rotationaxis);
virtual int GetActiveDOF() const { virtual int GetActiveDOF() const {
return _nActiveDOF >= 0 ? _nActiveDOF : GetDOF(); return _nActiveDOF >= 0 ? _nActiveDOF : GetDOF();
} }
virtual int GetAffineDOF() const { virtual int GetAffineDOF() const {
return _nAffineDOFs; return _nAffineDOFs;
} }
/// \brief If dof is set in the affine dofs, returns its index in the d /// \deprecated (11/10/07)
of values array, otherwise returns -1 virtual int GetAffineDOFIndex(DOFAffine dof) const {
virtual int GetAffineDOFIndex(DOFAffine dof) const; return GetActiveDOFIndices().size()+RaveGetIndexFromAffineDOF(GetAf
fineDOF(),dof);
}
/// \brief return the configuration specification of the active dofs
virtual const ConfigurationSpecification& GetActiveConfigurationSpecifi
cation() const;
/// \brief Return the set of active dof indices of the joints. /// \brief Return the set of active dof indices of the joints.
virtual const std::vector<int>& GetActiveDOFIndices() const; virtual const std::vector<int>& GetActiveDOFIndices() const;
virtual Vector GetAffineRotationAxis() const { virtual Vector GetAffineRotationAxis() const {
return vActvAffineRotationAxis; return vActvAffineRotationAxis;
} }
virtual void SetAffineTranslationLimits(const Vector& lower, const Vect or& upper); virtual void SetAffineTranslationLimits(const Vector& lower, const Vect or& upper);
virtual void SetAffineRotationAxisLimits(const Vector& lower, const Vec tor& upper); virtual void SetAffineRotationAxisLimits(const Vector& lower, const Vec tor& upper);
virtual void SetAffineRotation3DLimits(const Vector& lower, const Vecto r& upper); virtual void SetAffineRotation3DLimits(const Vector& lower, const Vecto r& upper);
skipping to change at line 499 skipping to change at line 533
return _fQuatAngleResolution; return _fQuatAngleResolution;
} }
virtual void SetActiveDOFValues(const std::vector<dReal>& values, bool bCheckLimits=false); virtual void SetActiveDOFValues(const std::vector<dReal>& values, bool bCheckLimits=false);
virtual void GetActiveDOFValues(std::vector<dReal>& v) const; virtual void GetActiveDOFValues(std::vector<dReal>& v) const;
virtual void SetActiveDOFVelocities(const std::vector<dReal>& velocitie s, bool bCheckLimits=false); virtual void SetActiveDOFVelocities(const std::vector<dReal>& velocitie s, bool bCheckLimits=false);
virtual void GetActiveDOFVelocities(std::vector<dReal>& velocities) con st; virtual void GetActiveDOFVelocities(std::vector<dReal>& velocities) con st;
virtual void GetActiveDOFLimits(std::vector<dReal>& lower, std::vector< dReal>& upper) const; virtual void GetActiveDOFLimits(std::vector<dReal>& lower, std::vector< dReal>& upper) const;
virtual void GetActiveDOFResolutions(std::vector<dReal>& v) const; virtual void GetActiveDOFResolutions(std::vector<dReal>& v) const;
virtual void GetActiveDOFWeights(std::vector<dReal>& v) const; virtual void GetActiveDOFWeights(std::vector<dReal>& v) const;
virtual void GetActiveDOFMaxVel(std::vector<dReal>& v) const; virtual void GetActiveDOFVelocityLimits(std::vector<dReal>& v) const;
virtual void GetActiveDOFMaxAccel(std::vector<dReal>& v) const; virtual void GetActiveDOFAccelerationLimits(std::vector<dReal>& v) cons
t;
virtual void GetActiveDOFMaxVel(std::vector<dReal>& v) const {
return GetActiveDOFVelocityLimits(v);
}
virtual void GetActiveDOFMaxAccel(std::vector<dReal>& v) const {
return GetActiveDOFAccelerationLimits(v);
}
/// computes the configuration difference q1-q2 and stores it in q1. Ta kes into account joint limits and circular joints /// computes the configuration difference q1-q2 and stores it in q1. Ta kes into account joint limits and circular joints
virtual void SubtractActiveDOFValues(std::vector<dReal>& q1, const std: :vector<dReal>& q2) const; virtual void SubtractActiveDOFValues(std::vector<dReal>& q1, const std: :vector<dReal>& q2) const;
/// sets the active manipulator of the robot /// sets the active manipulator of the robot
/// \param index manipulator index /// \param index manipulator index
virtual void SetActiveManipulator(int index); virtual void SetActiveManipulator(int index);
/// sets the active manipulator of the robot /// sets the active manipulator of the robot
/// \param manipname manipulator name /// \param manipname manipulator name
virtual void SetActiveManipulator(const std::string& manipname); virtual void SetActiveManipulator(const std::string& manipname);
virtual ManipulatorPtr GetActiveManipulator(); virtual ManipulatorPtr GetActiveManipulator();
virtual ManipulatorConstPtr GetActiveManipulator() const; virtual ManipulatorConstPtr GetActiveManipulator() const;
/// \return index of the current active manipulator /// \return index of the current active manipulator
virtual int GetActiveManipulatorIndex() const { virtual int GetActiveManipulatorIndex() const {
return _nActiveManip; return _nActiveManip;
} }
/// Converts a trajectory specified with the current active degrees of /// \deprecated (11/10/04) send directly through controller
freedom to a full joint/transform trajectory virtual bool SetMotion(TrajectoryBaseConstPtr ptraj) RAVE_DEPRECATED;
/// \param pFullTraj written with the final trajectory,
/// \param pActiveTraj the input active dof trajectory
/// \param bOverwriteTransforms if true will use the current robot tran
sform as the base (ie will ignore any transforms specified in pActiveTraj).
If false, will use the pActiveTraj transforms specified
virtual void GetFullTrajectoryFromActive(TrajectoryBasePtr pFullTraj, T
rajectoryBaseConstPtr pActiveTraj, bool bOverwriteTransforms = true);
virtual bool SetActiveMotion(TrajectoryBaseConstPtr ptraj) { /// \deprecated (11/10/04)
return false; virtual bool SetActiveMotion(TrajectoryBaseConstPtr ptraj) RAVE_DEPRECA
} TED;
/// the speed at which the robot should go at /// \deprecated (11/10/04)
virtual bool SetActiveMotion(TrajectoryBaseConstPtr ptraj, dReal fSpeed virtual bool SetActiveMotion(TrajectoryBaseConstPtr ptraj, dReal fSpeed
) { ) RAVE_DEPRECATED;
return SetActiveMotion(ptraj);
}
/** \brief Calculates the translation jacobian with respect to a link. /** \brief Calculates the translation jacobian with respect to a link.
Calculates the partial differentials for the active degrees of free dom that in the path from the root node to _veclinks[index] Calculates the partial differentials for the active degrees of free dom that in the path from the root node to _veclinks[index]
(doesn't touch the rest of the values). (doesn't touch the rest of the values).
\param mjacobian a 3 x ActiveDOF matrix \param mjacobian a 3 x ActiveDOF matrix
*/ */
virtual void CalculateActiveJacobian(int index, const Vector& offset, b oost::multi_array<dReal,2>& mjacobian) const; virtual void CalculateActiveJacobian(int index, const Vector& offset, b oost::multi_array<dReal,2>& mjacobian) const;
virtual void CalculateActiveJacobian(int index, const Vector& offset, s td::vector<dReal>& pfJacobian) const; virtual void CalculateActiveJacobian(int index, const Vector& offset, s td::vector<dReal>& pfJacobian) const;
skipping to change at line 681 skipping to change at line 715
/// \deprecated (10/11/16) /// \deprecated (10/11/16)
virtual bool SetController(ControllerBasePtr controller, const std::str ing& args) RAVE_DEPRECATED { virtual bool SetController(ControllerBasePtr controller, const std::str ing& args) RAVE_DEPRECATED {
std::vector<int> dofindices; std::vector<int> dofindices;
for(int i = 0; i < GetDOF(); ++i) { for(int i = 0; i < GetDOF(); ++i) {
dofindices.push_back(i); dofindices.push_back(i);
} }
return SetController(controller,dofindices,1); return SetController(controller,dofindices,1);
} }
/// \deprecated (11/10/04)
void GetFullTrajectoryFromActive(TrajectoryBasePtr pfulltraj, Trajector
yBaseConstPtr pActiveTraj, bool bOverwriteTransforms=true) RAVE_DEPRECATED;
protected: protected:
RobotBase(EnvironmentBasePtr penv); RobotBase(EnvironmentBasePtr penv);
inline RobotBasePtr shared_robot() { inline RobotBasePtr shared_robot() {
return boost::static_pointer_cast<RobotBase>(shared_from_this()); return boost::static_pointer_cast<RobotBase>(shared_from_this());
} }
inline RobotBaseConstPtr shared_robot_const() const { inline RobotBaseConstPtr shared_robot_const() const {
return boost::static_pointer_cast<RobotBase const>(shared_from_this ()); return boost::static_pointer_cast<RobotBase const>(shared_from_this ());
} }
skipping to change at line 719 skipping to change at line 756
int _nActiveDOF; ///< Active degrees of freedom; if -1, use robot dofs int _nActiveDOF; ///< Active degrees of freedom; if -1, use robot dofs
int _nAffineDOFs; ///< dofs describe what affine transformations are al lowed int _nAffineDOFs; ///< dofs describe what affine transformations are al lowed
Vector _vTranslationLowerLimits, _vTranslationUpperLimits, _vTranslatio nMaxVels, _vTranslationResolutions, _vTranslationWeights; Vector _vTranslationLowerLimits, _vTranslationUpperLimits, _vTranslatio nMaxVels, _vTranslationResolutions, _vTranslationWeights;
/// the xyz components are used if the rotation axis is solely about X, Y,or Z; otherwise the W component is used. /// the xyz components are used if the rotation axis is solely about X, Y,or Z; otherwise the W component is used.
Vector _vRotationAxisLowerLimits, _vRotationAxisUpperLimits, _vRotation AxisMaxVels, _vRotationAxisResolutions, _vRotationAxisWeights; Vector _vRotationAxisLowerLimits, _vRotationAxisUpperLimits, _vRotation AxisMaxVels, _vRotationAxisResolutions, _vRotationAxisWeights;
Vector _vRotation3DLowerLimits, _vRotation3DUpperLimits, _vRotation3DMa xVels, _vRotation3DResolutions, _vRotation3DWeights; Vector _vRotation3DLowerLimits, _vRotation3DUpperLimits, _vRotation3DMa xVels, _vRotation3DResolutions, _vRotation3DWeights;
Vector _vRotationQuatLimitStart; Vector _vRotationQuatLimitStart;
dReal _fQuatLimitMaxAngle, _fQuatMaxAngleVelocity, _fQuatAngleResolutio n, _fQuatAngleWeight; dReal _fQuatLimitMaxAngle, _fQuatMaxAngleVelocity, _fQuatAngleResolutio n, _fQuatAngleWeight;
ConfigurationSpecification _activespec;
private: private:
virtual const char* GetHash() const { virtual const char* GetHash() const {
return OPENRAVE_ROBOT_HASH; return OPENRAVE_ROBOT_HASH;
} }
virtual const char* GetKinBodyHash() const { virtual const char* GetKinBodyHash() const {
return OPENRAVE_KINBODY_HASH; return OPENRAVE_KINBODY_HASH;
} }
mutable std::string __hashrobotstructure; mutable std::string __hashrobotstructure;
mutable std::vector<dReal> _vTempRobotJoints; mutable std::vector<dReal> _vTempRobotJoints;
#ifdef RAVE_PRIVATE #ifdef RAVE_PRIVATE
#ifdef _MSC_VER #ifdef _MSC_VER
friend class Environment; friend class Environment;
friend class ColladaReader; friend class ColladaReader;
friend class ColladaWriter; friend class ColladaWriter;
friend class OpenRAVEXMLParser::RobotXMLReader; friend class OpenRAVEXMLParser::RobotXMLReader;
friend class OpenRAVEXMLParser::ManipulatorXMLReader; friend class OpenRAVEXMLParser::ManipulatorXMLReader;
friend class OpenRAVEXMLParser::AttachedSensorXMLReader; friend class OpenRAVEXMLParser::AttachedSensorXMLReader;
friend class XFileReader;
#else #else
friend class ::Environment; friend class ::Environment;
friend class ::ColladaReader; friend class ::ColladaReader;
friend class ::ColladaWriter; friend class ::ColladaWriter;
friend class ::OpenRAVEXMLParser::RobotXMLReader; friend class ::OpenRAVEXMLParser::RobotXMLReader;
friend class ::OpenRAVEXMLParser::ManipulatorXMLReader; friend class ::OpenRAVEXMLParser::ManipulatorXMLReader;
friend class ::OpenRAVEXMLParser::AttachedSensorXMLReader; friend class ::OpenRAVEXMLParser::AttachedSensorXMLReader;
friend class ::XFileReader;
#endif #endif
#endif #endif
friend class RaveDatabase; friend class RaveDatabase;
}; };
} // end namespace OpenRAVE } // end namespace OpenRAVE
#endif // ROBOT_H #endif // ROBOT_H
 End of changes. 32 change blocks. 
65 lines changed or deleted 109 lines changed or added


 sensor.h   sensor.h 
skipping to change at line 25 skipping to change at line 25
// 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.
*/ */
#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. See \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
{ {
public: public:
enum SensorType enum SensorType
{ {
ST_Invalid=0, ST_Invalid=0,
ST_Laser=1, ST_Laser=1,
ST_Camera=2, ST_Camera=2,
skipping to change at line 71 skipping to change at line 71
typedef boost::shared_ptr<SensorBase::SensorData> SensorDataPtr; typedef boost::shared_ptr<SensorBase::SensorData> SensorDataPtr;
typedef boost::shared_ptr<SensorBase::SensorData const> SensorDataConst Ptr; typedef boost::shared_ptr<SensorBase::SensorData const> SensorDataConst Ptr;
class OPENRAVE_API LaserSensorData : public SensorData class OPENRAVE_API LaserSensorData : public SensorData
{ {
public: public:
virtual SensorType GetType() { virtual SensorType GetType() {
return ST_Laser; return ST_Laser;
} }
std::vector<RaveVector<dReal> > positions; ///< world coord /** \brief World coordinates of the origins of each of the photon (
inates of the origins of each of the laser points. laser) rays.
///< if positions is empty, assume the origin is t.trans for all po
ints Each of the photons start from some 3D position and go a particu
lar direction. For most common 2D lasers and the kinect, the starting point
from each of the photons is the same, it is also called the focal point.
If positions is empty, assume the origin is t.trans for all rays
.
*/
std::vector<RaveVector<dReal> > positions;
std::vector<RaveVector<dReal> > ranges; ///< Range and dire ction readings in the form of direction*distance. The direction is in world coordinates. The values should be returned in the order laser detected the m in. std::vector<RaveVector<dReal> > ranges; ///< Range and dire ction readings in the form of direction*distance. The direction is in world coordinates. The values should be returned in the order laser detected the m in.
std::vector<dReal> intensity; ///< Intensity readings. std::vector<dReal> intensity; ///< Intensity readings.
virtual bool serialize(std::ostream& O) const; virtual bool serialize(std::ostream& O) const;
}; };
class OPENRAVE_API CameraSensorData : public SensorData class OPENRAVE_API CameraSensorData : public SensorData
{ {
public: public:
virtual SensorType GetType() { virtual SensorType GetType() {
return ST_Camera; return ST_Camera;
skipping to change at line 346 skipping to change at line 350
virtual bool GetSensorData(SensorDataPtr psensordata) = 0; virtual bool GetSensorData(SensorDataPtr psensordata) = 0;
/// \brief returns true if sensor supports a particular sensor type /// \brief returns true if sensor supports a particular sensor type
virtual bool Supports(SensorType type) = 0; virtual bool Supports(SensorType type) = 0;
/// \brief Set the transform of a sensor (global coordinate system). /// \brief Set the transform of a sensor (global coordinate system).
/// ///
/// Sensors attached to the robot have their transforms automatically s et every time the robot is moved /// Sensors attached to the robot have their transforms automatically s et every time the robot is moved
/// \param trans - The transform defining the frame of the sensor. /// \param trans - The transform defining the frame of the sensor.
virtual void SetTransform(const Transform& trans) = 0; virtual void SetTransform(const Transform& trans) = 0;
/// \brief the position of the sensor in the world coordinate system
virtual Transform GetTransform() = 0; virtual Transform GetTransform() = 0;
/// \brief Register a callback whenever new sensor data comes in. /// \brief Register a callback whenever new sensor data comes in.
///
/// \param type the sensor type to register for /// \param type the sensor type to register for
/// \param callback the user function to call, note that this might blo ck the thread generating/receiving sensor data /// \param callback the user function to call, note that this might blo ck the thread generating/receiving sensor data
virtual boost::shared_ptr<void> RegisterDataCallback(SensorType type, c onst boost::function<void(SensorDataConstPtr)>& callback) OPENRAVE_DUMMY_IM PLEMENTATION; virtual UserDataPtr RegisterDataCallback(SensorType type, const boost:: function<void(SensorDataConstPtr)>& callback) OPENRAVE_DUMMY_IMPLEMENTATION ;
/// \return the name of the sensor /// \return the name of the sensor
virtual const std::string& GetName() const { virtual const std::string& GetName() const {
return _name; return _name;
} }
virtual void SetName(const std::string& newname) { virtual void SetName(const std::string& newname) {
_name = newname; _name = newname;
} }
/// \deprecated (11/03/28) /// \deprecated (11/03/28)
 End of changes. 5 change blocks. 
6 lines changed or deleted 15 lines changed or added


 sensorsystem.h   sensorsystem.h 
skipping to change at line 170 skipping to change at line 170
virtual ProcessElement startElement(const std::string& name, const AttributesList& atts); virtual ProcessElement startElement(const std::string& name, const AttributesList& atts);
virtual bool endElement(const std::string& name); virtual bool endElement(const std::string& name);
virtual void characters(const std::string& ch); virtual void characters(const std::string& ch);
protected: protected:
boost::shared_ptr<XMLData> _pdata; boost::shared_ptr<XMLData> _pdata;
std::stringstream ss; std::stringstream ss;
}; };
/// registers the XML reader, do not call in the constructor of this cl ass! /// registers the XML reader, do not call in the constructor of this cl ass!
static boost::shared_ptr<void> RegisterXMLReaderId(EnvironmentBasePtr p env, const std::string& xmlid); static UserDataPtr RegisterXMLReaderId(EnvironmentBasePtr penv, const s td::string& xmlid);
SimpleSensorSystem(const std::string& xmlid, EnvironmentBasePtr penv); SimpleSensorSystem(const std::string& xmlid, EnvironmentBasePtr penv);
virtual ~SimpleSensorSystem(); virtual ~SimpleSensorSystem();
virtual void Reset(); virtual void Reset();
virtual void AddRegisteredBodies(const std::vector<KinBodyPtr>& vbodies ); virtual void AddRegisteredBodies(const std::vector<KinBodyPtr>& vbodies );
virtual KinBody::ManageDataPtr AddKinBody(KinBodyPtr pbody, XMLReadable ConstPtr pdata); virtual KinBody::ManageDataPtr AddKinBody(KinBodyPtr pbody, XMLReadable ConstPtr pdata);
virtual bool RemoveKinBody(KinBodyPtr pbody); virtual bool RemoveKinBody(KinBodyPtr pbody);
 End of changes. 1 change blocks. 
1 lines changed or deleted 1 lines changed or added


 spacesampler.h   spacesampler.h 
skipping to change at line 38 skipping to change at line 38
IT_OpenStart=1, ///< (a,b] IT_OpenStart=1, ///< (a,b]
IT_OpenEnd=2, ///< [a,b) IT_OpenEnd=2, ///< [a,b)
IT_Closed=3, ///< [a,b] IT_Closed=3, ///< [a,b]
}; };
enum SampleDataType { enum SampleDataType {
SDT_Real=1, SDT_Real=1,
SDT_Uint32=2, SDT_Uint32=2,
}; };
/** \brief <b>[interface]</b> Contains space samplers commonly used in plan ners. <b>Methods not multi-thread safe.</b> See \ref arch_spacesampler. /** \brief <b>[interface]</b> Contains space samplers commonly used in plan ners. <b>If not specified, method is not multi-thread safe.</b> See \ref ar ch_spacesampler.
\ingroup interfaces \ingroup interfaces
*/ */
class OPENRAVE_API SpaceSamplerBase : public InterfaceBase class OPENRAVE_API SpaceSamplerBase : public InterfaceBase
{ {
public: public:
SpaceSamplerBase(EnvironmentBasePtr penv) : InterfaceBase(PT_SpaceSampl er, penv) { SpaceSamplerBase(EnvironmentBasePtr penv) : InterfaceBase(PT_SpaceSampl er, penv) {
} }
virtual ~SpaceSamplerBase() { virtual ~SpaceSamplerBase() {
} }
 End of changes. 1 change blocks. 
1 lines changed or deleted 1 lines changed or added


 trajectory.h   trajectory.h 
// -*- coding: utf-8 -*- // -*- coding: utf-8 -*-
// Copyright (C) 2006-2010 Rosen Diankov (rosen.diankov@gmail.com) // Copyright (C) 2006-2011 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 trajectory.h /** \file trajectory.h
\brief Define a time-parameterized trajectory of robot configurations. \brief Definition of \ref OpenRAVE::TrajectoryBase
*/ */
#ifndef TRAJECTORY_H #ifndef OPENRAVE_TRAJECTORY_H
#define TRAJECTORY_H #define OPENRAVE_TRAJECTORY_H
namespace OpenRAVE { namespace OpenRAVE {
/** \brief <b>[interface]</b> Encapsulate a time-parameterized trajectories of robot configurations. See \ref 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
{ {
public: public:
/// \brief trajectory interpolation and sampling methods TrajectoryBase(EnvironmentBasePtr penv);
enum InterpEnum { virtual ~TrajectoryBase() {
NONE=0, ///< unspecified timing info }
LINEAR=1, ///< linear interpolation
LINEAR_BLEND=2, ///< linear with quadratic blends
CUBIC=3, ///< cubic spline interpolation
QUINTIC=4, ///< quintic min-jerk interpolation
NUM_METHODS=5, ///< number of interpolation methods
};
/// \brief options for serializing trajectories /// \brief return the static interface type this class points to (used
enum TrajectoryOptions { for safe casting)
TO_OneLine = 1, ///< if set, will write everything without newl static inline InterfaceType GetInterfaceTypeStatic() {
ines, otherwise return PT_Trajectory;
///< will start a newline for the header and ev }
ery trajectory point
TO_NoHeader = 2, ///< do not write the header that specifies nu
mber of points, degrees of freedom, and other options
TO_IncludeTimestamps = 4,
TO_IncludeBaseTransformation = 8,
TO_IncludeVelocities = 0x10, ///< include velocities. If TO_Inc
ludeBaseTransformation is also set, include the base
///< base link velocity in terms o
f linear and angular velocity
TO_IncludeTorques = 0x20, ///< include torques
TO_InterpolationMask = 0x1c0, ///< bits to store the interpolat
ion information
};
/// Via point along the trajectory (joint configuration with a timestam virtual void Init(const ConfigurationSpecification& spec) = 0;
p)
class TPOINT
{
public:
TPOINT() : time(0), blend_radius(0) {
}
TPOINT(const std::vector<dReal>& newq, dReal newtime) : time(newtim
e), blend_radius(0) {
q = newq;
}
TPOINT(const std::vector<dReal>& newq, const Transform& newtrans, d
Real newtime) : time(newtime), blend_radius(0) {
q = newq; trans = newtrans;
}
enum TPcomponent { POS=0, //!< joint angle position /** \brief Sets/inserts new waypoints in the same configuration specifi
VEL, //!< joint angle velocity cation as the trajectory.
ACC, //!< joint angle acceleration
NUM_COMPONENTS };
friend std::ostream& operator<<(std::ostream& O, const TPOINT& tp); \param index The index where to start modifying the trajectory.
void Setq(std::vector<dReal>* values) \param data The data to insert, can represent multiple consecutive
{ waypoints. data.size()/GetConfigurationSpecification().GetDOF() waypoints a
assert(values->size() == q.size()); re added.
for(int i = 0; i < (int)values->size(); i++) \param bOverwrite If true, will overwrite the waypoints starting at
q[i] = values->at(i); index, and will insert new waypoints only if end of trajectory is reached.
// reset the blend_radius If false, will insert the points before index: a 0 index inserts the new d
blend_radius=0; ata in the beginning, a GetNumWaypoints() index inserts the new data at the
end.
*/
virtual void Insert(size_t index, const std::vector<dReal>& data, bool
bOverwrite=false) = 0;
} /** \brief Sets/inserts new waypoints in a \b user-given configuration specification.
Transform trans; ///< transform of the first lin \param index The index where to start modifying the trajectory.
k \param data The data to insert, can represent multiple consecutive
Vector linearvel; ///< instanteneous linear veloc waypoints. data.size()/GetConfigurationSpecification().GetDOF() waypoints a
ity re added.
Vector angularvel; ///< instanteneous angular velo \param spec the specification in which the input data come in. Depe
city nding on what data is offered, some values of this trajectory's specificati
std::vector<dReal> q; ///< joint configuration on might not be initialized.
std::vector<dReal> qdot; ///< instantaneous joint veloci \param bOverwrite If true, will overwrite the waypoints starting at
ties index, and will insert new waypoints only if end of trajectory is reached.
std::vector<dReal> qtorque; ///< feedforward torque [option If false, will insert the points before index: a 0 index inserts the new d
al] ata in the beginning, a GetNumWaypoints() index inserts the new data at the
dReal time; ///< time stamp of trajectory p end.
oint */
dReal blend_radius; virtual void Insert(size_t index, const std::vector<dReal>& data, const
}; ConfigurationSpecification& spec, bool bOverwrite=false) = 0;
class TSEGMENT /// \brief removes a number of waypoints starting at the specified inde
{ x
public: virtual void Remove(size_t startindex, size_t endindex) = 0;
//! the different segment types
enum Type { START=0, //!< starting trajectory segment
MIDDLE, //!< middle trajectory segment
END, //!< ending trajectory segment
NUM_TYPES };
void SetDimensions(int curve_degree, int num_dof) { /** \brief samples a data point on the trajectory at a particular time
coeff.resize((curve_degree+1)*num_dof); _curvedegrees = curve_d
egree;
}
inline dReal Get(int deg, int dof) const { \param data[out] the sampled point
return coeff[dof*(_curvedegrees+1)+deg]; \param time[in] the time to sample
} */
dReal& Get(int deg, int dof) { virtual void Sample(std::vector<dReal>& data, dReal time) const = 0;
return coeff[dof*(_curvedegrees+1)+deg];
}
friend std::ostream& operator<<(std::ostream& O, const TSEGMENT& tp ); /** \brief samples a data point on the trajectory at a particular time and returns data for the group specified.
Vector linearvel; ///< instanteneous linear veloc The default implementation is slow, so interface developers should
ity override it.
Vector angularvel; ///< instanteneous angular velo \param data[out] the sampled point
city \param time[in] the time to sample
\param spec[in] the specification format to return the data in
*/
virtual void Sample(std::vector<dReal>& data, dReal time, const Configu
rationSpecification& spec) const;
private: virtual const ConfigurationSpecification& GetConfigurationSpecification
dReal _fduration; () const = 0;
int _curvedegrees;
std::vector<dReal> coeff; ///< num_degrees x num_dof
coefficients of the segment
friend class TrajectoryBase; /// \brief return the number of waypoints
}; virtual size_t GetNumWaypoints() const = 0;
TrajectoryBase(EnvironmentBasePtr penv, int nDOF); /** \brief return a set of waypoints in the range [startindex,endindex)
virtual ~TrajectoryBase() {
}
/// return the static interface type this class points to (used for saf \param startindex[in] the start index of the waypoint (included)
e casting) \param endindex[in] the end index of the waypoint (not included)
static inline InterfaceType GetInterfaceTypeStatic() { \param data[out] the data of the waypoint
return PT_Trajectory; */
} virtual void GetWaypoints(size_t startindex, size_t endindex, std::vect
or<dReal>& data) const = 0;
/// clears all points and resets the dof of the trajectory /** \brief return a set of waypoints in the range [startindex,endindex)
virtual void Reset(int nDOF); in a different configuration specification.
/// getting information about the trajectory The default implementation is very slow, so trajectory developers s
inline dReal GetTotalDuration() const { hould really override it.
return _vecpoints.size() > 0 ? _vecpoints.back().time : 0; \param startindex[in] the start index of the waypoint (included)
} \param endindex[in] the end index of the waypoint (not included)
InterpEnum GetInterpMethod() const { \param spec[in] the specification to return the data in
return _interpMethod; \param data[out] the data of the waypoint
} */
const std::vector<TrajectoryBase::TPOINT>& GetPoints() const { virtual void GetWaypoints(size_t startindex, size_t endindex, std::vect
return _vecpoints; or<dReal>& data, const ConfigurationSpecification& spec) const;
}
std::vector<TrajectoryBase::TPOINT>& GetPoints() {
return _vecpoints;
}
const std::vector<TrajectoryBase::TSEGMENT>& GetSegments() const {
return _vecsegments;
}
virtual void Clear(); /** \brief returns one waypoint
/// add a point to the trajectory \param index[in] index of the waypoint. If < 0, then counting start
virtual void AddPoint(const TrajectoryBase::TPOINT& p) { s from the last waypoint. For example GetWaypoints(-1,data) returns the las
assert( _nDOF == (int)p.q.size()); _vecpoints.push_back(p); t waypoint.
\param data[out] the data of the waypoint
*/
inline void GetWaypoint(int index, std::vector<dReal>& data) const
{
int numpoints = GetNumWaypoints();
BOOST_ASSERT(index >= -numpoints && index < numpoints);
if( index < 0 ) {
index += numpoints;
}
GetWaypoints(index,index+1,data);
} }
/** \brief Preprocesses the trajectory for later sampling and set its i nterpolation method. /** \brief returns one waypoint
\param[in] robot [optional] robot to do the timing for \param index[in] index of the waypoint. If < 0, then counting start
\param[in] interpolationMethod method to use for interpolation s from the last waypoint. For example GetWaypoints(-1,data) returns the las
\param bAutoCalcTiming If true, will retime the trajectory using ma t waypoint.
ximum joint velocities and accelerations, otherwise it expects the time sta \param data[out] the data of the waypoint
mps of each point to be set.
\param[in] bActiveDOFs If true, then the trajectory is specified in
the robot's active degrees of freedom
and the maximum velocities and accelerations will be extracted appr
opriately. Affine transformations
are ignored in the retiming if true. If false, then use the
robot's full joint configuration and affine transformation for max
velocities.
\param[in] fMaxVelMult The percentage of the max velocity of each j
oint to use when retiming.
*/ */
virtual bool CalcTrajTiming(RobotBaseConstPtr robot, InterpEnum interpo inline void GetWaypoint(int index, std::vector<dReal>& data, const Conf
lationMethod, bool bAutoCalcTiming, bool bActiveDOFs, dReal fMaxVelMult=1); igurationSpecification& spec) const
{
/// \deprecated (11/06/14) see planningutils::ValidateTrajectory int numpoints = GetNumWaypoints();
virtual bool IsValid() const RAVE_DEPRECATED; BOOST_ASSERT(index >= -numpoints && index < numpoints);
if( index < 0 ) {
/// tests if a point violates any position, velocity or accel constrain index += numpoints;
ts }
//virtual bool IsValidPoint(const TPOINT& tp) const; GetWaypoints(index,index+1,data,spec);
/// \brief Sample the trajectory at the given time using the current in
terpolation method.
virtual bool SampleTrajectory(dReal time, TrajectoryBase::TPOINT &sampl
e) const;
/// Write to a stream, see TrajectoryOptions for file format
/// \param sinput stream to read the data from
/// \param options a combination of enums in TrajectoryOptions
virtual bool Write(std::ostream& sinput, int options) const;
/// Reads the trajectory, expects the filename to have a header.
/// \param sout stream to output the trajectory data
/// \param robot The robot to attach the trajrectory to, if specified,
will
/// call CalcTrajTiming to get the correct trajectory velo
cities.
virtual bool Read(std::istream& sout, RobotBasePtr robot);
virtual bool Read(const std::string& filename, RobotBasePtr robot) RAVE
_DEPRECATED;
virtual bool Write(const std::string& filename, int options) const RAVE
_DEPRECATED;
virtual int GetDOF() const {
return _nDOF;
} }
private:
/// \brief Linear interpolation using the maximum joint velocities for /// \brief return the duration of the trajectory in seconds
timing. virtual dReal GetDuration() const = 0;
virtual bool _SetLinear(bool bAutoCalcTiming, bool bActiveDOFs);
//// linear interpolation with parabolic blends /// \brief output the trajectory in XML format
//virtual bool _SetLinearBlend(bool bAutoCalcTiming); virtual void serialize(std::ostream& O, int options=0) const;
/// calculate the coefficients of a the parabolic and linear blends /// \brief initialize the trajectory
/// with continuous endpoint positions and velocities for via points. virtual InterfaceBasePtr deserialize(std::istream& I);
//virtual void _CalculateLinearBlendCoefficients(TSEGMENT::Type segType
,TSEGMENT& seg, TSEGMENT& prev,TPOINT& p0, TPOINT& p1,const dReal blendAcce
l[]);
/// cubic spline interpolation virtual void Clone(InterfaceBaseConstPtr preference, int cloningoptions
virtual bool _SetCubic(bool bAutoCalcTiming, bool bActiveDOFs); );
/// calculate the coefficients of a smooth cubic spline with // Old Trajectory API
/// continuous endpoint positions and velocities for via points.
virtual void _CalculateCubicCoefficients(TrajectoryBase::TSEGMENT&, con
st TrajectoryBase::TPOINT& tp0, const TrajectoryBase::TPOINT& tp1);
//bool _SetQuintic(bool bAutoCalcTiming, bool bActiveDOFs); /// \deprecated (11/10/04), please use newer API!
class Point
{
public:
Point() : time(0) {
}
Point(const std::vector<dReal>& newq, dReal newtime) : time(newtime
) {
q = newq;
}
Point(const std::vector<dReal>& newq, const Transform& newtrans, dR
eal newtime) : time(newtime) {
q=newq;
trans=newtrans;
}
dReal time;
std::vector<dReal> q, qdot, qtorque;
Transform trans;
Vector linearvel, angularvel;
};
/// calculate the coefficients of a smooth quintic spline with /// \deprecated (11/10/04)
/// continuous endpoint positions and velocities for via points typedef Point TPOINT RAVE_DEPRECATED;
/// using minimum jerk heuristics
//void _CalculateQuinticCoefficients(TSEGMENT&, TPOINT& tp0, TPOINT& tp
1);
/// recalculate all via point velocities and accelerations /// \deprecated (11/10/04)
virtual void _RecalculateViaPointDerivatives(); virtual bool SampleTrajectory(dReal time, Point& tp) const RAVE_DEPRECA
TED;
/// computes minimum time interval for linear interpolation between /// \deprecated (11/10/04)
/// path points that does not exceed the maximum joint velocities virtual const std::vector<Point>& GetPoints() const RAVE_DEPRECATED;
virtual dReal _MinimumTimeLinear(const TrajectoryBase::TPOINT& p0, cons
t TrajectoryBase::TPOINT& p1, bool bActiveDOFs);
/// computes minimum time interval for cubic interpolation between /// \deprecated (11/10/04)
/// path points that does not exceed the maximum joint velocities inline int GetDOF() const RAVE_DEPRECATED {
/// or accelerations return GetConfigurationSpecification().GetDOF();
virtual dReal _MinimumTimeCubic(const TrajectoryBase::TPOINT& p0, const }
TrajectoryBase::TPOINT& p1, bool bActiveDOFs);
/// computes minimum time interval for cubic interpolation between /// \deprecated (11/10/04)
/// path points that does not exceed the maximum joint velocities virtual dReal GetTotalDuration() const RAVE_DEPRECATED
/// or accelerations assuming zero velocities at endpoints {
virtual dReal _MinimumTimeCubicZero(const TrajectoryBase::TPOINT& p0, c return GetDuration();
onst TrajectoryBase::TPOINT& p1, bool bActiveDOFs); }
/// computes minimum time interval for quintic interpolation between /// \deprecated (11/10/04)
/// path points that does not exceed the maximum joint velocities virtual bool Write(std::ostream& O, int options) const RAVE_DEPRECATED
/// or accelerations {
virtual dReal _MinimumTimeQuintic(const TrajectoryBase::TPOINT& p0, con serialize(O,options);
st TrajectoryBase::TPOINT& p1, bool bActiveDOFs); return true;
virtual dReal _MinimumTimeTransform(const Transform& t0, const Transfor }
m& t1);
/// find the active trajectory interval covering the given time /// \deprecated (11/10/04)
/// (returns the index of the start point of the interval) virtual bool Read(std::istream& I, RobotBaseConstPtr) RAVE_DEPRECATED {
virtual int _FindActiveInterval(dReal time) const; deserialize(I);
return true;
}
/// \brief Sample the trajectory using linear interpolation. /// \deprecated (11/10/04)
virtual bool _SampleLinear(const TrajectoryBase::TPOINT& p0, const Traj virtual int GetInterpMethod() const RAVE_DEPRECATED {
ectoryBase::TPOINT& p1, const TrajectoryBase::TSEGMENT& seg, dReal time, Tr return 0;
ajectoryBase::TPOINT& sample) const; }
/// \brief Sample using linear interpolation with parabolic blends. /// \deprecated (11/10/04)
virtual bool _SampleLinearBlend(const TrajectoryBase::TPOINT& p0, const virtual bool CalcTrajTiming(RobotBasePtr probot, int interp, bool auto
TrajectoryBase::TPOINT& p1, const TrajectoryBase::TSEGMENT& seg, dReal tim calc, bool activedof, dReal fmaxvelmult=1) RAVE_DEPRECATED;
e, TrajectoryBase::TPOINT& sample) const;
/// \brief Sample the trajectory using cubic interpolation. /// \deprecated (11/10/04)
virtual bool _SampleCubic(const TrajectoryBase::TPOINT& p0, const Traje virtual void Clear() RAVE_DEPRECATED
ctoryBase::TPOINT& p1, const TrajectoryBase::TSEGMENT& seg, dReal time, Tra {
jectoryBase::TPOINT& sample) const; Remove(0,GetNumWaypoints());
}
/// \brief Sample the trajectory using quintic interpolation with minim /// \deprecated (11/10/04)
um jerk. virtual void AddPoint(const Point& p) RAVE_DEPRECATED;
virtual bool _SampleQuintic(const TrajectoryBase::TPOINT& p0, const Tra
jectoryBase::TPOINT& p1, const TrajectoryBase::TSEGMENT& seg, dReal time, T
rajectoryBase::TPOINT& sample) const;
std::vector<TrajectoryBase::TPOINT> _vecpoints; /// \deprecated (11/10/04)
std::vector<TSEGMENT> _vecsegments; virtual void Reset(int dof) RAVE_DEPRECATED
std::vector<dReal> _lowerJointLimit, _upperJointLimit, _maxJointVel, _m {
axJointAccel; Remove(0,GetNumWaypoints());
Vector _maxAffineTranslationVel; }
dReal _maxAffineRotationQuatVel;
int _nQuaternionIndex; ///< the index of a quaternion rotation, if
one exists (interpolation is different for quaternions)
/// computes the difference of two states necessary for correct interpo /// \deprecated (11/10/04)
lation when there are circular joints. Default is regular subtraction. static const int TO_OneLine RAVE_DEPRECATED = 1;
/// _diffstatefn(q1,q2) -> q1 -= q2 static const int TO_NoHeader RAVE_DEPRECATED = 2;
boost::function<void(std::vector<dReal>&,const std::vector<dReal>&)> _d static const int TO_IncludeTimestamps RAVE_DEPRECATED = 4;
iffstatefn; static const int TO_IncludeBaseTransformation RAVE_DEPRECATED = 8;
static const int TO_IncludeVelocities RAVE_DEPRECATED = 0x10;
static const int TO_IncludeTorques RAVE_DEPRECATED = 0x20;
static const int TO_InterpolationMask RAVE_DEPRECATED = 0x1c0;
static const int NONE RAVE_DEPRECATED = 0;
static const int LINEAR RAVE_DEPRECATED = 1;
static const int LINEAR_BLEND RAVE_DEPRECATED = 2;
static const int CUBIC RAVE_DEPRECATED = 3;
static const int QUINTIC RAVE_DEPRECATED = 4;
static const int NUM_METHODS RAVE_DEPRECATED = 5;
InterpEnum _interpMethod; protected:
int _nDOF; inline TrajectoryBasePtr shared_trajectory() {
return boost::static_pointer_cast<TrajectoryBase>(shared_from_this(
));
}
inline TrajectoryBaseConstPtr shared_trajectory_const() const {
return boost::static_pointer_cast<TrajectoryBase const>(shared_from
_this());
}
private:
virtual const char* GetHash() const { virtual const char* GetHash() const {
return OPENRAVE_TRAJECTORY_HASH; return OPENRAVE_TRAJECTORY_HASH;
} }
/// \deprecated (11/10/04), unfortunately necessary for back compat
mutable std::vector<Point> __vdeprecatedpoints;
}; };
typedef TrajectoryBase Trajectory; /// \deprecated (11/10/04)
typedef TrajectoryBase Trajectory RAVE_DEPRECATED;
} // end namespace OpenRAVE } // end namespace OpenRAVE
#endif // TRAJECTORY_H #endif // TRAJECTORY_H
 End of changes. 51 change blocks. 
267 lines changed or deleted 215 lines changed or added


 viewer.h   viewer.h 
skipping to change at line 25 skipping to change at line 25
// 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.
*/ */
#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 /** \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. 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 Designed to be multi-thread safe and destruction and modification of th
e viewer plot can be done at any time. The viewers e viewer plot can be done at any time. The viewers
/// internally handle synchronization and threading issues. internally handle synchronization and threading issues.
*/
class OPENRAVE_API GraphHandle class OPENRAVE_API GraphHandle
{ {
public: public:
virtual ~GraphHandle() { virtual ~GraphHandle() {
} }
/// \brief Changes the underlying transformation of the plot. <b>[multi -thread safe]</b> /// \brief Changes the underlying transformation of the plot. <b>[multi -thread safe]</b>
/// ///
/// \param t new transformation of the plot /// \param t new transformation of the plot
virtual void SetTransform(const RaveTransform<float>& t) OPENRAVE_DUMMY _IMPLEMENTATION; virtual void SetTransform(const RaveTransform<float>& t) OPENRAVE_DUMMY _IMPLEMENTATION;
/// \brief Shows or hides the plot without destroying its resources. <b >[multi-thread safe]</b> /// \brief Shows or hides the plot without destroying its resources. <b >[multi-thread safe]</b>
virtual void SetShow(bool bshow) OPENRAVE_DUMMY_IMPLEMENTATION; virtual void SetShow(bool bshow) OPENRAVE_DUMMY_IMPLEMENTATION;
}; };
typedef boost::shared_ptr<GraphHandle> GraphHandlePtr; typedef boost::shared_ptr<GraphHandle> GraphHandlePtr;
typedef boost::shared_ptr<GraphHandle const> GraphHandleConstPtr; typedef boost::shared_ptr<GraphHandle const> GraphHandleConstPtr;
typedef boost::weak_ptr<GraphHandle const> GraphHandleWeakPtr; typedef boost::weak_ptr<GraphHandle const> GraphHandleWeakPtr;
/** \brief <b>[interface]</b> Base class for the graphics and gui engine th at renders the environment and provides visual sensor information. See \ref arch_viewer. /** \brief <b>[interface]</b> Base class for the graphics and gui engine th at renders the environment and provides visual sensor information. <b>If no t specified, method is not multi-thread safe.</b> See \ref arch_viewer.
\ingroup interfaces \ingroup interfaces
*/ */
class OPENRAVE_API ViewerBase : public InterfaceBase class OPENRAVE_API ViewerBase : public InterfaceBase
{ {
public: public:
enum ViewerEvents enum ViewerEvents
{ {
VE_ItemSelection = 1, VE_ItemSelection = 1,
} RAVE_DEPRECATED; } RAVE_DEPRECATED;
skipping to change at line 165 skipping to change at line 166
SetName(ptitle); SetName(ptitle);
} }
virtual const std::string& GetName() const OPENRAVE_DUMMY_IMPLEMENTATIO N; virtual const std::string& GetName() const OPENRAVE_DUMMY_IMPLEMENTATIO N;
/// \deprecated (11/06/10) /// \deprecated (11/06/10)
virtual void UpdateCameraTransform() RAVE_DEPRECATED OPENRAVE_DUMMY_IMP LEMENTATION; virtual void UpdateCameraTransform() RAVE_DEPRECATED OPENRAVE_DUMMY_IMP LEMENTATION;
/// \deprecated (11/06/10) /// \deprecated (11/06/10)
typedef ItemSelectionCallbackFn ViewerCallbackFn RAVE_DEPRECATED; typedef ItemSelectionCallbackFn ViewerCallbackFn RAVE_DEPRECATED;
/// \deprecated (11/06/10) /// \deprecated (11/06/10)
virtual boost::shared_ptr<void> RegisterCallback(int properties, const ViewerCallbackFn& fncallback) RAVE_DEPRECATED virtual boost::shared_ptr<void> RegisterCallback(int properties, const ItemSelectionCallbackFn& fncallback) RAVE_DEPRECATED
{ {
return RegisterItemSelectionCallback(fncallback); return RegisterItemSelectionCallback(fncallback);
} }
/// \deprecated (11/03/02) Any type of model should be added through th e openrave environment instead of viewer directly. /// \deprecated (11/03/02) Any type of model should be added through th e openrave environment instead of viewer directly.
virtual bool LoadModel(const std::string& pfilename) RAVE_DEPRECATED OP ENRAVE_DUMMY_IMPLEMENTATION; virtual bool LoadModel(const std::string& pfilename) RAVE_DEPRECATED OP ENRAVE_DUMMY_IMPLEMENTATION;
protected: protected:
virtual void SetViewerData(KinBodyPtr body, UserDataPtr data) {
body->SetViewerData(data);
}
virtual GraphHandlePtr plot3(const float* ppoints, int numPoints, int s tride, float fPointSize, const RaveVector<float>& color, int drawstyle = 0) OPENRAVE_DUMMY_IMPLEMENTATION; virtual GraphHandlePtr plot3(const float* ppoints, int numPoints, int s tride, float fPointSize, const RaveVector<float>& color, int drawstyle = 0) OPENRAVE_DUMMY_IMPLEMENTATION;
virtual GraphHandlePtr plot3(const float* ppoints, int numPoints, int s tride, float fPointSize, const float* colors, int drawstyle = 0, bool bhasa lpha=false) OPENRAVE_DUMMY_IMPLEMENTATION; virtual GraphHandlePtr plot3(const float* ppoints, int numPoints, int s tride, float fPointSize, const float* colors, int drawstyle = 0, bool bhasa lpha=false) OPENRAVE_DUMMY_IMPLEMENTATION;
virtual GraphHandlePtr drawlinestrip(const float* ppoints, int numPoint s, int stride, float fwidth, const RaveVector<float>& color) OPENRAVE_DUMMY _IMPLEMENTATION; virtual GraphHandlePtr drawlinestrip(const float* ppoints, int numPoint s, int stride, float fwidth, const RaveVector<float>& color) OPENRAVE_DUMMY _IMPLEMENTATION;
virtual GraphHandlePtr drawlinestrip(const float* ppoints, int numPoint s, int stride, float fwidth, const float* colors) OPENRAVE_DUMMY_IMPLEMENTA TION; virtual GraphHandlePtr drawlinestrip(const float* ppoints, int numPoint s, int stride, float fwidth, const float* colors) OPENRAVE_DUMMY_IMPLEMENTA TION;
virtual GraphHandlePtr drawlinelist(const float* ppoints, int numPoints , int stride, float fwidth, const RaveVector<float>& color) OPENRAVE_DUMMY_ IMPLEMENTATION; virtual GraphHandlePtr drawlinelist(const float* ppoints, int numPoints , int stride, float fwidth, const RaveVector<float>& color) OPENRAVE_DUMMY_ IMPLEMENTATION;
virtual GraphHandlePtr drawlinelist(const float* ppoints, int numPoints , int stride, float fwidth, const float* colors) OPENRAVE_DUMMY_IMPLEMENTAT ION; virtual GraphHandlePtr drawlinelist(const float* ppoints, int numPoints , int stride, float fwidth, const float* colors) OPENRAVE_DUMMY_IMPLEMENTAT ION;
virtual GraphHandlePtr drawarrow(const RaveVector<float>& p1, const Rav eVector<float>& p2, float fwidth, const RaveVector<float>& color) OPENRAVE_ DUMMY_IMPLEMENTATION; virtual GraphHandlePtr drawarrow(const RaveVector<float>& p1, const Rav eVector<float>& p2, float fwidth, const RaveVector<float>& color) OPENRAVE_ DUMMY_IMPLEMENTATION;
 End of changes. 4 change blocks. 
8 lines changed or deleted 13 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/