diff --git a/.clang-format/.clang-format b/.clang-format/.clang-format index ac12564..2fc64ef 100644 --- a/.clang-format/.clang-format +++ b/.clang-format/.clang-format @@ -62,8 +62,8 @@ UseTab: Never # Include IncludeBlocks: Regroup IncludeCategories: - # Headers containing "trajectory_generator" - - Regex: ".*trajectory_generator.*" + # Headers containing "traj_gen" + - Regex: ".*traj_gen.*" Priority: 5 CaseSensitive: true # Project's headers diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml new file mode 100644 index 0000000..cc141d0 --- /dev/null +++ b/.github/workflows/build.yml @@ -0,0 +1,40 @@ +name: ROS 2 CI + +on: + push: + branches: "main" + pull_request: + branches: "main" + +jobs: + build-and-test: + runs-on: ubuntu-22.04 + container: + image: ros:humble + + steps: + - name: Install tools + run: apt-get update && apt-get install -y git + + - name: Checkout repository + uses: actions/checkout@v4 + with: + path: src/trajectory_generator + + - name: Install dependencies + run: | + apt-get update + apt-get install -y python3-colcon-common-extensions + rosdep update + rosdep install --from-paths src --ignore-src -y + + - name: Build package + run: | + . /opt/ros/humble/setup.sh + colcon build --packages-select traj_gen --cmake-args -DBUILD_TESTING=ON + + - name: Run tests + run: | + . /opt/ros/humble/setup.sh + colcon test --packages-select traj_gen + colcon test-result --all diff --git a/CMakeLists.txt b/CMakeLists.txt index af92306..38017cf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(trajectory_generator) +project(traj_gen) set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) @@ -14,7 +14,7 @@ find_package(ament_cmake_gtest REQUIRED) # library add_library(${PROJECT_NAME} SHARED - src/trajectory_generator.cpp + src/common.cpp src/spline.cpp ) @@ -26,7 +26,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC ) target_compile_definitions(${PROJECT_NAME} - PRIVATE "TRAJECTORY_GENERATOR_BUILDING_LIBRARY" + PRIVATE "TRAJ_GEN_BUILDING_LIBRARY" ) # install setting @@ -80,14 +80,14 @@ if(BUILD_TESTING) find_package(GTest REQUIRED) - add_executable(unit_test_trajectory_generator - test/unit_test_trajectory_generator.cpp + add_executable(unit_test_traj_gen + test/unit_test_traj_gen.cpp ) - target_link_libraries(unit_test_trajectory_generator + target_link_libraries(unit_test_traj_gen ${PROJECT_NAME} GTest::gtest ) include(GoogleTest) - gtest_discover_tests(unit_test_trajectory_generator) + gtest_discover_tests(unit_test_traj_gen) endif() diff --git a/README.md b/README.md index d0a7e04..5d462a6 100644 --- a/README.md +++ b/README.md @@ -1,35 +1,123 @@ # trajectory_generator -## How to Install +[![Build Status](https://github.com/MasazumiImai/trajectory_generator/actions/workflows/build.yml/badge.svg)](https://github.com/MasazumiImai/trajectory_generator/actions) +[![License](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0) -```bash -mkdir -p ~/ros2_ws/src && ~/ros2_ws/src -git clone https://github.com/MasazumiImai/trajectory_generator.git -``` +A lightweight and extensible C++ library for generating and interpolating trajectories for robotics. It provides robust interpolation for N-dimensional vectors (e.g., joint angles, task-space positions) and SO(3) orientations (quaternions). + +This package can be used as a standalone C++ library or integrated seamlessly as a ROS 2 (`ament_cmake`) package. + +## Features -## How to Build +* **N-Dimensional Vector Trajectory:** Generate smooth spline trajectories with position, velocity, and acceleration constraints. +* **SO(3) Orientation Trajectory:** Perform accurate quaternion interpolation in the SO(3) space using Exponential and Logarithmic maps (`expMap` / `logMap`). +* **ROS 2 Ready:** Fully compatible with the ROS 2 build system (`colcon`). -### C++ +## Installation & Build + +### Prerequisites + +* C++ 17 or higher +* Eigen3 +* ROS 2 (Optional, for `colcon` build) + +### As a Standalone C++ Library ```bash +git clone https://github.com/MasazumiImai/trajectory_generator.git cd trajectory_generator -mkdir -p build && cd build -cmake .. && make && sudo make install +mkdir build && cd build +cmake .. +make +sudo make install ``` -### ROS 2 +### As a ROS 2 Package ```bash +mkdir -p ~/ros2_ws/src && cd ~/ros2_ws/src +git clone https://github.com/MasazumiImai/trajectory_generator.git cd ~/ros2_ws -colcon build --packages-select trajectory_generator && . install/setup.bash +colcon build --packages-select traj_gen +source install/setup.bash ``` -## How to Test +## Quick Start / Usage + +### For Pure C++ Projects + +If you are using standard CMake without ROS 2, simply find the package and link it in your `CMakeLists.txt`: + +```CMake +find_package(traj_gen REQUIRED) + +add_executable(your_app src/main.cpp) +target_link_libraries(your_app traj_gen::traj_gen) +``` + +### For ROS 2 Projects + +If you are using this library within a ROS 2 package, add the dependency to your `package.xml`: + +```xml +traj_gen +```` -### C++ +Then, configure your `CMakeLists.txt`: + +```CMake +find_package(traj_gen REQUIRED) + +add_executable(your_node src/your_node.cpp) +ament_target_dependencies(your_node traj_gen) +``` + +### C++ Example + +Include the master header to access all trajectory generation features. + +```C++ +#include +#include + +int main() { + // 1. Define constraints + int dof = 3; + + traj_gen::VectorStateConstraint start; + start.time = 0.0; + start.position = Eigen::VectorXd::Zero(dof); + start.velocity = Eigen::VectorXd::Zero(dof); + + traj_gen::VectorStateConstraint end; + end.time = 5.0; + end.position = Eigen::VectorXd::Ones(dof) * 1.5; + end.velocity = Eigen::VectorXd::Zero(dof); + + std::vector constraints = traj_gen::createBoundaryConditions(start, end); + + // 2. Generate Spline Trajectory + std::shared_ptr planner = std::make_shared(constraints, dof); + + // 3. Get state at specific time + double current_time = 2.5; + Eigen::VectorXd pos = planner->getPosition(current_time); + Eigen::VectorXd vel = planner->getVelocity(current_time); + + std::cout << "Position at t=2.5: \n" << pos << std::endl; + + return 0; +} +``` + +## Testing + +If you want to run the unit tests: ```bash +cd trajectory_generator mkdir -p build && cd build -cmake .. -DBUILD_TESTING=ON && make +cmake .. -DBUILD_TESTING=ON +make ctest -V ``` diff --git a/include/traj_gen/base.hpp b/include/traj_gen/base.hpp new file mode 100644 index 0000000..4c8e394 --- /dev/null +++ b/include/traj_gen/base.hpp @@ -0,0 +1,47 @@ +// Copyright (c) 2025 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 TRAJ_GEN__BASE_HPP_ +#define TRAJ_GEN__BASE_HPP_ + +#include + +#include "traj_gen/visibility_control.h" + +namespace traj_gen +{ + +class TRAJ_GEN_PUBLIC VectorTrajectoryBase +{ +public: + virtual ~VectorTrajectoryBase() = default; + + virtual Eigen::VectorXd getPosition(double time) = 0; + + virtual Eigen::VectorXd getVelocity(double time) = 0; +}; + +class TRAJ_GEN_PUBLIC OrientationTrajectoryBase +{ +public: + virtual ~OrientationTrajectoryBase() = default; + + virtual Eigen::Quaterniond getOrientation(double time) = 0; + + virtual Eigen::Vector3d getAngularVelocity(double time) = 0; +}; + +} // namespace traj_gen + +#endif // TRAJ_GEN__BASE_HPP_ diff --git a/include/trajectory_generator/trajectory_generator.hpp b/include/traj_gen/common.hpp similarity index 81% rename from include/trajectory_generator/trajectory_generator.hpp rename to include/traj_gen/common.hpp index 3ca928c..de220d6 100644 --- a/include/trajectory_generator/trajectory_generator.hpp +++ b/include/traj_gen/common.hpp @@ -12,19 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_GENERATOR__TRAJECTORY_GENERATOR_HPP_ -#define TRAJECTORY_GENERATOR__TRAJECTORY_GENERATOR_HPP_ +#ifndef TRAJ_GEN__COMMON_HPP_ +#define TRAJ_GEN__COMMON_HPP_ #include -#include -#include -#include #include -#include +#include -#include "trajectory_generator/visibility_control.hpp" +#include "traj_gen/visibility_control.h" -namespace trajectory_generator +namespace traj_gen { /** @@ -41,14 +38,14 @@ struct VectorStateConstraint /** * @brief Create a list of constraints with boundary conditions (start and end points) for vectors. */ -TRAJECTORY_GENERATOR_PUBLIC +TRAJ_GEN_PUBLIC std::vector createBoundaryConditions( const VectorStateConstraint & start_constraint, const VectorStateConstraint & end_constraint); /** * @brief Add a new constraint to the existing constraint list. */ -TRAJECTORY_GENERATOR_PUBLIC +TRAJ_GEN_PUBLIC void addConstraint( std::vector & constraints, const VectorStateConstraint & new_constraint); @@ -66,25 +63,25 @@ struct AngularStateConstraint /** * @brief Create a list of constraints with boundary conditions (start and end points) for orientation. */ -TRAJECTORY_GENERATOR_PUBLIC +TRAJ_GEN_PUBLIC std::vector createBoundaryConditions( const AngularStateConstraint & start_constraint, const AngularStateConstraint & end_constraint); /** * @brief Add a new constraint to the existing constraint list. */ -TRAJECTORY_GENERATOR_PUBLIC +TRAJ_GEN_PUBLIC void addConstraint( std::vector & constraints, const AngularStateConstraint & new_constraint); // --- Mathematical utility functions for quaternion calculations --- -TRAJECTORY_GENERATOR_PUBLIC +TRAJ_GEN_PUBLIC Eigen::Quaterniond expMap(const Eigen::Vector3d & omega); -TRAJECTORY_GENERATOR_PUBLIC +TRAJ_GEN_PUBLIC Eigen::Vector3d logMap(const Eigen::Quaterniond & q); -} // namespace trajectory_generator +} // namespace traj_gen -#endif // TRAJECTORY_GENERATOR__TRAJECTORY_GENERATOR_HPP_ +#endif // TRAJ_GEN__COMMON_HPP_ diff --git a/include/trajectory_generator/spline.hpp b/include/traj_gen/spline.hpp similarity index 73% rename from include/trajectory_generator/spline.hpp rename to include/traj_gen/spline.hpp index 07fac8a..5851bfe 100644 --- a/include/trajectory_generator/spline.hpp +++ b/include/traj_gen/spline.hpp @@ -12,36 +12,34 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_GENERATOR__SPLINE_HPP_ -#define TRAJECTORY_GENERATOR__SPLINE_HPP_ +#ifndef TRAJ_GEN__SPLINE_HPP_ +#define TRAJ_GEN__SPLINE_HPP_ #include -#include #include -#include -#include +#include -#include "trajectory_generator/trajectory_generator.hpp" -#include "trajectory_generator/visibility_control.hpp" +#include "traj_gen/base.hpp" +#include "traj_gen/common.hpp" +#include "traj_gen/visibility_control.h" -namespace trajectory_generator +namespace traj_gen { /** * @brief Class for generating and managing spline trajectories of N-dimensional vectors (joint angles, positions, etc.). */ -class VectorSpline +class TRAJ_GEN_PUBLIC VectorSpline : public VectorTrajectoryBase { public: - TRAJECTORY_GENERATOR_PUBLIC explicit VectorSpline(const std::vector & constraints, int dof); - virtual ~VectorSpline() = default; + ~VectorSpline() override = default; /** @brief Get position vector at specified time. */ - Eigen::VectorXd getPosition(double time); + Eigen::VectorXd getPosition(double time) override; /** @brief Get velocity vector at specified time. */ - Eigen::VectorXd getVelocity(double time); + Eigen::VectorXd getVelocity(double time) override; private: static std::map> constraintsToMap( @@ -57,18 +55,17 @@ class VectorSpline /** * @brief Class for generating and managing spline trajectories of orientation (quaternion). */ -class OrientationSpline +class TRAJ_GEN_PUBLIC OrientationSpline : public OrientationTrajectoryBase { public: - TRAJECTORY_GENERATOR_PUBLIC explicit OrientationSpline(const std::vector & constraints); - virtual ~OrientationSpline() = default; + ~OrientationSpline() override = default; /** @brief Get orientation (quaternion) at specified time. */ - Eigen::Quaterniond getOrientation(double time); + Eigen::Quaterniond getOrientation(double time) override; /** @brief Get angular velocity vector at specified time. */ - Eigen::Vector3d getAngularVelocity(double time); + Eigen::Vector3d getAngularVelocity(double time) override; private: static std::vector buildVectorConstraints( @@ -78,6 +75,6 @@ class OrientationSpline Eigen::Quaterniond start_orientation_; // Reference starting orientation }; -} // namespace trajectory_generator +} // namespace traj_gen -#endif // TRAJECTORY_GENERATOR__SPLINE_HPP_ +#endif // TRAJ_GEN__SPLINE_HPP_ diff --git a/include/traj_gen/traj_gen.hpp b/include/traj_gen/traj_gen.hpp new file mode 100644 index 0000000..e4f19e5 --- /dev/null +++ b/include/traj_gen/traj_gen.hpp @@ -0,0 +1,22 @@ +// Copyright (c) 2025 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 TRAJ_GEN__TRAJ_GEN_HPP_ +#define TRAJ_GEN__TRAJ_GEN_HPP_ + +#include "traj_gen/base.hpp" +#include "traj_gen/common.hpp" +#include "traj_gen/spline.hpp" + +#endif // TRAJ_GEN__TRAJ_GEN_HPP_ diff --git a/include/traj_gen/visibility_control.h b/include/traj_gen/visibility_control.h new file mode 100644 index 0000000..16f600e --- /dev/null +++ b/include/traj_gen/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright (c) 2025 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 TRAJ_GEN__VISIBILITY_CONTROL_H_ +#define TRAJ_GEN__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 TRAJ_GEN_EXPORT __attribute__((dllexport)) +#define TRAJ_GEN_IMPORT __attribute__((dllimport)) +#else +#define TRAJ_GEN_EXPORT __declspec(dllexport) +#define TRAJ_GEN_IMPORT __declspec(dllimport) +#endif +#ifdef TRAJ_GEN_BUILDING_LIBRARY +#define TRAJ_GEN_PUBLIC TRAJ_GEN_EXPORT +#else +#define TRAJ_GEN_PUBLIC TRAJ_GEN_IMPORT +#endif +#define TRAJ_GEN_PUBLIC_TYPE TRAJ_GEN_PUBLIC +#define TRAJ_GEN_LOCAL +#else +#define TRAJ_GEN_EXPORT __attribute__((visibility("default"))) +#define TRAJ_GEN_IMPORT +#if __GNUC__ >= 4 +#define TRAJ_GEN_PUBLIC __attribute__((visibility("default"))) +#define TRAJ_GEN_LOCAL __attribute__((visibility("hidden"))) +#else +#define TRAJ_GEN_PUBLIC +#define TRAJ_GEN_LOCAL +#endif +#define TRAJ_GEN_PUBLIC_TYPE +#endif + +#endif // TRAJ_GEN__VISIBILITY_CONTROL_H_ diff --git a/include/trajectory_generator/visibility_control.hpp b/include/trajectory_generator/visibility_control.hpp deleted file mode 100644 index 8d2850c..0000000 --- a/include/trajectory_generator/visibility_control.hpp +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright (c) 2025 Space Robotics Lab -- Tohoku University -// -// 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 TRAJECTORY_GENERATOR__VISIBILITY_CONTROL_HPP_ -#define TRAJECTORY_GENERATOR__VISIBILITY_CONTROL_HPP_ - -// 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 TRAJECTORY_GENERATOR_EXPORT __attribute__((dllexport)) -#define TRAJECTORY_GENERATOR_IMPORT __attribute__((dllimport)) -#else -#define TRAJECTORY_GENERATOR_EXPORT __declspec(dllexport) -#define TRAJECTORY_GENERATOR_IMPORT __declspec(dllimport) -#endif -#ifdef TRAJECTORY_GENERATOR_BUILDING_LIBRARY -#define TRAJECTORY_GENERATOR_PUBLIC TRAJECTORY_GENERATOR_EXPORT -#else -#define TRAJECTORY_GENERATOR_PUBLIC TRAJECTORY_GENERATOR_IMPORT -#endif -#define TRAJECTORY_GENERATOR_PUBLIC_TYPE TRAJECTORY_GENERATOR_PUBLIC -#define TRAJECTORY_GENERATOR_LOCAL -#else -#define TRAJECTORY_GENERATOR_EXPORT __attribute__((visibility("default"))) -#define TRAJECTORY_GENERATOR_IMPORT -#if __GNUC__ >= 4 -#define TRAJECTORY_GENERATOR_PUBLIC __attribute__((visibility("default"))) -#define TRAJECTORY_GENERATOR_LOCAL __attribute__((visibility("hidden"))) -#else -#define TRAJECTORY_GENERATOR_PUBLIC -#define TRAJECTORY_GENERATOR_LOCAL -#endif -#define TRAJECTORY_GENERATOR_PUBLIC_TYPE -#endif - -#endif // TRAJECTORY_GENERATOR__VISIBILITY_CONTROL_HPP_ diff --git a/package.xml b/package.xml index 53d6673..a1e4cb3 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ - trajectory_generator + traj_gen 0.0.0 Trajectory generation library (C++ / ROS 2 compatible) Masazumi Imai diff --git a/src/trajectory_generator.cpp b/src/common.cpp similarity index 94% rename from src/trajectory_generator.cpp rename to src/common.cpp index 50c3a03..47b25b2 100644 --- a/src/trajectory_generator.cpp +++ b/src/common.cpp @@ -12,11 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "trajectory_generator/trajectory_generator.hpp" +#include "traj_gen/common.hpp" -#define DEBUG false - -namespace trajectory_generator +namespace traj_gen { std::vector createBoundaryConditions( @@ -85,4 +83,4 @@ Eigen::Vector3d logMap(const Eigen::Quaterniond & q) return omega; } -} // namespace trajectory_generator +} // namespace traj_gen diff --git a/src/spline.cpp b/src/spline.cpp index 66e5173..a278a86 100644 --- a/src/spline.cpp +++ b/src/spline.cpp @@ -12,11 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "trajectory_generator/spline.hpp" +#include "traj_gen/spline.hpp" -#define DEBUG false +#include +#include -namespace trajectory_generator +namespace traj_gen { VectorSpline::VectorSpline(const std::vector & constraints, int dof) @@ -149,4 +150,4 @@ std::vector OrientationSpline::buildVectorConstraints( return vector_constraints; } -} // namespace trajectory_generator +} // namespace traj_gen diff --git a/test/unit_test_trajectory_generator.cpp b/test/unit_test_traj_gen.cpp similarity index 97% rename from test/unit_test_trajectory_generator.cpp rename to test/unit_test_traj_gen.cpp index c78e09a..a14157e 100644 --- a/test/unit_test_trajectory_generator.cpp +++ b/test/unit_test_traj_gen.cpp @@ -14,10 +14,11 @@ #include -#include "trajectory_generator/spline.hpp" -#include "trajectory_generator/trajectory_generator.hpp" +#include -namespace trajectory_generator +#include "traj_gen/spline.hpp" + +namespace traj_gen { TEST(TrajectoryGenerator, VectorSplineTaskSpace) @@ -168,7 +169,7 @@ TEST(TrajectoryGenerator, OrientationSpline) } } -} // namespace trajectory_generator +} // namespace traj_gen int main(int argc, char ** argv) {