Merge pull request #1479 from rdiankov/connected_body_callback_20241216

Follow-Up on Connected Body Changes
rdiankov · Dec 22, 2024 · 72db427 · 72db427
2 parents 86b0d87 + fd831f9
commit 72db427
Show file tree

Hide file tree

Showing 4 changed files with 14 additions and 5 deletions.
diff --git a/docs/source/changelog.rst b/docs/source/changelog.rst
@@ -6,6 +6,8 @@ ChangeLog
 Version 0.159.1
 ===============
 
+- Fix the problem that connected body resolved joint names that are empty are not skipped in `_UpdateConnectedBodyInfo`.
+- Fix the problem that `CompareTransform` does not consider the quaternions `quat` and `-quat` to be the same rotation.
 - Fix wrong ComputeInverseDynamics for Prismatic joint.
 
 Version 0.159.0

diff --git a/include/openrave/geometry.h b/include/openrave/geometry.h
@@ -493,8 +493,11 @@ class RaveTransform
 
     /// \brief return true if any element of transform is different by mor ethan epsilon, otherwise false.
     inline bool CompareTransform(const RaveTransform<T>& rhs, T epsilon) const {
-        return RaveFabs(trans.x - rhs.trans.x) > epsilon || RaveFabs(trans.y - rhs.trans.y) > epsilon || RaveFabs(trans.z - rhs.trans.z) > epsilon ||
-               RaveFabs(rot.x - rhs.rot.x) > epsilon || RaveFabs(rot.y - rhs.rot.y) > epsilon || RaveFabs(rot.z - rhs.rot.z) > epsilon || RaveFabs(rot.w - rhs.rot.w) > epsilon;
+        return (RaveFabs(trans.x - rhs.trans.x) > epsilon
+		|| RaveFabs(trans.y - rhs.trans.y) > epsilon
+		|| RaveFabs(trans.z - rhs.trans.z) > epsilon
+		|| ((RaveFabs(rot.x - rhs.rot.x) > epsilon || RaveFabs(rot.y - rhs.rot.y) > epsilon || RaveFabs(rot.z - rhs.rot.z) > epsilon || RaveFabs(rot.w - rhs.rot.w) > epsilon)
+		    && ((RaveFabs(rot.x + rhs.rot.x) > epsilon || RaveFabs(rot.y + rhs.rot.y) > epsilon || RaveFabs(rot.z + rhs.rot.z) > epsilon || RaveFabs(rot.w + rhs.rot.w) > epsilon))));
     }
 
     inline RaveTransform<T> inverse() const {

diff --git a/src/libopenrave-core/environment-core.h b/src/libopenrave-core/environment-core.h
@@ -3128,7 +3128,7 @@ class Environment : public EnvironmentBase
                 } else {
                     updateFromInfoResult = pMatchExistingBody->UpdateFromKinBodyInfo(*pKinBodyInfo);
                 }
-                RAVELOG_VERBOSE_FORMAT("env=%s, update body '%s' from info result %u", GetNameId()%pMatchExistingBody->_id%updateFromInfoResult);
+                RAVELOG_VERBOSE_FORMAT("env=%s, update body '%s' from info result %d", GetNameId() % pMatchExistingBody->_id % static_cast<int>(updateFromInfoResult));
                 if (updateFromInfoResult == UFIR_NoChange) {
                     continue;
                 }
@@ -3289,7 +3289,7 @@ class Environment : public EnvironmentBase
                 }
 
                 // re-grab after add this body back to the environment
-                for (int grabbingBodyIndex = 0; grabbingBodyIndex<pGrabbingBodies.size(); grabbingBodyIndex++) {
+                for (int grabbingBodyIndex = 0; grabbingBodyIndex < (int)pGrabbingBodies.size(); grabbingBodyIndex++) {
                     pGrabbingBodies[grabbingBodyIndex]->Grab(pInitBody, pGrabbingLinks[grabbingBodyIndex], linkIndicesToIgnore[grabbingBodyIndex], rGrabbedUserDataDocuments[grabbingBodyIndex]);
                 }
             }
@@ -3340,7 +3340,7 @@ class Environment : public EnvironmentBase
 
             if (itExistingBody != vBodies.end()) {
                 // grabbed infos
-                if (pKinBodyInfo->_vGrabbedInfos.size() != (*itExistingBody)->GetNumGrabbed()) {
+                if ((int)pKinBodyInfo->_vGrabbedInfos.size() != (*itExistingBody)->GetNumGrabbed()) {
                     RAVELOG_DEBUG_FORMAT("env=%s, body name='%s' updating grab from %d -> %d", GetNameId()%bodyName%(*itExistingBody)->GetNumGrabbed()%pKinBodyInfo->_vGrabbedInfos.size());
                     // when grab info changes, have to report to caller
                     if (std::find(vModifiedBodies.begin(), vModifiedBodies.end(), *itExistingBody) == vModifiedBodies.end() && std::find(vCreatedBodies.begin(), vCreatedBodies.end(), *itExistingBody) == vCreatedBodies.end()) {

diff --git a/src/libopenrave/robotconnectedbody.cpp b/src/libopenrave/robotconnectedbody.cpp
@@ -585,6 +585,10 @@ void RobotBase::ConnectedBody::_UpdateConnectedBodyInfo()
         if( !!pattachedrobot ) {
             for( const std::pair<std::string, RobotBase::JointPtr>& pair: _vResolvedJointNames ) {
                 const std::string& resolvedJointName = pair.first;
+                if( resolvedJointName.empty() ) {
+                    // Can be empty after being deinitialized
+                    continue;
+                }
                 KinBody::JointPtr pJoint = pattachedrobot->GetJoint(resolvedJointName);
                 if( !!pJoint ) {
                     const KinBody::JointInfo& newJointInfo = pJoint->UpdateAndGetInfo(); // source of new information