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