From 249d2d25249feececeb3fcdc747afcab43280cf1 Mon Sep 17 00:00:00 2001 From: MasazumiImai Date: Fri, 15 May 2026 02:09:47 +0900 Subject: [PATCH 1/7] Rename --- .clang-format/.clang-format | 4 +- CMakeLists.txt | 14 +++--- .../spline.hpp | 20 ++++---- .../traj_gen.hpp} | 25 +++++----- include/traj_gen/visibility_control.h | 49 +++++++++++++++++++ .../visibility_control.hpp | 49 ------------------- package.xml | 2 +- src/spline.cpp | 6 +-- ...{trajectory_generator.cpp => traj_gen.cpp} | 8 +-- ...y_generator.cpp => unit_test_traj_gen.cpp} | 8 +-- 10 files changed, 92 insertions(+), 93 deletions(-) rename include/{trajectory_generator => traj_gen}/spline.hpp (85%) rename include/{trajectory_generator/trajectory_generator.hpp => traj_gen/traj_gen.hpp} (83%) create mode 100644 include/traj_gen/visibility_control.h delete mode 100644 include/trajectory_generator/visibility_control.hpp rename src/{trajectory_generator.cpp => traj_gen.cpp} (95%) rename test/{unit_test_trajectory_generator.cpp => unit_test_traj_gen.cpp} (97%) 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/CMakeLists.txt b/CMakeLists.txt index af92306..d9a69f9 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/traj_gen.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/include/trajectory_generator/spline.hpp b/include/traj_gen/spline.hpp similarity index 85% rename from include/trajectory_generator/spline.hpp rename to include/traj_gen/spline.hpp index 07fac8a..462ee73 100644 --- a/include/trajectory_generator/spline.hpp +++ b/include/traj_gen/spline.hpp @@ -12,8 +12,8 @@ // 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 @@ -21,19 +21,18 @@ #include #include -#include "trajectory_generator/trajectory_generator.hpp" -#include "trajectory_generator/visibility_control.hpp" +#include "traj_gen/traj_gen.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: - TRAJECTORY_GENERATOR_PUBLIC explicit VectorSpline(const std::vector & constraints, int dof); virtual ~VectorSpline() = default; @@ -57,10 +56,9 @@ class VectorSpline /** * @brief Class for generating and managing spline trajectories of orientation (quaternion). */ -class OrientationSpline +class TRAJ_GEN_PUBLIC OrientationSpline { public: - TRAJECTORY_GENERATOR_PUBLIC explicit OrientationSpline(const std::vector & constraints); virtual ~OrientationSpline() = default; @@ -78,6 +76,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/trajectory_generator/trajectory_generator.hpp b/include/traj_gen/traj_gen.hpp similarity index 83% rename from include/trajectory_generator/trajectory_generator.hpp rename to include/traj_gen/traj_gen.hpp index 3ca928c..635c34d 100644 --- a/include/trajectory_generator/trajectory_generator.hpp +++ b/include/traj_gen/traj_gen.hpp @@ -12,19 +12,18 @@ // 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__TRAJ_GEN_HPP_ +#define TRAJ_GEN__TRAJ_GEN_HPP_ #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 +40,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 +65,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__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..32d537e --- /dev/null +++ b/include/traj_gen/visibility_control.h @@ -0,0 +1,49 @@ +// 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 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/spline.cpp b/src/spline.cpp index 66e5173..e306edf 100644 --- a/src/spline.cpp +++ b/src/spline.cpp @@ -12,11 +12,11 @@ // 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 -namespace trajectory_generator +namespace traj_gen { VectorSpline::VectorSpline(const std::vector & constraints, int dof) @@ -149,4 +149,4 @@ std::vector OrientationSpline::buildVectorConstraints( return vector_constraints; } -} // namespace trajectory_generator +} // namespace traj_gen diff --git a/src/trajectory_generator.cpp b/src/traj_gen.cpp similarity index 95% rename from src/trajectory_generator.cpp rename to src/traj_gen.cpp index 50c3a03..4e8dbd5 100644 --- a/src/trajectory_generator.cpp +++ b/src/traj_gen.cpp @@ -12,11 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "trajectory_generator/trajectory_generator.hpp" +#include "traj_gen/traj_gen.hpp" + +#include #define DEBUG false -namespace trajectory_generator +namespace traj_gen { std::vector createBoundaryConditions( @@ -85,4 +87,4 @@ Eigen::Vector3d logMap(const Eigen::Quaterniond & q) return omega; } -} // 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..540077e 100644 --- a/test/unit_test_trajectory_generator.cpp +++ b/test/unit_test_traj_gen.cpp @@ -14,10 +14,10 @@ #include -#include "trajectory_generator/spline.hpp" -#include "trajectory_generator/trajectory_generator.hpp" +#include "traj_gen/spline.hpp" +#include "traj_gen/traj_gen.hpp" -namespace trajectory_generator +namespace traj_gen { TEST(TrajectoryGenerator, VectorSplineTaskSpace) @@ -168,7 +168,7 @@ TEST(TrajectoryGenerator, OrientationSpline) } } -} // namespace trajectory_generator +} // namespace traj_gen int main(int argc, char ** argv) { From c98004752f7b31fe9e9d8c82630f55d447b96a17 Mon Sep 17 00:00:00 2001 From: MasazumiImai Date: Fri, 15 May 2026 02:43:14 +0900 Subject: [PATCH 2/7] Create base.hpp --- include/traj_gen/base.hpp | 47 +++++++++++++++++++++++++++ include/traj_gen/visibility_control.h | 2 +- 2 files changed, 48 insertions(+), 1 deletion(-) create mode 100644 include/traj_gen/base.hpp 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/traj_gen/visibility_control.h b/include/traj_gen/visibility_control.h index 32d537e..16f600e 100644 --- a/include/traj_gen/visibility_control.h +++ b/include/traj_gen/visibility_control.h @@ -1,4 +1,4 @@ -// Copyright (c) 2025 Space Robotics Lab -- Tohoku University +// 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. From 772a9feb5263a9516f6569725ab117fbe2c1bcaf Mon Sep 17 00:00:00 2001 From: MasazumiImai Date: Fri, 15 May 2026 02:43:51 +0900 Subject: [PATCH 3/7] Rename traj_gen to common --- include/traj_gen/common.hpp | 87 ++++++++++++++++++++++++++++++++ src/{traj_gen.cpp => common.cpp} | 6 +-- 2 files changed, 88 insertions(+), 5 deletions(-) create mode 100644 include/traj_gen/common.hpp rename src/{traj_gen.cpp => common.cpp} (97%) diff --git a/include/traj_gen/common.hpp b/include/traj_gen/common.hpp new file mode 100644 index 0000000..de220d6 --- /dev/null +++ b/include/traj_gen/common.hpp @@ -0,0 +1,87 @@ +// 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__COMMON_HPP_ +#define TRAJ_GEN__COMMON_HPP_ + +#include +#include +#include + +#include "traj_gen/visibility_control.h" + +namespace traj_gen +{ + +/** + * @brief Constraints for N-dimensional vector spaces (joint space, work space position, etc.). + */ +struct VectorStateConstraint +{ + double time; // [s] + std::optional position; + std::optional velocity; + std::optional acceleration; +}; + +/** + * @brief Create a list of constraints with boundary conditions (start and end points) for vectors. + */ +TRAJ_GEN_PUBLIC +std::vector createBoundaryConditions( + const VectorStateConstraint & start_constraint, const VectorStateConstraint & end_constraint); + +/** + * @brief Add a new constraint to the existing constraint list. + */ +TRAJ_GEN_PUBLIC +void addConstraint( + std::vector & constraints, const VectorStateConstraint & new_constraint); + +/** + * @brief Constraints for orientation (quaternion). + */ +struct AngularStateConstraint +{ + double time; // [s] + std::optional orientation; + std::optional angular_velocity; + std::optional angular_acceleration; +}; + +/** + * @brief Create a list of constraints with boundary conditions (start and end points) for orientation. + */ +TRAJ_GEN_PUBLIC +std::vector createBoundaryConditions( + const AngularStateConstraint & start_constraint, const AngularStateConstraint & end_constraint); + +/** + * @brief Add a new constraint to the existing constraint list. + */ +TRAJ_GEN_PUBLIC +void addConstraint( + std::vector & constraints, const AngularStateConstraint & new_constraint); + +// --- Mathematical utility functions for quaternion calculations --- + +TRAJ_GEN_PUBLIC +Eigen::Quaterniond expMap(const Eigen::Vector3d & omega); + +TRAJ_GEN_PUBLIC +Eigen::Vector3d logMap(const Eigen::Quaterniond & q); + +} // namespace traj_gen + +#endif // TRAJ_GEN__COMMON_HPP_ diff --git a/src/traj_gen.cpp b/src/common.cpp similarity index 97% rename from src/traj_gen.cpp rename to src/common.cpp index 4e8dbd5..47b25b2 100644 --- a/src/traj_gen.cpp +++ b/src/common.cpp @@ -12,11 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "traj_gen/traj_gen.hpp" - -#include - -#define DEBUG false +#include "traj_gen/common.hpp" namespace traj_gen { From f0d1d744e03ed8cd5e95b2f2e95785ae43b7ca34 Mon Sep 17 00:00:00 2001 From: MasazumiImai Date: Fri, 15 May 2026 02:44:16 +0900 Subject: [PATCH 4/7] Create master header --- CMakeLists.txt | 2 +- include/traj_gen/traj_gen.hpp | 73 ++--------------------------------- 2 files changed, 4 insertions(+), 71 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d9a69f9..38017cf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,7 +14,7 @@ find_package(ament_cmake_gtest REQUIRED) # library add_library(${PROJECT_NAME} SHARED - src/traj_gen.cpp + src/common.cpp src/spline.cpp ) diff --git a/include/traj_gen/traj_gen.hpp b/include/traj_gen/traj_gen.hpp index 635c34d..e4f19e5 100644 --- a/include/traj_gen/traj_gen.hpp +++ b/include/traj_gen/traj_gen.hpp @@ -15,75 +15,8 @@ #ifndef TRAJ_GEN__TRAJ_GEN_HPP_ #define TRAJ_GEN__TRAJ_GEN_HPP_ -#include -#include -#include -#include -#include - -#include "traj_gen/visibility_control.h" - -namespace traj_gen -{ - -/** - * @brief Constraints for N-dimensional vector spaces (joint space, work space position, etc.). - */ -struct VectorStateConstraint -{ - double time; // [s] - std::optional position; - std::optional velocity; - std::optional acceleration; -}; - -/** - * @brief Create a list of constraints with boundary conditions (start and end points) for vectors. - */ -TRAJ_GEN_PUBLIC -std::vector createBoundaryConditions( - const VectorStateConstraint & start_constraint, const VectorStateConstraint & end_constraint); - -/** - * @brief Add a new constraint to the existing constraint list. - */ -TRAJ_GEN_PUBLIC -void addConstraint( - std::vector & constraints, const VectorStateConstraint & new_constraint); - -/** - * @brief Constraints for orientation (quaternion). - */ -struct AngularStateConstraint -{ - double time; // [s] - std::optional orientation; - std::optional angular_velocity; - std::optional angular_acceleration; -}; - -/** - * @brief Create a list of constraints with boundary conditions (start and end points) for orientation. - */ -TRAJ_GEN_PUBLIC -std::vector createBoundaryConditions( - const AngularStateConstraint & start_constraint, const AngularStateConstraint & end_constraint); - -/** - * @brief Add a new constraint to the existing constraint list. - */ -TRAJ_GEN_PUBLIC -void addConstraint( - std::vector & constraints, const AngularStateConstraint & new_constraint); - -// --- Mathematical utility functions for quaternion calculations --- - -TRAJ_GEN_PUBLIC -Eigen::Quaterniond expMap(const Eigen::Vector3d & omega); - -TRAJ_GEN_PUBLIC -Eigen::Vector3d logMap(const Eigen::Quaterniond & q); - -} // namespace traj_gen +#include "traj_gen/base.hpp" +#include "traj_gen/common.hpp" +#include "traj_gen/spline.hpp" #endif // TRAJ_GEN__TRAJ_GEN_HPP_ From 7a6eb9cc6a999d283f3d424bfc9109bc590c17e7 Mon Sep 17 00:00:00 2001 From: MasazumiImai Date: Fri, 15 May 2026 02:45:33 +0900 Subject: [PATCH 5/7] Update spline --- include/traj_gen/spline.hpp | 23 +++++++++++------------ src/spline.cpp | 3 ++- test/unit_test_traj_gen.cpp | 3 ++- 3 files changed, 15 insertions(+), 14 deletions(-) diff --git a/include/traj_gen/spline.hpp b/include/traj_gen/spline.hpp index 462ee73..5851bfe 100644 --- a/include/traj_gen/spline.hpp +++ b/include/traj_gen/spline.hpp @@ -16,12 +16,11 @@ #define TRAJ_GEN__SPLINE_HPP_ #include -#include #include -#include -#include +#include -#include "traj_gen/traj_gen.hpp" +#include "traj_gen/base.hpp" +#include "traj_gen/common.hpp" #include "traj_gen/visibility_control.h" namespace traj_gen @@ -30,17 +29,17 @@ namespace traj_gen /** * @brief Class for generating and managing spline trajectories of N-dimensional vectors (joint angles, positions, etc.). */ -class TRAJ_GEN_PUBLIC VectorSpline +class TRAJ_GEN_PUBLIC VectorSpline : public VectorTrajectoryBase { 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( @@ -56,17 +55,17 @@ class TRAJ_GEN_PUBLIC VectorSpline /** * @brief Class for generating and managing spline trajectories of orientation (quaternion). */ -class TRAJ_GEN_PUBLIC OrientationSpline +class TRAJ_GEN_PUBLIC OrientationSpline : public OrientationTrajectoryBase { 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( diff --git a/src/spline.cpp b/src/spline.cpp index e306edf..a278a86 100644 --- a/src/spline.cpp +++ b/src/spline.cpp @@ -14,7 +14,8 @@ #include "traj_gen/spline.hpp" -#define DEBUG false +#include +#include namespace traj_gen { diff --git a/test/unit_test_traj_gen.cpp b/test/unit_test_traj_gen.cpp index 540077e..a14157e 100644 --- a/test/unit_test_traj_gen.cpp +++ b/test/unit_test_traj_gen.cpp @@ -14,8 +14,9 @@ #include +#include + #include "traj_gen/spline.hpp" -#include "traj_gen/traj_gen.hpp" namespace traj_gen { From e06537599282dee16285fd4519d7bfdfe897c9ea Mon Sep 17 00:00:00 2001 From: MasazumiImai Date: Fri, 15 May 2026 03:18:20 +0900 Subject: [PATCH 6/7] Create build.yml --- .github/workflows/build.yml | 40 +++++++++++++++++++++++++++++++++++++ 1 file changed, 40 insertions(+) create mode 100644 .github/workflows/build.yml 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 From e3a7880ee28899035fd9ccb4a5a9c968772d2b8e Mon Sep 17 00:00:00 2001 From: MasazumiImai Date: Fri, 15 May 2026 03:18:26 +0900 Subject: [PATCH 7/7] Update README --- README.md | 116 +++++++++++++++++++++++++++++++++++++++++++++++------- 1 file changed, 102 insertions(+), 14 deletions(-) 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 ```