From e96520b659c27aa420fe8c4925b925993a5ef131 Mon Sep 17 00:00:00 2001 From: Shintaro Matsuoka Date: Mon, 28 Mar 2022 14:00:04 +0900 Subject: [PATCH 01/18] 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); +} } From b0417b6c5069a1fe95b9813273247941de5cdc04 Mon Sep 17 00:00:00 2001 From: Shintaro Matsuoka Date: Mon, 28 Mar 2022 14:05:30 +0900 Subject: [PATCH 02/18] Skip serialising JointConfigurationState::ConnectedBodyName when empty --- src/libopenrave/kinbodyjoint.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/libopenrave/kinbodyjoint.cpp b/src/libopenrave/kinbodyjoint.cpp index fb4c02f1ac..4c05f50006 100644 --- a/src/libopenrave/kinbodyjoint.cpp +++ b/src/libopenrave/kinbodyjoint.cpp @@ -2929,7 +2929,9 @@ void KinBody::PositionConfiguration::JointConfigurationState::SerializeJSON(rapi } OpenRAVE::orjson::SetJsonValueByKey(value, "jointName", jointName, allocator); OpenRAVE::orjson::SetJsonValueByKey(value, "jointValue", jointValue, allocator); - OpenRAVE::orjson::SetJsonValueByKey(value, "connectedBodyName", connectedBodyName, allocator); + if( !connectedBodyName.empty() ) { + OpenRAVE::orjson::SetJsonValueByKey(value, "connectedBodyName", connectedBodyName, allocator); + } } void KinBody::PositionConfiguration::JointConfigurationState::DeserializeJSON(const rapidjson::Value& value, dReal fUnitScale, int options) From 812186d00d4f993de501fb66d0a2c02d3c2f97ba Mon Sep 17 00:00:00 2001 From: Shintaro Matsuoka Date: Mon, 28 Mar 2022 18:36:29 +0900 Subject: [PATCH 03/18] Adds JointConfigurationState.jointAxis --- include/openrave/kinbody.h | 3 +-- .../include/openravepy/openravepy_jointinfo.h | 1 + python/bindings/openravepy_kinbody.cpp | 12 ++++++++---- src/libopenrave/kinbodyjoint.cpp | 4 ++++ 4 files changed, 14 insertions(+), 6 deletions(-) diff --git a/include/openrave/kinbody.h b/include/openrave/kinbody.h index ae46298c68..c05850b4e4 100644 --- a/include/openrave/kinbody.h +++ b/include/openrave/kinbody.h @@ -2035,8 +2035,6 @@ class OPENRAVE_API KinBody : public InterfaceBase 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: @@ -2071,6 +2069,7 @@ class OPENRAVE_API KinBody : public InterfaceBase 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 + int jointAxis = 0; dReal jointValue = 0.0; std::string connectedBodyName; ///< the connected body name the jointName comes from }; diff --git a/python/bindings/include/openravepy/openravepy_jointinfo.h b/python/bindings/include/openravepy/openravepy_jointinfo.h index 7052330204..eb12bd8143 100644 --- a/python/bindings/include/openravepy/openravepy_jointinfo.h +++ b/python/bindings/include/openravepy/openravepy_jointinfo.h @@ -506,6 +506,7 @@ class PyPositionConfiguration_JointConfigurationState object _id = py::none_(); object jointName = py::none_(); + int jointAxis = 0; dReal jointValue = 0.0; object connectedBodyName = py::none_(); diff --git a/python/bindings/openravepy_kinbody.cpp b/python/bindings/openravepy_kinbody.cpp index 590844b822..dfb84ed1ab 100644 --- a/python/bindings/openravepy_kinbody.cpp +++ b/python/bindings/openravepy_kinbody.cpp @@ -2170,6 +2170,7 @@ KinBody::PositionConfiguration::JointConfigurationStatePtr PyPositionConfigurati if( !IS_PYTHONOBJECT_NONE(jointName) ) { jointConfigurationState->jointName = py::extract(jointName); } + jointConfigurationState->jointAxis = jointAxis; jointConfigurationState->jointValue = jointValue; if( !IS_PYTHONOBJECT_NONE(connectedBodyName) ) { jointConfigurationState->connectedBodyName = py::extract(connectedBodyName); @@ -2201,7 +2202,7 @@ std::string PyPositionConfiguration_JointConfigurationState::__str__() if( !IS_PYTHONOBJECT_NONE(jointName) ) { ss << (const std::string&) py::extract(jointName); } - ss << "\" jointValue=" << jointValue << " connectedBodyName=\""; + ss << "\" jointAxis=" << jointAxis << " jointValue=" << jointValue << " connectedBodyName=\""; if( !IS_PYTHONOBJECT_NONE(connectedBodyName) ) { ss << (const std::string&) py::extract(connectedBodyName); } @@ -2228,6 +2229,7 @@ void PyPositionConfiguration_JointConfigurationState::_Update(const KinBody::Pos { _id = ConvertStringToUnicode(jointConfigurationState._id); jointName = ConvertStringToUnicode(jointConfigurationState.jointName); + jointAxis = jointConfigurationState.jointAxis; jointValue = jointConfigurationState.jointValue; connectedBodyName = ConvertStringToUnicode(jointConfigurationState.connectedBodyName); } @@ -4598,13 +4600,14 @@ class PositionConfiguration_JointConfigurationState_pickle_suite public: static py::tuple getstate(const PyPositionConfiguration_JointConfigurationState& r) { - return py::make_tuple(r._id, r.jointName, r.jointValue, r.connectedBodyName); + return py::make_tuple(r._id, r.jointName, r.jointAxis, 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]; + r.jointAxis = py::extract(state[2]); + r.jointValue = py::extract(state[3]); + r.connectedBodyName = state[4]; } }; @@ -5253,6 +5256,7 @@ void init_openravepy_kinbody() #endif .def_readwrite("_id", &PyPositionConfiguration_JointConfigurationState::_id) .def_readwrite("jointName",&PyPositionConfiguration_JointConfigurationState::jointName) + .def_readwrite("jointAxis",&PyPositionConfiguration_JointConfigurationState::jointAxis) .def_readwrite("jointValue",&PyPositionConfiguration_JointConfigurationState::jointValue) .def_readwrite("connectedBodyName",&PyPositionConfiguration_JointConfigurationState::connectedBodyName) .def("__str__", &PyPositionConfiguration_JointConfigurationState::__str__) diff --git a/src/libopenrave/kinbodyjoint.cpp b/src/libopenrave/kinbodyjoint.cpp index 4c05f50006..8e830f35c6 100644 --- a/src/libopenrave/kinbodyjoint.cpp +++ b/src/libopenrave/kinbodyjoint.cpp @@ -2918,6 +2918,7 @@ void KinBody::PositionConfiguration::JointConfigurationState::Reset() { _id.clear(); jointName.clear(); + jointAxis = 0; jointValue = 0.0; connectedBodyName.clear(); } @@ -2928,6 +2929,7 @@ void KinBody::PositionConfiguration::JointConfigurationState::SerializeJSON(rapi OpenRAVE::orjson::SetJsonValueByKey(value, "id", _id, allocator); } OpenRAVE::orjson::SetJsonValueByKey(value, "jointName", jointName, allocator); + OpenRAVE::orjson::SetJsonValueByKey(value, "jointAxis", jointAxis, allocator); OpenRAVE::orjson::SetJsonValueByKey(value, "jointValue", jointValue, allocator); if( !connectedBodyName.empty() ) { OpenRAVE::orjson::SetJsonValueByKey(value, "connectedBodyName", connectedBodyName, allocator); @@ -2939,6 +2941,7 @@ void KinBody::PositionConfiguration::JointConfigurationState::DeserializeJSON(co Reset(); OpenRAVE::orjson::LoadJsonValueByKey(value, "id", _id); OpenRAVE::orjson::LoadJsonValueByKey(value, "jointName", jointName); + OpenRAVE::orjson::LoadJsonValueByKey(value, "jointAxis", jointAxis); OpenRAVE::orjson::LoadJsonValueByKey(value, "jointValue", jointValue); OpenRAVE::orjson::LoadJsonValueByKey(value, "connectedBodyName", connectedBodyName); } @@ -2947,6 +2950,7 @@ bool KinBody::PositionConfiguration::JointConfigurationState::operator==(const K { static constexpr dReal jointValueEpsilon = 1.0e-6; return jointName == other.jointName + && jointAxis == other.jointAxis && connectedBodyName == other.connectedBodyName && OpenRAVE::RaveFabs(jointValue - other.jointValue) < jointValueEpsilon; } From c31b8d65298770cac4e743a3aaa064493a654f5e Mon Sep 17 00:00:00 2001 From: Shintaro Matsuoka Date: Mon, 28 Mar 2022 19:13:28 +0900 Subject: [PATCH 04/18] Adds JSON serialisation/deserialisation for nonSelfCollidingPositionConfigurations --- include/openrave/kinbody.h | 9 +++++++++ src/libopenrave/kinbody.cpp | 21 +++++++++++++++++++++ 2 files changed, 30 insertions(+) diff --git a/include/openrave/kinbody.h b/include/openrave/kinbody.h index c05850b4e4..d74efce0b0 100644 --- a/include/openrave/kinbody.h +++ b/include/openrave/kinbody.h @@ -2045,6 +2045,13 @@ class OPENRAVE_API KinBody : public InterfaceBase 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; + inline const std::string& GetId() const { + return _id; + } + inline const std::string& GetName() const { + return name; + } + bool operator==(const PositionConfiguration& other) const; bool operator!=(const PositionConfiguration& other) const; @@ -2254,6 +2261,8 @@ class OPENRAVE_API KinBody : public InterfaceBase std::vector _vLinkInfos; ///< list of pointers to LinkInfo std::vector _vJointInfos; ///< list of pointers to JointInfo + std::vector _vNonSelfCollidingPositionConfigurations; ///< list of non-self-colliding position configurations + std::map _mReadableInterfaces; ///< readable interface mapping bool _isRobot = false; ///< true if should create a RobotBasePtr diff --git a/src/libopenrave/kinbody.cpp b/src/libopenrave/kinbody.cpp index 46bb4b1a71..e7db31baea 100644 --- a/src/libopenrave/kinbody.cpp +++ b/src/libopenrave/kinbody.cpp @@ -113,6 +113,7 @@ bool KinBody::KinBodyInfo::operator==(const KinBodyInfo& other) const { && AreVectorsDeepEqual(_vLinkInfos, other._vLinkInfos) && AreVectorsDeepEqual(_vJointInfos, other._vJointInfos) && AreVectorsDeepEqual(_vGrabbedInfos, other._vGrabbedInfos) + && _vNonSelfCollidingPositionConfigurations == other._vNonSelfCollidingPositionConfigurations && _mReadableInterfaces == other._mReadableInterfaces; } @@ -128,6 +129,7 @@ void KinBody::KinBodyInfo::Reset() _vGrabbedInfos.clear(); _vLinkInfos.clear(); _vJointInfos.clear(); + _vNonSelfCollidingPositionConfigurations.clear(); _mReadableInterfaces.clear(); _isRobot = false; _isPartial = true; @@ -218,6 +220,18 @@ void KinBody::KinBodyInfo::SerializeJSON(rapidjson::Value& rKinBodyInfo, rapidjs rKinBodyInfo.AddMember("joints", rJointInfoValues, allocator); } + if (_vNonSelfCollidingPositionConfigurations.size() > 0) { + rapidjson::Value rNonSelfCollidingPositionConfigurations; + rNonSelfCollidingPositionConfigurations.SetArray(); + rNonSelfCollidingPositionConfigurations.Reserve(_vNonSelfCollidingPositionConfigurations.size(), allocator); + FOREACHC(it, _vNonSelfCollidingPositionConfigurations) { + rapidjson::Value rNonSelfCollidingPositionConfiguration; + (*it)->SerializeJSON(rNonSelfCollidingPositionConfiguration, allocator, fUnitScale, options); + rNonSelfCollidingPositionConfigurations.PushBack(rNonSelfCollidingPositionConfiguration, allocator); + } + rKinBodyInfo.AddMember("nonSelfCollidingPositionConfigurations", rNonSelfCollidingPositionConfigurations, allocator); + } + if (_mReadableInterfaces.size() > 0) { rapidjson::Value rReadableInterfaces; rReadableInterfaces.SetObject(); @@ -354,6 +368,13 @@ void KinBody::KinBodyInfo::DeserializeJSON(const rapidjson::Value& value, dReal } } + if (value.HasMember("nonSelfCollidingPositionConfigurations")) { + _vNonSelfCollidingPositionConfigurations.reserve(value["nonSelfCollidingPositionConfigurations"].Size() + _vNonSelfCollidingPositionConfigurations.size()); + for (rapidjson::Value::ConstValueIterator it = value["nonSelfCollidingPositionConfigurations"].Begin(); it != value["nonSelfCollidingPositionConfigurations"].End(); ++it) { + UpdateOrCreateInfoWithNameCheck(*it, _vNonSelfCollidingPositionConfigurations, "name", fUnitScale, options); + } + } + if (value.HasMember("dofValues")) { _dofValues.resize(0); for(rapidjson::Value::ConstValueIterator itr = value["dofValues"].Begin(); itr != value["dofValues"].End(); ++itr) { From 9853c402ac478560bcf495e447c2940dee00fe2c Mon Sep 17 00:00:00 2001 From: Shintaro Matsuoka Date: Tue, 29 Mar 2022 18:51:39 +0900 Subject: [PATCH 05/18] Calculate non-adjacent link pairs based on nonSelfCollidingPositionConfigurations --- include/openrave/kinbody.h | 10 ++ .../include/openravepy/openravepy_jointinfo.h | 1 + .../include/openravepy/openravepy_kinbody.h | 1 + python/bindings/openravepy_kinbody.cpp | 18 ++ src/libopenrave/kinbody.cpp | 170 +++++++++++++++++- src/libopenrave/kinbodyjoint.cpp | 10 ++ src/libopenrave/robot.cpp | 3 + 7 files changed, 210 insertions(+), 3 deletions(-) diff --git a/include/openrave/kinbody.h b/include/openrave/kinbody.h index d74efce0b0..48deb82fe1 100644 --- a/include/openrave/kinbody.h +++ b/include/openrave/kinbody.h @@ -2074,6 +2074,8 @@ class OPENRAVE_API KinBody : public InterfaceBase bool operator==(const JointConfigurationState& other) const; bool operator!=(const JointConfigurationState& other) const; + std::string GetResolvedJointName() 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 int jointAxis = 0; @@ -3491,6 +3493,13 @@ class OPENRAVE_API KinBody : public InterfaceBase void _SetAdjacentLinksInternal(int linkindex0, int linkindex1); + void _CalculateAdjacentLinkFlagsFromNonSelfCollidingPositionConfigurations(std::vector& adjacentLinkFlags); + + /// \brief Returns a full list of DOFs which values are determinable given an initial list of such DOFs + /// \param[in,out] isDOFValueDeterminableList List of flags which indicate whether DOF values are determinable. Takes an initial list as input, and returns a full list as output. Size must match GetDOF(). + /// \return True if the list of DOFs has been converged, otherwise false. + bool _ResolveDOFValueDeterminableFlags(std::vector& isDOFValueDeterminableList, int maxNumIterations) const; + std::string _name; ///< name of body std::vector _vecjoints; ///< \see GetJoints @@ -3521,6 +3530,7 @@ class OPENRAVE_API KinBody : public InterfaceBase mutable boost::array, 4> _cacheSetNonAdjacentLinks; ///< used for caching return value of GetNonAdjacentLinks. mutable int _nNonAdjacentLinkCache; ///< specifies what information is currently valid in the AdjacentOptions. Declared as mutable since data is cached. If 0x80000000 (ie < 0), then everything needs to be recomputed including _setNonAdjacentLinks[0]. std::vector _vInitialLinkTransformations; ///< the initial transformations of each link specifying at least one pose where the robot is collision free + std::vector _vNonSelfCollidingPositionConfigurations; ///< list of non-self-colliding position configurations mutable std::vector _vAttachedVisitedCache; ///< cache mutable std::vector > _vVelocitiesCache; diff --git a/python/bindings/include/openravepy/openravepy_jointinfo.h b/python/bindings/include/openravepy/openravepy_jointinfo.h index eb12bd8143..55a9a539a8 100644 --- a/python/bindings/include/openravepy/openravepy_jointinfo.h +++ b/python/bindings/include/openravepy/openravepy_jointinfo.h @@ -536,6 +536,7 @@ class PyPositionConfiguration private: void _Update(const KinBody::PositionConfiguration& positionConfiguration); }; +typedef OPENRAVE_SHARED_PTR PyPositionConfigurationPtr; class PyKinBodyStateSaver { diff --git a/python/bindings/include/openravepy/openravepy_kinbody.h b/python/bindings/include/openravepy/openravepy_kinbody.h index b5090ab60b..38742217e5 100644 --- a/python/bindings/include/openravepy/openravepy_kinbody.h +++ b/python/bindings/include/openravepy/openravepy_kinbody.h @@ -157,6 +157,7 @@ class PyKinBody : public PyInterfaceBase bool _isRobot = false; bool _isPartial = true; py::object _dofValues = py::none_(); + py::object _vNonSelfCollidingPositionConfigurations = py::none_(); py::object _readableInterfaces = py::none_(); virtual std::string __str__(); virtual py::object __unicode__(); diff --git a/python/bindings/openravepy_kinbody.cpp b/python/bindings/openravepy_kinbody.cpp index dfb84ed1ab..4e98fb2fad 100644 --- a/python/bindings/openravepy_kinbody.cpp +++ b/python/bindings/openravepy_kinbody.cpp @@ -2490,6 +2490,16 @@ KinBody::KinBodyInfoPtr PyKinBody::PyKinBodyInfo::GetKinBodyInfo() const { pInfo->_isRobot = _isRobot; pInfo->_isPartial = _isPartial; + pInfo->_vNonSelfCollidingPositionConfigurations.clear(); + if (!IS_PYTHONOBJECT_NONE(_vNonSelfCollidingPositionConfigurations)) { + for (int positionConfigurationIndex = 0; positionConfigurationIndex < len(_vNonSelfCollidingPositionConfigurations); ++positionConfigurationIndex) { + PyPositionConfigurationPtr pyPositionConfiguration = py::extract(_vNonSelfCollidingPositionConfigurations[positionConfigurationIndex]); + if (!!pyPositionConfiguration) { + pInfo->_vNonSelfCollidingPositionConfigurations.push_back(pyPositionConfiguration->GetPositionConfiguration()); + } + } + } + pInfo->_mReadableInterfaces = ExtractReadableInterfaces(_readableInterfaces); return pInfo; } @@ -2549,6 +2559,14 @@ void PyKinBody::PyKinBodyInfo::_Update(const KinBody::KinBodyInfo& info) { _isRobot = info._isRobot; _isPartial = info._isPartial; _dofValues = ReturnDOFValues(info._dofValues); + + py::list vNonSelfCollidingPositionConfigurations; + for( const KinBody::PositionConfigurationPtr& positionConfiguration : info._vNonSelfCollidingPositionConfigurations ) { + PyPositionConfiguration pyPositionConfiguration(*positionConfiguration); + vNonSelfCollidingPositionConfigurations.append(pyPositionConfiguration); + } + _vNonSelfCollidingPositionConfigurations = vNonSelfCollidingPositionConfigurations; + _readableInterfaces = ReturnReadableInterfaces(info._mReadableInterfaces); } diff --git a/src/libopenrave/kinbody.cpp b/src/libopenrave/kinbody.cpp index e7db31baea..bd37ed0d20 100644 --- a/src/libopenrave/kinbody.cpp +++ b/src/libopenrave/kinbody.cpp @@ -56,11 +56,12 @@ inline int _GetIndex1d(int index0, int index1) } } -inline void _ResizeVectorFor2DTable(std::vector& vec, size_t vectorSize) +template< typename T > +inline void _ResizeVectorFor2DTable(std::vector& vec, size_t vectorSize, T value=0) { const size_t tableSize = vectorSize * (vectorSize - 1) / 2; if (vec.size() < tableSize) { - vec.resize(tableSize, 0); + vec.resize(tableSize, value); } } @@ -473,6 +474,7 @@ void KinBody::Destroy() _vAdjacentLinks.clear(); _vInitialLinkTransformations.clear(); + _vNonSelfCollidingPositionConfigurations.clear(); _vAllPairsShortestPaths.clear(); _vClosedLoops.clear(); _vClosedLoopIndices.clear(); @@ -759,6 +761,8 @@ bool KinBody::InitFromKinBodyInfo(const KinBodyInfo& info) SetReadableInterface(it->first, it->second); } + _vNonSelfCollidingPositionConfigurations = info._vNonSelfCollidingPositionConfigurations; // Shallow copy + if( GetXMLId() != info._interfaceType ) { RAVELOG_WARN_FORMAT("body '%s' interfaceType does not match %s != %s", GetName()%GetXMLId()%info._interfaceType); } @@ -5374,8 +5378,25 @@ const std::vector& KinBody::GetNonAdjacentLinks(int adjacentoptions) const std::vector vcurtrans; std::vector _vdoflastsetvalues; }; - CHECK_INTERNAL_COMPUTATION; + if( _nNonAdjacentLinkCache & 0x80000000 ) { + std::vector adjacentLinkFlags; + // FIXME: Avoid const cast + const_cast(this)->_CalculateAdjacentLinkFlagsFromNonSelfCollidingPositionConfigurations(adjacentLinkFlags); + + // Constructs _vNonAdjacentLinks[0] + _vNonAdjacentLinks[0].clear(); + for( size_t ind0 = 0; ind0 < _veclinks.size(); ++ind0 ) { + for( size_t ind1 = ind0 + 1; ind1 < _veclinks.size(); ++ind1 ) { + if( !adjacentLinkFlags.at(_GetIndex1d(ind0, ind1)) && !AreAdjacentLinks(ind0, ind1) ) { + _vNonAdjacentLinks[0].push_back(ind0|(ind1<<16)); + } + } + } + std::sort(_vNonAdjacentLinks[0].begin(), _vNonAdjacentLinks[0].end(), CompareNonAdjacentFarthest); + _nNonAdjacentLinkCache = 0; + } + /* if( _nNonAdjacentLinkCache & 0x80000000 ) { // Check for colliding link pairs given the initial pose _vInitialLinkTransformations // this is actually weird, we need to call the individual link collisions on a const body. in order to pull this off, we need to be very careful with the body state. @@ -5400,6 +5421,7 @@ const std::vector& KinBody::GetNonAdjacentLinks(int adjacentoptions) const _nUpdateStampId++; // because transforms were modified _nNonAdjacentLinkCache = 0; } + */ if( (_nNonAdjacentLinkCache&adjacentoptions) != adjacentoptions ) { int requestedoptions = (~_nNonAdjacentLinkCache)&adjacentoptions; // find out what needs to computed @@ -5421,6 +5443,140 @@ const std::vector& KinBody::GetNonAdjacentLinks(int adjacentoptions) const return _vNonAdjacentLinks.at(adjacentoptions); } +void KinBody::_CalculateAdjacentLinkFlagsFromNonSelfCollidingPositionConfigurations(std::vector& adjacentLinkFlags) +{ + _ResizeVectorFor2DTable(adjacentLinkFlags, _veclinks.size()); // Fills with 0 + if( _vNonSelfCollidingPositionConfigurations.empty() ) { + return; + } + + KinBodyStateSaver saver(shared_kinbody(), Save_LinkTransformation); + CollisionCheckerBasePtr collisionchecker = !!_selfcollisionchecker ? _selfcollisionchecker : GetEnv()->GetCollisionChecker(); + CollisionOptionsStateSaver colsaver(collisionchecker, 0); // have to reset the collision options + + std::vector dofValues; + std::vector dofValueIsSetFlags; // Used to determine which link pairs can be evaluated for adjacency + std::vector dofValueIsDeterminableFlags; + for( const PositionConfigurationPtr& nonSelfCollidingPositionConfiguration : _vNonSelfCollidingPositionConfigurations ){ + dofValues.clear(); + dofValues.resize(GetDOF(), 0); + dofValueIsSetFlags.clear(); + dofValueIsSetFlags.resize(GetDOF(), false); + + // Applies the position configuration + for( const PositionConfiguration::JointConfigurationState& jointConfiguration : nonSelfCollidingPositionConfiguration->jointConfigurationStates ) { + JointPtr joint = GetJoint(jointConfiguration.GetResolvedJointName()); + if( !!joint && jointConfiguration.jointAxis < joint->GetDOF() ) { + const int dofIndex = joint->GetDOFIndex() + jointConfiguration.jointAxis; + dofValues.at(dofIndex) = jointConfiguration.jointValue; + dofValueIsSetFlags.at(dofIndex) = true; + } + } + SetDOFValues(dofValues, CLA_CheckLimitsThrow); // Original DOF values will be restored by the state saver + + // Looks up which DOF values are determinable given the position configuration + dofValueIsDeterminableFlags = dofValueIsSetFlags; // Determinable if a value is explicitly given + static constexpr int maxNumIterations = 5; + if( !_ResolveDOFValueDeterminableFlags(dofValueIsDeterminableFlags, maxNumIterations) ) { + RAVELOG_WARN_FORMAT("Failed to calculate a converged list of value-determinable DOFs within %d attempts." + " There might be a circular mimic DOF dependencies or too deep mimic DOF dependencies." + " Continuing with a possibly subset of value-determinable DOFs, which is on the safe side." + " (i.e. there might be robot links on which self-collision checking is performed unintentionally.)", maxNumIterations); + } + const bool areAllDOFValuesDeterminable = std::find(dofValueIsDeterminableFlags.begin(), dofValueIsDeterminableFlags.end(), false) == dofValueIsDeterminableFlags.end(); + + // Updates adjacentLinkFlags + for( size_t ind0 = 0; ind0 < _veclinks.size(); ++ind0 ) { + for( size_t ind1 = ind0 + 1; ind1 < _veclinks.size(); ++ind1 ) { + const int adjacentLinkFlagIndex = _GetIndex1d(ind0, ind1); + if( adjacentLinkFlags.at(adjacentLinkFlagIndex) ) { + continue; // Already marked as adjacent, no need to check again + } + if( AreAdjacentLinks(ind0, ind1) ) { + adjacentLinkFlags.at(adjacentLinkFlagIndex) = true; // Optimisation; allows skipping the check next time + continue; + } + + bool areAllDependencyDOFValuesDeterminable = areAllDOFValuesDeterminable; + if( !areAllDependencyDOFValuesDeterminable ) { + // Since DOF values are given for a subset of DOFs, needs to check whether the chain between the links is fully inclusive + std::vector jointsInChain; + GetChain(ind0, ind1, jointsInChain); + areAllDependencyDOFValuesDeterminable = true; + for( JointPtr& joint : jointsInChain ) { + if( joint->GetDOFIndex() >= 0 ) { // Has active DOFs + for( int jointAxis = 0; jointAxis < joint->GetDOF(); ++jointAxis ) { + if( !dofValueIsDeterminableFlags.at(joint->GetDOFIndex() + jointAxis) ) { + areAllDependencyDOFValuesDeterminable = false; + break; + } + } + if( !areAllDependencyDOFValuesDeterminable ) { + break; // Early exit + } + } + } + } + + if( !areAllDependencyDOFValuesDeterminable ) { + continue; // Relative poses of the links are not determinable due to lack of DOF values in the chain, thus unable to evaluate collision + } + + if( collisionchecker->CheckCollision(LinkConstPtr(_veclinks[ind0]), LinkConstPtr(_veclinks[ind1])) ) { + adjacentLinkFlags.at(adjacentLinkFlagIndex) = true; + } + } + } + } // Loop over _vNonSelfCollidingPositionConfigurations + _nUpdateStampId++; // because transforms were modified +} + +bool KinBody::_ResolveDOFValueDeterminableFlags(std::vector& isDOFValueDeterminableList, int maxNumIterations) const +{ + OPENRAVE_ASSERT_OP((int)isDOFValueDeterminableList.size(), ==, GetDOF()); + std::vector tmpMimicDOFList; + bool hasConverged = false; + for( int iterationIndex = 0; iterationIndex < maxNumIterations; ++iterationIndex ) { + hasConverged = true; + for( int dofIndex = 0; dofIndex < GetDOF(); ++dofIndex ) { + if( isDOFValueDeterminableList.at(dofIndex) ) { + continue; + } + const JointPtr joint = GetJointFromDOFIndex(dofIndex); + if( !joint ) { + continue; + } + const int axisIndex = dofIndex - joint->GetDOFIndex(); + bool isDOFValueDeterminable = false; + if( joint->IsStatic() ) { + isDOFValueDeterminable = true; + } + else if( joint->IsMimic(axisIndex) ) { + tmpMimicDOFList.clear(); + joint->GetMimicDOFIndices(tmpMimicDOFList, axisIndex); + bool areAllDependencyDOFValuesDeterminable = true; + for( int dependencyDOFIndex: tmpMimicDOFList ) { + if( !isDOFValueDeterminableList.at(dependencyDOFIndex) ) { + areAllDependencyDOFValuesDeterminable = false; + break; + } + } + isDOFValueDeterminable = areAllDependencyDOFValuesDeterminable; + } + + if( isDOFValueDeterminable ) { + isDOFValueDeterminableList.at(dofIndex) = true; + hasConverged = false; + } + } + + if( hasConverged ) { + break; + } + } + return hasConverged; +} + bool KinBody::AreAdjacentLinks(int linkindex0, int linkindex1) const { CHECK_INTERNAL_COMPUTATION; @@ -5573,6 +5729,10 @@ void KinBody::Clone(InterfaceBaseConstPtr preference, int cloningoptions) _vAdjacentLinks = r->_vAdjacentLinks; _vInitialLinkTransformations = r->_vInitialLinkTransformations; + _vNonSelfCollidingPositionConfigurations.reserve(r->_vNonSelfCollidingPositionConfigurations.size()); + for( const PositionConfigurationPtr& positionConfiguration : r->_vNonSelfCollidingPositionConfigurations ){ + _vNonSelfCollidingPositionConfigurations.push_back(PositionConfigurationPtr(new PositionConfiguration(*positionConfiguration))); // Deep copy + } _vForcedAdjacentLinks = r->_vForcedAdjacentLinks; _vAllPairsShortestPaths = r->_vAllPairsShortestPaths; _vClosedLoopIndices = r->_vClosedLoopIndices; @@ -6032,6 +6192,8 @@ void KinBody::ExtractInfo(KinBodyInfo& info) info._mReadableInterfaces[it->first] = it->second->CloneSelf(); } } + + info._vNonSelfCollidingPositionConfigurations = _vNonSelfCollidingPositionConfigurations; // Shallow copy } UpdateFromInfoResult KinBody::UpdateFromKinBodyInfo(const KinBodyInfo& info) @@ -6198,6 +6360,8 @@ UpdateFromInfoResult KinBody::UpdateFromKinBodyInfo(const KinBodyInfo& info) RAVELOG_VERBOSE_FORMAT("body %s updated due to readable interface change", _id); } + _vNonSelfCollidingPositionConfigurations = info._vNonSelfCollidingPositionConfigurations; // Shallow copy + return updateFromInfoResult; } diff --git a/src/libopenrave/kinbodyjoint.cpp b/src/libopenrave/kinbodyjoint.cpp index 8e830f35c6..3a55b6aa2c 100644 --- a/src/libopenrave/kinbodyjoint.cpp +++ b/src/libopenrave/kinbodyjoint.cpp @@ -2960,4 +2960,14 @@ bool KinBody::PositionConfiguration::JointConfigurationState::operator!=(const K return !operator==(other); } +std::string KinBody::PositionConfiguration::JointConfigurationState::GetResolvedJointName() const +{ + if( connectedBodyName.empty() ) { + return jointName; + } + else { + return connectedBodyName + "_" + jointName; + } +} + } diff --git a/src/libopenrave/robot.cpp b/src/libopenrave/robot.cpp index 6647c1cfae..7d0821c3e4 100644 --- a/src/libopenrave/robot.cpp +++ b/src/libopenrave/robot.cpp @@ -830,6 +830,9 @@ bool RobotBase::InitFromRobotInfo(const RobotBaseInfo& info) FOREACH(it, info._mReadableInterfaces) { SetReadableInterface(it->first, it->second); } + + _vNonSelfCollidingPositionConfigurations = info._vNonSelfCollidingPositionConfigurations; // Shallow copy + return true; } From 93faf2c60239745e054a01fd01157a6aec822cee Mon Sep 17 00:00:00 2001 From: Shintaro Matsuoka Date: Wed, 30 Mar 2022 11:31:34 +0900 Subject: [PATCH 06/18] Gets rid of const cast in KinBody::GetNonAdjacentLinks() --- include/openrave/kinbody.h | 6 +- src/libopenrave/kinbody.cpp | 163 +++++++++++++++++++----------------- src/libopenrave/robot.cpp | 5 +- 3 files changed, 92 insertions(+), 82 deletions(-) diff --git a/include/openrave/kinbody.h b/include/openrave/kinbody.h index 48deb82fe1..7b070a3457 100644 --- a/include/openrave/kinbody.h +++ b/include/openrave/kinbody.h @@ -3493,7 +3493,7 @@ class OPENRAVE_API KinBody : public InterfaceBase void _SetAdjacentLinksInternal(int linkindex0, int linkindex1); - void _CalculateAdjacentLinkFlagsFromNonSelfCollidingPositionConfigurations(std::vector& adjacentLinkFlags); + void _CalculateAdjacentLinkFlagsFromNonSelfCollidingPositionConfigurations(std::vector& adjacentLinkFlags) const; /// \brief Returns a full list of DOFs which values are determinable given an initial list of such DOFs /// \param[in,out] isDOFValueDeterminableList List of flags which indicate whether DOF values are determinable. Takes an initial list as input, and returns a full list as output. Size must match GetDOF(). @@ -3529,8 +3529,8 @@ class OPENRAVE_API KinBody : public InterfaceBase mutable boost::array, 4> _vNonAdjacentLinks; ///< contains cached versions of the non-adjacent links depending on values in AdjacentOptions. Declared as mutable since data is cached. mutable boost::array, 4> _cacheSetNonAdjacentLinks; ///< used for caching return value of GetNonAdjacentLinks. mutable int _nNonAdjacentLinkCache; ///< specifies what information is currently valid in the AdjacentOptions. Declared as mutable since data is cached. If 0x80000000 (ie < 0), then everything needs to be recomputed including _setNonAdjacentLinks[0]. - std::vector _vInitialLinkTransformations; ///< the initial transformations of each link specifying at least one pose where the robot is collision free - std::vector _vNonSelfCollidingPositionConfigurations; ///< list of non-self-colliding position configurations + typedef std::pair > PositionConfigurationAndLinkTransformations; + std::vector _vNonSelfCollidingPositionConfigurationsAndLinkTransformations; ///< list of non-self-colliding position configurations and corresponding link transformations mutable std::vector _vAttachedVisitedCache; ///< cache mutable std::vector > _vVelocitiesCache; diff --git a/src/libopenrave/kinbody.cpp b/src/libopenrave/kinbody.cpp index bd37ed0d20..df7340b051 100644 --- a/src/libopenrave/kinbody.cpp +++ b/src/libopenrave/kinbody.cpp @@ -473,8 +473,7 @@ void KinBody::Destroy() _vDOFIndices.clear(); _vAdjacentLinks.clear(); - _vInitialLinkTransformations.clear(); - _vNonSelfCollidingPositionConfigurations.clear(); + _vNonSelfCollidingPositionConfigurationsAndLinkTransformations.clear(); _vAllPairsShortestPaths.clear(); _vClosedLoops.clear(); _vClosedLoopIndices.clear(); @@ -761,7 +760,10 @@ bool KinBody::InitFromKinBodyInfo(const KinBodyInfo& info) SetReadableInterface(it->first, it->second); } - _vNonSelfCollidingPositionConfigurations = info._vNonSelfCollidingPositionConfigurations; // Shallow copy + _vNonSelfCollidingPositionConfigurationsAndLinkTransformations.clear(); + for( const PositionConfigurationPtr& positionConfiguration : info._vNonSelfCollidingPositionConfigurations ) { + _vNonSelfCollidingPositionConfigurationsAndLinkTransformations.emplace_back(positionConfiguration, std::vector()); // Shallow copy + } if( GetXMLId() != info._interfaceType ) { RAVELOG_WARN_FORMAT("body '%s' interfaceType does not match %s != %s", GetName()%GetXMLId()%info._interfaceType); @@ -4849,6 +4851,7 @@ void KinBody::_ComputeInternalInformation() } _nHierarchyComputed = 2; + /* // because of mimic joints, need to call SetDOFValues at least once, also use this to check for links that are off { vector vprevtrans, vnewtrans; @@ -4873,6 +4876,29 @@ void KinBody::_ComputeInternalInformation() } _vInitialLinkTransformations = vnewtrans; } + */ + { + KinBodyStateSaver linkTransformSaver(shared_kinbody(), Save_LinkTransformation); + vector dofValues; + vector vnewdoflastsetvalues; + for( PositionConfigurationAndLinkTransformations& positionConfigurationAndLinkTransformations : _vNonSelfCollidingPositionConfigurationsAndLinkTransformations ) { + dofValues.clear(); + dofValues.resize(GetDOF(), 0); + // Applies the position configuration + for( const PositionConfiguration::JointConfigurationState& jointConfiguration : positionConfigurationAndLinkTransformations.first->jointConfigurationStates ) { + JointPtr joint = GetJoint(jointConfiguration.GetResolvedJointName()); + if( !!joint && jointConfiguration.jointAxis < joint->GetDOF() ) { + dofValues.at(joint->GetDOFIndex() + jointConfiguration.jointAxis) = jointConfiguration.jointValue; + } + } + // unfortunately if SetDOFValues is overloaded by the robot, it could call the robot's _UpdateGrabbedBodies, which is a problem during environment cloning since the grabbed bodies might not be initialized. Therefore, call KinBody::SetDOFValues + std::vector vGrabbedBodies; vGrabbedBodies.swap(_vGrabbedBodies); // swap to get rid of _vGrabbedBodies + KinBody::SetDOFValues(dofValues, CLA_CheckLimitsThrow); // Original DOF values will be restored by the state saver + vGrabbedBodies.swap(_vGrabbedBodies); // swap back + + GetLinkTransformations(positionConfigurationAndLinkTransformations.second, vnewdoflastsetvalues); + } + } { // do not initialize interpolation, since it implies a motion sampling strategy @@ -5324,7 +5350,7 @@ void KinBody::SetNonCollidingConfiguration() { _ResetInternalCollisionCache(); vector vdoflastsetvalues; - GetLinkTransformations(_vInitialLinkTransformations, vdoflastsetvalues); + //GetLinkTransformations(_vInitialLinkTransformations, vdoflastsetvalues); } void KinBody::_ResetInternalCollisionCache() @@ -5357,32 +5383,10 @@ bool CompareNonAdjacentFarthest(int pair0, int pair1) const std::vector& KinBody::GetNonAdjacentLinks(int adjacentoptions) const { - class TransformsSaver - { -public: - TransformsSaver(KinBodyConstPtr pbody) : _pbody(pbody) { - _pbody->GetLinkTransformations(vcurtrans, _vdoflastsetvalues); - } - ~TransformsSaver() { - for(size_t i = 0; i < _pbody->_veclinks.size(); ++i) { - boost::static_pointer_cast(_pbody->_veclinks[i])->_info._t = vcurtrans.at(i); - } - for(size_t i = 0; i < _pbody->_vecjoints.size(); ++i) { - for(int j = 0; j < _pbody->_vecjoints[i]->GetDOF(); ++j) { - _pbody->_vecjoints[i]->_doflastsetvalues[j] = _vdoflastsetvalues.at(_pbody->_vecjoints[i]->GetDOFIndex()+j); - } - } - } -private: - KinBodyConstPtr _pbody; - std::vector vcurtrans; - std::vector _vdoflastsetvalues; - }; CHECK_INTERNAL_COMPUTATION; if( _nNonAdjacentLinkCache & 0x80000000 ) { std::vector adjacentLinkFlags; - // FIXME: Avoid const cast - const_cast(this)->_CalculateAdjacentLinkFlagsFromNonSelfCollidingPositionConfigurations(adjacentLinkFlags); + _CalculateAdjacentLinkFlagsFromNonSelfCollidingPositionConfigurations(adjacentLinkFlags); // Constructs _vNonAdjacentLinks[0] _vNonAdjacentLinks[0].clear(); @@ -5396,32 +5400,6 @@ const std::vector& KinBody::GetNonAdjacentLinks(int adjacentoptions) const std::sort(_vNonAdjacentLinks[0].begin(), _vNonAdjacentLinks[0].end(), CompareNonAdjacentFarthest); _nNonAdjacentLinkCache = 0; } - /* - if( _nNonAdjacentLinkCache & 0x80000000 ) { - // Check for colliding link pairs given the initial pose _vInitialLinkTransformations - // this is actually weird, we need to call the individual link collisions on a const body. in order to pull this off, we need to be very careful with the body state. - TransformsSaver saver(shared_kinbody_const()); - CollisionCheckerBasePtr collisionchecker = !!_selfcollisionchecker ? _selfcollisionchecker : GetEnv()->GetCollisionChecker(); - CollisionOptionsStateSaver colsaver(collisionchecker,0); // have to reset the collision options - for(size_t i = 0; i < _veclinks.size(); ++i) { - boost::static_pointer_cast(_veclinks[i])->_info._t = _vInitialLinkTransformations.at(i); - } - _nUpdateStampId++; // because transforms were modified - _vNonAdjacentLinks[0].resize(0); - - for(size_t ind0 = 0; ind0 < _veclinks.size(); ++ind0) { - for(size_t ind1 = ind0+1; ind1 < _veclinks.size(); ++ind1) { - const bool bAdjacent = AreAdjacentLinks(ind0, ind1); - if(!bAdjacent && !collisionchecker->CheckCollision(LinkConstPtr(_veclinks[ind0]), LinkConstPtr(_veclinks[ind1])) ) { - _vNonAdjacentLinks[0].push_back(ind0|(ind1<<16)); - } - } - } - std::sort(_vNonAdjacentLinks[0].begin(), _vNonAdjacentLinks[0].end(), CompareNonAdjacentFarthest); - _nUpdateStampId++; // because transforms were modified - _nNonAdjacentLinkCache = 0; - } - */ if( (_nNonAdjacentLinkCache&adjacentoptions) != adjacentoptions ) { int requestedoptions = (~_nNonAdjacentLinkCache)&adjacentoptions; // find out what needs to computed @@ -5443,39 +5421,60 @@ const std::vector& KinBody::GetNonAdjacentLinks(int adjacentoptions) const return _vNonAdjacentLinks.at(adjacentoptions); } -void KinBody::_CalculateAdjacentLinkFlagsFromNonSelfCollidingPositionConfigurations(std::vector& adjacentLinkFlags) +void KinBody::_CalculateAdjacentLinkFlagsFromNonSelfCollidingPositionConfigurations(std::vector& adjacentLinkFlags) const { + class TransformsSaver + { + public: + TransformsSaver(KinBodyConstPtr pbody) : _pbody(pbody) { + _pbody->GetLinkTransformations(vcurtrans, _vdoflastsetvalues); + } + ~TransformsSaver() { + for(size_t i = 0; i < _pbody->_veclinks.size(); ++i) { + boost::static_pointer_cast(_pbody->_veclinks[i])->_info._t = vcurtrans.at(i); + } + for(size_t i = 0; i < _pbody->_vecjoints.size(); ++i) { + for(int j = 0; j < _pbody->_vecjoints[i]->GetDOF(); ++j) { + _pbody->_vecjoints[i]->_doflastsetvalues[j] = _vdoflastsetvalues.at(_pbody->_vecjoints[i]->GetDOFIndex()+j); + } + } + } + private: + KinBodyConstPtr _pbody; + std::vector vcurtrans; + std::vector _vdoflastsetvalues; + }; + _ResizeVectorFor2DTable(adjacentLinkFlags, _veclinks.size()); // Fills with 0 - if( _vNonSelfCollidingPositionConfigurations.empty() ) { + if( _vNonSelfCollidingPositionConfigurationsAndLinkTransformations.empty() ) { return; } - KinBodyStateSaver saver(shared_kinbody(), Save_LinkTransformation); + // Check for colliding link pairs given _vNonSelfCollidingPositionConfigurationsAndLinkTransformations + // this is actually weird, we need to call the individual link collisions on a const body. in order to pull this off, we need to be very careful with the body state. + TransformsSaver saver(shared_kinbody_const()); CollisionCheckerBasePtr collisionchecker = !!_selfcollisionchecker ? _selfcollisionchecker : GetEnv()->GetCollisionChecker(); CollisionOptionsStateSaver colsaver(collisionchecker, 0); // have to reset the collision options - std::vector dofValues; - std::vector dofValueIsSetFlags; // Used to determine which link pairs can be evaluated for adjacency - std::vector dofValueIsDeterminableFlags; - for( const PositionConfigurationPtr& nonSelfCollidingPositionConfiguration : _vNonSelfCollidingPositionConfigurations ){ - dofValues.clear(); - dofValues.resize(GetDOF(), 0); - dofValueIsSetFlags.clear(); - dofValueIsSetFlags.resize(GetDOF(), false); - - // Applies the position configuration - for( const PositionConfiguration::JointConfigurationState& jointConfiguration : nonSelfCollidingPositionConfiguration->jointConfigurationStates ) { + std::vector dofValueIsDeterminableFlags(GetDOF()); // Used to determine which link pairs can be evaluated for adjacency + for( const PositionConfigurationAndLinkTransformations& positionConfigurationAndLinkTransformations : _vNonSelfCollidingPositionConfigurationsAndLinkTransformations ){ + // Applies the non-self-colliding position configuration + OPENRAVE_ASSERT_OP(_veclinks.size(), ==, positionConfigurationAndLinkTransformations.second.size()); + for(size_t i = 0; i < _veclinks.size(); ++i) { + boost::static_pointer_cast(_veclinks[i])->_info._t = positionConfigurationAndLinkTransformations.second.at(i); + } + _nUpdateStampId++; // because transforms were modified + + // Constructs initial dofValueIsDeterminableFlags + std::fill(dofValueIsDeterminableFlags.begin(), dofValueIsDeterminableFlags.end(), false); + for( const PositionConfiguration::JointConfigurationState& jointConfiguration : positionConfigurationAndLinkTransformations.first->jointConfigurationStates ) { JointPtr joint = GetJoint(jointConfiguration.GetResolvedJointName()); if( !!joint && jointConfiguration.jointAxis < joint->GetDOF() ) { - const int dofIndex = joint->GetDOFIndex() + jointConfiguration.jointAxis; - dofValues.at(dofIndex) = jointConfiguration.jointValue; - dofValueIsSetFlags.at(dofIndex) = true; + dofValueIsDeterminableFlags.at(joint->GetDOFIndex() + jointConfiguration.jointAxis) = true; } } - SetDOFValues(dofValues, CLA_CheckLimitsThrow); // Original DOF values will be restored by the state saver // Looks up which DOF values are determinable given the position configuration - dofValueIsDeterminableFlags = dofValueIsSetFlags; // Determinable if a value is explicitly given static constexpr int maxNumIterations = 5; if( !_ResolveDOFValueDeterminableFlags(dofValueIsDeterminableFlags, maxNumIterations) ) { RAVELOG_WARN_FORMAT("Failed to calculate a converged list of value-determinable DOFs within %d attempts." @@ -5728,10 +5727,12 @@ void KinBody::Clone(InterfaceBaseConstPtr preference, int cloningoptions) _vDOFIndices = r->_vDOFIndices; _vAdjacentLinks = r->_vAdjacentLinks; - _vInitialLinkTransformations = r->_vInitialLinkTransformations; - _vNonSelfCollidingPositionConfigurations.reserve(r->_vNonSelfCollidingPositionConfigurations.size()); - for( const PositionConfigurationPtr& positionConfiguration : r->_vNonSelfCollidingPositionConfigurations ){ - _vNonSelfCollidingPositionConfigurations.push_back(PositionConfigurationPtr(new PositionConfiguration(*positionConfiguration))); // Deep copy + _vNonSelfCollidingPositionConfigurationsAndLinkTransformations.reserve(r->_vNonSelfCollidingPositionConfigurationsAndLinkTransformations.size()); + for( const PositionConfigurationAndLinkTransformations& positionConfigurationAndLinkTransformations : r->_vNonSelfCollidingPositionConfigurationsAndLinkTransformations ){ + _vNonSelfCollidingPositionConfigurationsAndLinkTransformations.push_back( + std::make_pair(PositionConfigurationPtr(new PositionConfiguration(*positionConfigurationAndLinkTransformations.first)), + positionConfigurationAndLinkTransformations.second + )); // Deep copy } _vForcedAdjacentLinks = r->_vForcedAdjacentLinks; _vAllPairsShortestPaths = r->_vAllPairsShortestPaths; @@ -6193,7 +6194,10 @@ void KinBody::ExtractInfo(KinBodyInfo& info) } } - info._vNonSelfCollidingPositionConfigurations = _vNonSelfCollidingPositionConfigurations; // Shallow copy + info._vNonSelfCollidingPositionConfigurations.clear(); + for( const PositionConfigurationAndLinkTransformations& positionConfigurationAndLinkTransformations : _vNonSelfCollidingPositionConfigurationsAndLinkTransformations ) { + info._vNonSelfCollidingPositionConfigurations.push_back(positionConfigurationAndLinkTransformations.first); // Shallow copy + } } UpdateFromInfoResult KinBody::UpdateFromKinBodyInfo(const KinBodyInfo& info) @@ -6360,7 +6364,10 @@ UpdateFromInfoResult KinBody::UpdateFromKinBodyInfo(const KinBodyInfo& info) RAVELOG_VERBOSE_FORMAT("body %s updated due to readable interface change", _id); } - _vNonSelfCollidingPositionConfigurations = info._vNonSelfCollidingPositionConfigurations; // Shallow copy + _vNonSelfCollidingPositionConfigurationsAndLinkTransformations.clear(); + for( const PositionConfigurationPtr& positionConfiguration : info._vNonSelfCollidingPositionConfigurations ) { + _vNonSelfCollidingPositionConfigurationsAndLinkTransformations.emplace_back(positionConfiguration, std::vector()); // Shallow copy + } return updateFromInfoResult; } diff --git a/src/libopenrave/robot.cpp b/src/libopenrave/robot.cpp index 7d0821c3e4..d2d60cc405 100644 --- a/src/libopenrave/robot.cpp +++ b/src/libopenrave/robot.cpp @@ -831,7 +831,10 @@ bool RobotBase::InitFromRobotInfo(const RobotBaseInfo& info) SetReadableInterface(it->first, it->second); } - _vNonSelfCollidingPositionConfigurations = info._vNonSelfCollidingPositionConfigurations; // Shallow copy + _vNonSelfCollidingPositionConfigurationsAndLinkTransformations.clear(); + for( const PositionConfigurationPtr& positionConfiguration : info._vNonSelfCollidingPositionConfigurations ) { + _vNonSelfCollidingPositionConfigurationsAndLinkTransformations.emplace_back(positionConfiguration, std::vector()); // Shallow copy + } return true; } From 3e4ed289538af2fc6ae352d78b656acbc8c0ec8f Mon Sep 17 00:00:00 2001 From: Shintaro Matsuoka Date: Wed, 30 Mar 2022 13:56:09 +0900 Subject: [PATCH 07/18] Adds {KinBody,RobotBase}::GetPositionConfiguration() --- include/openrave/kinbody.h | 3 ++ include/openrave/robot.h | 3 ++ .../include/openravepy/openravepy_kinbody.h | 1 + .../include/openravepy/openravepy_robotbase.h | 2 ++ python/bindings/openravepy_kinbody.cpp | 8 +++++ python/bindings/openravepy_robot.cpp | 8 +++++ src/libopenrave/kinbody.cpp | 15 +++++++++ src/libopenrave/robot.cpp | 33 +++++++++++++++++++ 8 files changed, 73 insertions(+) diff --git a/include/openrave/kinbody.h b/include/openrave/kinbody.h index 7b070a3457..69f43820a6 100644 --- a/include/openrave/kinbody.h +++ b/include/openrave/kinbody.h @@ -2496,6 +2496,9 @@ class OPENRAVE_API KinBody : public InterfaceBase /// \param dofindices the dof indices to return the values for. If empty, will compute for all the dofs void GetDOFVelocities(std::vector& v, const std::vector& dofindices = std::vector()) const; + /// \brief Returns the current position configuration + virtual void GetPositionConfiguration(PositionConfiguration& positionConfiguration) const; + /// \brief Returns all the joint limits as organized by the DOF indices. /// /// \param dofindices the dof indices to return the values for. If empty, will compute for all the dofs diff --git a/include/openrave/robot.h b/include/openrave/robot.h index bed5e35677..d076e78db3 100644 --- a/include/openrave/robot.h +++ b/include/openrave/robot.h @@ -993,6 +993,9 @@ class OPENRAVE_API RobotBase : public KinBody /// \brief initializes a robot with info structure virtual bool InitFromRobotInfo(const RobotBaseInfo& info); + /// \brief Returns the current position configuration + virtual void GetPositionConfiguration(PositionConfiguration& positionConfiguration) const; + /// \brief Returns the manipulators of the robot virtual const std::vector& GetManipulators() const; diff --git a/python/bindings/include/openravepy/openravepy_kinbody.h b/python/bindings/include/openravepy/openravepy_kinbody.h index 38742217e5..ca86740f16 100644 --- a/python/bindings/include/openravepy/openravepy_kinbody.h +++ b/python/bindings/include/openravepy/openravepy_kinbody.h @@ -202,6 +202,7 @@ class PyKinBody : public PyInterfaceBase py::object GetDOFValues(py::object oindices) const; py::object GetDOFVelocities() const; py::object GetDOFVelocities(py::object oindices) const; + py::object GetPositionConfiguration() const; py::object GetDOFLimits() const; py::object GetDOFVelocityLimits() const; py::object GetDOFAccelerationLimits() const; diff --git a/python/bindings/include/openravepy/openravepy_robotbase.h b/python/bindings/include/openravepy/openravepy_robotbase.h index 637da24c40..e6d71c2fe7 100644 --- a/python/bindings/include/openravepy/openravepy_robotbase.h +++ b/python/bindings/include/openravepy/openravepy_robotbase.h @@ -297,6 +297,8 @@ class PyRobotBase : public PyKinBody bool Init(object olinkinfos, object ojointinfos, object omanipinfos, object oattachedsensorinfos, const std::string& uri=std::string()); + py::object GetPositionConfiguration() const; + object GetManipulators(); object GetManipulators(const std::string& manipname); diff --git a/python/bindings/openravepy_kinbody.cpp b/python/bindings/openravepy_kinbody.cpp index 4e98fb2fad..6d0fe83aca 100644 --- a/python/bindings/openravepy_kinbody.cpp +++ b/python/bindings/openravepy_kinbody.cpp @@ -2825,6 +2825,13 @@ object PyKinBody::GetDOFVelocities(object oindices) const return toPyArray(values); } +object PyKinBody::GetPositionConfiguration() const +{ + KinBody::PositionConfiguration positionConfiguration; + _pbody->GetPositionConfiguration(positionConfiguration); + return object(PyPositionConfigurationPtr(new PyPositionConfiguration(positionConfiguration))); +} + object PyKinBody::GetDOFLimits() const { std::vector vlower, vupper; @@ -5618,6 +5625,7 @@ void init_openravepy_kinbody() .def("GetDOFValues",getdofvalues2,PY_ARGS("indices") DOXY_FN(KinBody,GetDOFValues)) .def("GetDOFVelocities",getdofvelocities1, DOXY_FN(KinBody,GetDOFVelocities)) .def("GetDOFVelocities",getdofvelocities2, PY_ARGS("indices") DOXY_FN(KinBody,GetDOFVelocities)) + .def("GetPositionConfiguration",&PyKinBody::GetPositionConfiguration,DOXY_FN(KinBody,GetPositionConfiguration)) .def("GetDOFLimits",getdoflimits1, DOXY_FN(KinBody,GetDOFLimits)) .def("GetDOFLimits",getdoflimits2, PY_ARGS("indices") DOXY_FN(KinBody,GetDOFLimits)) .def("GetDOFVelocityLimits",getdofvelocitylimits1, DOXY_FN(KinBody,GetDOFVelocityLimits)) diff --git a/python/bindings/openravepy_robot.cpp b/python/bindings/openravepy_robot.cpp index 6862477c42..fc2d5b886b 100644 --- a/python/bindings/openravepy_robot.cpp +++ b/python/bindings/openravepy_robot.cpp @@ -1591,6 +1591,13 @@ bool PyRobotBase::Init(object olinkinfos, object ojointinfos, object omanipinfos return _probot->Init(vlinkinfos, vjointinfos, vmanipinfos, vattachedsensorinfos, uri); } +object PyRobotBase::GetPositionConfiguration() const +{ + KinBody::PositionConfiguration positionConfiguration; + _probot->GetPositionConfiguration(positionConfiguration); + return object(PyPositionConfigurationPtr(new PyPositionConfiguration(positionConfiguration))); +} + object PyRobotBase::GetManipulators() { py::list manips; @@ -2550,6 +2557,7 @@ void init_openravepy_robot() #else .def("Init", initrobot, Init_overloads(PY_ARGS("linkinfos", "jointinfos", "manipinfos", "attachedsensorinfos", "uri") DOXY_FN(RobotBase, Init))) #endif + .def("GetPositionConfiguration",&PyRobotBase::GetPositionConfiguration, DOXY_FN(RobotBase,GetPositionConfiguration)) .def("GetManipulators",GetManipulators1, DOXY_FN(RobotBase,GetManipulators)) .def("GetManipulators",GetManipulators2, PY_ARGS("manipname") DOXY_FN(RobotBase,GetManipulators)) .def("GetManipulator",&PyRobotBase::GetManipulator,PY_ARGS("manipname") "Return the manipulator whose name matches") diff --git a/src/libopenrave/kinbody.cpp b/src/libopenrave/kinbody.cpp index df7340b051..a80410b169 100644 --- a/src/libopenrave/kinbody.cpp +++ b/src/libopenrave/kinbody.cpp @@ -890,6 +890,21 @@ void KinBody::GetDOFVelocities(std::vector& v, const std::vector& do } } +void KinBody::GetPositionConfiguration(PositionConfiguration& positionConfiguration) const +{ + positionConfiguration.jointConfigurationStates.clear(); + positionConfiguration.jointConfigurationStates.reserve(GetDOF()); + for( JointPtr joint : _vDOFOrderedJoints ) { + for( int jointAxis = 0; jointAxis < joint->GetDOF(); ++jointAxis ) { + PositionConfiguration::JointConfigurationState jointConfigurationState; + jointConfigurationState.jointName = joint->GetName(); + jointConfigurationState.jointAxis = jointAxis; + jointConfigurationState.jointValue = joint->GetValue(jointAxis); + positionConfiguration.jointConfigurationStates.push_back(jointConfigurationState); + } + } +} + void KinBody::GetDOFLimits(std::vector& vLowerLimit, std::vector& vUpperLimit, const std::vector& dofindices) const { if( dofindices.size() == 0 ) { diff --git a/src/libopenrave/robot.cpp b/src/libopenrave/robot.cpp index d2d60cc405..5b0fc41d46 100644 --- a/src/libopenrave/robot.cpp +++ b/src/libopenrave/robot.cpp @@ -839,6 +839,39 @@ bool RobotBase::InitFromRobotInfo(const RobotBaseInfo& info) return true; } +void RobotBase::GetPositionConfiguration(PositionConfiguration& positionConfiguration) const +{ + positionConfiguration.jointConfigurationStates.clear(); + positionConfiguration.jointConfigurationStates.reserve(GetDOF()); + for( JointPtr joint : _vDOFOrderedJoints ) { + PositionConfiguration::JointConfigurationState jointConfigurationState; + jointConfigurationState.jointName = joint->GetName(); + + // Looks up whether the joint belongs to a connected body + bool isInConnectedBody = false; + for( const ConnectedBodyPtr& connectedBody : _vecConnectedBodies ) { + OPENRAVE_ASSERT_OP(connectedBody->_vResolvedJointNames.size(), ==, connectedBody->_info._vJointInfos.size()); + for( int jointIndex = 0; jointIndex < (int)connectedBody->_vResolvedJointNames.size(); ++jointIndex ) { + if( joint->GetName() == connectedBody->_vResolvedJointNames.at(jointIndex).first ) { + jointConfigurationState.jointName = connectedBody->_info._vJointInfos.at(jointIndex)->GetName(); // Joint name without prefix (connected body name) + jointConfigurationState.connectedBodyName = connectedBody->GetName(); + isInConnectedBody = true; + break; + } + } + if( isInConnectedBody ) { + break; // Optimisation: Early exit + } + } + + for( int jointAxis = 0; jointAxis < joint->GetDOF(); ++jointAxis ) { + jointConfigurationState.jointAxis = jointAxis; + jointConfigurationState.jointValue = joint->GetValue(jointAxis); + positionConfiguration.jointConfigurationStates.push_back(jointConfigurationState); + } + } +} + bool RobotBase::SetController(ControllerBasePtr controller, const std::vector& jointindices, int nControlTransformation) { RAVELOG_DEBUG_FORMAT("env=%d, default robot doesn't not support setting controllers (try GenericRobot)", GetEnv()->GetId()); From 84cee627d06fdd4f5b6e1f2cf2d0717e82610b6e Mon Sep 17 00:00:00 2001 From: Shintaro Matsuoka Date: Wed, 30 Mar 2022 14:23:29 +0900 Subject: [PATCH 08/18] Reimplements KinBody::SetNonCollidingConfiguration() with _vNonSelfCollidingPositionConfigurationsAndLinkTransformations --- src/libopenrave/kinbody.cpp | 20 +++++++++++++++++++- 1 file changed, 19 insertions(+), 1 deletion(-) diff --git a/src/libopenrave/kinbody.cpp b/src/libopenrave/kinbody.cpp index a80410b169..a81effec16 100644 --- a/src/libopenrave/kinbody.cpp +++ b/src/libopenrave/kinbody.cpp @@ -5364,8 +5364,26 @@ int8_t KinBody::DoesDOFAffectLink(int dofindex, int linkindex ) const void KinBody::SetNonCollidingConfiguration() { _ResetInternalCollisionCache(); + + PositionConfigurationPtr positionConfiguration(new PositionConfiguration()); + GetPositionConfiguration(*positionConfiguration); + positionConfiguration->name = "_configuration_added_by_SetNonCollidingConfiguration_"; + + std::vector linkTransforms; vector vdoflastsetvalues; - //GetLinkTransformations(_vInitialLinkTransformations, vdoflastsetvalues); + GetLinkTransformations(linkTransforms, vdoflastsetvalues); + + // Overwrites an entry in _vNonSelfCollidingPositionConfigurationsAndLinkTransformations if already exists under the same name + for( PositionConfigurationAndLinkTransformations& positionConfigurationAndLinkTransformations : _vNonSelfCollidingPositionConfigurationsAndLinkTransformations ) { + if( positionConfigurationAndLinkTransformations.first->name == positionConfiguration->name ) { + positionConfigurationAndLinkTransformations.first.swap(positionConfiguration); + positionConfigurationAndLinkTransformations.second.swap(linkTransforms); + return; + } + } + + // Adds a new entry + _vNonSelfCollidingPositionConfigurationsAndLinkTransformations.emplace_back(positionConfiguration, linkTransforms); } void KinBody::_ResetInternalCollisionCache() From 5f311cc6fafcb981b36491cb311cff7828747764 Mon Sep 17 00:00:00 2001 From: Shintaro Matsuoka Date: Wed, 30 Mar 2022 14:26:57 +0900 Subject: [PATCH 09/18] Clean up --- src/libopenrave/kinbody.cpp | 26 -------------------------- 1 file changed, 26 deletions(-) diff --git a/src/libopenrave/kinbody.cpp b/src/libopenrave/kinbody.cpp index a81effec16..ff58cad3b2 100644 --- a/src/libopenrave/kinbody.cpp +++ b/src/libopenrave/kinbody.cpp @@ -4866,32 +4866,6 @@ void KinBody::_ComputeInternalInformation() } _nHierarchyComputed = 2; - /* - // because of mimic joints, need to call SetDOFValues at least once, also use this to check for links that are off - { - vector vprevtrans, vnewtrans; - vector vprevdoflastsetvalues, vnewdoflastsetvalues; - GetLinkTransformations(vprevtrans, vprevdoflastsetvalues); - vector vcurrentvalues; - // unfortunately if SetDOFValues is overloaded by the robot, it could call the robot's _UpdateGrabbedBodies, which is a problem during environment cloning since the grabbed bodies might not be initialized. Therefore, call KinBody::SetDOFValues - GetDOFValues(vcurrentvalues); - std::vector vGrabbedBodies; vGrabbedBodies.swap(_vGrabbedBodies); // swap to get rid of _vGrabbedBodies - KinBody::SetDOFValues(vcurrentvalues,CLA_CheckLimits, std::vector()); - vGrabbedBodies.swap(_vGrabbedBodies); // swap back - GetLinkTransformations(vnewtrans, vnewdoflastsetvalues); - for(size_t i = 0; i < vprevtrans.size(); ++i) { - if( TransformDistanceFast(vprevtrans[i],vnewtrans[i]) > 1e-5 ) { - RAVELOG_VERBOSE(str(boost::format("link %d has different transformation after SetDOFValues (error=%f), this could be due to mimic joint equations kicking into effect.")%_veclinks.at(i)->GetName()%TransformDistanceFast(vprevtrans[i],vnewtrans[i]))); - } - } - for(int i = 0; i < GetDOF(); ++i) { - if( vprevdoflastsetvalues.at(i) != vnewdoflastsetvalues.at(i) ) { - RAVELOG_VERBOSE(str(boost::format("dof %d has different values after SetDOFValues %d!=%d, this could be due to mimic joint equations kicking into effect.")%i%vprevdoflastsetvalues.at(i)%vnewdoflastsetvalues.at(i))); - } - } - _vInitialLinkTransformations = vnewtrans; - } - */ { KinBodyStateSaver linkTransformSaver(shared_kinbody(), Save_LinkTransformation); vector dofValues; From d8c8ef7a70d1883e0e1ed9383ad27e7d4352f435 Mon Sep 17 00:00:00 2001 From: Shintaro Matsuoka Date: Wed, 30 Mar 2022 16:58:06 +0900 Subject: [PATCH 10/18] Adds Python binding for KinBody::AreAdjacentLinks() --- python/bindings/include/openravepy/openravepy_kinbody.h | 1 + python/bindings/openravepy_kinbody.cpp | 6 ++++++ 2 files changed, 7 insertions(+) diff --git a/python/bindings/include/openravepy/openravepy_kinbody.h b/python/bindings/include/openravepy/openravepy_kinbody.h index ca86740f16..f805d39cf8 100644 --- a/python/bindings/include/openravepy/openravepy_kinbody.h +++ b/python/bindings/include/openravepy/openravepy_kinbody.h @@ -328,6 +328,7 @@ class PyKinBody : public PyInterfaceBase py::object GetURI() const; py::object GetNonAdjacentLinks() const; py::object GetNonAdjacentLinks(int adjacentoptions) const; + bool AreAdjacentLinks(int linkindex0, int linkindex1) const; void SetAdjacentLinks(int linkindex0, int linkindex1); void SetAdjacentLinksCombinations(py::object olinkIndices); py::object GetAdjacentLinks() const; diff --git a/python/bindings/openravepy_kinbody.cpp b/python/bindings/openravepy_kinbody.cpp index 6d0fe83aca..4d92651473 100644 --- a/python/bindings/openravepy_kinbody.cpp +++ b/python/bindings/openravepy_kinbody.cpp @@ -4103,6 +4103,11 @@ object PyKinBody::GetNonAdjacentLinks(int adjacentoptions) const return ononadjacent; } +bool PyKinBody::AreAdjacentLinks(int linkindex0, int linkindex1) const +{ + return _pbody->AreAdjacentLinks(linkindex0, linkindex1); +} + void PyKinBody::SetAdjacentLinks(int linkindex0, int linkindex1) { _pbody->SetAdjacentLinks(linkindex0, linkindex1); @@ -5953,6 +5958,7 @@ void init_openravepy_kinbody() .def("GetXMLFilename",&PyKinBody::GetURI, DOXY_FN(InterfaceBase,GetURI)) .def("GetNonAdjacentLinks",GetNonAdjacentLinks1, DOXY_FN(KinBody,GetNonAdjacentLinks)) .def("GetNonAdjacentLinks",GetNonAdjacentLinks2, PY_ARGS("adjacentoptions") DOXY_FN(KinBody,GetNonAdjacentLinks)) + .def("AreAdjacentLinks",&PyKinBody::AreAdjacentLinks, PY_ARGS("linkindex0", "linkindex1") DOXY_FN(KinBody,AreAdjacentLinks)) .def("SetAdjacentLinks",&PyKinBody::SetAdjacentLinks, PY_ARGS("linkindex0", "linkindex1") DOXY_FN(KinBody,SetAdjacentLinks)) .def("SetAdjacentLinksCombinations",&PyKinBody::SetAdjacentLinksCombinations, PY_ARGS("linkIndices") DOXY_FN(KinBody,SetAdjacentLinksCombinations)) .def("GetAdjacentLinks",&PyKinBody::GetAdjacentLinks, DOXY_FN(KinBody,GetAdjacentLinks)) From bd16f550825246a70e2085aca65e7cb752537342 Mon Sep 17 00:00:00 2001 From: Shintaro Matsuoka Date: Thu, 31 Mar 2022 11:55:38 +0900 Subject: [PATCH 11/18] kinbody.h: Better annotations --- include/openrave/kinbody.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/openrave/kinbody.h b/include/openrave/kinbody.h index 69f43820a6..f2aedc3985 100644 --- a/include/openrave/kinbody.h +++ b/include/openrave/kinbody.h @@ -2077,10 +2077,10 @@ class OPENRAVE_API KinBody : public InterfaceBase std::string GetResolvedJointName() 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 + std::string jointName; ///< name of the joint. If the joint belong to a connectedBody, then its resolved name is connectedBodyName+"_"+jointName int jointAxis = 0; dReal jointValue = 0.0; - std::string connectedBodyName; ///< the connected body name the jointName comes from + std::string connectedBodyName; ///< Name of the connected body the joint comes from. Set to empty if the joint belongs to a robot, not a connected body. }; typedef boost::shared_ptr JointConfigurationStatePtr; typedef boost::shared_ptr JointConfigurationStateConstPtr; From 7cd1f1bb5b95212179397c1a2e4aeb72cb3f32c1 Mon Sep 17 00:00:00 2001 From: Shintaro Matsuoka <15539618+strmio@users.noreply.github.com> Date: Thu, 31 Mar 2022 15:56:16 +0900 Subject: [PATCH 12/18] Annotate JointConfigurationState Co-authored-by: Felix von Drigalski --- include/openrave/kinbody.h | 1 + 1 file changed, 1 insertion(+) diff --git a/include/openrave/kinbody.h b/include/openrave/kinbody.h index f2aedc3985..8fabfdd78b 100644 --- a/include/openrave/kinbody.h +++ b/include/openrave/kinbody.h @@ -2061,6 +2061,7 @@ class OPENRAVE_API KinBody : public InterfaceBase jointConfigurationStates.swap(rhs.jointConfigurationStates); } + /// name and value of one joint class JointConfigurationState : public InfoBase { public: From e0a4edbeb17c3214647a9f21bec76614025a7f27 Mon Sep 17 00:00:00 2001 From: Shintaro Matsuoka Date: Thu, 31 Mar 2022 16:11:18 +0900 Subject: [PATCH 13/18] Need to check value equality of PositionConfigurations, not reference equality --- src/libopenrave/kinbody.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/libopenrave/kinbody.cpp b/src/libopenrave/kinbody.cpp index ff58cad3b2..b17749d9a0 100644 --- a/src/libopenrave/kinbody.cpp +++ b/src/libopenrave/kinbody.cpp @@ -114,7 +114,7 @@ bool KinBody::KinBodyInfo::operator==(const KinBodyInfo& other) const { && AreVectorsDeepEqual(_vLinkInfos, other._vLinkInfos) && AreVectorsDeepEqual(_vJointInfos, other._vJointInfos) && AreVectorsDeepEqual(_vGrabbedInfos, other._vGrabbedInfos) - && _vNonSelfCollidingPositionConfigurations == other._vNonSelfCollidingPositionConfigurations + && AreVectorsDeepEqual(_vNonSelfCollidingPositionConfigurations, other._vNonSelfCollidingPositionConfigurations) && _mReadableInterfaces == other._mReadableInterfaces; } From 2a9e9b5416f61330d740aef92ce21d784084a933 Mon Sep 17 00:00:00 2001 From: Shintaro Matsuoka Date: Thu, 31 Mar 2022 16:34:44 +0900 Subject: [PATCH 14/18] Replace 0x80000000 for KinBody::_nNonAdjacentLinkCache with a constant --- include/openrave/kinbody.h | 3 ++- src/libopenrave/kinbody.cpp | 6 +++--- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/include/openrave/kinbody.h b/include/openrave/kinbody.h index 8fabfdd78b..631870ba48 100644 --- a/include/openrave/kinbody.h +++ b/include/openrave/kinbody.h @@ -3532,7 +3532,8 @@ class OPENRAVE_API KinBody : public InterfaceBase mutable boost::array, 4> _vNonAdjacentLinks; ///< contains cached versions of the non-adjacent links depending on values in AdjacentOptions. Declared as mutable since data is cached. mutable boost::array, 4> _cacheSetNonAdjacentLinks; ///< used for caching return value of GetNonAdjacentLinks. - mutable int _nNonAdjacentLinkCache; ///< specifies what information is currently valid in the AdjacentOptions. Declared as mutable since data is cached. If 0x80000000 (ie < 0), then everything needs to be recomputed including _setNonAdjacentLinks[0]. + static constexpr int NonAdjacentLinkCache_Uninitialized = 0x80000000; ///< A constant for _nNonAdjacentLinkCache, which indicates everything needs to be recomputed including _setNonAdjacentLinks[0]. + mutable int _nNonAdjacentLinkCache = NonAdjacentLinkCache_Uninitialized; ///< specifies what information is currently valid in the AdjacentOptions. Declared as mutable since data is cached. If NonAdjacentLinkCache_Uninitialized, then everything needs to be recomputed including _setNonAdjacentLinks[0]. typedef std::pair > PositionConfigurationAndLinkTransformations; std::vector _vNonSelfCollidingPositionConfigurationsAndLinkTransformations; ///< list of non-self-colliding position configurations and corresponding link transformations diff --git a/src/libopenrave/kinbody.cpp b/src/libopenrave/kinbody.cpp index b17749d9a0..9e045c5276 100644 --- a/src/libopenrave/kinbody.cpp +++ b/src/libopenrave/kinbody.cpp @@ -435,7 +435,7 @@ KinBody::KinBody(InterfaceType type, EnvironmentBasePtr penv) : InterfaceBase(ty _nParametersChanged = 0; _bMakeJoinedLinksAdjacent = true; _environmentBodyIndex = 0; - _nNonAdjacentLinkCache = 0x80000000; + _nNonAdjacentLinkCache = NonAdjacentLinkCache_Uninitialized; _nUpdateStampId = 0; _bAreAllJoints1DOFAndNonCircular = false; } @@ -5362,7 +5362,7 @@ void KinBody::SetNonCollidingConfiguration() void KinBody::_ResetInternalCollisionCache() { - _nNonAdjacentLinkCache = 0x80000000; + _nNonAdjacentLinkCache = NonAdjacentLinkCache_Uninitialized; FOREACH(it,_vNonAdjacentLinks) { it->resize(0); } @@ -5391,7 +5391,7 @@ bool CompareNonAdjacentFarthest(int pair0, int pair1) const std::vector& KinBody::GetNonAdjacentLinks(int adjacentoptions) const { CHECK_INTERNAL_COMPUTATION; - if( _nNonAdjacentLinkCache & 0x80000000 ) { + if( _nNonAdjacentLinkCache & NonAdjacentLinkCache_Uninitialized ) { std::vector adjacentLinkFlags; _CalculateAdjacentLinkFlagsFromNonSelfCollidingPositionConfigurations(adjacentLinkFlags); From ddc5424278b878a262c7130f43b4fe26d179c1ae Mon Sep 17 00:00:00 2001 From: Shintaro Matsuoka Date: Thu, 31 Mar 2022 17:12:28 +0900 Subject: [PATCH 15/18] Add annotations to KinBody::_CalculateAdjacentLinkFlagsFromNonSelfCollidingPositionConfigurations() --- include/openrave/kinbody.h | 2 ++ src/libopenrave/kinbody.cpp | 5 +++-- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/include/openrave/kinbody.h b/include/openrave/kinbody.h index 631870ba48..f7800375b0 100644 --- a/include/openrave/kinbody.h +++ b/include/openrave/kinbody.h @@ -3497,6 +3497,8 @@ class OPENRAVE_API KinBody : public InterfaceBase void _SetAdjacentLinksInternal(int linkindex0, int linkindex1); + /// \brief Returns adjacent link pair flags calculated from non-self-colliding position configurations + /// \param[out] adjacentLinkFlags List of flags indicating whether link pairs can be treated as adjacent. Indexed in the same order as _vAdjacentLinks. void _CalculateAdjacentLinkFlagsFromNonSelfCollidingPositionConfigurations(std::vector& adjacentLinkFlags) const; /// \brief Returns a full list of DOFs which values are determinable given an initial list of such DOFs diff --git a/src/libopenrave/kinbody.cpp b/src/libopenrave/kinbody.cpp index 9e045c5276..277894532a 100644 --- a/src/libopenrave/kinbody.cpp +++ b/src/libopenrave/kinbody.cpp @@ -5472,7 +5472,8 @@ void KinBody::_CalculateAdjacentLinkFlagsFromNonSelfCollidingPositionConfigurati } _nUpdateStampId++; // because transforms were modified - // Constructs initial dofValueIsDeterminableFlags + // Constructs initial dofValueIsDeterminableFlags (static/mimic joints are not reflected at this stage) + // This bit mask will be used to determine which link transforms are determinable and which link pair collisions are evaluatable. std::fill(dofValueIsDeterminableFlags.begin(), dofValueIsDeterminableFlags.end(), false); for( const PositionConfiguration::JointConfigurationState& jointConfiguration : positionConfigurationAndLinkTransformations.first->jointConfigurationStates ) { JointPtr joint = GetJoint(jointConfiguration.GetResolvedJointName()); @@ -5481,7 +5482,7 @@ void KinBody::_CalculateAdjacentLinkFlagsFromNonSelfCollidingPositionConfigurati } } - // Looks up which DOF values are determinable given the position configuration + // Updates dofValueIsDeterminableFlags to include DOF which values are determinable due to static/mimic joints static constexpr int maxNumIterations = 5; if( !_ResolveDOFValueDeterminableFlags(dofValueIsDeterminableFlags, maxNumIterations) ) { RAVELOG_WARN_FORMAT("Failed to calculate a converged list of value-determinable DOFs within %d attempts." From 5709ac7ec81ced4d60db7262d80bfe0caeb114ca Mon Sep 17 00:00:00 2001 From: Shintaro Matsuoka Date: Thu, 31 Mar 2022 17:38:36 +0900 Subject: [PATCH 16/18] Need to skip inactive connected bodies in RobotBase::GetPositionConfiguration() --- src/libopenrave/robot.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/libopenrave/robot.cpp b/src/libopenrave/robot.cpp index 5b0fc41d46..037eaae5aa 100644 --- a/src/libopenrave/robot.cpp +++ b/src/libopenrave/robot.cpp @@ -850,6 +850,9 @@ void RobotBase::GetPositionConfiguration(PositionConfiguration& positionConfigur // Looks up whether the joint belongs to a connected body bool isInConnectedBody = false; for( const ConnectedBodyPtr& connectedBody : _vecConnectedBodies ) { + if( !connectedBody->IsActive() ) { + continue; + } OPENRAVE_ASSERT_OP(connectedBody->_vResolvedJointNames.size(), ==, connectedBody->_info._vJointInfos.size()); for( int jointIndex = 0; jointIndex < (int)connectedBody->_vResolvedJointNames.size(); ++jointIndex ) { if( joint->GetName() == connectedBody->_vResolvedJointNames.at(jointIndex).first ) { From d2b5f46477a2a785b17cb8b7af7bb68bbb50b1eb Mon Sep 17 00:00:00 2001 From: Shintaro Matsuoka Date: Thu, 31 Mar 2022 18:54:16 +0900 Subject: [PATCH 17/18] Optimisation in KinBody::GetNonAdjacentLinks() --- include/openrave/kinbody.h | 5 +++-- src/libopenrave/kinbody.cpp | 13 ++++--------- 2 files changed, 7 insertions(+), 11 deletions(-) diff --git a/include/openrave/kinbody.h b/include/openrave/kinbody.h index f7800375b0..15d00f7043 100644 --- a/include/openrave/kinbody.h +++ b/include/openrave/kinbody.h @@ -3498,8 +3498,9 @@ class OPENRAVE_API KinBody : public InterfaceBase void _SetAdjacentLinksInternal(int linkindex0, int linkindex1); /// \brief Returns adjacent link pair flags calculated from non-self-colliding position configurations - /// \param[out] adjacentLinkFlags List of flags indicating whether link pairs can be treated as adjacent. Indexed in the same order as _vAdjacentLinks. - void _CalculateAdjacentLinkFlagsFromNonSelfCollidingPositionConfigurations(std::vector& adjacentLinkFlags) const; + /// \note Computation cost can be reduced by giving adjacentLinkFlags in which already known adjacent link pairs are marked as input + /// \param[in,out] adjacentLinkFlags List of flags indicating whether link pairs can be treated as adjacent. Indexed in the same order as _vAdjacentLinks. Size needs to match that of _vAdjacentLinks. + void _CalculateAdjacentLinkFlagsFromNonSelfCollidingPositionConfigurations(std::vector& adjacentLinkFlags) const; /// \brief Returns a full list of DOFs which values are determinable given an initial list of such DOFs /// \param[in,out] isDOFValueDeterminableList List of flags which indicate whether DOF values are determinable. Takes an initial list as input, and returns a full list as output. Size must match GetDOF(). diff --git a/src/libopenrave/kinbody.cpp b/src/libopenrave/kinbody.cpp index 277894532a..ac95eb8878 100644 --- a/src/libopenrave/kinbody.cpp +++ b/src/libopenrave/kinbody.cpp @@ -5392,14 +5392,14 @@ const std::vector& KinBody::GetNonAdjacentLinks(int adjacentoptions) const { CHECK_INTERNAL_COMPUTATION; if( _nNonAdjacentLinkCache & NonAdjacentLinkCache_Uninitialized ) { - std::vector adjacentLinkFlags; + std::vector adjacentLinkFlags = _vAdjacentLinks; // To reduce computation cost, initialises with already known adjacent link pairs _CalculateAdjacentLinkFlagsFromNonSelfCollidingPositionConfigurations(adjacentLinkFlags); // Constructs _vNonAdjacentLinks[0] _vNonAdjacentLinks[0].clear(); for( size_t ind0 = 0; ind0 < _veclinks.size(); ++ind0 ) { for( size_t ind1 = ind0 + 1; ind1 < _veclinks.size(); ++ind1 ) { - if( !adjacentLinkFlags.at(_GetIndex1d(ind0, ind1)) && !AreAdjacentLinks(ind0, ind1) ) { + if( !adjacentLinkFlags.at(_GetIndex1d(ind0, ind1)) ) { _vNonAdjacentLinks[0].push_back(ind0|(ind1<<16)); } } @@ -5428,7 +5428,7 @@ const std::vector& KinBody::GetNonAdjacentLinks(int adjacentoptions) const return _vNonAdjacentLinks.at(adjacentoptions); } -void KinBody::_CalculateAdjacentLinkFlagsFromNonSelfCollidingPositionConfigurations(std::vector& adjacentLinkFlags) const +void KinBody::_CalculateAdjacentLinkFlagsFromNonSelfCollidingPositionConfigurations(std::vector& adjacentLinkFlags) const { class TransformsSaver { @@ -5452,7 +5452,6 @@ void KinBody::_CalculateAdjacentLinkFlagsFromNonSelfCollidingPositionConfigurati std::vector _vdoflastsetvalues; }; - _ResizeVectorFor2DTable(adjacentLinkFlags, _veclinks.size()); // Fills with 0 if( _vNonSelfCollidingPositionConfigurationsAndLinkTransformations.empty() ) { return; } @@ -5499,10 +5498,6 @@ void KinBody::_CalculateAdjacentLinkFlagsFromNonSelfCollidingPositionConfigurati if( adjacentLinkFlags.at(adjacentLinkFlagIndex) ) { continue; // Already marked as adjacent, no need to check again } - if( AreAdjacentLinks(ind0, ind1) ) { - adjacentLinkFlags.at(adjacentLinkFlagIndex) = true; // Optimisation; allows skipping the check next time - continue; - } bool areAllDependencyDOFValuesDeterminable = areAllDOFValuesDeterminable; if( !areAllDependencyDOFValuesDeterminable ) { @@ -5530,7 +5525,7 @@ void KinBody::_CalculateAdjacentLinkFlagsFromNonSelfCollidingPositionConfigurati } if( collisionchecker->CheckCollision(LinkConstPtr(_veclinks[ind0]), LinkConstPtr(_veclinks[ind1])) ) { - adjacentLinkFlags.at(adjacentLinkFlagIndex) = true; + adjacentLinkFlags.at(adjacentLinkFlagIndex) = 1; } } } From aef1465fb9fc06fd62d45c146a1c763ece7fae5c Mon Sep 17 00:00:00 2001 From: Shintaro Matsuoka <15539618+strmio@users.noreply.github.com> Date: Fri, 1 Apr 2022 13:22:50 +0900 Subject: [PATCH 18/18] Annotates JointConfigurationState::_id Co-authored-by: Felix von Drigalski --- include/openrave/kinbody.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/openrave/kinbody.h b/include/openrave/kinbody.h index 15d00f7043..aca5e31a64 100644 --- a/include/openrave/kinbody.h +++ b/include/openrave/kinbody.h @@ -2077,7 +2077,7 @@ class OPENRAVE_API KinBody : public InterfaceBase std::string GetResolvedJointName() const; - std::string _id; ///< id of joint configuration state, for incremental update + std::string _id; ///< id of the joint configuration state, for incremental update. Unique among JointConfigurationStates. std::string jointName; ///< name of the joint. If the joint belong to a connectedBody, then its resolved name is connectedBodyName+"_"+jointName int jointAxis = 0; dReal jointValue = 0.0;