Merge pull request #109 from BenjaminNavarro/relative_jacobian_fix

Relative jacobian fix
jrl-umi3218 · Nov 13, 2023 · 237b74d · 237b74d
2 parents 88aa2dd + 37f6361
commit 237b74d
Showing 1 changed file with 34 additions and 13 deletions.
diff --git 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);
   }
 
@@ -65,16 +83,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);