Merge remote-tracking branch 'origin/fix_check_20240110' into truckbotv2

CMakeLists.txt

@@ -4,7 +4,7 @@ set( CMAKE_ALLOW_LOOSE_LOOP_CONSTRUCTS TRUE )
 
 # Define here the needed parameters
 set (OPENRAVE_VERSION_MAJOR 0)
-set (OPENRAVE_VERSION_MINOR 136)
+set (OPENRAVE_VERSION_MINOR 138)
 set (OPENRAVE_VERSION_PATCH 1)
 set (OPENRAVE_VERSION ${OPENRAVE_VERSION_MAJOR}.${OPENRAVE_VERSION_MINOR}.${OPENRAVE_VERSION_PATCH})
 set (OPENRAVE_SOVERSION ${OPENRAVE_VERSION_MAJOR}.${OPENRAVE_VERSION_MINOR})

docs/source/changelog.rst

@@ -6,11 +6,21 @@ ChangeLog
 Unreleased
 ==========
 
-Version 0.136.1
+Version 0.138.1
 ===============
 
 * Fix the issue that some robot configurations might not be checked in `Check` function.
 
+Version 0.138.0
+===============
+
+* Added new apis efficient sampling of trajectory range
+
+Version 0.137.0
+===============
+
+* Add `GetId` to python bindings
+
 Version 0.136.0
 ===============
 

include/openrave/trajectory.h

@@ -24,6 +24,8 @@
 
 namespace OpenRAVE {
 
+template <typename T> class RangeGenerator;
+
 /** \brief <b>[interface]</b> Encapsulate a time-parameterized trajectories of robot configurations. <b>If not specified, method is not multi-thread safe.</b> \arch_trajectory
     \ingroup interfaces
  */
@@ -133,6 +135,28 @@ class OPENRAVE_API TrajectoryBase : public InterfaceBase
      */
     virtual void SamplePointsSameDeltaTime(std::vector<dReal>& data, dReal deltatime, bool ensureLastPoint, const ConfigurationSpecification& spec) const;
 
+    /** \brief bulk samples the trajectory evenly given a delta time and a specific configuration specification.
+
+        The default implementation is slow, so interface developers should override it.
+        \param data[out] the sampled points for every time entry.
+        \param deltatime[in] the delta time to sample
+        \param startTime[in] start time to sample from
+        \param stopTime[in] stop time to sample to
+        \param ensureLastPoint[in] if true, data at duration of trajectory is sampled
+     */
+    virtual void SampleRangeSameDeltaTime(std::vector<dReal>& data, dReal deltatime, dReal startTime, dReal stopTime, bool ensureLastPoint) const;
+
+    /** \brief bulk samples the trajectory evenly given a delta time and a specific configuration specification.
+
+        The default implementation is slow, so interface developers should override it.
+        \param data[out] the sampled points for every time entry.
+        \param deltatime[in] the delta time to sample
+        \param startTime[in] start time to sample from
+        \param stopTime[in] stop time to sample to
+        \param ensureLastPoint[in] if true, data at duration of trajectory is sampled
+        \param spec[in] the specification format to return the data in
+     */
+    virtual void SampleRangeSameDeltaTime(std::vector<dReal>& data, dReal deltatime, dReal startTime, dReal stopTime, bool ensureLastPoint, const ConfigurationSpecification& spec) const;
     virtual const ConfigurationSpecification& GetConfigurationSpecification() const = 0;
 
     /// \brief return the number of waypoints
@@ -220,6 +244,21 @@ class OPENRAVE_API TrajectoryBase : public InterfaceBase
         return boost::static_pointer_cast<TrajectoryBase const>(shared_from_this());
     }
 
