From 5310701eabf45a0839d76aaaf13457c060cda417 Mon Sep 17 00:00:00 2001 From: Yoshiki Kanemoto Date: Wed, 10 Jan 2024 14:46:22 +0900 Subject: [PATCH 01/16] release GIL before potentially heavy computations --- python/bindings/openravepy_int.cpp | 15 +++++++++-- python/bindings/openravepy_kinbody.cpp | 35 +++++++++++++++++++++++--- 2 files changed, 44 insertions(+), 6 deletions(-) diff --git a/python/bindings/openravepy_int.cpp b/python/bindings/openravepy_int.cpp index 69e0e47e14..c0e863cc0c 100644 --- a/python/bindings/openravepy_int.cpp +++ b/python/bindings/openravepy_int.cpp @@ -2001,6 +2001,7 @@ void PyEnvironmentBase::Add(PyInterfaceBasePtr pinterface, py::object oAddMode, addMode = py::extract(oAddMode); } } + PythonThreadSaver threadsaver; _penv->Add(pinterface->GetInterfaceBase(), addMode, cmdargs); } @@ -2463,14 +2464,24 @@ object PyEnvironmentBase::plot3(object opoints,float pointsize,object ocolors, i pair 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 vcolor; for(int i = 0; i < (int)vcolors.size(); ++i) { vcolor[i] = vcolors[i]; } - return toPyGraphHandle(_penv->plot3(vpoints.data(),sizes.first,sizeof(float)*3,pointsize,vcolor,drawstyle)); + GraphHandlePtr phandle; + { + PythonThreadSaver saver; + phandle = _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) diff --git a/python/bindings/openravepy_kinbody.cpp b/python/bindings/openravepy_kinbody.cpp index e67d18963c..8aed456c6f 100644 --- a/python/bindings/openravepy_kinbody.cpp +++ b/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 intOption = pyGetIntFromPy(options, 0); + { + openravepy::PythonThreadSaver threadsaver; + pgeominfo->SerializeJSON(doc, doc.GetAllocator(), fUnitScale, intOption); + } 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 intOption = pyGetIntFromPy(options, 0); + { + openravepy::PythonThreadSaver threadsaver; + pgeominfo->DeserializeJSON(doc, fUnitScale, intOption); + } 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 intOption = pyGetIntFromPy(options, 0); + { + openravepy::PythonThreadSaver threadsaver; + pInfo->SerializeJSON(doc, doc.GetAllocator(), fUnitScale, intOption); + } 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 intOption = pyGetIntFromPy(options, 0); + { + openravepy::PythonThreadSaver threadsaver; + pInfo->DeserializeJSON(doc, fUnitScale, intOption); + } _Update(*pInfo); } @@ -1769,6 +1785,7 @@ void PyLink::InitGeometries(object ogeometryinfos) } geometries[i] = pygeom->GetGeometryInfo(); } + openravepy::PythonThreadSaver threadsaver; return _plink->InitGeometries(geometries); } @@ -1819,6 +1836,7 @@ void PyLink::SetGroupGeometries(const std::string& name, object ogeometryinfos) } geometries[i] = pygeom->GetGeometryInfo(); } + openravepy::PythonThreadSaver threadsaver; _plink->SetGroupGeometries(name, geometries); } @@ -2686,6 +2704,7 @@ bool PyKinBody::InitFromKinBodyInfo(const object pyKinBodyInfo) KinBody::KinBodyInfoPtr pKinBodyInfo; pKinBodyInfo = ExtractKinBodyInfo(pyKinBodyInfo); if(!!pKinBodyInfo) { + openravepy::PythonThreadSaver threadsaver; return _pbody->InitFromKinBodyInfo(*pKinBodyInfo); } return false; @@ -2714,6 +2733,7 @@ bool PyKinBody::InitFromBoxes(const boost::multi_array& vboxes, bool bD vaabbs[i].pos = Vector(vboxes[i][0],vboxes[i][1],vboxes[i][2]); vaabbs[i].extents = Vector(vboxes[i][3],vboxes[i][4],vboxes[i][5]); } + openravepy::PythonThreadSaver threadsaver; return _pbody->InitFromBoxes(vaabbs,bDraw,uri); } @@ -2739,6 +2759,7 @@ bool PyKinBody::InitFromSpheres(const boost::multi_array& vspheres, boo for(size_t i = 0; i < vvspheres.size(); ++i) { vvspheres[i] = Vector(vspheres[i][0],vspheres[i][1],vspheres[i][2],vspheres[i][3]); } + openravepy::PythonThreadSaver threadsaver; return _pbody->InitFromSpheres(vvspheres,bDraw,uri); } @@ -2746,6 +2767,7 @@ bool PyKinBody::InitFromTrimesh(object pytrimesh, bool bDraw, const std::string& { TriMesh mesh; if( ExtractTriMesh(pytrimesh,mesh) ) { + openravepy::PythonThreadSaver threadsaver; return _pbody->InitFromTrimesh(mesh,bDraw,uri); } else { @@ -2763,6 +2785,7 @@ bool PyKinBody::InitFromGeometries(object ogeometries, const std::string& uri) } geometries[i] = pygeom->GetGeometryInfo(); } + openravepy::PythonThreadSaver threadsaver; return _pbody->InitFromGeometries(geometries, uri); } @@ -2776,6 +2799,7 @@ void PyKinBody::InitFromLinkInfos(py::object olinkinfos, const std::string& uri) } linkInfos[i] = *pylinkinfo->GetLinkInfo(); } + openravepy::PythonThreadSaver threadsaver; return _pbody->InitFromLinkInfos(linkInfos, uri); } @@ -2785,11 +2809,13 @@ bool PyKinBody::Init(object olinkinfos, object ojointinfos, const std::string& u _ParseLinkInfos(olinkinfos, vlinkinfos); std::vector vjointinfos; _ParseJointInfos(ojointinfos, vjointinfos); + openravepy::PythonThreadSaver threadsaver; return _pbody->Init(vlinkinfos, vjointinfos, uri); } void PyKinBody::SetLinkGeometriesFromGroup(const std::string& geomname, const bool propagateGroupNameToSelfCollisionChecker) { + openravepy::PythonThreadSaver threadsaver; _pbody->SetLinkGeometriesFromGroup(geomname, propagateGroupNameToSelfCollisionChecker); } @@ -2808,6 +2834,7 @@ void PyKinBody::SetLinkGroupGeometries(const std::string& geomname, object olink geometries[j] = pygeom->GetGeometryInfo(); } } + openravepy::PythonThreadSaver threadsaver; _pbody->SetLinkGroupGeometries(geomname, linkgeometries); } From 4b8ebe874b0f36d463866dfb93f9f6495f689896 Mon Sep 17 00:00:00 2001 From: Kei Usui Date: Wed, 10 Jan 2024 17:57:40 +0900 Subject: [PATCH 02/16] new api SampleRangeSameDeltaTime for uniform sampling of given range --- include/openrave/trajectory.h | 22 +++ .../openravepy/openravepy_trajectorybase.h | 5 + python/bindings/openravepy_trajectory.cpp | 74 ++++++-- src/libopenrave-core/generictrajectory.cpp | 162 +++++++++++------- src/libopenrave/trajectory.cpp | 43 +++-- 5 files changed, 212 insertions(+), 94 deletions(-) diff --git a/include/openrave/trajectory.h b/include/openrave/trajectory.h index 7e620c941f..0a1b323e4e 100644 --- a/include/openrave/trajectory.h +++ b/include/openrave/trajectory.h @@ -133,6 +133,28 @@ class OPENRAVE_API TrajectoryBase : public InterfaceBase */ virtual void SamplePointsSameDeltaTime(std::vector& 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& 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& 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 diff --git a/python/bindings/include/openravepy/openravepy_trajectorybase.h b/python/bindings/include/openravepy/openravepy_trajectorybase.h index 59c92cb863..a52b126d50 100644 --- a/python/bindings/include/openravepy/openravepy_trajectorybase.h +++ b/python/bindings/include/openravepy/openravepy_trajectorybase.h @@ -56,6 +56,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,6 +107,7 @@ class OPENRAVEPY_API PyTrajectoryBase : public PyInterfaceBase object SampleFromPrevious(object odata, dReal time, OPENRAVE_SHARED_PTR pygroup) const; object SamplePoints2D(object otimes, OPENRAVE_SHARED_PTR pygroup) const; object SamplePointsSameDeltaTime2D(dReal deltatime, bool ensureLastPoint, OPENRAVE_SHARED_PTR pygroup) const; + object SampleRangeSameDeltaTime2D(dReal deltatime, dReal startTime, dReal stopTime, bool ensureLastPoint, OPENRAVE_SHARED_PTR pygroup) const; object GetWaypoints(size_t startindex, size_t endindex, OPENRAVE_SHARED_PTR pygroup) const; object GetWaypoints2D(size_t startindex, size_t endindex, OPENRAVE_SHARED_PTR pygroup) const; object GetAllWaypoints2D(OPENRAVE_SHARED_PTR pygroup) const; diff --git a/python/bindings/openravepy_trajectory.cpp b/python/bindings/openravepy_trajectory.cpp index 7711bb83d2..e565436fc3 100644 --- a/python/bindings/openravepy_trajectory.cpp +++ b/python/bindings/openravepy_trajectory.cpp @@ -300,13 +300,8 @@ object PyTrajectoryBase::SamplePoints2D(object otimes, PyConfigurationSpecificat } -object PyTrajectoryBase::SamplePointsSameDeltaTime2D(dReal deltatime, - bool ensureLastPoint) const +object _ConvertToObject(const std::vector& values, int numdof) { - std::vector& values = _vdataCache; - _ptrajectory->SamplePointsSameDeltaTime(values, deltatime, ensureLastPoint); - - const int numdof = _ptrajectory->GetConfigurationSpecification().GetDOF(); #ifdef USE_PYBIND11_PYTHON_BINDINGS py::array_t pypos = toPyArray(values); pypos.resize({(int) values.size()/numdof, numdof}); @@ -321,6 +316,15 @@ object PyTrajectoryBase::SamplePointsSameDeltaTime2D(dReal deltatime, #endif // USE_PYBIND11_PYTHON_BINDINGS } +object PyTrajectoryBase::SamplePointsSameDeltaTime2D(dReal deltatime, + bool ensureLastPoint) const +{ + std::vector& values = _vdataCache; + _ptrajectory->SamplePointsSameDeltaTime(values, deltatime, ensureLastPoint); + const int numdof = _ptrajectory->GetConfigurationSpecification().GetDOF(); + return _ConvertToObject(values, numdof); +} + object PyTrajectoryBase::SamplePointsSameDeltaTime2D(dReal deltatime, bool ensureLastPoint, @@ -331,18 +335,7 @@ object PyTrajectoryBase::SamplePointsSameDeltaTime2D(dReal deltatime, _ptrajectory->SamplePointsSameDeltaTime(values, deltatime, ensureLastPoint, spec); const int numdof = spec.GetDOF(); -#ifdef USE_PYBIND11_PYTHON_BINDINGS - py::array_t pypos = toPyArray(values); - pypos.resize({(int) values.size()/numdof, numdof}); - return pypos; -#else // USE_PYBIND11_PYTHON_BINDINGS - npy_intp dims[] = { npy_intp(values.size()/numdof), npy_intp(numdof) }; - PyObject *pypos = PyArray_SimpleNew(2,dims, sizeof(dReal)==8 ? PyArray_DOUBLE : PyArray_FLOAT); - if( !values.empty() ) { - memcpy(PyArray_DATA(pypos), values.data(), values.size()*sizeof(values[0])); - } - return py::to_array_astype(pypos); -#endif // USE_PYBIND11_PYTHON_BINDINGS + return _ConvertToObject(values, numdof); } object PyTrajectoryBase::SamplePointsSameDeltaTime2D(dReal deltatime, @@ -353,6 +346,42 @@ object PyTrajectoryBase::SamplePointsSameDeltaTime2D(dReal deltatime, return this->SamplePointsSameDeltaTime2D(deltatime, ensureLastPoint, pyspec); } +object PyTrajectoryBase::SampleRangeSameDeltaTime2D(dReal deltatime, + dReal startTime, + dReal stopTime, + bool ensureLastPoint) const +{ + std::vector& values = _vdataCache; + _ptrajectory->SampleRangeSameDeltaTime(values, deltatime, startTime, stopTime, ensureLastPoint); + const int numdof = _ptrajectory->GetConfigurationSpecification().GetDOF(); + return _ConvertToObject(values, numdof); +} + + +object PyTrajectoryBase::SampleRangeSameDeltaTime2D(dReal deltatime, + dReal startTime, + dReal stopTime, + bool ensureLastPoint, + PyConfigurationSpecificationPtr pyspec) const +{ + std::vector& values = _vdataCache; + ConfigurationSpecification spec = openravepy::GetConfigurationSpecification(pyspec); + _ptrajectory->SampleRangeSameDeltaTime(values, deltatime, startTime, stopTime, ensureLastPoint, spec); + + const int numdof = spec.GetDOF(); + return _ConvertToObject(values, numdof); +} + +object PyTrajectoryBase::SampleRangeSameDeltaTime2D(dReal deltatime, + dReal startTime, + dReal stopTime, + bool ensureLastPoint, + OPENRAVE_SHARED_PTR pygroup) const +{ + PyConfigurationSpecificationPtr pyspec(new PyConfigurationSpecification(*pygroup)); + return this->SampleRangeSameDeltaTime2D(deltatime, startTime, stopTime, ensureLastPoint, pyspec); +} + object PyTrajectoryBase::SamplePoints2D(object otimes, OPENRAVE_SHARED_PTR pygroup) const { PyConfigurationSpecificationPtr pyspec(new PyConfigurationSpecification(*pygroup)); @@ -654,6 +683,9 @@ void init_openravepy_trajectory() object (PyTrajectoryBase::*SamplePointsSameDeltaTime2D1)(dReal, bool) const = &PyTrajectoryBase::SamplePointsSameDeltaTime2D; object (PyTrajectoryBase::*SamplePointsSameDeltaTime2D2)(dReal, bool, PyConfigurationSpecificationPtr) const = &PyTrajectoryBase::SamplePointsSameDeltaTime2D; object (PyTrajectoryBase::*SamplePointsSameDeltaTime2D3)(dReal, bool, OPENRAVE_SHARED_PTR) const = &PyTrajectoryBase::SamplePointsSameDeltaTime2D; + object (PyTrajectoryBase::*SampleRangeSameDeltaTime2D1)(dReal, dReal, dReal, bool) const = &PyTrajectoryBase::SampleRangeSameDeltaTime2D; + object (PyTrajectoryBase::*SampleRangeSameDeltaTime2D2)(dReal, dReal, dReal, bool, PyConfigurationSpecificationPtr) const = &PyTrajectoryBase::SampleRangeSameDeltaTime2D; + object (PyTrajectoryBase::*SampleRangeSameDeltaTime2D3)(dReal, dReal, dReal, bool, OPENRAVE_SHARED_PTR) const = &PyTrajectoryBase::SampleRangeSameDeltaTime2D; object (PyTrajectoryBase::*GetWaypoints1)(size_t,size_t) const = &PyTrajectoryBase::GetWaypoints; object (PyTrajectoryBase::*GetWaypoints2)(size_t,size_t,PyConfigurationSpecificationPtr) const = &PyTrajectoryBase::GetWaypoints; object (PyTrajectoryBase::*GetWaypoints3)(size_t, size_t, OPENRAVE_SHARED_PTR) const = &PyTrajectoryBase::GetWaypoints; @@ -696,6 +728,12 @@ void init_openravepy_trajectory() #ifdef USE_PYBIND11_PYTHON_BINDINGS // why boost::python can resolve this overload? .def("SamplePointsSameDeltaTime2D",SamplePointsSameDeltaTime2D3, PY_ARGS("deltatime","ensurelastpoint","group") DOXY_FN(TrajectoryBase,SamplePointsSameDeltaTime2D "dReal; bool; const ConfigurationSpecification::Group")) +#endif + .def("SampleRangeSameDeltaTime2D",SampleRangeSameDeltaTime2D1, PY_ARGS("deltatime","startTime","stopTime","ensurelastpoint") DOXY_FN(TrajectoryBase,SampleRangeSameDeltaTime2D "dReal; dReal; dReal; bool")) + .def("SampleRangeSameDeltaTime2D",SampleRangeSameDeltaTime2D2, PY_ARGS("deltatime","startTime","stopTime","ensurelastpoint","spec") DOXY_FN(TrajectoryBase,SampleRangeSameDeltaTime2D "dReal; dReal; dReal; bool; const ConfigurationSpecification")) +#ifdef USE_PYBIND11_PYTHON_BINDINGS + // why boost::python can resolve this overload? + .def("SampleRangeSameDeltaTime2D",SampleRangeSameDeltaTime2D3, PY_ARGS("deltatime","startTime","stopTime","ensurelastpoint","group") DOXY_FN(TrajectoryBase,SampleRangeSameDeltaTime2D "dReal; dReal; dReal; bool; const ConfigurationSpecification::Group")) #endif .def("GetConfigurationSpecification",&PyTrajectoryBase::GetConfigurationSpecification,DOXY_FN(TrajectoryBase,GetConfigurationSpecification)) .def("GetNumWaypoints",&PyTrajectoryBase::GetNumWaypoints,DOXY_FN(TrajectoryBase,GetNumWaypoints)) diff --git a/src/libopenrave-core/generictrajectory.cpp b/src/libopenrave-core/generictrajectory.cpp index df32ae9be8..4af162ac69 100644 --- a/src/libopenrave-core/generictrajectory.cpp +++ b/src/libopenrave-core/generictrajectory.cpp @@ -440,80 +440,46 @@ class GenericTrajectory : public TrajectoryBase void SamplePointsSameDeltaTime(std::vector& data, dReal deltatime, bool ensureLastPoint) const override { - BOOST_ASSERT(_bInit); - BOOST_ASSERT(_timeoffset>=0); - _ComputeInternal(); - OPENRAVE_ASSERT_OP_FORMAT0((int)_vtrajdata.size(),>=,_spec.GetDOF(), "trajectory needs at least one point to sample from", ORE_InvalidArguments); - if( IS_DEBUGLEVEL(Level_Verbose) || (RaveGetDebugLevel() & Level_VerifyPlans) ) { - _VerifySampling(); - } + return _SampleRangeSameDeltaTime(data, deltatime, 0, GetDuration(), ensureLastPoint); + } - const dReal duration = GetDuration(); - int numPoints = int(ceil(duration / deltatime)); // ceil to make it behave same way as numpy arange(0, duration, deltatime) - if (ensureLastPoint && (numPoints - 1) * deltatime + g_fEpsilon < duration) { - numPoints++; + void SamplePointsSameDeltaTime(std::vector& data, dReal deltatime, bool ensureLastPoint, const ConfigurationSpecification& spec) const override + { + // avoid unnecessary computation if spec is same as this->_spec + if (spec == _spec) { + return SamplePointsSameDeltaTime(data, deltatime, ensureLastPoint); } - int dof = GetConfigurationSpecification().GetDOF(); - //std::vector dataPerTimestep(dof,0); - data.resize(dof*numPoints); - - const std::vector::const_iterator begin = _vaccumtime.begin(); - std::vector::const_iterator it = begin; - - std::vector::iterator itdata = data.begin(); + std::vector dataInSourceSpec; // TODO perhaps not a good idea to create a separate vector like this... + SamplePointsSameDeltaTime(dataInSourceSpec, deltatime, ensureLastPoint); - for(int i = 0; i < (ensureLastPoint ? numPoints-1 : numPoints); ++i, itdata += dof) { - dReal sampletime = i * deltatime; - if( sampletime >= duration ) { - std::copy(_vtrajdata.end() - _spec.GetDOF(), _vtrajdata.end(), itdata); - } - else { - // knowing time always increases, it is safe to search in [it, end] instead of [begin, end] - it = std::lower_bound(it, _vaccumtime.cend(), sampletime); + int dofSourceSpec = _spec.GetDOF(); + OPENRAVE_ASSERT_OP(dataInSourceSpec.size() % dofSourceSpec,==, 0); + int numPoints = dataInSourceSpec.size() / dofSourceSpec; + int dof = spec.GetDOF(); + data.resize(dof*numPoints); - if( it == begin ) { - std::copy(_vtrajdata.begin(),_vtrajdata.begin()+_spec.GetDOF(),itdata); - *(itdata + _timeoffset) = sampletime; - } - else { - size_t index = it - begin; - dReal timeFromLowerWaypoint = sampletime - _vaccumtime.at(index-1); - dReal waypointdeltatime = _vtrajdata.at(_spec.GetDOF()*index + _timeoffset); - // unfortunately due to floating-point error timeFromLowerWaypoint might not be in the range [0, waypointdeltatime], so double check! - if( timeFromLowerWaypoint < 0 ) { - // most likely small epsilon - timeFromLowerWaypoint = 0; - } - else if( timeFromLowerWaypoint > waypointdeltatime ) { - timeFromLowerWaypoint = waypointdeltatime; - } - for(size_t j = 0; j < _vgroupinterpolators.size(); ++j) { - if( !!_vgroupinterpolators[j] ) { - _vgroupinterpolators[j](index-1, timeFromLowerWaypoint, itdata); - } - } - // should return the sample time relative to the last endpoint so it is easier to re-insert in the trajectory - *(itdata + _timeoffset) = timeFromLowerWaypoint; - } - } - } + ConfigurationSpecification::ConvertData(data.begin(), + spec, + dataInSourceSpec.begin(), + _spec, + numPoints, + GetEnv()); + } - if (ensureLastPoint) { - // copy the last point, itdata should point to that - std::copy(_vtrajdata.end() - _spec.GetDOF(), _vtrajdata.end(), itdata); - } + void SampleRangeSameDeltaTime(std::vector& data, dReal deltatime, dReal startTime, dReal stopTime, bool ensureLastPoint) const override + { + return _SampleRangeSameDeltaTime(data, deltatime, startTime, stopTime, ensureLastPoint); } - void SamplePointsSameDeltaTime(std::vector& data, dReal deltatime, bool ensureLastPoint, const ConfigurationSpecification& spec) const override + void SampleRangeSameDeltaTime(std::vector& data, dReal deltatime, dReal startTime, dReal stopTime, bool ensureLastPoint, const ConfigurationSpecification& spec) const override { // avoid unnecessary computation if spec is same as this->_spec if (spec == _spec) { - return SamplePointsSameDeltaTime(data, deltatime, ensureLastPoint); + return SampleRangeSameDeltaTime(data, deltatime, startTime, stopTime, ensureLastPoint); } - std::vector dataInSourceSpec; // TODO perhaps not a good idea to create a separate vector like this... - SamplePointsSameDeltaTime(dataInSourceSpec, deltatime, ensureLastPoint); + SampleRangeSameDeltaTime(dataInSourceSpec, deltatime, startTime, stopTime, ensureLastPoint); int dofSourceSpec = _spec.GetDOF(); OPENRAVE_ASSERT_OP(dataInSourceSpec.size() % dofSourceSpec,==, 0); @@ -1747,6 +1713,80 @@ class GenericTrajectory : public TrajectoryBase { } + void _SampleRangeSameDeltaTime(std::vector& data, dReal deltatime, dReal startTime, dReal stopTime, bool ensureLastPoint) const + { + BOOST_ASSERT(_bInit); + BOOST_ASSERT(_timeoffset>=0); + BOOST_ASSERT(startTime>=0); + BOOST_ASSERT(stopTime>=startTime); + + _ComputeInternal(); + OPENRAVE_ASSERT_OP_FORMAT0((int)_vtrajdata.size(),>=,_spec.GetDOF(), "trajectory needs at least one point to sample from", ORE_InvalidArguments); + if( IS_DEBUGLEVEL(Level_Verbose) || (RaveGetDebugLevel() & Level_VerifyPlans) ) { + _VerifySampling(); + } + + const dReal trajDuration = GetDuration(); + BOOST_ASSERT(trajDuration>=stopTime); + int numPoints = 0; + { + const dReal duration = stopTime - startTime; + numPoints = int(ceil(duration / deltatime)); // ceil to make it behave same way as numpy arange(0, duration, deltatime) + if (ensureLastPoint && (numPoints - 1) * deltatime + g_fEpsilon < duration) { + numPoints++; + } + } + int dof = GetConfigurationSpecification().GetDOF(); + //std::vector dataPerTimestep(dof,0); + data.resize(dof*numPoints); + + const std::vector::const_iterator begin = _vaccumtime.begin(); + std::vector::const_iterator it = begin; + + std::vector::iterator itdata = data.begin(); + + for(int i = 0; i < (ensureLastPoint ? numPoints-1 : numPoints); ++i, itdata += dof) { + const dReal sampletime = startTime + i * deltatime; + if( sampletime >= trajDuration ) { + std::copy(_vtrajdata.end() - _spec.GetDOF(), _vtrajdata.end(), itdata); + } + else { + // knowing time always increases, it is safe to search in [it, end] instead of [begin, end] + it = std::lower_bound(it, _vaccumtime.cend(), sampletime); + + if( it == begin ) { + std::copy(_vtrajdata.begin(),_vtrajdata.begin()+_spec.GetDOF(),itdata); + *(itdata + _timeoffset) = sampletime; + } + else { + size_t index = it - begin; + dReal timeFromLowerWaypoint = sampletime - _vaccumtime.at(index-1); + dReal waypointdeltatime = _vtrajdata.at(_spec.GetDOF()*index + _timeoffset); + // unfortunately due to floating-point error timeFromLowerWaypoint might not be in the range [0, waypointdeltatime], so double check! + if( timeFromLowerWaypoint < 0 ) { + // most likely small epsilon + timeFromLowerWaypoint = 0; + } + else if( timeFromLowerWaypoint > waypointdeltatime ) { + timeFromLowerWaypoint = waypointdeltatime; + } + for(size_t j = 0; j < _vgroupinterpolators.size(); ++j) { + if( !!_vgroupinterpolators[j] ) { + _vgroupinterpolators[j](index-1, timeFromLowerWaypoint, itdata); + } + } + // should return the sample time relative to the last endpoint so it is easier to re-insert in the trajectory + *(itdata + _timeoffset) = timeFromLowerWaypoint; + } + } + } + + if (ensureLastPoint) { + // copy the last point, itdata should point to that + std::copy(_vtrajdata.end() - _spec.GetDOF(), _vtrajdata.end(), itdata); + } + } + ConfigurationSpecification _spec; std::vector< boost::function::iterator&)> > _vgroupinterpolators; std::vector< boost::function > _vgroupvalidators; diff --git a/src/libopenrave/trajectory.cpp b/src/libopenrave/trajectory.cpp index 8fa3f0aa4b..748c9e57c0 100644 --- a/src/libopenrave/trajectory.cpp +++ b/src/libopenrave/trajectory.cpp @@ -125,31 +125,44 @@ void TrajectoryBase::SamplePoints(std::vector& data, const std::vector& data, dReal deltatime, bool ensureLastPoint) const +void _SampleEvenly(std::vector& vtimes, dReal step, dReal start, dReal stop, bool ensureLast) { - const dReal duration = GetDuration(); - int numPoints = int(ceil(duration / deltatime)); // ceil to make it behave same way as numpy arange(0, duration, deltatime) - std::vector vtimes(numPoints, deltatime); + const dReal duration = stop - start; + const int numPoints = int(ceil(duration / step)); // ceil to make it behave same way as numpy arange(0, duration, step) + vtimes.assign(numPoints, start); for (int i = 0; i < numPoints; ++i) { - vtimes[i] *= i; + vtimes[i] += step*i; } - if (ensureLastPoint && vtimes.back() < duration) { + if (ensureLast && vtimes.back() < duration) { vtimes.push_back(duration); } +} + +void TrajectoryBase::SamplePointsSameDeltaTime(std::vector& data, dReal deltatime, bool ensureLastPoint) const +{ + std::vector vtimes; + _SampleEvenly(vtimes, deltatime, 0, GetDuration(), ensureLastPoint); return SamplePoints(data, vtimes); } void TrajectoryBase::SamplePointsSameDeltaTime(std::vector& data, dReal deltatime, bool ensureLastPoint, const ConfigurationSpecification& spec) const { - const dReal duration = GetDuration(); - int numPoints = int(ceil(duration / deltatime)); // ceil to make it behave same way as numpy arange(0, duration, deltatime) - std::vector vtimes(numPoints, deltatime); - for (int i = 0; i < numPoints; ++i) { - vtimes[i] *= i; - } - if (ensureLastPoint && vtimes.back() < duration) { - vtimes.push_back(duration); - } + std::vector vtimes; + _SampleEvenly(vtimes, deltatime, 0, GetDuration(), ensureLastPoint); + return SamplePoints(data, vtimes, spec); +} + +void TrajectoryBase::SampleRangeSameDeltaTime(std::vector& data, dReal deltatime, dReal startTime, dReal stopTime, bool ensureLastPoint) const +{ + std::vector vtimes; + _SampleEvenly(vtimes, deltatime, startTime, stopTime, ensureLastPoint); + return SamplePoints(data, vtimes); +} + +void TrajectoryBase::SampleRangeSameDeltaTime(std::vector& data, dReal deltatime, dReal startTime, dReal stopTime, bool ensureLastPoint, const ConfigurationSpecification& spec) const +{ + std::vector vtimes; + _SampleEvenly(vtimes, deltatime, startTime, stopTime, ensureLastPoint); return SamplePoints(data, vtimes, spec); } From f41199c3030ccbca91103302b93ba30d3c6cf33c Mon Sep 17 00:00:00 2001 From: Kei Usui Date: Wed, 10 Jan 2024 18:41:22 +0900 Subject: [PATCH 03/16] changelog --- CMakeLists.txt | 2 +- docs/source/changelog.rst | 5 +++++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 6258aa10d6..96ae39a147 100644 --- a/CMakeLists.txt +++ b/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 137) 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}) diff --git a/docs/source/changelog.rst b/docs/source/changelog.rst index 7e962a2b57..17466cb74b 100644 --- a/docs/source/changelog.rst +++ b/docs/source/changelog.rst @@ -6,6 +6,11 @@ ChangeLog Unreleased ========== +Version 0.137.0 +=============== + +* Added new apis efficient sampling of trajectory range + Version 0.136.0 =============== From bbdc1b834223209ad5689f56947a7c649bf6601f Mon Sep 17 00:00:00 2001 From: YoshitoMori <87542096+y423610m@users.noreply.github.com> Date: Fri, 12 Jan 2024 09:27:32 +0900 Subject: [PATCH 04/16] fix log message in plugins/configurationcache/configurationjitterer.cpp --- plugins/configurationcache/configurationjitterer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/plugins/configurationcache/configurationjitterer.cpp b/plugins/configurationcache/configurationjitterer.cpp index ade223b741..6be7f9e8b2 100644 --- a/plugins/configurationcache/configurationjitterer.cpp +++ b/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; } From 78ceece6a016da1c7bb045e74c0999b9363b27b9 Mon Sep 17 00:00:00 2001 From: Yoshiki Kanemoto Date: Fri, 12 Jan 2024 10:46:19 +0900 Subject: [PATCH 05/16] cosme, include a little more c++ computation into GIL unlock scope --- python/bindings/openravepy_int.cpp | 8 ++++---- python/bindings/openravepy_kinbody.cpp | 16 ++++++++-------- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/python/bindings/openravepy_int.cpp b/python/bindings/openravepy_int.cpp index c0e863cc0c..3be7f8b9c5 100644 --- a/python/bindings/openravepy_int.cpp +++ b/python/bindings/openravepy_int.cpp @@ -2472,13 +2472,13 @@ object PyEnvironmentBase::plot3(object opoints,float pointsize,object ocolors, i return toPyGraphHandle(phandle); } BOOST_ASSERT(vcolors.size()<=4); - RaveVector vcolor; - for(int i = 0; i < (int)vcolors.size(); ++i) { - vcolor[i] = vcolors[i]; - } GraphHandlePtr phandle; { PythonThreadSaver saver; + RaveVector 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(phandle); diff --git a/python/bindings/openravepy_kinbody.cpp b/python/bindings/openravepy_kinbody.cpp index 8aed456c6f..2ee8ac1f58 100644 --- a/python/bindings/openravepy_kinbody.cpp +++ b/python/bindings/openravepy_kinbody.cpp @@ -366,10 +366,10 @@ object PyGeometryInfo::SerializeJSON(dReal fUnitScale, object options) { rapidjson::Document doc; KinBody::GeometryInfoPtr pgeominfo = GetGeometryInfo(); - const int intOption = pyGetIntFromPy(options, 0); + const int intOptions = pyGetIntFromPy(options, 0); { openravepy::PythonThreadSaver threadsaver; - pgeominfo->SerializeJSON(doc, doc.GetAllocator(), fUnitScale, intOption); + pgeominfo->SerializeJSON(doc, doc.GetAllocator(), fUnitScale, intOptions); } return toPyObject(doc); } @@ -379,10 +379,10 @@ void PyGeometryInfo::DeserializeJSON(object obj, dReal fUnitScale, object option rapidjson::Document doc; toRapidJSONValue(obj, doc, doc.GetAllocator()); KinBody::GeometryInfoPtr pgeominfo = GetGeometryInfo(); - const int intOption = pyGetIntFromPy(options, 0); + const int intOptions = pyGetIntFromPy(options, 0); { openravepy::PythonThreadSaver threadsaver; - pgeominfo->DeserializeJSON(doc, fUnitScale, intOption); + pgeominfo->DeserializeJSON(doc, fUnitScale, intOptions); } Init(*pgeominfo); } @@ -616,10 +616,10 @@ py::object PyLinkInfo::SerializeJSON(dReal fUnitScale, object options) { rapidjson::Document doc; KinBody::LinkInfoPtr pInfo = GetLinkInfo(); - const int intOption = pyGetIntFromPy(options, 0); + const int intOptions = pyGetIntFromPy(options, 0); { openravepy::PythonThreadSaver threadsaver; - pInfo->SerializeJSON(doc, doc.GetAllocator(), fUnitScale, intOption); + pInfo->SerializeJSON(doc, doc.GetAllocator(), fUnitScale, intOptions); } return toPyObject(doc); } @@ -629,10 +629,10 @@ void PyLinkInfo::DeserializeJSON(object obj, dReal fUnitScale, py::object option rapidjson::Document doc; toRapidJSONValue(obj, doc, doc.GetAllocator()); KinBody::LinkInfoPtr pInfo = GetLinkInfo(); - const int intOption = pyGetIntFromPy(options, 0); + const int intOptions = pyGetIntFromPy(options, 0); { openravepy::PythonThreadSaver threadsaver; - pInfo->DeserializeJSON(doc, fUnitScale, intOption); + pInfo->DeserializeJSON(doc, fUnitScale, intOptions); } _Update(*pInfo); } From ed2a2821aee82374785c53d032e728907fff7b98 Mon Sep 17 00:00:00 2001 From: Yoshiki Kanemoto Date: Fri, 12 Jan 2024 15:38:47 +0900 Subject: [PATCH 06/16] do not release GIL on box and sphere kinbody init. add GIL release on ExtractInfo --- python/bindings/openravepy_kinbody.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/python/bindings/openravepy_kinbody.cpp b/python/bindings/openravepy_kinbody.cpp index 2ee8ac1f58..eb238ecf5c 100644 --- a/python/bindings/openravepy_kinbody.cpp +++ b/python/bindings/openravepy_kinbody.cpp @@ -2733,7 +2733,6 @@ bool PyKinBody::InitFromBoxes(const boost::multi_array& vboxes, bool bD vaabbs[i].pos = Vector(vboxes[i][0],vboxes[i][1],vboxes[i][2]); vaabbs[i].extents = Vector(vboxes[i][3],vboxes[i][4],vboxes[i][5]); } - openravepy::PythonThreadSaver threadsaver; return _pbody->InitFromBoxes(vaabbs,bDraw,uri); } @@ -2759,7 +2758,6 @@ bool PyKinBody::InitFromSpheres(const boost::multi_array& vspheres, boo for(size_t i = 0; i < vvspheres.size(); ++i) { vvspheres[i] = Vector(vspheres[i][0],vspheres[i][1],vspheres[i][2],vspheres[i][3]); } - openravepy::PythonThreadSaver threadsaver; return _pbody->InitFromSpheres(vvspheres,bDraw,uri); } @@ -4288,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(new PyKinBody::PyKinBodyInfo(info))); } From e756cbc20ae58efd184e18ba53cc14a5d097e380 Mon Sep 17 00:00:00 2001 From: Yoshiki Kanemoto Date: Fri, 12 Jan 2024 15:53:00 +0900 Subject: [PATCH 07/16] do not release GIL on kinbody/geom init since they are not obvious on profiler --- python/bindings/openravepy_kinbody.cpp | 9 --------- 1 file changed, 9 deletions(-) diff --git a/python/bindings/openravepy_kinbody.cpp b/python/bindings/openravepy_kinbody.cpp index eb238ecf5c..369925ab5a 100644 --- a/python/bindings/openravepy_kinbody.cpp +++ b/python/bindings/openravepy_kinbody.cpp @@ -1785,7 +1785,6 @@ void PyLink::InitGeometries(object ogeometryinfos) } geometries[i] = pygeom->GetGeometryInfo(); } - openravepy::PythonThreadSaver threadsaver; return _plink->InitGeometries(geometries); } @@ -1836,7 +1835,6 @@ void PyLink::SetGroupGeometries(const std::string& name, object ogeometryinfos) } geometries[i] = pygeom->GetGeometryInfo(); } - openravepy::PythonThreadSaver threadsaver; _plink->SetGroupGeometries(name, geometries); } @@ -2704,7 +2702,6 @@ bool PyKinBody::InitFromKinBodyInfo(const object pyKinBodyInfo) KinBody::KinBodyInfoPtr pKinBodyInfo; pKinBodyInfo = ExtractKinBodyInfo(pyKinBodyInfo); if(!!pKinBodyInfo) { - openravepy::PythonThreadSaver threadsaver; return _pbody->InitFromKinBodyInfo(*pKinBodyInfo); } return false; @@ -2765,7 +2762,6 @@ bool PyKinBody::InitFromTrimesh(object pytrimesh, bool bDraw, const std::string& { TriMesh mesh; if( ExtractTriMesh(pytrimesh,mesh) ) { - openravepy::PythonThreadSaver threadsaver; return _pbody->InitFromTrimesh(mesh,bDraw,uri); } else { @@ -2783,7 +2779,6 @@ bool PyKinBody::InitFromGeometries(object ogeometries, const std::string& uri) } geometries[i] = pygeom->GetGeometryInfo(); } - openravepy::PythonThreadSaver threadsaver; return _pbody->InitFromGeometries(geometries, uri); } @@ -2797,7 +2792,6 @@ void PyKinBody::InitFromLinkInfos(py::object olinkinfos, const std::string& uri) } linkInfos[i] = *pylinkinfo->GetLinkInfo(); } - openravepy::PythonThreadSaver threadsaver; return _pbody->InitFromLinkInfos(linkInfos, uri); } @@ -2807,13 +2801,11 @@ bool PyKinBody::Init(object olinkinfos, object ojointinfos, const std::string& u _ParseLinkInfos(olinkinfos, vlinkinfos); std::vector vjointinfos; _ParseJointInfos(ojointinfos, vjointinfos); - openravepy::PythonThreadSaver threadsaver; return _pbody->Init(vlinkinfos, vjointinfos, uri); } void PyKinBody::SetLinkGeometriesFromGroup(const std::string& geomname, const bool propagateGroupNameToSelfCollisionChecker) { - openravepy::PythonThreadSaver threadsaver; _pbody->SetLinkGeometriesFromGroup(geomname, propagateGroupNameToSelfCollisionChecker); } @@ -2832,7 +2824,6 @@ void PyKinBody::SetLinkGroupGeometries(const std::string& geomname, object olink geometries[j] = pygeom->GetGeometryInfo(); } } - openravepy::PythonThreadSaver threadsaver; _pbody->SetLinkGroupGeometries(geomname, linkgeometries); } From 6b2562049d60c626a4abce575e1e1069817b9916 Mon Sep 17 00:00:00 2001 From: Kei Usui Date: Tue, 16 Jan 2024 10:39:56 +0900 Subject: [PATCH 08/16] improve diagnosability --- src/libopenrave-core/generictrajectory.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/libopenrave-core/generictrajectory.cpp b/src/libopenrave-core/generictrajectory.cpp index 4af162ac69..2b69e6b9bc 100644 --- a/src/libopenrave-core/generictrajectory.cpp +++ b/src/libopenrave-core/generictrajectory.cpp @@ -1717,8 +1717,8 @@ class GenericTrajectory : public TrajectoryBase { BOOST_ASSERT(_bInit); BOOST_ASSERT(_timeoffset>=0); - BOOST_ASSERT(startTime>=0); - BOOST_ASSERT(stopTime>=startTime); + OPENRAVE_ASSERT_OP_FORMAT0(startTime,>=,0, "start time needs to be non-negative", ORE_InvalidArguments); + OPENRAVE_ASSERT_OP_FORMAT0(stopTime,>=,startTime, "stop time needs to be at least start time", ORE_InvalidArguments); _ComputeInternal(); OPENRAVE_ASSERT_OP_FORMAT0((int)_vtrajdata.size(),>=,_spec.GetDOF(), "trajectory needs at least one point to sample from", ORE_InvalidArguments); @@ -1727,7 +1727,7 @@ class GenericTrajectory : public TrajectoryBase } const dReal trajDuration = GetDuration(); - BOOST_ASSERT(trajDuration>=stopTime); + OPENRAVE_ASSERT_OP_FORMAT0(trajDuration,>=,stopTime, "stop time needs to be at most trajectory duration", ORE_InvalidArguments); int numPoints = 0; { const dReal duration = stopTime - startTime; From 2cd690dc6a4be2715a8e1d2c20705dc37c367adf Mon Sep 17 00:00:00 2001 From: Kei Usui Date: Tue, 16 Jan 2024 13:17:38 +0900 Subject: [PATCH 09/16] allow time beyond trajectory duration to be sampled to be consistent with other sampling api's --- src/libopenrave-core/generictrajectory.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/libopenrave-core/generictrajectory.cpp b/src/libopenrave-core/generictrajectory.cpp index 2b69e6b9bc..4a9e119a20 100644 --- a/src/libopenrave-core/generictrajectory.cpp +++ b/src/libopenrave-core/generictrajectory.cpp @@ -1727,7 +1727,6 @@ class GenericTrajectory : public TrajectoryBase } const dReal trajDuration = GetDuration(); - OPENRAVE_ASSERT_OP_FORMAT0(trajDuration,>=,stopTime, "stop time needs to be at most trajectory duration", ORE_InvalidArguments); int numPoints = 0; { const dReal duration = stopTime - startTime; From 37970b13e8adcebbf2f0addc098b09ca7dfbab5e Mon Sep 17 00:00:00 2001 From: Yoshiki Kanemoto Date: Fri, 26 Jan 2024 18:43:42 +0900 Subject: [PATCH 10/16] add GetId() python bindings to KinBody, Link, Joint, Geometry --- python/bindings/openravepy_kinbody.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/python/bindings/openravepy_kinbody.cpp b/python/bindings/openravepy_kinbody.cpp index 369925ab5a..f9f6f7fad7 100644 --- a/python/bindings/openravepy_kinbody.cpp +++ b/python/bindings/openravepy_kinbody.cpp @@ -1506,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()); } @@ -1579,6 +1582,9 @@ KinBody::LinkPtr PyLink::GetLink() { return _plink; } +std::string PyLink::GetId() { + return _plink->GetId(); +} object PyLink::GetName() { return ConvertStringToUnicode(_plink->GetName()); } @@ -1952,6 +1958,9 @@ KinBody::JointPtr PyJoint::GetJoint() { return _pjoint; } +std::string PyJoint::GetId() { + return _pjoint->GetId(); +} object PyJoint::GetName() { return ConvertStringToUnicode(_pjoint->GetName()); } @@ -6110,6 +6119,7 @@ void init_openravepy_kinbody() #else scope_ link = class_, bases >("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)) @@ -6252,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)) @@ -6284,6 +6295,7 @@ void init_openravepy_kinbody() #else scope_ joint = class_, bases >("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, From be5cc72954a333cb126d7e814da2cb2a3d00c7ca Mon Sep 17 00:00:00 2001 From: Yoshiki Kanemoto Date: Fri, 26 Jan 2024 18:58:44 +0900 Subject: [PATCH 11/16] add const --- python/bindings/include/openravepy/openravepy_jointinfo.h | 7 +++++-- python/bindings/openravepy_kinbody.cpp | 8 ++++---- 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/python/bindings/include/openravepy/openravepy_jointinfo.h b/python/bindings/include/openravepy/openravepy_jointinfo.h index 1a61245cf3..279c011f7f 100644 --- a/python/bindings/include/openravepy/openravepy_jointinfo.h +++ b/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); diff --git a/python/bindings/openravepy_kinbody.cpp b/python/bindings/openravepy_kinbody.cpp index f9f6f7fad7..8e24e93084 100644 --- a/python/bindings/openravepy_kinbody.cpp +++ b/python/bindings/openravepy_kinbody.cpp @@ -1582,10 +1582,10 @@ KinBody::LinkPtr PyLink::GetLink() { return _plink; } -std::string PyLink::GetId() { +std::string PyLink::GetId() const { return _plink->GetId(); } -object PyLink::GetName() { +object PyLink::GetName() const { return ConvertStringToUnicode(_plink->GetName()); } int PyLink::GetIndex() { @@ -1958,10 +1958,10 @@ KinBody::JointPtr PyJoint::GetJoint() { return _pjoint; } -std::string PyJoint::GetId() { +std::string PyJoint::GetId() const { return _pjoint->GetId(); } -object PyJoint::GetName() { +object PyJoint::GetName() const { return ConvertStringToUnicode(_pjoint->GetName()); } bool PyJoint::IsMimic(int iaxis) { From 3ae6a4a76a7bd00e992670494216ef428c9af673 Mon Sep 17 00:00:00 2001 From: Yoshiki Kanemoto Date: Fri, 26 Jan 2024 19:14:50 +0900 Subject: [PATCH 12/16] more places to add python bindings of `GetId` --- CMakeLists.txt | 2 +- docs/source/changelog.rst | 5 +++++ .../openravepy/openravepy_ikparameterization.h | 5 +++-- .../include/openravepy/openravepy_robotbase.h | 5 ++++- .../include/openravepy/openravepy_sensorbase.h | 2 +- .../bindings/openravepy_ikparameterization.cpp | 9 +++++++-- python/bindings/openravepy_robot.cpp | 16 +++++++++++++++- python/bindings/openravepy_sensor.cpp | 2 +- 8 files changed, 37 insertions(+), 9 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 6258aa10d6..96ae39a147 100644 --- a/CMakeLists.txt +++ b/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 137) 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}) diff --git a/docs/source/changelog.rst b/docs/source/changelog.rst index 7e962a2b57..1945a32d5e 100644 --- a/docs/source/changelog.rst +++ b/docs/source/changelog.rst @@ -6,6 +6,11 @@ ChangeLog Unreleased ========== +Version 0.137.0 +=============== + +* Add `GetId` to python bindings + Version 0.136.0 =============== diff --git a/python/bindings/include/openravepy/openravepy_ikparameterization.h b/python/bindings/include/openravepy/openravepy_ikparameterization.h index 3f29840c2a..cb70ccb8c6 100644 --- a/python/bindings/include/openravepy/openravepy_ikparameterization.h +++ b/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(); diff --git a/python/bindings/include/openravepy/openravepy_robotbase.h b/python/bindings/include/openravepy/openravepy_robotbase.h index 7701633eab..db3b1fd29b 100644 --- a/python/bindings/include/openravepy/openravepy_robotbase.h +++ b/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(); diff --git a/python/bindings/include/openravepy/openravepy_sensorbase.h b/python/bindings/include/openravepy/openravepy_sensorbase.h index ccc6eac369..639df7071e 100644 --- a/python/bindings/include/openravepy/openravepy_sensorbase.h +++ b/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); diff --git a/python/bindings/openravepy_ikparameterization.cpp b/python/bindings/openravepy_ikparameterization.cpp index 85deedf7ef..ea08921c5a 100644 --- a/python/bindings/openravepy_ikparameterization.cpp +++ b/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 >(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)) diff --git a/python/bindings/openravepy_robot.cpp b/python/bindings/openravepy_robot.cpp index 8e6c626097..464d5fcfd8 100644 --- a/python/bindings/openravepy_robot.cpp +++ b/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_ >("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)) diff --git a/python/bindings/openravepy_sensor.cpp b/python/bindings/openravepy_sensor.cpp index 2d088a7d1f..121b90c848 100644 --- a/python/bindings/openravepy_sensor.cpp +++ b/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()); } From 4ae6837f12feee18108219979f17166e7bec9ae4 Mon Sep 17 00:00:00 2001 From: Yoshiki Kanemoto Date: Fri, 26 Jan 2024 20:22:30 +0900 Subject: [PATCH 13/16] fixed KinBody::Clone() did not copy _referenceUri --- src/libopenrave/kinbody.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/libopenrave/kinbody.cpp b/src/libopenrave/kinbody.cpp index 3ea9026b18..930f9ae644 100644 --- a/src/libopenrave/kinbody.cpp +++ b/src/libopenrave/kinbody.cpp @@ -5593,6 +5593,7 @@ void KinBody::Clone(InterfaceBaseConstPtr preference, int cloningoptions) _pKinematicsGenerator.reset(); _pCurrentKinematicsFunctions.reset(); _name = r->_name; + _referenceUri = r->_referenceUri; _nHierarchyComputed = r->_nHierarchyComputed; _bMakeJoinedLinksAdjacent = r->_bMakeJoinedLinksAdjacent; __hashKinematicsGeometryDynamics = r->__hashKinematicsGeometryDynamics; From d85a4ce63c68ddaa325adc294eaf8ef84b04cd54 Mon Sep 17 00:00:00 2001 From: Kei Usui Date: Mon, 5 Feb 2024 14:55:14 +0900 Subject: [PATCH 14/16] avoid memory allocation by using generator like class --- include/openrave/trajectory.h | 17 ++++++ src/libopenrave/trajectory.cpp | 106 ++++++++++++++++++++++++++------- 2 files changed, 103 insertions(+), 20 deletions(-) diff --git a/include/openrave/trajectory.h b/include/openrave/trajectory.h index 0a1b323e4e..a741454b53 100644 --- a/include/openrave/trajectory.h +++ b/include/openrave/trajectory.h @@ -24,6 +24,8 @@ namespace OpenRAVE { +template class RangeGenerator; + /** \brief [interface] Encapsulate a time-parameterized trajectories of robot configurations. If not specified, method is not multi-thread safe. \arch_trajectory \ingroup interfaces */ @@ -242,6 +244,21 @@ class OPENRAVE_API TrajectoryBase : public InterfaceBase return boost::static_pointer_cast(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 + void _SamplePointsInRange(std::vector& data, RangeGenerator& 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 + void _SamplePointsInRange(std::vector& data, RangeGenerator& timeRange, const ConfigurationSpecification& spec) const; + private: virtual const char* GetHash() const { return OPENRAVE_TRAJECTORY_HASH; diff --git a/src/libopenrave/trajectory.cpp b/src/libopenrave/trajectory.cpp index 748c9e57c0..7f53078d8e 100644 --- a/src/libopenrave/trajectory.cpp +++ b/src/libopenrave/trajectory.cpp @@ -125,45 +125,111 @@ void TrajectoryBase::SamplePoints(std::vector& data, const std::vector& vtimes, dReal step, dReal start, dReal stop, bool ensureLast) +/// \brief generator of range from start to stop +template +class RangeGenerator { - const dReal duration = stop - start; - const int numPoints = int(ceil(duration / step)); // ceil to make it behave same way as numpy arange(0, duration, step) - vtimes.assign(numPoints, start); - for (int i = 0; i < numPoints; ++i) { - vtimes[i] += step*i; + public: + RangeGenerator(T step, T start, T stop, bool ensureLast) + : _index(0), _numPoints(std::ceil((stop - start)/step)), _start(start), _step(step), _stop(stop), _ensureLast(ensureLast) + { } - if (ensureLast && vtimes.back() < duration) { - vtimes.push_back(duration); + + /// \brief resets current index to start + void Reset() + { + _index = 0; + } + + /// \brief return current value and iterate to next value + /// \return current value + T GetAndIncrement() + { + { + const T value = _start + _step * _index; + const bool lessThanStop = _index < _numPoints; + if (lessThanStop) { + ++_index; + return value; + } + } + if (_index == _numPoints) { + if (_IsPaddedByEnsureLast()) { + ++_index; + return _stop; + } + } + throw std::out_of_range("index=" + std::to_string(_index) + " is out of size=" + std::to_string(GetSize()) + ", ensureLast=" + std::to_string(_ensureLast)); + } + + /// \brief size of range + size_t GetSize() const + { + return _numPoints + _IsPaddedByEnsureLast(); + } + + private: + bool _IsPaddedByEnsureLast() const + { + return _ensureLast && (_start + _step * (_numPoints - 1)) < _stop; + } + + size_t _index; ///< current index, can go up to _numPoints if _ensureLast is true and other conditions are met. + const size_t _numPoints; ///< number of data points. excludes padding added by _ensureLast + const T _start, _step, _stop; ///< defines linearly interpolated range from start to stop separated by step size. + const bool _ensureLast; ///< if true, _stop is guaranteed to be returned by GetAndIncrement +}; + +template +void TrajectoryBase::_SamplePointsInRange(std::vector& data, RangeGenerator& timeRange) const +{ + std::vector tempdata; + int dof = GetConfigurationSpecification().GetDOF(); + data.resize(dof*timeRange.GetSize()); + std::vector::iterator itdata = data.begin(); + for(size_t i = 0; i < timeRange.GetSize(); ++i, itdata += dof) { + const dReal time = timeRange.GetAndIncrement(); + Sample(tempdata, time); + std::copy(tempdata.begin(), tempdata.end(), itdata); + } +} + +template +void TrajectoryBase::_SamplePointsInRange(std::vector& data, RangeGenerator& timeRange, const ConfigurationSpecification& spec) const +{ + std::vector tempdata; + int dof = GetConfigurationSpecification().GetDOF(); + data.resize(dof*timeRange.GetSize()); + std::vector::iterator itdata = data.begin(); + for(size_t i = 0; i < timeRange.GetSize(); ++i, itdata += dof) { + const dReal time = timeRange.GetAndIncrement(); + Sample(tempdata, time, spec); + std::copy(tempdata.begin(), tempdata.end(), itdata); } } void TrajectoryBase::SamplePointsSameDeltaTime(std::vector& data, dReal deltatime, bool ensureLastPoint) const { - std::vector vtimes; - _SampleEvenly(vtimes, deltatime, 0, GetDuration(), ensureLastPoint); - return SamplePoints(data, vtimes); + RangeGenerator timeRange(deltatime, 0, GetDuration(), ensureLastPoint); + return _SamplePointsInRange(data, timeRange); } void TrajectoryBase::SamplePointsSameDeltaTime(std::vector& data, dReal deltatime, bool ensureLastPoint, const ConfigurationSpecification& spec) const { - std::vector vtimes; - _SampleEvenly(vtimes, deltatime, 0, GetDuration(), ensureLastPoint); - return SamplePoints(data, vtimes, spec); + RangeGenerator timeRange(deltatime, 0, GetDuration(), ensureLastPoint); + return _SamplePointsInRange(data, timeRange, spec); } void TrajectoryBase::SampleRangeSameDeltaTime(std::vector& data, dReal deltatime, dReal startTime, dReal stopTime, bool ensureLastPoint) const { - std::vector vtimes; - _SampleEvenly(vtimes, deltatime, startTime, stopTime, ensureLastPoint); - return SamplePoints(data, vtimes); + RangeGenerator timeRange(deltatime, startTime, stopTime, ensureLastPoint); + return _SamplePointsInRange(data, timeRange); } void TrajectoryBase::SampleRangeSameDeltaTime(std::vector& data, dReal deltatime, dReal startTime, dReal stopTime, bool ensureLastPoint, const ConfigurationSpecification& spec) const { - std::vector vtimes; - _SampleEvenly(vtimes, deltatime, startTime, stopTime, ensureLastPoint); - return SamplePoints(data, vtimes, spec); + RangeGenerator timeRange(deltatime, startTime, stopTime, ensureLastPoint); + return _SamplePointsInRange(data, timeRange, spec); } void TrajectoryBase::GetWaypoints(size_t startindex, size_t endindex, std::vector& data, const ConfigurationSpecification& spec) const From 8a9fb7625de722609f5bd3fdb0bb568d8a02251e Mon Sep 17 00:00:00 2001 From: Kei Usui Date: Wed, 7 Feb 2024 09:51:03 +0900 Subject: [PATCH 15/16] make concurrent reads safe by using TLS. Added comments. --- .../bindings/include/openravepy/openravepy_trajectorybase.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/python/bindings/include/openravepy/openravepy_trajectorybase.h b/python/bindings/include/openravepy/openravepy_trajectorybase.h index a52b126d50..a9328c66c6 100644 --- a/python/bindings/include/openravepy/openravepy_trajectorybase.h +++ b/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: @@ -114,7 +116,7 @@ class OPENRAVEPY_API PyTrajectoryBase : public PyInterfaceBase object GetWaypoint(int index, OPENRAVE_SHARED_PTR pygroup) const; private: - mutable std::vector _vdataCache, _vtimesCache; ///< caches to avoid memory allocation + static thread_local std::vector _vdataCache, _vtimesCache; ///< caches to avoid memory allocation. TLS to suppport concurrent data read ( getting waypoint, sampling and so on ) from multiple threads. }; } // namespace openravepy From e1fb7c860c409e3e775cdfc00eb1fb3c7a5cd42c Mon Sep 17 00:00:00 2001 From: Kei Usui Date: Wed, 7 Feb 2024 10:18:30 +0900 Subject: [PATCH 16/16] add definition of static members --- python/bindings/openravepy_trajectory.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/python/bindings/openravepy_trajectory.cpp b/python/bindings/openravepy_trajectory.cpp index e565436fc3..af626842eb 100644 --- a/python/bindings/openravepy_trajectory.cpp +++ b/python/bindings/openravepy_trajectory.cpp @@ -123,6 +123,8 @@ inline int ExtractContiguousArrayToPointer(py::object oContiguousArray, return pointsnum; } +extern thread_local std::vector PyTrajectoryBase::_vdataCache, PyTrajectoryBase::_vtimesCache; + PyTrajectoryBase::PyTrajectoryBase(TrajectoryBasePtr pTrajectory, PyEnvironmentBasePtr pyenv) : PyInterfaceBase(pTrajectory, pyenv),_ptrajectory(pTrajectory) { } PyTrajectoryBase::~PyTrajectoryBase() {