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] [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);
   }