Merge branch 'production' of github.com:rdiankov/openrave into checkV…

…alidityOfMesh
rdiankov · Jan 27, 2025 · 3b6804a · 3b6804a
2 parents dd8c881 + c846371
commit 3b6804a
Show file tree

Hide file tree

Showing 10 changed files with 82 additions and 29 deletions.
diff --git 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 2)
+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
@@ -3,6 +3,16 @@
 ChangeLog
 #########
 
+Version 0.162.0
+===============
+
+- Add combined SetTransformAndVelocity method to reduce _UpdateGrabbedBodies calls
+
+Version 0.161.3
+===============
+
+- Fix for not respecting mustresolveuri in the json reader for invalid URIs.
+
 Version 0.161.2
 ===============
 

diff --git 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/python/bindings/openravepy_int.cpp b/python/bindings/openravepy_int.cpp
@@ -3435,7 +3435,7 @@ OPENRAVE_PYTHON_MODULE(openravepy_int)
 
     static PyOpenRAVEException<OpenRAVEException> 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");

diff --git 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
@@ -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
@@ -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/kinbody.cpp b/src/libopenrave/kinbody.cpp
@@ -1459,46 +1459,66 @@ void KinBody::SubtractDOFValues(std::vector<dReal>& q1, const std::vector<dReal>
     }
 }
 
-// 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<std::pair<Vector,Vector> >& velocities = _vVelocitiesCache;
+        std::vector<std::pair<Vector, 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
@@ -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 {

diff --git 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