fix relative Jacobians

it is possible to construct relative Jacobians without any duplicated
joints (e.g both chains end at root). Trying to remove joints in this
case is undefined behavior

CMakeLists.txt

@@ -10,7 +10,7 @@ set(PROJECT_NAME RBDyn)
 set(PROJECT_DESCRIPTION "...")
 set(PROJECT_URL "https://github.com/jrl-umi3218/RBDyn")
 set(PROJECT_DEBUG_POSTFIX "_d")
-set(PROJECT_VERSION 1.4.0)
+set(PROJECT_VERSION 1.4.1)
 set(PROJECT_USE_CMAKE_EXPORT TRUE)
 
 option(BUILD_RBDYN_PARSERS "Build URDF and YAML parsers" ON)

debian/changelog

@@ -1,3 +1,9 @@
+rbdyn (1.4.1-1ubuntu1) unstable; urgency=medium
+
+  * Update upstream version
+
+ -- Pierre Gergondet <[email protected]>  Mon, 08 Dec 2020 12:04:00 +0800
+
 rbdyn (1.4.0-1ubuntu1) unstable; urgency=medium
 
   * Update upstream version

src/RBDyn/Jacobian.cpp

@@ -65,16 +65,19 @@ Jacobian::Jacobian(const MultiBody & mb,
       count++;
     } while(std::find(jointsPath_.begin() + count, jointsPath_.end(), index) == jointsPath_.end());
 
-    // Delete common joints previously added
-    int commonIdx = count;
-    while(jointsPath_[static_cast<size_t>(++commonIdx)] != index)
+    if(index > 0)
     {
-      // Get to the common node
+      // Delete joints between the common joint and the root
+      int commonIdx = count;
+      while(jointsPath_[static_cast<size_t>(++commonIdx)] != index)
+      {
+        // Get to the common node
+      }
+      dof -= std::accumulate(jointsPath_.begin() + count, jointsPath_.begin() + commonIdx + 1, 0,
+                             [&](int dofC, int idx) { return dofC + mb.joint(idx).dof(); });
+      jointsPath_.erase(jointsPath_.begin() + count, jointsPath_.begin() + commonIdx + 1);
+      jointsSign_.erase(jointsSign_.begin() + count, jointsSign_.begin() + commonIdx + 1);
     }
-    dof -= std::accumulate(jointsPath_.begin() + count, jointsPath_.begin() + commonIdx + 1, 0,
-                           [&](int dofC, int idx) { return dofC + mb.joint(idx).dof(); });
-    jointsPath_.erase(jointsPath_.begin() + count, jointsPath_.begin() + commonIdx + 1);
-    jointsSign_.erase(jointsSign_.begin() + count, jointsSign_.begin() + commonIdx + 1);
   }
 
   jac_.resize(6, dof);