+    /// \brief sample points in time range
+    ///
+    /// \param data[out] the sampled points for every time entry.
+    /// \param timeRange[in] time range to sample points at
+    template <typename T>
+    void _SamplePointsInRange(std::vector<dReal>& data, RangeGenerator<T>& timeRange) const;
+
+    /// \brief sample points in time range
+    ///
+    /// \param data[out] the sampled points for every time entry.
+    /// \param timeRange[in] time range to sample points at
+    /// \param spec[in] the specification to return the data in
+    template <typename T>
+    void _SamplePointsInRange(std::vector<dReal>& data, RangeGenerator<T>& timeRange, const ConfigurationSpecification& spec) const;
+
 private:
     virtual const char* GetHash() const {
         return OPENRAVE_TRAJECTORY_HASH;

python/bindings/include/openravepy/openravepy_ikparameterization.h

@@ -33,8 +33,9 @@ class PyIkParameterization
     PyIkParameterization(const IkParameterization &ikparam);
     virtual ~PyIkParameterization();
 
-    IkParameterizationType GetType();
-    object GetName();
+    IkParameterizationType GetType() const;
+    std::string GetId() const;
+    object GetName() const;
 
     int GetDOF();
 

python/bindings/include/openravepy/openravepy_jointinfo.h

@@ -310,6 +310,7 @@ class PyLink : public PyReadablesContainer
         object GetContainerBottom() const;
         object GetRenderScale() const;
         object GetRenderFilename() const;
+        std::string GetId() const;
         object GetName() const;
         float GetTransparency() const;
         object GetDiffuseColor() const;
@@ -336,7 +337,8 @@ class PyLink : public PyReadablesContainer
 
     KinBody::LinkPtr GetLink();
 
-    object GetName();
+    std::string GetId() const;
+    object GetName() const;
     int GetIndex();
     void Enable(bool bEnable);
     bool IsEnabled() const;
@@ -442,7 +444,8 @@ class PyJoint : public PyReadablesContainer
 
     KinBody::JointPtr GetJoint();
 
-    object GetName();
+    std::string GetId() const;
+    object GetName() const;
     bool IsMimic(int iaxis=-1);
     string GetMimicEquation(int iaxis=0, int itype=0, const std::string& format="");
     object GetMimicDOFIndices(int iaxis=0);

python/bindings/include/openravepy/openravepy_robotbase.h

@@ -71,6 +71,7 @@ class OPENRAVEPY_API PyRobotBase : public PyKinBody
 
         object GetVelocity() const;
 
+        std::string GetId() const;
         object GetName() const;
 
         void SetName(const std::string& s);
@@ -190,6 +191,7 @@ class OPENRAVEPY_API PyRobotBase : public PyKinBody
         object GetTransform() const;
         object GetTransformPose() const;
         PyRobotBasePtr GetRobot() const;
+        std::string GetId() const;
         object GetName() const;
 
         object GetData();
@@ -223,7 +225,8 @@ class OPENRAVEPY_API PyRobotBase : public PyKinBody
         virtual ~PyConnectedBody();
         RobotBase::ConnectedBodyPtr GetConnectedBody() const;
 
-        object GetName();
+        std::string GetId() const;
+        object GetName() const;
 
         object GetInfo();
 

python/bindings/include/openravepy/openravepy_sensorbase.h

@@ -306,7 +306,7 @@ class PySensorBase : public PyInterfaceBase
     object GetTransform();
     object GetTransformPose();
 
-    object GetName();
+    object GetName() const;
 
     void SetName(const std::string& name);
 

python/bindings/include/openravepy/openravepy_trajectorybase.h

@@ -23,6 +23,8 @@
 namespace openravepy {
 using py::object;
 
+/// \brief wrapper around TrajectoryBase.
+/// This class is not multi-thread safe in general, however concurrent read operations (Sample and GetWaypoints methods) are supported.
 class OPENRAVEPY_API PyTrajectoryBase : public PyInterfaceBase
 {
 protected:
@@ -56,6 +58,10 @@ class OPENRAVEPY_API PyTrajectoryBase : public PyInterfaceBase
 
     object SamplePointsSameDeltaTime2D(dReal deltatime, bool ensureLastPoint, PyConfigurationSpecificationPtr pyspec) const;
 
+    object SampleRangeSameDeltaTime2D(dReal deltatime, dReal startTime, dReal stopTime, bool ensureLastPoint) const;
+
+    object SampleRangeSameDeltaTime2D(dReal deltatime, dReal startTime, dReal stopTime, bool ensureLastPoint, PyConfigurationSpecificationPtr pyspec) const;
+
     object GetConfigurationSpecification() const;
 
     size_t GetNumWaypoints() const;
@@ -103,13 +109,14 @@ class OPENRAVEPY_API PyTrajectoryBase : public PyInterfaceBase
     object SampleFromPrevious(object odata, dReal time, OPENRAVE_SHARED_PTR<ConfigurationSpecification::Group> pygroup) const;
     object SamplePoints2D(object otimes, OPENRAVE_SHARED_PTR<ConfigurationSpecification::Group> pygroup) const;
     object SamplePointsSameDeltaTime2D(dReal deltatime, bool ensureLastPoint, OPENRAVE_SHARED_PTR<ConfigurationSpecification::Group> pygroup) const;
+    object SampleRangeSameDeltaTime2D(dReal deltatime, dReal startTime, dReal stopTime, bool ensureLastPoint, OPENRAVE_SHARED_PTR<ConfigurationSpecification::Group> pygroup) const;
     object GetWaypoints(size_t startindex, size_t endindex, OPENRAVE_SHARED_PTR<ConfigurationSpecification::Group> pygroup) const;
     object GetWaypoints2D(size_t startindex, size_t endindex, OPENRAVE_SHARED_PTR<ConfigurationSpecification::Group> pygroup) const;
     object GetAllWaypoints2D(OPENRAVE_SHARED_PTR<ConfigurationSpecification::Group> pygroup) const;
     object GetWaypoint(int index, OPENRAVE_SHARED_PTR<ConfigurationSpecification::Group> pygroup) const;
 
 private:
-    mutable std::vector<dReal> _vdataCache, _vtimesCache; ///< caches to avoid memory allocation
+    static thread_local std::vector<dReal> _vdataCache, _vtimesCache; ///< caches to avoid memory allocation. TLS to suppport concurrent data read ( getting waypoint, sampling and so on ) from multiple threads.
 };
 
 } // namespace openravepy

python/bindings/openravepy_ikparameterization.cpp

@@ -85,11 +85,15 @@ PyIkParameterization::PyIkParameterization(const IkParameterization &ikparam) {
 PyIkParameterization::~PyIkParameterization() {
 }
 
-IkParameterizationType PyIkParameterization::GetType() {
+IkParameterizationType PyIkParameterization::GetType() const {
     return _param.GetType();
 }
 
-object PyIkParameterization::GetName() {
+std::string PyIkParameterization::GetId() const {
+    return _param.GetId();
+}
+
+object PyIkParameterization::GetName() const {
     return ConvertStringToUnicode(_param.GetName());
 }
 
@@ -495,6 +499,7 @@ void init_openravepy_ikparameterization()
                                     .def(init<OPENRAVE_SHARED_PTR<PyIkParameterization> >(py::args("ikparam")))
 #endif
                                     .def("GetType",&PyIkParameterization::GetType, DOXY_FN(IkParameterization,GetType))
+                                    .def("GetId",&PyIkParameterization::GetId, DOXY_FN(IkParameterization,GetId))
                                     .def("GetName",&PyIkParameterization::GetName, DOXY_FN(IkParameterization,GetName))
                                     .def("SetTransform6D",&PyIkParameterization::SetTransform6D, PY_ARGS("transform") DOXY_FN(IkParameterization,SetTransform6D))
                                     .def("SetRotation3D",&PyIkParameterization::SetRotation3D, PY_ARGS("quat") DOXY_FN(IkParameterization,SetRotation3D))

python/bindings/openravepy_kinbody.cpp

@@ -1512,6 +1512,9 @@ object PyLink::PyGeometry::GetRenderScale() const {
 object PyLink::PyGeometry::GetRenderFilename() const {
     return ConvertStringToUnicode(_pgeometry->GetRenderFilename());
 }
+std::string PyLink::PyGeometry::GetId() const {
+    return _pgeometry->GetId();
+}
 object PyLink::PyGeometry::GetName() const {
     return ConvertStringToUnicode(_pgeometry->GetName());
 }
@@ -1585,7 +1588,10 @@ KinBody::LinkPtr PyLink::GetLink() {
     return _plink;
 }
 
-object PyLink::GetName() {
+std::string PyLink::GetId() const {
+    return _plink->GetId();
+}
+object PyLink::GetName() const {
     return ConvertStringToUnicode(_plink->GetName());
 }
 int PyLink::GetIndex() {
@@ -1960,7 +1966,10 @@ KinBody::JointPtr PyJoint::GetJoint() {
     return _pjoint;
 }
 
-object PyJoint::GetName() {
+std::string PyJoint::GetId() const {
+    return _pjoint->GetId();
+}
+object PyJoint::GetName() const {
     return ConvertStringToUnicode(_pjoint->GetName());
 }
 bool PyJoint::IsMimic(int iaxis) {
@@ -6127,6 +6136,7 @@ void init_openravepy_kinbody()
 #else
             scope_ link = class_<PyLink, OPENRAVE_SHARED_PTR<PyLink>, bases<PyReadablesContainer> >("Link", DOXY_CLASS(KinBody::Link), no_init)
 #endif
+                          .def("GetId",&PyLink::GetId, DOXY_FN(KinBody::Link,GetId))
                           .def("GetName",&PyLink::GetName, DOXY_FN(KinBody::Link,GetName))
                           .def("GetIndex",&PyLink::GetIndex, DOXY_FN(KinBody::Link,GetIndex))
                           .def("Enable",&PyLink::Enable,PY_ARGS("enable") DOXY_FN(KinBody::Link,Enable))
@@ -6269,6 +6279,7 @@ void init_openravepy_kinbody()
                                   .def("GetContainerBottom",&PyLink::PyGeometry::GetContainerBottom, DOXY_FN(KinBody::Link::Geometry,GetContainerBottom))
                                   .def("GetRenderScale",&PyLink::PyGeometry::GetRenderScale, DOXY_FN(KinBody::Link::Geometry,GetRenderScale))
                                   .def("GetRenderFilename",&PyLink::PyGeometry::GetRenderFilename, DOXY_FN(KinBody::Link::Geometry,GetRenderFilename))
+                                  .def("GetId",&PyLink::PyGeometry::GetId, DOXY_FN(KinBody::Link::Geometry,GetId))
                                   .def("GetName",&PyLink::PyGeometry::GetName, DOXY_FN(KinBody::Link::Geometry,GetName))
                                   .def("GetTransparency",&PyLink::PyGeometry::GetTransparency,DOXY_FN(KinBody::Link::Geometry,GetTransparency))
                                   .def("GetDiffuseColor",&PyLink::PyGeometry::GetDiffuseColor,DOXY_FN(KinBody::Link::Geometry,GetDiffuseColor))
@@ -6301,6 +6312,7 @@ void init_openravepy_kinbody()
 #else
             scope_ joint = class_<PyJoint, OPENRAVE_SHARED_PTR<PyJoint>, bases<PyReadablesContainer> >("Joint", DOXY_CLASS(KinBody::Joint),no_init)
 #endif
+                           .def("GetId", &PyJoint::GetId, DOXY_FN(KinBody::Joint,GetId))
                            .def("GetName", &PyJoint::GetName, DOXY_FN(KinBody::Joint,GetName))
 #ifdef USE_PYBIND11_PYTHON_BINDINGS
                            .def("IsMimic",&PyJoint::IsMimic,

python/bindings/openravepy_robot.cpp

@@ -711,6 +711,10 @@ object PyRobotBase::PyManipulator::GetVelocity() const {
 #endif
 }
 
+std::string PyRobotBase::PyManipulator::GetId() const {
+    return _pmanip->GetId();
+}
+
 object PyRobotBase::PyManipulator::GetName() const {
     return ConvertStringToUnicode(_pmanip->GetName());
 }
@@ -1321,6 +1325,9 @@ object PyRobotBase::PyAttachedSensor::GetTransformPose() const {
 PyRobotBasePtr PyRobotBase::PyAttachedSensor::GetRobot() const {
     return !_pattached->GetRobot() ? PyRobotBasePtr() : PyRobotBasePtr(new PyRobotBase(_pattached->GetRobot(), _pyenv));
 }
+std::string PyRobotBase::PyAttachedSensor::GetId() const {
+    return _pattached->GetId();
+}
 object PyRobotBase::PyAttachedSensor::GetName() const {
     return ConvertStringToUnicode(_pattached->GetName());
 }
@@ -1385,7 +1392,11 @@ RobotBase::ConnectedBodyPtr PyRobotBase::PyConnectedBody::GetConnectedBody() con
     return _pconnected;
 }
 
-object PyRobotBase::PyConnectedBody::GetName() {
+std::string PyRobotBase::PyConnectedBody::GetId() const {
+    return _pconnected->GetId();
+}
+
+object PyRobotBase::PyConnectedBody::GetName() const {
     return ConvertStringToUnicode(_pconnected->GetName());
 }
 
@@ -2774,6 +2785,7 @@ void init_openravepy_robot()
         .def("GetTransform", &PyRobotBase::PyManipulator::GetTransform, DOXY_FN(RobotBase::Manipulator,GetTransform))
         .def("GetTransformPose", &PyRobotBase::PyManipulator::GetTransformPose, DOXY_FN(RobotBase::Manipulator,GetTransform))
         .def("GetVelocity", &PyRobotBase::PyManipulator::GetVelocity, DOXY_FN(RobotBase::Manipulator,GetVelocity))
+        .def("GetId",&PyRobotBase::PyManipulator::GetId, DOXY_FN(RobotBase::Manipulator,GetId))
         .def("GetName",&PyRobotBase::PyManipulator::GetName, DOXY_FN(RobotBase::Manipulator,GetName))
         .def("SetName",&PyRobotBase::PyManipulator::SetName, PY_ARGS("name") DOXY_FN(RobotBase::Manipulator,SetName))
         .def("GetGripperName",&PyRobotBase::PyManipulator::GetGripperName, DOXY_FN(RobotBase::Manipulator,GetGripperName))
@@ -2938,6 +2950,7 @@ void init_openravepy_robot()
         .def("GetTransform",&PyRobotBase::PyAttachedSensor::GetTransform, DOXY_FN(RobotBase::AttachedSensor,GetTransform))
         .def("GetTransformPose",&PyRobotBase::PyAttachedSensor::GetTransformPose, DOXY_FN(RobotBase::AttachedSensor,GetTransform))
         .def("GetRobot",&PyRobotBase::PyAttachedSensor::GetRobot, DOXY_FN(RobotBase::AttachedSensor,GetRobot))
+        .def("GetId",&PyRobotBase::PyAttachedSensor::GetId, DOXY_FN(RobotBase::AttachedSensor,GetId))
         .def("GetName",&PyRobotBase::PyAttachedSensor::GetName, DOXY_FN(RobotBase::AttachedSensor,GetName))
         .def("GetData",&PyRobotBase::PyAttachedSensor::GetData, DOXY_FN(RobotBase::AttachedSensor,GetData))
         .def("SetRelativeTransform",&PyRobotBase::PyAttachedSensor::SetRelativeTransform, PY_ARGS("transform") DOXY_FN(RobotBase::AttachedSensor,SetRelativeTransform))
@@ -2972,6 +2985,7 @@ void init_openravepy_robot()
 #else
         class_<PyRobotBase::PyConnectedBody, OPENRAVE_SHARED_PTR<PyRobotBase::PyConnectedBody> >("ConnectedBody", DOXY_CLASS(RobotBase::ConnectedBody), no_init)
 #endif
+        .def("GetId",&PyRobotBase::PyConnectedBody::GetId, DOXY_FN(RobotBase::ConnectedBody,GetId))
         .def("GetName",&PyRobotBase::PyConnectedBody::GetName, DOXY_FN(RobotBase::ConnectedBody,GetName))
         .def("GetInfo",&PyRobotBase::PyConnectedBody::GetInfo, DOXY_FN(RobotBase::ConnectedBody,GetInfo))
         .def("SetActive", &PyRobotBase::PyConnectedBody::SetActive, DOXY_FN(RobotBase::ConnectedBody,SetActive))

python/bindings/openravepy_sensor.cpp

@@ -631,7 +631,7 @@ object PySensorBase::GetTransformPose() {
     return toPyArray(_psensor->GetTransform());
 }
 
-object PySensorBase::GetName() {
+object PySensorBase::GetName() const {
     return ConvertStringToUnicode(_psensor->GetName());
 }