Merge branch 'production' of https://github.com/rdiankov/openrave int…

…o embedGripperValuesIntoJointValues20231030

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 0)
 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,6 +6,16 @@ ChangeLog
 Unreleased
 ==========
 
+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;

plugins/configurationcache/configurationjitterer.cpp

@@ -729,7 +729,7 @@ By default will sample the robot's active DOFs. Parameters part of the interface
                                 }
                             }
                             ss << "]";
-                            RAVELOG_VERBOSE_FORMAT("env=%s, link '%s' exceeded linkdisthresh=%e. ellipdist[%e] > rhs[%e], %s", GetEnv()->GetNameId()%_linkdistthresh%_vLinks[ilink]->GetName()%ellipdist%rhs%ss.str());
+                            RAVELOG_VERBOSE_FORMAT("env=%s, link '%s' exceeded linkdisthresh=%e. ellipdist[%e] > rhs[%e], %s", GetEnv()->GetNameId()%_vLinks[ilink]->GetName()%_linkdistthresh%ellipdist%rhs%ss.str());
                         }
                         break;
                     }

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_int.cpp

@@ -2001,6 +2001,7 @@ void PyEnvironmentBase::Add(PyInterfaceBasePtr pinterface, py::object oAddMode,
             addMode = py::extract<InterfaceAddMode>(oAddMode);
         }
     }
+    PythonThreadSaver threadsaver;
     _penv->Add(pinterface->GetInterfaceBase(), addMode, cmdargs);
 }
 
@@ -2463,14 +2464,24 @@ object PyEnvironmentBase::plot3(object opoints,float pointsize,object ocolors, i
     pair<size_t,size_t> sizes = _getGraphPointsColors(opoints,ocolors,vpoints,vcolors);
     bool bhasalpha = vcolors.size() == 4*sizes.second;
     if( sizes.first == sizes.second ) {
-        return toPyGraphHandle(_penv->plot3(vpoints.data(),sizes.first,sizeof(float)*3,pointsize,vcolors.data(),drawstyle,bhasalpha));
+        GraphHandlePtr phandle;
+        {
+            PythonThreadSaver saver;
+            phandle = _penv->plot3(vpoints.data(),sizes.first,sizeof(float)*3,pointsize,vcolors.data(),drawstyle,bhasalpha);
+        }
+        return toPyGraphHandle(phandle);
     }
     BOOST_ASSERT(vcolors.size()<=4);
-    RaveVector<float> vcolor;
-    for(int i = 0; i < (int)vcolors.size(); ++i) {
-        vcolor[i] = vcolors[i];
+    GraphHandlePtr phandle;
+    {
+        PythonThreadSaver saver;
+        RaveVector<float> vcolor;
+        for(int i = 0; i < (int)vcolors.size(); ++i) {
+            vcolor[i] = vcolors[i];
+        }
+        phandle = _penv->plot3(vpoints.data(),sizes.first,sizeof(float)*3,pointsize,vcolor,drawstyle);
     }
-    return toPyGraphHandle(_penv->plot3(vpoints.data(),sizes.first,sizeof(float)*3,pointsize,vcolor,drawstyle));
+    return toPyGraphHandle(phandle);
 }
 
 object PyEnvironmentBase::drawlinestrip(object opoints,float linewidth,object ocolors, int drawstyle)

python/bindings/openravepy_kinbody.cpp

@@ -366,7 +366,11 @@ object PyGeometryInfo::SerializeJSON(dReal fUnitScale, object options)
 {
     rapidjson::Document doc;
     KinBody::GeometryInfoPtr pgeominfo = GetGeometryInfo();
-    pgeominfo->SerializeJSON(doc, doc.GetAllocator(), fUnitScale, pyGetIntFromPy(options, 0));
+    const int intOptions = pyGetIntFromPy(options, 0);
+    {
+        openravepy::PythonThreadSaver threadsaver;
+        pgeominfo->SerializeJSON(doc, doc.GetAllocator(), fUnitScale, intOptions);
+    }
     return toPyObject(doc);
 }
 
@@ -375,7 +379,11 @@ void PyGeometryInfo::DeserializeJSON(object obj, dReal fUnitScale, object option
     rapidjson::Document doc;
     toRapidJSONValue(obj, doc, doc.GetAllocator());
     KinBody::GeometryInfoPtr pgeominfo = GetGeometryInfo();
-    pgeominfo->DeserializeJSON(doc, fUnitScale, pyGetIntFromPy(options, 0));
+    const int intOptions = pyGetIntFromPy(options, 0);
+    {
+        openravepy::PythonThreadSaver threadsaver;
+        pgeominfo->DeserializeJSON(doc, fUnitScale, intOptions);
+    }
     Init(*pgeominfo);
 }
 
@@ -608,7 +616,11 @@ py::object PyLinkInfo::SerializeJSON(dReal fUnitScale, object options)
 {
     rapidjson::Document doc;
     KinBody::LinkInfoPtr pInfo = GetLinkInfo();
-    pInfo->SerializeJSON(doc, doc.GetAllocator(), fUnitScale, pyGetIntFromPy(options, 0));
+    const int intOptions = pyGetIntFromPy(options, 0);
+    {
+        openravepy::PythonThreadSaver threadsaver;
+        pInfo->SerializeJSON(doc, doc.GetAllocator(), fUnitScale, intOptions);
+    }
     return toPyObject(doc);
 }
 
@@ -617,7 +629,11 @@ void PyLinkInfo::DeserializeJSON(object obj, dReal fUnitScale, py::object option
     rapidjson::Document doc;
     toRapidJSONValue(obj, doc, doc.GetAllocator());
     KinBody::LinkInfoPtr pInfo = GetLinkInfo();
-    pInfo->DeserializeJSON(doc, fUnitScale, pyGetIntFromPy(options, 0));
+    const int intOptions = pyGetIntFromPy(options, 0);
+    {
+        openravepy::PythonThreadSaver threadsaver;
+        pInfo->DeserializeJSON(doc, fUnitScale, intOptions);
+    }
     _Update(*pInfo);
 }
 
@@ -1490,6 +1506,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());
 }
@@ -1563,7 +1582,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() {
@@ -1936,7 +1958,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) {
@@ -4261,7 +4286,10 @@ PyStateRestoreContextBase* PyKinBody::CreateKinBodyStateSaver(object options)
 object PyKinBody::ExtractInfo(ExtractInfoOptions options) const
 {
     KinBody::KinBodyInfo info;
-    _pbody->ExtractInfo(info, options);
+    {
+        openravepy::PythonThreadSaver threadsaver;
+        _pbody->ExtractInfo(info, options);
+    }
     return py::to_object(boost::shared_ptr<PyKinBody::PyKinBodyInfo>(new PyKinBody::PyKinBodyInfo(info)));
 }
 
@@ -6091,6 +6119,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))
@@ -6233,6 +6262,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))
@@ -6265,6 +6295,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,