diff --git a/CMakeLists.txt b/CMakeLists.txt index 7e27bd81ff..edb9f2b68d 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 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}) diff --git a/docs/source/changelog.rst b/docs/source/changelog.rst index 2eba356bc5..0941904cf1 100644 --- a/docs/source/changelog.rst +++ b/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 =============== diff --git a/include/openrave/trajectory.h b/include/openrave/trajectory.h index 7e620c941f..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 */ @@ -133,6 +135,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 @@ -220,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/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_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/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/include/openravepy/openravepy_trajectorybase.h b/python/bindings/include/openravepy/openravepy_trajectorybase.h index 59c92cb863..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: @@ -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 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; 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 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_kinbody.cpp b/python/bindings/openravepy_kinbody.cpp index 62b0538e58..660f325ac4 100644 --- a/python/bindings/openravepy_kinbody.cpp +++ b/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_, 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)) @@ -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_, 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, 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 2de04ada28..1a1fa339b4 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()); } diff --git a/python/bindings/openravepy_trajectory.cpp b/python/bindings/openravepy_trajectory.cpp index 7711bb83d2..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() { @@ -300,13 +302,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 +318,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 +337,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 +348,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 +685,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 +730,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..4a9e119a20 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,79 @@ 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); + 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); + if( IS_DEBUGLEVEL(Level_Verbose) || (RaveGetDebugLevel() & Level_VerifyPlans) ) { + _VerifySampling(); + } + + const dReal trajDuration = GetDuration(); + 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..7f53078d8e 100644 --- a/src/libopenrave/trajectory.cpp +++ b/src/libopenrave/trajectory.cpp @@ -125,32 +125,111 @@ void TrajectoryBase::SamplePoints(std::vector& data, const std::vector& data, dReal deltatime, bool ensureLastPoint) const +/// \brief generator of range from start to stop +template +class RangeGenerator { - 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; + 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 (ensureLastPoint && vtimes.back() < duration) { - vtimes.push_back(duration); + + /// \brief resets current index to start + void Reset() + { + _index = 0; } - return SamplePoints(data, vtimes); -} -void TrajectoryBase::SamplePointsSameDeltaTime(std::vector& data, dReal deltatime, bool ensureLastPoint, const ConfigurationSpecification& spec) const + /// \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 { - 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; + 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); } - if (ensureLastPoint && vtimes.back() < duration) { - vtimes.push_back(duration); +} + +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); } - return SamplePoints(data, vtimes, spec); +} + +void TrajectoryBase::SamplePointsSameDeltaTime(std::vector& data, dReal deltatime, bool ensureLastPoint) const +{ + RangeGenerator timeRange(deltatime, 0, GetDuration(), ensureLastPoint); + return _SamplePointsInRange(data, timeRange); +} + +void TrajectoryBase::SamplePointsSameDeltaTime(std::vector& data, dReal deltatime, bool ensureLastPoint, const ConfigurationSpecification& spec) const +{ + 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 +{ + 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 +{ + 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