From e96520b659c27aa420fe8c4925b925993a5ef131 Mon Sep 17 00:00:00 2001 From: Shintaro Matsuoka Date: Mon, 28 Mar 2022 14:00:04 +0900 Subject: [PATCH] Adds PositionConfiguration --- include/openrave/kinbody.h | 50 +++ .../include/openravepy/openravepy_jointinfo.h | 46 +++ python/bindings/openravepy_kinbody.cpp | 326 ++++++++++++++++++ src/libopenrave/kinbodyjoint.cpp | 99 ++++++ 4 files changed, 521 insertions(+) diff --git a/include/openrave/kinbody.h b/include/openrave/kinbody.h index dc81de9fad..ae46298c68 100644 --- a/include/openrave/kinbody.h +++ b/include/openrave/kinbody.h @@ -2034,6 +2034,56 @@ class OPENRAVE_API KinBody : public InterfaceBase typedef boost::shared_ptr JointConstPtr; typedef boost::weak_ptr JointWeakPtr; + /// \brief Holds a joint value set representing a KinBody/Robot pose + /// + /// Currently, this structure supports only joints which have single DoF + class OPENRAVE_API PositionConfiguration : public InfoBase + { +public: + PositionConfiguration() = default; + virtual ~PositionConfiguration() = default; + + void Reset() override; + void SerializeJSON(rapidjson::Value& value, rapidjson::Document::AllocatorType& allocator, dReal fUnitScale, int options=0) const override; + void DeserializeJSON(const rapidjson::Value& value, dReal fUnitScale, int options) override; + + bool operator==(const PositionConfiguration& other) const; + bool operator!=(const PositionConfiguration& other) const; + + inline void Swap(PositionConfiguration& rhs) { + _id.swap(rhs._id); + name.swap(rhs.name); + jointConfigurationStates.swap(rhs.jointConfigurationStates); + } + + class JointConfigurationState : public InfoBase + { +public: + JointConfigurationState() = default; + virtual ~JointConfigurationState() = default; + + void Reset() override; + void SerializeJSON(rapidjson::Value& value, rapidjson::Document::AllocatorType& allocator, dReal fUnitScale, int options=0) const override; + void DeserializeJSON(const rapidjson::Value& value, dReal fUnitScale, int options) override; + + bool operator==(const JointConfigurationState& other) const; + bool operator!=(const JointConfigurationState& other) const; + + std::string _id; ///< id of joint configuration state, for incremental update + std::string jointName; ///< name of the joint. If the joint belong to a connectedBody, then it's resolved name is connectedBodyName+"_"+jointName + dReal jointValue = 0.0; + std::string connectedBodyName; ///< the connected body name the jointName comes from + }; + typedef boost::shared_ptr JointConfigurationStatePtr; + typedef boost::shared_ptr JointConfigurationStateConstPtr; + + std::string _id; ///< unique id of the configuration used to identify it when changing it. + std::string name; ///< name of the configuration + std::vector jointConfigurationStates; ///< joint name to joint values mapping + }; + typedef boost::shared_ptr PositionConfigurationPtr; + typedef boost::shared_ptr PositionConfigurationConstPtr; + /// \brief holds all user-set attached sensor information used to initialize the AttachedSensor class. /// /// This is serializable and independent of environment. diff --git a/python/bindings/include/openravepy/openravepy_jointinfo.h b/python/bindings/include/openravepy/openravepy_jointinfo.h index 24a803705a..7052330204 100644 --- a/python/bindings/include/openravepy/openravepy_jointinfo.h +++ b/python/bindings/include/openravepy/openravepy_jointinfo.h @@ -490,6 +490,52 @@ class PyJoint int __hash__(); }; +class PyPositionConfiguration_JointConfigurationState +{ +public: + PyPositionConfiguration_JointConfigurationState() = default; + PyPositionConfiguration_JointConfigurationState(const KinBody::PositionConfiguration::JointConfigurationState& jointConfigurationState); + + KinBody::PositionConfiguration::JointConfigurationStatePtr GetJointConfigurationState() const; + object SerializeJSON(dReal fUnitScale = 1.0, object options = py::none_()); + void DeserializeJSON(object obj, dReal fUnitScale = 1.0, object options = py::none_()); + + std::string __str__(); + bool __eq__(OPENRAVE_SHARED_PTR p); + bool __ne__(OPENRAVE_SHARED_PTR p); + + object _id = py::none_(); + object jointName = py::none_(); + dReal jointValue = 0.0; + object connectedBodyName = py::none_(); + +private: + void _Update(const KinBody::PositionConfiguration::JointConfigurationState& jointConfigurationState); +}; +typedef OPENRAVE_SHARED_PTR PyPositionConfiguration_JointConfigurationStatePtr; + +class PyPositionConfiguration +{ +public: + PyPositionConfiguration() = default; + PyPositionConfiguration(const KinBody::PositionConfiguration& positionConfiguration); + + KinBody::PositionConfigurationPtr GetPositionConfiguration() const; + object SerializeJSON(dReal fUnitScale = 1.0, object options = py::none_()); + void DeserializeJSON(object obj, dReal fUnitScale = 1.0, object options = py::none_()); + + std::string __str__(); + bool __eq__(OPENRAVE_SHARED_PTR p); + bool __ne__(OPENRAVE_SHARED_PTR p); + + object _id = py::none_(); + object name = py::none_(); + py::list jointConfigurationStates; + +private: + void _Update(const KinBody::PositionConfiguration& positionConfiguration); +}; + class PyKinBodyStateSaver { PyEnvironmentBasePtr _pyenv; diff --git a/python/bindings/openravepy_kinbody.cpp b/python/bindings/openravepy_kinbody.cpp index a56a3a3d95..590844b822 100644 --- a/python/bindings/openravepy_kinbody.cpp +++ b/python/bindings/openravepy_kinbody.cpp @@ -2071,6 +2071,167 @@ PyGeometryInfoPtr toPyGeometryInfo(const KinBody::GeometryInfo& geominfo) return PyGeometryInfoPtr(new PyGeometryInfo(geominfo)); } +PyPositionConfiguration::PyPositionConfiguration(const KinBody::PositionConfiguration& positionConfiguration) +{ + _Update(positionConfiguration); +} + +KinBody::PositionConfigurationPtr PyPositionConfiguration::GetPositionConfiguration() const +{ + KinBody::PositionConfigurationPtr positionConfiguration(new KinBody::PositionConfiguration()); + if( !IS_PYTHONOBJECT_NONE(_id) ) { + positionConfiguration->_id = py::extract(_id); + } + if( !IS_PYTHONOBJECT_NONE(name) ) { + positionConfiguration->name = py::extract(name); + } + positionConfiguration->jointConfigurationStates.resize(len(jointConfigurationStates)); + for( int jointConfigurationIndex = 0; jointConfigurationIndex < len(jointConfigurationStates); ++jointConfigurationIndex ) { + PyPositionConfiguration_JointConfigurationStatePtr pyJointConfigurationState = py::extract(jointConfigurationStates[jointConfigurationIndex]); + positionConfiguration->jointConfigurationStates[jointConfigurationIndex] = *(pyJointConfigurationState->GetJointConfigurationState()); + } + return positionConfiguration; +} + +object PyPositionConfiguration::SerializeJSON(dReal fUnitScale, object options) +{ + rapidjson::Document doc; + KinBody::PositionConfigurationPtr positionConfiguration = GetPositionConfiguration(); + positionConfiguration->SerializeJSON(doc, doc.GetAllocator(), fUnitScale, pyGetIntFromPy(options, 0)); + return toPyObject(doc); +} + +void PyPositionConfiguration::DeserializeJSON(object obj, dReal fUnitScale, object options) +{ + rapidjson::Document doc; + toRapidJSONValue(obj, doc, doc.GetAllocator()); + KinBody::PositionConfiguration positionConfiguration; + positionConfiguration.DeserializeJSON(doc, fUnitScale, pyGetIntFromPy(options, 0)); + _Update(positionConfiguration); +} + +std::string PyPositionConfiguration::__str__() +{ + std::stringstream ss; + ss << "(name) << "\""; + } + ss << " jointConfigurationStates=["; + for( int jointConfigurationIndex = 0; jointConfigurationIndex < len(jointConfigurationStates); ++jointConfigurationIndex ) { + PyPositionConfiguration_JointConfigurationStatePtr pyJointConfigurationState = py::extract(jointConfigurationStates[jointConfigurationIndex]); + if( jointConfigurationIndex > 0 ) { + ss << ", "; + } + ss << pyJointConfigurationState->__str__(); + } + ss << "]>"; + return ss.str(); +} + +bool PyPositionConfiguration::__eq__(OPENRAVE_SHARED_PTR p) +{ + if( !p ) { + return false; + } + KinBody::PositionConfigurationConstPtr positionConfiguration0 = GetPositionConfiguration(); + KinBody::PositionConfigurationConstPtr positionConfiguration1 = p->GetPositionConfiguration(); + return *positionConfiguration0 == *positionConfiguration1; +} + +bool PyPositionConfiguration::__ne__(OPENRAVE_SHARED_PTR p) +{ + return !__eq__(p); +} + +void PyPositionConfiguration::_Update(const KinBody::PositionConfiguration& positionConfiguration) +{ + _id = ConvertStringToUnicode(positionConfiguration._id); + name = ConvertStringToUnicode(positionConfiguration.name); + + py::list newJointConfigurationStates; + for( const KinBody::PositionConfiguration::JointConfigurationState& cppJointConfiguration : positionConfiguration.jointConfigurationStates ) { + newJointConfigurationStates.append(PyPositionConfiguration_JointConfigurationStatePtr(new PyPositionConfiguration_JointConfigurationState(cppJointConfiguration))); + } + jointConfigurationStates = newJointConfigurationStates; +} + +PyPositionConfiguration_JointConfigurationState::PyPositionConfiguration_JointConfigurationState(const KinBody::PositionConfiguration::JointConfigurationState& jointConfigurationState) +{ + _Update(jointConfigurationState); +} + +KinBody::PositionConfiguration::JointConfigurationStatePtr PyPositionConfiguration_JointConfigurationState::GetJointConfigurationState() const +{ + KinBody::PositionConfiguration::JointConfigurationStatePtr jointConfigurationState(new KinBody::PositionConfiguration::JointConfigurationState()); + if( !IS_PYTHONOBJECT_NONE(_id) ) { + jointConfigurationState->_id = py::extract(_id); + } + if( !IS_PYTHONOBJECT_NONE(jointName) ) { + jointConfigurationState->jointName = py::extract(jointName); + } + jointConfigurationState->jointValue = jointValue; + if( !IS_PYTHONOBJECT_NONE(connectedBodyName) ) { + jointConfigurationState->connectedBodyName = py::extract(connectedBodyName); + } + return jointConfigurationState; +} + +object PyPositionConfiguration_JointConfigurationState::SerializeJSON(dReal fUnitScale, object options) +{ + rapidjson::Document doc; + KinBody::PositionConfiguration::JointConfigurationStatePtr jointConfigurationState = GetJointConfigurationState(); + jointConfigurationState->SerializeJSON(doc, doc.GetAllocator(), fUnitScale, pyGetIntFromPy(options, 0)); + return toPyObject(doc); +} + +void PyPositionConfiguration_JointConfigurationState::DeserializeJSON(object obj, dReal fUnitScale, object options) +{ + rapidjson::Document doc; + toRapidJSONValue(obj, doc, doc.GetAllocator()); + KinBody::PositionConfiguration::JointConfigurationState jointConfigurationState; + jointConfigurationState.DeserializeJSON(doc, fUnitScale, pyGetIntFromPy(options, 0)); + _Update(jointConfigurationState); +} + +std::string PyPositionConfiguration_JointConfigurationState::__str__() +{ + std::stringstream ss; + ss << "(jointName); + } + ss << "\" jointValue=" << jointValue << " connectedBodyName=\""; + if( !IS_PYTHONOBJECT_NONE(connectedBodyName) ) { + ss << (const std::string&) py::extract(connectedBodyName); + } + ss << "\">"; + return ss.str(); +} + +bool PyPositionConfiguration_JointConfigurationState::__eq__(OPENRAVE_SHARED_PTR p) +{ + if( !p ) { + return false; + } + KinBody::PositionConfiguration::JointConfigurationStateConstPtr jointConfigurationState0 = GetJointConfigurationState(); + KinBody::PositionConfiguration::JointConfigurationStateConstPtr jointConfigurationState1 = p->GetJointConfigurationState(); + return *jointConfigurationState0 == *jointConfigurationState1; +} + +bool PyPositionConfiguration_JointConfigurationState::__ne__(OPENRAVE_SHARED_PTR p) +{ + return !__eq__(p); +} + +void PyPositionConfiguration_JointConfigurationState::_Update(const KinBody::PositionConfiguration::JointConfigurationState& jointConfigurationState) +{ + _id = ConvertStringToUnicode(jointConfigurationState._id); + jointName = ConvertStringToUnicode(jointConfigurationState.jointName); + jointValue = jointConfigurationState.jointValue; + connectedBodyName = ConvertStringToUnicode(jointConfigurationState.connectedBodyName); +} + PyKinBodyStateSaver::PyKinBodyStateSaver(PyKinBodyPtr pybody) : _pyenv(pybody->GetEnv()), _state(pybody->GetBody()) { // python should not support restoring on destruction since there's garbage collection _state.SetRestoreOnDestructor(false); @@ -4429,6 +4590,56 @@ class JointInfo_pickle_suite } }; +class PositionConfiguration_JointConfigurationState_pickle_suite +#ifndef USE_PYBIND11_PYTHON_BINDINGS + : public pickle_suite +#endif +{ +public: + static py::tuple getstate(const PyPositionConfiguration_JointConfigurationState& r) + { + return py::make_tuple(r._id, r.jointName, r.jointValue, r.connectedBodyName); + } + static void setstate(PyPositionConfiguration_JointConfigurationState& r, py::tuple state) { + r._id = state[0]; + r.jointName = state[1]; + r.jointValue = py::extract(state[2]); + r.connectedBodyName = state[3]; + } +}; + +class PositionConfiguration_pickle_suite +#ifndef USE_PYBIND11_PYTHON_BINDINGS + : public pickle_suite +#endif +{ +public: + static py::tuple getstate(const PyPositionConfiguration& r) + { + py::list pickledJointConfigurationStates; + for( int jointConfigurationIndex = 0; jointConfigurationIndex < len(r.jointConfigurationStates); ++jointConfigurationIndex ) { + PyPositionConfiguration_JointConfigurationStatePtr pyJointConfigurationState = py::extract(r.jointConfigurationStates[jointConfigurationIndex]); + pickledJointConfigurationStates.append(PositionConfiguration_JointConfigurationState_pickle_suite::getstate(*pyJointConfigurationState)); + } + + return py::make_tuple(r._id, r.name, pickledJointConfigurationStates); + } + static void setstate(PyPositionConfiguration& r, py::tuple state) { + r._id = state[0]; + r.name = state[1]; + + py::list pickledJointConfigurationStateList = py::list(state[2]); + py::list newJointConfigurationStates; + for( int jointConfigurationIndex = 0; jointConfigurationIndex < len(pickledJointConfigurationStateList); ++jointConfigurationIndex ) { + PyPositionConfiguration_JointConfigurationStatePtr pyJointConfigurationState(new PyPositionConfiguration_JointConfigurationState()); + PositionConfiguration_JointConfigurationState_pickle_suite::setstate(*pyJointConfigurationState, py::tuple(pickledJointConfigurationStateList[jointConfigurationIndex])); + newJointConfigurationStates.append(pyJointConfigurationState); + } + + r.jointConfigurationStates = newJointConfigurationStates; + } +}; + class GrabbedInfo_pickle_suite #ifndef USE_PYBIND11_PYTHON_BINDINGS : public pickle_suite @@ -4503,6 +4714,8 @@ BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(PyElectricMotorActuatorInfo_SerializeJSON BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(PyGeometryInfo_SerializeJSON_overloads, SerializeJSON, 0, 2) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(PyLinkInfo_SerializeJSON_overloads, SerializeJSON, 0, 2) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(PyJointInfo_SerializeJSON_overloads, SerializeJSON, 0, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(PyPositionConfiguration_SerializeJSON_overloads, SerializeJSON, 0, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(PyPositionConfiguration_JointConfigurationState_SerializeJSON_overloads, SerializeJSON, 0, 2) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(PyGrabbedInfo_SerializeJSON_overloads, SerializeJSON, 0, 2) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(PyKinBodyInfo_SerializeJSON_overloads, SerializeJSON, 0, 2) // DeserializeJSON @@ -4510,6 +4723,8 @@ BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(PyElectricMotorActuatorInfo_DeserializeJS BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(PyGeometryInfo_DeserializeJSON_overloads, DeserializeJSON, 1, 3) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(PyLinkInfo_DeserializeJSON_overloads, DeserializeJSON, 1, 3) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(PyJointInfo_DeserializeJSON_overloads, DeserializeJSON, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(PyPositionConfiguration_DeserializeJSON_overloads, DeserializeJSON, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(PyPositionConfiguration_JointConfigurationState_DeserializeJSON_overloads, DeserializeJSON, 1, 3) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(PyGrabbedInfo_DeserializeJSON_overloads, DeserializeJSON, 1, 3) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(PyKinBodyInfo_DeserializeJSON_overloads, DeserializeJSON, 1, 3) // end of JSON @@ -5030,6 +5245,117 @@ void init_openravepy_kinbody() #endif ; +#ifdef USE_PYBIND11_PYTHON_BINDINGS + object positionconfiguration_jointconfigurationstate = class_ >(m, "PositionConfiguration_JointConfigurationState", DOXY_CLASS(KinBody::PositionConfiguration::JointConfigurationState)) + .def(init<>()) +#else + object positionconfiguration_jointconfigurationstate = class_ >("PositionConfiguration_JointConfigurationState", DOXY_CLASS(KinBody::PositionConfiguration::JointConfigurationState)) +#endif + .def_readwrite("_id", &PyPositionConfiguration_JointConfigurationState::_id) + .def_readwrite("jointName",&PyPositionConfiguration_JointConfigurationState::jointName) + .def_readwrite("jointValue",&PyPositionConfiguration_JointConfigurationState::jointValue) + .def_readwrite("connectedBodyName",&PyPositionConfiguration_JointConfigurationState::connectedBodyName) + .def("__str__", &PyPositionConfiguration_JointConfigurationState::__str__) + .def("__eq__", &PyPositionConfiguration_JointConfigurationState::__eq__) + .def("__ne__", &PyPositionConfiguration_JointConfigurationState::__ne__) +#ifdef USE_PYBIND11_PYTHON_BINDINGS + .def("SerializeJSON", &PyPositionConfiguration_JointConfigurationState::SerializeJSON, + "unitScale"_a = 1.0, + "options"_a = py::none_(), + DOXY_FN(KinBody::PositionConfiguration::JointConfigurationState, SerializeJSON) + ) + .def("DeserializeJSON", &PyPositionConfiguration_JointConfigurationState::DeserializeJSON, + "obj"_a, + "unitScale"_a = 1.0, + "options"_a = py::none_(), + DOXY_FN(KinBody::PositionConfiguration::JointConfigurationState, DeserializeJSON) + ) +#else + .def("SerializeJSON", &PyPositionConfiguration_JointConfigurationState::SerializeJSON, PyPositionConfiguration_JointConfigurationState_SerializeJSON_overloads(PY_ARGS("unitScale", "options") DOXY_FN(KinBody::PositionConfiguration::JointConfigurationState, SerializeJSON))) + .def("DeserializeJSON", &PyPositionConfiguration_JointConfigurationState::DeserializeJSON, PyPositionConfiguration_JointConfigurationState_DeserializeJSON_overloads(PY_ARGS("obj", "unitScale", "options") DOXY_FN(KinBody::PositionConfiguration::JointConfigurationState, DeserializeJSON))) +#endif +#ifdef USE_PYBIND11_PYTHON_BINDINGS + .def(py::pickle( + [](const PyPositionConfiguration_JointConfigurationState &pyJointConfigurationState) { + // __getstate__ + return PositionConfiguration_JointConfigurationState_pickle_suite::getstate(pyJointConfigurationState); + }, + [](py::tuple state) { + PyPositionConfiguration_JointConfigurationState &pyJointConfigurationState; + PositionConfiguration_JointConfigurationState_pickle_suite::setstate(pyJointConfigurationState, state); + return pyJointConfigurationState; + })) + .def("__copy__", + [](const PyPositionConfiguration_JointConfigurationState& self) { + return self; + }) + .def("__deepcopy__", + [](const PyPositionConfiguration_JointConfigurationState &pyJointConfigurationState, const py::dict& memo) { + auto state = PositionConfiguration_JointConfigurationState_pickle_suite::getstate(pyJointConfigurationState); + PyPositionConfiguration_JointConfigurationState pyJointConfigurationState_new; + PositionConfiguration_JointConfigurationState_pickle_suite::setstate(pyJointConfigurationState_new, state); + return pyJointConfigurationState_new; + }) +#else + .def_pickle(PositionConfiguration_JointConfigurationState_pickle_suite()) +#endif + ; + +#ifdef USE_PYBIND11_PYTHON_BINDINGS + object positionconfiguration = class_ >(m, "PositionConfiguration", DOXY_CLASS(KinBody::PositionConfiguration)) + .def(init<>()) +#else + object positionconfiguration = class_ >("PositionConfiguration", DOXY_CLASS(KinBody::PositionConfiguration)) +#endif + .def_readwrite("_id", &PyPositionConfiguration::_id) + .def_readwrite("name",&PyPositionConfiguration::name) + .def_readwrite("jointConfigurationStates",&PyPositionConfiguration::jointConfigurationStates) + .def("__str__", &PyPositionConfiguration::__str__) + .def("__eq__", &PyPositionConfiguration::__eq__) + .def("__ne__", &PyPositionConfiguration::__ne__) +#ifdef USE_PYBIND11_PYTHON_BINDINGS + .def("SerializeJSON", &PyPositionConfiguration::SerializeJSON, + "unitScale"_a = 1.0, + "options"_a = py::none_(), + DOXY_FN(KinBody::PositionConfiguration, SerializeJSON) + ) + .def("DeserializeJSON", &PyPositionConfiguration::DeserializeJSON, + "obj"_a, + "unitScale"_a = 1.0, + "options"_a = py::none_(), + DOXY_FN(KinBody::PositionConfiguration, DeserializeJSON) + ) +#else + .def("SerializeJSON", &PyPositionConfiguration::SerializeJSON, PyPositionConfiguration_SerializeJSON_overloads(PY_ARGS("unitScale", "options") DOXY_FN(KinBody::PositionConfiguration, SerializeJSON))) + .def("DeserializeJSON", &PyPositionConfiguration::DeserializeJSON, PyPositionConfiguration_DeserializeJSON_overloads(PY_ARGS("obj", "unitScale", "options") DOXY_FN(KinBody::PositionConfiguration, DeserializeJSON))) +#endif +#ifdef USE_PYBIND11_PYTHON_BINDINGS + .def(py::pickle( + [](const PyPositionConfiguration &pyPositionConfiguration) { + // __getstate__ + return PositionConfiguration_pickle_suite::getstate(pyPositionConfiguration); + }, + [](py::tuple state) { + PyPositionConfiguration &pyPositionConfiguration; + PositionConfiguration_pickle_suite::setstate(pyPositionConfiguration, state); + return pyPositionConfiguration; + })) + .def("__copy__", + [](const PyPositionConfiguration& self) { + return self; + }) + .def("__deepcopy__", + [](const PyPositionConfiguration &pyPositionConfiguration, const py::dict& memo) { + auto state = PositionConfiguration_pickle_suite::getstate(pyPositionConfiguration); + PyPositionConfiguration pyPositionConfiguration_new; + PositionConfiguration_pickle_suite::setstate(pyPositionConfiguration_new, state); + return pyPositionConfiguration_new; + }) +#else + .def_pickle(PositionConfiguration_pickle_suite()) +#endif + ; + #ifdef USE_PYBIND11_PYTHON_BINDINGS object grabbedinfo = class_ >(m, "GrabbedInfo", DOXY_CLASS(KinBody::GrabbedInfo)) .def(init<>()) diff --git a/src/libopenrave/kinbodyjoint.cpp b/src/libopenrave/kinbodyjoint.cpp index 8e1965a303..fb4c02f1ac 100644 --- a/src/libopenrave/kinbodyjoint.cpp +++ b/src/libopenrave/kinbodyjoint.cpp @@ -2854,5 +2854,104 @@ void KinBody::MimicInfo::DeserializeJSON(const rapidjson::Value& value, dReal fU orjson::LoadJsonValueByKey(value, "equations", _equations); } +void KinBody::PositionConfiguration::Reset() +{ + _id.clear(); + name.clear(); + jointConfigurationStates.clear(); +} + +void KinBody::PositionConfiguration::SerializeJSON(rapidjson::Value& value, rapidjson::Document::AllocatorType& allocator, dReal fUnitScale, int options) const +{ + if( !value.IsObject() ) { + value.SetObject(); + } + + rapidjson::Value rJointConfigurationStates; + rJointConfigurationStates.SetArray(); + rJointConfigurationStates.Reserve(jointConfigurationStates.size(), allocator); + for( const JointConfigurationState& jointConfigurationState : jointConfigurationStates ) { + rapidjson::Value rJointConfigurationState; + rJointConfigurationState.SetObject(); + jointConfigurationState.SerializeJSON(rJointConfigurationState, allocator, fUnitScale, options); + rJointConfigurationStates.PushBack(rJointConfigurationState, allocator); + } + + if( !_id.empty() ) { + OpenRAVE::orjson::SetJsonValueByKey(value, "id", _id, allocator); + } + OpenRAVE::orjson::SetJsonValueByKey(value, "name", name, allocator); + OpenRAVE::orjson::SetJsonValueByKey(value, "jointConfigurationStates", rJointConfigurationStates, allocator); +} + +void KinBody::PositionConfiguration::DeserializeJSON(const rapidjson::Value& value, dReal fUnitScale, int options) +{ + Reset(); + OpenRAVE::orjson::LoadJsonValueByKey(value, "id", _id); + OpenRAVE::orjson::LoadJsonValueByKey(value, "name", name); + + if( value.HasMember("jointConfigurationStates") && value["jointConfigurationStates"].IsArray() ) { + jointConfigurationStates.reserve(value["jointConfigurationStates"].GetArray().Size()); + for( const rapidjson::Value& rJointConfigurationState : value["jointConfigurationStates"].GetArray() ) { + if( rJointConfigurationState.HasMember("jointName") && rJointConfigurationState.HasMember("jointValue") ) { + JointConfigurationState jointConfigurationState = JointConfigurationState(); + jointConfigurationState.DeserializeJSON(rJointConfigurationState, fUnitScale, options); + jointConfigurationStates.push_back(jointConfigurationState); + } + } + } +} + +bool KinBody::PositionConfiguration::operator==(const PositionConfiguration& other) const +{ + return _id == other._id + && name == other.name + && jointConfigurationStates == other.jointConfigurationStates; +} + +bool KinBody::PositionConfiguration::operator!=(const PositionConfiguration& other) const +{ + return !operator==(other); +} + +void KinBody::PositionConfiguration::JointConfigurationState::Reset() +{ + _id.clear(); + jointName.clear(); + jointValue = 0.0; + connectedBodyName.clear(); +} + +void KinBody::PositionConfiguration::JointConfigurationState::SerializeJSON(rapidjson::Value& value, rapidjson::Document::AllocatorType& allocator, dReal fUnitScale, int options) const +{ + if( !_id.empty() ) { + OpenRAVE::orjson::SetJsonValueByKey(value, "id", _id, allocator); + } + OpenRAVE::orjson::SetJsonValueByKey(value, "jointName", jointName, allocator); + OpenRAVE::orjson::SetJsonValueByKey(value, "jointValue", jointValue, allocator); + OpenRAVE::orjson::SetJsonValueByKey(value, "connectedBodyName", connectedBodyName, allocator); +} + +void KinBody::PositionConfiguration::JointConfigurationState::DeserializeJSON(const rapidjson::Value& value, dReal fUnitScale, int options) +{ + Reset(); + OpenRAVE::orjson::LoadJsonValueByKey(value, "id", _id); + OpenRAVE::orjson::LoadJsonValueByKey(value, "jointName", jointName); + OpenRAVE::orjson::LoadJsonValueByKey(value, "jointValue", jointValue); + OpenRAVE::orjson::LoadJsonValueByKey(value, "connectedBodyName", connectedBodyName); +} + +bool KinBody::PositionConfiguration::JointConfigurationState::operator==(const KinBody::PositionConfiguration::JointConfigurationState& other) const +{ + static constexpr dReal jointValueEpsilon = 1.0e-6; + return jointName == other.jointName + && connectedBodyName == other.connectedBodyName + && OpenRAVE::RaveFabs(jointValue - other.jointValue) < jointValueEpsilon; +} + +bool KinBody::PositionConfiguration::JointConfigurationState::operator!=(const KinBody::PositionConfiguration::JointConfigurationState& other) const +{ + return !operator==(other); +} }