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