Feature/implement core functions (#1)

.clang-format/.clang-format

@@ -0,0 +1,96 @@
+---
+# Modified from https://github.com/ament/ament_lint/blob/humble/ament_clang_format/ament_clang_format/configuration/.clang-format
+Language: Cpp
+
+# Use Google style as a base
+BasedOnStyle: Google
+
+# Overwrite access modifier like "private", "public" by ROS style
+AccessModifierOffset: -2
+
+# New line after opening bracket
+AlignAfterOpenBracket: AlwaysBreak
+
+# Do not align operands of binary and ternary expressions
+AlignOperands: DontAlign
+
+# Do not align trailing comments
+AlignTrailingComments:
+  Kind: Always
+  OverEmptyLines: 0
+
+# Allows short functions to remain on a single line only if they are inline
+AllowShortFunctionsOnASingleLine: InlineOnly
+
+AllowShortIfStatementsOnASingleLine: AllIfsAndElse
+
+# Specifies brace-breaking rules
+BraceWrapping:
+  AfterClass: true
+  AfterControlStatement: Never
+  AfterFunction: true
+  AfterNamespace: true
+  AfterStruct: true
+BreakBeforeBraces: Custom
+
+# Break constructor initializers before the colon and after the commas
+BreakConstructorInitializers: BeforeColon # Dose not work currently
+
+# Overwrite column max length
+ColumnLimit: 100
+
+# Indentation width for constructor initializer lists
+ConstructorInitializerIndentWidth: 0
+
+# Indentation width for line continuations
+ContinuationIndentWidth: 2
+
+# Disables automatic pointer alignment
+DerivePointerAlignment: false
+# Aligns pointers in the middle
+PointerAlignment: Middle
+
+# Formatting off for one line
+OneLineFormatOffRegex: ^(// NOLINT|logger$)
+
+# Disables automatic reformatting of comments
+ReflowComments: false
+
+# Do not use tab
+UseTab: Never
+
+# Include
+IncludeBlocks: Regroup
+IncludeCategories:
+  # Headers containing "fbml"
+  - Regex: ".*fbml.*"
+    Priority: 5
+    CaseSensitive: true
+  # Headers containing "fbml"
+  - Regex: <.*fbml.*>
+    Priority: 5
+    CaseSensitive: true
+  # Project's headers
+  - Regex: '.*\.hpp'
+    Priority: 4
+    CaseSensitive: true
+  # C++ system headers
+  - Regex: <[a-z_]*>
+    Priority: 1
+    CaseSensitive: true
+  # Eigen headers
+  - Regex: <.*Eigen.*>
+    Priority: 1
+    CaseSensitive: true
+  # C system headers
+  - Regex: <.*\.h>
+    Priority: 0
+    CaseSensitive: true
+  # Local package headers
+  - Regex: '".*"'
+    Priority: 3
+    CaseSensitive: true
+  # Other Package headers
+  - Regex: <.*>
+    Priority: 2
+    CaseSensitive: true

.github/workflows/ci.yml

@@ -0,0 +1,35 @@
+name: ROS 2 CI
+
+on:
+  pull_request:
+    branches: [ main ]
+  # push:
+  #   branches: [ main ]
+
+jobs:
+  build-and-test:
+    name: Build and Test on Humble
+    runs-on: ubuntu-22.04
+    strategy:
+      fail-fast: false
+      matrix:
+        ros_distro: [ humble ]
+
+    steps:
+      # Checkout repository
+      - name: Checkout repository
+        uses: actions/checkout@v4
+
+      # Setup ROS 2 env
+      - name: Setup ROS 2
+        uses: ros-tooling/setup-ros@v0.7
+        with:
+          required-ros-distributions: ${{ matrix.ros_distro }}
+
+      # Install dependencies, build, and test
+      - name: Build and Test
+        uses: ros-tooling/action-ros-ci@v0.3
+        with:
+          target-ros2-distro: ${{ matrix.ros_distro }}
+          package-name: fbml
+          vcs-repo-file-url: ""

.gitignore

@@ -0,0 +1,5 @@
+build/
+install
+log/
+
+.vscode/

CMakeLists.txt

@@ -0,0 +1,57 @@
+cmake_minimum_required(VERSION 3.8)
+project(fbml)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(pinocchio REQUIRED)
+
+add_library(${PROJECT_NAME} SHARED
+  src/core.cpp
+  src/kinematics.cpp
+  src/dynamics.cpp
+)
+
+target_include_directories(${PROJECT_NAME} PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
+target_link_libraries(${PROJECT_NAME} PUBLIC
+  Eigen3::Eigen
+  pinocchio::pinocchio
+)
+
+ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET)
+ament_export_dependencies(Eigen3 pinocchio)
+
+install(DIRECTORY include/
+  DESTINATION include
+)
+
+install(TARGETS ${PROJECT_NAME}
+  EXPORT ${PROJECT_NAME}Targets
+  LIBRARY DESTINATION lib
+  ARCHIVE DESTINATION lib
+  RUNTIME DESTINATION bin
+  INCLUDES DESTINATION include
+)
+
+# if(BUILD_TESTING)
+#   find_package(ament_lint_auto REQUIRED)
+#   set(ament_cmake_copyright_FOUND TRUE)
+#   set(ament_cmake_cpplint_FOUND TRUE)
+#   ament_lint_auto_find_test_dependencies()
+
+  # find_package(ament_cmake_gtest REQUIRED)
+  # ament_add_gtest(unit_test_${PROJECT_NAME} test/unit_test_${PROJECT_NAME}.cpp)
+  # if(TARGET unit_test_${PROJECT_NAME})
+  #   target_link_libraries(unit_test_${PROJECT_NAME} ${PROJECT_NAME})
+  # endif()
+# endif()
+
+ament_package()

README.md

@@ -1,2 +1,51 @@
-# fbml
-Floating-Base Multibody Library
+# FBML: Floating-Base Multibody Library
+
+![ROS 2 CI](https://github.com/MasazumiImai/fbml/actions/workflows/ci.yml/badge.svg)
+
+## Requirements
+
+- [ROS 2](https://docs.ros.org/en/humble/index.html) (Humble)
+- [Pinocchio](https://github.com/stack-of-tasks/pinocchio)
+
+## Installation
+
+```bash
+# Clone repository
+mkdir -p ~/fbml/src
+cd ~/fbml/src
+git clone https://github.com/MasazumiImai/fbml.git
+
+# Build
+colcon build --packages-select fbml --symlink-install
+source install/setup.bash
+```
+
+## Usage
+
+If you are using FBML from other ROS 2 packages, please add the following dependencies to `CMakeLists.txt` and `package.xml`.
+
+#### `CMakeLists.txt`
+
+```CMakeLists.txt
+find_package(fbml REQUIRED)
+
+add_executable(your_robot_node src/your_robot_node.cpp)
+ament_target_dependencies(your_robot_node fbml)
+```
+
+#### `package.xml`
+
+```package.xml
+<depend>fbml</depend>
+```
+
+#### Example of an include in C++
+
+```cpp
+#include <fbml/core.hpp>
+#include <fbml/kinematics.hpp>
+#include <fbml/dynamics.hpp>
+
+fbml::RobotCore robot("model.urdf");
+fbml::Kinematics kin(robot);
+```

include/fbml/core.hpp

@@ -0,0 +1,49 @@
+// Copyright (c) 2026 Masazumi Imai
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef FBML__CORE_HPP_
+#define FBML__CORE_HPP_
+
+#include <Eigen/Dense>
+#include <string>
+
+#include <pinocchio/multibody/model.hpp>
+
+#include "fbml/visibility_control.h"
+
+namespace fbml
+{
+
+class FBML_PUBLIC RobotCore
+{
+public:
+  explicit RobotCore(
+    const std::string & urdf_path,
+    const Eigen::Vector3d & gravity = Eigen::Vector3d(0.0, 0.0, -9.81));
+  virtual ~RobotCore() = default;
+
+  const pinocchio::Model & getModel() const { return model_; }
+
+  void setActuatorParameters(double armature, double damping);
+
+  void setActuatorParameters(
+    const Eigen::VectorXd & armature_vector, const Eigen::VectorXd & damping_vector);
+
+private:
+  pinocchio::Model model_;
+};
+
+}  // namespace fbml
+
+#endif  // FBML__CORE_HPP_

include/fbml/dynamics.hpp

@@ -0,0 +1,57 @@
+// Copyright (c) 2026 Masazumi Imai
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef FBML__DYNAMICS_HPP_
+#define FBML__DYNAMICS_HPP_
+
+#include <Eigen/Dense>
+#include <string>
+
+#include <pinocchio/multibody/data.hpp>
+
+#include "fbml/core.hpp"
+#include "fbml/types.hpp"
+
+namespace fbml
+{
+
+class FBML_PUBLIC Dynamics
+{
+public:
+  explicit Dynamics(const RobotCore & core);
+  virtual ~Dynamics() = default;
+
+  Eigen::MatrixXd computeGeneralizedJacobian(
+    const Eigen::VectorXd & q, const std::string & frame_name);
+
+  Eigen::MatrixXd computeMassMatrix(const Eigen::VectorXd & q);
+
+  // ?: Should use struct (PartitionedMassMatrix{fbml::Matrix6d base; Eigen::MatrixXd coupling;}) as output?
+  void computePartitionedMassMatrices(
+    const Eigen::VectorXd & q, Eigen::MatrixXd & M_b, Eigen::MatrixXd & M_bm);
+
+  Eigen::VectorXd computeNonLinearEffects(const Eigen::VectorXd & q, const Eigen::VectorXd & v);
+
+  Eigen::MatrixXd computeGeneralizedJacobian(
+    const Eigen::VectorXd & q, const std::string & frame_name,
+    pinocchio::ReferenceFrame reference_frame = pinocchio::LOCAL);
+
+private:
+  const pinocchio::Model & model_;
+  pinocchio::Data data_;
+};
+
+}  // namespace fbml
+
+#endif  // FBML__DYNAMICS_HPP_