Feature/get joint names (#5)

.gitignore

@@ -2,4 +2,4 @@ build/
 install
 log/
 
-.vscode/
+# .vscode/

.vscode/c_cpp_properties.json

@@ -0,0 +1,24 @@
+{
+    "configurations": [
+        {
+            "name": "Linux",
+            "includePath": [
+                "${workspaceFolder}/**",
+                "/usr/include/",
+                "/usr/include/eigen3",
+                "/usr/include/opencv4/",
+                "/usr/local/include/",
+                "/opt/openrobots/include/pinocchio/**",
+                "/opt/ros/humble/include/**",
+                "${workspaceFolder}/../../install/**"
+            ],
+            "defines": [],
+            "compileCommands": "${workspaceFolder}/../..//build/compile_commands.json",
+            "compilerPath": "/usr/bin/gcc",
+            "cStandard": "c17",
+            "cppStandard": "gnu++17",
+            "intelliSenseMode": "linux-gcc-x64"
+        }
+    ],
+    "version": 4
+}

.vscode/settings.json

@@ -0,0 +1,61 @@
+{
+  "C_Cpp.clang_format_path": "",
+  "C_Cpp.clang_format_style": "file:${workspaceFolder}/.clang-format/.clang-format",
+  "C_Cpp.default.browse.databaseFilename": "null",
+  "C_Cpp.errorSquiggles": "enabled",
+  "C_Cpp.intelliSenseCacheSize": 1024,
+  // "C_Cpp.intelliSenseEngine": "disabled",
+  "C_Cpp.intelliSenseMemoryLimit": 2048,
+  "C_Cpp.loggingLevel": "Error",
+  "C_Cpp.workspaceParsingPriority": "low",
+  "[cpp]": {
+    "editor.defaultFormatter": "ms-vscode.cpptools"
+  },
+  "[markdown]": {
+    "files.trimTrailingWhitespace": false
+  },
+  // Code Spell Checker setting
+  "cSpell.ignoreWords": [
+    "buildtool",
+    "colcon",
+    "crba",
+    "cwise",
+    "Eigen",
+    "FBML",
+    "GNUCXX",
+    "Imai",
+    "Jacobians",
+    "ldlt",
+    "Masazumi",
+    "Multibody",
+    "njoints",
+    "RNEA",
+    "schematypens",
+    "URDF",
+    "Wextra",
+    "Wpedantic"
+  ],
+  "cmake.ignoreCMakeListsMissing": true,
+  "editor.formatOnSave": true,
+  "editor.hover.delay": 600,
+  "editor.renderWhitespace": "all",
+  "editor.tabSize": 2,
+  "editor.trimAutoWhitespace": true,
+  "files.associations": {},
+  "files.insertFinalNewline": true,
+  "files.trimTrailingWhitespace": true,
+  "markdownlint.config": {
+    "MD033": false,
+    "MD041": false
+  },
+  "python.analysis.extraPaths": [
+    "/opt/ros/humble/lib/python3.10/site-packages",
+    "/opt/ros/humble/local/lib/python3.10/dist-packages"
+  ],
+  "python.autoComplete.extraPaths": [
+    "/opt/ros/humble/lib/python3.10/site-packages",
+    "/opt/ros/humble/local/lib/python3.10/dist-packages"
+  ],
+  "ros.distro": "humble",
+  "cSpell.words": []
+}

README.md

@@ -11,11 +11,12 @@
 
 ```bash
 # Clone repository
-mkdir -p ~/fbml/src
-cd ~/fbml/src
+mkdir -p ~/fbml_ws/src
+cd ~/fbml_ws/src
 git clone https://github.com/MasazumiImai/fbml.git
 
 # Build
+cd ~/fbml_ws
 colcon build --packages-select fbml --symlink-install
 source install/setup.bash
 ```

include/fbml/core.hpp

@@ -17,6 +17,7 @@
 
 #include <Eigen/Dense>
 #include <string>
+#include <vector>
 
 #include <pinocchio/multibody/model.hpp>
 
@@ -40,6 +41,9 @@ class FBML_PUBLIC RobotCore
   void setActuatorParameters(
     const Eigen::VectorXd & armature_vector, const Eigen::VectorXd & damping_vector);
 
+  std::vector<std::string> getJointNamesBetweenFrames(
+    const std::string & start_frame_name, const std::string & end_frame_name) const;
+
 private:
   pinocchio::Model model_;
 };

src/core.cpp

@@ -67,4 +67,60 @@ void RobotCore::setActuatorParameters(
   model_.friction.tail(num_actuated_joints) = damping_vector;
 }
 
+std::vector<std::string> RobotCore::getJointNamesBetweenFrames(
+  const std::string & start_frame_name, const std::string & end_frame_name) const
+{
+  if (!model_.existFrame(start_frame_name)) {
+    throw std::invalid_argument("Frame '" + start_frame_name + "' does not exist.");
+  }
+  if (!model_.existFrame(end_frame_name)) {
+    throw std::invalid_argument("Frame '" + end_frame_name + "' does not exist.");
+  }
+
+  pinocchio::FrameIndex start_frame_id = model_.getFrameId(start_frame_name);
+  pinocchio::FrameIndex end_frame_id = model_.getFrameId(end_frame_name);
+
+  pinocchio::JointIndex start_joint = model_.frames[start_frame_id].parentJoint;
+  pinocchio::JointIndex end_joint = model_.frames[end_frame_id].parentJoint;
+
+  auto getPathToRoot = [this](pinocchio::JointIndex j) {
+    std::vector<pinocchio::JointIndex> path;
+    while (j > 0) {
+      path.push_back(j);
+      j = model_.parents[j];
+    }
+    return path;
+  };
+
+  std::vector<pinocchio::JointIndex> path_start = getPathToRoot(start_joint);
+  std::vector<pinocchio::JointIndex> path_end = getPathToRoot(end_joint);
+
+  int idx_start = path_start.size() - 1;
+  int idx_end = path_end.size() - 1;
+
+  // Find common ancestor
+  while (idx_start >= 0 && idx_end >= 0 && path_start[idx_start] == path_end[idx_end]) {
+    idx_start--;
+    idx_end--;
+  }
+
+  std::vector<pinocchio::JointIndex> final_path_ids;
+
+  // Start to common ancestor
+  for (int i = 0; i <= idx_start; ++i) {
+    final_path_ids.push_back(path_start[i]);
+  }
+  // Common ancestor to end
+  for (int i = idx_end; i >= 0; --i) {
+    final_path_ids.push_back(path_end[i]);
+  }
+
+  std::vector<std::string> joint_names;
+  for (const auto & j_id : final_path_ids) {
+    joint_names.push_back(model_.names[j_id]);
+  }
+
+  return joint_names;
+}
+
 }  // namespace fbml