From 4cf027d45dc28f02db0c3c6144e53ab010952ce6 Mon Sep 17 00:00:00 2001 From: Taiju Yamada Date: Tue, 12 Nov 2024 21:50:04 +0900 Subject: [PATCH 1/4] fix exception msg --- python/bindings/openravepy_int.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/bindings/openravepy_int.cpp b/python/bindings/openravepy_int.cpp index be44521a1b..810bc7ea09 100644 --- a/python/bindings/openravepy_int.cpp +++ b/python/bindings/openravepy_int.cpp @@ -3377,7 +3377,7 @@ OPENRAVE_PYTHON_MODULE(openravepy_int) static PyOpenRAVEException pyOpenRAVEException(m, OPENRAVE_EXCEPTION_CLASS_NAME, PyExc_Exception); pyOpenRAVEException.def_property_readonly("message",[](py::object o) { - return o.attr("args")[0]; + return o.attr("args")[py::to_object(0)]; }); pyOpenRAVEException.def_property_readonly("errortype",[](py::object o) { py::object oargs = o.attr("args"); From 4abaf50d81b31f84313a24dca45e901817d3f2ce Mon Sep 17 00:00:00 2001 From: Ross Schlaikjer Date: Thu, 16 Jan 2025 14:11:46 +0900 Subject: [PATCH 2/4] Add combined SetTransformAndVelocity method When setting the transform/velocity of a body that is grabbing other bodies, _UpdateGrabbedBodies will call SetTransform / SetVelocity on all grabbed bodies, _each_ of which will then recursively call _UpdateGrabbedBodies on all of their grabbed bodies, etc. Since more calls are generated at each level, for heavily nested grabs, the exponential increase in duplicated calls becomes untenable. Allowing for a single update position + velocity call that only calls update grabbed bodies _once_ removes this expontential time problem. --- include/openrave/kinbody.h | 14 +++++++++++ src/libopenrave/kinbody.cpp | 44 ++++++++++++++++++++++++--------- src/libopenrave/kinbodygrab.cpp | 6 ++--- 3 files changed, 49 insertions(+), 15 deletions(-) diff --git a/include/openrave/kinbody.h b/include/openrave/kinbody.h index b9156a1243..ac5115f447 100644 --- a/include/openrave/kinbody.h +++ b/include/openrave/kinbody.h @@ -2966,6 +2966,20 @@ class OPENRAVE_API KinBody : public InterfaceBase */ virtual void SetTransform(const Transform& transform); + /// \brief Set both the transform and velocity of the body in one update + // + /// This call is more efficient than separately calling SetTransform/SetVelocity, especially if the body is grabbing other bodies + /// See SetTransform, SetVelocity + void SetTransformAndVelocity(const Transform& bodyTransform, const Vector& linearvel, const Vector& angularvel); + +private: + /// Set the transform of the body without invoking any post-processing. Internal use only. + void _SetTransformNoPostProcess(const Transform& transform); + + /// Set the velocity of the body without invoking any post-processing. Internal use only. + bool _SetVelocityNoPostProcess(const Vector& linearvel, const Vector& angularvel); + +public: /// \brief Return an axis-aligned bounding box of the entire object in the world coordinate system. /// /// \brief bEnabledOnlyLinks if true, will only count links that are enabled. By default this is false diff --git a/src/libopenrave/kinbody.cpp b/src/libopenrave/kinbody.cpp index 8fe16c72de..d2b238665a 100644 --- a/src/libopenrave/kinbody.cpp +++ b/src/libopenrave/kinbody.cpp @@ -1437,46 +1437,66 @@ void KinBody::SubtractDOFValues(std::vector& q1, const std::vector } } -// like apply transform except everything is relative to the first frame -void KinBody::SetTransform(const Transform& bodyTransform) +void KinBody::_SetTransformNoPostProcess(const Transform& bodyTransform) { - if( _veclinks.size() == 0 ) { + if (_veclinks.empty()) { return; } + Transform baseLinkTransform = bodyTransform * _baseLinkInBodyTransform; Transform tbaseinv = _veclinks.front()->GetTransform().inverse(); Transform tapply = baseLinkTransform * tbaseinv; FOREACH(itlink, _veclinks) { (*itlink)->SetTransform(tapply * (*itlink)->GetTransform()); } - _UpdateGrabbedBodies(); - _PostprocessChangedParameters(Prop_LinkTransforms); } -bool KinBody::SetVelocity(const Vector& linearvel, const Vector& angularvel) +bool KinBody::_SetVelocityNoPostProcess(const Vector& linearvel, const Vector& angularvel) { - if (_veclinks.size() == 0) { + if (_veclinks.empty()) { return false; } bool bSuccess = false; - if( _veclinks.size() == 1 ) { + if (_veclinks.size() == 1) { bSuccess = GetEnv()->GetPhysicsEngine()->SetLinkVelocity(_veclinks[0], linearvel, angularvel); } else { - std::vector >& velocities = _vVelocitiesCache; + std::vector>& velocities = _vVelocitiesCache; velocities.resize(_veclinks.size()); velocities.at(0).first = linearvel; velocities.at(0).second = angularvel; Vector vlinktrans = _veclinks.at(0)->GetTransform().trans; - for(size_t ilink = 1; ilink < _veclinks.size(); ++ilink) { - velocities[ilink].first = linearvel + angularvel.cross(_veclinks[ilink]->GetTransform().trans-vlinktrans); + for (size_t ilink = 1; ilink < _veclinks.size(); ++ilink) { + velocities[ilink].first = linearvel + angularvel.cross(_veclinks[ilink]->GetTransform().trans - vlinktrans); velocities[ilink].second = angularvel; } - bSuccess = GetEnv()->GetPhysicsEngine()->SetLinkVelocities(shared_kinbody(),velocities); + bSuccess = GetEnv()->GetPhysicsEngine()->SetLinkVelocities(shared_kinbody(), velocities); } + return bSuccess; +} + +void KinBody::SetTransformAndVelocity(const Transform& bodyTransform, const Vector& linearvel, const Vector& angularvel) +{ + _SetTransformNoPostProcess(bodyTransform); + _SetVelocityNoPostProcess(linearvel, angularvel); + _UpdateGrabbedBodies(); + _PostprocessChangedParameters(Prop_LinkTransforms); +} + +// like apply transform except everything is relative to the first frame +void KinBody::SetTransform(const Transform& bodyTransform) +{ + _SetTransformNoPostProcess(bodyTransform); + _UpdateGrabbedBodies(); + _PostprocessChangedParameters(Prop_LinkTransforms); +} + +bool KinBody::SetVelocity(const Vector& linearvel, const Vector& angularvel) +{ + const bool bSuccess = _SetVelocityNoPostProcess(linearvel, angularvel); _UpdateGrabbedBodies(); return bSuccess; } diff --git a/src/libopenrave/kinbodygrab.cpp b/src/libopenrave/kinbodygrab.cpp index 77f79786b7..01d5c963b5 100644 --- a/src/libopenrave/kinbodygrab.cpp +++ b/src/libopenrave/kinbodygrab.cpp @@ -996,11 +996,11 @@ void KinBody::_UpdateGrabbedBodies() if( !!pGrabbedBody ) { const Transform& tGrabbingLink = pgrabbed->_pGrabbingLink->GetTransform(); tGrabbedBody = tGrabbingLink * pgrabbed->_tRelative; - pGrabbedBody->SetTransform(tGrabbedBody); - // set the correct velocity pgrabbed->_pGrabbingLink->GetVelocity(velocity.first, velocity.second); velocity.first += velocity.second.cross(tGrabbedBody.trans - tGrabbingLink.trans); - pGrabbedBody->SetVelocity(velocity.first, velocity.second); + // Set the transform and velocity in one go so that we only cause one update of the grab sub-tree of this body + // If we update them separately, we duplicate work / can cause exponential wasted time for deeply nested grabs + pGrabbedBody->SetTransformAndVelocity(tGrabbedBody, velocity.first, velocity.second); ++grabIt; } else { From fe9184117e1706cf02913e1bda2de002876cd01f Mon Sep 17 00:00:00 2001 From: "rosen.diankov@gmail.com" Date: Fri, 17 Jan 2025 14:04:15 -0500 Subject: [PATCH 3/4] Fix for not respecting mustresolveuri in the json reader for invalid URIs. --- CMakeLists.txt | 2 +- docs/source/changelog.rst | 5 +++++ src/libopenrave-core/jsonparser/jsondownloader.cpp | 12 +++++++++--- src/libopenrave-core/jsonparser/jsondownloader.h | 2 +- src/libopenrave-core/jsonparser/jsonreader.cpp | 8 ++++---- src/libopenrave/openraveexception.cpp | 9 ++++++--- 6 files changed, 26 insertions(+), 12 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index bfaa0de913..8ed00a9b84 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,7 +5,7 @@ set( CMAKE_ALLOW_LOOSE_LOOP_CONSTRUCTS TRUE ) # Define here the needed parameters set (OPENRAVE_VERSION_MAJOR 0) set (OPENRAVE_VERSION_MINOR 161) -set (OPENRAVE_VERSION_PATCH 2) +set (OPENRAVE_VERSION_PATCH 3) set (OPENRAVE_VERSION ${OPENRAVE_VERSION_MAJOR}.${OPENRAVE_VERSION_MINOR}.${OPENRAVE_VERSION_PATCH}) set (OPENRAVE_SOVERSION ${OPENRAVE_VERSION_MAJOR}.${OPENRAVE_VERSION_MINOR}) message(STATUS "Compiling OpenRAVE Version ${OPENRAVE_VERSION}, soversion=${OPENRAVE_SOVERSION}") diff --git a/docs/source/changelog.rst b/docs/source/changelog.rst index 3325e9a7d9..110437d351 100644 --- a/docs/source/changelog.rst +++ b/docs/source/changelog.rst @@ -3,6 +3,11 @@ ChangeLog ######### +Version 0.161.3 +=============== + +- Fix for not respecting mustresolveuri in the json reader for invalid URIs. + Version 0.161.2 =============== diff --git a/src/libopenrave-core/jsonparser/jsondownloader.cpp b/src/libopenrave-core/jsonparser/jsondownloader.cpp index 927c20c52f..e189b1e962 100644 --- a/src/libopenrave-core/jsonparser/jsondownloader.cpp +++ b/src/libopenrave-core/jsonparser/jsondownloader.cpp @@ -214,7 +214,7 @@ bool JSONDownloaderScope::WaitForDownloads(bool bMustResolveURI, uint64_t timeou // queue other resources to be downloaded if (_downloadRecursively) { - QueueDownloadReferenceURIs(doc); + QueueDownloadReferenceURIs(bMustResolveURI, doc); } } } @@ -338,7 +338,7 @@ void JSONDownloaderScope::_QueueDownloadURI(const char* pUri, rapidjson::Documen RAVELOG_VERBOSE_FORMAT("%s start to download uri '%s' from '%s'", _contextdesc%canonicalUri%url); } -void JSONDownloaderScope::QueueDownloadReferenceURIs(const rapidjson::Value& rEnvInfo) +void JSONDownloaderScope::QueueDownloadReferenceURIs(bool bMustResolveURI, const rapidjson::Value& rEnvInfo) { if (!rEnvInfo.IsObject()) { return; @@ -363,7 +363,13 @@ void JSONDownloaderScope::QueueDownloadReferenceURIs(const rapidjson::Value& rEn } if (!_IsExpandableReferenceUri(pReferenceUri)) { const char* pId = orjson::GetCStringJsonValueByKey(rBody, "id",""); - throw OPENRAVE_EXCEPTION_FORMAT("bodyId '%s' has invalid referenceUri='%s", pId%pReferenceUri, ORE_InvalidURI); + if( bMustResolveURI ) { + throw OPENRAVE_EXCEPTION_FORMAT("bodyId '%s' has invalid referenceUri='%s'.", pId%pReferenceUri, ORE_InvalidURI); + } + else { + RAVELOG_VERBOSE_FORMAT("bodyId '%s' has invalid referenceUri='%s', but continuing since does not need to resolve uri", pId%pReferenceUri); + return; + } } QueueDownloadURI(pReferenceUri); } diff --git a/src/libopenrave-core/jsonparser/jsondownloader.h b/src/libopenrave-core/jsonparser/jsondownloader.h index 216ba50a80..ac697c7526 100644 --- a/src/libopenrave-core/jsonparser/jsondownloader.h +++ b/src/libopenrave-core/jsonparser/jsondownloader.h @@ -87,7 +87,7 @@ class JSONDownloaderScope { /// \brief Downloads all reference uris in the supplied env info, as well as all their further references /// \param rEnvInfo env info where reference uris should be discovered - void QueueDownloadReferenceURIs(const rapidjson::Value& rEnvInfo); + void QueueDownloadReferenceURIs(bool bMustResolveURI, const rapidjson::Value& rEnvInfo); /// \brief Wait for queued downloads to finish, downloaded documents are inserted into rapidJSONDocuments passed in constructor /// \param timeoutUS timeout in microseconds to wait for download to finish diff --git a/src/libopenrave-core/jsonparser/jsonreader.cpp b/src/libopenrave-core/jsonparser/jsonreader.cpp index dceb3535e2..9c32138d64 100644 --- a/src/libopenrave-core/jsonparser/jsonreader.cpp +++ b/src/libopenrave-core/jsonparser/jsonreader.cpp @@ -257,7 +257,7 @@ class JSONReader if (IsDownloadingFromRemote()) { #if OPENRAVE_CURL JSONDownloaderScope jsonDownload(_contextdesc, *_pDownloader, alloc, !(_deserializeOptions & IDO_IgnoreReferenceUri)); - jsonDownload.QueueDownloadReferenceURIs(rEnvInfo); + jsonDownload.QueueDownloadReferenceURIs(_bMustResolveURI, rEnvInfo); if( !jsonDownload.WaitForDownloads(_bMustResolveURI, _downloadTimeoutUS) ) { RAVELOG_VERBOSE("failed downloads"); //return false; @@ -483,7 +483,7 @@ class JSONReader if (IsDownloadingFromRemote()) { #if OPENRAVE_CURL JSONDownloaderScope jsonDownload(_contextdesc, *_pDownloader, alloc, !(_deserializeOptions & IDO_IgnoreReferenceUri)); - jsonDownload.QueueDownloadReferenceURIs(rEnvInfo); + jsonDownload.QueueDownloadReferenceURIs(_bMustResolveEnvironmentURI, rEnvInfo); if( !jsonDownload.WaitForDownloads(_bMustResolveEnvironmentURI, _downloadTimeoutUS) ) { //return false; } @@ -515,7 +515,7 @@ class JSONReader if (IsDownloadingFromRemote()) { #if OPENRAVE_CURL JSONDownloaderScope jsonDownload(_contextdesc, *_pDownloader, alloc, !(_deserializeOptions & IDO_IgnoreReferenceUri)); - jsonDownload.QueueDownloadReferenceURIs(rEnvInfo); + jsonDownload.QueueDownloadReferenceURIs(_bMustResolveEnvironmentURI, rEnvInfo); if( !jsonDownload.WaitForDownloads(_bMustResolveEnvironmentURI, _downloadTimeoutUS) ) { //return false; } @@ -678,7 +678,7 @@ class JSONReader throw OPENRAVE_EXCEPTION_FORMAT("body '%s' has invalid referenceUri='%s", bodyId%pReferenceUri, ORE_InvalidURI); } - RAVELOG_WARN_FORMAT("env=%d, body '%s' has invalid referenceUri='%s'", _penv->GetId()%bodyId%pReferenceUri); + RAVELOG_WARN_FORMAT("env=%s, body '%s' has invalid referenceUri='%s'", _penv->GetNameId()%bodyId%pReferenceUri); } } } diff --git a/src/libopenrave/openraveexception.cpp b/src/libopenrave/openraveexception.cpp index f597417d51..ef115b99eb 100644 --- a/src/libopenrave/openraveexception.cpp +++ b/src/libopenrave/openraveexception.cpp @@ -60,22 +60,25 @@ const char* RaveGetErrorCodeString(OpenRAVEErrorCode error) case ORE_Timeout: return "Timeout"; case ORE_InvalidURI: return "InvalidURI"; case ORE_BodyNameConflict: return "BodyNameConflict"; + case ORE_SensorNameConflict: return "SensorNameConflict"; + case ORE_BodyIdConflict: return "BodyIdConflict"; case ORE_LengthUnitInvalid: return "LengthUnitInvalid"; case ORE_MassUnitInvalid: return "MassUnitInvalid"; case ORE_TimeDurationUnitInvalid: return "TimeDurationUnitInvalid"; case ORE_AngleUnitInvalid: return "AngleUnitInvalid"; case ORE_TimeStampUnitInvalid: return "TimeStampUnitInvalid"; + case ORE_EnvironmentBodyIndexConflict: return "EnvironmentBodyIndexConflict"; - case ORE_SensorNameConflict: return "SensorNameConflict"; - case ORE_BodyIdConflict: return "BodyIdConflict"; case ORE_EnvironmentFormatUnrecognized: return "EnvironmentFormatUnrecognized"; + case ORE_CurlTimeout: return "CurlTimeout"; case ORE_CurlInvalidHandle: return "CurlInvalidHandle"; case ORE_CurlInvalidResponse: return "CurlInvalidResponse"; + } // should throw an exception? - return ""; + return "(unknown)"; } } // end namespace OpenRAVE From 032bec1a94b04094067ef1589947875b9b7e23b2 Mon Sep 17 00:00:00 2001 From: Ross Schlaikjer Date: Thu, 23 Jan 2025 09:54:46 -0500 Subject: [PATCH 4/4] Version, changelog --- CMakeLists.txt | 4 ++-- docs/source/changelog.rst | 5 +++++ 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8ed00a9b84..26247e47ef 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,8 +4,8 @@ set( CMAKE_ALLOW_LOOSE_LOOP_CONSTRUCTS TRUE ) # Define here the needed parameters set (OPENRAVE_VERSION_MAJOR 0) -set (OPENRAVE_VERSION_MINOR 161) -set (OPENRAVE_VERSION_PATCH 3) +set (OPENRAVE_VERSION_MINOR 162) +set (OPENRAVE_VERSION_PATCH 0) set (OPENRAVE_VERSION ${OPENRAVE_VERSION_MAJOR}.${OPENRAVE_VERSION_MINOR}.${OPENRAVE_VERSION_PATCH}) set (OPENRAVE_SOVERSION ${OPENRAVE_VERSION_MAJOR}.${OPENRAVE_VERSION_MINOR}) message(STATUS "Compiling OpenRAVE Version ${OPENRAVE_VERSION}, soversion=${OPENRAVE_SOVERSION}") diff --git a/docs/source/changelog.rst b/docs/source/changelog.rst index 110437d351..615fac2903 100644 --- a/docs/source/changelog.rst +++ b/docs/source/changelog.rst @@ -3,6 +3,11 @@ ChangeLog ######### +Version 0.162.0 +=============== + +- Add combined SetTransformAndVelocity method to reduce _UpdateGrabbedBodies calls + Version 0.161.3 ===============