Skip to content

Commit

Permalink
new api SampleRangeSameDeltaTime for uniform sampling of given range
Browse files Browse the repository at this point in the history
  • Loading branch information
Kei Usui committed Jan 10, 2024
1 parent dd0d774 commit 4b8ebe8
Show file tree
Hide file tree
Showing 5 changed files with 212 additions and 94 deletions.
22 changes: 22 additions & 0 deletions include/openrave/trajectory.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -133,6 +133,28 @@ class OPENRAVE_API TrajectoryBase : public InterfaceBase
*/
virtual void SamplePointsSameDeltaTime(std::vector<dReal>& data, dReal deltatime, bool ensureLastPoint, const ConfigurationSpecification& spec) const;

/** \brief bulk samples the trajectory evenly given a delta time and a specific configuration specification.
The default implementation is slow, so interface developers should override it.
\param data[out] the sampled points for every time entry.
\param deltatime[in] the delta time to sample
\param startTime[in] start time to sample from
\param stopTime[in] stop time to sample to
\param ensureLastPoint[in] if true, data at duration of trajectory is sampled
*/
virtual void SampleRangeSameDeltaTime(std::vector<dReal>& data, dReal deltatime, dReal startTime, dReal stopTime, bool ensureLastPoint) const;

/** \brief bulk samples the trajectory evenly given a delta time and a specific configuration specification.
The default implementation is slow, so interface developers should override it.
\param data[out] the sampled points for every time entry.
\param deltatime[in] the delta time to sample
\param startTime[in] start time to sample from
\param stopTime[in] stop time to sample to
\param ensureLastPoint[in] if true, data at duration of trajectory is sampled
\param spec[in] the specification format to return the data in
*/
virtual void SampleRangeSameDeltaTime(std::vector<dReal>& data, dReal deltatime, dReal startTime, dReal stopTime, bool ensureLastPoint, const ConfigurationSpecification& spec) const;
virtual const ConfigurationSpecification& GetConfigurationSpecification() const = 0;

/// \brief return the number of waypoints
Expand Down
5 changes: 5 additions & 0 deletions python/bindings/include/openravepy/openravepy_trajectorybase.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -103,6 +107,7 @@ class OPENRAVEPY_API PyTrajectoryBase : public PyInterfaceBase
object SampleFromPrevious(object odata, dReal time, OPENRAVE_SHARED_PTR<ConfigurationSpecification::Group> pygroup) const;
object SamplePoints2D(object otimes, OPENRAVE_SHARED_PTR<ConfigurationSpecification::Group> pygroup) const;
object SamplePointsSameDeltaTime2D(dReal deltatime, bool ensureLastPoint, OPENRAVE_SHARED_PTR<ConfigurationSpecification::Group> pygroup) const;
object SampleRangeSameDeltaTime2D(dReal deltatime, dReal startTime, dReal stopTime, bool ensureLastPoint, OPENRAVE_SHARED_PTR<ConfigurationSpecification::Group> pygroup) const;
object GetWaypoints(size_t startindex, size_t endindex, OPENRAVE_SHARED_PTR<ConfigurationSpecification::Group> pygroup) const;
object GetWaypoints2D(size_t startindex, size_t endindex, OPENRAVE_SHARED_PTR<ConfigurationSpecification::Group> pygroup) const;
object GetAllWaypoints2D(OPENRAVE_SHARED_PTR<ConfigurationSpecification::Group> pygroup) const;
Expand Down
74 changes: 56 additions & 18 deletions python/bindings/openravepy_trajectory.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -300,13 +300,8 @@ object PyTrajectoryBase::SamplePoints2D(object otimes, PyConfigurationSpecificat
}


object PyTrajectoryBase::SamplePointsSameDeltaTime2D(dReal deltatime,
bool ensureLastPoint) const
object _ConvertToObject(const std::vector<dReal>& values, int numdof)
{
std::vector<dReal>& values = _vdataCache;
_ptrajectory->SamplePointsSameDeltaTime(values, deltatime, ensureLastPoint);

const int numdof = _ptrajectory->GetConfigurationSpecification().GetDOF();
#ifdef USE_PYBIND11_PYTHON_BINDINGS
py::array_t<dReal> pypos = toPyArray(values);
pypos.resize({(int) values.size()/numdof, numdof});
Expand All @@ -321,6 +316,15 @@ object PyTrajectoryBase::SamplePointsSameDeltaTime2D(dReal deltatime,
#endif // USE_PYBIND11_PYTHON_BINDINGS
}

object PyTrajectoryBase::SamplePointsSameDeltaTime2D(dReal deltatime,
bool ensureLastPoint) const
{
std::vector<dReal>& values = _vdataCache;
_ptrajectory->SamplePointsSameDeltaTime(values, deltatime, ensureLastPoint);
const int numdof = _ptrajectory->GetConfigurationSpecification().GetDOF();
return _ConvertToObject(values, numdof);
}


object PyTrajectoryBase::SamplePointsSameDeltaTime2D(dReal deltatime,
bool ensureLastPoint,
Expand All @@ -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<dReal> 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<dReal>(pypos);
#endif // USE_PYBIND11_PYTHON_BINDINGS
return _ConvertToObject(values, numdof);
}

object PyTrajectoryBase::SamplePointsSameDeltaTime2D(dReal deltatime,
Expand All @@ -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<dReal>& 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<dReal>& 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<ConfigurationSpecification::Group> pygroup) const
{
PyConfigurationSpecificationPtr pyspec(new PyConfigurationSpecification(*pygroup));
return this->SampleRangeSameDeltaTime2D(deltatime, startTime, stopTime, ensureLastPoint, pyspec);
}

object PyTrajectoryBase::SamplePoints2D(object otimes, OPENRAVE_SHARED_PTR<ConfigurationSpecification::Group> pygroup) const
{
PyConfigurationSpecificationPtr pyspec(new PyConfigurationSpecification(*pygroup));
Expand Down Expand Up @@ -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<ConfigurationSpecification::Group>) 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<ConfigurationSpecification::Group>) 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<ConfigurationSpecification::Group>) const = &PyTrajectoryBase::GetWaypoints;
Expand Down Expand Up @@ -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))
Expand Down
Loading

0 comments on commit 4b8ebe8

Please sign in to comment.