diff --git a/.clang-format/.clang-format b/.clang-format/.clang-format new file mode 100644 index 0000000..2de0069 --- /dev/null +++ b/.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 diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml new file mode 100644 index 0000000..481a201 --- /dev/null +++ b/.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: "" diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..9aa3b2a --- /dev/null +++ b/.gitignore @@ -0,0 +1,5 @@ +build/ +install +log/ + +.vscode/ diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..9e00600 --- /dev/null +++ b/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 + $ + $ +) + +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() diff --git a/README.md b/README.md index 5b418b0..67a11eb 100644 --- a/README.md +++ b/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 +fbml +``` + +#### Example of an include in C++ + +```cpp +#include +#include +#include + +fbml::RobotCore robot("model.urdf"); +fbml::Kinematics kin(robot); +``` diff --git a/include/fbml/core.hpp b/include/fbml/core.hpp new file mode 100644 index 0000000..ca2592f --- /dev/null +++ b/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 +#include + +#include + +#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_ diff --git a/include/fbml/dynamics.hpp b/include/fbml/dynamics.hpp new file mode 100644 index 0000000..4de0616 --- /dev/null +++ b/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 +#include + +#include + +#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_ diff --git a/include/fbml/kinematics.hpp b/include/fbml/kinematics.hpp new file mode 100644 index 0000000..7e3b3fd --- /dev/null +++ b/include/fbml/kinematics.hpp @@ -0,0 +1,89 @@ +// 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__KINEMATICS_HPP_ +#define FBML__KINEMATICS_HPP_ + +#include +#include +#include + +#include +#include + +#include "fbml/core.hpp" +#include "fbml/types.hpp" +#include "fbml/visibility_control.h" + +namespace fbml +{ + +struct IKSettings +{ + double tolerance = 1e-4; + int max_iterations = 100; + double damping_factor = 1e-2; + + // Weights for errors on each axis [vx, vy, vz, wx, wy, wz] + fbml::Vector6d task_weights = fbml::Vector6d::Ones(); +}; + +class FBML_PUBLIC Kinematics +{ +public: + explicit Kinematics(const RobotCore & core); + virtual ~Kinematics() = default; + + Eigen::MatrixXd computeJacobian( + const Eigen::VectorXd & q, const std::string & frame_name, + pinocchio::ReferenceFrame reference_frame = pinocchio::LOCAL); + + pinocchio::SE3 solveFK( + const Eigen::VectorXd & q, const std::string & target_frame, + const std::string & reference_frame = "world"); + + bool solveNumericalIK( + Eigen::VectorXd & q, const std::string & frame_name, const pinocchio::SE3 & desired_pose, + const std::vector & joint_names, const std::string & reference_frame = "world", + const IKSettings & settings = IKSettings()); + + bool solveNumericalIK( + Eigen::VectorXd & q, const std::string & frame_name, const Eigen::Isometry3d & desired_pose, + const std::vector & joint_names, const std::string & reference_frame = "world", + const IKSettings & settings = IKSettings()) + { + pinocchio::SE3 pose_se3(desired_pose.matrix()); + return solveNumericalIK(q, frame_name, pose_se3, joint_names, reference_frame, settings); + } + + bool solvePositionIK( + Eigen::VectorXd & q, const std::string & frame_name, const Eigen::Vector3d & desired_position, + const std::vector & joint_names, const std::string & reference_frame = "world", + IKSettings settings = IKSettings()) + { + settings.task_weights << 1.0, 1.0, 1.0, 0.0, 0.0, 0.0; // Only for position + + pinocchio::SE3 desired_pose(Eigen::Matrix3d::Identity(), desired_position); + + return solveNumericalIK(q, frame_name, desired_pose, joint_names, reference_frame, settings); + } + +private: + const pinocchio::Model & model_; + pinocchio::Data data_; +}; + +} // namespace fbml + +#endif // FBML__KINEMATICS_HPP_ diff --git a/include/fbml/types.hpp b/include/fbml/types.hpp new file mode 100644 index 0000000..c203b0a --- /dev/null +++ b/include/fbml/types.hpp @@ -0,0 +1,28 @@ +// 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__TYPES_HPP_ +#define FBML__TYPES_HPP_ + +#include + +namespace fbml +{ + +using Vector6d = Eigen::Matrix; +using Matrix6d = Eigen::Matrix; + +} // namespace fbml + +#endif // FBML__TYPES_HPP_ diff --git a/include/fbml/visibility_control.h b/include/fbml/visibility_control.h new file mode 100644 index 0000000..dbb628c --- /dev/null +++ b/include/fbml/visibility_control.h @@ -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__VISIBILITY_CONTROL_H_ +#define FBML__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define FBML_EXPORT __attribute__((dllexport)) +#define FBML_IMPORT __attribute__((dllimport)) +#else +#define FBML_EXPORT __declspec(dllexport) +#define FBML_IMPORT __declspec(dllimport) +#endif +#ifdef FBML_BUILDING_LIBRARY +#define FBML_PUBLIC FBML_EXPORT +#else +#define FBML_PUBLIC FBML_IMPORT +#endif +#define FBML_PUBLIC_TYPE FBML_PUBLIC +#define FBML_LOCAL +#else +#define FBML_EXPORT __attribute__((visibility("default"))) +#define FBML_IMPORT +#if __GNUC__ >= 4 +#define FBML_PUBLIC __attribute__((visibility("default"))) +#define FBML_LOCAL __attribute__((visibility("hidden"))) +#else +#define FBML_PUBLIC +#define FBML_LOCAL +#endif +#define FBML_PUBLIC_TYPE +#endif + +#endif // FBML__VISIBILITY_CONTROL_H_ diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..e567e54 --- /dev/null +++ b/package.xml @@ -0,0 +1,28 @@ + + + + fbml + 0.0.0 + Floating-Base Multibody Library (powered by Pinocchio) + Masazumi Imai + Apache-2.0 + + ament_cmake + eigen3_cmake_module + + eigen + pinocchio + + + + + ament_cmake + + diff --git a/src/core.cpp b/src/core.cpp new file mode 100644 index 0000000..7caa129 --- /dev/null +++ b/src/core.cpp @@ -0,0 +1,70 @@ +// 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. + +#include "fbml/core.hpp" + +#include +#include + +#include + +namespace fbml +{ + +RobotCore::RobotCore(const std::string & urdf_path, const Eigen::Vector3d & gravity) +{ + try { + pinocchio::urdf::buildModel(urdf_path, pinocchio::JointModelFreeFlyer(), model_); + + model_.gravity.linear() = gravity; + + std::cout << "[fbml] Successfully loaded URDF." << std::endl; + std::cout << "[fbml] - Number of joints: " << model_.njoints << std::endl; + std::cout << "[fbml] - Degrees of freedom (nv): " << model_.nv << std::endl; + std::cout << "[fbml] - Configuration size (nq): " << model_.nq << std::endl; + + } catch (const std::exception & e) { + std::cerr << "[fbml] Failed to build Pinocchio model from URDF: " << urdf_path << std::endl; + std::cerr << e.what() << std::endl; + throw; + } +} + +void RobotCore::setActuatorParameters(double armature, double damping) +{ + const int num_actuated_joints = model_.nv - 6; + + if (num_actuated_joints > 0) { + model_.rotorInertia.tail(num_actuated_joints).setConstant(armature); + model_.friction.tail(num_actuated_joints).setConstant(damping); + } +} + +void RobotCore::setActuatorParameters( + const Eigen::VectorXd & armature_vector, const Eigen::VectorXd & damping_vector) +{ + const int num_actuated_joints = model_.nv - 6; + + if ( + armature_vector.size() != num_actuated_joints || damping_vector.size() != num_actuated_joints) { + throw std::invalid_argument( + "[fbml] Size of parameter vectors must match the number of actuated joints (" + + std::to_string(num_actuated_joints) + ")."); + } + + model_.rotorInertia.tail(num_actuated_joints) = armature_vector; + model_.friction.tail(num_actuated_joints) = damping_vector; +} + +} // namespace fbml diff --git a/src/dynamics.cpp b/src/dynamics.cpp new file mode 100644 index 0000000..b9178a2 --- /dev/null +++ b/src/dynamics.cpp @@ -0,0 +1,94 @@ +// 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. + +#include "fbml/dynamics.hpp" + +#include + +#include +#include +#include +#include + +namespace fbml +{ + +Dynamics::Dynamics(const RobotCore & core) : model_(core.getModel()), data_(pinocchio::Data(model_)) +{ +} + +Eigen::MatrixXd Dynamics::computeMassMatrix(const Eigen::VectorXd & q) +{ + pinocchio::crba(model_, data_, q); + data_.M.triangularView() = + data_.M.transpose().triangularView(); + return data_.M; +} + +void Dynamics::computePartitionedMassMatrices( + const Eigen::VectorXd & q, Eigen::MatrixXd & M_b, Eigen::MatrixXd & M_bm) +{ + pinocchio::crba(model_, data_, q); + data_.M.triangularView() = + data_.M.transpose().triangularView(); + + const int njoints = model_.nv - 6; + + M_b = data_.M.block(0, 0, 6, 6); + M_bm = data_.M.block(0, 6, 6, njoints); +} + +Eigen::VectorXd Dynamics::computeNonLinearEffects( + const Eigen::VectorXd & q, const Eigen::VectorXd & v) +{ + // Non-linear term (coriolis + gravity) (RNEA: Recursive Newton-Euler Algorithm) + return pinocchio::nonLinearEffects(model_, data_, q, v); +} + +Eigen::MatrixXd Dynamics::computeGeneralizedJacobian( + const Eigen::VectorXd & q, const std::string & frame_name, + pinocchio::ReferenceFrame reference_frame) +{ + if (!model_.existFrame(frame_name)) { + throw std::invalid_argument("Frame '" + frame_name + "' does not exist."); + } + pinocchio::FrameIndex frame_id = model_.getFrameId(frame_name); + + pinocchio::computeJointJacobians(model_, data_, q); + pinocchio::updateFramePlacements(model_, data_); + + // Inertial matrix M (Composite Rigid Body Algorithm) + pinocchio::crba(model_, data_, q); + data_.M.triangularView() = + data_.M.transpose().triangularView(); + + pinocchio::Data::Matrix6x J_full(6, model_.nv); + J_full.setZero(); + pinocchio::getFrameJacobian(model_, data_, frame_id, reference_frame, J_full); + + const int njoints = model_.nv - 6; + + fbml::Matrix6d M_b = data_.M.block<6, 6>(0, 0); + Eigen::MatrixXd M_bm = data_.M.block(0, 6, njoints, 6).transpose(); + + fbml::Matrix6d J_b = J_full.block<6, 6>(0, 0); + Eigen::MatrixXd J_m = J_full.block(0, 6, 6, njoints); + + // J* = J_m - J_b * M_b^{-1} * M_bm + Eigen::MatrixXd J_g = J_m - J_b * M_b.llt().solve(M_bm); + + return J_g; +} + +} // namespace fbml diff --git a/src/kinematics.cpp b/src/kinematics.cpp new file mode 100644 index 0000000..ce8e739 --- /dev/null +++ b/src/kinematics.cpp @@ -0,0 +1,170 @@ +// 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. + +#include "fbml/kinematics.hpp" + +#include + +#include +#include +#include +#include + +namespace fbml +{ + +Kinematics::Kinematics(const RobotCore & core) +: model_(core.getModel()), data_(pinocchio::Data(model_)) +{ +} + +Eigen::MatrixXd Kinematics::computeJacobian( + const Eigen::VectorXd & q, const std::string & frame_name, + pinocchio::ReferenceFrame reference_frame) +{ + if (!model_.existFrame(frame_name)) { + throw std::invalid_argument("Frame '" + frame_name + "' does not exist in the model."); + } + + pinocchio::computeJointJacobians(model_, data_, q); + pinocchio::updateFramePlacements(model_, data_); + + pinocchio::FrameIndex frame_id = model_.getFrameId(frame_name); + + pinocchio::Data::Matrix6x J(6, model_.nv); + J.setZero(); + + pinocchio::getFrameJacobian(model_, data_, frame_id, reference_frame, J); + + return J; +} + +pinocchio::SE3 Kinematics::solveFK( + const Eigen::VectorXd & q, const std::string & target_frame, const std::string & reference_frame) +{ + if (!model_.existFrame(target_frame)) { + throw std::invalid_argument("Frame '" + target_frame + "' does not exist in the model."); + } + + pinocchio::forwardKinematics(model_, data_, q); + pinocchio::updateFramePlacements(model_, data_); + + pinocchio::FrameIndex target_id = model_.getFrameId(target_frame); + + if (reference_frame == "world") { + pinocchio::SE3 pose_in_world = data_.oMf[target_id]; + return pose_in_world; + } else if (!model_.existFrame(reference_frame)) { + throw std::invalid_argument("Frame '" + reference_frame + "' does not exist in the model."); + } + + pinocchio::FrameIndex ref_id = model_.getFrameId(reference_frame); + + pinocchio::SE3 pose_in_ref = data_.oMf[ref_id].actInv(data_.oMf[target_id]); + return pose_in_ref; +} + +bool Kinematics::solveNumericalIK( + Eigen::VectorXd & q, const std::string & frame_name, const pinocchio::SE3 & desired_pose, + const std::vector & joint_names, const std::string & reference_frame, + const IKSettings & settings) +{ + // TODO: Add kinematic feasibility (joint limits) check (Stop if a joint seems like it’s about to exceed its limit) + + if (!model_.existFrame(frame_name)) { + throw std::invalid_argument("Frame '" + frame_name + "' does not exist."); + } + pinocchio::FrameIndex frame_id = model_.getFrameId(frame_name); + + pinocchio::FrameIndex ref_id = 0; + bool use_ref = (reference_frame != "world"); + if (use_ref) { + if (!model_.existFrame(reference_frame)) { + throw std::invalid_argument("Frame '" + reference_frame + "' does not exist."); + } + ref_id = model_.getFrameId(reference_frame); + } + + std::vector joint_ids; + int sub_nv = 0; + for (const auto & name : joint_names) { + if (!model_.existJointName(name)) { + throw std::invalid_argument("Joint '" + name + "' does not exist."); + } + pinocchio::JointIndex j_id = model_.getJointId(name); + joint_ids.push_back(j_id); + sub_nv += model_.joints[j_id].nv(); + } + + pinocchio::Data::Matrix6x J_sub(6, sub_nv); + pinocchio::Data::Matrix6x J_full(6, model_.nv); + + for (int iter = 0; iter < settings.max_iterations; ++iter) { + pinocchio::computeJointJacobians(model_, data_, q); + pinocchio::updateFramePlacements(model_, data_); + + pinocchio::SE3 des_pose_in_world = desired_pose; + if (use_ref) { + // _oM_ref * _refM_des = _oM_des + des_pose_in_world = data_.oMf[ref_id] * desired_pose; + } + + pinocchio::SE3 T_cur = data_.oMf[frame_id]; + pinocchio::SE3 T_err = T_cur.inverse() * des_pose_in_world; + Eigen::VectorXd error = pinocchio::log6(T_err).toVector(); + + error = settings.task_weights.cwiseProduct(error); + + if (error.norm() < settings.tolerance) { + return true; + } + + J_full.setZero(); + pinocchio::getFrameJacobian(model_, data_, frame_id, pinocchio::LOCAL, J_full); + + int col_offset = 0; + for (const auto & j_id : joint_ids) { + int nv_i = model_.joints[j_id].nv(); + int idx_v = model_.joints[j_id].idx_v(); + J_sub.middleCols(col_offset, nv_i) = J_full.middleCols(idx_v, nv_i); + col_offset += nv_i; + } + + J_sub = settings.task_weights.asDiagonal() * J_sub; + + // Damped Least Squares (DLS) + + // A = J * J^T + lambda * I + Eigen::MatrixXd A = J_sub * J_sub.transpose(); + A.diagonal().array() += settings.damping_factor; + + // dq_sub = J^T * A^-1 * error + Eigen::VectorXd dq_sub = J_sub.transpose() * A.ldlt().solve(error); + + Eigen::VectorXd v_full = Eigen::VectorXd::Zero(model_.nv); + col_offset = 0; + for (const auto & j_id : joint_ids) { + int nv_i = model_.joints[j_id].nv(); + int idx_v = model_.joints[j_id].idx_v(); + v_full.segment(idx_v, nv_i) = dq_sub.segment(col_offset, nv_i); + col_offset += nv_i; + } + + q = pinocchio::integrate(model_, q, v_full); + } + + return false; +} + +} // namespace fbml