From 4037a20dfe9b774f13cecf68b9c8297dea15b74c Mon Sep 17 00:00:00 2001
From: Benjamin Navarro <navarro.benjamin13@gmail.com>
Date: Tue, 8 Dec 2020 11:36:04 +0100
Subject: [PATCH 1/2] [RBDyn] Do not try to remove joints if both ends of a
 relative jacobian ends at root

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
---
 src/RBDyn/Jacobian.cpp | 19 +++++++++++--------
 1 file changed, 11 insertions(+), 8 deletions(-)

diff --git a/src/RBDyn/Jacobian.cpp b/src/RBDyn/Jacobian.cpp
index 2fe6b627..8c0dc390 100644
--- a/src/RBDyn/Jacobian.cpp
+++ b/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);

From 37f6361e32961a4c021dcdd7650e8cb510d47ea1 Mon Sep 17 00:00:00 2001
From: Benjamin Navarro <navarro.benjamin13@gmail.com>
Date: Fri, 3 Nov 2023 16:09:07 +0100
Subject: [PATCH 2/2] [RBDyn] fix possible incorrect joint path for relative
 jacobians

If both bodies belong to the same chain then the parent joint of the
reference body was always included, possibly increasing the number
of dofs
---
 src/RBDyn/Jacobian.cpp | 28 +++++++++++++++++++++++-----
 1 file changed, 23 insertions(+), 5 deletions(-)

diff --git a/src/RBDyn/Jacobian.cpp b/src/RBDyn/Jacobian.cpp
index 8c0dc390..2d9bdd7a 100644
--- a/src/RBDyn/Jacobian.cpp
+++ b/src/RBDyn/Jacobian.cpp
@@ -20,8 +20,25 @@ namespace rbd
 Jacobian::Jacobian() {}
 
 Jacobian::Jacobian(const MultiBody & mb, const std::string & bodyName, const Eigen::Vector3d & point)
-: Jacobian(mb, bodyName, mb.body(0).name(), point)
+: jointsPath_(), point_(point), jac_(), jacDot_()
 {
+  bodyIndex_ = mb.sBodyIndexByName(bodyName);
+  refBodyIndex_ = 0;
+
+  int index = bodyIndex_;
+
+  int dof = 0;
+  while(index != -1)
+  {
+    jointsPath_.insert(jointsPath_.begin(), index);
+    dof += mb.joint(index).dof();
+    jointsSign_.insert(jointsSign_.begin(), 1);
+
+    index = mb.parent(index);
+  }
+
+  jac_.resize(6, dof);
+  jacDot_.resize(6, dof);
 }
 
 Jacobian::Jacobian(const MultiBody & mb,
@@ -38,14 +55,15 @@ Jacobian::Jacobian(const MultiBody & mb,
   int dof = 0;
   while(index != -1)
   {
-    jointsPath_.insert(jointsPath_.begin(), index);
-    dof += mb.joint(index).dof();
-    jointsSign_.insert(jointsSign_.begin(), 1);
-
     if(index == refBodyIndex_)
     {
       break;
     }
+
+    jointsPath_.insert(jointsPath_.begin(), index);
+    dof += mb.joint(index).dof();
+    jointsSign_.insert(jointsSign_.begin(), 1);
+
     index = mb.parent(index);
   }