From 61455af163d3c0e9444a629a7e3a0ba70d37c23e Mon Sep 17 00:00:00 2001 From: Vaios Papaspyros <8146703+bpapaspyros@users.noreply.github.com> Date: Mon, 13 Jan 2025 18:01:57 +0100 Subject: [PATCH 01/52] feat: move up to C++20 --- .vscode/c_cpp_properties.json | 4 ++-- demos/CMakeLists.txt | 4 ++-- protocol/clproto_cpp/CMakeLists.txt | 4 ++-- source/CMakeLists.txt | 4 ++-- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 119ea5080..92cdc6cea 100644 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -9,8 +9,8 @@ "/src/source/**/include" ], "compilerPath": "/usr/bin/gcc", - "cStandard": "c17", - "cppStandard": "gnu++17", + "cStandard": "c23", + "cppStandard": "gnu++20", "intelliSenseMode": "linux-gcc-x64" } ], diff --git a/demos/CMakeLists.txt b/demos/CMakeLists.txt index 9482fc0fd..4e0e8d346 100644 --- a/demos/CMakeLists.txt +++ b/demos/CMakeLists.txt @@ -6,9 +6,9 @@ if(NOT CMAKE_C_STANDARD) set(CMAKE_C_STANDARD 99) endif() -# Default to C++17 +# Default to C++20 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD 20) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/protocol/clproto_cpp/CMakeLists.txt b/protocol/clproto_cpp/CMakeLists.txt index c614e9e34..e7f984c3d 100644 --- a/protocol/clproto_cpp/CMakeLists.txt +++ b/protocol/clproto_cpp/CMakeLists.txt @@ -7,9 +7,9 @@ if(NOT CMAKE_C_STANDARD) set(CMAKE_C_STANDARD 99) endif() -# Default to C++17 +# Default to C++20 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD 20) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/source/CMakeLists.txt b/source/CMakeLists.txt index b4083986c..9158b6319 100644 --- a/source/CMakeLists.txt +++ b/source/CMakeLists.txt @@ -15,9 +15,9 @@ if(NOT CMAKE_C_STANDARD) set(CMAKE_C_STANDARD 99) endif() -# Default to C++17 +# Default to C++20 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD 20) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") From 1c0c110d6722b305c39d1b6d88cd06576b7bf826 Mon Sep 17 00:00:00 2001 From: Vaios Papaspyros <8146703+bpapaspyros@users.noreply.github.com> Date: Mon, 13 Jan 2025 17:55:29 +0100 Subject: [PATCH 02/52] refactor: format Trajectory.hpp --- .../trajectories/Trajectory.hpp | 136 +++++++++--------- 1 file changed, 71 insertions(+), 65 deletions(-) diff --git a/source/state_representation/include/state_representation/trajectories/Trajectory.hpp b/source/state_representation/include/state_representation/trajectories/Trajectory.hpp index 6b8c4c7ed..4515f0c18 100644 --- a/source/state_representation/include/state_representation/trajectories/Trajectory.hpp +++ b/source/state_representation/include/state_representation/trajectories/Trajectory.hpp @@ -1,16 +1,15 @@ #pragma once +#include "state_representation/State.hpp" #include #include -#include "state_representation/State.hpp" namespace state_representation { -template -class Trajectory : public State { +template class Trajectory : public State { private: std::deque points_; std::deque times_; - std::string reference_frame_; ///< name of the reference frame + std::string reference_frame_; ///< name of the reference frame std::vector joint_names_; ///< names of the joints public: @@ -23,7 +22,7 @@ class Trajectory : public State { * @brief Constructor with name and reference frame provided * @brief name the name of the state */ - explicit Trajectory(const std::string& name); + explicit Trajectory(const std::string &name); /** * @brief Getter of the reference frame as const reference @@ -33,12 +32,12 @@ class Trajectory : public State { /** * @brief Setter of the reference frame */ - virtual void set_reference_frame(const std::string& reference_frame); + virtual void set_reference_frame(const std::string &reference_frame); /** * @brief Getter of the names attribute */ - const std::vector& get_joint_names() const; + const std::vector &get_joint_names() const; /** * @brief Setter of the names attribute from the number of joints @@ -48,7 +47,7 @@ class Trajectory : public State { /** * @brief Setter of the names attribute from the joints names */ - void set_joint_names(const std::vector& joint_names); + void set_joint_names(const std::vector &joint_names); /** * @brief Initialize trajectory @@ -58,14 +57,18 @@ class Trajectory : public State { /** * @brief Add new point and corresponding time to trajectory */ - template - void add_point(const StateT& new_point, const std::chrono::duration& new_time); + template + void add_point(const StateT &new_point, + const std::chrono::duration &new_time); /** - * @brief Insert new point and corresponding time to trajectory between two already existing points + * @brief Insert new point and corresponding time to trajectory between two + * already existing points */ - template - void insert_point(const StateT& new_point, const std::chrono::duration& new_time, int pos); + template + void insert_point(const StateT &new_point, + const std::chrono::duration &new_time, + int pos); /** * @brief Delete last point and corresponding time from trajectory @@ -80,24 +83,24 @@ class Trajectory : public State { /** * @brief Get attribute list of trajectory points */ - const std::deque& get_points() const; + const std::deque &get_points() const; /** * @brief Get the trajectory point at given index * @param index the index */ - const StateT& get_point(unsigned int index) const; + const StateT &get_point(unsigned int index) const; /** * @brief Get the trajectory point at given index * @param index the index */ - StateT& get_point(unsigned int index); + StateT &get_point(unsigned int index); /** * @brief Get attribute list of trajectory times */ - const std::deque& get_times() const; + const std::deque &get_times() const; /** * @brief Get attribute number of point in trajectory @@ -105,48 +108,49 @@ class Trajectory : public State { int get_size() const; /** - * @brief Operator overload for returning a single trajectory point and corresponding time + * @brief Operator overload for returning a single trajectory point and + * corresponding time */ - const std::pair operator[](unsigned int idx) const; + const std::pair + operator[](unsigned int idx) const; /** - * @brief Operator overload for returning a single trajectory point and corresponding time + * @brief Operator overload for returning a single trajectory point and + * corresponding time */ std::pair operator[](unsigned int idx); - }; -template -Trajectory::Trajectory(): - State() { +template Trajectory::Trajectory() : State() { this->set_type(StateType::TRAJECTORY); this->reset(); } -template -Trajectory::Trajectory(const std::string& name): - State(name), - reference_frame_("") { +template +Trajectory::Trajectory(const std::string &name) + : State(name), reference_frame_("") { this->set_type(StateType::TRAJECTORY); this->reset(); } -template +template inline const std::string Trajectory::get_reference_frame() const { return this->reference_frame_; } -template -inline void Trajectory::set_reference_frame(const std::string& reference_frame) { +template +inline void +Trajectory::set_reference_frame(const std::string &reference_frame) { this->reference_frame_ = reference_frame; } -template -inline const std::vector& Trajectory::get_joint_names() const { +template +inline const std::vector & +Trajectory::get_joint_names() const { return this->joint_names_; } -template +template inline void Trajectory::set_joint_names(unsigned int nb_joints) { this->joint_names_.resize(nb_joints); for (unsigned int i = 0; i < nb_joints; i++) { @@ -154,21 +158,23 @@ inline void Trajectory::set_joint_names(unsigned int nb_joints) { } } -template -inline void Trajectory::set_joint_names(const std::vector& joint_names) { +template +inline void Trajectory::set_joint_names( + const std::vector &joint_names) { this->joint_names_ = joint_names; } -template -void Trajectory::reset() { +template void Trajectory::reset() { this->State::reset(); this->points_.clear(); this->times_.clear(); } -template -template -void Trajectory::add_point(const StateT& new_point, const std::chrono::duration& new_time) { +template +template +void Trajectory::add_point( + const StateT &new_point, + const std::chrono::duration &new_time) { this->set_empty(false); this->points_.push_back(new_point); @@ -180,11 +186,11 @@ void Trajectory::add_point(const StateT& new_point, const std::chrono::d } } -template -template -void Trajectory::insert_point(const StateT& new_point, - const std::chrono::duration& new_time, - int pos) { +template +template +void Trajectory::insert_point( + const StateT &new_point, + const std::chrono::duration &new_time, int pos) { this->set_empty(false); auto it_points = this->points_.begin(); @@ -202,8 +208,7 @@ void Trajectory::insert_point(const StateT& new_point, } } -template -void Trajectory::delete_point() { +template void Trajectory::delete_point() { this->set_empty(false); if (!this->points_.empty()) { this->points_.pop_back(); @@ -213,45 +218,46 @@ void Trajectory::delete_point() { } } -template -void Trajectory::clear() { +template void Trajectory::clear() { this->points_.clear(); this->times_.clear(); } -template -inline const std::deque& Trajectory::get_points() const { +template +inline const std::deque &Trajectory::get_points() const { return this->points_; } -template -const StateT& Trajectory::get_point(unsigned int index) const { +template +const StateT &Trajectory::get_point(unsigned int index) const { return this->points_[index]; } -template -StateT& Trajectory::get_point(unsigned int index) { +template +StateT &Trajectory::get_point(unsigned int index) { return this->points_[index]; } -template -inline const std::deque& Trajectory::get_times() const { +template +inline const std::deque & +Trajectory::get_times() const { return this->times_; } -template -int Trajectory::get_size() const { +template int Trajectory::get_size() const { return this->points_.size(); } -template -const std::pair Trajectory::operator[](unsigned int idx) const { +template +const std::pair +Trajectory::operator[](unsigned int idx) const { return std::make_pair(this->points_[idx], this->times_[idx]); } -template -std::pair Trajectory::operator[](unsigned int idx) { +template +std::pair +Trajectory::operator[](unsigned int idx) { this->set_empty(false); return std::make_pair(this->points_[idx], this->times_[idx]); } -} +} // namespace state_representation From b957af4ae02a976ed17c21acd2451bc2762ae155 Mon Sep 17 00:00:00 2001 From: Vaios Papaspyros <8146703+bpapaspyros@users.noreply.github.com> Date: Mon, 13 Jan 2025 18:58:08 +0100 Subject: [PATCH 03/52] refactor: simplify Trajectory to a flatter base class --- .../trajectories/Trajectory.hpp | 263 ------------------ .../trajectories/TrajectoryBase.hpp | 226 +++++++++++++++ .../test/tests/test_trajectory.cpp | 154 +++++----- 3 files changed, 305 insertions(+), 338 deletions(-) delete mode 100644 source/state_representation/include/state_representation/trajectories/Trajectory.hpp create mode 100644 source/state_representation/include/state_representation/trajectories/TrajectoryBase.hpp diff --git a/source/state_representation/include/state_representation/trajectories/Trajectory.hpp b/source/state_representation/include/state_representation/trajectories/Trajectory.hpp deleted file mode 100644 index 4515f0c18..000000000 --- a/source/state_representation/include/state_representation/trajectories/Trajectory.hpp +++ /dev/null @@ -1,263 +0,0 @@ -#pragma once - -#include "state_representation/State.hpp" -#include -#include - -namespace state_representation { -template class Trajectory : public State { -private: - std::deque points_; - std::deque times_; - std::string reference_frame_; ///< name of the reference frame - std::vector joint_names_; ///< names of the joints - -public: - /** - * @brief Empty constructor - */ - explicit Trajectory(); - - /** - * @brief Constructor with name and reference frame provided - * @brief name the name of the state - */ - explicit Trajectory(const std::string &name); - - /** - * @brief Getter of the reference frame as const reference - */ - const std::string get_reference_frame() const; - - /** - * @brief Setter of the reference frame - */ - virtual void set_reference_frame(const std::string &reference_frame); - - /** - * @brief Getter of the names attribute - */ - const std::vector &get_joint_names() const; - - /** - * @brief Setter of the names attribute from the number of joints - */ - void set_joint_names(unsigned int nb_joints); - - /** - * @brief Setter of the names attribute from the joints names - */ - void set_joint_names(const std::vector &joint_names); - - /** - * @brief Initialize trajectory - */ - void reset(); - - /** - * @brief Add new point and corresponding time to trajectory - */ - template - void add_point(const StateT &new_point, - const std::chrono::duration &new_time); - - /** - * @brief Insert new point and corresponding time to trajectory between two - * already existing points - */ - template - void insert_point(const StateT &new_point, - const std::chrono::duration &new_time, - int pos); - - /** - * @brief Delete last point and corresponding time from trajectory - */ - void delete_point(); - - /** - * @brief Clear trajectory - */ - void clear(); - - /** - * @brief Get attribute list of trajectory points - */ - const std::deque &get_points() const; - - /** - * @brief Get the trajectory point at given index - * @param index the index - */ - const StateT &get_point(unsigned int index) const; - - /** - * @brief Get the trajectory point at given index - * @param index the index - */ - StateT &get_point(unsigned int index); - - /** - * @brief Get attribute list of trajectory times - */ - const std::deque &get_times() const; - - /** - * @brief Get attribute number of point in trajectory - */ - int get_size() const; - - /** - * @brief Operator overload for returning a single trajectory point and - * corresponding time - */ - const std::pair - operator[](unsigned int idx) const; - - /** - * @brief Operator overload for returning a single trajectory point and - * corresponding time - */ - std::pair operator[](unsigned int idx); -}; - -template Trajectory::Trajectory() : State() { - this->set_type(StateType::TRAJECTORY); - this->reset(); -} - -template -Trajectory::Trajectory(const std::string &name) - : State(name), reference_frame_("") { - this->set_type(StateType::TRAJECTORY); - this->reset(); -} - -template -inline const std::string Trajectory::get_reference_frame() const { - return this->reference_frame_; -} - -template -inline void -Trajectory::set_reference_frame(const std::string &reference_frame) { - this->reference_frame_ = reference_frame; -} - -template -inline const std::vector & -Trajectory::get_joint_names() const { - return this->joint_names_; -} - -template -inline void Trajectory::set_joint_names(unsigned int nb_joints) { - this->joint_names_.resize(nb_joints); - for (unsigned int i = 0; i < nb_joints; i++) { - this->joint_names_[i] = "joint_" + std::to_string(i + 1); - } -} - -template -inline void Trajectory::set_joint_names( - const std::vector &joint_names) { - this->joint_names_ = joint_names; -} - -template void Trajectory::reset() { - this->State::reset(); - this->points_.clear(); - this->times_.clear(); -} - -template -template -void Trajectory::add_point( - const StateT &new_point, - const std::chrono::duration &new_time) { - this->set_empty(false); - this->points_.push_back(new_point); - - if (!this->times_.empty()) { - auto const previous_time = this->times_.back(); - this->times_.push_back(previous_time + new_time); - } else { - this->times_.push_back(new_time); - } -} - -template -template -void Trajectory::insert_point( - const StateT &new_point, - const std::chrono::duration &new_time, int pos) { - this->set_empty(false); - - auto it_points = this->points_.begin(); - auto it_times = this->times_.begin(); - std::advance(it_points, pos); - std::advance(it_times, pos); - - this->points_.insert(it_points, new_point); - - auto previous_time = this->times_[pos - 1]; - this->times_.insert(it_times, previous_time + new_time); - - for (unsigned int i = pos + 1; i <= this->points_.size(); i++) { - this->times_[i] += new_time; - } -} - -template void Trajectory::delete_point() { - this->set_empty(false); - if (!this->points_.empty()) { - this->points_.pop_back(); - } - if (!this->times_.empty()) { - this->times_.pop_back(); - } -} - -template void Trajectory::clear() { - this->points_.clear(); - this->times_.clear(); -} - -template -inline const std::deque &Trajectory::get_points() const { - return this->points_; -} - -template -const StateT &Trajectory::get_point(unsigned int index) const { - return this->points_[index]; -} - -template -StateT &Trajectory::get_point(unsigned int index) { - return this->points_[index]; -} - -template -inline const std::deque & -Trajectory::get_times() const { - return this->times_; -} - -template int Trajectory::get_size() const { - return this->points_.size(); -} - -template -const std::pair -Trajectory::operator[](unsigned int idx) const { - return std::make_pair(this->points_[idx], this->times_[idx]); -} - -template -std::pair -Trajectory::operator[](unsigned int idx) { - this->set_empty(false); - return std::make_pair(this->points_[idx], this->times_[idx]); -} -} // namespace state_representation diff --git a/source/state_representation/include/state_representation/trajectories/TrajectoryBase.hpp b/source/state_representation/include/state_representation/trajectories/TrajectoryBase.hpp new file mode 100644 index 000000000..0e4bec478 --- /dev/null +++ b/source/state_representation/include/state_representation/trajectories/TrajectoryBase.hpp @@ -0,0 +1,226 @@ +#pragma once + +#include "state_representation/State.hpp" +#include "state_representation/StateType.hpp" + +#include +#include + +namespace state_representation { +template +class TrajectoryBase : public State { +private: + std::deque points_; + std::deque times_; + +public: + /** + * @brief Empty constructor + */ + explicit TrajectoryBase(); + + /** + * @brief Constructor with name provided + * @brief name the name of the state + */ + explicit TrajectoryBase(const std::string& name); + + /** + * @brief Reset trajectory + */ + void reset(); + + /** + * @brief Delete last point and corresponding time from trajectory + */ + void delete_point(); + + /** + * @brief Clear trajectory + */ + void clear(); + + /** + * @brief Get attribute list of trajectory times + */ + const std::deque& get_times() const; + + /** + * @brief Get attribute number of point in trajectory + */ + int get_size() const; + +protected: + /** + * @brief Add new point and corresponding time to trajectory + */ + template + void add_point(const TrajectoryT& new_point, const std::chrono::duration& new_time); + + /** + * @brief Insert new point and corresponding time to trajectory between two + * already existing points + */ + template + void insert_point(const TrajectoryT& new_point, const std::chrono::duration& new_time, int pos); + + /** + * @brief Delete last point and corresponding time from trajectory + */ + void delete_point(); + + /** + * @brief Clear trajectory + */ + void clear(); + + /** + * @brief Get attribute list of trajectory points + */ + const std::deque& get_points() const; + + /** + * @brief Get the trajectory point at given index + * @param index the index + */ + const TrajectoryT& get_point(unsigned int index) const; + + /** + * @brief Get the trajectory point at given index + * @param index the index + */ + TrajectoryT& get_point(unsigned int index); + + /** + * @brief Get attribute list of trajectory times + */ + const std::deque &get_times() const; + + /** + * @brief Get attribute number of point in trajectory + */ + int get_size() const; + + /** + * @brief Operator overload for returning a single trajectory point and + * corresponding time + */ + const std::pair operator[](unsigned int idx) const; + + /** + * @brief Operator overload for returning a single trajectory point and + * corresponding time + */ + std::pair operator[](unsigned int idx); +}; + +template +TrajectoryBase::TrajectoryBase() : State() { + this->set_type(StateType::TRAJECTORY); + this->reset(); +} + +template +TrajectoryBase::TrajectoryBase(const std::string& name) : State(name) { + this->set_type(StateType::NONE); + this->reset(); +} + +template +void TrajectoryBase::reset() { + this->State::reset(); + this->points_.clear(); + this->times_.clear(); +} + +template +template +void TrajectoryBase::add_point( + const TrajectoryT& new_point, const std::chrono::duration& new_time +) { + this->set_empty(false); + this->points_.push_back(new_point); + + if (!this->times_.empty()) { + auto const previous_time = this->times_.back(); + this->times_.push_back(previous_time + new_time); + } else { + this->times_.push_back(new_time); + } +} + +template +template +void TrajectoryBase::insert_point( + const TrajectoryT& new_point, const std::chrono::duration& new_time, int pos +) { + this->set_empty(false); + + auto it_points = this->points_.begin(); + auto it_times = this->times_.begin(); + std::advance(it_points, pos); + std::advance(it_times, pos); + + this->points_.insert(it_points, new_point); + + auto previous_time = this->times_[pos - 1]; + this->times_.insert(it_times, previous_time + new_time); + + for (unsigned int i = pos + 1; i <= this->points_.size(); i++) { + this->times_[i] += new_time; + } +} + +template +void TrajectoryBase::delete_point() { + this->set_empty(false); + if (!this->points_.empty()) { + this->points_.pop_back(); + } + if (!this->times_.empty()) { + this->times_.pop_back(); + } +} + +template +void TrajectoryBase::clear() { + this->points_.clear(); + this->times_.clear(); +} + +template +inline const std::deque& TrajectoryBase::get_points() const { + return this->points_; +} + +template +const TrajectoryT& TrajectoryBase::get_point(unsigned int index) const { + return this->points_[index]; +} + +template +TrajectoryT& TrajectoryBase::get_point(unsigned int index) { + return this->points_[index]; +} + +template +inline const std::deque& TrajectoryBase::get_times() const { + return this->times_; +} + +template +int TrajectoryBase::get_size() const { + return this->points_.size(); +} + +template +const std::pair TrajectoryBase::operator[](unsigned int idx) const { + return std::make_pair(this->points_[idx], this->times_[idx]); +} + +template +std::pair TrajectoryBase::operator[](unsigned int idx) { + this->set_empty(false); + return std::make_pair(this->points_[idx], this->times_[idx]); +} +}// namespace state_representation diff --git a/source/state_representation/test/tests/test_trajectory.cpp b/source/state_representation/test/tests/test_trajectory.cpp index e35b6639b..dfe9606cb 100644 --- a/source/state_representation/test/tests/test_trajectory.cpp +++ b/source/state_representation/test/tests/test_trajectory.cpp @@ -1,97 +1,101 @@ -#include -#include -#include -#include "state_representation/trajectories/Trajectory.hpp" #include "state_representation/space/cartesian/CartesianState.hpp" #include "state_representation/space/joint/JointState.hpp" +#include "state_representation/trajectories/TrajectoryBase.hpp" +#include +#include +#include +#include TEST(TrajectoryTest, CreateTrajectory) { - state_representation::Trajectory trajectory; - std::deque points = trajectory.get_points(); + state_representation::TrajectoryBase trajectory; + auto points = trajectory.get_points(); std::deque times = trajectory.get_times(); EXPECT_TRUE(points.empty()); EXPECT_TRUE(times.empty()); } -TEST(TrajectoryTest, AddPoint) { - state_representation::Trajectory trajectory; - state_representation::JointState point("robot", 1); +// TEST(TrajectoryTest, AddPoint) { +// state_representation::Trajectory +// trajectory; state_representation::JointState point("robot", 1); - std::deque points = trajectory.get_points(); - std::deque times = trajectory.get_times(); +// std::deque points = +// trajectory.get_points(); std::deque times = +// trajectory.get_times(); - unsigned int prev_size_points = points.size(); - unsigned int prev_size_times = times.size(); +// unsigned int prev_size_points = points.size(); +// unsigned int prev_size_times = times.size(); - std::chrono::nanoseconds period(100); - Eigen::ArrayXd positions(1); - positions << 0.2; - point.set_positions(positions); - trajectory.add_point(point, period); +// std::chrono::nanoseconds period(100); +// Eigen::ArrayXd positions(1); +// positions << 0.2; +// point.set_positions(positions); +// trajectory.add_point(point, period); - points = trajectory.get_points(); - times = trajectory.get_times(); +// points = trajectory.get_points(); +// times = trajectory.get_times(); - unsigned int new_size_points = points.size(); - unsigned int new_size_times = times.size(); +// unsigned int new_size_points = points.size(); +// unsigned int new_size_times = times.size(); - EXPECT_TRUE(new_size_points == prev_size_points + 1); - EXPECT_TRUE(new_size_times == prev_size_times + 1); -} +// EXPECT_TRUE(new_size_points == prev_size_points + 1); +// EXPECT_TRUE(new_size_times == prev_size_times + 1); +// } -TEST(TrajectoryTest, ClearPoint) { - state_representation::Trajectory trajectory; - state_representation::JointState point("robot", 1); +// TEST(TrajectoryTest, ClearPoint) { +// state_representation::Trajectory +// trajectory; state_representation::JointState point("robot", 1); - std::chrono::nanoseconds period(100); - Eigen::ArrayXd positions(1); - positions << 0.2; - point.set_positions(positions); - trajectory.add_point(point, period); +// std::chrono::nanoseconds period(100); +// Eigen::ArrayXd positions(1); +// positions << 0.2; +// point.set_positions(positions); +// trajectory.add_point(point, period); - std::deque points = trajectory.get_points(); - std::deque times = trajectory.get_times(); +// std::deque points = +// trajectory.get_points(); std::deque times = +// trajectory.get_times(); - unsigned int size_points = points.size(); - unsigned int size_times = times.size(); +// unsigned int size_points = points.size(); +// unsigned int size_times = times.size(); - EXPECT_TRUE(size_points == 1); - EXPECT_TRUE(size_times == 1); +// EXPECT_TRUE(size_points == 1); +// EXPECT_TRUE(size_times == 1); - trajectory.clear(); - points = trajectory.get_points(); - times = trajectory.get_times(); +// trajectory.clear(); +// points = trajectory.get_points(); +// times = trajectory.get_times(); - EXPECT_TRUE(points.empty()); - EXPECT_TRUE(times.empty()); -} +// EXPECT_TRUE(points.empty()); +// EXPECT_TRUE(times.empty()); +// } -TEST(TrajectoryTest, OverloadIndex) { - state_representation::Trajectory trajectory; - state_representation::JointState point("robot", 1); - - std::chrono::nanoseconds period(100); - Eigen::ArrayXd positions(1); - positions << 0.2; - point.set_positions(positions); - trajectory.add_point(point, period); - positions << 0.7; - point.set_positions(positions); - trajectory.add_point(point, period); - - std::pair point0 = trajectory[0]; - std::pair point1 = trajectory[1]; - - EXPECT_TRUE(point0.first.get_positions()[0] == 0.2); - EXPECT_TRUE(point1.first.get_positions()[0] == 0.7); - EXPECT_TRUE(point0.second == period); - EXPECT_TRUE(point1.second == 2 * period); -} +// TEST(TrajectoryTest, OverloadIndex) { +// state_representation::Trajectory +// trajectory; state_representation::JointState point("robot", 1); + +// std::chrono::nanoseconds period(100); +// Eigen::ArrayXd positions(1); +// positions << 0.2; +// point.set_positions(positions); +// trajectory.add_point(point, period); +// positions << 0.7; +// point.set_positions(positions); +// trajectory.add_point(point, period); + +// std::pair +// point0 = trajectory[0]; std::pair point1 = trajectory[1]; + +// EXPECT_TRUE(point0.first.get_positions()[0] == 0.2); +// EXPECT_TRUE(point1.first.get_positions()[0] == 0.7); +// EXPECT_TRUE(point0.second == period); +// EXPECT_TRUE(point1.second == 2 * period); +// } // TEST(TrajectoryTest, InsertPoint) // { -// state_representation::Trajectory trajectory; -// state_representation::JointState point("robot", 1); +// state_representation::Trajectory +// trajectory; state_representation::JointState point("robot", 1); // std::chrono::nanoseconds period(100); // Eigen::ArrayXd positions(1); @@ -102,9 +106,10 @@ TEST(TrajectoryTest, OverloadIndex) { // point.set_positions(positions); // trajectory.add_point(point, period); -// std::pair last_point = trajectory[1]; -// std::deque points = trajectory.get_points(); -// std::deque times = trajectory.get_times(); +// std::pair +// last_point = trajectory[1]; std::deque +// points = trajectory.get_points(); std::deque +// times = trajectory.get_times(); // EXPECT_TRUE(points.size() == 2); // EXPECT_TRUE(times.size() == 2); @@ -115,10 +120,9 @@ TEST(TrajectoryTest, OverloadIndex) { // point.set_positions(positions); // trajectory.insert_point(point, period, 1); -// std::pair inserted_point = trajectory[1]; -// last_point = trajectory[2]; -// points = trajectory.get_points(); -// times = trajectory.get_times(); +// std::pair +// inserted_point = trajectory[1]; last_point = trajectory[2]; points = +// trajectory.get_points(); times = trajectory.get_times(); // EXPECT_TRUE(points.size() == 3); // EXPECT_TRUE(times.size() == 3); From cee8f9714acf15ffd0af48afab33d42ed39ccf74 Mon Sep 17 00:00:00 2001 From: bpapaspyros Date: Tue, 14 Jan 2025 14:36:17 +0100 Subject: [PATCH 04/52] feat: add Cartesian/JointTrajectory to StateType --- .../state_representation/StateType.hpp | 55 ++++++++++--------- .../trajectories/TrajectoryBase.hpp | 26 +-------- 2 files changed, 33 insertions(+), 48 deletions(-) diff --git a/source/state_representation/include/state_representation/StateType.hpp b/source/state_representation/include/state_representation/StateType.hpp index 4cadb1f3f..3cbbd3d87 100644 --- a/source/state_representation/include/state_representation/StateType.hpp +++ b/source/state_representation/include/state_representation/StateType.hpp @@ -1,5 +1,7 @@ #pragma once +#include + /** * @namespace state_representation * @brief Core state variables and objects @@ -11,30 +13,31 @@ namespace state_representation { * @brief The class types inheriting from State */ enum class StateType { - NONE, - STATE, - SPATIAL_STATE, - CARTESIAN_STATE, - CARTESIAN_POSE, - CARTESIAN_TWIST, - CARTESIAN_ACCELERATION, - CARTESIAN_WRENCH, - JOINT_STATE, - JOINT_POSITIONS, - JOINT_VELOCITIES, - JOINT_ACCELERATIONS, - JOINT_TORQUES, - JACOBIAN, - PARAMETER, - GEOMETRY_SHAPE, - GEOMETRY_ELLIPSOID, - TRAJECTORY, - DIGITAL_IO_STATE, - ANALOG_IO_STATE, + NONE = 0, + STATE = 1, + SPATIAL_STATE = 2, + CARTESIAN_STATE = 3, + CARTESIAN_POSE = 4, + CARTESIAN_TWIST = 5, + CARTESIAN_ACCELERATION = 6, + CARTESIAN_WRENCH = 7, + JOINT_STATE = 8, + JOINT_POSITIONS = 9, + JOINT_VELOCITIES = 10, + JOINT_ACCELERATIONS = 11, + JOINT_TORQUES = 12, + JACOBIAN = 13, + PARAMETER = 14, + GEOMETRY_SHAPE = 15, + GEOMETRY_ELLIPSOID = 16, + CARTESIAN_TRAJECTORY = 17, + JOINT_TRAJECTORY = 18, + DIGITAL_IO_STATE = 19, + ANALOG_IO_STATE = 20, #ifdef EXPERIMENTAL_FEATURES - DUAL_QUATERNION_STATE, - DUAL_QUATERNION_POSE, - DUAL_QUATERNION_TWIST + DUAL_QUATERNION_STATE = 21, + DUAL_QUATERNION_POSE = 22, + DUAL_QUATERNION_TWIST = 23 #endif }; @@ -75,8 +78,10 @@ enum class StateType { return "Shape"; case StateType::GEOMETRY_ELLIPSOID: return "Ellipsoid"; - case StateType::TRAJECTORY: - return "Trajectory"; + case StateType::CARTESIAN_TRAJECTORY: + return "CartesianTrajectory"; + case StateType::JOINT_TRAJECTORY: + return "JointTrajectory"; case StateType::DIGITAL_IO_STATE: return "DigitalIOState"; case StateType::ANALOG_IO_STATE: diff --git a/source/state_representation/include/state_representation/trajectories/TrajectoryBase.hpp b/source/state_representation/include/state_representation/trajectories/TrajectoryBase.hpp index 0e4bec478..25d69054c 100644 --- a/source/state_representation/include/state_representation/trajectories/TrajectoryBase.hpp +++ b/source/state_representation/include/state_representation/trajectories/TrajectoryBase.hpp @@ -64,16 +64,6 @@ class TrajectoryBase : public State { template void insert_point(const TrajectoryT& new_point, const std::chrono::duration& new_time, int pos); - /** - * @brief Delete last point and corresponding time from trajectory - */ - void delete_point(); - - /** - * @brief Clear trajectory - */ - void clear(); - /** * @brief Get attribute list of trajectory points */ @@ -91,16 +81,6 @@ class TrajectoryBase : public State { */ TrajectoryT& get_point(unsigned int index); - /** - * @brief Get attribute list of trajectory times - */ - const std::deque &get_times() const; - - /** - * @brief Get attribute number of point in trajectory - */ - int get_size() const; - /** * @brief Operator overload for returning a single trajectory point and * corresponding time @@ -116,12 +96,12 @@ class TrajectoryBase : public State { template TrajectoryBase::TrajectoryBase() : State() { - this->set_type(StateType::TRAJECTORY); + this->set_type(StateType::NONE); this->reset(); } -template -TrajectoryBase::TrajectoryBase(const std::string& name) : State(name) { +template +TrajectoryBase::TrajectoryBase(const std::string &name) : State(name) { this->set_type(StateType::NONE); this->reset(); } From c76f70da9151a515777ccb0a7290dc5593cdeb43 Mon Sep 17 00:00:00 2001 From: Vaios Papaspyros <8146703+bpapaspyros@users.noreply.github.com> Date: Wed, 15 Jan 2025 11:11:17 +0100 Subject: [PATCH 05/52] feat: implement CartesianTrajectory --- source/state_representation/CMakeLists.txt | 1 + .../trajectories/CartesianTrajectory.hpp | 90 +++++++++ .../trajectories/TrajectoryBase.hpp | 10 +- .../src/trajectories/CartesianTrajectory.cpp | 40 ++++ .../test/tests/test_trajectory.cpp | 184 +++++++----------- 5 files changed, 203 insertions(+), 122 deletions(-) create mode 100644 source/state_representation/include/state_representation/trajectories/CartesianTrajectory.hpp create mode 100644 source/state_representation/src/trajectories/CartesianTrajectory.cpp diff --git a/source/state_representation/CMakeLists.txt b/source/state_representation/CMakeLists.txt index b6615bd3f..b1510e144 100644 --- a/source/state_representation/CMakeLists.txt +++ b/source/state_representation/CMakeLists.txt @@ -29,6 +29,7 @@ set(CORE_SOURCES src/parameters/Predicate.cpp src/geometry/Shape.cpp src/geometry/Ellipsoid.cpp + src/trajectories/CartesianTrajectory.cpp ) if (EXPERIMENTAL_FEATURES) diff --git a/source/state_representation/include/state_representation/trajectories/CartesianTrajectory.hpp b/source/state_representation/include/state_representation/trajectories/CartesianTrajectory.hpp new file mode 100644 index 000000000..ce0b5511f --- /dev/null +++ b/source/state_representation/include/state_representation/trajectories/CartesianTrajectory.hpp @@ -0,0 +1,90 @@ +#pragma once + +#include "state_representation/space/cartesian/CartesianState.hpp" +#include "state_representation/trajectories/TrajectoryBase.hpp" + +#include +#include +#include + +namespace state_representation { +class CartesianTrajectory : public TrajectoryBase { +private: + std::string reference_frame_;///< name of the reference frame + +public: + /** + * @brief Constructor with name and reference frame provided + * @param reference_frame reference frame of the trajectory points + * @param name the name of the state + */ + explicit CartesianTrajectory(const std::string& reference_frame = "world", const std::string& name = ""); + + /** + * @brief Getter of the reference frame as const reference + */ + const std::string get_reference_frame() const; + + /** + * @brief Add new point and corresponding time to trajectory + * @return Success of the operation + */ + template + bool add_point(const CartesianState& new_point, const std::chrono::duration& new_time); + + /** + * @brief Insert new point and corresponding time to trajectory between two + * already existing points + * @return Success of the operation + */ + template + bool + insert_point(const CartesianState& new_point, const std::chrono::duration& new_time, int pos); + + /** + * @brief Get attribute list of trajectory points + */ + const std::deque get_points() const; + + /** + * @brief Get the trajectory point at given index + * @param index the index + */ + CartesianState get_point(unsigned int index) const; + + /** + * @brief Operator overload for returning a single trajectory point and + * corresponding time + */ + std::pair operator[](unsigned int idx); +}; + +template +inline bool CartesianTrajectory::add_point( + const CartesianState& new_point, const std::chrono::duration& new_time) { + if (this->get_size() > 0) { + if (new_point.get_reference_frame() != reference_frame_) { + return false; + } + } else if (reference_frame_.empty()) { + reference_frame_ = new_point.get_reference_frame(); + } + this->TrajectoryBase::add_point(new_point.data(), new_time); + return true; +} + +template +inline bool CartesianTrajectory::insert_point( + const CartesianState& new_point, const std::chrono::duration& new_time, int pos) { + if (this->get_size() > 0) { + if (new_point.get_reference_frame() != reference_frame_) { + return false; + } + } else if (reference_frame_.empty()) { + reference_frame_ = new_point.get_reference_frame(); + } + this->TrajectoryBase::insert_point(new_point.data(), new_time, pos); + return true; +} + +}// namespace state_representation diff --git a/source/state_representation/include/state_representation/trajectories/TrajectoryBase.hpp b/source/state_representation/include/state_representation/trajectories/TrajectoryBase.hpp index 25d69054c..703fa4f8a 100644 --- a/source/state_representation/include/state_representation/trajectories/TrajectoryBase.hpp +++ b/source/state_representation/include/state_representation/trajectories/TrajectoryBase.hpp @@ -100,8 +100,8 @@ TrajectoryBase::TrajectoryBase() : State() { this->reset(); } -template -TrajectoryBase::TrajectoryBase(const std::string &name) : State(name) { +template +TrajectoryBase::TrajectoryBase(const std::string& name) : State(name) { this->set_type(StateType::NONE); this->reset(); } @@ -116,8 +116,7 @@ void TrajectoryBase::reset() { template template void TrajectoryBase::add_point( - const TrajectoryT& new_point, const std::chrono::duration& new_time -) { + const TrajectoryT& new_point, const std::chrono::duration& new_time) { this->set_empty(false); this->points_.push_back(new_point); @@ -132,8 +131,7 @@ void TrajectoryBase::add_point( template template void TrajectoryBase::insert_point( - const TrajectoryT& new_point, const std::chrono::duration& new_time, int pos -) { + const TrajectoryT& new_point, const std::chrono::duration& new_time, int pos) { this->set_empty(false); auto it_points = this->points_.begin(); diff --git a/source/state_representation/src/trajectories/CartesianTrajectory.cpp b/source/state_representation/src/trajectories/CartesianTrajectory.cpp new file mode 100644 index 000000000..7c2a7cd4e --- /dev/null +++ b/source/state_representation/src/trajectories/CartesianTrajectory.cpp @@ -0,0 +1,40 @@ +#include "state_representation/trajectories/CartesianTrajectory.hpp" +#include "state_representation/space/cartesian/CartesianState.hpp" + +#include + +namespace state_representation { +CartesianTrajectory::CartesianTrajectory(const std::string& reference_frame, const std::string& name) + : TrajectoryBase(name), reference_frame_(reference_frame) { + this->set_type(StateType::CARTESIAN_TRAJECTORY); + this->reset(); +} + +const std::string CartesianTrajectory::get_reference_frame() const { + return this->reference_frame_; +} + +const std::deque CartesianTrajectory::get_points() const { + std::deque points; + for (const auto& point : this->TrajectoryBase::get_points()) { + CartesianState state; + state.set_data(point); + points.push_back(state); + } + return points; +} + +CartesianState CartesianTrajectory::get_point(unsigned int index) const { + auto point = this->TrajectoryBase::get_point(index); + CartesianState state; + state.set_data(point); + return state; +} + +std::pair CartesianTrajectory::operator[](unsigned int idx) { + auto pair = this->TrajectoryBase::operator[](idx); + CartesianState state; + state.set_data(pair.first); + return std::make_pair(state, pair.second); +} +}// namespace state_representation diff --git a/source/state_representation/test/tests/test_trajectory.cpp b/source/state_representation/test/tests/test_trajectory.cpp index dfe9606cb..75eadfa4e 100644 --- a/source/state_representation/test/tests/test_trajectory.cpp +++ b/source/state_representation/test/tests/test_trajectory.cpp @@ -1,133 +1,85 @@ #include "state_representation/space/cartesian/CartesianState.hpp" -#include "state_representation/space/joint/JointState.hpp" -#include "state_representation/trajectories/TrajectoryBase.hpp" +#include "state_representation/trajectories/CartesianTrajectory.hpp" +// #include "state_representation/trajectories/JointTrajectory.hpp" + #include -#include + #include #include TEST(TrajectoryTest, CreateTrajectory) { state_representation::TrajectoryBase trajectory; - auto points = trajectory.get_points(); std::deque times = trajectory.get_times(); - EXPECT_TRUE(points.empty()); + EXPECT_TRUE(trajectory.get_size() == 0); EXPECT_TRUE(times.empty()); } -// TEST(TrajectoryTest, AddPoint) { -// state_representation::Trajectory -// trajectory; state_representation::JointState point("robot", 1); - -// std::deque points = -// trajectory.get_points(); std::deque times = -// trajectory.get_times(); - -// unsigned int prev_size_points = points.size(); -// unsigned int prev_size_times = times.size(); - -// std::chrono::nanoseconds period(100); -// Eigen::ArrayXd positions(1); -// positions << 0.2; -// point.set_positions(positions); -// trajectory.add_point(point, period); - -// points = trajectory.get_points(); -// times = trajectory.get_times(); - -// unsigned int new_size_points = points.size(); -// unsigned int new_size_times = times.size(); - -// EXPECT_TRUE(new_size_points == prev_size_points + 1); -// EXPECT_TRUE(new_size_times == prev_size_times + 1); -// } - -// TEST(TrajectoryTest, ClearPoint) { -// state_representation::Trajectory -// trajectory; state_representation::JointState point("robot", 1); - -// std::chrono::nanoseconds period(100); -// Eigen::ArrayXd positions(1); -// positions << 0.2; -// point.set_positions(positions); -// trajectory.add_point(point, period); - -// std::deque points = -// trajectory.get_points(); std::deque times = -// trajectory.get_times(); - -// unsigned int size_points = points.size(); -// unsigned int size_times = times.size(); +TEST(TrajectoryTest, AddPoint) { + { + state_representation::CartesianTrajectory trajectory("world"); + EXPECT_STREQ(trajectory.get_reference_frame().c_str(), "world"); -// EXPECT_TRUE(size_points == 1); -// EXPECT_TRUE(size_times == 1); + auto point1 = state_representation::CartesianState::Random("point1"); + EXPECT_TRUE(trajectory.add_point(point1, std::chrono::nanoseconds(100))); -// trajectory.clear(); -// points = trajectory.get_points(); -// times = trajectory.get_times(); + auto point2 = state_representation::CartesianState::Random("point2", "foo_reference_frame"); + EXPECT_FALSE(trajectory.add_point(point2, std::chrono::nanoseconds(200))); -// EXPECT_TRUE(points.empty()); -// EXPECT_TRUE(times.empty()); -// } - -// TEST(TrajectoryTest, OverloadIndex) { -// state_representation::Trajectory -// trajectory; state_representation::JointState point("robot", 1); - -// std::chrono::nanoseconds period(100); -// Eigen::ArrayXd positions(1); -// positions << 0.2; -// point.set_positions(positions); -// trajectory.add_point(point, period); -// positions << 0.7; -// point.set_positions(positions); -// trajectory.add_point(point, period); - -// std::pair -// point0 = trajectory[0]; std::pair point1 = trajectory[1]; - -// EXPECT_TRUE(point0.first.get_positions()[0] == 0.2); -// EXPECT_TRUE(point1.first.get_positions()[0] == 0.7); -// EXPECT_TRUE(point0.second == period); -// EXPECT_TRUE(point1.second == 2 * period); -// } - -// TEST(TrajectoryTest, InsertPoint) -// { -// state_representation::Trajectory -// trajectory; state_representation::JointState point("robot", 1); - -// std::chrono::nanoseconds period(100); -// Eigen::ArrayXd positions(1); -// positions << 0.2; -// point.set_positions(positions); -// trajectory.add_point(point, period); -// positions << 0.7; -// point.set_positions(positions); -// trajectory.add_point(point, period); - -// std::pair -// last_point = trajectory[1]; std::deque -// points = trajectory.get_points(); std::deque -// times = trajectory.get_times(); - -// EXPECT_TRUE(points.size() == 2); -// EXPECT_TRUE(times.size() == 2); -// EXPECT_TRUE(last_point.first.get_positions()[0] == 0.7); -// EXPECT_TRUE(last_point.second == 2*period); + auto point3 = state_representation::CartesianState::Random("point3", "world"); + EXPECT_TRUE(trajectory.add_point(point3, std::chrono::nanoseconds(300))); + } +} -// positions << 0.8; -// point.set_positions(positions); -// trajectory.insert_point(point, period, 1); +TEST(TrajectoryTest, ClearPoint) { + { + state_representation::CartesianTrajectory trajectory("world"); + auto point1 = state_representation::CartesianState::Random("point1"); + auto point2 = state_representation::CartesianState::Random("point2"); + auto point3 = state_representation::CartesianState::Random("point3"); + trajectory.add_point(point1, std::chrono::nanoseconds(100)); + trajectory.add_point(point2, std::chrono::nanoseconds(300)); + trajectory.add_point(point3, std::chrono::nanoseconds(300)); + EXPECT_EQ(trajectory.get_size(), 3); + EXPECT_EQ(trajectory.get_times().size(), 3); + trajectory.delete_point(); + EXPECT_EQ(trajectory.get_size(), 2); + EXPECT_EQ(trajectory.get_times().size(), 2); + trajectory.clear(); + EXPECT_EQ(trajectory.get_size(), 0); + EXPECT_EQ(trajectory.get_times().size(), 0); + } +} -// std::pair -// inserted_point = trajectory[1]; last_point = trajectory[2]; points = -// trajectory.get_points(); times = trajectory.get_times(); +TEST(TrajectoryTest, OverloadIndex) { + { + state_representation::CartesianTrajectory trajectory; + auto point1 = state_representation::CartesianState::Random("point1"); + auto point2 = state_representation::CartesianState::Random("point2"); + trajectory.add_point(point1, std::chrono::nanoseconds(300)); + trajectory.add_point(point2, std::chrono::nanoseconds(500)); + EXPECT_TRUE(trajectory[0].second == std::chrono::nanoseconds(300)); + EXPECT_TRUE(trajectory[0].first.data() == point1.data()); + EXPECT_TRUE(trajectory[1].second == std::chrono::nanoseconds(800)); + EXPECT_TRUE(trajectory[1].first.data() == point2.data()); + } +} -// EXPECT_TRUE(points.size() == 3); -// EXPECT_TRUE(times.size() == 3); -// EXPECT_TRUE(inserted_point.first.get_positions()[0] == 0.8); -// EXPECT_TRUE(inserted_point.second == 2*period); -// EXPECT_TRUE(last_point.first.get_positions()[0] == 0.7); -// EXPECT_TRUE(last_point.second == 3*period); -// } +TEST(TrajectoryTest, InsertPoint) { + { + state_representation::CartesianTrajectory trajectory; + auto point1 = state_representation::CartesianState::Random("point1"); + auto point2 = state_representation::CartesianState::Random("point2"); + auto point3 = state_representation::CartesianState::Random("point3"); + trajectory.add_point(point1, std::chrono::nanoseconds(300)); + trajectory.add_point(point2, std::chrono::nanoseconds(500)); + + trajectory.insert_point(point3, std::chrono::nanoseconds(100), 1); + EXPECT_TRUE(trajectory.get_size() == 3); + EXPECT_TRUE(trajectory[1].second == std::chrono::nanoseconds(400)); + EXPECT_TRUE(trajectory[1].first.data() == point3.data()); + EXPECT_TRUE(trajectory[0].second == std::chrono::nanoseconds(300)); + EXPECT_TRUE(trajectory[0].first.data() == point1.data()); + EXPECT_TRUE(trajectory[2].second == std::chrono::nanoseconds(900)); + EXPECT_TRUE(trajectory[2].first.data() == point2.data()); + } +} From 12782ac70d64afb977b17341743c3d08b65fcc9d Mon Sep 17 00:00:00 2001 From: Vaios Papaspyros <8146703+bpapaspyros@users.noreply.github.com> Date: Wed, 15 Jan 2025 12:42:18 +0100 Subject: [PATCH 06/52] feat: implement JointTrajectory class --- source/state_representation/CMakeLists.txt | 1 + .../trajectories/CartesianTrajectory.hpp | 4 - .../trajectories/JointTrajectory.hpp | 87 +++++++++++++++++++ .../src/trajectories/JointTrajectory.cpp | 43 +++++++++ .../test/tests/test_trajectory.cpp | 67 +++++++++++++- 5 files changed, 197 insertions(+), 5 deletions(-) create mode 100644 source/state_representation/include/state_representation/trajectories/JointTrajectory.hpp create mode 100644 source/state_representation/src/trajectories/JointTrajectory.cpp diff --git a/source/state_representation/CMakeLists.txt b/source/state_representation/CMakeLists.txt index b1510e144..d54c9177f 100644 --- a/source/state_representation/CMakeLists.txt +++ b/source/state_representation/CMakeLists.txt @@ -30,6 +30,7 @@ set(CORE_SOURCES src/geometry/Shape.cpp src/geometry/Ellipsoid.cpp src/trajectories/CartesianTrajectory.cpp + src/trajectories/JointTrajectory.cpp ) if (EXPERIMENTAL_FEATURES) diff --git a/source/state_representation/include/state_representation/trajectories/CartesianTrajectory.hpp b/source/state_representation/include/state_representation/trajectories/CartesianTrajectory.hpp index ce0b5511f..786b78298 100644 --- a/source/state_representation/include/state_representation/trajectories/CartesianTrajectory.hpp +++ b/source/state_representation/include/state_representation/trajectories/CartesianTrajectory.hpp @@ -3,10 +3,6 @@ #include "state_representation/space/cartesian/CartesianState.hpp" #include "state_representation/trajectories/TrajectoryBase.hpp" -#include -#include -#include - namespace state_representation { class CartesianTrajectory : public TrajectoryBase { private: diff --git a/source/state_representation/include/state_representation/trajectories/JointTrajectory.hpp b/source/state_representation/include/state_representation/trajectories/JointTrajectory.hpp new file mode 100644 index 000000000..b05c677c8 --- /dev/null +++ b/source/state_representation/include/state_representation/trajectories/JointTrajectory.hpp @@ -0,0 +1,87 @@ +#pragma once + +#include "state_representation/space/joint/JointState.hpp" +#include "state_representation/trajectories/TrajectoryBase.hpp" + +namespace state_representation { +class JointTrajectory : public TrajectoryBase { +private: + std::vector joint_names_;///< names of the joints + std::string robot_name_; ///< name of the robot + +public: + /** + * @brief Empty constructor + */ + explicit JointTrajectory(); + + /** + * @brief Constructor with name and reference frame provided + * @brief name the name of the state + */ + explicit JointTrajectory(const std::string& name); + + /** + * @brief Getter of the names attribute + */ + const std::vector& get_joint_names() const; + + /** + * @brief Add new point and corresponding time to trajectory + * @return Success of the operation + */ + template + bool add_point(const JointState& new_point, const std::chrono::duration& new_time); + + /** + * @brief Insert new point and corresponding time to trajectory between two + * already existing points + * @return Success of the operation + */ + template + bool insert_point(const JointState& new_point, const std::chrono::duration& new_time, int pos); + + /** + * @brief Get attribute list of trajectory points + */ + const std::deque get_points() const; + + /** + * @brief Get the trajectory point at given index + * @param index the index + */ + const JointState get_point(unsigned int index) const; + + /** + * @brief Operator overload for returning a single trajectory point and + * corresponding time + */ + std::pair operator[](unsigned int idx); +}; + +template +inline bool +JointTrajectory::add_point(const JointState& new_point, const std::chrono::duration& new_time) { + if (this->get_size() == 0) { + this->joint_names_ = new_point.get_names(); + this->robot_name_ = new_point.get_name(); + } else if (this->joint_names_ != new_point.get_names()) { + return false; + } + this->TrajectoryBase::add_point(new_point.data(), new_time); + return true; +} + +template +inline bool JointTrajectory::insert_point( + const JointState& new_point, const std::chrono::duration& new_time, int pos) { + if (this->get_size() == 0) { + this->joint_names_ = new_point.get_names(); + this->robot_name_ = new_point.get_name(); + } else if (this->joint_names_ != new_point.get_names()) { + return false; + } + this->TrajectoryBase::insert_point(new_point.data(), new_time, pos); + return true; +} +}// namespace state_representation diff --git a/source/state_representation/src/trajectories/JointTrajectory.cpp b/source/state_representation/src/trajectories/JointTrajectory.cpp new file mode 100644 index 000000000..db1331217 --- /dev/null +++ b/source/state_representation/src/trajectories/JointTrajectory.cpp @@ -0,0 +1,43 @@ +#include "state_representation/trajectories/JointTrajectory.hpp" +#include "state_representation/space/joint/JointState.hpp" + +namespace state_representation { + +JointTrajectory::JointTrajectory() : TrajectoryBase() { + this->set_type(StateType::JOINT_TRAJECTORY); + this->reset(); +} + +JointTrajectory::JointTrajectory(const std::string& name) : TrajectoryBase(name) { + this->set_type(StateType::JOINT_TRAJECTORY); + this->reset(); +} + +const std::vector& JointTrajectory::get_joint_names() const { + return this->joint_names_; +} + +const std::deque JointTrajectory::get_points() const { + std::deque points; + for (const auto& point : this->TrajectoryBase::get_points()) { + JointState state(this->robot_name_, point.size() / 4); + state.set_data(point); + points.push_back(state); + } + return points; +} + +const JointState JointTrajectory::get_point(unsigned int index) const { + auto point = this->TrajectoryBase::get_point(index); + JointState state(this->robot_name_, point.size() / 4); + state.set_data(point); + return state; +} + +std::pair JointTrajectory::operator[](unsigned int idx) { + auto pair = this->TrajectoryBase::operator[](idx); + JointState state(this->robot_name_, pair.first.size() / 4); + state.set_data(pair.first); + return std::make_pair(state, pair.second); +} +}// namespace state_representation diff --git a/source/state_representation/test/tests/test_trajectory.cpp b/source/state_representation/test/tests/test_trajectory.cpp index 75eadfa4e..136f125f2 100644 --- a/source/state_representation/test/tests/test_trajectory.cpp +++ b/source/state_representation/test/tests/test_trajectory.cpp @@ -1,6 +1,7 @@ #include "state_representation/space/cartesian/CartesianState.hpp" +#include "state_representation/space/joint/JointState.hpp" #include "state_representation/trajectories/CartesianTrajectory.hpp" -// #include "state_representation/trajectories/JointTrajectory.hpp" +#include "state_representation/trajectories/JointTrajectory.hpp" #include @@ -28,6 +29,19 @@ TEST(TrajectoryTest, AddPoint) { auto point3 = state_representation::CartesianState::Random("point3", "world"); EXPECT_TRUE(trajectory.add_point(point3, std::chrono::nanoseconds(300))); } + + { + state_representation::JointTrajectory trajectory("joint_trajectory"); + + auto point1 = state_representation::JointState::Random("foo", 25); + EXPECT_TRUE(trajectory.add_point(point1, std::chrono::nanoseconds(100))); + + auto point2 = state_representation::JointState::Random("foo", 1); + EXPECT_FALSE(trajectory.add_point(point2, std::chrono::nanoseconds(200))); + + auto point3 = state_representation::JointState::Random("foo", 25); + EXPECT_TRUE(trajectory.add_point(point3, std::chrono::nanoseconds(300))); + } } TEST(TrajectoryTest, ClearPoint) { @@ -48,6 +62,27 @@ TEST(TrajectoryTest, ClearPoint) { EXPECT_EQ(trajectory.get_size(), 0); EXPECT_EQ(trajectory.get_times().size(), 0); } + + { + state_representation::JointTrajectory trajectory; + auto point1 = state_representation::JointState::Random("point1", 25); + auto point2 = state_representation::JointState::Random("point2", 25); + auto point3 = state_representation::JointState::Random("point3", 25); + trajectory.add_point(point1, std::chrono::nanoseconds(100)); + trajectory.add_point(point2, std::chrono::nanoseconds(300)); + trajectory.add_point(point3, std::chrono::nanoseconds(300)); + EXPECT_EQ(trajectory.get_size(), 3); + EXPECT_EQ(trajectory.get_times().size(), 3); + EXPECT_EQ(trajectory.get_joint_names().size(), 3); + trajectory.delete_point(); + EXPECT_EQ(trajectory.get_size(), 2); + EXPECT_EQ(trajectory.get_times().size(), 2); + EXPECT_EQ(trajectory.get_joint_names().size(), 2); + trajectory.clear(); + EXPECT_EQ(trajectory.get_size(), 0); + EXPECT_EQ(trajectory.get_times().size(), 0); + EXPECT_EQ(trajectory.get_joint_names().size(), 0); + } } TEST(TrajectoryTest, OverloadIndex) { @@ -62,6 +97,18 @@ TEST(TrajectoryTest, OverloadIndex) { EXPECT_TRUE(trajectory[1].second == std::chrono::nanoseconds(800)); EXPECT_TRUE(trajectory[1].first.data() == point2.data()); } + + { + state_representation::JointTrajectory trajectory; + auto point1 = state_representation::JointState::Random("foo", 25); + auto point2 = state_representation::JointState::Random("foo", 25); + trajectory.add_point(point1, std::chrono::nanoseconds(300)); + trajectory.add_point(point2, std::chrono::nanoseconds(500)); + EXPECT_TRUE(trajectory[0].second == std::chrono::nanoseconds(300)); + EXPECT_TRUE(trajectory[0].first.data() == point1.data()); + EXPECT_TRUE(trajectory[1].second == std::chrono::nanoseconds(800)); + EXPECT_TRUE(trajectory[1].first.data() == point2.data()); + } } TEST(TrajectoryTest, InsertPoint) { @@ -82,4 +129,22 @@ TEST(TrajectoryTest, InsertPoint) { EXPECT_TRUE(trajectory[2].second == std::chrono::nanoseconds(900)); EXPECT_TRUE(trajectory[2].first.data() == point2.data()); } + + { + state_representation::JointTrajectory trajectory; + auto point1 = state_representation::JointState::Random("point1", 25); + auto point2 = state_representation::JointState::Random("point2", 25); + auto point3 = state_representation::JointState::Random("point3", 25); + trajectory.add_point(point1, std::chrono::nanoseconds(300)); + trajectory.add_point(point2, std::chrono::nanoseconds(500)); + + trajectory.insert_point(point3, std::chrono::nanoseconds(100), 1); + EXPECT_TRUE(trajectory.get_size() == 3); + EXPECT_TRUE(trajectory[1].second == std::chrono::nanoseconds(400)); + EXPECT_TRUE(trajectory[1].first.data() == point3.data()); + EXPECT_TRUE(trajectory[0].second == std::chrono::nanoseconds(300)); + EXPECT_TRUE(trajectory[0].first.data() == point1.data()); + EXPECT_TRUE(trajectory[2].second == std::chrono::nanoseconds(900)); + EXPECT_TRUE(trajectory[2].first.data() == point2.data()); + } } From a6761ef2e38a91cc0e10c6a907e6094d0ca5833c Mon Sep 17 00:00:00 2001 From: Vaios Papaspyros <8146703+bpapaspyros@users.noreply.github.com> Date: Wed, 15 Jan 2025 12:56:00 +0100 Subject: [PATCH 07/52] fix: override cleanup functions and allow derived classes to clean --- .../trajectories/CartesianTrajectory.hpp | 15 +++++++++++++++ .../trajectories/JointTrajectory.hpp | 15 +++++++++++++++ .../trajectories/TrajectoryBase.hpp | 6 +++--- .../src/trajectories/CartesianTrajectory.cpp | 17 +++++++++++++++++ .../src/trajectories/JointTrajectory.cpp | 18 ++++++++++++++++++ .../test/tests/test_trajectory.cpp | 4 ++-- 6 files changed, 70 insertions(+), 5 deletions(-) diff --git a/source/state_representation/include/state_representation/trajectories/CartesianTrajectory.hpp b/source/state_representation/include/state_representation/trajectories/CartesianTrajectory.hpp index 786b78298..8e1b2961c 100644 --- a/source/state_representation/include/state_representation/trajectories/CartesianTrajectory.hpp +++ b/source/state_representation/include/state_representation/trajectories/CartesianTrajectory.hpp @@ -53,6 +53,21 @@ class CartesianTrajectory : public TrajectoryBase { * corresponding time */ std::pair operator[](unsigned int idx); + + /** + * @brief Reset trajectory + */ + virtual void reset() override; + + /** + * @brief Delete last point and corresponding time from trajectory + */ + virtual void delete_point() override; + + /** + * @brief Clear trajectory + */ + virtual void clear() override; }; template diff --git a/source/state_representation/include/state_representation/trajectories/JointTrajectory.hpp b/source/state_representation/include/state_representation/trajectories/JointTrajectory.hpp index b05c677c8..9a6e5259c 100644 --- a/source/state_representation/include/state_representation/trajectories/JointTrajectory.hpp +++ b/source/state_representation/include/state_representation/trajectories/JointTrajectory.hpp @@ -57,6 +57,21 @@ class JointTrajectory : public TrajectoryBase { * corresponding time */ std::pair operator[](unsigned int idx); + + /** + * @brief Reset trajectory + */ + virtual void reset() override; + + /** + * @brief Delete last point and corresponding time from trajectory + */ + virtual void delete_point() override; + + /** + * @brief Clear trajectory + */ + virtual void clear() override; }; template diff --git a/source/state_representation/include/state_representation/trajectories/TrajectoryBase.hpp b/source/state_representation/include/state_representation/trajectories/TrajectoryBase.hpp index 703fa4f8a..07c51fadc 100644 --- a/source/state_representation/include/state_representation/trajectories/TrajectoryBase.hpp +++ b/source/state_representation/include/state_representation/trajectories/TrajectoryBase.hpp @@ -28,17 +28,17 @@ class TrajectoryBase : public State { /** * @brief Reset trajectory */ - void reset(); + virtual void reset(); /** * @brief Delete last point and corresponding time from trajectory */ - void delete_point(); + virtual void delete_point(); /** * @brief Clear trajectory */ - void clear(); + virtual void clear(); /** * @brief Get attribute list of trajectory times diff --git a/source/state_representation/src/trajectories/CartesianTrajectory.cpp b/source/state_representation/src/trajectories/CartesianTrajectory.cpp index 7c2a7cd4e..df0a7cbb3 100644 --- a/source/state_representation/src/trajectories/CartesianTrajectory.cpp +++ b/source/state_representation/src/trajectories/CartesianTrajectory.cpp @@ -37,4 +37,21 @@ std::pair CartesianTrajectory::operato state.set_data(pair.first); return std::make_pair(state, pair.second); } + +void CartesianTrajectory::reset() { + this->TrajectoryBase::reset(); + this->reference_frame_ = "world"; +} + +void CartesianTrajectory::delete_point() { + this->TrajectoryBase::delete_point(); + if (this->get_size() == 0) { + this->reference_frame_ = ""; + } +} + +void CartesianTrajectory::clear() { + this->TrajectoryBase::clear(); + this->reference_frame_ = ""; +} }// namespace state_representation diff --git a/source/state_representation/src/trajectories/JointTrajectory.cpp b/source/state_representation/src/trajectories/JointTrajectory.cpp index db1331217..900970b03 100644 --- a/source/state_representation/src/trajectories/JointTrajectory.cpp +++ b/source/state_representation/src/trajectories/JointTrajectory.cpp @@ -40,4 +40,22 @@ std::pair JointTrajectory::operator[](unsi state.set_data(pair.first); return std::make_pair(state, pair.second); } + +void JointTrajectory::reset() { + this->TrajectoryBase::reset(); + this->robot_name_ = ""; + this->joint_names_.clear(); +} + +void JointTrajectory::delete_point() { + this->TrajectoryBase::delete_point(); + if (this->get_size() == 0) { + this->joint_names_.clear(); + } +} + +void JointTrajectory::clear() { + this->TrajectoryBase::clear(); + this->joint_names_.clear(); +} }// namespace state_representation diff --git a/source/state_representation/test/tests/test_trajectory.cpp b/source/state_representation/test/tests/test_trajectory.cpp index 136f125f2..f45d40154 100644 --- a/source/state_representation/test/tests/test_trajectory.cpp +++ b/source/state_representation/test/tests/test_trajectory.cpp @@ -73,11 +73,11 @@ TEST(TrajectoryTest, ClearPoint) { trajectory.add_point(point3, std::chrono::nanoseconds(300)); EXPECT_EQ(trajectory.get_size(), 3); EXPECT_EQ(trajectory.get_times().size(), 3); - EXPECT_EQ(trajectory.get_joint_names().size(), 3); + EXPECT_EQ(trajectory.get_joint_names().size(), 25); trajectory.delete_point(); EXPECT_EQ(trajectory.get_size(), 2); EXPECT_EQ(trajectory.get_times().size(), 2); - EXPECT_EQ(trajectory.get_joint_names().size(), 2); + EXPECT_EQ(trajectory.get_joint_names().size(), 25); trajectory.clear(); EXPECT_EQ(trajectory.get_size(), 0); EXPECT_EQ(trajectory.get_times().size(), 0); From 094c7e3d873600f894bf363d67423701352cc6e0 Mon Sep 17 00:00:00 2001 From: Vaios Papaspyros <8146703+bpapaspyros@users.noreply.github.com> Date: Wed, 15 Jan 2025 14:13:01 +0100 Subject: [PATCH 08/52] feat: add set_point(s) functionality --- .../trajectories/CartesianTrajectory.hpp | 64 +++++++++++++++ .../trajectories/JointTrajectory.hpp | 62 +++++++++++++++ .../trajectories/TrajectoryBase.hpp | 79 ++++++++++++++++--- 3 files changed, 193 insertions(+), 12 deletions(-) diff --git a/source/state_representation/include/state_representation/trajectories/CartesianTrajectory.hpp b/source/state_representation/include/state_representation/trajectories/CartesianTrajectory.hpp index 8e1b2961c..cb2a1d93b 100644 --- a/source/state_representation/include/state_representation/trajectories/CartesianTrajectory.hpp +++ b/source/state_representation/include/state_representation/trajectories/CartesianTrajectory.hpp @@ -48,6 +48,28 @@ class CartesianTrajectory : public TrajectoryBase { */ CartesianState get_point(unsigned int index) const; + /** + * @brief Set the trajectory point at given index + * @param index the index + * @param point the new point + * @param new_time the new time + * @return Success of the operation + */ + template + bool + set_point(unsigned int index, const CartesianState& point, const std::chrono::duration& new_time); + + /** + * @brief Set the trajectory point at given index + * @param points vector of new points + * @param new_time vector of new times + * @return Success of the operation + */ + template + bool set_points( + const std::vector& points, + const std::vector>& new_times); + /** * @brief Operator overload for returning a single trajectory point and * corresponding time @@ -73,6 +95,9 @@ class CartesianTrajectory : public TrajectoryBase { template inline bool CartesianTrajectory::add_point( const CartesianState& new_point, const std::chrono::duration& new_time) { + if (new_point.is_empty()) { + return false; + } if (this->get_size() > 0) { if (new_point.get_reference_frame() != reference_frame_) { return false; @@ -87,6 +112,9 @@ inline bool CartesianTrajectory::add_point( template inline bool CartesianTrajectory::insert_point( const CartesianState& new_point, const std::chrono::duration& new_time, int pos) { + if (new_point.is_empty()) { + return false; + } if (this->get_size() > 0) { if (new_point.get_reference_frame() != reference_frame_) { return false; @@ -98,4 +126,40 @@ inline bool CartesianTrajectory::insert_point( return true; } +template +inline bool CartesianTrajectory::set_point( + unsigned int index, const CartesianState& point, const std::chrono::duration& new_time) { + if (point.is_empty()) { + return false; + } + if (point.get_reference_frame() != reference_frame_) { + return false; + } + return this->TrajectoryBase::set_point(index, point.data(), new_time); +} + +template +inline bool CartesianTrajectory::set_points( + const std::vector& points, + const std::vector>& new_times) { + if (points.empty()) { + return false; + } + std::string candidate_reference_frame = points[0].get_reference_frame(); + + std::vector data; + for (const auto& point : points) { + if (point.is_empty() || (point.get_reference_frame() != candidate_reference_frame)) { + return false; + } + data.push_back(point.data()); + } + if (this->TrajectoryBase::set_points(data, new_times)) { + reference_frame_ = candidate_reference_frame; + return true; + } else { + return false; + } +} + }// namespace state_representation diff --git a/source/state_representation/include/state_representation/trajectories/JointTrajectory.hpp b/source/state_representation/include/state_representation/trajectories/JointTrajectory.hpp index 9a6e5259c..dceec7b15 100644 --- a/source/state_representation/include/state_representation/trajectories/JointTrajectory.hpp +++ b/source/state_representation/include/state_representation/trajectories/JointTrajectory.hpp @@ -52,6 +52,27 @@ class JointTrajectory : public TrajectoryBase { */ const JointState get_point(unsigned int index) const; + /** + * @brief Set the trajectory point at given index + * @param index the index + * @param point the new point + * @param new_time the new time + * @return Success of the operation + */ + template + bool + set_point(unsigned int index, const JointState& point, const std::chrono::duration& new_time); + + /** + * @brief Set the trajectory point at given index + * @param points vector of new points + * @param new_time vector of new times + * @return Success of the operation + */ + template + bool set_points( + const std::vector& points, const std::vector>& new_times); + /** * @brief Operator overload for returning a single trajectory point and * corresponding time @@ -77,6 +98,9 @@ class JointTrajectory : public TrajectoryBase { template inline bool JointTrajectory::add_point(const JointState& new_point, const std::chrono::duration& new_time) { + if (new_point.is_empty()) { + return false; + } if (this->get_size() == 0) { this->joint_names_ = new_point.get_names(); this->robot_name_ = new_point.get_name(); @@ -90,6 +114,9 @@ JointTrajectory::add_point(const JointState& new_point, const std::chrono::durat template inline bool JointTrajectory::insert_point( const JointState& new_point, const std::chrono::duration& new_time, int pos) { + if (new_point.is_empty()) { + return false; + } if (this->get_size() == 0) { this->joint_names_ = new_point.get_names(); this->robot_name_ = new_point.get_name(); @@ -99,4 +126,39 @@ inline bool JointTrajectory::insert_point( this->TrajectoryBase::insert_point(new_point.data(), new_time, pos); return true; } + +template +inline bool JointTrajectory::set_point( + unsigned int index, const JointState& point, const std::chrono::duration& new_time) { + if (point.is_empty() || (point.get_names() != this->joint_names_) || (point.get_name() != this->robot_name_)) { + return false; + } + return this->TrajectoryBase::set_point(index, point.data(), new_time); +} + +template +inline bool JointTrajectory::set_points( + const std::vector& points, const std::vector>& new_times) { + if (points.empty() || points[0].is_empty()) { + return false; + } + auto candidate_robot_name = points[0].get_name(); + auto candidate_joint_names = points[0].get_names(); + + std::vector data; + for (const auto& point : points) { + if (point.is_empty() || (point.get_names() != candidate_joint_names) + || (point.get_name() != candidate_robot_name)) { + return false; + } + data.push_back(point.data()); + } + if (this->TrajectoryBase::set_points(data, new_times)) { + this->joint_names_ = candidate_joint_names; + this->robot_name_ = candidate_robot_name; + return true; + } else { + return false; + } +} }// namespace state_representation diff --git a/source/state_representation/include/state_representation/trajectories/TrajectoryBase.hpp b/source/state_representation/include/state_representation/trajectories/TrajectoryBase.hpp index 07c51fadc..21fa9caa1 100644 --- a/source/state_representation/include/state_representation/trajectories/TrajectoryBase.hpp +++ b/source/state_representation/include/state_representation/trajectories/TrajectoryBase.hpp @@ -81,6 +81,27 @@ class TrajectoryBase : public State { */ TrajectoryT& get_point(unsigned int index); + /** + * @brief Set the trajectory point at given index + * @param index the index + * @param point the new point + * @param new_time the new time + * @return Success of the operation + */ + template + bool + set_point(unsigned int index, const TrajectoryT& point, const std::chrono::duration& new_time); + + /** + * @brief Set the trajectory point at given index + * @param points vector of new points + * @param new_time vector of new times + * @return Success of the operation + */ + template + bool set_points( + const std::vector& points, const std::vector>& new_times); + /** * @brief Operator overload for returning a single trajectory point and * corresponding time @@ -95,19 +116,19 @@ class TrajectoryBase : public State { }; template -TrajectoryBase::TrajectoryBase() : State() { +inline TrajectoryBase::TrajectoryBase() : State() { this->set_type(StateType::NONE); this->reset(); } template -TrajectoryBase::TrajectoryBase(const std::string& name) : State(name) { +inline TrajectoryBase::TrajectoryBase(const std::string& name) : State(name) { this->set_type(StateType::NONE); this->reset(); } template -void TrajectoryBase::reset() { +inline void TrajectoryBase::reset() { this->State::reset(); this->points_.clear(); this->times_.clear(); @@ -115,7 +136,7 @@ void TrajectoryBase::reset() { template template -void TrajectoryBase::add_point( +inline void TrajectoryBase::add_point( const TrajectoryT& new_point, const std::chrono::duration& new_time) { this->set_empty(false); this->points_.push_back(new_point); @@ -130,7 +151,7 @@ void TrajectoryBase::add_point( template template -void TrajectoryBase::insert_point( +inline void TrajectoryBase::insert_point( const TrajectoryT& new_point, const std::chrono::duration& new_time, int pos) { this->set_empty(false); @@ -150,7 +171,7 @@ void TrajectoryBase::insert_point( } template -void TrajectoryBase::delete_point() { +inline void TrajectoryBase::delete_point() { this->set_empty(false); if (!this->points_.empty()) { this->points_.pop_back(); @@ -161,7 +182,7 @@ void TrajectoryBase::delete_point() { } template -void TrajectoryBase::clear() { +inline void TrajectoryBase::clear() { this->points_.clear(); this->times_.clear(); } @@ -172,32 +193,66 @@ inline const std::deque& TrajectoryBase::get_points() } template -const TrajectoryT& TrajectoryBase::get_point(unsigned int index) const { +inline const TrajectoryT& TrajectoryBase::get_point(unsigned int index) const { return this->points_[index]; } template -TrajectoryT& TrajectoryBase::get_point(unsigned int index) { +inline TrajectoryT& TrajectoryBase::get_point(unsigned int index) { return this->points_[index]; } +template +template +inline bool TrajectoryBase::set_point( + unsigned int index, const TrajectoryT& point, const std::chrono::duration& new_time) { + if (index < this->points_.size()) { + this->points_[index] = point; + if (index == 0) { + this->times_[index] = new_time; + } else { + this->times_[index] = this->times_[index - 1] + new_time; + } + for (unsigned int i = index + 1; i < this->points_.size(); ++i) { + this->times_[i] += this->times_[index - 1]; + } + return true; + } + return false; +} + +template +template +inline bool TrajectoryBase::set_points( + const std::vector& points, const std::vector>& new_times) { + if (points.size() != new_times.size()) { + return false; + } + this->clear(); + for (unsigned int i = 0; i < points.size(); ++i) { + this->add_point(points[i], new_times[i]); + } + return true; +} + template inline const std::deque& TrajectoryBase::get_times() const { return this->times_; } template -int TrajectoryBase::get_size() const { +inline int TrajectoryBase::get_size() const { return this->points_.size(); } template -const std::pair TrajectoryBase::operator[](unsigned int idx) const { +inline const std::pair +TrajectoryBase::operator[](unsigned int idx) const { return std::make_pair(this->points_[idx], this->times_[idx]); } template -std::pair TrajectoryBase::operator[](unsigned int idx) { +inline std::pair TrajectoryBase::operator[](unsigned int idx) { this->set_empty(false); return std::make_pair(this->points_[idx], this->times_[idx]); } From 9c11b71d1dbc389640e04edc3a327e474e266dae Mon Sep 17 00:00:00 2001 From: Vaios Papaspyros <8146703+bpapaspyros@users.noreply.github.com> Date: Wed, 15 Jan 2025 14:13:23 +0100 Subject: [PATCH 09/52] test: expand test case coverage --- .../test/tests/test_trajectory.cpp | 109 +++++++++++++++++- 1 file changed, 106 insertions(+), 3 deletions(-) diff --git a/source/state_representation/test/tests/test_trajectory.cpp b/source/state_representation/test/tests/test_trajectory.cpp index f45d40154..9f6a94afb 100644 --- a/source/state_representation/test/tests/test_trajectory.cpp +++ b/source/state_representation/test/tests/test_trajectory.cpp @@ -11,7 +11,7 @@ TEST(TrajectoryTest, CreateTrajectory) { state_representation::TrajectoryBase trajectory; std::deque times = trajectory.get_times(); - EXPECT_TRUE(trajectory.get_size() == 0); + EXPECT_EQ(trajectory.get_size(), 0); EXPECT_TRUE(times.empty()); } @@ -121,7 +121,7 @@ TEST(TrajectoryTest, InsertPoint) { trajectory.add_point(point2, std::chrono::nanoseconds(500)); trajectory.insert_point(point3, std::chrono::nanoseconds(100), 1); - EXPECT_TRUE(trajectory.get_size() == 3); + EXPECT_EQ(trajectory.get_size(), 3); EXPECT_TRUE(trajectory[1].second == std::chrono::nanoseconds(400)); EXPECT_TRUE(trajectory[1].first.data() == point3.data()); EXPECT_TRUE(trajectory[0].second == std::chrono::nanoseconds(300)); @@ -139,7 +139,7 @@ TEST(TrajectoryTest, InsertPoint) { trajectory.add_point(point2, std::chrono::nanoseconds(500)); trajectory.insert_point(point3, std::chrono::nanoseconds(100), 1); - EXPECT_TRUE(trajectory.get_size() == 3); + EXPECT_EQ(trajectory.get_size(), 3); EXPECT_TRUE(trajectory[1].second == std::chrono::nanoseconds(400)); EXPECT_TRUE(trajectory[1].first.data() == point3.data()); EXPECT_TRUE(trajectory[0].second == std::chrono::nanoseconds(300)); @@ -148,3 +148,106 @@ TEST(TrajectoryTest, InsertPoint) { EXPECT_TRUE(trajectory[2].first.data() == point2.data()); } } + +TEST(TrajectoryTest, GetPoints) { + { + state_representation::CartesianTrajectory trajectory; + auto point1 = state_representation::CartesianState::Random("point1"); + auto point2 = state_representation::CartesianState::Random("point2"); + auto point3 = state_representation::CartesianState::Random("point3"); + trajectory.add_point(point1, std::chrono::nanoseconds(300)); + trajectory.add_point(point2, std::chrono::nanoseconds(500)); + trajectory.add_point(point3, std::chrono::nanoseconds(100)); + + auto points = trajectory.get_points(); + EXPECT_EQ(points.size(), 3); + EXPECT_TRUE(points[0].data() == point1.data()); + EXPECT_TRUE(points[1].data() == point2.data()); + EXPECT_TRUE(points[2].data() == point3.data()); + + EXPECT_TRUE(trajectory.get_point(0).data() == point1.data()); + EXPECT_TRUE(trajectory.get_point(1).data() == point2.data()); + EXPECT_TRUE(trajectory.get_point(2).data() == point3.data()); + } + + { + state_representation::JointTrajectory trajectory; + auto point1 = state_representation::JointState::Random("point1", 25); + auto point2 = state_representation::JointState::Random("point2", 25); + auto point3 = state_representation::JointState::Random("point3", 25); + trajectory.add_point(point1, std::chrono::nanoseconds(300)); + trajectory.add_point(point2, std::chrono::nanoseconds(500)); + trajectory.add_point(point3, std::chrono::nanoseconds(100)); + + auto points = trajectory.get_points(); + EXPECT_EQ(points.size(), 3); + EXPECT_TRUE(points[0].data() == point1.data()); + EXPECT_TRUE(points[1].data() == point2.data()); + EXPECT_TRUE(points[2].data() == point3.data()); + + EXPECT_TRUE(trajectory.get_point(0).data() == point1.data()); + EXPECT_TRUE(trajectory.get_point(1).data() == point2.data()); + EXPECT_TRUE(trajectory.get_point(2).data() == point3.data()); + } +} + +TEST(TrajectoryTest, SetPoints) { + { + state_representation::CartesianTrajectory trajectory; + auto point1 = state_representation::CartesianState::Random("point1"); + auto point2 = state_representation::CartesianState::Random("point2"); + auto point3 = state_representation::CartesianState::Random("point3"); + + auto replacement1 = state_representation::CartesianState::Random("replacement1"); + auto replacement2 = state_representation::CartesianState::Random("replacement2"); + std::vector replacement_points = {replacement1, replacement2}; + std::vector replacement_times = { + std::chrono::nanoseconds(100), std::chrono::nanoseconds(200)}; + + trajectory.add_point(point1, std::chrono::nanoseconds(300)); + trajectory.add_point(point2, std::chrono::nanoseconds(500)); + trajectory.add_point(point3, std::chrono::nanoseconds(100)); + + trajectory.set_point(1, replacement1, std::chrono::nanoseconds(50)); + EXPECT_TRUE(trajectory[1].first.data() == replacement1.data()); + EXPECT_TRUE(trajectory[1].second == std::chrono::nanoseconds(350)); + + EXPECT_TRUE(trajectory.set_points(replacement_points, replacement_times)); + EXPECT_EQ(trajectory.get_size(), 2); + EXPECT_TRUE(trajectory[0].first.data() == replacement1.data()); + EXPECT_TRUE(trajectory[0].second == std::chrono::nanoseconds(100)); + EXPECT_TRUE(trajectory[1].first.data() == replacement2.data()); + EXPECT_TRUE(trajectory[1].second == std::chrono::nanoseconds(300)); + } + + { + state_representation::JointTrajectory trajectory; + auto point1 = state_representation::JointState::Random("foo", 25); + auto point2 = state_representation::JointState::Random("foo", 25); + auto point3 = state_representation::JointState::Random("foo", 25); + + auto replacement1 = state_representation::JointState::Random("foo", 25); + auto replacement2 = state_representation::JointState::Random("foo", 25); + std::vector replacement_points = {replacement1, replacement2}; + std::vector replacement_times = { + std::chrono::nanoseconds(100), std::chrono::nanoseconds(200)}; + + trajectory.add_point(point1, std::chrono::nanoseconds(300)); + trajectory.add_point(point2, std::chrono::nanoseconds(500)); + trajectory.add_point(point3, std::chrono::nanoseconds(100)); + + trajectory.set_point(1, replacement1, std::chrono::nanoseconds(50)); + EXPECT_TRUE(trajectory[1].first.data() == replacement1.data()); + EXPECT_TRUE(trajectory[1].second == std::chrono::nanoseconds(350)); + + EXPECT_TRUE(trajectory.set_points(replacement_points, replacement_times)); + EXPECT_EQ(trajectory.get_size(), 2); + EXPECT_TRUE(trajectory[0].first.data() == replacement1.data()); + EXPECT_TRUE(trajectory[0].second == std::chrono::nanoseconds(100)); + EXPECT_TRUE(trajectory[1].first.data() == replacement2.data()); + EXPECT_TRUE(trajectory[1].second == std::chrono::nanoseconds(300)); + + replacement1.set_name("bar"); + EXPECT_FALSE(trajectory.set_point(1, replacement1, std::chrono::nanoseconds(50))); + } +} From eeae5ab8f0904ef29d79e0e1d0c43c5246810fea Mon Sep 17 00:00:00 2001 From: Vaios Papaspyros <8146703+bpapaspyros@users.noreply.github.com> Date: Wed, 15 Jan 2025 14:22:47 +0100 Subject: [PATCH 10/52] refactor: rename directory from trajectories->trajectory --- source/state_representation/CMakeLists.txt | 4 ++-- .../{trajectories => trajectory}/CartesianTrajectory.hpp | 2 +- .../{trajectories => trajectory}/JointTrajectory.hpp | 2 +- .../{trajectories => trajectory}/TrajectoryBase.hpp | 0 .../src/{trajectories => trajectory}/CartesianTrajectory.cpp | 2 +- .../src/{trajectories => trajectory}/JointTrajectory.cpp | 2 +- source/state_representation/test/tests/test_trajectory.cpp | 4 ++-- 7 files changed, 8 insertions(+), 8 deletions(-) rename source/state_representation/include/state_representation/{trajectories => trajectory}/CartesianTrajectory.hpp (98%) rename source/state_representation/include/state_representation/{trajectories => trajectory}/JointTrajectory.hpp (98%) rename source/state_representation/include/state_representation/{trajectories => trajectory}/TrajectoryBase.hpp (100%) rename source/state_representation/src/{trajectories => trajectory}/CartesianTrajectory.cpp (96%) rename source/state_representation/src/{trajectories => trajectory}/JointTrajectory.cpp (96%) diff --git a/source/state_representation/CMakeLists.txt b/source/state_representation/CMakeLists.txt index d54c9177f..b06c0fa4c 100644 --- a/source/state_representation/CMakeLists.txt +++ b/source/state_representation/CMakeLists.txt @@ -29,8 +29,8 @@ set(CORE_SOURCES src/parameters/Predicate.cpp src/geometry/Shape.cpp src/geometry/Ellipsoid.cpp - src/trajectories/CartesianTrajectory.cpp - src/trajectories/JointTrajectory.cpp + src/trajectory/CartesianTrajectory.cpp + src/trajectory/JointTrajectory.cpp ) if (EXPERIMENTAL_FEATURES) diff --git a/source/state_representation/include/state_representation/trajectories/CartesianTrajectory.hpp b/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp similarity index 98% rename from source/state_representation/include/state_representation/trajectories/CartesianTrajectory.hpp rename to source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp index cb2a1d93b..f6f537aaf 100644 --- a/source/state_representation/include/state_representation/trajectories/CartesianTrajectory.hpp +++ b/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp @@ -1,7 +1,7 @@ #pragma once #include "state_representation/space/cartesian/CartesianState.hpp" -#include "state_representation/trajectories/TrajectoryBase.hpp" +#include "state_representation/trajectory/TrajectoryBase.hpp" namespace state_representation { class CartesianTrajectory : public TrajectoryBase { diff --git a/source/state_representation/include/state_representation/trajectories/JointTrajectory.hpp b/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp similarity index 98% rename from source/state_representation/include/state_representation/trajectories/JointTrajectory.hpp rename to source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp index dceec7b15..34742e86c 100644 --- a/source/state_representation/include/state_representation/trajectories/JointTrajectory.hpp +++ b/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp @@ -1,7 +1,7 @@ #pragma once #include "state_representation/space/joint/JointState.hpp" -#include "state_representation/trajectories/TrajectoryBase.hpp" +#include "state_representation/trajectory/TrajectoryBase.hpp" namespace state_representation { class JointTrajectory : public TrajectoryBase { diff --git a/source/state_representation/include/state_representation/trajectories/TrajectoryBase.hpp b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp similarity index 100% rename from source/state_representation/include/state_representation/trajectories/TrajectoryBase.hpp rename to source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp diff --git a/source/state_representation/src/trajectories/CartesianTrajectory.cpp b/source/state_representation/src/trajectory/CartesianTrajectory.cpp similarity index 96% rename from source/state_representation/src/trajectories/CartesianTrajectory.cpp rename to source/state_representation/src/trajectory/CartesianTrajectory.cpp index df0a7cbb3..9818ada0b 100644 --- a/source/state_representation/src/trajectories/CartesianTrajectory.cpp +++ b/source/state_representation/src/trajectory/CartesianTrajectory.cpp @@ -1,4 +1,4 @@ -#include "state_representation/trajectories/CartesianTrajectory.hpp" +#include "state_representation/trajectory/CartesianTrajectory.hpp" #include "state_representation/space/cartesian/CartesianState.hpp" #include diff --git a/source/state_representation/src/trajectories/JointTrajectory.cpp b/source/state_representation/src/trajectory/JointTrajectory.cpp similarity index 96% rename from source/state_representation/src/trajectories/JointTrajectory.cpp rename to source/state_representation/src/trajectory/JointTrajectory.cpp index 900970b03..aab9e928a 100644 --- a/source/state_representation/src/trajectories/JointTrajectory.cpp +++ b/source/state_representation/src/trajectory/JointTrajectory.cpp @@ -1,4 +1,4 @@ -#include "state_representation/trajectories/JointTrajectory.hpp" +#include "state_representation/trajectory/JointTrajectory.hpp" #include "state_representation/space/joint/JointState.hpp" namespace state_representation { diff --git a/source/state_representation/test/tests/test_trajectory.cpp b/source/state_representation/test/tests/test_trajectory.cpp index 9f6a94afb..80f70b8a1 100644 --- a/source/state_representation/test/tests/test_trajectory.cpp +++ b/source/state_representation/test/tests/test_trajectory.cpp @@ -1,7 +1,7 @@ #include "state_representation/space/cartesian/CartesianState.hpp" #include "state_representation/space/joint/JointState.hpp" -#include "state_representation/trajectories/CartesianTrajectory.hpp" -#include "state_representation/trajectories/JointTrajectory.hpp" +#include "state_representation/trajectory/CartesianTrajectory.hpp" +#include "state_representation/trajectory/JointTrajectory.hpp" #include From d2dcae764fa8af127879f70f0c97ba73788935d7 Mon Sep 17 00:00:00 2001 From: Vaios Papaspyros <8146703+bpapaspyros@users.noreply.github.com> Date: Wed, 15 Jan 2025 14:24:50 +0100 Subject: [PATCH 11/52] docs: update CHANGELOG.md --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 6cfb0830b..199804486 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -20,6 +20,7 @@ Release Versions ## Upcoming changes - feat: improve support for transformation matrices (#146) +- feat: feat: split trajectory classes and implement Cartesian/JointState specializations (#216) ## 9.1.0 From 6326d0da4cf307a357b8af1e5108932391695cbc Mon Sep 17 00:00:00 2001 From: Vaios Papaspyros <8146703+bpapaspyros@users.noreply.github.com> Date: Wed, 15 Jan 2025 14:34:53 +0100 Subject: [PATCH 12/52] fix: make get_point(s) public in TrajectoryBase --- .../trajectory/TrajectoryBase.hpp | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp index 21fa9caa1..3a49b6954 100644 --- a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp +++ b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp @@ -25,6 +25,17 @@ class TrajectoryBase : public State { */ explicit TrajectoryBase(const std::string& name); + /** + * @brief Get attribute list of trajectory points + */ + const std::deque& get_points() const; + + /** + * @brief Get the trajectory point at given index + * @param index the index + */ + const TrajectoryT& get_point(unsigned int index) const; + /** * @brief Reset trajectory */ @@ -64,17 +75,6 @@ class TrajectoryBase : public State { template void insert_point(const TrajectoryT& new_point, const std::chrono::duration& new_time, int pos); - /** - * @brief Get attribute list of trajectory points - */ - const std::deque& get_points() const; - - /** - * @brief Get the trajectory point at given index - * @param index the index - */ - const TrajectoryT& get_point(unsigned int index) const; - /** * @brief Get the trajectory point at given index * @param index the index From c41fd6724c0eb0f47cb9fbcc47e44439d7af43b1 Mon Sep 17 00:00:00 2001 From: Vaios Papaspyros <8146703+bpapaspyros@users.noreply.github.com> Date: Wed, 15 Jan 2025 15:03:14 +0100 Subject: [PATCH 13/52] docs: update CHANGELOG.md --- CHANGELOG.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 199804486..ad5510b32 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -20,7 +20,7 @@ Release Versions ## Upcoming changes - feat: improve support for transformation matrices (#146) -- feat: feat: split trajectory classes and implement Cartesian/JointState specializations (#216) +- feat: split trajectory classes and implement Cartesian/JointState specializations (#216) ## 9.1.0 From 216f1b96bc929dfe3cf46d020d47e19730c2a70e Mon Sep 17 00:00:00 2001 From: Vaios Papaspyros <8146703+bpapaspyros@users.noreply.github.com> Date: Wed, 15 Jan 2025 15:05:20 +0100 Subject: [PATCH 14/52] fix: python update bindings --- python/source/state_representation/bind_state.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/python/source/state_representation/bind_state.cpp b/python/source/state_representation/bind_state.cpp index 904075c9f..86364df3b 100644 --- a/python/source/state_representation/bind_state.cpp +++ b/python/source/state_representation/bind_state.cpp @@ -22,7 +22,8 @@ void state_type(py::module_& m) { .value("PARAMETER", StateType::PARAMETER) .value("GEOMETRY_SHAPE", StateType::GEOMETRY_SHAPE) .value("GEOMETRY_ELLIPSOID", StateType::GEOMETRY_ELLIPSOID) - .value("TRAJECTORY", StateType::TRAJECTORY) + .value("CARTESIAN_TRAJECTORY", StateType::CARTESIAN_TRAJECTORY) + .value("JOINT_TRAJECTORY", StateType::JOINT_TRAJECTORY) .value("DIGITAL_IO_STATE", StateType::DIGITAL_IO_STATE) .value("ANALOG_IO_STATE", StateType::ANALOG_IO_STATE) .export_values(); From 0dbf0f1de32cf945dba35f5b0e0eed519069a6ef Mon Sep 17 00:00:00 2001 From: Vaios Papaspyros <8146703+bpapaspyros@users.noreply.github.com> Date: Thu, 16 Jan 2025 08:23:23 +0100 Subject: [PATCH 15/52] Revert "feat: move up to C++20" This reverts commit 61455af163d3c0e9444a629a7e3a0ba70d37c23e. --- .vscode/c_cpp_properties.json | 4 ++-- demos/CMakeLists.txt | 4 ++-- protocol/clproto_cpp/CMakeLists.txt | 4 ++-- source/CMakeLists.txt | 4 ++-- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 92cdc6cea..119ea5080 100644 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -9,8 +9,8 @@ "/src/source/**/include" ], "compilerPath": "/usr/bin/gcc", - "cStandard": "c23", - "cppStandard": "gnu++20", + "cStandard": "c17", + "cppStandard": "gnu++17", "intelliSenseMode": "linux-gcc-x64" } ], diff --git a/demos/CMakeLists.txt b/demos/CMakeLists.txt index 4e0e8d346..9482fc0fd 100644 --- a/demos/CMakeLists.txt +++ b/demos/CMakeLists.txt @@ -6,9 +6,9 @@ if(NOT CMAKE_C_STANDARD) set(CMAKE_C_STANDARD 99) endif() -# Default to C++20 +# Default to C++17 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 20) + set(CMAKE_CXX_STANDARD 17) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/protocol/clproto_cpp/CMakeLists.txt b/protocol/clproto_cpp/CMakeLists.txt index e7f984c3d..c614e9e34 100644 --- a/protocol/clproto_cpp/CMakeLists.txt +++ b/protocol/clproto_cpp/CMakeLists.txt @@ -7,9 +7,9 @@ if(NOT CMAKE_C_STANDARD) set(CMAKE_C_STANDARD 99) endif() -# Default to C++20 +# Default to C++17 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 20) + set(CMAKE_CXX_STANDARD 17) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/source/CMakeLists.txt b/source/CMakeLists.txt index 9158b6319..b4083986c 100644 --- a/source/CMakeLists.txt +++ b/source/CMakeLists.txt @@ -15,9 +15,9 @@ if(NOT CMAKE_C_STANDARD) set(CMAKE_C_STANDARD 99) endif() -# Default to C++20 +# Default to C++17 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 20) + set(CMAKE_CXX_STANDARD 17) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") From e4f3fb0add89aa9b718d07125603b39e9e7150a3 Mon Sep 17 00:00:00 2001 From: Vaios Papaspyros <8146703+bpapaspyros@users.noreply.github.com> Date: Thu, 16 Jan 2025 08:27:21 +0100 Subject: [PATCH 16/52] refactor: tidy up headers (public/protected/private) --- .../trajectory/CartesianTrajectory.hpp | 6 +++--- .../state_representation/trajectory/JointTrajectory.hpp | 8 ++++---- .../state_representation/trajectory/TrajectoryBase.hpp | 8 ++++---- 3 files changed, 11 insertions(+), 11 deletions(-) diff --git a/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp b/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp index f6f537aaf..bb91064ae 100644 --- a/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp +++ b/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp @@ -5,9 +5,6 @@ namespace state_representation { class CartesianTrajectory : public TrajectoryBase { -private: - std::string reference_frame_;///< name of the reference frame - public: /** * @brief Constructor with name and reference frame provided @@ -90,6 +87,9 @@ class CartesianTrajectory : public TrajectoryBase { * @brief Clear trajectory */ virtual void clear() override; + +private: + std::string reference_frame_;///< name of the reference frame }; template diff --git a/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp b/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp index 34742e86c..758c897b8 100644 --- a/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp +++ b/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp @@ -5,10 +5,6 @@ namespace state_representation { class JointTrajectory : public TrajectoryBase { -private: - std::vector joint_names_;///< names of the joints - std::string robot_name_; ///< name of the robot - public: /** * @brief Empty constructor @@ -93,6 +89,10 @@ class JointTrajectory : public TrajectoryBase { * @brief Clear trajectory */ virtual void clear() override; + +private: + std::vector joint_names_;///< names of the joints + std::string robot_name_; ///< name of the robot }; template diff --git a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp index 3a49b6954..eb00c93d7 100644 --- a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp +++ b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp @@ -9,10 +9,6 @@ namespace state_representation { template class TrajectoryBase : public State { -private: - std::deque points_; - std::deque times_; - public: /** * @brief Empty constructor @@ -113,6 +109,10 @@ class TrajectoryBase : public State { * corresponding time */ std::pair operator[](unsigned int idx); + +private: + std::deque points_; + std::deque times_; }; template From f1844f64e3c7bee0eeafc123cbccf76f25fef2c2 Mon Sep 17 00:00:00 2001 From: Vaios Papaspyros <8146703+bpapaspyros@users.noreply.github.com> Date: Thu, 16 Jan 2025 08:29:45 +0100 Subject: [PATCH 17/52] refactor: redifine number sequence of StateType enum --- .../include/state_representation/StateType.hpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/source/state_representation/include/state_representation/StateType.hpp b/source/state_representation/include/state_representation/StateType.hpp index 3cbbd3d87..8e35b3070 100644 --- a/source/state_representation/include/state_representation/StateType.hpp +++ b/source/state_representation/include/state_representation/StateType.hpp @@ -30,14 +30,14 @@ enum class StateType { PARAMETER = 14, GEOMETRY_SHAPE = 15, GEOMETRY_ELLIPSOID = 16, - CARTESIAN_TRAJECTORY = 17, - JOINT_TRAJECTORY = 18, - DIGITAL_IO_STATE = 19, - ANALOG_IO_STATE = 20, + DIGITAL_IO_STATE = 18, + ANALOG_IO_STATE = 19, + CARTESIAN_TRAJECTORY = 20, + JOINT_TRAJECTORY = 21, #ifdef EXPERIMENTAL_FEATURES - DUAL_QUATERNION_STATE = 21, - DUAL_QUATERNION_POSE = 22, - DUAL_QUATERNION_TWIST = 23 + DUAL_QUATERNION_STATE = 22, + DUAL_QUATERNION_POSE = 23, + DUAL_QUATERNION_TWIST = 24 #endif }; From 94ef3f381dfd2092e9bc64fe8e1fb21bebcd56d0 Mon Sep 17 00:00:00 2001 From: Vaios Papaspyros <8146703+bpapaspyros@users.noreply.github.com> Date: Thu, 16 Jan 2025 08:30:59 +0100 Subject: [PATCH 18/52] fix: make TrajectoryBase contructors protected --- .../trajectory/TrajectoryBase.hpp | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp index eb00c93d7..e22142ae1 100644 --- a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp +++ b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp @@ -10,17 +10,6 @@ namespace state_representation { template class TrajectoryBase : public State { public: - /** - * @brief Empty constructor - */ - explicit TrajectoryBase(); - - /** - * @brief Constructor with name provided - * @brief name the name of the state - */ - explicit TrajectoryBase(const std::string& name); - /** * @brief Get attribute list of trajectory points */ @@ -58,6 +47,17 @@ class TrajectoryBase : public State { int get_size() const; protected: + /** + * @brief Empty constructor + */ + explicit TrajectoryBase(); + + /** + * @brief Constructor with name provided + * @brief name the name of the state + */ + explicit TrajectoryBase(const std::string& name); + /** * @brief Add new point and corresponding time to trajectory */ From 5c45becb1764e7bd109c82927f1c42f50994eba4 Mon Sep 17 00:00:00 2001 From: Vaios Papaspyros <8146703+bpapaspyros@users.noreply.github.com> Date: Thu, 16 Jan 2025 08:36:43 +0100 Subject: [PATCH 19/52] docs: docstrings --- .../trajectory/TrajectoryBase.hpp | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp index e22142ae1..5dd07e244 100644 --- a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp +++ b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp @@ -22,29 +22,29 @@ class TrajectoryBase : public State { const TrajectoryT& get_point(unsigned int index) const; /** - * @brief Reset trajectory + * @brief Get attribute list of trajectory times */ - virtual void reset(); + const std::deque& get_times() const; /** - * @brief Delete last point and corresponding time from trajectory + * @brief Get attribute number of point in trajectory */ - virtual void delete_point(); + int get_size() const; /** - * @brief Clear trajectory + * @brief Reset trajectory */ - virtual void clear(); + virtual void reset(); /** - * @brief Get attribute list of trajectory times + * @brief Delete last point and corresponding time from trajectory */ - const std::deque& get_times() const; + virtual void delete_point(); /** - * @brief Get attribute number of point in trajectory + * @brief Clear trajectory */ - int get_size() const; + virtual void clear(); protected: /** @@ -89,7 +89,7 @@ class TrajectoryBase : public State { set_point(unsigned int index, const TrajectoryT& point, const std::chrono::duration& new_time); /** - * @brief Set the trajectory point at given index + * @brief Set the trajectory points from a vector of points * @param points vector of new points * @param new_time vector of new times * @return Success of the operation From 0b06d51b32d66d5db4d00e30f01575514d80f363 Mon Sep 17 00:00:00 2001 From: Vaios Papaspyros <8146703+bpapaspyros@users.noreply.github.com> Date: Thu, 16 Jan 2025 08:47:24 +0100 Subject: [PATCH 20/52] fix: change set_point signature to move index at the end --- .../trajectory/CartesianTrajectory.hpp | 8 ++++---- .../state_representation/trajectory/JointTrajectory.hpp | 8 ++++---- .../state_representation/trajectory/TrajectoryBase.hpp | 6 +++--- 3 files changed, 11 insertions(+), 11 deletions(-) diff --git a/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp b/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp index bb91064ae..41d18891e 100644 --- a/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp +++ b/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp @@ -47,14 +47,14 @@ class CartesianTrajectory : public TrajectoryBase { /** * @brief Set the trajectory point at given index - * @param index the index * @param point the new point * @param new_time the new time + * @param index the index * @return Success of the operation */ template bool - set_point(unsigned int index, const CartesianState& point, const std::chrono::duration& new_time); + set_point(const CartesianState& point, const std::chrono::duration& new_time, unsigned int index); /** * @brief Set the trajectory point at given index @@ -128,14 +128,14 @@ inline bool CartesianTrajectory::insert_point( template inline bool CartesianTrajectory::set_point( - unsigned int index, const CartesianState& point, const std::chrono::duration& new_time) { + const CartesianState& point, const std::chrono::duration& new_time, unsigned int index) { if (point.is_empty()) { return false; } if (point.get_reference_frame() != reference_frame_) { return false; } - return this->TrajectoryBase::set_point(index, point.data(), new_time); + return this->TrajectoryBase::set_point(point.data(), new_time, index); } template diff --git a/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp b/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp index 758c897b8..982cb0524 100644 --- a/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp +++ b/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp @@ -50,14 +50,14 @@ class JointTrajectory : public TrajectoryBase { /** * @brief Set the trajectory point at given index - * @param index the index * @param point the new point * @param new_time the new time + * @param index the index * @return Success of the operation */ template bool - set_point(unsigned int index, const JointState& point, const std::chrono::duration& new_time); + set_point(const JointState& point, const std::chrono::duration& new_time, unsigned int index); /** * @brief Set the trajectory point at given index @@ -129,11 +129,11 @@ inline bool JointTrajectory::insert_point( template inline bool JointTrajectory::set_point( - unsigned int index, const JointState& point, const std::chrono::duration& new_time) { + const JointState& point, const std::chrono::duration& new_time, unsigned int index) { if (point.is_empty() || (point.get_names() != this->joint_names_) || (point.get_name() != this->robot_name_)) { return false; } - return this->TrajectoryBase::set_point(index, point.data(), new_time); + return this->TrajectoryBase::set_point(point.data(), new_time, index); } template diff --git a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp index 5dd07e244..65e98db31 100644 --- a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp +++ b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp @@ -79,14 +79,14 @@ class TrajectoryBase : public State { /** * @brief Set the trajectory point at given index - * @param index the index * @param point the new point * @param new_time the new time + * @param index the index * @return Success of the operation */ template bool - set_point(unsigned int index, const TrajectoryT& point, const std::chrono::duration& new_time); + set_point(const TrajectoryT& point, const std::chrono::duration& new_time, unsigned int index); /** * @brief Set the trajectory points from a vector of points @@ -205,7 +205,7 @@ inline TrajectoryT& TrajectoryBase::get_point(unsigned int index) { template template inline bool TrajectoryBase::set_point( - unsigned int index, const TrajectoryT& point, const std::chrono::duration& new_time) { + const TrajectoryT& point, const std::chrono::duration& new_time, unsigned int index) { if (index < this->points_.size()) { this->points_[index] = point; if (index == 0) { From 0748fb2c03732a72758a0a7052e061d6fdbd2b47 Mon Sep 17 00:00:00 2001 From: Vaios Papaspyros <8146703+bpapaspyros@users.noreply.github.com> Date: Thu, 16 Jan 2025 08:55:19 +0100 Subject: [PATCH 21/52] fix: minor fix in TrajectoryBase [] overload --- .../include/state_representation/trajectory/TrajectoryBase.hpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp index 65e98db31..1e8c36e23 100644 --- a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp +++ b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp @@ -117,13 +117,11 @@ class TrajectoryBase : public State { template inline TrajectoryBase::TrajectoryBase() : State() { - this->set_type(StateType::NONE); this->reset(); } template inline TrajectoryBase::TrajectoryBase(const std::string& name) : State(name) { - this->set_type(StateType::NONE); this->reset(); } @@ -253,7 +251,6 @@ TrajectoryBase::operator[](unsigned int idx) const { template inline std::pair TrajectoryBase::operator[](unsigned int idx) { - this->set_empty(false); return std::make_pair(this->points_[idx], this->times_[idx]); } }// namespace state_representation From 2b40eb98f0af85098f593fcdf4ed6cab884fbb0f Mon Sep 17 00:00:00 2001 From: Vaios Papaspyros <8146703+bpapaspyros@users.noreply.github.com> Date: Thu, 16 Jan 2025 09:21:04 +0100 Subject: [PATCH 22/52] feat: add more Trajectory constructors --- .../trajectory/CartesianTrajectory.hpp | 46 ++++++++++++++++++- .../trajectory/JointTrajectory.hpp | 44 ++++++++++++++++-- .../src/trajectory/CartesianTrajectory.cpp | 2 +- .../src/trajectory/JointTrajectory.cpp | 5 -- 4 files changed, 86 insertions(+), 11 deletions(-) diff --git a/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp b/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp index 41d18891e..d4de9395c 100644 --- a/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp +++ b/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp @@ -8,10 +8,34 @@ class CartesianTrajectory : public TrajectoryBase { public: /** * @brief Constructor with name and reference frame provided + * @param name the name of the state + * @param reference_frame reference frame of the trajectory points + */ + explicit CartesianTrajectory(const std::string& name = "", const std::string& reference_frame = "world"); + + /** + * @brief Constructor with name and reference frame provided + * @param point the initial point + * @param time the initial time + * @param name the name of the state * @param reference_frame reference frame of the trajectory points + */ + template + explicit CartesianTrajectory( + const CartesianState& point, const std::chrono::duration& time, const std::string& name = "", + const std::string& reference_frame = "world"); + + /** + * @brief Constructor with name and reference frame provided + * @param points vector of initial points + * @param times vector of initial times * @param name the name of the state + * @param reference_frame reference frame of the trajectory points */ - explicit CartesianTrajectory(const std::string& reference_frame = "world", const std::string& name = ""); + template + explicit CartesianTrajectory( + const std::vector& points, const std::vector>& times, + const std::string& name = "", const std::string& reference_frame = "world"); /** * @brief Getter of the reference frame as const reference @@ -92,6 +116,26 @@ class CartesianTrajectory : public TrajectoryBase { std::string reference_frame_;///< name of the reference frame }; +template +CartesianTrajectory::CartesianTrajectory( + const CartesianState& point, const std::chrono::duration& time, const std::string& name, + const std::string& reference_frame) + : TrajectoryBase(name), reference_frame_(reference_frame) { + this->set_type(StateType::CARTESIAN_TRAJECTORY); + this->reset(); + this->add_point(point, time); +} + +template +CartesianTrajectory::CartesianTrajectory( + const std::vector& points, const std::vector>& times, + const std::string& name, const std::string& reference_frame) + : TrajectoryBase(name), reference_frame_(reference_frame) { + this->set_type(StateType::CARTESIAN_TRAJECTORY); + this->reset(); + this->set_points(points, times); +} + template inline bool CartesianTrajectory::add_point( const CartesianState& new_point, const std::chrono::duration& new_time) { diff --git a/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp b/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp index 982cb0524..f38c86dd9 100644 --- a/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp +++ b/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp @@ -4,18 +4,35 @@ #include "state_representation/trajectory/TrajectoryBase.hpp" namespace state_representation { + class JointTrajectory : public TrajectoryBase { public: /** - * @brief Empty constructor + * @brief Constructor with name and reference frame provided + * @brief name the name of the state */ - explicit JointTrajectory(); + explicit JointTrajectory(const std::string& name = ""); /** * @brief Constructor with name and reference frame provided - * @brief name the name of the state + * @param point the initial point + * @param time the initial time + * @param name the name of the state + */ + template + explicit JointTrajectory( + const JointState& point, const std::chrono::duration& time, const std::string& name = ""); + + /** + * @brief Constructor with name and reference frame provided + * @param points vector of initial points + * @param times vector of initial times + * @param name the name of the state */ - explicit JointTrajectory(const std::string& name); + template + explicit JointTrajectory( + const std::vector& points, const std::vector>& times, + const std::string& name = ""); /** * @brief Getter of the names attribute @@ -95,6 +112,25 @@ class JointTrajectory : public TrajectoryBase { std::string robot_name_; ///< name of the robot }; +template +JointTrajectory::JointTrajectory( + const JointState& point, const std::chrono::duration& time, const std::string& name) + : TrajectoryBase(name) { + this->set_type(StateType::JOINT_TRAJECTORY); + this->reset(); + this->add_point(point, time); +} + +template +JointTrajectory::JointTrajectory( + const std::vector& points, const std::vector>& times, + const std::string& name) + : TrajectoryBase(name) { + this->set_type(StateType::JOINT_TRAJECTORY); + this->reset(); + this->set_points(points, times); +} + template inline bool JointTrajectory::add_point(const JointState& new_point, const std::chrono::duration& new_time) { diff --git a/source/state_representation/src/trajectory/CartesianTrajectory.cpp b/source/state_representation/src/trajectory/CartesianTrajectory.cpp index 9818ada0b..04bafa1ab 100644 --- a/source/state_representation/src/trajectory/CartesianTrajectory.cpp +++ b/source/state_representation/src/trajectory/CartesianTrajectory.cpp @@ -4,7 +4,7 @@ #include namespace state_representation { -CartesianTrajectory::CartesianTrajectory(const std::string& reference_frame, const std::string& name) +CartesianTrajectory::CartesianTrajectory(const std::string& name, const std::string& reference_frame) : TrajectoryBase(name), reference_frame_(reference_frame) { this->set_type(StateType::CARTESIAN_TRAJECTORY); this->reset(); diff --git a/source/state_representation/src/trajectory/JointTrajectory.cpp b/source/state_representation/src/trajectory/JointTrajectory.cpp index aab9e928a..2c743bd33 100644 --- a/source/state_representation/src/trajectory/JointTrajectory.cpp +++ b/source/state_representation/src/trajectory/JointTrajectory.cpp @@ -3,11 +3,6 @@ namespace state_representation { -JointTrajectory::JointTrajectory() : TrajectoryBase() { - this->set_type(StateType::JOINT_TRAJECTORY); - this->reset(); -} - JointTrajectory::JointTrajectory(const std::string& name) : TrajectoryBase(name) { this->set_type(StateType::JOINT_TRAJECTORY); this->reset(); From 198569d0e189409e83c7822212452fbfdd92a101 Mon Sep 17 00:00:00 2001 From: Vaios Papaspyros <8146703+bpapaspyros@users.noreply.github.com> Date: Thu, 16 Jan 2025 10:01:15 +0100 Subject: [PATCH 23/52] fix: set_point(s) functions throw instead of returning bool --- .../trajectory/CartesianTrajectory.hpp | 47 ++++++++++----- .../trajectory/JointTrajectory.hpp | 60 +++++++++++++------ .../trajectory/TrajectoryBase.hpp | 23 +++---- 3 files changed, 85 insertions(+), 45 deletions(-) diff --git a/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp b/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp index d4de9395c..b948b4722 100644 --- a/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp +++ b/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp @@ -3,6 +3,9 @@ #include "state_representation/space/cartesian/CartesianState.hpp" #include "state_representation/trajectory/TrajectoryBase.hpp" +#include "state_representation/exceptions/EmptyStateException.hpp" +#include "state_representation/exceptions/IncompatibleReferenceFramesException.hpp" + namespace state_representation { class CartesianTrajectory : public TrajectoryBase { public: @@ -74,20 +77,24 @@ class CartesianTrajectory : public TrajectoryBase { * @param point the new point * @param new_time the new time * @param index the index - * @return Success of the operation + * @throw std::out_of_range if index is out of range + * @throw EmptyStateException if point is empty + * @throw IncompatibleReferenceFramesException if point has different reference frame */ template - bool + void set_point(const CartesianState& point, const std::chrono::duration& new_time, unsigned int index); /** * @brief Set the trajectory point at given index * @param points vector of new points * @param new_time vector of new times - * @return Success of the operation + * @throw IncompatibleSizeException if points and new_times have different sizes + * @throw EmptyStateException if point is empty + * @throw IncompatibleReferenceFramesException if point has different reference frame */ template - bool set_points( + void set_points( const std::vector& points, const std::vector>& new_times); @@ -171,38 +178,46 @@ inline bool CartesianTrajectory::insert_point( } template -inline bool CartesianTrajectory::set_point( +inline void CartesianTrajectory::set_point( const CartesianState& point, const std::chrono::duration& new_time, unsigned int index) { if (point.is_empty()) { - return false; + throw exceptions::EmptyStateException("Point is empty"); } if (point.get_reference_frame() != reference_frame_) { - return false; + throw exceptions::IncompatibleReferenceFramesException( + "Incompatible reference frames: " + point.get_reference_frame() + " and " + reference_frame_); + } + try { + this->TrajectoryBase::set_point(point.data(), new_time, index); + } catch (...) { + throw; } - return this->TrajectoryBase::set_point(point.data(), new_time, index); } template -inline bool CartesianTrajectory::set_points( +inline void CartesianTrajectory::set_points( const std::vector& points, const std::vector>& new_times) { if (points.empty()) { - return false; + throw exceptions::IncompatibleSizeException("Points vector is empty"); } std::string candidate_reference_frame = points[0].get_reference_frame(); std::vector data; for (const auto& point : points) { - if (point.is_empty() || (point.get_reference_frame() != candidate_reference_frame)) { - return false; + if (point.is_empty()) { + throw exceptions::EmptyStateException("Vector contains at least one point that is empty"); + } else if (point.get_reference_frame() != candidate_reference_frame) { + throw exceptions::IncompatibleReferenceFramesException( + "Incompatible reference frames: " + point.get_reference_frame() + " and " + candidate_reference_frame); } data.push_back(point.data()); } - if (this->TrajectoryBase::set_points(data, new_times)) { + try { + this->TrajectoryBase::set_points(data, new_times); reference_frame_ = candidate_reference_frame; - return true; - } else { - return false; + } catch (...) { + throw; } } diff --git a/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp b/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp index f38c86dd9..341fb6585 100644 --- a/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp +++ b/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp @@ -3,6 +3,9 @@ #include "state_representation/space/joint/JointState.hpp" #include "state_representation/trajectory/TrajectoryBase.hpp" +#include "state_representation/exceptions/EmptyStateException.hpp" +#include "state_representation/exceptions/IncompatibleStatesException.hpp" + namespace state_representation { class JointTrajectory : public TrajectoryBase { @@ -70,20 +73,24 @@ class JointTrajectory : public TrajectoryBase { * @param point the new point * @param new_time the new time * @param index the index - * @return Success of the operation + * @throw std::out_of_range if index is out of range + * @throw EmptyStateException if point is empty + * @throw IncompatibleReferenceFramesException if point has different reference frame */ template - bool + void set_point(const JointState& point, const std::chrono::duration& new_time, unsigned int index); /** * @brief Set the trajectory point at given index * @param points vector of new points * @param new_time vector of new times - * @return Success of the operation + * @throw IncompatibleSizeException if points and new_times have different sizes + * @throw EmptyStateException if point is empty + * @throw IncompatibleReferenceFramesException if point has different reference frame */ template - bool set_points( + void set_points( const std::vector& points, const std::vector>& new_times); /** @@ -164,37 +171,54 @@ inline bool JointTrajectory::insert_point( } template -inline bool JointTrajectory::set_point( +inline void JointTrajectory::set_point( const JointState& point, const std::chrono::duration& new_time, unsigned int index) { - if (point.is_empty() || (point.get_names() != this->joint_names_) || (point.get_name() != this->robot_name_)) { - return false; + if (point.is_empty()) { + throw exceptions::EmptyStateException("Point is empty"); + } else if (point.get_names() != this->joint_names_) { + throw exceptions::IncompatibleStatesException( + "Incompatible joint names between the new point and current trajectory"); + } else if (point.get_name() != this->robot_name_) { + throw exceptions::IncompatibleStatesException( + "Incompatible robot name between the new point and current trajectory"); + } + try { + this->TrajectoryBase::set_point(point.data(), new_time, index); + } catch (...) { + throw; } - return this->TrajectoryBase::set_point(point.data(), new_time, index); } template -inline bool JointTrajectory::set_points( +inline void JointTrajectory::set_points( const std::vector& points, const std::vector>& new_times) { - if (points.empty() || points[0].is_empty()) { - return false; + if (points.empty()) { + throw exceptions::EmptyStateException("Points vector is empty"); + } else if (points[0].is_empty()) { + throw exceptions::EmptyStateException("Vector contains at least one point that is empty"); } auto candidate_robot_name = points[0].get_name(); auto candidate_joint_names = points[0].get_names(); std::vector data; for (const auto& point : points) { - if (point.is_empty() || (point.get_names() != candidate_joint_names) - || (point.get_name() != candidate_robot_name)) { - return false; + if (point.is_empty()) { + throw exceptions::EmptyStateException("Vector contains at least one point that is empty"); + } else if (point.get_names() != candidate_joint_names) { + throw exceptions::IncompatibleStatesException( + "Incompatible joint names between the new point and current trajectory"); + } else if (point.get_name() != candidate_robot_name) { + throw exceptions::IncompatibleStatesException( + "Incompatible robot name between the new point and current trajectory"); } data.push_back(point.data()); } - if (this->TrajectoryBase::set_points(data, new_times)) { + try { + this->TrajectoryBase::set_points(data, new_times); this->joint_names_ = candidate_joint_names; this->robot_name_ = candidate_robot_name; - return true; - } else { - return false; + } catch (...) { + throw; } } }// namespace state_representation diff --git a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp index 1e8c36e23..4f80275b9 100644 --- a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp +++ b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp @@ -1,7 +1,7 @@ #pragma once #include "state_representation/State.hpp" -#include "state_representation/StateType.hpp" +#include "state_representation/exceptions/IncompatibleSizeException.hpp" #include #include @@ -82,20 +82,20 @@ class TrajectoryBase : public State { * @param point the new point * @param new_time the new time * @param index the index - * @return Success of the operation + * @throw std::out_of_range if index is out of range */ template - bool + void set_point(const TrajectoryT& point, const std::chrono::duration& new_time, unsigned int index); /** * @brief Set the trajectory points from a vector of points * @param points vector of new points * @param new_time vector of new times - * @return Success of the operation + * @throw IncompatibleSizeException if points and new_times have different sizes */ template - bool set_points( + void set_points( const std::vector& points, const std::vector>& new_times); /** @@ -202,7 +202,7 @@ inline TrajectoryT& TrajectoryBase::get_point(unsigned int index) { template template -inline bool TrajectoryBase::set_point( +inline void TrajectoryBase::set_point( const TrajectoryT& point, const std::chrono::duration& new_time, unsigned int index) { if (index < this->points_.size()) { this->points_[index] = point; @@ -214,23 +214,24 @@ inline bool TrajectoryBase::set_point( for (unsigned int i = index + 1; i < this->points_.size(); ++i) { this->times_[i] += this->times_[index - 1]; } - return true; + } else { + throw std::out_of_range("Index out of range"); } - return false; } template template -inline bool TrajectoryBase::set_points( +inline void TrajectoryBase::set_points( const std::vector& points, const std::vector>& new_times) { if (points.size() != new_times.size()) { - return false; + throw exceptions::IncompatibleSizeException( + "The point and time vectors provided have different sizes " + std::to_string(points.size()) + " and " + + std::to_string(new_times.size())); } this->clear(); for (unsigned int i = 0; i < points.size(); ++i) { this->add_point(points[i], new_times[i]); } - return true; } template From c5c7139d2115e169343484169568bc8fec8ce3e7 Mon Sep 17 00:00:00 2001 From: Vaios Papaspyros <8146703+bpapaspyros@users.noreply.github.com> Date: Thu, 16 Jan 2025 10:01:42 +0100 Subject: [PATCH 24/52] test: adapt tests --- .../test/tests/test_trajectory.cpp | 19 +++++++------------ 1 file changed, 7 insertions(+), 12 deletions(-) diff --git a/source/state_representation/test/tests/test_trajectory.cpp b/source/state_representation/test/tests/test_trajectory.cpp index 80f70b8a1..7b3f2aa7c 100644 --- a/source/state_representation/test/tests/test_trajectory.cpp +++ b/source/state_representation/test/tests/test_trajectory.cpp @@ -8,13 +8,6 @@ #include #include -TEST(TrajectoryTest, CreateTrajectory) { - state_representation::TrajectoryBase trajectory; - std::deque times = trajectory.get_times(); - EXPECT_EQ(trajectory.get_size(), 0); - EXPECT_TRUE(times.empty()); -} - TEST(TrajectoryTest, AddPoint) { { state_representation::CartesianTrajectory trajectory("world"); @@ -208,11 +201,11 @@ TEST(TrajectoryTest, SetPoints) { trajectory.add_point(point2, std::chrono::nanoseconds(500)); trajectory.add_point(point3, std::chrono::nanoseconds(100)); - trajectory.set_point(1, replacement1, std::chrono::nanoseconds(50)); + trajectory.set_point(replacement1, std::chrono::nanoseconds(50), 1); EXPECT_TRUE(trajectory[1].first.data() == replacement1.data()); EXPECT_TRUE(trajectory[1].second == std::chrono::nanoseconds(350)); - EXPECT_TRUE(trajectory.set_points(replacement_points, replacement_times)); + EXPECT_NO_THROW(trajectory.set_points(replacement_points, replacement_times)); EXPECT_EQ(trajectory.get_size(), 2); EXPECT_TRUE(trajectory[0].first.data() == replacement1.data()); EXPECT_TRUE(trajectory[0].second == std::chrono::nanoseconds(100)); @@ -236,11 +229,11 @@ TEST(TrajectoryTest, SetPoints) { trajectory.add_point(point2, std::chrono::nanoseconds(500)); trajectory.add_point(point3, std::chrono::nanoseconds(100)); - trajectory.set_point(1, replacement1, std::chrono::nanoseconds(50)); + trajectory.set_point(replacement1, std::chrono::nanoseconds(50), 1); EXPECT_TRUE(trajectory[1].first.data() == replacement1.data()); EXPECT_TRUE(trajectory[1].second == std::chrono::nanoseconds(350)); - EXPECT_TRUE(trajectory.set_points(replacement_points, replacement_times)); + EXPECT_NO_THROW(trajectory.set_points(replacement_points, replacement_times)); EXPECT_EQ(trajectory.get_size(), 2); EXPECT_TRUE(trajectory[0].first.data() == replacement1.data()); EXPECT_TRUE(trajectory[0].second == std::chrono::nanoseconds(100)); @@ -248,6 +241,8 @@ TEST(TrajectoryTest, SetPoints) { EXPECT_TRUE(trajectory[1].second == std::chrono::nanoseconds(300)); replacement1.set_name("bar"); - EXPECT_FALSE(trajectory.set_point(1, replacement1, std::chrono::nanoseconds(50))); + EXPECT_THROW( + trajectory.set_point(replacement1, std::chrono::nanoseconds(50), 1), + state_representation::exceptions::IncompatibleStatesException); } } From cf3013724a56ed7bb1dd85edbed747b65fca4b85 Mon Sep 17 00:00:00 2001 From: Vaios Papaspyros <8146703+bpapaspyros@users.noreply.github.com> Date: Sun, 19 Jan 2025 17:11:38 +0100 Subject: [PATCH 25/52] refactor: redesign the flat trajectory classes and pack data better --- .../trajectory/CartesianTrajectory.hpp | 154 ++----- .../trajectory/JointTrajectory.hpp | 163 ++----- .../trajectory/TrajectoryBase.hpp | 161 ++++--- .../src/trajectory/CartesianTrajectory.cpp | 169 +++++++- .../src/trajectory/JointTrajectory.cpp | 164 ++++++- .../test/tests/test_trajectory.cpp | 399 +++++++++--------- 6 files changed, 617 insertions(+), 593 deletions(-) diff --git a/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp b/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp index b948b4722..5fe21e867 100644 --- a/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp +++ b/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp @@ -3,11 +3,13 @@ #include "state_representation/space/cartesian/CartesianState.hpp" #include "state_representation/trajectory/TrajectoryBase.hpp" -#include "state_representation/exceptions/EmptyStateException.hpp" -#include "state_representation/exceptions/IncompatibleReferenceFramesException.hpp" - namespace state_representation { -class CartesianTrajectory : public TrajectoryBase { + +struct CartesianTrajectoryPoint : public TrajectoryPoint { + std::string name; +}; + +class CartesianTrajectory : public TrajectoryBase { public: /** * @brief Constructor with name and reference frame provided @@ -19,90 +21,87 @@ class CartesianTrajectory : public TrajectoryBase { /** * @brief Constructor with name and reference frame provided * @param point the initial point - * @param time the initial time + * @param duration the initial duration * @param name the name of the state * @param reference_frame reference frame of the trajectory points */ - template explicit CartesianTrajectory( - const CartesianState& point, const std::chrono::duration& time, const std::string& name = "", + const CartesianState& point, const std::chrono::nanoseconds& duration, const std::string& name = "", const std::string& reference_frame = "world"); /** * @brief Constructor with name and reference frame provided * @param points vector of initial points - * @param times vector of initial times + * @param durations vector of initial durations * @param name the name of the state * @param reference_frame reference frame of the trajectory points */ - template explicit CartesianTrajectory( - const std::vector& points, const std::vector>& times, + const std::vector& points, const std::vector& durations, const std::string& name = "", const std::string& reference_frame = "world"); /** * @brief Getter of the reference frame as const reference + * @return the reference frame associated with the trajectory */ const std::string get_reference_frame() const; /** * @brief Add new point and corresponding time to trajectory - * @return Success of the operation + * @param new_point the new trajectory point + * @param duration the duration for the new point */ - template - bool add_point(const CartesianState& new_point, const std::chrono::duration& new_time); + void add_point(const CartesianState& new_point, const std::chrono::nanoseconds& duration); /** * @brief Insert new point and corresponding time to trajectory between two * already existing points - * @return Success of the operation + * @param new_point the new trajectory point + * @param duration the duration for the new point + * @param pos the desired position of the new point in the queue */ - template - bool - insert_point(const CartesianState& new_point, const std::chrono::duration& new_time, int pos); + void insert_point(const CartesianState& new_point, const std::chrono::nanoseconds& duration, unsigned int pos); /** * @brief Get attribute list of trajectory points + * @return queue of the Cartesian states of the trajectory */ const std::deque get_points() const; /** * @brief Get the trajectory point at given index * @param index the index + * @return the Cartesian state that corresponds to the index */ CartesianState get_point(unsigned int index) const; /** * @brief Set the trajectory point at given index * @param point the new point - * @param new_time the new time + * @param duration the new time * @param index the index * @throw std::out_of_range if index is out of range * @throw EmptyStateException if point is empty * @throw IncompatibleReferenceFramesException if point has different reference frame */ - template - void - set_point(const CartesianState& point, const std::chrono::duration& new_time, unsigned int index); + void set_point(const CartesianState& point, const std::chrono::nanoseconds& duration, unsigned int index); /** * @brief Set the trajectory point at given index * @param points vector of new points - * @param new_time vector of new times - * @throw IncompatibleSizeException if points and new_times have different sizes + * @param durations vector of new durations + * @throw IncompatibleSizeException if points and durations have different sizes * @throw EmptyStateException if point is empty * @throw IncompatibleReferenceFramesException if point has different reference frame */ - template - void set_points( - const std::vector& points, - const std::vector>& new_times); + void set_points(const std::vector& points, const std::vector& durations); /** * @brief Operator overload for returning a single trajectory point and * corresponding time + * @return the Cartesian state and duration pair that corresponds to the index */ - std::pair operator[](unsigned int idx); + std::pair operator[](unsigned int idx) const; /** * @brief Reset trajectory @@ -122,103 +121,4 @@ class CartesianTrajectory : public TrajectoryBase { private: std::string reference_frame_;///< name of the reference frame }; - -template -CartesianTrajectory::CartesianTrajectory( - const CartesianState& point, const std::chrono::duration& time, const std::string& name, - const std::string& reference_frame) - : TrajectoryBase(name), reference_frame_(reference_frame) { - this->set_type(StateType::CARTESIAN_TRAJECTORY); - this->reset(); - this->add_point(point, time); -} - -template -CartesianTrajectory::CartesianTrajectory( - const std::vector& points, const std::vector>& times, - const std::string& name, const std::string& reference_frame) - : TrajectoryBase(name), reference_frame_(reference_frame) { - this->set_type(StateType::CARTESIAN_TRAJECTORY); - this->reset(); - this->set_points(points, times); -} - -template -inline bool CartesianTrajectory::add_point( - const CartesianState& new_point, const std::chrono::duration& new_time) { - if (new_point.is_empty()) { - return false; - } - if (this->get_size() > 0) { - if (new_point.get_reference_frame() != reference_frame_) { - return false; - } - } else if (reference_frame_.empty()) { - reference_frame_ = new_point.get_reference_frame(); - } - this->TrajectoryBase::add_point(new_point.data(), new_time); - return true; -} - -template -inline bool CartesianTrajectory::insert_point( - const CartesianState& new_point, const std::chrono::duration& new_time, int pos) { - if (new_point.is_empty()) { - return false; - } - if (this->get_size() > 0) { - if (new_point.get_reference_frame() != reference_frame_) { - return false; - } - } else if (reference_frame_.empty()) { - reference_frame_ = new_point.get_reference_frame(); - } - this->TrajectoryBase::insert_point(new_point.data(), new_time, pos); - return true; -} - -template -inline void CartesianTrajectory::set_point( - const CartesianState& point, const std::chrono::duration& new_time, unsigned int index) { - if (point.is_empty()) { - throw exceptions::EmptyStateException("Point is empty"); - } - if (point.get_reference_frame() != reference_frame_) { - throw exceptions::IncompatibleReferenceFramesException( - "Incompatible reference frames: " + point.get_reference_frame() + " and " + reference_frame_); - } - try { - this->TrajectoryBase::set_point(point.data(), new_time, index); - } catch (...) { - throw; - } -} - -template -inline void CartesianTrajectory::set_points( - const std::vector& points, - const std::vector>& new_times) { - if (points.empty()) { - throw exceptions::IncompatibleSizeException("Points vector is empty"); - } - std::string candidate_reference_frame = points[0].get_reference_frame(); - - std::vector data; - for (const auto& point : points) { - if (point.is_empty()) { - throw exceptions::EmptyStateException("Vector contains at least one point that is empty"); - } else if (point.get_reference_frame() != candidate_reference_frame) { - throw exceptions::IncompatibleReferenceFramesException( - "Incompatible reference frames: " + point.get_reference_frame() + " and " + candidate_reference_frame); - } - data.push_back(point.data()); - } - try { - this->TrajectoryBase::set_points(data, new_times); - reference_frame_ = candidate_reference_frame; - } catch (...) { - throw; - } -} - }// namespace state_representation diff --git a/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp b/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp index 341fb6585..8e2f513cc 100644 --- a/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp +++ b/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp @@ -3,101 +3,99 @@ #include "state_representation/space/joint/JointState.hpp" #include "state_representation/trajectory/TrajectoryBase.hpp" -#include "state_representation/exceptions/EmptyStateException.hpp" -#include "state_representation/exceptions/IncompatibleStatesException.hpp" - namespace state_representation { -class JointTrajectory : public TrajectoryBase { +struct JointTrajectoryPoint : public TrajectoryPoint {}; + +class JointTrajectory : public TrajectoryBase { public: /** * @brief Constructor with name and reference frame provided - * @brief name the name of the state + * @param name the name of the state */ explicit JointTrajectory(const std::string& name = ""); /** * @brief Constructor with name and reference frame provided * @param point the initial point - * @param time the initial time + * @param duration the initial duration * @param name the name of the state */ - template explicit JointTrajectory( - const JointState& point, const std::chrono::duration& time, const std::string& name = ""); + const JointState& point, const std::chrono::nanoseconds& duration, const std::string& name = ""); /** * @brief Constructor with name and reference frame provided * @param points vector of initial points - * @param times vector of initial times + * @param durations vector of initial durations * @param name the name of the state */ - template explicit JointTrajectory( - const std::vector& points, const std::vector>& times, + const std::vector& points, const std::vector& durations, const std::string& name = ""); /** * @brief Getter of the names attribute + * @return vector of joint names associated with the trajectory */ const std::vector& get_joint_names() const; /** - * @brief Add new point and corresponding time to trajectory - * @return Success of the operation + * @brief Add new point and corresponding duration to trajectory + * @param new_point the new trajectory point + * @param duration the duration for the new point */ - template - bool add_point(const JointState& new_point, const std::chrono::duration& new_time); + void add_point(const JointState& new_point, const std::chrono::nanoseconds& duration); /** - * @brief Insert new point and corresponding time to trajectory between two + * @brief Insert new point and corresponding duration to trajectory between two * already existing points - * @return Success of the operation + * @param new_point the new trajectory point + * @param duration the duration for the new point + * @param pos the desired position of the new point in the queue */ - template - bool insert_point(const JointState& new_point, const std::chrono::duration& new_time, int pos); + void insert_point(const JointState& new_point, const std::chrono::nanoseconds& duration, unsigned int pos); /** * @brief Get attribute list of trajectory points + * @return queue of the Joint states of the trajectory */ const std::deque get_points() const; /** * @brief Get the trajectory point at given index * @param index the index + * @return the Joint state that corresponds to the index */ const JointState get_point(unsigned int index) const; /** * @brief Set the trajectory point at given index * @param point the new point - * @param new_time the new time + * @param duration the new duration * @param index the index * @throw std::out_of_range if index is out of range * @throw EmptyStateException if point is empty * @throw IncompatibleReferenceFramesException if point has different reference frame */ - template - void - set_point(const JointState& point, const std::chrono::duration& new_time, unsigned int index); + void set_point(const JointState& point, const std::chrono::nanoseconds& duration, unsigned int index); /** * @brief Set the trajectory point at given index * @param points vector of new points - * @param new_time vector of new times - * @throw IncompatibleSizeException if points and new_times have different sizes + * @param duration vector of new durations + * @throw IncompatibleSizeException if points and durations have different sizes * @throw EmptyStateException if point is empty * @throw IncompatibleReferenceFramesException if point has different reference frame */ - template - void set_points( - const std::vector& points, const std::vector>& new_times); + void set_points(const std::vector& points, const std::vector& durations); /** * @brief Operator overload for returning a single trajectory point and - * corresponding time + * corresponding duration + * @return the Joint state and duration pair that corresponds to the index */ - std::pair operator[](unsigned int idx); + std::pair operator[](unsigned int idx) const; /** * @brief Reset trajectory @@ -105,7 +103,7 @@ class JointTrajectory : public TrajectoryBase { virtual void reset() override; /** - * @brief Delete last point and corresponding time from trajectory + * @brief Delete last point and corresponding duration from trajectory */ virtual void delete_point() override; @@ -118,107 +116,4 @@ class JointTrajectory : public TrajectoryBase { std::vector joint_names_;///< names of the joints std::string robot_name_; ///< name of the robot }; - -template -JointTrajectory::JointTrajectory( - const JointState& point, const std::chrono::duration& time, const std::string& name) - : TrajectoryBase(name) { - this->set_type(StateType::JOINT_TRAJECTORY); - this->reset(); - this->add_point(point, time); -} - -template -JointTrajectory::JointTrajectory( - const std::vector& points, const std::vector>& times, - const std::string& name) - : TrajectoryBase(name) { - this->set_type(StateType::JOINT_TRAJECTORY); - this->reset(); - this->set_points(points, times); -} - -template -inline bool -JointTrajectory::add_point(const JointState& new_point, const std::chrono::duration& new_time) { - if (new_point.is_empty()) { - return false; - } - if (this->get_size() == 0) { - this->joint_names_ = new_point.get_names(); - this->robot_name_ = new_point.get_name(); - } else if (this->joint_names_ != new_point.get_names()) { - return false; - } - this->TrajectoryBase::add_point(new_point.data(), new_time); - return true; -} - -template -inline bool JointTrajectory::insert_point( - const JointState& new_point, const std::chrono::duration& new_time, int pos) { - if (new_point.is_empty()) { - return false; - } - if (this->get_size() == 0) { - this->joint_names_ = new_point.get_names(); - this->robot_name_ = new_point.get_name(); - } else if (this->joint_names_ != new_point.get_names()) { - return false; - } - this->TrajectoryBase::insert_point(new_point.data(), new_time, pos); - return true; -} - -template -inline void JointTrajectory::set_point( - const JointState& point, const std::chrono::duration& new_time, unsigned int index) { - if (point.is_empty()) { - throw exceptions::EmptyStateException("Point is empty"); - } else if (point.get_names() != this->joint_names_) { - throw exceptions::IncompatibleStatesException( - "Incompatible joint names between the new point and current trajectory"); - } else if (point.get_name() != this->robot_name_) { - throw exceptions::IncompatibleStatesException( - "Incompatible robot name between the new point and current trajectory"); - } - try { - this->TrajectoryBase::set_point(point.data(), new_time, index); - } catch (...) { - throw; - } -} - -template -inline void JointTrajectory::set_points( - const std::vector& points, const std::vector>& new_times) { - if (points.empty()) { - throw exceptions::EmptyStateException("Points vector is empty"); - } else if (points[0].is_empty()) { - throw exceptions::EmptyStateException("Vector contains at least one point that is empty"); - } - auto candidate_robot_name = points[0].get_name(); - auto candidate_joint_names = points[0].get_names(); - - std::vector data; - for (const auto& point : points) { - if (point.is_empty()) { - throw exceptions::EmptyStateException("Vector contains at least one point that is empty"); - } else if (point.get_names() != candidate_joint_names) { - throw exceptions::IncompatibleStatesException( - "Incompatible joint names between the new point and current trajectory"); - } else if (point.get_name() != candidate_robot_name) { - throw exceptions::IncompatibleStatesException( - "Incompatible robot name between the new point and current trajectory"); - } - data.push_back(point.data()); - } - try { - this->TrajectoryBase::set_points(data, new_times); - this->joint_names_ = candidate_joint_names; - this->robot_name_ = candidate_robot_name; - } catch (...) { - throw; - } -} }// namespace state_representation diff --git a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp index 4f80275b9..3b2740500 100644 --- a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp +++ b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp @@ -1,35 +1,52 @@ #pragma once #include "state_representation/State.hpp" -#include "state_representation/exceptions/IncompatibleSizeException.hpp" #include #include +#include namespace state_representation { + +struct TrajectoryPoint { + Eigen::VectorXd data; + std::chrono::nanoseconds duration; +}; + template class TrajectoryBase : public State { public: /** * @brief Get attribute list of trajectory points + * @return the list of trajectory points */ const std::deque& get_points() const; /** * @brief Get the trajectory point at given index * @param index the index + * @return the trajectory point + * @throw std::out_of_range if pos is out of range */ const TrajectoryT& get_point(unsigned int index) const; /** - * @brief Get attribute list of trajectory times + * @brief Get attribute list of trajectory point durations + * @return the list of trajectory point durations + */ + const std::deque get_durations() const; + + /** + * @brief Get attribute list of trajectory point times from start + * @return the list of trajectory point times from start */ - const std::deque& get_times() const; + const std::deque get_times_from_start() const; /** * @brief Get attribute number of point in trajectory + * @return the number of points in trajectory */ - int get_size() const; + unsigned int get_size() const; /** * @brief Reset trajectory @@ -54,65 +71,56 @@ class TrajectoryBase : public State { /** * @brief Constructor with name provided - * @brief name the name of the state + * @param name the name of the state */ explicit TrajectoryBase(const std::string& name); /** - * @brief Add new point and corresponding time to trajectory + * @brief Add new point to trajectory + * @param new_point the new point */ - template - void add_point(const TrajectoryT& new_point, const std::chrono::duration& new_time); + void add_point(const TrajectoryT& new_point); /** - * @brief Insert new point and corresponding time to trajectory between two - * already existing points + * @brief Insert new trajectory point between two already existing points + * @param new_point the new point + * @param pos the desired position of the new point in the queue + * @throw std::out_of_range if pos is out of range */ - template - void insert_point(const TrajectoryT& new_point, const std::chrono::duration& new_time, int pos); + void insert_point(const TrajectoryT& new_point, unsigned int pos); /** * @brief Get the trajectory point at given index * @param index the index + * @return the trajectory point */ TrajectoryT& get_point(unsigned int index); /** * @brief Set the trajectory point at given index * @param point the new point - * @param new_time the new time * @param index the index * @throw std::out_of_range if index is out of range */ - template - void - set_point(const TrajectoryT& point, const std::chrono::duration& new_time, unsigned int index); + void set_point(const TrajectoryT& point, unsigned int index); /** * @brief Set the trajectory points from a vector of points * @param points vector of new points - * @param new_time vector of new times - * @throw IncompatibleSizeException if points and new_times have different sizes - */ - template - void set_points( - const std::vector& points, const std::vector>& new_times); - - /** - * @brief Operator overload for returning a single trajectory point and - * corresponding time */ - const std::pair operator[](unsigned int idx) const; + void set_points(const std::vector& points); /** * @brief Operator overload for returning a single trajectory point and * corresponding time + * @param idx the index + * @return the trajectory point + * @throw std::out_of_range if index is out of range */ - std::pair operator[](unsigned int idx); + const TrajectoryT operator[](unsigned int idx) const; private: std::deque points_; - std::deque times_; }; template @@ -129,43 +137,23 @@ template inline void TrajectoryBase::reset() { this->State::reset(); this->points_.clear(); - this->times_.clear(); } template -template -inline void TrajectoryBase::add_point( - const TrajectoryT& new_point, const std::chrono::duration& new_time) { +inline void TrajectoryBase::add_point(const TrajectoryT& new_point) { this->set_empty(false); this->points_.push_back(new_point); - - if (!this->times_.empty()) { - auto const previous_time = this->times_.back(); - this->times_.push_back(previous_time + new_time); - } else { - this->times_.push_back(new_time); - } } template -template -inline void TrajectoryBase::insert_point( - const TrajectoryT& new_point, const std::chrono::duration& new_time, int pos) { +inline void TrajectoryBase::insert_point(const TrajectoryT& new_point, unsigned int pos) { + if (pos > this->points_.size()) { + throw std::out_of_range("Index out of range"); + } this->set_empty(false); - auto it_points = this->points_.begin(); - auto it_times = this->times_.begin(); std::advance(it_points, pos); - std::advance(it_times, pos); - this->points_.insert(it_points, new_point); - - auto previous_time = this->times_[pos - 1]; - this->times_.insert(it_times, previous_time + new_time); - - for (unsigned int i = pos + 1; i <= this->points_.size(); i++) { - this->times_[i] += new_time; - } } template @@ -174,15 +162,11 @@ inline void TrajectoryBase::delete_point() { if (!this->points_.empty()) { this->points_.pop_back(); } - if (!this->times_.empty()) { - this->times_.pop_back(); - } } template inline void TrajectoryBase::clear() { this->points_.clear(); - this->times_.clear(); } template @@ -201,57 +185,56 @@ inline TrajectoryT& TrajectoryBase::get_point(unsigned int index) { } template -template -inline void TrajectoryBase::set_point( - const TrajectoryT& point, const std::chrono::duration& new_time, unsigned int index) { - if (index < this->points_.size()) { - this->points_[index] = point; - if (index == 0) { - this->times_[index] = new_time; - } else { - this->times_[index] = this->times_[index - 1] + new_time; - } - for (unsigned int i = index + 1; i < this->points_.size(); ++i) { - this->times_[i] += this->times_[index - 1]; - } - } else { +inline void TrajectoryBase::set_point(const TrajectoryT& point, unsigned int index) { + if (index >= this->points_.size()) { throw std::out_of_range("Index out of range"); } + this->points_[index] = point; } template -template -inline void TrajectoryBase::set_points( - const std::vector& points, const std::vector>& new_times) { - if (points.size() != new_times.size()) { - throw exceptions::IncompatibleSizeException( - "The point and time vectors provided have different sizes " + std::to_string(points.size()) + " and " - + std::to_string(new_times.size())); +inline void TrajectoryBase::set_points(const std::vector& points) { + if (points.size() == 0) { + return; } this->clear(); for (unsigned int i = 0; i < points.size(); ++i) { - this->add_point(points[i], new_times[i]); + this->add_point(points[i]); } } template -inline const std::deque& TrajectoryBase::get_times() const { - return this->times_; +inline const std::deque TrajectoryBase::get_durations() const { + std::deque durations; + for (unsigned int i = 0; i < this->points_.size(); ++i) { + durations.push_back(this->points_[i].duration); + } + return durations; } template -inline int TrajectoryBase::get_size() const { - return this->points_.size(); +inline const std::deque TrajectoryBase::get_times_from_start() const { + std::deque times_from_start; + for (unsigned int i = 0; i < this->points_.size(); ++i) { + std::chrono::nanoseconds time_from_start = std::chrono::nanoseconds(0); + for (unsigned int j = 0; j <= i; ++j) { + time_from_start += this->points_[j].duration; + } + times_from_start.push_back(time_from_start); + } + return times_from_start; } template -inline const std::pair -TrajectoryBase::operator[](unsigned int idx) const { - return std::make_pair(this->points_[idx], this->times_[idx]); +inline unsigned int TrajectoryBase::get_size() const { + return this->points_.size(); } template -inline std::pair TrajectoryBase::operator[](unsigned int idx) { - return std::make_pair(this->points_[idx], this->times_[idx]); +inline const TrajectoryT TrajectoryBase::operator[](unsigned int idx) const { + if (idx >= this->points_.size()) { + throw std::out_of_range("Index out of range"); + } + return this->points_[idx]; } }// namespace state_representation diff --git a/source/state_representation/src/trajectory/CartesianTrajectory.cpp b/source/state_representation/src/trajectory/CartesianTrajectory.cpp index 04bafa1ab..f99c13ab1 100644 --- a/source/state_representation/src/trajectory/CartesianTrajectory.cpp +++ b/source/state_representation/src/trajectory/CartesianTrajectory.cpp @@ -1,11 +1,15 @@ #include "state_representation/trajectory/CartesianTrajectory.hpp" #include "state_representation/space/cartesian/CartesianState.hpp" +#include "state_representation/exceptions/EmptyStateException.hpp" +#include "state_representation/exceptions/IncompatibleReferenceFramesException.hpp" +#include "state_representation/exceptions/IncompatibleSizeException.hpp" + #include namespace state_representation { CartesianTrajectory::CartesianTrajectory(const std::string& name, const std::string& reference_frame) - : TrajectoryBase(name), reference_frame_(reference_frame) { + : TrajectoryBase(name), reference_frame_(reference_frame) { this->set_type(StateType::CARTESIAN_TRAJECTORY); this->reset(); } @@ -14,44 +18,173 @@ const std::string CartesianTrajectory::get_reference_frame() const { return this->reference_frame_; } +CartesianTrajectory::CartesianTrajectory( + const CartesianState& point, const std::chrono::nanoseconds& duration, const std::string& name, + const std::string& reference_frame) + : TrajectoryBase(name), reference_frame_(reference_frame) { + this->set_type(StateType::CARTESIAN_TRAJECTORY); + this->reset(); + CartesianTrajectoryPoint trajectory_point; + trajectory_point.data = point.data(); + trajectory_point.duration = duration; + trajectory_point.name = point.get_name(); + this->TrajectoryBase::add_point(trajectory_point); +} + +CartesianTrajectory::CartesianTrajectory( + const std::vector& points, const std::vector& durations, + const std::string& name, const std::string& reference_frame) + : TrajectoryBase(name), reference_frame_(reference_frame) { + this->set_type(StateType::CARTESIAN_TRAJECTORY); + this->reset(); + try { + this->set_points(points, durations); + } catch (...) { + throw; + } +} + +void CartesianTrajectory::add_point(const CartesianState& new_point, const std::chrono::nanoseconds& duration) { + if (new_point.is_empty()) { + throw exceptions::EmptyStateException("The Cartesian state provided is empty"); + } + if (this->get_size() > 0) { + if (new_point.get_reference_frame() != reference_frame_) { + throw exceptions::IncompatibleReferenceFramesException( + "Incompatible reference frames: " + new_point.get_reference_frame() + " and " + reference_frame_); + } + } else { + reference_frame_ = new_point.get_reference_frame(); + } + CartesianTrajectoryPoint trajectory_point; + trajectory_point.data = new_point.data(); + trajectory_point.duration = duration; + trajectory_point.name = new_point.get_name(); + this->TrajectoryBase::add_point(trajectory_point); +} + +void CartesianTrajectory::insert_point( + const CartesianState& new_point, const std::chrono::nanoseconds& duration, unsigned int pos) { + if (new_point.is_empty()) { + throw exceptions::EmptyStateException("Point is empty"); + } + if (this->get_size() > 0) { + if (new_point.get_reference_frame() != reference_frame_) { + throw exceptions::IncompatibleReferenceFramesException( + "Incompatible reference frames: " + new_point.get_reference_frame() + " and " + reference_frame_); + } + } else if (reference_frame_.empty()) { + reference_frame_ = new_point.get_reference_frame(); + } + CartesianTrajectoryPoint trajectory_point; + trajectory_point.data = new_point.data(); + trajectory_point.duration = duration; + trajectory_point.name = new_point.get_name(); + try { + this->TrajectoryBase::insert_point(trajectory_point, pos); + } catch (...) { + throw; + } +} + +void CartesianTrajectory::set_point( + const CartesianState& point, const std::chrono::nanoseconds& duration, unsigned int index) { + if (point.is_empty()) { + throw exceptions::EmptyStateException("Point is empty"); + } + if (point.get_reference_frame() != reference_frame_) { + throw exceptions::IncompatibleReferenceFramesException( + "Incompatible reference frames: " + point.get_reference_frame() + " and " + reference_frame_); + } + CartesianTrajectoryPoint trajectory_point; + trajectory_point.data = point.data(); + trajectory_point.duration = duration; + trajectory_point.name = point.get_name(); + try { + this->TrajectoryBase::set_point(trajectory_point, index); + } catch (...) { + throw; + } +} + +void CartesianTrajectory::set_points( + const std::vector& points, const std::vector& durations) { + if (points.size() != durations.size()) { + throw exceptions::IncompatibleSizeException("The size of the points and durations vectors are not equal"); + } + if (points.empty()) { + this->reset(); + return; + } + std::string candidate_reference_frame = points[0].get_reference_frame(); + + std::vector trajectory_points; + for (unsigned int i = 0; i < points.size(); ++i) { + if (points[i].is_empty()) { + throw exceptions::EmptyStateException("Vector contains at least one point that is empty"); + } else if (points[i].get_reference_frame() != candidate_reference_frame) { + throw exceptions::IncompatibleReferenceFramesException( + "Incompatible reference frames: " + points[i].get_reference_frame() + " and " + candidate_reference_frame); + } + CartesianTrajectoryPoint trajectory_point; + trajectory_point.data = points[i].data(); + trajectory_point.duration = durations[i]; + trajectory_point.name = points[i].get_name(); + trajectory_points.push_back(trajectory_point); + } + try { + this->TrajectoryBase::set_points(trajectory_points); + reference_frame_ = candidate_reference_frame; + } catch (...) { + throw; + } +} + const std::deque CartesianTrajectory::get_points() const { std::deque points; - for (const auto& point : this->TrajectoryBase::get_points()) { - CartesianState state; - state.set_data(point); - points.push_back(state); + for (unsigned int i = 0; i < this->get_size(); ++i) { + auto state = this->operator[](i); + points.push_back(state.first); } return points; } CartesianState CartesianTrajectory::get_point(unsigned int index) const { - auto point = this->TrajectoryBase::get_point(index); - CartesianState state; - state.set_data(point); - return state; + try { + return this->operator[](index).first; + } catch (...) { + throw; + } } -std::pair CartesianTrajectory::operator[](unsigned int idx) { - auto pair = this->TrajectoryBase::operator[](idx); - CartesianState state; - state.set_data(pair.first); - return std::make_pair(state, pair.second); +std::pair CartesianTrajectory::operator[](unsigned int idx) const { + try { + auto point = this->TrajectoryBase::operator[](idx); + CartesianState state; + state.set_data(point.data); + state.set_name(point.name); + state.set_reference_frame(this->reference_frame_); + auto duration = point.duration; + return std::make_pair(state, duration); + } catch (...) { + throw; + } } void CartesianTrajectory::reset() { - this->TrajectoryBase::reset(); + this->TrajectoryBase::reset(); this->reference_frame_ = "world"; } void CartesianTrajectory::delete_point() { - this->TrajectoryBase::delete_point(); + this->TrajectoryBase::delete_point(); if (this->get_size() == 0) { - this->reference_frame_ = ""; + this->clear(); } } void CartesianTrajectory::clear() { - this->TrajectoryBase::clear(); + this->TrajectoryBase::clear(); this->reference_frame_ = ""; } }// namespace state_representation diff --git a/source/state_representation/src/trajectory/JointTrajectory.cpp b/source/state_representation/src/trajectory/JointTrajectory.cpp index 2c743bd33..ac662f6c3 100644 --- a/source/state_representation/src/trajectory/JointTrajectory.cpp +++ b/source/state_representation/src/trajectory/JointTrajectory.cpp @@ -1,11 +1,133 @@ #include "state_representation/trajectory/JointTrajectory.hpp" #include "state_representation/space/joint/JointState.hpp" +#include "state_representation/exceptions/EmptyStateException.hpp" +#include "state_representation/exceptions/IncompatibleStatesException.hpp" +#include + namespace state_representation { -JointTrajectory::JointTrajectory(const std::string& name) : TrajectoryBase(name) { +JointTrajectory::JointTrajectory(const std::string& name) : TrajectoryBase(name) { + this->set_type(StateType::JOINT_TRAJECTORY); + this->reset(); +} + +JointTrajectory::JointTrajectory( + const JointState& point, const std::chrono::nanoseconds& duration, const std::string& name) + : TrajectoryBase(name) { + this->set_type(StateType::JOINT_TRAJECTORY); + this->reset(); + this->add_point(point, duration); +} + +JointTrajectory::JointTrajectory( + const std::vector& points, const std::vector& durations, + const std::string& name) + : TrajectoryBase(name) { this->set_type(StateType::JOINT_TRAJECTORY); this->reset(); + this->set_points(points, durations); +} + +void JointTrajectory::add_point(const JointState& new_point, const std::chrono::nanoseconds& duration) { + if (new_point.is_empty()) { + throw exceptions::EmptyStateException("Point is empty"); + } + if (this->get_size() == 0) { + this->joint_names_ = new_point.get_names(); + this->robot_name_ = new_point.get_name(); + } else if (this->joint_names_ != new_point.get_names()) { + throw exceptions::IncompatibleStatesException( + "Incompatible joint names between the new point and current trajectory"); + } else if (this->robot_name_ != new_point.get_name()) { + throw exceptions::IncompatibleStatesException( + "Incompatible robot name between the new point and current trajectory"); + } + JointTrajectoryPoint trajectory_point; + trajectory_point.data = new_point.data(); + trajectory_point.duration = duration; + this->TrajectoryBase::add_point(trajectory_point); +} + +void JointTrajectory::insert_point( + const JointState& new_point, const std::chrono::nanoseconds& duration, unsigned int pos) { + if (new_point.is_empty()) { + throw exceptions::EmptyStateException("Point is empty"); + } + if (this->get_size() == 0) { + this->joint_names_ = new_point.get_names(); + this->robot_name_ = new_point.get_name(); + } else if (this->joint_names_ != new_point.get_names()) { + throw exceptions::IncompatibleStatesException( + "Incompatible joint names between the new point and current trajectory"); + } else if (this->robot_name_ != new_point.get_name()) { + throw exceptions::IncompatibleStatesException( + "Incompatible robot name between the new point and current trajectory"); + } + try { + JointTrajectoryPoint trajectory_point; + trajectory_point.data = new_point.data(); + trajectory_point.duration = duration; + this->TrajectoryBase::insert_point(trajectory_point, pos); + } catch (...) { + throw; + } +} + +void JointTrajectory::set_point(const JointState& point, const std::chrono::nanoseconds& duration, unsigned int index) { + if (point.is_empty()) { + throw exceptions::EmptyStateException("Point is empty"); + } else if (point.get_names() != this->joint_names_) { + throw exceptions::IncompatibleStatesException( + "Incompatible joint names between the new point and current trajectory"); + } else if (point.get_name() != this->robot_name_) { + throw exceptions::IncompatibleStatesException( + "Incompatible robot name between the new point and current trajectory"); + } + JointTrajectoryPoint trajectory_point; + trajectory_point.data = point.data(); + trajectory_point.duration = duration; + try { + this->TrajectoryBase::set_point(trajectory_point, index); + } catch (...) { + throw; + } +} + +void JointTrajectory::set_points( + const std::vector& points, const std::vector& durations) { + if (points.size() != durations.size()) { + throw exceptions::IncompatibleSizeException("The size of the points and durations vectors are not equal"); + } + if (points.empty()) { + throw exceptions::EmptyStateException("Points vector is empty"); + } + auto candidate_robot_name = points[0].get_name(); + auto candidate_joint_names = points[0].get_names(); + + std::vector trajectory_points; + for (unsigned int i = 0; i < points.size(); ++i) { + if (points[i].is_empty()) { + throw exceptions::EmptyStateException("Vector contains at least one point that is empty"); + } else if (points[i].get_names() != candidate_joint_names) { + throw exceptions::IncompatibleStatesException( + "Incompatible joint names between the new point and current trajectory"); + } else if (points[i].get_name() != candidate_robot_name) { + throw exceptions::IncompatibleStatesException( + "Incompatible robot name between the new point and current trajectory"); + } + JointTrajectoryPoint trajectory_point; + trajectory_point.data = points[i].data(); + trajectory_point.duration = durations[i]; + trajectory_points.push_back(trajectory_point); + } + try { + this->TrajectoryBase::set_points(trajectory_points); + this->joint_names_ = candidate_joint_names; + this->robot_name_ = candidate_robot_name; + } catch (...) { + throw; + } } const std::vector& JointTrajectory::get_joint_names() const { @@ -14,43 +136,49 @@ const std::vector& JointTrajectory::get_joint_names() const { const std::deque JointTrajectory::get_points() const { std::deque points; - for (const auto& point : this->TrajectoryBase::get_points()) { - JointState state(this->robot_name_, point.size() / 4); - state.set_data(point); - points.push_back(state); + for (unsigned int i = 0; i < this->get_size(); ++i) { + auto state = this->operator[](i); + points.push_back(state.first); } return points; } const JointState JointTrajectory::get_point(unsigned int index) const { - auto point = this->TrajectoryBase::get_point(index); - JointState state(this->robot_name_, point.size() / 4); - state.set_data(point); - return state; + try { + return this->operator[](index).first; + } catch (...) { + throw; + } } -std::pair JointTrajectory::operator[](unsigned int idx) { - auto pair = this->TrajectoryBase::operator[](idx); - JointState state(this->robot_name_, pair.first.size() / 4); - state.set_data(pair.first); - return std::make_pair(state, pair.second); +std::pair JointTrajectory::operator[](unsigned int idx) const { + try { + auto point = this->TrajectoryBase::operator[](idx); + JointState state(this->robot_name_, this->joint_names_); + state.set_data(point.data); + auto duration = point.duration; + return std::make_pair(state, duration); + } catch (...) { + throw; + } } void JointTrajectory::reset() { - this->TrajectoryBase::reset(); + this->TrajectoryBase::reset(); this->robot_name_ = ""; this->joint_names_.clear(); } void JointTrajectory::delete_point() { - this->TrajectoryBase::delete_point(); + this->TrajectoryBase::delete_point(); if (this->get_size() == 0) { - this->joint_names_.clear(); + this->clear(); } } void JointTrajectory::clear() { - this->TrajectoryBase::clear(); + this->TrajectoryBase::clear(); this->joint_names_.clear(); + this->robot_name_ = ""; } }// namespace state_representation diff --git a/source/state_representation/test/tests/test_trajectory.cpp b/source/state_representation/test/tests/test_trajectory.cpp index 7b3f2aa7c..5bc3c2304 100644 --- a/source/state_representation/test/tests/test_trajectory.cpp +++ b/source/state_representation/test/tests/test_trajectory.cpp @@ -1,248 +1,233 @@ -#include "state_representation/space/cartesian/CartesianState.hpp" +#include "state_representation/exceptions/IncompatibleStatesException.hpp" #include "state_representation/space/joint/JointState.hpp" #include "state_representation/trajectory/CartesianTrajectory.hpp" #include "state_representation/trajectory/JointTrajectory.hpp" +#include "state_representation/exceptions/EmptyStateException.hpp" +#include "state_representation/exceptions/IncompatibleReferenceFramesException.hpp" +#include "state_representation/exceptions/IncompatibleSizeException.hpp" +#include "state_representation/space/cartesian/CartesianState.hpp" + +#include #include #include +#include #include +#include + +using namespace state_representation; + +class TrajectoryBaseInterface : public TrajectoryBase { +public: + using TrajectoryBase::TrajectoryBase; + using TrajectoryBase::add_point; + using TrajectoryBase::get_point; + using TrajectoryBase::insert_point; + using TrajectoryBase::operator[]; +}; + +TEST(TrajectoryTest, TestTrajectoryBase) { + TrajectoryBaseInterface trajectory; + EXPECT_EQ(trajectory.get_size(), 0); + + TrajectoryPoint point0; + TrajectoryPoint point1; + TrajectoryPoint point2; + point0.data = Eigen::VectorXd::Random(3); + point0.duration = std::chrono::nanoseconds(100); + point1.data = Eigen::VectorXd::Random(3); + point1.duration = std::chrono::nanoseconds(200); + point2.data = Eigen::VectorXd::Random(3); + point2.duration = std::chrono::nanoseconds(300); + + trajectory.add_point(point0); + trajectory.add_point(point1); + EXPECT_EQ(trajectory.get_size(), 2); + EXPECT_EQ(trajectory.get_durations().size(), 2); + EXPECT_EQ(trajectory.get_times_from_start().size(), 2); + EXPECT_EQ(trajectory.get_point(0).data, point0.data); + EXPECT_EQ(trajectory.get_point(1).data, point1.data); + EXPECT_EQ(trajectory.get_times_from_start()[1], point0.duration + point1.duration); + + auto points = trajectory.get_points(); + EXPECT_EQ(points.size(), 2); + EXPECT_EQ(points[0].data, point0.data); + EXPECT_EQ(points[1].data, point1.data); + + EXPECT_THROW(trajectory.insert_point(point2, 10), std::out_of_range); + EXPECT_NO_THROW(trajectory.insert_point(point2, 1)); + EXPECT_EQ(trajectory.get_size(), 3); + EXPECT_EQ(trajectory[1].data, point2.data); + EXPECT_EQ(trajectory.get_times_from_start()[1], point0.duration + point2.duration); + EXPECT_EQ(trajectory.get_times_from_start()[2], point0.duration + point2.duration + point1.duration); + + trajectory.clear(); + EXPECT_EQ(trajectory.get_size(), 0); + trajectory.reset(); + EXPECT_TRUE(trajectory.is_empty()); +} -TEST(TrajectoryTest, AddPoint) { +TEST(TrajectoryTest, CartesianTrajectory) { { - state_representation::CartesianTrajectory trajectory("world"); - EXPECT_STREQ(trajectory.get_reference_frame().c_str(), "world"); - - auto point1 = state_representation::CartesianState::Random("point1"); - EXPECT_TRUE(trajectory.add_point(point1, std::chrono::nanoseconds(100))); - - auto point2 = state_representation::CartesianState::Random("point2", "foo_reference_frame"); - EXPECT_FALSE(trajectory.add_point(point2, std::chrono::nanoseconds(200))); - - auto point3 = state_representation::CartesianState::Random("point3", "world"); - EXPECT_TRUE(trajectory.add_point(point3, std::chrono::nanoseconds(300))); + CartesianTrajectory trajectory(CartesianState::Random("foo"), std::chrono::nanoseconds(100)); + EXPECT_EQ(trajectory.get_size(), 1); } { - state_representation::JointTrajectory trajectory("joint_trajectory"); + auto points = { + CartesianState::Random("foo"), + CartesianState::Random("bar"), + }; + auto durations = {std::chrono::nanoseconds(100), std::chrono::nanoseconds(200)}; + CartesianTrajectory trajectory(points, durations); + EXPECT_EQ(trajectory.get_size(), 2); + } - auto point1 = state_representation::JointState::Random("foo", 25); - EXPECT_TRUE(trajectory.add_point(point1, std::chrono::nanoseconds(100))); + CartesianTrajectory trajectory; + EXPECT_EQ(trajectory.get_size(), 0); - auto point2 = state_representation::JointState::Random("foo", 1); - EXPECT_FALSE(trajectory.add_point(point2, std::chrono::nanoseconds(200))); + {// incompatible sizes + CartesianState state; + EXPECT_THROW(trajectory.add_point(state, std::chrono::nanoseconds(100)), exceptions::EmptyStateException); + EXPECT_EQ(trajectory.get_size(), 0); - auto point3 = state_representation::JointState::Random("foo", 25); - EXPECT_TRUE(trajectory.add_point(point3, std::chrono::nanoseconds(300))); + auto points = { + CartesianState::Random("foo"), + CartesianState::Random("bar"), + }; + auto durations = {std::chrono::nanoseconds(100)}; + EXPECT_THROW(trajectory.set_points(points, durations), exceptions::IncompatibleSizeException); + EXPECT_EQ(trajectory.get_size(), 0); } -} -TEST(TrajectoryTest, ClearPoint) { - { - state_representation::CartesianTrajectory trajectory("world"); - auto point1 = state_representation::CartesianState::Random("point1"); - auto point2 = state_representation::CartesianState::Random("point2"); - auto point3 = state_representation::CartesianState::Random("point3"); - trajectory.add_point(point1, std::chrono::nanoseconds(100)); - trajectory.add_point(point2, std::chrono::nanoseconds(300)); - trajectory.add_point(point3, std::chrono::nanoseconds(300)); - EXPECT_EQ(trajectory.get_size(), 3); - EXPECT_EQ(trajectory.get_times().size(), 3); - trajectory.delete_point(); - EXPECT_EQ(trajectory.get_size(), 2); - EXPECT_EQ(trajectory.get_times().size(), 2); + {// incompatible reference frames + CartesianState point0("empty", "world1"); + auto point1 = CartesianState::Random("foo", "world2"); + auto point2 = CartesianState::Random("bar", "world3"); + EXPECT_THROW(trajectory.add_point(point0, std::chrono::nanoseconds(100)), exceptions::EmptyStateException); + EXPECT_NO_THROW(trajectory.add_point(point1, std::chrono::nanoseconds(200))); + EXPECT_THROW( + trajectory.add_point(point2, std::chrono::nanoseconds(200)), exceptions::IncompatibleReferenceFramesException); + EXPECT_STREQ(trajectory.get_reference_frame().c_str(), "world2"); trajectory.clear(); - EXPECT_EQ(trajectory.get_size(), 0); - EXPECT_EQ(trajectory.get_times().size(), 0); + EXPECT_STREQ(trajectory.get_reference_frame().c_str(), ""); } - { - state_representation::JointTrajectory trajectory; - auto point1 = state_representation::JointState::Random("point1", 25); - auto point2 = state_representation::JointState::Random("point2", 25); - auto point3 = state_representation::JointState::Random("point3", 25); - trajectory.add_point(point1, std::chrono::nanoseconds(100)); - trajectory.add_point(point2, std::chrono::nanoseconds(300)); - trajectory.add_point(point3, std::chrono::nanoseconds(300)); - EXPECT_EQ(trajectory.get_size(), 3); - EXPECT_EQ(trajectory.get_times().size(), 3); - EXPECT_EQ(trajectory.get_joint_names().size(), 25); - trajectory.delete_point(); - EXPECT_EQ(trajectory.get_size(), 2); - EXPECT_EQ(trajectory.get_times().size(), 2); - EXPECT_EQ(trajectory.get_joint_names().size(), 25); - trajectory.clear(); - EXPECT_EQ(trajectory.get_size(), 0); - EXPECT_EQ(trajectory.get_times().size(), 0); - EXPECT_EQ(trajectory.get_joint_names().size(), 0); + auto point0 = CartesianState::Random("foo"); + auto point1 = CartesianState::Random("bar"); + auto point2 = CartesianState::Random("baz"); + + trajectory.add_point(point0, std::chrono::nanoseconds(100)); + EXPECT_EQ(trajectory[0].first.data(), point0.data()); + EXPECT_EQ(trajectory[0].first.get_name(), point0.get_name()); + EXPECT_EQ(trajectory[0].first.get_reference_frame(), point0.get_reference_frame()); + trajectory.add_point(point2, std::chrono::nanoseconds(200)); + + EXPECT_NO_THROW(trajectory.insert_point(point2, std::chrono::nanoseconds(300), 1)); + EXPECT_EQ(trajectory.get_size(), 3); + + point0.set_name("foofoo"); + EXPECT_NO_THROW(trajectory.set_point(point0, std::chrono::nanoseconds(50), 0)); + EXPECT_EQ(trajectory[0].first.get_name(), point0.get_name()); + + std::vector points = {point0, point1, point2}; + std::vector durations = { + std::chrono::nanoseconds(10), std::chrono::nanoseconds(20), std::chrono::nanoseconds(30)}; + trajectory.set_points(points, durations); + for (unsigned int i = 0; i < trajectory.get_size(); ++i) { + EXPECT_EQ(trajectory[i].first.data(), points[i].data()); + EXPECT_EQ(trajectory[i].first.get_name(), points[i].get_name()); + EXPECT_EQ(trajectory[i].first.get_reference_frame(), points[i].get_reference_frame()); + EXPECT_EQ(trajectory[i].second, durations[i]); } } -TEST(TrajectoryTest, OverloadIndex) { +TEST(TrajectoryTest, JointTrajectory) { { - state_representation::CartesianTrajectory trajectory; - auto point1 = state_representation::CartesianState::Random("point1"); - auto point2 = state_representation::CartesianState::Random("point2"); - trajectory.add_point(point1, std::chrono::nanoseconds(300)); - trajectory.add_point(point2, std::chrono::nanoseconds(500)); - EXPECT_TRUE(trajectory[0].second == std::chrono::nanoseconds(300)); - EXPECT_TRUE(trajectory[0].first.data() == point1.data()); - EXPECT_TRUE(trajectory[1].second == std::chrono::nanoseconds(800)); - EXPECT_TRUE(trajectory[1].first.data() == point2.data()); + JointTrajectory trajectory(JointState::Random("foo", 25), std::chrono::nanoseconds(100)); + EXPECT_EQ(trajectory.get_size(), 1); } { - state_representation::JointTrajectory trajectory; - auto point1 = state_representation::JointState::Random("foo", 25); - auto point2 = state_representation::JointState::Random("foo", 25); - trajectory.add_point(point1, std::chrono::nanoseconds(300)); - trajectory.add_point(point2, std::chrono::nanoseconds(500)); - EXPECT_TRUE(trajectory[0].second == std::chrono::nanoseconds(300)); - EXPECT_TRUE(trajectory[0].first.data() == point1.data()); - EXPECT_TRUE(trajectory[1].second == std::chrono::nanoseconds(800)); - EXPECT_TRUE(trajectory[1].first.data() == point2.data()); + auto points = { + JointState::Random("foo", 25), + JointState::Random("foo", 25), + }; + auto durations = {std::chrono::nanoseconds(100), std::chrono::nanoseconds(200)}; + JointTrajectory trajectory(points, durations); + EXPECT_EQ(trajectory.get_size(), 2); } -} -TEST(TrajectoryTest, InsertPoint) { - { - state_representation::CartesianTrajectory trajectory; - auto point1 = state_representation::CartesianState::Random("point1"); - auto point2 = state_representation::CartesianState::Random("point2"); - auto point3 = state_representation::CartesianState::Random("point3"); - trajectory.add_point(point1, std::chrono::nanoseconds(300)); - trajectory.add_point(point2, std::chrono::nanoseconds(500)); - - trajectory.insert_point(point3, std::chrono::nanoseconds(100), 1); - EXPECT_EQ(trajectory.get_size(), 3); - EXPECT_TRUE(trajectory[1].second == std::chrono::nanoseconds(400)); - EXPECT_TRUE(trajectory[1].first.data() == point3.data()); - EXPECT_TRUE(trajectory[0].second == std::chrono::nanoseconds(300)); - EXPECT_TRUE(trajectory[0].first.data() == point1.data()); - EXPECT_TRUE(trajectory[2].second == std::chrono::nanoseconds(900)); - EXPECT_TRUE(trajectory[2].first.data() == point2.data()); - } + JointTrajectory trajectory("foo"); + EXPECT_EQ(trajectory.get_size(), 0); - { - state_representation::JointTrajectory trajectory; - auto point1 = state_representation::JointState::Random("point1", 25); - auto point2 = state_representation::JointState::Random("point2", 25); - auto point3 = state_representation::JointState::Random("point3", 25); - trajectory.add_point(point1, std::chrono::nanoseconds(300)); - trajectory.add_point(point2, std::chrono::nanoseconds(500)); - - trajectory.insert_point(point3, std::chrono::nanoseconds(100), 1); - EXPECT_EQ(trajectory.get_size(), 3); - EXPECT_TRUE(trajectory[1].second == std::chrono::nanoseconds(400)); - EXPECT_TRUE(trajectory[1].first.data() == point3.data()); - EXPECT_TRUE(trajectory[0].second == std::chrono::nanoseconds(300)); - EXPECT_TRUE(trajectory[0].first.data() == point1.data()); - EXPECT_TRUE(trajectory[2].second == std::chrono::nanoseconds(900)); - EXPECT_TRUE(trajectory[2].first.data() == point2.data()); + {// incompatible name + auto point0 = JointState::Random("foo", 25); + auto point1 = JointState::Random("bar", 25); + EXPECT_NO_THROW(trajectory.add_point(point0, std::chrono::nanoseconds(100))); + EXPECT_THROW(trajectory.add_point(point1, std::chrono::nanoseconds(200)), exceptions::IncompatibleStatesException); + trajectory.clear(); } -} -TEST(TrajectoryTest, GetPoints) { - { - state_representation::CartesianTrajectory trajectory; - auto point1 = state_representation::CartesianState::Random("point1"); - auto point2 = state_representation::CartesianState::Random("point2"); - auto point3 = state_representation::CartesianState::Random("point3"); - trajectory.add_point(point1, std::chrono::nanoseconds(300)); - trajectory.add_point(point2, std::chrono::nanoseconds(500)); - trajectory.add_point(point3, std::chrono::nanoseconds(100)); - - auto points = trajectory.get_points(); - EXPECT_EQ(points.size(), 3); - EXPECT_TRUE(points[0].data() == point1.data()); - EXPECT_TRUE(points[1].data() == point2.data()); - EXPECT_TRUE(points[2].data() == point3.data()); - - EXPECT_TRUE(trajectory.get_point(0).data() == point1.data()); - EXPECT_TRUE(trajectory.get_point(1).data() == point2.data()); - EXPECT_TRUE(trajectory.get_point(2).data() == point3.data()); - } + {// incompatible sizes + JointState state; + EXPECT_THROW(trajectory.add_point(state, std::chrono::nanoseconds(100)), exceptions::EmptyStateException); + EXPECT_EQ(trajectory.get_size(), 0); - { - state_representation::JointTrajectory trajectory; - auto point1 = state_representation::JointState::Random("point1", 25); - auto point2 = state_representation::JointState::Random("point2", 25); - auto point3 = state_representation::JointState::Random("point3", 25); - trajectory.add_point(point1, std::chrono::nanoseconds(300)); - trajectory.add_point(point2, std::chrono::nanoseconds(500)); - trajectory.add_point(point3, std::chrono::nanoseconds(100)); - - auto points = trajectory.get_points(); - EXPECT_EQ(points.size(), 3); - EXPECT_TRUE(points[0].data() == point1.data()); - EXPECT_TRUE(points[1].data() == point2.data()); - EXPECT_TRUE(points[2].data() == point3.data()); - - EXPECT_TRUE(trajectory.get_point(0).data() == point1.data()); - EXPECT_TRUE(trajectory.get_point(1).data() == point2.data()); - EXPECT_TRUE(trajectory.get_point(2).data() == point3.data()); + auto points = { + JointState::Random("foo", 25), + JointState::Random("foo", 25), + }; + auto durations = {std::chrono::nanoseconds(100)}; + EXPECT_THROW(trajectory.set_points(points, durations), exceptions::IncompatibleSizeException); + EXPECT_EQ(trajectory.get_size(), 0); } -} -TEST(TrajectoryTest, SetPoints) { - { - state_representation::CartesianTrajectory trajectory; - auto point1 = state_representation::CartesianState::Random("point1"); - auto point2 = state_representation::CartesianState::Random("point2"); - auto point3 = state_representation::CartesianState::Random("point3"); - - auto replacement1 = state_representation::CartesianState::Random("replacement1"); - auto replacement2 = state_representation::CartesianState::Random("replacement2"); - std::vector replacement_points = {replacement1, replacement2}; - std::vector replacement_times = { - std::chrono::nanoseconds(100), std::chrono::nanoseconds(200)}; - - trajectory.add_point(point1, std::chrono::nanoseconds(300)); - trajectory.add_point(point2, std::chrono::nanoseconds(500)); - trajectory.add_point(point3, std::chrono::nanoseconds(100)); - - trajectory.set_point(replacement1, std::chrono::nanoseconds(50), 1); - EXPECT_TRUE(trajectory[1].first.data() == replacement1.data()); - EXPECT_TRUE(trajectory[1].second == std::chrono::nanoseconds(350)); - - EXPECT_NO_THROW(trajectory.set_points(replacement_points, replacement_times)); - EXPECT_EQ(trajectory.get_size(), 2); - EXPECT_TRUE(trajectory[0].first.data() == replacement1.data()); - EXPECT_TRUE(trajectory[0].second == std::chrono::nanoseconds(100)); - EXPECT_TRUE(trajectory[1].first.data() == replacement2.data()); - EXPECT_TRUE(trajectory[1].second == std::chrono::nanoseconds(300)); + {// incompatible joint names + auto states = { + JointState::Random("foo", {"j_foo_1", "j_foo_2"}), JointState::Random("foo", {"j_bar_1", "j_bar_2"})}; + auto durations = {std::chrono::nanoseconds(100), std::chrono::nanoseconds(200)}; + EXPECT_THROW(trajectory.set_points(states, durations), exceptions::IncompatibleStatesException); } - { - state_representation::JointTrajectory trajectory; - auto point1 = state_representation::JointState::Random("foo", 25); - auto point2 = state_representation::JointState::Random("foo", 25); - auto point3 = state_representation::JointState::Random("foo", 25); - - auto replacement1 = state_representation::JointState::Random("foo", 25); - auto replacement2 = state_representation::JointState::Random("foo", 25); - std::vector replacement_points = {replacement1, replacement2}; - std::vector replacement_times = { - std::chrono::nanoseconds(100), std::chrono::nanoseconds(200)}; - - trajectory.add_point(point1, std::chrono::nanoseconds(300)); - trajectory.add_point(point2, std::chrono::nanoseconds(500)); - trajectory.add_point(point3, std::chrono::nanoseconds(100)); - - trajectory.set_point(replacement1, std::chrono::nanoseconds(50), 1); - EXPECT_TRUE(trajectory[1].first.data() == replacement1.data()); - EXPECT_TRUE(trajectory[1].second == std::chrono::nanoseconds(350)); - - EXPECT_NO_THROW(trajectory.set_points(replacement_points, replacement_times)); - EXPECT_EQ(trajectory.get_size(), 2); - EXPECT_TRUE(trajectory[0].first.data() == replacement1.data()); - EXPECT_TRUE(trajectory[0].second == std::chrono::nanoseconds(100)); - EXPECT_TRUE(trajectory[1].first.data() == replacement2.data()); - EXPECT_TRUE(trajectory[1].second == std::chrono::nanoseconds(300)); + {// check joint names + std::vector joint_names = {"j_foo_1", "j_foo_2", "j_foo_3"}; + trajectory.add_point(JointState::Random("foo", joint_names), std::chrono::nanoseconds(100)); + EXPECT_EQ(trajectory.get_joint_names(), joint_names); + trajectory.clear(); + EXPECT_EQ(trajectory.get_size(), 0); + } - replacement1.set_name("bar"); - EXPECT_THROW( - trajectory.set_point(replacement1, std::chrono::nanoseconds(50), 1), - state_representation::exceptions::IncompatibleStatesException); + auto point0 = JointState::Random("foo", 25); + auto point1 = JointState::Random("foo", 25); + auto point2 = JointState::Random("foo", 25); + + trajectory.add_point(point0, std::chrono::nanoseconds(100)); + + EXPECT_EQ(trajectory[0].first.data(), point0.data()); + EXPECT_EQ(trajectory[0].first.get_name(), point0.get_name()); + EXPECT_EQ(trajectory[0].first.get_names(), trajectory.get_joint_names()); + EXPECT_EQ(trajectory[0].first.get_names(), point0.get_names()); + trajectory.add_point(point2, std::chrono::nanoseconds(200)); + + EXPECT_NO_THROW(trajectory.insert_point(point2, std::chrono::nanoseconds(300), 1)); + EXPECT_EQ(trajectory.get_size(), 3); + + point0.set_name("foofoo"); + EXPECT_THROW(trajectory.set_point(point0, std::chrono::nanoseconds(50), 0), exceptions::IncompatibleStatesException); + EXPECT_NE(trajectory[0].first.get_name(), point0.get_name()); + point0.set_name("foo"); + + std::vector points = {point0, point1, point2}; + std::vector durations = { + std::chrono::nanoseconds(10), std::chrono::nanoseconds(20), std::chrono::nanoseconds(30)}; + trajectory.set_points(points, durations); + for (unsigned int i = 0; i < trajectory.get_size(); ++i) { + EXPECT_EQ(trajectory[i].first.data(), points[i].data()); + EXPECT_EQ(trajectory[i].first.get_name(), points[i].get_name()); + EXPECT_EQ(trajectory[i].first.get_names(), points[i].get_names()); + EXPECT_EQ(trajectory[i].first.get_name(), points[i].get_name()); + EXPECT_EQ(trajectory[i].second, durations[i]); } } From cf54ae0a883906a380c444d4674a820e967e82c6 Mon Sep 17 00:00:00 2001 From: Vaios Papaspyros <8146703+bpapaspyros@users.noreply.github.com> Date: Sun, 19 Jan 2025 17:14:18 +0100 Subject: [PATCH 26/52] refactor: make base class get_point(s) classes protected --- .../trajectory/TrajectoryBase.hpp | 28 +++++++++---------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp index 3b2740500..9058b7e61 100644 --- a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp +++ b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp @@ -16,20 +16,6 @@ struct TrajectoryPoint { template class TrajectoryBase : public State { public: - /** - * @brief Get attribute list of trajectory points - * @return the list of trajectory points - */ - const std::deque& get_points() const; - - /** - * @brief Get the trajectory point at given index - * @param index the index - * @return the trajectory point - * @throw std::out_of_range if pos is out of range - */ - const TrajectoryT& get_point(unsigned int index) const; - /** * @brief Get attribute list of trajectory point durations * @return the list of trajectory point durations @@ -75,6 +61,20 @@ class TrajectoryBase : public State { */ explicit TrajectoryBase(const std::string& name); + /** + * @brief Get attribute list of trajectory points + * @return the list of trajectory points + */ + const std::deque& get_points() const; + + /** + * @brief Get the trajectory point at given index + * @param index the index + * @return the trajectory point + * @throw std::out_of_range if pos is out of range + */ + const TrajectoryT& get_point(unsigned int index) const; + /** * @brief Add new point to trajectory * @param new_point the new point From 3478b0b327d972611c3f6af517074211bc795cb1 Mon Sep 17 00:00:00 2001 From: Vaios Papaspyros <8146703+bpapaspyros@users.noreply.github.com> Date: Sun, 19 Jan 2025 17:24:47 +0100 Subject: [PATCH 27/52] fix: expose get_points functions in test interface --- source/state_representation/test/tests/test_trajectory.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/source/state_representation/test/tests/test_trajectory.cpp b/source/state_representation/test/tests/test_trajectory.cpp index 5bc3c2304..efe490fa6 100644 --- a/source/state_representation/test/tests/test_trajectory.cpp +++ b/source/state_representation/test/tests/test_trajectory.cpp @@ -23,6 +23,7 @@ class TrajectoryBaseInterface : public TrajectoryBase { using TrajectoryBase::TrajectoryBase; using TrajectoryBase::add_point; using TrajectoryBase::get_point; + using TrajectoryBase::get_points; using TrajectoryBase::insert_point; using TrajectoryBase::operator[]; }; From ced7d7c8e5b90a8630764de2fa6548de0d571a1e Mon Sep 17 00:00:00 2001 From: bpapaspyros Date: Tue, 28 Jan 2025 08:05:11 +0100 Subject: [PATCH 28/52] fix: rearrange includes --- .../trajectory/TrajectoryBase.hpp | 4 +- .../src/trajectory/CartesianTrajectory.cpp | 29 +++++++++----- .../src/trajectory/JointTrajectory.cpp | 40 +++++++++++-------- 3 files changed, 44 insertions(+), 29 deletions(-) diff --git a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp index 9058b7e61..5c350cc10 100644 --- a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp +++ b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp @@ -1,11 +1,11 @@ #pragma once -#include "state_representation/State.hpp" - #include #include #include +#include "state_representation/State.hpp" + namespace state_representation { struct TrajectoryPoint { diff --git a/source/state_representation/src/trajectory/CartesianTrajectory.cpp b/source/state_representation/src/trajectory/CartesianTrajectory.cpp index f99c13ab1..2c6b28c68 100644 --- a/source/state_representation/src/trajectory/CartesianTrajectory.cpp +++ b/source/state_representation/src/trajectory/CartesianTrajectory.cpp @@ -5,8 +5,6 @@ #include "state_representation/exceptions/IncompatibleReferenceFramesException.hpp" #include "state_representation/exceptions/IncompatibleSizeException.hpp" -#include - namespace state_representation { CartesianTrajectory::CartesianTrajectory(const std::string& name, const std::string& reference_frame) : TrajectoryBase(name), reference_frame_(reference_frame) { @@ -20,7 +18,8 @@ const std::string CartesianTrajectory::get_reference_frame() const { CartesianTrajectory::CartesianTrajectory( const CartesianState& point, const std::chrono::nanoseconds& duration, const std::string& name, - const std::string& reference_frame) + const std::string& reference_frame +) : TrajectoryBase(name), reference_frame_(reference_frame) { this->set_type(StateType::CARTESIAN_TRAJECTORY); this->reset(); @@ -33,7 +32,8 @@ CartesianTrajectory::CartesianTrajectory( CartesianTrajectory::CartesianTrajectory( const std::vector& points, const std::vector& durations, - const std::string& name, const std::string& reference_frame) + const std::string& name, const std::string& reference_frame +) : TrajectoryBase(name), reference_frame_(reference_frame) { this->set_type(StateType::CARTESIAN_TRAJECTORY); this->reset(); @@ -51,7 +51,8 @@ void CartesianTrajectory::add_point(const CartesianState& new_point, const std:: if (this->get_size() > 0) { if (new_point.get_reference_frame() != reference_frame_) { throw exceptions::IncompatibleReferenceFramesException( - "Incompatible reference frames: " + new_point.get_reference_frame() + " and " + reference_frame_); + "Incompatible reference frames: " + new_point.get_reference_frame() + " and " + reference_frame_ + ); } } else { reference_frame_ = new_point.get_reference_frame(); @@ -64,14 +65,16 @@ void CartesianTrajectory::add_point(const CartesianState& new_point, const std:: } void CartesianTrajectory::insert_point( - const CartesianState& new_point, const std::chrono::nanoseconds& duration, unsigned int pos) { + const CartesianState& new_point, const std::chrono::nanoseconds& duration, unsigned int pos +) { if (new_point.is_empty()) { throw exceptions::EmptyStateException("Point is empty"); } if (this->get_size() > 0) { if (new_point.get_reference_frame() != reference_frame_) { throw exceptions::IncompatibleReferenceFramesException( - "Incompatible reference frames: " + new_point.get_reference_frame() + " and " + reference_frame_); + "Incompatible reference frames: " + new_point.get_reference_frame() + " and " + reference_frame_ + ); } } else if (reference_frame_.empty()) { reference_frame_ = new_point.get_reference_frame(); @@ -88,13 +91,15 @@ void CartesianTrajectory::insert_point( } void CartesianTrajectory::set_point( - const CartesianState& point, const std::chrono::nanoseconds& duration, unsigned int index) { + const CartesianState& point, const std::chrono::nanoseconds& duration, unsigned int index +) { if (point.is_empty()) { throw exceptions::EmptyStateException("Point is empty"); } if (point.get_reference_frame() != reference_frame_) { throw exceptions::IncompatibleReferenceFramesException( - "Incompatible reference frames: " + point.get_reference_frame() + " and " + reference_frame_); + "Incompatible reference frames: " + point.get_reference_frame() + " and " + reference_frame_ + ); } CartesianTrajectoryPoint trajectory_point; trajectory_point.data = point.data(); @@ -108,7 +113,8 @@ void CartesianTrajectory::set_point( } void CartesianTrajectory::set_points( - const std::vector& points, const std::vector& durations) { + const std::vector& points, const std::vector& durations +) { if (points.size() != durations.size()) { throw exceptions::IncompatibleSizeException("The size of the points and durations vectors are not equal"); } @@ -124,7 +130,8 @@ void CartesianTrajectory::set_points( throw exceptions::EmptyStateException("Vector contains at least one point that is empty"); } else if (points[i].get_reference_frame() != candidate_reference_frame) { throw exceptions::IncompatibleReferenceFramesException( - "Incompatible reference frames: " + points[i].get_reference_frame() + " and " + candidate_reference_frame); + "Incompatible reference frames: " + points[i].get_reference_frame() + " and " + candidate_reference_frame + ); } CartesianTrajectoryPoint trajectory_point; trajectory_point.data = points[i].data(); diff --git a/source/state_representation/src/trajectory/JointTrajectory.cpp b/source/state_representation/src/trajectory/JointTrajectory.cpp index ac662f6c3..cf214d477 100644 --- a/source/state_representation/src/trajectory/JointTrajectory.cpp +++ b/source/state_representation/src/trajectory/JointTrajectory.cpp @@ -3,7 +3,6 @@ #include "state_representation/exceptions/EmptyStateException.hpp" #include "state_representation/exceptions/IncompatibleStatesException.hpp" -#include namespace state_representation { @@ -13,7 +12,8 @@ JointTrajectory::JointTrajectory(const std::string& name) : TrajectoryBase(name) { this->set_type(StateType::JOINT_TRAJECTORY); this->reset(); @@ -22,7 +22,8 @@ JointTrajectory::JointTrajectory( JointTrajectory::JointTrajectory( const std::vector& points, const std::vector& durations, - const std::string& name) + const std::string& name +) : TrajectoryBase(name) { this->set_type(StateType::JOINT_TRAJECTORY); this->reset(); @@ -38,10 +39,11 @@ void JointTrajectory::add_point(const JointState& new_point, const std::chrono:: this->robot_name_ = new_point.get_name(); } else if (this->joint_names_ != new_point.get_names()) { throw exceptions::IncompatibleStatesException( - "Incompatible joint names between the new point and current trajectory"); + "Incompatible joint names between the new point and current trajectory" + ); } else if (this->robot_name_ != new_point.get_name()) { - throw exceptions::IncompatibleStatesException( - "Incompatible robot name between the new point and current trajectory"); + throw exceptions::IncompatibleStatesException("Incompatible robot name between the new point and current trajectory" + ); } JointTrajectoryPoint trajectory_point; trajectory_point.data = new_point.data(); @@ -50,7 +52,8 @@ void JointTrajectory::add_point(const JointState& new_point, const std::chrono:: } void JointTrajectory::insert_point( - const JointState& new_point, const std::chrono::nanoseconds& duration, unsigned int pos) { + const JointState& new_point, const std::chrono::nanoseconds& duration, unsigned int pos +) { if (new_point.is_empty()) { throw exceptions::EmptyStateException("Point is empty"); } @@ -59,10 +62,11 @@ void JointTrajectory::insert_point( this->robot_name_ = new_point.get_name(); } else if (this->joint_names_ != new_point.get_names()) { throw exceptions::IncompatibleStatesException( - "Incompatible joint names between the new point and current trajectory"); + "Incompatible joint names between the new point and current trajectory" + ); } else if (this->robot_name_ != new_point.get_name()) { - throw exceptions::IncompatibleStatesException( - "Incompatible robot name between the new point and current trajectory"); + throw exceptions::IncompatibleStatesException("Incompatible robot name between the new point and current trajectory" + ); } try { JointTrajectoryPoint trajectory_point; @@ -79,10 +83,11 @@ void JointTrajectory::set_point(const JointState& point, const std::chrono::nano throw exceptions::EmptyStateException("Point is empty"); } else if (point.get_names() != this->joint_names_) { throw exceptions::IncompatibleStatesException( - "Incompatible joint names between the new point and current trajectory"); + "Incompatible joint names between the new point and current trajectory" + ); } else if (point.get_name() != this->robot_name_) { - throw exceptions::IncompatibleStatesException( - "Incompatible robot name between the new point and current trajectory"); + throw exceptions::IncompatibleStatesException("Incompatible robot name between the new point and current trajectory" + ); } JointTrajectoryPoint trajectory_point; trajectory_point.data = point.data(); @@ -95,7 +100,8 @@ void JointTrajectory::set_point(const JointState& point, const std::chrono::nano } void JointTrajectory::set_points( - const std::vector& points, const std::vector& durations) { + const std::vector& points, const std::vector& durations +) { if (points.size() != durations.size()) { throw exceptions::IncompatibleSizeException("The size of the points and durations vectors are not equal"); } @@ -111,10 +117,12 @@ void JointTrajectory::set_points( throw exceptions::EmptyStateException("Vector contains at least one point that is empty"); } else if (points[i].get_names() != candidate_joint_names) { throw exceptions::IncompatibleStatesException( - "Incompatible joint names between the new point and current trajectory"); + "Incompatible joint names between the new point and current trajectory" + ); } else if (points[i].get_name() != candidate_robot_name) { throw exceptions::IncompatibleStatesException( - "Incompatible robot name between the new point and current trajectory"); + "Incompatible robot name between the new point and current trajectory" + ); } JointTrajectoryPoint trajectory_point; trajectory_point.data = points[i].data(); From 741c0179ed564500331aa6cc529c296cbab20dd0 Mon Sep 17 00:00:00 2001 From: bpapaspyros Date: Tue, 28 Jan 2025 08:17:34 +0100 Subject: [PATCH 29/52] docs: update docstrings --- .../trajectory/CartesianTrajectory.hpp | 20 ++++++++++++----- .../trajectory/JointTrajectory.hpp | 22 ++++++++++++++----- .../trajectory/TrajectoryBase.hpp | 18 ++++++++++----- 3 files changed, 44 insertions(+), 16 deletions(-) diff --git a/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp b/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp index 5fe21e867..f232fe2ad 100644 --- a/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp +++ b/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp @@ -5,10 +5,18 @@ namespace state_representation { +/** + * @class CartesianTrajectoryPoint + * @brief Struct to represent a Cartesian trajectory point + */ struct CartesianTrajectoryPoint : public TrajectoryPoint { std::string name; }; +/** + * @class CartesianTrajectory + * @brief Class to represent a trajectory of Cartesian points and corresponding durations + */ class CartesianTrajectory : public TrajectoryBase { public: /** @@ -19,7 +27,7 @@ class CartesianTrajectory : public TrajectoryBase { explicit CartesianTrajectory(const std::string& name = "", const std::string& reference_frame = "world"); /** - * @brief Constructor with name and reference frame provided + * @brief Constructor with initial point, duration, name, and reference frame provided * @param point the initial point * @param duration the initial duration * @param name the name of the state @@ -27,10 +35,11 @@ class CartesianTrajectory : public TrajectoryBase { */ explicit CartesianTrajectory( const CartesianState& point, const std::chrono::nanoseconds& duration, const std::string& name = "", - const std::string& reference_frame = "world"); + const std::string& reference_frame = "world" + ); /** - * @brief Constructor with name and reference frame provided + * @brief Constructor with intial points, durations, name and reference frame provided * @param points vector of initial points * @param durations vector of initial durations * @param name the name of the state @@ -38,7 +47,8 @@ class CartesianTrajectory : public TrajectoryBase { */ explicit CartesianTrajectory( const std::vector& points, const std::vector& durations, - const std::string& name = "", const std::string& reference_frame = "world"); + const std::string& name = "", const std::string& reference_frame = "world" + ); /** * @brief Getter of the reference frame as const reference @@ -63,7 +73,7 @@ class CartesianTrajectory : public TrajectoryBase { void insert_point(const CartesianState& new_point, const std::chrono::nanoseconds& duration, unsigned int pos); /** - * @brief Get attribute list of trajectory points + * @brief Get list of trajectory points * @return queue of the Cartesian states of the trajectory */ const std::deque get_points() const; diff --git a/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp b/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp index 8e2f513cc..2310861b4 100644 --- a/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp +++ b/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp @@ -5,8 +5,16 @@ namespace state_representation { +/** + * @class JointTrajectoryPoint + * @brief Struct to represent a joint trajectory point + */ struct JointTrajectoryPoint : public TrajectoryPoint {}; +/** + * @class JointTrajectory + * @brief Class to represent a trajectory of joint points and corresponding durations + */ class JointTrajectory : public TrajectoryBase { public: /** @@ -16,26 +24,28 @@ class JointTrajectory : public TrajectoryBase { explicit JointTrajectory(const std::string& name = ""); /** - * @brief Constructor with name and reference frame provided + * @brief Constructor with initial point, duration, name, and reference frame provided * @param point the initial point * @param duration the initial duration * @param name the name of the state */ explicit JointTrajectory( - const JointState& point, const std::chrono::nanoseconds& duration, const std::string& name = ""); + const JointState& point, const std::chrono::nanoseconds& duration, const std::string& name = "" + ); /** - * @brief Constructor with name and reference frame provided + * @brief Constructor with initial points, durations, name, and reference frame provided * @param points vector of initial points * @param durations vector of initial durations * @param name the name of the state */ explicit JointTrajectory( const std::vector& points, const std::vector& durations, - const std::string& name = ""); + const std::string& name = "" + ); /** - * @brief Getter of the names attribute + * @brief Getter of the names * @return vector of joint names associated with the trajectory */ const std::vector& get_joint_names() const; @@ -57,7 +67,7 @@ class JointTrajectory : public TrajectoryBase { void insert_point(const JointState& new_point, const std::chrono::nanoseconds& duration, unsigned int pos); /** - * @brief Get attribute list of trajectory points + * @brief Get list of trajectory points * @return queue of the Joint states of the trajectory */ const std::deque get_points() const; diff --git a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp index 5c350cc10..16be7a3e6 100644 --- a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp +++ b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp @@ -8,28 +8,36 @@ namespace state_representation { +/** + * @class TrajectoryPoint + * @brief Struct to represent the base characteristics of a trajectory point + */ struct TrajectoryPoint { Eigen::VectorXd data; std::chrono::nanoseconds duration; }; +/** + * @class TrajectoryBase + * @brief Base class that offers common functionalities for trajectory classes + */ template class TrajectoryBase : public State { public: /** - * @brief Get attribute list of trajectory point durations + * @brief Get list of trajectory point durations * @return the list of trajectory point durations */ const std::deque get_durations() const; /** - * @brief Get attribute list of trajectory point times from start + * @brief Get list of trajectory point times from start * @return the list of trajectory point times from start */ const std::deque get_times_from_start() const; /** - * @brief Get attribute number of point in trajectory + * @brief Get number of points in trajectory * @return the number of points in trajectory */ unsigned int get_size() const; @@ -40,7 +48,7 @@ class TrajectoryBase : public State { virtual void reset(); /** - * @brief Delete last point and corresponding time from trajectory + * @brief Delete the last point from trajectory */ virtual void delete_point(); @@ -62,7 +70,7 @@ class TrajectoryBase : public State { explicit TrajectoryBase(const std::string& name); /** - * @brief Get attribute list of trajectory points + * @brief Get list of trajectory points * @return the list of trajectory points */ const std::deque& get_points() const; From 47e63119a22330639bb69ec76756550030a229f4 Mon Sep 17 00:00:00 2001 From: bpapaspyros Date: Tue, 28 Jan 2025 08:36:34 +0100 Subject: [PATCH 30/52] fix: tidy up clear, reset, and delete functions --- .../trajectory/CartesianTrajectory.hpp | 10 ---------- .../trajectory/JointTrajectory.hpp | 10 ---------- .../trajectory/TrajectoryBase.hpp | 15 +++----------- .../src/trajectory/CartesianTrajectory.cpp | 12 ----------- .../src/trajectory/JointTrajectory.cpp | 13 ------------ .../test/tests/test_trajectory.cpp | 20 +++++++++++-------- 6 files changed, 15 insertions(+), 65 deletions(-) diff --git a/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp b/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp index f232fe2ad..9d026d680 100644 --- a/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp +++ b/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp @@ -118,16 +118,6 @@ class CartesianTrajectory : public TrajectoryBase { */ virtual void reset() override; - /** - * @brief Delete last point and corresponding time from trajectory - */ - virtual void delete_point() override; - - /** - * @brief Clear trajectory - */ - virtual void clear() override; - private: std::string reference_frame_;///< name of the reference frame }; diff --git a/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp b/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp index 2310861b4..9b02d9725 100644 --- a/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp +++ b/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp @@ -112,16 +112,6 @@ class JointTrajectory : public TrajectoryBase { */ virtual void reset() override; - /** - * @brief Delete last point and corresponding duration from trajectory - */ - virtual void delete_point() override; - - /** - * @brief Clear trajectory - */ - virtual void clear() override; - private: std::vector joint_names_;///< names of the joints std::string robot_name_; ///< name of the robot diff --git a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp index 16be7a3e6..cab9f1927 100644 --- a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp +++ b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp @@ -52,11 +52,6 @@ class TrajectoryBase : public State { */ virtual void delete_point(); - /** - * @brief Clear trajectory - */ - virtual void clear(); - protected: /** * @brief Empty constructor @@ -166,17 +161,13 @@ inline void TrajectoryBase::insert_point(const TrajectoryT& new_poi template inline void TrajectoryBase::delete_point() { - this->set_empty(false); if (!this->points_.empty()) { this->points_.pop_back(); + } else { + this->set_empty(false); } } -template -inline void TrajectoryBase::clear() { - this->points_.clear(); -} - template inline const std::deque& TrajectoryBase::get_points() const { return this->points_; @@ -205,7 +196,7 @@ inline void TrajectoryBase::set_points(const std::vectorclear(); + this->reset(); for (unsigned int i = 0; i < points.size(); ++i) { this->add_point(points[i]); } diff --git a/source/state_representation/src/trajectory/CartesianTrajectory.cpp b/source/state_representation/src/trajectory/CartesianTrajectory.cpp index 2c6b28c68..8f7ffec83 100644 --- a/source/state_representation/src/trajectory/CartesianTrajectory.cpp +++ b/source/state_representation/src/trajectory/CartesianTrajectory.cpp @@ -182,16 +182,4 @@ void CartesianTrajectory::reset() { this->TrajectoryBase::reset(); this->reference_frame_ = "world"; } - -void CartesianTrajectory::delete_point() { - this->TrajectoryBase::delete_point(); - if (this->get_size() == 0) { - this->clear(); - } -} - -void CartesianTrajectory::clear() { - this->TrajectoryBase::clear(); - this->reference_frame_ = ""; -} }// namespace state_representation diff --git a/source/state_representation/src/trajectory/JointTrajectory.cpp b/source/state_representation/src/trajectory/JointTrajectory.cpp index cf214d477..8401c86c9 100644 --- a/source/state_representation/src/trajectory/JointTrajectory.cpp +++ b/source/state_representation/src/trajectory/JointTrajectory.cpp @@ -176,17 +176,4 @@ void JointTrajectory::reset() { this->robot_name_ = ""; this->joint_names_.clear(); } - -void JointTrajectory::delete_point() { - this->TrajectoryBase::delete_point(); - if (this->get_size() == 0) { - this->clear(); - } -} - -void JointTrajectory::clear() { - this->TrajectoryBase::clear(); - this->joint_names_.clear(); - this->robot_name_ = ""; -} }// namespace state_representation diff --git a/source/state_representation/test/tests/test_trajectory.cpp b/source/state_representation/test/tests/test_trajectory.cpp index efe490fa6..64b5acec5 100644 --- a/source/state_representation/test/tests/test_trajectory.cpp +++ b/source/state_representation/test/tests/test_trajectory.cpp @@ -63,7 +63,7 @@ TEST(TrajectoryTest, TestTrajectoryBase) { EXPECT_EQ(trajectory.get_times_from_start()[1], point0.duration + point2.duration); EXPECT_EQ(trajectory.get_times_from_start()[2], point0.duration + point2.duration + point1.duration); - trajectory.clear(); + trajectory.reset(); EXPECT_EQ(trajectory.get_size(), 0); trajectory.reset(); EXPECT_TRUE(trajectory.is_empty()); @@ -109,9 +109,10 @@ TEST(TrajectoryTest, CartesianTrajectory) { EXPECT_THROW(trajectory.add_point(point0, std::chrono::nanoseconds(100)), exceptions::EmptyStateException); EXPECT_NO_THROW(trajectory.add_point(point1, std::chrono::nanoseconds(200))); EXPECT_THROW( - trajectory.add_point(point2, std::chrono::nanoseconds(200)), exceptions::IncompatibleReferenceFramesException); + trajectory.add_point(point2, std::chrono::nanoseconds(200)), exceptions::IncompatibleReferenceFramesException + ); EXPECT_STREQ(trajectory.get_reference_frame().c_str(), "world2"); - trajectory.clear(); + trajectory.reset(); EXPECT_STREQ(trajectory.get_reference_frame().c_str(), ""); } @@ -134,7 +135,8 @@ TEST(TrajectoryTest, CartesianTrajectory) { std::vector points = {point0, point1, point2}; std::vector durations = { - std::chrono::nanoseconds(10), std::chrono::nanoseconds(20), std::chrono::nanoseconds(30)}; + std::chrono::nanoseconds(10), std::chrono::nanoseconds(20), std::chrono::nanoseconds(30) + }; trajectory.set_points(points, durations); for (unsigned int i = 0; i < trajectory.get_size(); ++i) { EXPECT_EQ(trajectory[i].first.data(), points[i].data()); @@ -168,7 +170,7 @@ TEST(TrajectoryTest, JointTrajectory) { auto point1 = JointState::Random("bar", 25); EXPECT_NO_THROW(trajectory.add_point(point0, std::chrono::nanoseconds(100))); EXPECT_THROW(trajectory.add_point(point1, std::chrono::nanoseconds(200)), exceptions::IncompatibleStatesException); - trajectory.clear(); + trajectory.reset(); } {// incompatible sizes @@ -187,7 +189,8 @@ TEST(TrajectoryTest, JointTrajectory) { {// incompatible joint names auto states = { - JointState::Random("foo", {"j_foo_1", "j_foo_2"}), JointState::Random("foo", {"j_bar_1", "j_bar_2"})}; + JointState::Random("foo", {"j_foo_1", "j_foo_2"}), JointState::Random("foo", {"j_bar_1", "j_bar_2"}) + }; auto durations = {std::chrono::nanoseconds(100), std::chrono::nanoseconds(200)}; EXPECT_THROW(trajectory.set_points(states, durations), exceptions::IncompatibleStatesException); } @@ -196,7 +199,7 @@ TEST(TrajectoryTest, JointTrajectory) { std::vector joint_names = {"j_foo_1", "j_foo_2", "j_foo_3"}; trajectory.add_point(JointState::Random("foo", joint_names), std::chrono::nanoseconds(100)); EXPECT_EQ(trajectory.get_joint_names(), joint_names); - trajectory.clear(); + trajectory.reset(); EXPECT_EQ(trajectory.get_size(), 0); } @@ -222,7 +225,8 @@ TEST(TrajectoryTest, JointTrajectory) { std::vector points = {point0, point1, point2}; std::vector durations = { - std::chrono::nanoseconds(10), std::chrono::nanoseconds(20), std::chrono::nanoseconds(30)}; + std::chrono::nanoseconds(10), std::chrono::nanoseconds(20), std::chrono::nanoseconds(30) + }; trajectory.set_points(points, durations); for (unsigned int i = 0; i < trajectory.get_size(); ++i) { EXPECT_EQ(trajectory[i].first.data(), points[i].data()); From ee987c7883495819be6c75c17ea66705e284d6d4 Mon Sep 17 00:00:00 2001 From: bpapaspyros Date: Tue, 28 Jan 2025 08:47:26 +0100 Subject: [PATCH 31/52] feat: implement add_points function in TrajectoryBase --- .../trajectory/TrajectoryBase.hpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp index cab9f1927..0b47c63a5 100644 --- a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp +++ b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp @@ -84,6 +84,12 @@ class TrajectoryBase : public State { */ void add_point(const TrajectoryT& new_point); + /** + * @brief Add new points to trajectory + * @param new_point the new point + */ + void add_points(const std::vector& new_points); + /** * @brief Insert new trajectory point between two already existing points * @param new_point the new point @@ -148,6 +154,16 @@ inline void TrajectoryBase::add_point(const TrajectoryT& new_point) this->points_.push_back(new_point); } +template +inline void TrajectoryBase::add_points(const std::vector& new_points) { + if (new_points.size() == 0) { + return; + } + for (auto point : new_points) { + points_.push_back(point); + } +} + template inline void TrajectoryBase::insert_point(const TrajectoryT& new_point, unsigned int pos) { if (pos > this->points_.size()) { From f0e75bc0da7add708de3a5322f75539c183f9831 Mon Sep 17 00:00:00 2001 From: bpapaspyros Date: Tue, 28 Jan 2025 09:01:16 +0100 Subject: [PATCH 32/52] feat: implement add_points functions for derived classes --- .../trajectory/CartesianTrajectory.hpp | 21 +++++++++++++++---- .../trajectory/JointTrajectory.hpp | 16 ++++++++++++-- .../src/trajectory/CartesianTrajectory.cpp | 11 ++++++++++ .../src/trajectory/JointTrajectory.cpp | 11 ++++++++++ 4 files changed, 53 insertions(+), 6 deletions(-) diff --git a/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp b/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp index 9d026d680..e943bcd5b 100644 --- a/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp +++ b/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp @@ -57,14 +57,27 @@ class CartesianTrajectory : public TrajectoryBase { const std::string get_reference_frame() const; /** - * @brief Add new point and corresponding time to trajectory + * @brief Add new point and corresponding duration to trajectory * @param new_point the new trajectory point * @param duration the duration for the new point + * @throw EmptyStateException if point is empty + * @throw IncompatibleReferenceFramesException if point has different reference frame */ void add_point(const CartesianState& new_point, const std::chrono::nanoseconds& duration); /** - * @brief Insert new point and corresponding time to trajectory between two + * @brief Add new points and corresponding durations to trajectory + * @param new_points the new trajectory point + * @param durations the duration for the new point + * @throw IncompatibleSizeException if points and durations have different sizes + * @throw EmptyStateException if point is empty + * @throw IncompatibleReferenceFramesException if point has different reference frame + */ + void + add_points(const std::vector& new_points, const std::vector& durations); + + /** + * @brief Insert new point and corresponding duration to trajectory between two * already existing points * @param new_point the new trajectory point * @param duration the duration for the new point @@ -88,7 +101,7 @@ class CartesianTrajectory : public TrajectoryBase { /** * @brief Set the trajectory point at given index * @param point the new point - * @param duration the new time + * @param duration the new duration * @param index the index * @throw std::out_of_range if index is out of range * @throw EmptyStateException if point is empty @@ -108,7 +121,7 @@ class CartesianTrajectory : public TrajectoryBase { /** * @brief Operator overload for returning a single trajectory point and - * corresponding time + * corresponding duration * @return the Cartesian state and duration pair that corresponds to the index */ std::pair operator[](unsigned int idx) const; diff --git a/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp b/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp index 9b02d9725..fae7bfe11 100644 --- a/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp +++ b/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp @@ -54,9 +54,21 @@ class JointTrajectory : public TrajectoryBase { * @brief Add new point and corresponding duration to trajectory * @param new_point the new trajectory point * @param duration the duration for the new point + * @throw EmptyStateException if point is empty + * @throw IncompatibleStatesException if point has different robot name */ void add_point(const JointState& new_point, const std::chrono::nanoseconds& duration); + /** + * @brief Add new points and corresponding durations to trajectory + * @param new_points the new trajectory point + * @param durations the duration for the new point + * @throw IncompatibleSizeException if points and durations have different sizes + * @throw EmptyStateException if point is empty + * @throw IncompatibleStatesException if point has different robot name + */ + void add_points(const std::vector& new_points, const std::vector& durations); + /** * @brief Insert new point and corresponding duration to trajectory between two * already existing points @@ -86,7 +98,7 @@ class JointTrajectory : public TrajectoryBase { * @param index the index * @throw std::out_of_range if index is out of range * @throw EmptyStateException if point is empty - * @throw IncompatibleReferenceFramesException if point has different reference frame + * @throw IncompatibleStatesException if point has different robot name */ void set_point(const JointState& point, const std::chrono::nanoseconds& duration, unsigned int index); @@ -96,7 +108,7 @@ class JointTrajectory : public TrajectoryBase { * @param duration vector of new durations * @throw IncompatibleSizeException if points and durations have different sizes * @throw EmptyStateException if point is empty - * @throw IncompatibleReferenceFramesException if point has different reference frame + * @throw IncompatibleStatesException if point has different robot name */ void set_points(const std::vector& points, const std::vector& durations); diff --git a/source/state_representation/src/trajectory/CartesianTrajectory.cpp b/source/state_representation/src/trajectory/CartesianTrajectory.cpp index 8f7ffec83..a4612e547 100644 --- a/source/state_representation/src/trajectory/CartesianTrajectory.cpp +++ b/source/state_representation/src/trajectory/CartesianTrajectory.cpp @@ -64,6 +64,17 @@ void CartesianTrajectory::add_point(const CartesianState& new_point, const std:: this->TrajectoryBase::add_point(trajectory_point); } +void CartesianTrajectory::add_points( + const std::vector& new_points, const std::vector& durations +) { + if (new_points.size() != durations.size()) { + throw exceptions::IncompatibleSizeException("The size of the points and durations vectors are not equal"); + } + for (unsigned int i = 0; i < new_points.size(); ++i) { + add_point(new_points[i], durations[i]); + } +} + void CartesianTrajectory::insert_point( const CartesianState& new_point, const std::chrono::nanoseconds& duration, unsigned int pos ) { diff --git a/source/state_representation/src/trajectory/JointTrajectory.cpp b/source/state_representation/src/trajectory/JointTrajectory.cpp index 8401c86c9..f2d6e81cd 100644 --- a/source/state_representation/src/trajectory/JointTrajectory.cpp +++ b/source/state_representation/src/trajectory/JointTrajectory.cpp @@ -51,6 +51,17 @@ void JointTrajectory::add_point(const JointState& new_point, const std::chrono:: this->TrajectoryBase::add_point(trajectory_point); } +void JointTrajectory::add_points( + const std::vector& new_points, const std::vector& durations +) { + if (new_points.size() != durations.size()) { + throw exceptions::IncompatibleSizeException("The size of the points and durations vectors are not equal"); + } + for (unsigned int i = 0; i < new_points.size(); ++i) { + add_point(new_points[i], durations[i]); + } +} + void JointTrajectory::insert_point( const JointState& new_point, const std::chrono::nanoseconds& duration, unsigned int pos ) { From 1e8e12c0f823c03f5dff4a99f52c99b1bcd0d3ba Mon Sep 17 00:00:00 2001 From: bpapaspyros Date: Tue, 28 Jan 2025 09:04:06 +0100 Subject: [PATCH 33/52] test: fix tests on reset --- source/state_representation/test/tests/test_trajectory.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/source/state_representation/test/tests/test_trajectory.cpp b/source/state_representation/test/tests/test_trajectory.cpp index 64b5acec5..be7837144 100644 --- a/source/state_representation/test/tests/test_trajectory.cpp +++ b/source/state_representation/test/tests/test_trajectory.cpp @@ -113,7 +113,7 @@ TEST(TrajectoryTest, CartesianTrajectory) { ); EXPECT_STREQ(trajectory.get_reference_frame().c_str(), "world2"); trajectory.reset(); - EXPECT_STREQ(trajectory.get_reference_frame().c_str(), ""); + EXPECT_STREQ(trajectory.get_reference_frame().c_str(), "world"); } auto point0 = CartesianState::Random("foo"); From 8bc0280201f75f9564f4023593add652d5e0e796 Mon Sep 17 00:00:00 2001 From: bpapaspyros Date: Tue, 28 Jan 2025 09:05:49 +0100 Subject: [PATCH 34/52] refactor: variable renaming --- .../src/trajectory/CartesianTrajectory.cpp | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/source/state_representation/src/trajectory/CartesianTrajectory.cpp b/source/state_representation/src/trajectory/CartesianTrajectory.cpp index a4612e547..e1a3ba778 100644 --- a/source/state_representation/src/trajectory/CartesianTrajectory.cpp +++ b/source/state_representation/src/trajectory/CartesianTrajectory.cpp @@ -49,13 +49,13 @@ void CartesianTrajectory::add_point(const CartesianState& new_point, const std:: throw exceptions::EmptyStateException("The Cartesian state provided is empty"); } if (this->get_size() > 0) { - if (new_point.get_reference_frame() != reference_frame_) { + if (new_point.get_reference_frame() != this->reference_frame_) { throw exceptions::IncompatibleReferenceFramesException( - "Incompatible reference frames: " + new_point.get_reference_frame() + " and " + reference_frame_ + "Incompatible reference frames: " + new_point.get_reference_frame() + " and " + this->reference_frame_ ); } } else { - reference_frame_ = new_point.get_reference_frame(); + this->reference_frame_ = new_point.get_reference_frame(); } CartesianTrajectoryPoint trajectory_point; trajectory_point.data = new_point.data(); @@ -82,13 +82,13 @@ void CartesianTrajectory::insert_point( throw exceptions::EmptyStateException("Point is empty"); } if (this->get_size() > 0) { - if (new_point.get_reference_frame() != reference_frame_) { + if (new_point.get_reference_frame() != this->reference_frame_) { throw exceptions::IncompatibleReferenceFramesException( - "Incompatible reference frames: " + new_point.get_reference_frame() + " and " + reference_frame_ + "Incompatible reference frames: " + new_point.get_reference_frame() + " and " + this->reference_frame_ ); } - } else if (reference_frame_.empty()) { - reference_frame_ = new_point.get_reference_frame(); + } else if (this->reference_frame_.empty()) { + this->reference_frame_ = new_point.get_reference_frame(); } CartesianTrajectoryPoint trajectory_point; trajectory_point.data = new_point.data(); @@ -107,9 +107,9 @@ void CartesianTrajectory::set_point( if (point.is_empty()) { throw exceptions::EmptyStateException("Point is empty"); } - if (point.get_reference_frame() != reference_frame_) { + if (point.get_reference_frame() != this->reference_frame_) { throw exceptions::IncompatibleReferenceFramesException( - "Incompatible reference frames: " + point.get_reference_frame() + " and " + reference_frame_ + "Incompatible reference frames: " + point.get_reference_frame() + " and " + this->reference_frame_ ); } CartesianTrajectoryPoint trajectory_point; @@ -152,7 +152,7 @@ void CartesianTrajectory::set_points( } try { this->TrajectoryBase::set_points(trajectory_points); - reference_frame_ = candidate_reference_frame; + this->reference_frame_ = candidate_reference_frame; } catch (...) { throw; } From f95ad5e70d51afaae7ff04d41760825f7adf666a Mon Sep 17 00:00:00 2001 From: bpapaspyros Date: Tue, 28 Jan 2025 09:26:03 +0100 Subject: [PATCH 35/52] fix: improve get_times_from_start --- .../state_representation/trajectory/TrajectoryBase.hpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp index 0b47c63a5..edb4ce868 100644 --- a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp +++ b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp @@ -213,8 +213,8 @@ inline void TrajectoryBase::set_points(const std::vectorreset(); - for (unsigned int i = 0; i < points.size(); ++i) { - this->add_point(points[i]); + for (auto point : points) { + this->add_point(point); } } @@ -230,11 +230,9 @@ inline const std::deque TrajectoryBase::g template inline const std::deque TrajectoryBase::get_times_from_start() const { std::deque times_from_start; + std::chrono::nanoseconds time_from_start = std::chrono::nanoseconds(0); for (unsigned int i = 0; i < this->points_.size(); ++i) { - std::chrono::nanoseconds time_from_start = std::chrono::nanoseconds(0); - for (unsigned int j = 0; j <= i; ++j) { - time_from_start += this->points_[j].duration; - } + time_from_start += this->points_[i].duration; times_from_start.push_back(time_from_start); } return times_from_start; From 3d7e7f24f5bbaf86d61edb6c183d0d2c0bc3bc96 Mon Sep 17 00:00:00 2001 From: bpapaspyros Date: Tue, 28 Jan 2025 09:31:28 +0100 Subject: [PATCH 36/52] fix: tidy up TrajectoryBase [] overload and exceptions --- .../trajectory/TrajectoryBase.hpp | 31 ++++++++++++++++--- 1 file changed, 27 insertions(+), 4 deletions(-) diff --git a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp index edb4ce868..8bdbacb75 100644 --- a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp +++ b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp @@ -126,7 +126,16 @@ class TrajectoryBase : public State { * @return the trajectory point * @throw std::out_of_range if index is out of range */ - const TrajectoryT operator[](unsigned int idx) const; + const TrajectoryT& operator[](unsigned int idx) const; + + /** + * @brief Operator overload for returning a single trajectory point and + * corresponding time + * @param idx the index + * @return the trajectory point + * @throw std::out_of_range if index is out of range + */ + TrajectoryT& operator[](unsigned int idx); private: std::deque points_; @@ -191,11 +200,17 @@ inline const std::deque& TrajectoryBase::get_points() template inline const TrajectoryT& TrajectoryBase::get_point(unsigned int index) const { + if (index >= this->points_.size()) { + throw std::out_of_range("Index out of range"); + } return this->points_[index]; } template inline TrajectoryT& TrajectoryBase::get_point(unsigned int index) { + if (index >= this->points_.size()) { + throw std::out_of_range("Index out of range"); + } return this->points_[index]; } @@ -244,10 +259,18 @@ inline unsigned int TrajectoryBase::get_size() const { } template -inline const TrajectoryT TrajectoryBase::operator[](unsigned int idx) const { - if (idx >= this->points_.size()) { +inline const TrajectoryT& TrajectoryBase::operator[](unsigned int index) const { + if (index >= this->points_.size()) { throw std::out_of_range("Index out of range"); } - return this->points_[idx]; + return this->points_[index]; +} + +template +inline TrajectoryT& TrajectoryBase::operator[](unsigned int index) { + if (index >= this->points_.size()) { + throw std::out_of_range("Index out of range"); + } + return this->points_[index]; } }// namespace state_representation From abc89bf78141bf754a0a00c62edb79622bf54265 Mon Sep 17 00:00:00 2001 From: bpapaspyros Date: Tue, 28 Jan 2025 09:35:50 +0100 Subject: [PATCH 37/52] fix: use add_point in CartesianTrajectory constructor --- .../src/trajectory/CartesianTrajectory.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/source/state_representation/src/trajectory/CartesianTrajectory.cpp b/source/state_representation/src/trajectory/CartesianTrajectory.cpp index e1a3ba778..20b3e70e6 100644 --- a/source/state_representation/src/trajectory/CartesianTrajectory.cpp +++ b/source/state_representation/src/trajectory/CartesianTrajectory.cpp @@ -23,11 +23,7 @@ CartesianTrajectory::CartesianTrajectory( : TrajectoryBase(name), reference_frame_(reference_frame) { this->set_type(StateType::CARTESIAN_TRAJECTORY); this->reset(); - CartesianTrajectoryPoint trajectory_point; - trajectory_point.data = point.data(); - trajectory_point.duration = duration; - trajectory_point.name = point.get_name(); - this->TrajectoryBase::add_point(trajectory_point); + this->add_point(point, duration); } CartesianTrajectory::CartesianTrajectory( From fe5c9a21c9711332a3fbf4902a01033dd73c7bfb Mon Sep 17 00:00:00 2001 From: bpapaspyros Date: Tue, 28 Jan 2025 09:39:29 +0100 Subject: [PATCH 38/52] fix: condition and state update after delete_point --- .../state_representation/trajectory/TrajectoryBase.hpp | 3 ++- .../src/trajectory/CartesianTrajectory.cpp | 4 +--- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp index 8bdbacb75..be5a067dd 100644 --- a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp +++ b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp @@ -188,7 +188,8 @@ template inline void TrajectoryBase::delete_point() { if (!this->points_.empty()) { this->points_.pop_back(); - } else { + } + if (this->points_.empty()) { this->set_empty(false); } } diff --git a/source/state_representation/src/trajectory/CartesianTrajectory.cpp b/source/state_representation/src/trajectory/CartesianTrajectory.cpp index 20b3e70e6..007448ae5 100644 --- a/source/state_representation/src/trajectory/CartesianTrajectory.cpp +++ b/source/state_representation/src/trajectory/CartesianTrajectory.cpp @@ -174,10 +174,8 @@ CartesianState CartesianTrajectory::get_point(unsigned int index) const { std::pair CartesianTrajectory::operator[](unsigned int idx) const { try { auto point = this->TrajectoryBase::operator[](idx); - CartesianState state; + CartesianState state(point.name, this->reference_frame_); state.set_data(point.data); - state.set_name(point.name); - state.set_reference_frame(this->reference_frame_); auto duration = point.duration; return std::make_pair(state, duration); } catch (...) { From a903f1854cff58bb888b0ffeb3648f600bbcd1d7 Mon Sep 17 00:00:00 2001 From: bpapaspyros Date: Tue, 28 Jan 2025 10:26:47 +0100 Subject: [PATCH 39/52] feat: delete_point by providing and index --- .../trajectory/CartesianTrajectory.hpp | 4 +- .../trajectory/JointTrajectory.hpp | 4 +- .../trajectory/TrajectoryBase.hpp | 38 ++++++++++++++----- .../src/trajectory/CartesianTrajectory.cpp | 4 +- .../src/trajectory/JointTrajectory.cpp | 4 +- 5 files changed, 36 insertions(+), 18 deletions(-) diff --git a/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp b/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp index e943bcd5b..8a757e991 100644 --- a/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp +++ b/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp @@ -81,9 +81,9 @@ class CartesianTrajectory : public TrajectoryBase { * already existing points * @param new_point the new trajectory point * @param duration the duration for the new point - * @param pos the desired position of the new point in the queue + * @param index the desired position of the new point in the queue */ - void insert_point(const CartesianState& new_point, const std::chrono::nanoseconds& duration, unsigned int pos); + void insert_point(const CartesianState& new_point, const std::chrono::nanoseconds& duration, unsigned int index); /** * @brief Get list of trajectory points diff --git a/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp b/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp index fae7bfe11..c21f7a2ca 100644 --- a/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp +++ b/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp @@ -74,9 +74,9 @@ class JointTrajectory : public TrajectoryBase { * already existing points * @param new_point the new trajectory point * @param duration the duration for the new point - * @param pos the desired position of the new point in the queue + * @param index the desired position of the new point in the queue */ - void insert_point(const JointState& new_point, const std::chrono::nanoseconds& duration, unsigned int pos); + void insert_point(const JointState& new_point, const std::chrono::nanoseconds& duration, unsigned int index); /** * @brief Get list of trajectory points diff --git a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp index be5a067dd..21480fc21 100644 --- a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp +++ b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp @@ -43,14 +43,21 @@ class TrajectoryBase : public State { unsigned int get_size() const; /** - * @brief Reset trajectory + * @brief Delete the last point from trajectory */ - virtual void reset(); + void delete_point(); /** * @brief Delete the last point from trajectory + * @param index the index of the point to delete + * @throw std::out_of_range if index is out of range + */ + void delete_point(unsigned int index); + + /** + * @brief Reset trajectory */ - virtual void delete_point(); + virtual void reset(); protected: /** @@ -74,7 +81,7 @@ class TrajectoryBase : public State { * @brief Get the trajectory point at given index * @param index the index * @return the trajectory point - * @throw std::out_of_range if pos is out of range + * @throw std::out_of_range if index is out of range */ const TrajectoryT& get_point(unsigned int index) const; @@ -93,10 +100,10 @@ class TrajectoryBase : public State { /** * @brief Insert new trajectory point between two already existing points * @param new_point the new point - * @param pos the desired position of the new point in the queue - * @throw std::out_of_range if pos is out of range + * @param index the desired position of the new point in the queue + * @throw std::out_of_range if index is out of range */ - void insert_point(const TrajectoryT& new_point, unsigned int pos); + void insert_point(const TrajectoryT& new_point, unsigned int index); /** * @brief Get the trajectory point at given index @@ -174,13 +181,13 @@ inline void TrajectoryBase::add_points(const std::vector -inline void TrajectoryBase::insert_point(const TrajectoryT& new_point, unsigned int pos) { - if (pos > this->points_.size()) { +inline void TrajectoryBase::insert_point(const TrajectoryT& new_point, unsigned int index) { + if (index > this->points_.size()) { throw std::out_of_range("Index out of range"); } this->set_empty(false); auto it_points = this->points_.begin(); - std::advance(it_points, pos); + std::advance(it_points, index); this->points_.insert(it_points, new_point); } @@ -194,6 +201,17 @@ inline void TrajectoryBase::delete_point() { } } +template +inline void TrajectoryBase::delete_point(unsigned int index) { + if (index >= this->points_.size()) { + throw std::out_of_range("Index out of range"); + } + this->points_.erase(this->points_.begin() + index); + if (this->points_.empty()) { + this->set_empty(false); + } +} + template inline const std::deque& TrajectoryBase::get_points() const { return this->points_; diff --git a/source/state_representation/src/trajectory/CartesianTrajectory.cpp b/source/state_representation/src/trajectory/CartesianTrajectory.cpp index 007448ae5..e7cd83620 100644 --- a/source/state_representation/src/trajectory/CartesianTrajectory.cpp +++ b/source/state_representation/src/trajectory/CartesianTrajectory.cpp @@ -72,7 +72,7 @@ void CartesianTrajectory::add_points( } void CartesianTrajectory::insert_point( - const CartesianState& new_point, const std::chrono::nanoseconds& duration, unsigned int pos + const CartesianState& new_point, const std::chrono::nanoseconds& duration, unsigned int index ) { if (new_point.is_empty()) { throw exceptions::EmptyStateException("Point is empty"); @@ -91,7 +91,7 @@ void CartesianTrajectory::insert_point( trajectory_point.duration = duration; trajectory_point.name = new_point.get_name(); try { - this->TrajectoryBase::insert_point(trajectory_point, pos); + this->TrajectoryBase::insert_point(trajectory_point, index); } catch (...) { throw; } diff --git a/source/state_representation/src/trajectory/JointTrajectory.cpp b/source/state_representation/src/trajectory/JointTrajectory.cpp index f2d6e81cd..fddf561f1 100644 --- a/source/state_representation/src/trajectory/JointTrajectory.cpp +++ b/source/state_representation/src/trajectory/JointTrajectory.cpp @@ -63,7 +63,7 @@ void JointTrajectory::add_points( } void JointTrajectory::insert_point( - const JointState& new_point, const std::chrono::nanoseconds& duration, unsigned int pos + const JointState& new_point, const std::chrono::nanoseconds& duration, unsigned int index ) { if (new_point.is_empty()) { throw exceptions::EmptyStateException("Point is empty"); @@ -83,7 +83,7 @@ void JointTrajectory::insert_point( JointTrajectoryPoint trajectory_point; trajectory_point.data = new_point.data(); trajectory_point.duration = duration; - this->TrajectoryBase::insert_point(trajectory_point, pos); + this->TrajectoryBase::insert_point(trajectory_point, index); } catch (...) { throw; } From f0ffbcc5bd4ae6292347337133ca54d8e50c52cc Mon Sep 17 00:00:00 2001 From: bpapaspyros Date: Wed, 29 Jan 2025 12:46:36 +0100 Subject: [PATCH 40/52] fix: use vectors instead of queues in getters --- .../trajectory/CartesianTrajectory.hpp | 2 +- .../trajectory/JointTrajectory.hpp | 4 ++-- .../trajectory/TrajectoryBase.hpp | 18 +++++++++--------- .../src/trajectory/CartesianTrajectory.cpp | 4 ++-- .../src/trajectory/JointTrajectory.cpp | 4 ++-- 5 files changed, 16 insertions(+), 16 deletions(-) diff --git a/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp b/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp index 8a757e991..41f8fcb86 100644 --- a/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp +++ b/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp @@ -89,7 +89,7 @@ class CartesianTrajectory : public TrajectoryBase { * @brief Get list of trajectory points * @return queue of the Cartesian states of the trajectory */ - const std::deque get_points() const; + const std::vector get_points() const; /** * @brief Get the trajectory point at given index diff --git a/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp b/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp index c21f7a2ca..e775bc916 100644 --- a/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp +++ b/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp @@ -80,9 +80,9 @@ class JointTrajectory : public TrajectoryBase { /** * @brief Get list of trajectory points - * @return queue of the Joint states of the trajectory + * @return vector of the Joint states of the trajectory */ - const std::deque get_points() const; + const std::vector get_points() const; /** * @brief Get the trajectory point at given index diff --git a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp index 21480fc21..1ecbf36f5 100644 --- a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp +++ b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp @@ -28,13 +28,13 @@ class TrajectoryBase : public State { * @brief Get list of trajectory point durations * @return the list of trajectory point durations */ - const std::deque get_durations() const; + const std::vector get_durations() const; /** * @brief Get list of trajectory point times from start * @return the list of trajectory point times from start */ - const std::deque get_times_from_start() const; + const std::vector get_times_from_start() const; /** * @brief Get number of points in trajectory @@ -75,7 +75,7 @@ class TrajectoryBase : public State { * @brief Get list of trajectory points * @return the list of trajectory points */ - const std::deque& get_points() const; + const std::vector get_points() const; /** * @brief Get the trajectory point at given index @@ -213,8 +213,8 @@ inline void TrajectoryBase::delete_point(unsigned int index) { } template -inline const std::deque& TrajectoryBase::get_points() const { - return this->points_; +inline const std::vector TrajectoryBase::get_points() const { + return std::vector(this->points_.begin(), this->points_.end()); } template @@ -253,8 +253,8 @@ inline void TrajectoryBase::set_points(const std::vector -inline const std::deque TrajectoryBase::get_durations() const { - std::deque durations; +inline const std::vector TrajectoryBase::get_durations() const { + std::vector durations; for (unsigned int i = 0; i < this->points_.size(); ++i) { durations.push_back(this->points_[i].duration); } @@ -262,8 +262,8 @@ inline const std::deque TrajectoryBase::g } template -inline const std::deque TrajectoryBase::get_times_from_start() const { - std::deque times_from_start; +inline const std::vector TrajectoryBase::get_times_from_start() const { + std::vector times_from_start; std::chrono::nanoseconds time_from_start = std::chrono::nanoseconds(0); for (unsigned int i = 0; i < this->points_.size(); ++i) { time_from_start += this->points_[i].duration; diff --git a/source/state_representation/src/trajectory/CartesianTrajectory.cpp b/source/state_representation/src/trajectory/CartesianTrajectory.cpp index e7cd83620..0365effda 100644 --- a/source/state_representation/src/trajectory/CartesianTrajectory.cpp +++ b/source/state_representation/src/trajectory/CartesianTrajectory.cpp @@ -154,8 +154,8 @@ void CartesianTrajectory::set_points( } } -const std::deque CartesianTrajectory::get_points() const { - std::deque points; +const std::vector CartesianTrajectory::get_points() const { + std::vector points; for (unsigned int i = 0; i < this->get_size(); ++i) { auto state = this->operator[](i); points.push_back(state.first); diff --git a/source/state_representation/src/trajectory/JointTrajectory.cpp b/source/state_representation/src/trajectory/JointTrajectory.cpp index fddf561f1..8e5a5399e 100644 --- a/source/state_representation/src/trajectory/JointTrajectory.cpp +++ b/source/state_representation/src/trajectory/JointTrajectory.cpp @@ -153,8 +153,8 @@ const std::vector& JointTrajectory::get_joint_names() const { return this->joint_names_; } -const std::deque JointTrajectory::get_points() const { - std::deque points; +const std::vector JointTrajectory::get_points() const { + std::vector points; for (unsigned int i = 0; i < this->get_size(); ++i) { auto state = this->operator[](i); points.push_back(state.first); From 8610d36966d8540fbf1d7ffa8b9fa10827e63356 Mon Sep 17 00:00:00 2001 From: bpapaspyros Date: Wed, 29 Jan 2025 12:54:51 +0100 Subject: [PATCH 41/52] fix: rearrange constructor arguments --- .../trajectory/CartesianTrajectory.hpp | 6 +++--- .../trajectory/JointTrajectory.hpp | 12 +++++------- .../src/trajectory/CartesianTrajectory.cpp | 9 +++------ .../src/trajectory/JointTrajectory.cpp | 9 +++------ .../test/tests/test_trajectory.cpp | 8 ++++---- 5 files changed, 18 insertions(+), 26 deletions(-) diff --git a/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp b/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp index 41f8fcb86..10cb17d91 100644 --- a/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp +++ b/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp @@ -34,7 +34,7 @@ class CartesianTrajectory : public TrajectoryBase { * @param reference_frame reference frame of the trajectory points */ explicit CartesianTrajectory( - const CartesianState& point, const std::chrono::nanoseconds& duration, const std::string& name = "", + const std::string& name, const CartesianState& point, const std::chrono::nanoseconds& duration, const std::string& reference_frame = "world" ); @@ -46,8 +46,8 @@ class CartesianTrajectory : public TrajectoryBase { * @param reference_frame reference frame of the trajectory points */ explicit CartesianTrajectory( - const std::vector& points, const std::vector& durations, - const std::string& name = "", const std::string& reference_frame = "world" + const std::string& name, const std::vector& points, + const std::vector& durations, const std::string& reference_frame = "world" ); /** diff --git a/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp b/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp index e775bc916..79449ed1c 100644 --- a/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp +++ b/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp @@ -25,23 +25,21 @@ class JointTrajectory : public TrajectoryBase { /** * @brief Constructor with initial point, duration, name, and reference frame provided + * @param name the name of the state * @param point the initial point * @param duration the initial duration - * @param name the name of the state */ - explicit JointTrajectory( - const JointState& point, const std::chrono::nanoseconds& duration, const std::string& name = "" - ); + explicit JointTrajectory(const std::string& name, const JointState& point, const std::chrono::nanoseconds& duration); /** * @brief Constructor with initial points, durations, name, and reference frame provided + * @param name the name of the state * @param points vector of initial points * @param durations vector of initial durations - * @param name the name of the state */ explicit JointTrajectory( - const std::vector& points, const std::vector& durations, - const std::string& name = "" + const std::string& name, const std::vector& points, + const std::vector& durations ); /** diff --git a/source/state_representation/src/trajectory/CartesianTrajectory.cpp b/source/state_representation/src/trajectory/CartesianTrajectory.cpp index 0365effda..418cf2f4f 100644 --- a/source/state_representation/src/trajectory/CartesianTrajectory.cpp +++ b/source/state_representation/src/trajectory/CartesianTrajectory.cpp @@ -9,7 +9,6 @@ namespace state_representation { CartesianTrajectory::CartesianTrajectory(const std::string& name, const std::string& reference_frame) : TrajectoryBase(name), reference_frame_(reference_frame) { this->set_type(StateType::CARTESIAN_TRAJECTORY); - this->reset(); } const std::string CartesianTrajectory::get_reference_frame() const { @@ -17,22 +16,20 @@ const std::string CartesianTrajectory::get_reference_frame() const { } CartesianTrajectory::CartesianTrajectory( - const CartesianState& point, const std::chrono::nanoseconds& duration, const std::string& name, + const std::string& name, const CartesianState& point, const std::chrono::nanoseconds& duration, const std::string& reference_frame ) : TrajectoryBase(name), reference_frame_(reference_frame) { this->set_type(StateType::CARTESIAN_TRAJECTORY); - this->reset(); this->add_point(point, duration); } CartesianTrajectory::CartesianTrajectory( - const std::vector& points, const std::vector& durations, - const std::string& name, const std::string& reference_frame + const std::string& name, const std::vector& points, + const std::vector& durations, const std::string& reference_frame ) : TrajectoryBase(name), reference_frame_(reference_frame) { this->set_type(StateType::CARTESIAN_TRAJECTORY); - this->reset(); try { this->set_points(points, durations); } catch (...) { diff --git a/source/state_representation/src/trajectory/JointTrajectory.cpp b/source/state_representation/src/trajectory/JointTrajectory.cpp index 8e5a5399e..e742f2629 100644 --- a/source/state_representation/src/trajectory/JointTrajectory.cpp +++ b/source/state_representation/src/trajectory/JointTrajectory.cpp @@ -8,25 +8,22 @@ namespace state_representation { JointTrajectory::JointTrajectory(const std::string& name) : TrajectoryBase(name) { this->set_type(StateType::JOINT_TRAJECTORY); - this->reset(); } JointTrajectory::JointTrajectory( - const JointState& point, const std::chrono::nanoseconds& duration, const std::string& name + const std::string& name, const JointState& point, const std::chrono::nanoseconds& duration ) : TrajectoryBase(name) { this->set_type(StateType::JOINT_TRAJECTORY); - this->reset(); this->add_point(point, duration); } JointTrajectory::JointTrajectory( - const std::vector& points, const std::vector& durations, - const std::string& name + const std::string& name, const std::vector& points, + const std::vector& durations ) : TrajectoryBase(name) { this->set_type(StateType::JOINT_TRAJECTORY); - this->reset(); this->set_points(points, durations); } diff --git a/source/state_representation/test/tests/test_trajectory.cpp b/source/state_representation/test/tests/test_trajectory.cpp index be7837144..e1a7eba4e 100644 --- a/source/state_representation/test/tests/test_trajectory.cpp +++ b/source/state_representation/test/tests/test_trajectory.cpp @@ -71,7 +71,7 @@ TEST(TrajectoryTest, TestTrajectoryBase) { TEST(TrajectoryTest, CartesianTrajectory) { { - CartesianTrajectory trajectory(CartesianState::Random("foo"), std::chrono::nanoseconds(100)); + CartesianTrajectory trajectory("foo_trajectory", CartesianState::Random("foo"), std::chrono::nanoseconds(100)); EXPECT_EQ(trajectory.get_size(), 1); } @@ -81,7 +81,7 @@ TEST(TrajectoryTest, CartesianTrajectory) { CartesianState::Random("bar"), }; auto durations = {std::chrono::nanoseconds(100), std::chrono::nanoseconds(200)}; - CartesianTrajectory trajectory(points, durations); + CartesianTrajectory trajectory("foo_trajectory", points, durations); EXPECT_EQ(trajectory.get_size(), 2); } @@ -148,7 +148,7 @@ TEST(TrajectoryTest, CartesianTrajectory) { TEST(TrajectoryTest, JointTrajectory) { { - JointTrajectory trajectory(JointState::Random("foo", 25), std::chrono::nanoseconds(100)); + JointTrajectory trajectory("foo_trajectory", JointState::Random("foo", 25), std::chrono::nanoseconds(100)); EXPECT_EQ(trajectory.get_size(), 1); } @@ -158,7 +158,7 @@ TEST(TrajectoryTest, JointTrajectory) { JointState::Random("foo", 25), }; auto durations = {std::chrono::nanoseconds(100), std::chrono::nanoseconds(200)}; - JointTrajectory trajectory(points, durations); + JointTrajectory trajectory("foo_trajectory", points, durations); EXPECT_EQ(trajectory.get_size(), 2); } From 63cb9688bed2145a9571e0f8e2bceb91526135a6 Mon Sep 17 00:00:00 2001 From: bpapaspyros Date: Wed, 29 Jan 2025 17:08:24 +0100 Subject: [PATCH 42/52] refactor: change reference frame/joint names/robot name setting logic --- .../trajectory/CartesianTrajectory.hpp | 15 +-- .../trajectory/JointTrajectory.hpp | 25 +++-- .../trajectory/TrajectoryBase.hpp | 12 ++- .../src/trajectory/CartesianTrajectory.cpp | 100 +++++++++++------- .../src/trajectory/JointTrajectory.cpp | 70 +++++++----- .../test/tests/test_trajectory.cpp | 24 +++-- 6 files changed, 160 insertions(+), 86 deletions(-) diff --git a/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp b/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp index 10cb17d91..38775ffe8 100644 --- a/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp +++ b/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp @@ -1,5 +1,6 @@ #pragma once +#include "state_representation/space/cartesian/CartesianPose.hpp" #include "state_representation/space/cartesian/CartesianState.hpp" #include "state_representation/trajectory/TrajectoryBase.hpp" @@ -54,7 +55,14 @@ class CartesianTrajectory : public TrajectoryBase { * @brief Getter of the reference frame as const reference * @return the reference frame associated with the trajectory */ - const std::string get_reference_frame() const; + const std::string& get_reference_frame() const; + + /** + * @brief Getter of the reference frame as const reference + * @param the reference frame associated with the trajectory + * @throws EmptyStateException if pose is empty + */ + void set_reference_frame(const CartesianPose& pose); /** * @brief Add new point and corresponding duration to trajectory @@ -126,11 +134,6 @@ class CartesianTrajectory : public TrajectoryBase { */ std::pair operator[](unsigned int idx) const; - /** - * @brief Reset trajectory - */ - virtual void reset() override; - private: std::string reference_frame_;///< name of the reference frame }; diff --git a/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp b/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp index 79449ed1c..f16f50bf9 100644 --- a/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp +++ b/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp @@ -43,11 +43,29 @@ class JointTrajectory : public TrajectoryBase { ); /** - * @brief Getter of the names + * @brief Getter of the joint names * @return vector of joint names associated with the trajectory */ const std::vector& get_joint_names() const; + /** + * @brief Setter of the joint names + * @param joint_names vector of joint names associated with the trajectory + */ + void set_joint_names(const std::vector& joint_names); + + /** + * @brief Getter of the robot name + * @return the robot name associated with the trajectory + */ + const std::string& get_robot_name() const; + + /** + * @brief Setter of the robot name + * @param the robot name associated with the trajectory + */ + void set_robot_name(const std::string& robot_name); + /** * @brief Add new point and corresponding duration to trajectory * @param new_point the new trajectory point @@ -117,11 +135,6 @@ class JointTrajectory : public TrajectoryBase { */ std::pair operator[](unsigned int idx) const; - /** - * @brief Reset trajectory - */ - virtual void reset() override; - private: std::vector joint_names_;///< names of the joints std::string robot_name_; ///< name of the robot diff --git a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp index 1ecbf36f5..c307f1950 100644 --- a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp +++ b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp @@ -5,6 +5,7 @@ #include #include "state_representation/State.hpp" +#include "state_representation/exceptions/IncompatibleSizeException.hpp" namespace state_representation { @@ -123,6 +124,7 @@ class TrajectoryBase : public State { /** * @brief Set the trajectory points from a vector of points * @param points vector of new points + * @throw IncompatibleSizeException if points vector is empty */ void set_points(const std::vector& points); @@ -244,11 +246,13 @@ inline void TrajectoryBase::set_point(const TrajectoryT& point, uns template inline void TrajectoryBase::set_points(const std::vector& points) { if (points.size() == 0) { - return; + throw exceptions::IncompatibleSizeException("No points provided"); } - this->reset(); - for (auto point : points) { - this->add_point(point); + if (points.size() != this->points_.size()) { + throw exceptions::IncompatibleSizeException("The size of the current vector and the new vector are not equal"); + } + for (unsigned int i = 0; i < points.size(); ++i) { + this->points_[i] = points[i]; } } diff --git a/source/state_representation/src/trajectory/CartesianTrajectory.cpp b/source/state_representation/src/trajectory/CartesianTrajectory.cpp index 418cf2f4f..e71715a32 100644 --- a/source/state_representation/src/trajectory/CartesianTrajectory.cpp +++ b/source/state_representation/src/trajectory/CartesianTrajectory.cpp @@ -3,7 +3,6 @@ #include "state_representation/exceptions/EmptyStateException.hpp" #include "state_representation/exceptions/IncompatibleReferenceFramesException.hpp" -#include "state_representation/exceptions/IncompatibleSizeException.hpp" namespace state_representation { CartesianTrajectory::CartesianTrajectory(const std::string& name, const std::string& reference_frame) @@ -11,16 +10,40 @@ CartesianTrajectory::CartesianTrajectory(const std::string& name, const std::str this->set_type(StateType::CARTESIAN_TRAJECTORY); } -const std::string CartesianTrajectory::get_reference_frame() const { +const std::string& CartesianTrajectory::get_reference_frame() const { return this->reference_frame_; } +void CartesianTrajectory::set_reference_frame(const CartesianPose& pose) { + if (pose.is_empty()) { + throw exceptions::EmptyStateException("Pose is empty"); + } + this->reference_frame_ = pose.get_reference_frame(); + auto points = this->get_points(); + for (auto& point : points) { + point *= pose; + } + try { + this->set_points(points, this->get_durations()); + } catch (...) { + throw; + } +} + CartesianTrajectory::CartesianTrajectory( const std::string& name, const CartesianState& point, const std::chrono::nanoseconds& duration, const std::string& reference_frame ) : TrajectoryBase(name), reference_frame_(reference_frame) { this->set_type(StateType::CARTESIAN_TRAJECTORY); + if (point.is_empty()) { + throw exceptions::EmptyStateException("The Cartesian state provided is empty"); + } else if (point.get_reference_frame() != reference_frame) { + throw exceptions::IncompatibleReferenceFramesException( + "Incompatible reference frames: " + point.get_reference_frame() + " and " + reference_frame + ); + } + this->reference_frame_ = reference_frame; this->add_point(point, duration); } @@ -30,8 +53,20 @@ CartesianTrajectory::CartesianTrajectory( ) : TrajectoryBase(name), reference_frame_(reference_frame) { this->set_type(StateType::CARTESIAN_TRAJECTORY); + for (unsigned int i = 1; i < points.size(); ++i) { + if (points[i - 1].is_empty()) { + throw exceptions::EmptyStateException("Vector contains at least one point that is empty"); + } else if (points[i - 1].get_reference_frame() != points[i].get_reference_frame()) { + throw exceptions::IncompatibleReferenceFramesException( + "Incompatible reference frames within the new points vector" + ); + } + } + if (points.size() > 0) { + this->reference_frame_ = points[0].get_reference_frame(); + } try { - this->set_points(points, durations); + this->add_points(points, durations); } catch (...) { throw; } @@ -40,15 +75,10 @@ CartesianTrajectory::CartesianTrajectory( void CartesianTrajectory::add_point(const CartesianState& new_point, const std::chrono::nanoseconds& duration) { if (new_point.is_empty()) { throw exceptions::EmptyStateException("The Cartesian state provided is empty"); - } - if (this->get_size() > 0) { - if (new_point.get_reference_frame() != this->reference_frame_) { - throw exceptions::IncompatibleReferenceFramesException( - "Incompatible reference frames: " + new_point.get_reference_frame() + " and " + this->reference_frame_ - ); - } - } else { - this->reference_frame_ = new_point.get_reference_frame(); + } else if (new_point.get_reference_frame() != this->reference_frame_) { + throw exceptions::IncompatibleReferenceFramesException( + "Incompatible reference frames: " + new_point.get_reference_frame() + " and " + this->reference_frame_ + ); } CartesianTrajectoryPoint trajectory_point; trajectory_point.data = new_point.data(); @@ -63,6 +93,15 @@ void CartesianTrajectory::add_points( if (new_points.size() != durations.size()) { throw exceptions::IncompatibleSizeException("The size of the points and durations vectors are not equal"); } + for (unsigned int i = 1; i < new_points.size(); ++i) { + if (new_points[i - 1].is_empty()) { + throw exceptions::EmptyStateException("Vector contains at least one point that is empty"); + } else if (new_points[i - 1].get_reference_frame() != new_points[i].get_reference_frame()) { + throw exceptions::IncompatibleReferenceFramesException( + "Incompatible reference frames within the new points vector" + ); + } + } for (unsigned int i = 0; i < new_points.size(); ++i) { add_point(new_points[i], durations[i]); } @@ -73,15 +112,10 @@ void CartesianTrajectory::insert_point( ) { if (new_point.is_empty()) { throw exceptions::EmptyStateException("Point is empty"); - } - if (this->get_size() > 0) { - if (new_point.get_reference_frame() != this->reference_frame_) { - throw exceptions::IncompatibleReferenceFramesException( - "Incompatible reference frames: " + new_point.get_reference_frame() + " and " + this->reference_frame_ - ); - } - } else if (this->reference_frame_.empty()) { - this->reference_frame_ = new_point.get_reference_frame(); + } else if (new_point.get_reference_frame() != this->reference_frame_) { + throw exceptions::IncompatibleReferenceFramesException( + "Incompatible reference frames: " + new_point.get_reference_frame() + " and " + this->reference_frame_ + ); } CartesianTrajectoryPoint trajectory_point; trajectory_point.data = new_point.data(); @@ -99,8 +133,7 @@ void CartesianTrajectory::set_point( ) { if (point.is_empty()) { throw exceptions::EmptyStateException("Point is empty"); - } - if (point.get_reference_frame() != this->reference_frame_) { + } else if (point.get_reference_frame() != this->reference_frame_) { throw exceptions::IncompatibleReferenceFramesException( "Incompatible reference frames: " + point.get_reference_frame() + " and " + this->reference_frame_ ); @@ -119,22 +152,21 @@ void CartesianTrajectory::set_point( void CartesianTrajectory::set_points( const std::vector& points, const std::vector& durations ) { - if (points.size() != durations.size()) { - throw exceptions::IncompatibleSizeException("The size of the points and durations vectors are not equal"); - } if (points.empty()) { - this->reset(); - return; + throw exceptions::IncompatibleSizeException("No points provided"); + } else if (points.size() != this->get_size()) { + throw exceptions::IncompatibleSizeException("The size of the current vector and the new vector are not equal"); + } else if (points.size() != durations.size()) { + throw exceptions::IncompatibleSizeException("The size of the points and durations vectors are not equal"); } - std::string candidate_reference_frame = points[0].get_reference_frame(); std::vector trajectory_points; for (unsigned int i = 0; i < points.size(); ++i) { if (points[i].is_empty()) { throw exceptions::EmptyStateException("Vector contains at least one point that is empty"); - } else if (points[i].get_reference_frame() != candidate_reference_frame) { + } else if (points[i].get_reference_frame() != this->reference_frame_) { throw exceptions::IncompatibleReferenceFramesException( - "Incompatible reference frames: " + points[i].get_reference_frame() + " and " + candidate_reference_frame + "Incompatible reference frames: " + points[i].get_reference_frame() + " and " + this->reference_frame_ ); } CartesianTrajectoryPoint trajectory_point; @@ -145,7 +177,6 @@ void CartesianTrajectory::set_points( } try { this->TrajectoryBase::set_points(trajectory_points); - this->reference_frame_ = candidate_reference_frame; } catch (...) { throw; } @@ -179,9 +210,4 @@ std::pair CartesianTrajectory::o throw; } } - -void CartesianTrajectory::reset() { - this->TrajectoryBase::reset(); - this->reference_frame_ = "world"; -} }// namespace state_representation diff --git a/source/state_representation/src/trajectory/JointTrajectory.cpp b/source/state_representation/src/trajectory/JointTrajectory.cpp index e742f2629..4d345678a 100644 --- a/source/state_representation/src/trajectory/JointTrajectory.cpp +++ b/source/state_representation/src/trajectory/JointTrajectory.cpp @@ -15,6 +15,11 @@ JointTrajectory::JointTrajectory( ) : TrajectoryBase(name) { this->set_type(StateType::JOINT_TRAJECTORY); + if (point.is_empty()) { + throw exceptions::EmptyStateException("Point is empty"); + } + this->joint_names_ = point.get_names(); + this->robot_name_ = point.get_name(); this->add_point(point, duration); } @@ -24,16 +29,25 @@ JointTrajectory::JointTrajectory( ) : TrajectoryBase(name) { this->set_type(StateType::JOINT_TRAJECTORY); - this->set_points(points, durations); + for (unsigned int i = 1; i < points.size(); ++i) { + if (points[i - 1].is_empty()) { + throw exceptions::EmptyStateException("Vector contains at least one point that is empty"); + } else if (points[i - 1].get_names() != points[i].get_names()) { + throw exceptions::IncompatibleStatesException("Incompatible joint names within the new points vector"); + } else if (points[i - 1].get_name() != points[i].get_name()) { + throw exceptions::IncompatibleStatesException("Incompatible robot name within the new points vector"); + } + } + if (points.size() > 0) { + this->joint_names_ = points[0].get_names(); + this->robot_name_ = points[0].get_name(); + } + this->add_points(points, durations); } void JointTrajectory::add_point(const JointState& new_point, const std::chrono::nanoseconds& duration) { if (new_point.is_empty()) { throw exceptions::EmptyStateException("Point is empty"); - } - if (this->get_size() == 0) { - this->joint_names_ = new_point.get_names(); - this->robot_name_ = new_point.get_name(); } else if (this->joint_names_ != new_point.get_names()) { throw exceptions::IncompatibleStatesException( "Incompatible joint names between the new point and current trajectory" @@ -54,6 +68,15 @@ void JointTrajectory::add_points( if (new_points.size() != durations.size()) { throw exceptions::IncompatibleSizeException("The size of the points and durations vectors are not equal"); } + for (unsigned int i = 1; i < new_points.size(); ++i) { + if (new_points[i - 1].is_empty()) { + throw exceptions::EmptyStateException("Vector contains at least one point that is empty"); + } else if (new_points[i - 1].get_names() != new_points[i].get_names()) { + throw exceptions::IncompatibleStatesException("Incompatible joint names within the new points vector"); + } else if (new_points[i - 1].get_name() != new_points[i].get_name()) { + throw exceptions::IncompatibleStatesException("Incompatible robot name within the new points vector"); + } + } for (unsigned int i = 0; i < new_points.size(); ++i) { add_point(new_points[i], durations[i]); } @@ -64,10 +87,6 @@ void JointTrajectory::insert_point( ) { if (new_point.is_empty()) { throw exceptions::EmptyStateException("Point is empty"); - } - if (this->get_size() == 0) { - this->joint_names_ = new_point.get_names(); - this->robot_name_ = new_point.get_name(); } else if (this->joint_names_ != new_point.get_names()) { throw exceptions::IncompatibleStatesException( "Incompatible joint names between the new point and current trajectory" @@ -110,24 +129,23 @@ void JointTrajectory::set_point(const JointState& point, const std::chrono::nano void JointTrajectory::set_points( const std::vector& points, const std::vector& durations ) { - if (points.size() != durations.size()) { - throw exceptions::IncompatibleSizeException("The size of the points and durations vectors are not equal"); - } if (points.empty()) { throw exceptions::EmptyStateException("Points vector is empty"); + } else if (points.size() != this->get_size()) { + throw exceptions::IncompatibleSizeException("The size of the current vector and the new vector are not equal"); + } else if (points.size() != durations.size()) { + throw exceptions::IncompatibleSizeException("The size of the points and durations vectors are not equal"); } - auto candidate_robot_name = points[0].get_name(); - auto candidate_joint_names = points[0].get_names(); std::vector trajectory_points; for (unsigned int i = 0; i < points.size(); ++i) { if (points[i].is_empty()) { throw exceptions::EmptyStateException("Vector contains at least one point that is empty"); - } else if (points[i].get_names() != candidate_joint_names) { + } else if (points[i].get_names() != this->joint_names_) { throw exceptions::IncompatibleStatesException( "Incompatible joint names between the new point and current trajectory" ); - } else if (points[i].get_name() != candidate_robot_name) { + } else if (points[i].get_name() != this->robot_name_) { throw exceptions::IncompatibleStatesException( "Incompatible robot name between the new point and current trajectory" ); @@ -139,8 +157,6 @@ void JointTrajectory::set_points( } try { this->TrajectoryBase::set_points(trajectory_points); - this->joint_names_ = candidate_joint_names; - this->robot_name_ = candidate_robot_name; } catch (...) { throw; } @@ -150,6 +166,18 @@ const std::vector& JointTrajectory::get_joint_names() const { return this->joint_names_; } +void JointTrajectory::set_joint_names(const std::vector& joint_names) { + this->joint_names_ = joint_names; +} + +const std::string& JointTrajectory::get_robot_name() const { + return this->robot_name_; +} + +void JointTrajectory::set_robot_name(const std::string& robot_name) { + this->robot_name_ = robot_name; +} + const std::vector JointTrajectory::get_points() const { std::vector points; for (unsigned int i = 0; i < this->get_size(); ++i) { @@ -178,10 +206,4 @@ std::pair JointTrajectory::operator[ throw; } } - -void JointTrajectory::reset() { - this->TrajectoryBase::reset(); - this->robot_name_ = ""; - this->joint_names_.clear(); -} }// namespace state_representation diff --git a/source/state_representation/test/tests/test_trajectory.cpp b/source/state_representation/test/tests/test_trajectory.cpp index e1a7eba4e..6b5a140cb 100644 --- a/source/state_representation/test/tests/test_trajectory.cpp +++ b/source/state_representation/test/tests/test_trajectory.cpp @@ -92,28 +92,29 @@ TEST(TrajectoryTest, CartesianTrajectory) { CartesianState state; EXPECT_THROW(trajectory.add_point(state, std::chrono::nanoseconds(100)), exceptions::EmptyStateException); EXPECT_EQ(trajectory.get_size(), 0); + EXPECT_NO_THROW(trajectory.add_point(CartesianState::Random("foo"), std::chrono::nanoseconds(100))); - auto points = { - CartesianState::Random("foo"), - CartesianState::Random("bar"), - }; + auto points = {CartesianState::Random("bar")}; auto durations = {std::chrono::nanoseconds(100)}; + EXPECT_NO_THROW(trajectory.set_points(points, durations)); + + EXPECT_NO_THROW(trajectory.add_point(CartesianState::Random("foo"), std::chrono::nanoseconds(100))); EXPECT_THROW(trajectory.set_points(points, durations), exceptions::IncompatibleSizeException); - EXPECT_EQ(trajectory.get_size(), 0); + EXPECT_EQ(trajectory.get_size(), 2); } {// incompatible reference frames CartesianState point0("empty", "world1"); - auto point1 = CartesianState::Random("foo", "world2"); + auto point1 = CartesianState::Random("foo", "world"); auto point2 = CartesianState::Random("bar", "world3"); EXPECT_THROW(trajectory.add_point(point0, std::chrono::nanoseconds(100)), exceptions::EmptyStateException); EXPECT_NO_THROW(trajectory.add_point(point1, std::chrono::nanoseconds(200))); EXPECT_THROW( trajectory.add_point(point2, std::chrono::nanoseconds(200)), exceptions::IncompatibleReferenceFramesException ); - EXPECT_STREQ(trajectory.get_reference_frame().c_str(), "world2"); - trajectory.reset(); EXPECT_STREQ(trajectory.get_reference_frame().c_str(), "world"); + trajectory.reset(); + EXPECT_EQ(trajectory.get_size(), 0); } auto point0 = CartesianState::Random("foo"); @@ -168,6 +169,8 @@ TEST(TrajectoryTest, JointTrajectory) { {// incompatible name auto point0 = JointState::Random("foo", 25); auto point1 = JointState::Random("bar", 25); + trajectory.set_joint_names(point0.get_names()); + trajectory.set_robot_name(point0.get_name()); EXPECT_NO_THROW(trajectory.add_point(point0, std::chrono::nanoseconds(100))); EXPECT_THROW(trajectory.add_point(point1, std::chrono::nanoseconds(200)), exceptions::IncompatibleStatesException); trajectory.reset(); @@ -192,11 +195,13 @@ TEST(TrajectoryTest, JointTrajectory) { JointState::Random("foo", {"j_foo_1", "j_foo_2"}), JointState::Random("foo", {"j_bar_1", "j_bar_2"}) }; auto durations = {std::chrono::nanoseconds(100), std::chrono::nanoseconds(200)}; - EXPECT_THROW(trajectory.set_points(states, durations), exceptions::IncompatibleStatesException); + EXPECT_THROW(trajectory.set_points(states, durations), exceptions::IncompatibleSizeException); } {// check joint names std::vector joint_names = {"j_foo_1", "j_foo_2", "j_foo_3"}; + trajectory.reset(); + trajectory.set_joint_names(joint_names); trajectory.add_point(JointState::Random("foo", joint_names), std::chrono::nanoseconds(100)); EXPECT_EQ(trajectory.get_joint_names(), joint_names); trajectory.reset(); @@ -206,6 +211,7 @@ TEST(TrajectoryTest, JointTrajectory) { auto point0 = JointState::Random("foo", 25); auto point1 = JointState::Random("foo", 25); auto point2 = JointState::Random("foo", 25); + trajectory.set_joint_names(point0.get_names()); trajectory.add_point(point0, std::chrono::nanoseconds(100)); From 1f8b30edcb621909963e918e64d2e43542c099da Mon Sep 17 00:00:00 2001 From: bpapaspyros Date: Wed, 29 Jan 2025 17:09:15 +0100 Subject: [PATCH 43/52] fix: correct flag value when deleting results in empty queue --- .../state_representation/trajectory/TrajectoryBase.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp index c307f1950..5319bf3c3 100644 --- a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp +++ b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp @@ -199,7 +199,7 @@ inline void TrajectoryBase::delete_point() { this->points_.pop_back(); } if (this->points_.empty()) { - this->set_empty(false); + this->set_empty(true); } } @@ -210,7 +210,7 @@ inline void TrajectoryBase::delete_point(unsigned int index) { } this->points_.erase(this->points_.begin() + index); if (this->points_.empty()) { - this->set_empty(false); + this->set_empty(true); } } From 45346794145b6844fc69034c4096a130e74d25ed Mon Sep 17 00:00:00 2001 From: bpapaspyros Date: Thu, 30 Jan 2025 15:41:59 +0100 Subject: [PATCH 44/52] fix: further restructure constructors --- .../trajectory/CartesianTrajectory.hpp | 23 ++++--- .../trajectory/JointTrajectory.hpp | 13 ---- .../trajectory/TrajectoryBase.hpp | 1 + .../src/trajectory/CartesianTrajectory.cpp | 64 +++++++++---------- .../src/trajectory/JointTrajectory.cpp | 36 ++--------- .../test/tests/test_trajectory.cpp | 23 ++----- 6 files changed, 57 insertions(+), 103 deletions(-) diff --git a/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp b/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp index 38775ffe8..53c9146ef 100644 --- a/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp +++ b/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp @@ -10,9 +10,7 @@ namespace state_representation { * @class CartesianTrajectoryPoint * @brief Struct to represent a Cartesian trajectory point */ -struct CartesianTrajectoryPoint : public TrajectoryPoint { - std::string name; -}; +struct CartesianTrajectoryPoint : public TrajectoryPoint {}; /** * @class CartesianTrajectory @@ -20,23 +18,28 @@ struct CartesianTrajectoryPoint : public TrajectoryPoint { */ class CartesianTrajectory : public TrajectoryBase { public: + /** + * @brief Empty constructor + */ + explicit CartesianTrajectory(); + /** * @brief Constructor with name and reference frame provided * @param name the name of the state * @param reference_frame reference frame of the trajectory points */ - explicit CartesianTrajectory(const std::string& name = "", const std::string& reference_frame = "world"); + explicit CartesianTrajectory(const std::string& name, const std::string& reference_frame = "world"); /** * @brief Constructor with initial point, duration, name, and reference frame provided * @param point the initial point * @param duration the initial duration * @param name the name of the state - * @param reference_frame reference frame of the trajectory points + * @throw EmptyStateException if point is empty + * @throw IncompatibleReferenceFramesException if point has different reference frame */ explicit CartesianTrajectory( - const std::string& name, const CartesianState& point, const std::chrono::nanoseconds& duration, - const std::string& reference_frame = "world" + const std::string& name, const CartesianState& point, const std::chrono::nanoseconds& duration ); /** @@ -44,11 +47,13 @@ class CartesianTrajectory : public TrajectoryBase { * @param points vector of initial points * @param durations vector of initial durations * @param name the name of the state - * @param reference_frame reference frame of the trajectory points + * @throw EmptyStateException if any point is empty + * @throw IncompatibleReferenceFramesException if any point has different reference frame from others + * @throw IncompatibleSizeException if points and durations have different sizes */ explicit CartesianTrajectory( const std::string& name, const std::vector& points, - const std::vector& durations, const std::string& reference_frame = "world" + const std::vector& durations ); /** diff --git a/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp b/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp index f16f50bf9..eaf0e6f73 100644 --- a/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp +++ b/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp @@ -54,18 +54,6 @@ class JointTrajectory : public TrajectoryBase { */ void set_joint_names(const std::vector& joint_names); - /** - * @brief Getter of the robot name - * @return the robot name associated with the trajectory - */ - const std::string& get_robot_name() const; - - /** - * @brief Setter of the robot name - * @param the robot name associated with the trajectory - */ - void set_robot_name(const std::string& robot_name); - /** * @brief Add new point and corresponding duration to trajectory * @param new_point the new trajectory point @@ -137,6 +125,5 @@ class JointTrajectory : public TrajectoryBase { private: std::vector joint_names_;///< names of the joints - std::string robot_name_; ///< name of the robot }; }// namespace state_representation diff --git a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp index 5319bf3c3..aa6483530 100644 --- a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp +++ b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp @@ -14,6 +14,7 @@ namespace state_representation { * @brief Struct to represent the base characteristics of a trajectory point */ struct TrajectoryPoint { + std::string name; Eigen::VectorXd data; std::chrono::nanoseconds duration; }; diff --git a/source/state_representation/src/trajectory/CartesianTrajectory.cpp b/source/state_representation/src/trajectory/CartesianTrajectory.cpp index e71715a32..f6f2625f1 100644 --- a/source/state_representation/src/trajectory/CartesianTrajectory.cpp +++ b/source/state_representation/src/trajectory/CartesianTrajectory.cpp @@ -5,53 +5,32 @@ #include "state_representation/exceptions/IncompatibleReferenceFramesException.hpp" namespace state_representation { -CartesianTrajectory::CartesianTrajectory(const std::string& name, const std::string& reference_frame) - : TrajectoryBase(name), reference_frame_(reference_frame) { +CartesianTrajectory::CartesianTrajectory() { this->set_type(StateType::CARTESIAN_TRAJECTORY); } -const std::string& CartesianTrajectory::get_reference_frame() const { - return this->reference_frame_; -} - -void CartesianTrajectory::set_reference_frame(const CartesianPose& pose) { - if (pose.is_empty()) { - throw exceptions::EmptyStateException("Pose is empty"); - } - this->reference_frame_ = pose.get_reference_frame(); - auto points = this->get_points(); - for (auto& point : points) { - point *= pose; - } - try { - this->set_points(points, this->get_durations()); - } catch (...) { - throw; - } +CartesianTrajectory::CartesianTrajectory(const std::string& name, const std::string& reference_frame) + : TrajectoryBase(name), reference_frame_(reference_frame) { + this->set_type(StateType::CARTESIAN_TRAJECTORY); } CartesianTrajectory::CartesianTrajectory( - const std::string& name, const CartesianState& point, const std::chrono::nanoseconds& duration, - const std::string& reference_frame + const std::string& name, const CartesianState& point, const std::chrono::nanoseconds& duration ) - : TrajectoryBase(name), reference_frame_(reference_frame) { + : TrajectoryBase(name) { this->set_type(StateType::CARTESIAN_TRAJECTORY); if (point.is_empty()) { throw exceptions::EmptyStateException("The Cartesian state provided is empty"); - } else if (point.get_reference_frame() != reference_frame) { - throw exceptions::IncompatibleReferenceFramesException( - "Incompatible reference frames: " + point.get_reference_frame() + " and " + reference_frame - ); } - this->reference_frame_ = reference_frame; + this->reference_frame_ = point.get_reference_frame(); this->add_point(point, duration); } CartesianTrajectory::CartesianTrajectory( const std::string& name, const std::vector& points, - const std::vector& durations, const std::string& reference_frame + const std::vector& durations ) - : TrajectoryBase(name), reference_frame_(reference_frame) { + : TrajectoryBase(name) { this->set_type(StateType::CARTESIAN_TRAJECTORY); for (unsigned int i = 1; i < points.size(); ++i) { if (points[i - 1].is_empty()) { @@ -72,6 +51,28 @@ CartesianTrajectory::CartesianTrajectory( } } +const std::string& CartesianTrajectory::get_reference_frame() const { + return this->reference_frame_; +} + +void CartesianTrajectory::set_reference_frame(const CartesianPose& pose) { + if (pose.is_empty()) { + throw exceptions::EmptyStateException("Pose is empty"); + } else if (this->get_size() == 0) { + throw exceptions::EmptyStateException("Trajectory is empty"); + } + this->reference_frame_ = pose.get_reference_frame(); + auto points = this->get_points(); + for (auto& point : points) { + point *= pose; + } + try { + this->set_points(points, this->get_durations()); + } catch (...) { + throw; + } +} + void CartesianTrajectory::add_point(const CartesianState& new_point, const std::chrono::nanoseconds& duration) { if (new_point.is_empty()) { throw exceptions::EmptyStateException("The Cartesian state provided is empty"); @@ -185,8 +186,7 @@ void CartesianTrajectory::set_points( const std::vector CartesianTrajectory::get_points() const { std::vector points; for (unsigned int i = 0; i < this->get_size(); ++i) { - auto state = this->operator[](i); - points.push_back(state.first); + points.push_back(this->operator[](i).first); } return points; } diff --git a/source/state_representation/src/trajectory/JointTrajectory.cpp b/source/state_representation/src/trajectory/JointTrajectory.cpp index 4d345678a..e027c0e33 100644 --- a/source/state_representation/src/trajectory/JointTrajectory.cpp +++ b/source/state_representation/src/trajectory/JointTrajectory.cpp @@ -19,7 +19,6 @@ JointTrajectory::JointTrajectory( throw exceptions::EmptyStateException("Point is empty"); } this->joint_names_ = point.get_names(); - this->robot_name_ = point.get_name(); this->add_point(point, duration); } @@ -34,13 +33,10 @@ JointTrajectory::JointTrajectory( throw exceptions::EmptyStateException("Vector contains at least one point that is empty"); } else if (points[i - 1].get_names() != points[i].get_names()) { throw exceptions::IncompatibleStatesException("Incompatible joint names within the new points vector"); - } else if (points[i - 1].get_name() != points[i].get_name()) { - throw exceptions::IncompatibleStatesException("Incompatible robot name within the new points vector"); } } if (points.size() > 0) { this->joint_names_ = points[0].get_names(); - this->robot_name_ = points[0].get_name(); } this->add_points(points, durations); } @@ -52,13 +48,11 @@ void JointTrajectory::add_point(const JointState& new_point, const std::chrono:: throw exceptions::IncompatibleStatesException( "Incompatible joint names between the new point and current trajectory" ); - } else if (this->robot_name_ != new_point.get_name()) { - throw exceptions::IncompatibleStatesException("Incompatible robot name between the new point and current trajectory" - ); } JointTrajectoryPoint trajectory_point; trajectory_point.data = new_point.data(); trajectory_point.duration = duration; + trajectory_point.name = new_point.get_name(); this->TrajectoryBase::add_point(trajectory_point); } @@ -73,8 +67,6 @@ void JointTrajectory::add_points( throw exceptions::EmptyStateException("Vector contains at least one point that is empty"); } else if (new_points[i - 1].get_names() != new_points[i].get_names()) { throw exceptions::IncompatibleStatesException("Incompatible joint names within the new points vector"); - } else if (new_points[i - 1].get_name() != new_points[i].get_name()) { - throw exceptions::IncompatibleStatesException("Incompatible robot name within the new points vector"); } } for (unsigned int i = 0; i < new_points.size(); ++i) { @@ -91,14 +83,12 @@ void JointTrajectory::insert_point( throw exceptions::IncompatibleStatesException( "Incompatible joint names between the new point and current trajectory" ); - } else if (this->robot_name_ != new_point.get_name()) { - throw exceptions::IncompatibleStatesException("Incompatible robot name between the new point and current trajectory" - ); } try { JointTrajectoryPoint trajectory_point; trajectory_point.data = new_point.data(); trajectory_point.duration = duration; + trajectory_point.name = new_point.get_name(); this->TrajectoryBase::insert_point(trajectory_point, index); } catch (...) { throw; @@ -112,13 +102,11 @@ void JointTrajectory::set_point(const JointState& point, const std::chrono::nano throw exceptions::IncompatibleStatesException( "Incompatible joint names between the new point and current trajectory" ); - } else if (point.get_name() != this->robot_name_) { - throw exceptions::IncompatibleStatesException("Incompatible robot name between the new point and current trajectory" - ); } JointTrajectoryPoint trajectory_point; trajectory_point.data = point.data(); trajectory_point.duration = duration; + trajectory_point.name = point.get_name(); try { this->TrajectoryBase::set_point(trajectory_point, index); } catch (...) { @@ -145,14 +133,11 @@ void JointTrajectory::set_points( throw exceptions::IncompatibleStatesException( "Incompatible joint names between the new point and current trajectory" ); - } else if (points[i].get_name() != this->robot_name_) { - throw exceptions::IncompatibleStatesException( - "Incompatible robot name between the new point and current trajectory" - ); } JointTrajectoryPoint trajectory_point; trajectory_point.data = points[i].data(); trajectory_point.duration = durations[i]; + trajectory_point.name = points[i].get_name(); trajectory_points.push_back(trajectory_point); } try { @@ -170,19 +155,10 @@ void JointTrajectory::set_joint_names(const std::vector& joint_name this->joint_names_ = joint_names; } -const std::string& JointTrajectory::get_robot_name() const { - return this->robot_name_; -} - -void JointTrajectory::set_robot_name(const std::string& robot_name) { - this->robot_name_ = robot_name; -} - const std::vector JointTrajectory::get_points() const { std::vector points; for (unsigned int i = 0; i < this->get_size(); ++i) { - auto state = this->operator[](i); - points.push_back(state.first); + points.push_back(this->operator[](i).first); } return points; } @@ -198,7 +174,7 @@ const JointState JointTrajectory::get_point(unsigned int index) const { std::pair JointTrajectory::operator[](unsigned int idx) const { try { auto point = this->TrajectoryBase::operator[](idx); - JointState state(this->robot_name_, this->joint_names_); + JointState state(point.name, this->joint_names_); state.set_data(point.data); auto duration = point.duration; return std::make_pair(state, duration); diff --git a/source/state_representation/test/tests/test_trajectory.cpp b/source/state_representation/test/tests/test_trajectory.cpp index 6b5a140cb..27ac6e28e 100644 --- a/source/state_representation/test/tests/test_trajectory.cpp +++ b/source/state_representation/test/tests/test_trajectory.cpp @@ -85,7 +85,7 @@ TEST(TrajectoryTest, CartesianTrajectory) { EXPECT_EQ(trajectory.get_size(), 2); } - CartesianTrajectory trajectory; + CartesianTrajectory trajectory("foo_trajectory"); EXPECT_EQ(trajectory.get_size(), 0); {// incompatible sizes @@ -166,16 +166,6 @@ TEST(TrajectoryTest, JointTrajectory) { JointTrajectory trajectory("foo"); EXPECT_EQ(trajectory.get_size(), 0); - {// incompatible name - auto point0 = JointState::Random("foo", 25); - auto point1 = JointState::Random("bar", 25); - trajectory.set_joint_names(point0.get_names()); - trajectory.set_robot_name(point0.get_name()); - EXPECT_NO_THROW(trajectory.add_point(point0, std::chrono::nanoseconds(100))); - EXPECT_THROW(trajectory.add_point(point1, std::chrono::nanoseconds(200)), exceptions::IncompatibleStatesException); - trajectory.reset(); - } - {// incompatible sizes JointState state; EXPECT_THROW(trajectory.add_point(state, std::chrono::nanoseconds(100)), exceptions::EmptyStateException); @@ -192,7 +182,7 @@ TEST(TrajectoryTest, JointTrajectory) { {// incompatible joint names auto states = { - JointState::Random("foo", {"j_foo_1", "j_foo_2"}), JointState::Random("foo", {"j_bar_1", "j_bar_2"}) + JointState::Random("foo", {"j_foo_1", "j_foo_2"}), JointState::Random("bar", {"j_bar_1", "j_bar_2"}) }; auto durations = {std::chrono::nanoseconds(100), std::chrono::nanoseconds(200)}; EXPECT_THROW(trajectory.set_points(states, durations), exceptions::IncompatibleSizeException); @@ -209,8 +199,8 @@ TEST(TrajectoryTest, JointTrajectory) { } auto point0 = JointState::Random("foo", 25); - auto point1 = JointState::Random("foo", 25); - auto point2 = JointState::Random("foo", 25); + auto point1 = JointState::Random("bar", 25); + auto point2 = JointState::Random("baz", 25); trajectory.set_joint_names(point0.get_names()); trajectory.add_point(point0, std::chrono::nanoseconds(100)); @@ -224,11 +214,6 @@ TEST(TrajectoryTest, JointTrajectory) { EXPECT_NO_THROW(trajectory.insert_point(point2, std::chrono::nanoseconds(300), 1)); EXPECT_EQ(trajectory.get_size(), 3); - point0.set_name("foofoo"); - EXPECT_THROW(trajectory.set_point(point0, std::chrono::nanoseconds(50), 0), exceptions::IncompatibleStatesException); - EXPECT_NE(trajectory[0].first.get_name(), point0.get_name()); - point0.set_name("foo"); - std::vector points = {point0, point1, point2}; std::vector durations = { std::chrono::nanoseconds(10), std::chrono::nanoseconds(20), std::chrono::nanoseconds(30) From 739ba8ad08ee15f830f5c432e51acb4680397c7f Mon Sep 17 00:00:00 2001 From: bpapaspyros Date: Thu, 30 Jan 2025 19:16:06 +0100 Subject: [PATCH 45/52] test: clean up and improve test structure and test coverage --- .../test/tests/test_trajectory.cpp | 504 +++++++++++------- 1 file changed, 324 insertions(+), 180 deletions(-) diff --git a/source/state_representation/test/tests/test_trajectory.cpp b/source/state_representation/test/tests/test_trajectory.cpp index 27ac6e28e..2101f583e 100644 --- a/source/state_representation/test/tests/test_trajectory.cpp +++ b/source/state_representation/test/tests/test_trajectory.cpp @@ -1,20 +1,15 @@ -#include "state_representation/exceptions/IncompatibleStatesException.hpp" -#include "state_representation/space/joint/JointState.hpp" +#include +#include +#include +#include + #include "state_representation/trajectory/CartesianTrajectory.hpp" #include "state_representation/trajectory/JointTrajectory.hpp" #include "state_representation/exceptions/EmptyStateException.hpp" #include "state_representation/exceptions/IncompatibleReferenceFramesException.hpp" #include "state_representation/exceptions/IncompatibleSizeException.hpp" -#include "state_representation/space/cartesian/CartesianState.hpp" - -#include -#include - -#include -#include -#include -#include +#include "state_representation/exceptions/IncompatibleStatesException.hpp" using namespace state_representation; @@ -22,208 +17,357 @@ class TrajectoryBaseInterface : public TrajectoryBase { public: using TrajectoryBase::TrajectoryBase; using TrajectoryBase::add_point; + using TrajectoryBase::add_points; + using TrajectoryBase::set_point; + using TrajectoryBase::set_points; using TrajectoryBase::get_point; using TrajectoryBase::get_points; using TrajectoryBase::insert_point; using TrajectoryBase::operator[]; -}; -TEST(TrajectoryTest, TestTrajectoryBase) { - TrajectoryBaseInterface trajectory; - EXPECT_EQ(trajectory.get_size(), 0); - - TrajectoryPoint point0; - TrajectoryPoint point1; - TrajectoryPoint point2; - point0.data = Eigen::VectorXd::Random(3); - point0.duration = std::chrono::nanoseconds(100); - point1.data = Eigen::VectorXd::Random(3); - point1.duration = std::chrono::nanoseconds(200); - point2.data = Eigen::VectorXd::Random(3); - point2.duration = std::chrono::nanoseconds(300); - - trajectory.add_point(point0); - trajectory.add_point(point1); - EXPECT_EQ(trajectory.get_size(), 2); - EXPECT_EQ(trajectory.get_durations().size(), 2); - EXPECT_EQ(trajectory.get_times_from_start().size(), 2); - EXPECT_EQ(trajectory.get_point(0).data, point0.data); - EXPECT_EQ(trajectory.get_point(1).data, point1.data); - EXPECT_EQ(trajectory.get_times_from_start()[1], point0.duration + point1.duration); - - auto points = trajectory.get_points(); - EXPECT_EQ(points.size(), 2); - EXPECT_EQ(points[0].data, point0.data); - EXPECT_EQ(points[1].data, point1.data); - - EXPECT_THROW(trajectory.insert_point(point2, 10), std::out_of_range); - EXPECT_NO_THROW(trajectory.insert_point(point2, 1)); - EXPECT_EQ(trajectory.get_size(), 3); - EXPECT_EQ(trajectory[1].data, point2.data); - EXPECT_EQ(trajectory.get_times_from_start()[1], point0.duration + point2.duration); - EXPECT_EQ(trajectory.get_times_from_start()[2], point0.duration + point2.duration + point1.duration); - - trajectory.reset(); - EXPECT_EQ(trajectory.get_size(), 0); - trajectory.reset(); - EXPECT_TRUE(trajectory.is_empty()); -} + TrajectoryBaseInterface() : TrajectoryBase() {} + TrajectoryBaseInterface(const std::string& name) : TrajectoryBase(name) {} +}; -TEST(TrajectoryTest, CartesianTrajectory) { - { - CartesianTrajectory trajectory("foo_trajectory", CartesianState::Random("foo"), std::chrono::nanoseconds(100)); - EXPECT_EQ(trajectory.get_size(), 1); +template +class TrajectoryTest : public testing::Test { +public: + template + void add_point(PointType point, std::chrono::nanoseconds duration = std::chrono::nanoseconds(100)) { + if constexpr (std::is_same_v) { + this->trajectory->add_point(point); + } else { + this->trajectory->add_point(point, duration); + } } - { - auto points = { - CartesianState::Random("foo"), - CartesianState::Random("bar"), - }; - auto durations = {std::chrono::nanoseconds(100), std::chrono::nanoseconds(200)}; - CartesianTrajectory trajectory("foo_trajectory", points, durations); - EXPECT_EQ(trajectory.get_size(), 2); + template + void add_points(std::vector& points, std::vector& durations) { + if constexpr (std::is_same_v) { + this->trajectory->add_points(points); + } else { + this->trajectory->add_points(points, durations); + } } - CartesianTrajectory trajectory("foo_trajectory"); - EXPECT_EQ(trajectory.get_size(), 0); - - {// incompatible sizes - CartesianState state; - EXPECT_THROW(trajectory.add_point(state, std::chrono::nanoseconds(100)), exceptions::EmptyStateException); - EXPECT_EQ(trajectory.get_size(), 0); - EXPECT_NO_THROW(trajectory.add_point(CartesianState::Random("foo"), std::chrono::nanoseconds(100))); - - auto points = {CartesianState::Random("bar")}; - auto durations = {std::chrono::nanoseconds(100)}; - EXPECT_NO_THROW(trajectory.set_points(points, durations)); - - EXPECT_NO_THROW(trajectory.add_point(CartesianState::Random("foo"), std::chrono::nanoseconds(100))); - EXPECT_THROW(trajectory.set_points(points, durations), exceptions::IncompatibleSizeException); - EXPECT_EQ(trajectory.get_size(), 2); + template + void + insert_point(PointType point, unsigned int index, std::chrono::nanoseconds duration = std::chrono::nanoseconds(100)) { + if constexpr (std::is_same_v) { + this->trajectory->insert_point(point, index); + } else { + this->trajectory->insert_point(point, duration, index); + } } - {// incompatible reference frames - CartesianState point0("empty", "world1"); - auto point1 = CartesianState::Random("foo", "world"); - auto point2 = CartesianState::Random("bar", "world3"); - EXPECT_THROW(trajectory.add_point(point0, std::chrono::nanoseconds(100)), exceptions::EmptyStateException); - EXPECT_NO_THROW(trajectory.add_point(point1, std::chrono::nanoseconds(200))); - EXPECT_THROW( - trajectory.add_point(point2, std::chrono::nanoseconds(200)), exceptions::IncompatibleReferenceFramesException - ); - EXPECT_STREQ(trajectory.get_reference_frame().c_str(), "world"); - trajectory.reset(); - EXPECT_EQ(trajectory.get_size(), 0); + template + void + set_point(PointType point, unsigned int index, std::chrono::nanoseconds duration = std::chrono::nanoseconds(100)) { + if constexpr (std::is_same_v) { + this->trajectory->set_point(point, index); + } else { + this->trajectory->set_point(point, duration, index); + } } - auto point0 = CartesianState::Random("foo"); - auto point1 = CartesianState::Random("bar"); - auto point2 = CartesianState::Random("baz"); + template + void set_points(std::vector& points, std::vector& durations) { + if constexpr (std::is_same_v) { + this->trajectory->set_points(points); + } else { + this->trajectory->set_points(points, durations); + } + } - trajectory.add_point(point0, std::chrono::nanoseconds(100)); - EXPECT_EQ(trajectory[0].first.data(), point0.data()); - EXPECT_EQ(trajectory[0].first.get_name(), point0.get_name()); - EXPECT_EQ(trajectory[0].first.get_reference_frame(), point0.get_reference_frame()); - trajectory.add_point(point2, std::chrono::nanoseconds(200)); + void delete_point() { this->trajectory->delete_point(); } + + void delete_point_index(unsigned int index) { this->trajectory->delete_point(index); } + + template + void + expect_equal(PointType point, unsigned int index, std::chrono::nanoseconds duration = std::chrono::nanoseconds(100)) { + if constexpr (std::is_same_v) { + EXPECT_EQ(this->trajectory->operator[](index).data, point.data); + EXPECT_EQ(this->trajectory->operator[](index).duration, point.duration); + } else { + EXPECT_EQ(this->trajectory->operator[](index).first.data(), point.data()); + EXPECT_EQ(this->trajectory->operator[](index).first.get_name(), point.get_name()); + if constexpr (std::is_same_v) { + EXPECT_EQ(this->trajectory->operator[](index).first.get_reference_frame(), point.get_reference_frame()); + } else if constexpr (std::is_same_v) { + EXPECT_EQ(this->trajectory->operator[](index).first.get_names(), point.get_names()); + } + EXPECT_EQ(this->trajectory->operator[](index).second, duration); + } + } - EXPECT_NO_THROW(trajectory.insert_point(point2, std::chrono::nanoseconds(300), 1)); - EXPECT_EQ(trajectory.get_size(), 3); + std::shared_ptr trajectory; +}; +TYPED_TEST_SUITE_P(TrajectoryTest); + +TEST(TrajectoryTest, ConstructTrajectory) { + // Base class + EXPECT_NO_THROW(TrajectoryBaseInterface trajectory); + EXPECT_NO_THROW(TrajectoryBaseInterface trajectory("foo")); + + // Cartesian trajectory + EXPECT_NO_THROW(CartesianTrajectory trajectory("foo")); + EXPECT_NO_THROW(CartesianTrajectory trajectory("foo", CartesianState::Random("foo"), std::chrono::nanoseconds(100))); + EXPECT_NO_THROW( + CartesianTrajectory trajectory("foo", {CartesianState::Random("foo")}, {std::chrono::nanoseconds(100)}) + ); + + EXPECT_THROW( + CartesianTrajectory trajectory("foo", CartesianState(), std::chrono::nanoseconds(100)), + exceptions::EmptyStateException + ); + EXPECT_THROW( + CartesianTrajectory trajectory( + "foo", {CartesianState::Random("foo", "some_world"), CartesianState::Random("foo")}, + {std::chrono::nanoseconds(100), std::chrono::nanoseconds(200)} + ), + exceptions::IncompatibleReferenceFramesException + ); + EXPECT_THROW( + CartesianTrajectory trajectory( + "foo", {CartesianState::Random("foo"), CartesianState::Random("foo")}, {std::chrono::nanoseconds(100)} + ), + exceptions::IncompatibleSizeException + ); + + // Joint trajectory + EXPECT_NO_THROW(JointTrajectory trajectory("foo")); + EXPECT_NO_THROW(JointTrajectory trajectory("foo", JointState::Random("foo", 25), std::chrono::nanoseconds(100))); + EXPECT_NO_THROW(JointTrajectory trajectory("foo", {JointState::Random("foo", 25)}, {std::chrono::nanoseconds(100)})); + + EXPECT_THROW( + JointTrajectory trajectory("foo", JointState(), std::chrono::nanoseconds(100)), exceptions::EmptyStateException + ); + EXPECT_THROW( + JointTrajectory trajectory( + "foo", {JointState::Random("foo", 25), JointState::Random("foo", 24)}, + {std::chrono::nanoseconds(100), std::chrono::nanoseconds(200)} + ), + exceptions::IncompatibleStatesException + ); + EXPECT_THROW( + JointTrajectory trajectory( + "foo", {JointState::Random("foo", 25), JointState::Random("foo", 25)}, {std::chrono::nanoseconds(100)} + ), + exceptions::IncompatibleSizeException + ); +} - point0.set_name("foofoo"); - EXPECT_NO_THROW(trajectory.set_point(point0, std::chrono::nanoseconds(50), 0)); - EXPECT_EQ(trajectory[0].first.get_name(), point0.get_name()); +TYPED_TEST_P(TrajectoryTest, AddRemovePoints) { + EXPECT_NO_THROW(this->trajectory = std::make_shared("trajectory")); + EXPECT_EQ(this->trajectory->get_size(), 0); + + using PointType = typename std::conditional< + std::is_same_v, TrajectoryPoint, + typename std::conditional, CartesianState, JointState>::type>:: + type; + + PointType point0; + PointType point1; + PointType point2; + PointType point3; + + if constexpr (std::is_same_v) { + point0.data = Eigen::VectorXd::Random(3); + point0.duration = std::chrono::nanoseconds(10); + point1.data = Eigen::VectorXd::Random(3); + point1.duration = std::chrono::nanoseconds(20); + point2.data = Eigen::VectorXd::Random(3); + point2.duration = std::chrono::nanoseconds(30); + point3.data = Eigen::VectorXd::Random(3); + point3.duration = std::chrono::nanoseconds(40); + } else if constexpr (std::is_same_v) { + point0 = CartesianState::Random("foo"); + point1 = CartesianState::Random("bar"); + point2 = CartesianState::Random("baz"); + point3 = CartesianState::Random("qux"); + } else if constexpr (std::is_same_v) { + point0 = JointState::Random("foo", 25); + point1 = JointState::Random("bar", 25); + point2 = JointState::Random("baz", 25); + point3 = JointState::Random("qux", 25); + this->trajectory->set_joint_names(point0.get_names()); + } - std::vector points = {point0, point1, point2}; + // additions and insertions of single points + EXPECT_NO_THROW(this->add_point(point0)); + EXPECT_EQ(this->trajectory->get_size(), 1); + this->expect_equal(point0, 0); + EXPECT_NO_THROW(this->add_point(point2)); + EXPECT_EQ(this->trajectory->get_size(), 2); + this->expect_equal(point2, 1); + EXPECT_NO_THROW(this->insert_point(point1, 1)); + EXPECT_EQ(this->trajectory->get_size(), 3); + this->expect_equal(point1, 1); + this->expect_equal(point2, 2); + EXPECT_EQ(this->trajectory->get_durations().size(), 3); + this->set_point(point3, 1); + this->expect_equal(point3, 1); + + // deletions + EXPECT_NO_THROW(this->delete_point_index(1)); + this->expect_equal(point2, 1); + EXPECT_NO_THROW(this->delete_point()); + this->expect_equal(point0, 0); + EXPECT_NO_THROW(this->delete_point()); + EXPECT_EQ(this->trajectory->get_size(), 0); + + // additons and insertions of multiple points + std::vector points = {point0, point1, point2}; std::vector durations = { std::chrono::nanoseconds(10), std::chrono::nanoseconds(20), std::chrono::nanoseconds(30) }; - trajectory.set_points(points, durations); - for (unsigned int i = 0; i < trajectory.get_size(); ++i) { - EXPECT_EQ(trajectory[i].first.data(), points[i].data()); - EXPECT_EQ(trajectory[i].first.get_name(), points[i].get_name()); - EXPECT_EQ(trajectory[i].first.get_reference_frame(), points[i].get_reference_frame()); - EXPECT_EQ(trajectory[i].second, durations[i]); + EXPECT_NO_THROW(this->add_points(points, durations)); + for (unsigned int i = 0; i < this->trajectory->get_size(); ++i) { + this->expect_equal(points[i], i, durations[i]); } -} - -TEST(TrajectoryTest, JointTrajectory) { - { - JointTrajectory trajectory("foo_trajectory", JointState::Random("foo", 25), std::chrono::nanoseconds(100)); - EXPECT_EQ(trajectory.get_size(), 1); + std::vector shuffled_points = {point2, point0, point1}; + std::vector shuffled_durations = { + std::chrono::nanoseconds(30), std::chrono::nanoseconds(10), std::chrono::nanoseconds(20) + }; + EXPECT_NO_THROW(this->set_points(shuffled_points, shuffled_durations)); + for (unsigned int i = 0; i < this->trajectory->get_size(); ++i) { + this->expect_equal(shuffled_points[i], i, shuffled_durations[i]); } +} - { - auto points = { - JointState::Random("foo", 25), - JointState::Random("foo", 25), - }; - auto durations = {std::chrono::nanoseconds(100), std::chrono::nanoseconds(200)}; - JointTrajectory trajectory("foo_trajectory", points, durations); - EXPECT_EQ(trajectory.get_size(), 2); +TYPED_TEST_P(TrajectoryTest, Exceptions) { + EXPECT_NO_THROW(this->trajectory = std::make_shared("trajectory")); + EXPECT_EQ(this->trajectory->get_size(), 0); + + using PointType = typename std::conditional< + std::is_same_v, TrajectoryPoint, + typename std::conditional, CartesianState, JointState>::type>:: + type; + + PointType point0; + PointType point1; + PointType point2; + PointType point3; + + if constexpr (std::is_same_v) { + point0.data = Eigen::VectorXd::Random(3); + point0.duration = std::chrono::nanoseconds(10); + point1.data = Eigen::VectorXd::Random(3); + point1.duration = std::chrono::nanoseconds(20); + point2.data = Eigen::VectorXd::Random(3); + point2.duration = std::chrono::nanoseconds(30); + point3.data = Eigen::VectorXd::Random(3); + point3.duration = std::chrono::nanoseconds(40); + } else if constexpr (std::is_same_v) { + point0 = CartesianState::Random("foo"); + point1 = CartesianState::Random("bar"); + point2 = CartesianState::Random("baz"); + point3 = CartesianState::Random("qux", "some_other_world"); + } else if constexpr (std::is_same_v) { + point0 = JointState::Random("foo", 25); + point1 = JointState::Random("bar", 25); + point2 = JointState::Random("baz", 25); + point3 = JointState::Random("qux", 20); + this->trajectory->set_joint_names(point0.get_names()); } - JointTrajectory trajectory("foo"); - EXPECT_EQ(trajectory.get_size(), 0); - - {// incompatible sizes - JointState state; - EXPECT_THROW(trajectory.add_point(state, std::chrono::nanoseconds(100)), exceptions::EmptyStateException); - EXPECT_EQ(trajectory.get_size(), 0); - - auto points = { - JointState::Random("foo", 25), - JointState::Random("foo", 25), - }; - auto durations = {std::chrono::nanoseconds(100)}; - EXPECT_THROW(trajectory.set_points(points, durations), exceptions::IncompatibleSizeException); - EXPECT_EQ(trajectory.get_size(), 0); + std::vector points = {point0, point1, point2}; + std::vector durations = { + std::chrono::nanoseconds(10), std::chrono::nanoseconds(20), std::chrono::nanoseconds(30) + }; + EXPECT_NO_THROW(this->add_points(points, durations)); + + EXPECT_THROW(this->delete_point_index(10), std::out_of_range); + + points.push_back(point1); + durations.push_back(std::chrono::nanoseconds(40)); + EXPECT_THROW(this->set_points(points, durations), exceptions::IncompatibleSizeException); + + points.pop_back(); + durations.pop_back(); + points[2] = point3; + if constexpr (std::is_same_v) { + EXPECT_THROW(this->set_points(points, durations), exceptions::IncompatibleReferenceFramesException); + EXPECT_THROW(this->set_point(point3, 1), exceptions::IncompatibleReferenceFramesException); + EXPECT_THROW(this->add_point(point3), exceptions::IncompatibleReferenceFramesException); + EXPECT_THROW(this->add_point(CartesianState()), exceptions::EmptyStateException); + } else if constexpr (std::is_same_v) { + EXPECT_THROW(this->set_points(points, durations), exceptions::IncompatibleStatesException); + EXPECT_THROW(this->set_point(point3, 1), exceptions::IncompatibleStatesException); + EXPECT_THROW(this->add_point(point3), exceptions::IncompatibleStatesException); + EXPECT_THROW(this->add_point(JointState()), exceptions::EmptyStateException); } - {// incompatible joint names - auto states = { - JointState::Random("foo", {"j_foo_1", "j_foo_2"}), JointState::Random("bar", {"j_bar_1", "j_bar_2"}) - }; - auto durations = {std::chrono::nanoseconds(100), std::chrono::nanoseconds(200)}; - EXPECT_THROW(trajectory.set_points(states, durations), exceptions::IncompatibleSizeException); - } + EXPECT_THROW(this->set_point(point1, 10), std::out_of_range); + EXPECT_THROW(this->insert_point(point1, 10), std::out_of_range); + EXPECT_THROW(this->trajectory->get_point(10), std::out_of_range); + EXPECT_THROW(this->trajectory->operator[](10), std::out_of_range); +} - {// check joint names - std::vector joint_names = {"j_foo_1", "j_foo_2", "j_foo_3"}; - trajectory.reset(); - trajectory.set_joint_names(joint_names); - trajectory.add_point(JointState::Random("foo", joint_names), std::chrono::nanoseconds(100)); - EXPECT_EQ(trajectory.get_joint_names(), joint_names); - trajectory.reset(); - EXPECT_EQ(trajectory.get_size(), 0); +TYPED_TEST_P(TrajectoryTest, Getters) { + EXPECT_NO_THROW(this->trajectory = std::make_shared("trajectory")); + EXPECT_EQ(this->trajectory->get_size(), 0); + + using PointType = typename std::conditional< + std::is_same_v, TrajectoryPoint, + typename std::conditional, CartesianState, JointState>::type>:: + type; + + PointType point0; + PointType point1; + PointType point2; + + if constexpr (std::is_same_v) { + point0.data = Eigen::VectorXd::Random(3); + point0.duration = std::chrono::nanoseconds(10); + point1.data = Eigen::VectorXd::Random(3); + point1.duration = std::chrono::nanoseconds(20); + point2.data = Eigen::VectorXd::Random(3); + point2.duration = std::chrono::nanoseconds(30); + } else if constexpr (std::is_same_v) { + point0 = CartesianState::Random("foo"); + point1 = CartesianState::Random("bar"); + point2 = CartesianState::Random("baz"); + } else if constexpr (std::is_same_v) { + point0 = JointState::Random("foo", 25); + point1 = JointState::Random("bar", 25); + point2 = JointState::Random("baz", 25); + this->trajectory->set_joint_names(point0.get_names()); } - auto point0 = JointState::Random("foo", 25); - auto point1 = JointState::Random("bar", 25); - auto point2 = JointState::Random("baz", 25); - trajectory.set_joint_names(point0.get_names()); - - trajectory.add_point(point0, std::chrono::nanoseconds(100)); - - EXPECT_EQ(trajectory[0].first.data(), point0.data()); - EXPECT_EQ(trajectory[0].first.get_name(), point0.get_name()); - EXPECT_EQ(trajectory[0].first.get_names(), trajectory.get_joint_names()); - EXPECT_EQ(trajectory[0].first.get_names(), point0.get_names()); - trajectory.add_point(point2, std::chrono::nanoseconds(200)); - - EXPECT_NO_THROW(trajectory.insert_point(point2, std::chrono::nanoseconds(300), 1)); - EXPECT_EQ(trajectory.get_size(), 3); - - std::vector points = {point0, point1, point2}; + std::vector points = {point0, point1, point2}; std::vector durations = { std::chrono::nanoseconds(10), std::chrono::nanoseconds(20), std::chrono::nanoseconds(30) }; - trajectory.set_points(points, durations); - for (unsigned int i = 0; i < trajectory.get_size(); ++i) { - EXPECT_EQ(trajectory[i].first.data(), points[i].data()); - EXPECT_EQ(trajectory[i].first.get_name(), points[i].get_name()); - EXPECT_EQ(trajectory[i].first.get_names(), points[i].get_names()); - EXPECT_EQ(trajectory[i].first.get_name(), points[i].get_name()); - EXPECT_EQ(trajectory[i].second, durations[i]); + EXPECT_NO_THROW(this->add_points(points, durations)); + auto trajectory_points = this->trajectory->get_points(); + auto trajectory_durations = this->trajectory->get_durations(); + for (unsigned int i = 0; i < this->trajectory->get_size(); ++i) { + EXPECT_NO_THROW(this->expect_equal(points[i], i, durations[i])); + EXPECT_NO_THROW(this->expect_equal(trajectory_points[i], i, durations[i])); + EXPECT_NO_THROW(this->expect_equal(this->trajectory->get_point(i), i, trajectory_durations[i])); + } + + auto times_from_start = this->trajectory->get_times_from_start(); + std::chrono::nanoseconds time_from_start = std::chrono::nanoseconds(0); + for (unsigned int i = 0; i < this->trajectory->get_size(); ++i) { + time_from_start += durations[i]; + EXPECT_EQ(times_from_start[i], time_from_start); + } + + this->trajectory->reset(); + EXPECT_EQ(this->trajectory->get_size(), 0); + EXPECT_EQ(this->trajectory->get_durations().size(), 0); + if constexpr (std::is_same_v) { + EXPECT_STREQ(this->trajectory->get_reference_frame().c_str(), "world"); + } else if constexpr (std::is_same_v) { + EXPECT_NE(this->trajectory->get_joint_names().size(), 0); } + EXPECT_TRUE(this->trajectory->is_empty()); } + +REGISTER_TYPED_TEST_SUITE_P(TrajectoryTest, AddRemovePoints, Exceptions, Getters); + +using TrajectoryTypes = testing::Types; +INSTANTIATE_TYPED_TEST_SUITE_P(Type, TrajectoryTest, TrajectoryTypes); From 914a8269c804501143f5c8137d067cd043577373 Mon Sep 17 00:00:00 2001 From: bpapaspyros Date: Thu, 30 Jan 2025 19:58:52 +0100 Subject: [PATCH 46/52] refactor: improve condition checks in vector operations --- .../src/trajectory/CartesianTrajectory.cpp | 28 ++++++++----------- .../src/trajectory/JointTrajectory.cpp | 22 ++++++--------- 2 files changed, 21 insertions(+), 29 deletions(-) diff --git a/source/state_representation/src/trajectory/CartesianTrajectory.cpp b/source/state_representation/src/trajectory/CartesianTrajectory.cpp index f6f2625f1..d446276d6 100644 --- a/source/state_representation/src/trajectory/CartesianTrajectory.cpp +++ b/source/state_representation/src/trajectory/CartesianTrajectory.cpp @@ -32,14 +32,12 @@ CartesianTrajectory::CartesianTrajectory( ) : TrajectoryBase(name) { this->set_type(StateType::CARTESIAN_TRAJECTORY); - for (unsigned int i = 1; i < points.size(); ++i) { - if (points[i - 1].is_empty()) { - throw exceptions::EmptyStateException("Vector contains at least one point that is empty"); - } else if (points[i - 1].get_reference_frame() != points[i].get_reference_frame()) { - throw exceptions::IncompatibleReferenceFramesException( - "Incompatible reference frames within the new points vector" - ); - } + if (std::ranges::any_of(points, [&](const auto& p) { return p.is_empty(); })) { + throw exceptions::EmptyStateException("Vector contains at least one point that is empty"); + } else if (!std::ranges::all_of(points, [&](const auto& p) { + return p.get_reference_frame() == points.front().get_reference_frame(); + })) { + throw exceptions::IncompatibleReferenceFramesException("Incompatible reference frames within the points vector"); } if (points.size() > 0) { this->reference_frame_ = points[0].get_reference_frame(); @@ -94,14 +92,12 @@ void CartesianTrajectory::add_points( if (new_points.size() != durations.size()) { throw exceptions::IncompatibleSizeException("The size of the points and durations vectors are not equal"); } - for (unsigned int i = 1; i < new_points.size(); ++i) { - if (new_points[i - 1].is_empty()) { - throw exceptions::EmptyStateException("Vector contains at least one point that is empty"); - } else if (new_points[i - 1].get_reference_frame() != new_points[i].get_reference_frame()) { - throw exceptions::IncompatibleReferenceFramesException( - "Incompatible reference frames within the new points vector" - ); - } + if (std::ranges::any_of(new_points, [&](const auto& p) { return p.is_empty(); })) { + throw exceptions::EmptyStateException("Vector contains at least one point that is empty"); + } else if (!std::ranges::all_of(new_points, [&](const auto& p) { + return p.get_reference_frame() == this->reference_frame_; + })) { + throw exceptions::IncompatibleReferenceFramesException("Incompatible reference frames within the points vector"); } for (unsigned int i = 0; i < new_points.size(); ++i) { add_point(new_points[i], durations[i]); diff --git a/source/state_representation/src/trajectory/JointTrajectory.cpp b/source/state_representation/src/trajectory/JointTrajectory.cpp index e027c0e33..22cb634c0 100644 --- a/source/state_representation/src/trajectory/JointTrajectory.cpp +++ b/source/state_representation/src/trajectory/JointTrajectory.cpp @@ -3,6 +3,7 @@ #include "state_representation/exceptions/EmptyStateException.hpp" #include "state_representation/exceptions/IncompatibleStatesException.hpp" +#include namespace state_representation { @@ -28,12 +29,10 @@ JointTrajectory::JointTrajectory( ) : TrajectoryBase(name) { this->set_type(StateType::JOINT_TRAJECTORY); - for (unsigned int i = 1; i < points.size(); ++i) { - if (points[i - 1].is_empty()) { - throw exceptions::EmptyStateException("Vector contains at least one point that is empty"); - } else if (points[i - 1].get_names() != points[i].get_names()) { - throw exceptions::IncompatibleStatesException("Incompatible joint names within the new points vector"); - } + if (std::ranges::any_of(points, [&](const auto& p) { return p.is_empty(); })) { + throw exceptions::EmptyStateException("Vector contains at least one point that is empty"); + } else if (!std::ranges::all_of(points, [&](const auto& p) { return p.get_names() == points.front().get_names(); })) { + throw exceptions::IncompatibleStatesException("Incompatible joint names within the new points vector"); } if (points.size() > 0) { this->joint_names_ = points[0].get_names(); @@ -61,13 +60,10 @@ void JointTrajectory::add_points( ) { if (new_points.size() != durations.size()) { throw exceptions::IncompatibleSizeException("The size of the points and durations vectors are not equal"); - } - for (unsigned int i = 1; i < new_points.size(); ++i) { - if (new_points[i - 1].is_empty()) { - throw exceptions::EmptyStateException("Vector contains at least one point that is empty"); - } else if (new_points[i - 1].get_names() != new_points[i].get_names()) { - throw exceptions::IncompatibleStatesException("Incompatible joint names within the new points vector"); - } + } else if (std::ranges::any_of(new_points, [&](const auto& p) { return p.is_empty(); })) { + throw exceptions::EmptyStateException("Vector contains at least one point that is empty"); + } else if (!std::ranges::all_of(new_points, [&](const auto& p) { return p.get_names() == this->joint_names_; })) { + throw exceptions::IncompatibleStatesException("Incompatible joint names within the new points vector"); } for (unsigned int i = 0; i < new_points.size(); ++i) { add_point(new_points[i], durations[i]); From 0ff12106751fa3c57c828f31196f3c1bddb6eabb Mon Sep 17 00:00:00 2001 From: bpapaspyros Date: Thu, 30 Jan 2025 20:00:34 +0100 Subject: [PATCH 47/52] refactor: re-arrange includes --- .../state_representation/src/trajectory/JointTrajectory.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/source/state_representation/src/trajectory/JointTrajectory.cpp b/source/state_representation/src/trajectory/JointTrajectory.cpp index 22cb634c0..ecf10cf9f 100644 --- a/source/state_representation/src/trajectory/JointTrajectory.cpp +++ b/source/state_representation/src/trajectory/JointTrajectory.cpp @@ -1,9 +1,10 @@ -#include "state_representation/trajectory/JointTrajectory.hpp" +#include + #include "state_representation/space/joint/JointState.hpp" +#include "state_representation/trajectory/JointTrajectory.hpp" #include "state_representation/exceptions/EmptyStateException.hpp" #include "state_representation/exceptions/IncompatibleStatesException.hpp" -#include namespace state_representation { From 93419cafbd5684ed0254ffdc855b4dd86b9da4fe Mon Sep 17 00:00:00 2001 From: bpapaspyros Date: Thu, 30 Jan 2025 20:30:39 +0100 Subject: [PATCH 48/52] fix: better checks for empty vector arguments --- .../trajectory/TrajectoryBase.hpp | 11 ++++++----- .../src/trajectory/CartesianTrajectory.cpp | 15 ++++++++------- .../src/trajectory/JointTrajectory.cpp | 12 +++++++----- 3 files changed, 21 insertions(+), 17 deletions(-) diff --git a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp index aa6483530..1e330b30a 100644 --- a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp +++ b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp @@ -96,6 +96,7 @@ class TrajectoryBase : public State { /** * @brief Add new points to trajectory * @param new_point the new point + * @throw IncompatibleSizeException if points vector is empty */ void add_points(const std::vector& new_points); @@ -134,6 +135,7 @@ class TrajectoryBase : public State { * corresponding time * @param idx the index * @return the trajectory point + * @throw IncompatibleSizeException if points vector is empty or different size than current points * @throw std::out_of_range if index is out of range */ const TrajectoryT& operator[](unsigned int idx) const; @@ -175,8 +177,8 @@ inline void TrajectoryBase::add_point(const TrajectoryT& new_point) template inline void TrajectoryBase::add_points(const std::vector& new_points) { - if (new_points.size() == 0) { - return; + if (new_points.empty()) { + throw exceptions::IncompatibleSizeException("No points provided"); } for (auto point : new_points) { points_.push_back(point); @@ -246,10 +248,9 @@ inline void TrajectoryBase::set_point(const TrajectoryT& point, uns template inline void TrajectoryBase::set_points(const std::vector& points) { - if (points.size() == 0) { + if (points.empty()) { throw exceptions::IncompatibleSizeException("No points provided"); - } - if (points.size() != this->points_.size()) { + } else if (points.size() != this->points_.size()) { throw exceptions::IncompatibleSizeException("The size of the current vector and the new vector are not equal"); } for (unsigned int i = 0; i < points.size(); ++i) { diff --git a/source/state_representation/src/trajectory/CartesianTrajectory.cpp b/source/state_representation/src/trajectory/CartesianTrajectory.cpp index d446276d6..316e32c52 100644 --- a/source/state_representation/src/trajectory/CartesianTrajectory.cpp +++ b/source/state_representation/src/trajectory/CartesianTrajectory.cpp @@ -32,16 +32,16 @@ CartesianTrajectory::CartesianTrajectory( ) : TrajectoryBase(name) { this->set_type(StateType::CARTESIAN_TRAJECTORY); - if (std::ranges::any_of(points, [&](const auto& p) { return p.is_empty(); })) { + if (points.empty()) { + throw exceptions::IncompatibleSizeException("No points provided"); + } else if (std::ranges::any_of(points, [&](const auto& p) { return p.is_empty(); })) { throw exceptions::EmptyStateException("Vector contains at least one point that is empty"); } else if (!std::ranges::all_of(points, [&](const auto& p) { return p.get_reference_frame() == points.front().get_reference_frame(); })) { throw exceptions::IncompatibleReferenceFramesException("Incompatible reference frames within the points vector"); } - if (points.size() > 0) { - this->reference_frame_ = points[0].get_reference_frame(); - } + this->reference_frame_ = points[0].get_reference_frame(); try { this->add_points(points, durations); } catch (...) { @@ -89,10 +89,11 @@ void CartesianTrajectory::add_point(const CartesianState& new_point, const std:: void CartesianTrajectory::add_points( const std::vector& new_points, const std::vector& durations ) { - if (new_points.size() != durations.size()) { + if (new_points.empty()) { + throw exceptions::IncompatibleSizeException("No points provided"); + } else if (new_points.size() != durations.size()) { throw exceptions::IncompatibleSizeException("The size of the points and durations vectors are not equal"); - } - if (std::ranges::any_of(new_points, [&](const auto& p) { return p.is_empty(); })) { + } else if (std::ranges::any_of(new_points, [&](const auto& p) { return p.is_empty(); })) { throw exceptions::EmptyStateException("Vector contains at least one point that is empty"); } else if (!std::ranges::all_of(new_points, [&](const auto& p) { return p.get_reference_frame() == this->reference_frame_; diff --git a/source/state_representation/src/trajectory/JointTrajectory.cpp b/source/state_representation/src/trajectory/JointTrajectory.cpp index ecf10cf9f..1657a3e25 100644 --- a/source/state_representation/src/trajectory/JointTrajectory.cpp +++ b/source/state_representation/src/trajectory/JointTrajectory.cpp @@ -30,14 +30,14 @@ JointTrajectory::JointTrajectory( ) : TrajectoryBase(name) { this->set_type(StateType::JOINT_TRAJECTORY); - if (std::ranges::any_of(points, [&](const auto& p) { return p.is_empty(); })) { + if (points.empty()) { + throw exceptions::IncompatibleSizeException("No points provided"); + } else if (std::ranges::any_of(points, [&](const auto& p) { return p.is_empty(); })) { throw exceptions::EmptyStateException("Vector contains at least one point that is empty"); } else if (!std::ranges::all_of(points, [&](const auto& p) { return p.get_names() == points.front().get_names(); })) { throw exceptions::IncompatibleStatesException("Incompatible joint names within the new points vector"); } - if (points.size() > 0) { - this->joint_names_ = points[0].get_names(); - } + this->joint_names_ = points[0].get_names(); this->add_points(points, durations); } @@ -59,7 +59,9 @@ void JointTrajectory::add_point(const JointState& new_point, const std::chrono:: void JointTrajectory::add_points( const std::vector& new_points, const std::vector& durations ) { - if (new_points.size() != durations.size()) { + if (new_points.empty()) { + throw exceptions::IncompatibleSizeException("No points provided"); + } else if (new_points.size() != durations.size()) { throw exceptions::IncompatibleSizeException("The size of the points and durations vectors are not equal"); } else if (std::ranges::any_of(new_points, [&](const auto& p) { return p.is_empty(); })) { throw exceptions::EmptyStateException("Vector contains at least one point that is empty"); From 2ed62289f3db0065c1f710af06510443a05242d6 Mon Sep 17 00:00:00 2001 From: bpapaspyros Date: Fri, 31 Jan 2025 10:49:19 +0100 Subject: [PATCH 49/52] refactor: improve/optimize code structure and readability --- .../trajectory/CartesianTrajectory.hpp | 46 ++++- .../trajectory/JointTrajectory.hpp | 45 ++++- .../trajectory/TrajectoryBase.hpp | 189 ++++++++++++++---- .../src/trajectory/CartesianTrajectory.cpp | 181 +++++------------ .../src/trajectory/JointTrajectory.cpp | 157 +++++---------- 5 files changed, 331 insertions(+), 287 deletions(-) diff --git a/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp b/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp index 53c9146ef..6a1f1cd55 100644 --- a/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp +++ b/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp @@ -10,7 +10,23 @@ namespace state_representation { * @class CartesianTrajectoryPoint * @brief Struct to represent a Cartesian trajectory point */ -struct CartesianTrajectoryPoint : public TrajectoryPoint {}; +struct CartesianTrajectoryPoint : public TrajectoryPoint { + /** + * @brief Empty constructor + */ + CartesianTrajectoryPoint() = default; + + /** + * @brief Constructor with name, data, and duration + * @param name the trajectory point name + * @param data the (flattened) trajectory data + * @param duration the intended duration for the trajectory point + */ + CartesianTrajectoryPoint( + const std::string& name, const Eigen::VectorXd& data, const std::chrono::nanoseconds& duration + ) + : TrajectoryPoint(name, data, duration) {} +}; /** * @class CartesianTrajectory @@ -71,32 +87,31 @@ class CartesianTrajectory : public TrajectoryBase { /** * @brief Add new point and corresponding duration to trajectory - * @param new_point the new trajectory point + * @param point the new trajectory point * @param duration the duration for the new point * @throw EmptyStateException if point is empty * @throw IncompatibleReferenceFramesException if point has different reference frame */ - void add_point(const CartesianState& new_point, const std::chrono::nanoseconds& duration); + void add_point(const CartesianState& point, const std::chrono::nanoseconds& duration); /** * @brief Add new points and corresponding durations to trajectory - * @param new_points the new trajectory point + * @param points the new trajectory point * @param durations the duration for the new point * @throw IncompatibleSizeException if points and durations have different sizes * @throw EmptyStateException if point is empty * @throw IncompatibleReferenceFramesException if point has different reference frame */ - void - add_points(const std::vector& new_points, const std::vector& durations); + void add_points(const std::vector& points, const std::vector& durations); /** * @brief Insert new point and corresponding duration to trajectory between two * already existing points - * @param new_point the new trajectory point + * @param point the new trajectory point * @param duration the duration for the new point * @param index the desired position of the new point in the queue */ - void insert_point(const CartesianState& new_point, const std::chrono::nanoseconds& duration, unsigned int index); + void insert_point(const CartesianState& point, const std::chrono::nanoseconds& duration, unsigned int index); /** * @brief Get list of trajectory points @@ -140,6 +155,21 @@ class CartesianTrajectory : public TrajectoryBase { std::pair operator[](unsigned int idx) const; private: + /** + * @brief Assert that all states of a vector carry the same reference frame + * @param states the states to check + * @throw IncompatibleReferenceFramesException if a state has a different reference frame + */ + void assert_same_reference_frame(const std::vector& states) const; + + /** + * @brief Assert that all states of a vector carry the same reference frame as the one provided + * @param states the states to check + * @param reference_frame the reference frame to check against + * @throw IncompatibleReferenceFramesException if a state has a different reference frame + */ + void assert_same_reference_frame(const std::vector& states, const std::string& reference_frame) const; + std::string reference_frame_;///< name of the reference frame }; }// namespace state_representation diff --git a/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp b/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp index eaf0e6f73..37c007669 100644 --- a/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp +++ b/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp @@ -9,7 +9,21 @@ namespace state_representation { * @class JointTrajectoryPoint * @brief Struct to represent a joint trajectory point */ -struct JointTrajectoryPoint : public TrajectoryPoint {}; +struct JointTrajectoryPoint : public TrajectoryPoint { + /** + * @brief Empty constructor + */ + JointTrajectoryPoint() = default; + + /** + * @brief Constructor with name, data, and duration + * @param name the trajectory point name + * @param data the (flattened) trajectory data + * @param duration the intended duration for the trajectory point + */ + JointTrajectoryPoint(const std::string& name, const Eigen::VectorXd& data, const std::chrono::nanoseconds& duration) + : TrajectoryPoint(name, data, duration) {} +}; /** * @class JointTrajectory @@ -56,31 +70,31 @@ class JointTrajectory : public TrajectoryBase { /** * @brief Add new point and corresponding duration to trajectory - * @param new_point the new trajectory point + * @param point the new trajectory point * @param duration the duration for the new point * @throw EmptyStateException if point is empty * @throw IncompatibleStatesException if point has different robot name */ - void add_point(const JointState& new_point, const std::chrono::nanoseconds& duration); + void add_point(const JointState& point, const std::chrono::nanoseconds& duration); /** * @brief Add new points and corresponding durations to trajectory - * @param new_points the new trajectory point + * @param points the new trajectory point * @param durations the duration for the new point * @throw IncompatibleSizeException if points and durations have different sizes * @throw EmptyStateException if point is empty * @throw IncompatibleStatesException if point has different robot name */ - void add_points(const std::vector& new_points, const std::vector& durations); + void add_points(const std::vector& points, const std::vector& durations); /** * @brief Insert new point and corresponding duration to trajectory between two * already existing points - * @param new_point the new trajectory point + * @param point the new trajectory point * @param duration the duration for the new point * @param index the desired position of the new point in the queue */ - void insert_point(const JointState& new_point, const std::chrono::nanoseconds& duration, unsigned int index); + void insert_point(const JointState& point, const std::chrono::nanoseconds& duration, unsigned int index); /** * @brief Get list of trajectory points @@ -124,6 +138,23 @@ class JointTrajectory : public TrajectoryBase { std::pair operator[](unsigned int idx) const; private: + /** + * @brief Assert that all states of a vector carry the same joint names + * @param states the states to check + * @throw IncompatibleReferenceFramesException if a state has a different joint names + */ + void assert_incompatible_joint_names(const std::vector& states) const; + + /** + * @brief Assert that all states of a vector carry the same joint names as the one provided + * @param states the states to check + * @param reference_frame the joint names to check against + * @throw IncompatibleReferenceFramesException if a state has a different joint names + */ + void assert_incompatible_joint_names( + const std::vector& states, const std::vector& joint_names + ) const; + std::vector joint_names_;///< names of the joints }; }// namespace state_representation diff --git a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp index 1e330b30a..bd2f2dff9 100644 --- a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp +++ b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp @@ -1,10 +1,13 @@ #pragma once #include +#include #include #include +#include #include "state_representation/State.hpp" +#include "state_representation/exceptions/EmptyStateException.hpp" #include "state_representation/exceptions/IncompatibleSizeException.hpp" namespace state_representation { @@ -14,6 +17,20 @@ namespace state_representation { * @brief Struct to represent the base characteristics of a trajectory point */ struct TrajectoryPoint { + /** + * @brief Empty constructor + */ + TrajectoryPoint() = default; + + /** + * @brief Constructor with name, data, and duration + * @param name the trajectory point name + * @param data the (flattened) trajectory data + * @param duration the intended duration for the trajectory point + */ + TrajectoryPoint(const std::string& name, const Eigen::VectorXd& data, const std::chrono::nanoseconds& duration) + : name(name), data(data), duration(duration) {} + std::string name; Eigen::VectorXd data; std::chrono::nanoseconds duration; @@ -65,7 +82,7 @@ class TrajectoryBase : public State { /** * @brief Empty constructor */ - explicit TrajectoryBase(); + explicit TrajectoryBase() = default; /** * @brief Constructor with name provided @@ -133,35 +150,90 @@ class TrajectoryBase : public State { /** * @brief Operator overload for returning a single trajectory point and * corresponding time - * @param idx the index + * @param index the index * @return the trajectory point * @throw IncompatibleSizeException if points vector is empty or different size than current points * @throw std::out_of_range if index is out of range */ - const TrajectoryT& operator[](unsigned int idx) const; + const TrajectoryT& operator[](unsigned int index) const; /** * @brief Operator overload for returning a single trajectory point and * corresponding time - * @param idx the index + * @param index the index * @return the trajectory point * @throw std::out_of_range if index is out of range */ - TrajectoryT& operator[](unsigned int idx); + TrajectoryT& operator[](unsigned int index); + + /** + * @brief Assert the index provided is in range of the current points list + * @param index the index + * @throw std::out_of_range if index is out of range + */ + void assert_index_in_range(unsigned int index) const; + + /** + * @brief Assert the points vector provided is not empty + * @param points the vector of points to check + * @throw IncompatibleSizeException if points empty + */ + template + void assert_points_empty(const std::vector& points) const; + + /** + * @brief Assert the points vector provided is of the same size as the current points + * @param points the vector of points to check + * @throw IncompatibleSizeException if size points provided different to current points + */ + template + void assert_points_size(const std::vector& points) const; + + /** + * @brief Assert the points vector and durations vectors are of equal size + * @param points the vector of points to check + * @param durations the vector of durations to check + * @throw IncompatibleSizeException if the two vector sizes are not equal + */ + template + void assert_points_durations_sizes_equal( + const std::vector& points, const std::vector& durations + ) const; + + /** + * @brief Assert the that 2 vectors are element wise equal + * @param lvec the vector of points to check + * @param rvec the vector of durations to check + * @throws std::runtime-derived exception if vectors differ + */ + template + requires std::derived_from + void assert_vector_ewise_equal( + const std::vector& lvec, const std::vector& rvec, + const std::string& msg = "The vectors provided contain elements that differ!" + ) const; + + /** + * @brief Assert that vector of State type does not contain empty elements + * @param states the vector of State-type elements to check + * @throws EmptyStateException if any of the elements is empty + */ + template + requires std::derived_from + void assert_contains_empty_state(const std::vector& states) const; + + /** + * @brief Assert that the trajectory is empty + * @throws EmptyStateException if any of the elements is empty + */ + void assert_trajectory_empty() const; private: std::deque points_; }; template -inline TrajectoryBase::TrajectoryBase() : State() { - this->reset(); -} - -template -inline TrajectoryBase::TrajectoryBase(const std::string& name) : State(name) { - this->reset(); -} +inline TrajectoryBase::TrajectoryBase(const std::string& name) : State(name) {} template inline void TrajectoryBase::reset() { @@ -187,10 +259,8 @@ inline void TrajectoryBase::add_points(const std::vector inline void TrajectoryBase::insert_point(const TrajectoryT& new_point, unsigned int index) { - if (index > this->points_.size()) { - throw std::out_of_range("Index out of range"); - } this->set_empty(false); + this->assert_index_in_range(index); auto it_points = this->points_.begin(); std::advance(it_points, index); this->points_.insert(it_points, new_point); @@ -208,9 +278,7 @@ inline void TrajectoryBase::delete_point() { template inline void TrajectoryBase::delete_point(unsigned int index) { - if (index >= this->points_.size()) { - throw std::out_of_range("Index out of range"); - } + this->assert_index_in_range(index); this->points_.erase(this->points_.begin() + index); if (this->points_.empty()) { this->set_empty(true); @@ -224,35 +292,26 @@ inline const std::vector TrajectoryBase::get_points() template inline const TrajectoryT& TrajectoryBase::get_point(unsigned int index) const { - if (index >= this->points_.size()) { - throw std::out_of_range("Index out of range"); - } + this->assert_index_in_range(index); return this->points_[index]; } template inline TrajectoryT& TrajectoryBase::get_point(unsigned int index) { - if (index >= this->points_.size()) { - throw std::out_of_range("Index out of range"); - } + this->assert_index_in_range(index); return this->points_[index]; } template inline void TrajectoryBase::set_point(const TrajectoryT& point, unsigned int index) { - if (index >= this->points_.size()) { - throw std::out_of_range("Index out of range"); - } + this->assert_index_in_range(index); this->points_[index] = point; } template inline void TrajectoryBase::set_points(const std::vector& points) { - if (points.empty()) { - throw exceptions::IncompatibleSizeException("No points provided"); - } else if (points.size() != this->points_.size()) { - throw exceptions::IncompatibleSizeException("The size of the current vector and the new vector are not equal"); - } + this->assert_points_empty(points); + this->assert_points_size(points); for (unsigned int i = 0; i < points.size(); ++i) { this->points_[i] = points[i]; } @@ -285,17 +344,73 @@ inline unsigned int TrajectoryBase::get_size() const { template inline const TrajectoryT& TrajectoryBase::operator[](unsigned int index) const { - if (index >= this->points_.size()) { - throw std::out_of_range("Index out of range"); - } + this->assert_index_in_range(index); return this->points_[index]; } template inline TrajectoryT& TrajectoryBase::operator[](unsigned int index) { + this->assert_index_in_range(index); + return this->points_[index]; +} + +template +inline void TrajectoryBase::assert_index_in_range(unsigned int index) const { if (index >= this->points_.size()) { throw std::out_of_range("Index out of range"); } - return this->points_[index]; +} + +template +template +inline void TrajectoryBase::assert_points_empty(const std::vector& points) const { + if (points.empty()) { + throw exceptions::IncompatibleSizeException("Empty points vector provided!"); + } +} + +template +template +inline void TrajectoryBase::assert_points_size(const std::vector& points) const { + if (points.size() != this->points_.size()) { + throw exceptions::IncompatibleSizeException("The size of the current vector and the new vector are not equal"); + } +} + +template +template +inline void TrajectoryBase::assert_points_durations_sizes_equal( + const std::vector& points, const std::vector& durations +) const { + if (points.size() != durations.size()) { + throw exceptions::IncompatibleSizeException("The size of the points and durations vectors are not equal"); + } +} + +template +template + requires std::derived_from +inline void TrajectoryBase::assert_vector_ewise_equal( + const std::vector& lvec, const std::vector& rvec, const std::string& msg +) const { + if (lvec != rvec) { + throw ExceptionType(msg); + } +} + +template +template + requires std::derived_from +inline void TrajectoryBase::assert_contains_empty_state(const std::vector& states) const { + if (std::ranges::any_of(states, [&](const auto& state) { return state.is_empty(); })) { + throw exceptions::EmptyStateException("Empty state variable provided"); + } +} + +template +inline void TrajectoryBase::assert_trajectory_empty() const { + if (this->is_empty()) { + throw exceptions::EmptyStateException("Trajectory is empty"); + } } }// namespace state_representation diff --git a/source/state_representation/src/trajectory/CartesianTrajectory.cpp b/source/state_representation/src/trajectory/CartesianTrajectory.cpp index 316e32c52..6945e96c0 100644 --- a/source/state_representation/src/trajectory/CartesianTrajectory.cpp +++ b/source/state_representation/src/trajectory/CartesianTrajectory.cpp @@ -1,7 +1,6 @@ #include "state_representation/trajectory/CartesianTrajectory.hpp" #include "state_representation/space/cartesian/CartesianState.hpp" -#include "state_representation/exceptions/EmptyStateException.hpp" #include "state_representation/exceptions/IncompatibleReferenceFramesException.hpp" namespace state_representation { @@ -19,9 +18,7 @@ CartesianTrajectory::CartesianTrajectory( ) : TrajectoryBase(name) { this->set_type(StateType::CARTESIAN_TRAJECTORY); - if (point.is_empty()) { - throw exceptions::EmptyStateException("The Cartesian state provided is empty"); - } + this->assert_points_empty({point}); this->reference_frame_ = point.get_reference_frame(); this->add_point(point, duration); } @@ -32,21 +29,11 @@ CartesianTrajectory::CartesianTrajectory( ) : TrajectoryBase(name) { this->set_type(StateType::CARTESIAN_TRAJECTORY); - if (points.empty()) { - throw exceptions::IncompatibleSizeException("No points provided"); - } else if (std::ranges::any_of(points, [&](const auto& p) { return p.is_empty(); })) { - throw exceptions::EmptyStateException("Vector contains at least one point that is empty"); - } else if (!std::ranges::all_of(points, [&](const auto& p) { - return p.get_reference_frame() == points.front().get_reference_frame(); - })) { - throw exceptions::IncompatibleReferenceFramesException("Incompatible reference frames within the points vector"); - } + this->assert_points_empty(points); + this->assert_contains_empty_state(points); + this->assert_same_reference_frame(points); this->reference_frame_ = points[0].get_reference_frame(); - try { - this->add_points(points, durations); - } catch (...) { - throw; - } + this->add_points(points, durations); } const std::string& CartesianTrajectory::get_reference_frame() const { @@ -54,129 +41,61 @@ const std::string& CartesianTrajectory::get_reference_frame() const { } void CartesianTrajectory::set_reference_frame(const CartesianPose& pose) { - if (pose.is_empty()) { - throw exceptions::EmptyStateException("Pose is empty"); - } else if (this->get_size() == 0) { - throw exceptions::EmptyStateException("Trajectory is empty"); - } + this->assert_trajectory_empty(); this->reference_frame_ = pose.get_reference_frame(); auto points = this->get_points(); for (auto& point : points) { point *= pose; } - try { - this->set_points(points, this->get_durations()); - } catch (...) { - throw; - } + this->set_points(points, this->get_durations()); } -void CartesianTrajectory::add_point(const CartesianState& new_point, const std::chrono::nanoseconds& duration) { - if (new_point.is_empty()) { - throw exceptions::EmptyStateException("The Cartesian state provided is empty"); - } else if (new_point.get_reference_frame() != this->reference_frame_) { - throw exceptions::IncompatibleReferenceFramesException( - "Incompatible reference frames: " + new_point.get_reference_frame() + " and " + this->reference_frame_ - ); - } - CartesianTrajectoryPoint trajectory_point; - trajectory_point.data = new_point.data(); - trajectory_point.duration = duration; - trajectory_point.name = new_point.get_name(); - this->TrajectoryBase::add_point(trajectory_point); +void CartesianTrajectory::add_point(const CartesianState& point, const std::chrono::nanoseconds& duration) { + this->add_points({point}, {duration}); } void CartesianTrajectory::add_points( - const std::vector& new_points, const std::vector& durations + const std::vector& points, const std::vector& durations ) { - if (new_points.empty()) { - throw exceptions::IncompatibleSizeException("No points provided"); - } else if (new_points.size() != durations.size()) { - throw exceptions::IncompatibleSizeException("The size of the points and durations vectors are not equal"); - } else if (std::ranges::any_of(new_points, [&](const auto& p) { return p.is_empty(); })) { - throw exceptions::EmptyStateException("Vector contains at least one point that is empty"); - } else if (!std::ranges::all_of(new_points, [&](const auto& p) { - return p.get_reference_frame() == this->reference_frame_; - })) { - throw exceptions::IncompatibleReferenceFramesException("Incompatible reference frames within the points vector"); - } - for (unsigned int i = 0; i < new_points.size(); ++i) { - add_point(new_points[i], durations[i]); + this->assert_points_empty(points); + this->assert_points_durations_sizes_equal(points, durations); + this->assert_contains_empty_state(points); + this->assert_same_reference_frame(points, this->reference_frame_); + for (unsigned int i = 0; i < points.size(); ++i) { + this->TrajectoryBase::add_point( + CartesianTrajectoryPoint(points[i].get_name(), points[i].data(), durations[i]) + ); } } void CartesianTrajectory::insert_point( - const CartesianState& new_point, const std::chrono::nanoseconds& duration, unsigned int index + const CartesianState& point, const std::chrono::nanoseconds& duration, unsigned int index ) { - if (new_point.is_empty()) { - throw exceptions::EmptyStateException("Point is empty"); - } else if (new_point.get_reference_frame() != this->reference_frame_) { - throw exceptions::IncompatibleReferenceFramesException( - "Incompatible reference frames: " + new_point.get_reference_frame() + " and " + this->reference_frame_ - ); - } - CartesianTrajectoryPoint trajectory_point; - trajectory_point.data = new_point.data(); - trajectory_point.duration = duration; - trajectory_point.name = new_point.get_name(); - try { - this->TrajectoryBase::insert_point(trajectory_point, index); - } catch (...) { - throw; - } + this->assert_contains_empty_state({point}); + this->assert_same_reference_frame({point}, this->reference_frame_); + this->TrajectoryBase::insert_point( + CartesianTrajectoryPoint(point.get_name(), point.data(), duration), index + ); } void CartesianTrajectory::set_point( const CartesianState& point, const std::chrono::nanoseconds& duration, unsigned int index ) { - if (point.is_empty()) { - throw exceptions::EmptyStateException("Point is empty"); - } else if (point.get_reference_frame() != this->reference_frame_) { - throw exceptions::IncompatibleReferenceFramesException( - "Incompatible reference frames: " + point.get_reference_frame() + " and " + this->reference_frame_ - ); - } - CartesianTrajectoryPoint trajectory_point; - trajectory_point.data = point.data(); - trajectory_point.duration = duration; - trajectory_point.name = point.get_name(); - try { - this->TrajectoryBase::set_point(trajectory_point, index); - } catch (...) { - throw; - } + this->assert_contains_empty_state({point}); + this->assert_same_reference_frame({point}, this->reference_frame_); + this->TrajectoryBase::set_point( + CartesianTrajectoryPoint(point.get_name(), point.data(), duration), index + ); } void CartesianTrajectory::set_points( const std::vector& points, const std::vector& durations ) { - if (points.empty()) { - throw exceptions::IncompatibleSizeException("No points provided"); - } else if (points.size() != this->get_size()) { - throw exceptions::IncompatibleSizeException("The size of the current vector and the new vector are not equal"); - } else if (points.size() != durations.size()) { - throw exceptions::IncompatibleSizeException("The size of the points and durations vectors are not equal"); - } - - std::vector trajectory_points; + this->assert_points_empty(points); + this->assert_points_size(points); + this->assert_points_durations_sizes_equal(points, durations); for (unsigned int i = 0; i < points.size(); ++i) { - if (points[i].is_empty()) { - throw exceptions::EmptyStateException("Vector contains at least one point that is empty"); - } else if (points[i].get_reference_frame() != this->reference_frame_) { - throw exceptions::IncompatibleReferenceFramesException( - "Incompatible reference frames: " + points[i].get_reference_frame() + " and " + this->reference_frame_ - ); - } - CartesianTrajectoryPoint trajectory_point; - trajectory_point.data = points[i].data(); - trajectory_point.duration = durations[i]; - trajectory_point.name = points[i].get_name(); - trajectory_points.push_back(trajectory_point); - } - try { - this->TrajectoryBase::set_points(trajectory_points); - } catch (...) { - throw; + this->set_point(points[i], durations[i], i); } } @@ -189,22 +108,30 @@ const std::vector CartesianTrajectory::get_points() const { } CartesianState CartesianTrajectory::get_point(unsigned int index) const { - try { - return this->operator[](index).first; - } catch (...) { - throw; - } + return this->operator[](index).first; } std::pair CartesianTrajectory::operator[](unsigned int idx) const { - try { - auto point = this->TrajectoryBase::operator[](idx); - CartesianState state(point.name, this->reference_frame_); - state.set_data(point.data); - auto duration = point.duration; - return std::make_pair(state, duration); - } catch (...) { - throw; + auto point = this->TrajectoryBase::operator[](idx); + CartesianState state(point.name, this->reference_frame_); + state.set_data(point.data); + auto duration = point.duration; + return std::make_pair(state, duration); +} + +void CartesianTrajectory::assert_same_reference_frame(const std::vector& states) const { + if (!states.empty()) { + assert_same_reference_frame(states, states[0].get_reference_frame()); + } +} + +void CartesianTrajectory::assert_same_reference_frame( + const std::vector& states, const std::string& reference_frame +) const { + if (!std::ranges::all_of(states, [&](const auto& state) { return state.get_reference_frame() == reference_frame; })) { + throw exceptions::IncompatibleReferenceFramesException( + "Incompatible reference frame " + states.front().get_reference_frame() + " and " + reference_frame + ); } } }// namespace state_representation diff --git a/source/state_representation/src/trajectory/JointTrajectory.cpp b/source/state_representation/src/trajectory/JointTrajectory.cpp index 1657a3e25..776bfddbb 100644 --- a/source/state_representation/src/trajectory/JointTrajectory.cpp +++ b/source/state_representation/src/trajectory/JointTrajectory.cpp @@ -3,7 +3,6 @@ #include "state_representation/space/joint/JointState.hpp" #include "state_representation/trajectory/JointTrajectory.hpp" -#include "state_representation/exceptions/EmptyStateException.hpp" #include "state_representation/exceptions/IncompatibleStatesException.hpp" namespace state_representation { @@ -17,9 +16,7 @@ JointTrajectory::JointTrajectory( ) : TrajectoryBase(name) { this->set_type(StateType::JOINT_TRAJECTORY); - if (point.is_empty()) { - throw exceptions::EmptyStateException("Point is empty"); - } + this->assert_points_empty({point}); this->joint_names_ = point.get_names(); this->add_point(point, duration); } @@ -30,119 +27,57 @@ JointTrajectory::JointTrajectory( ) : TrajectoryBase(name) { this->set_type(StateType::JOINT_TRAJECTORY); - if (points.empty()) { - throw exceptions::IncompatibleSizeException("No points provided"); - } else if (std::ranges::any_of(points, [&](const auto& p) { return p.is_empty(); })) { - throw exceptions::EmptyStateException("Vector contains at least one point that is empty"); - } else if (!std::ranges::all_of(points, [&](const auto& p) { return p.get_names() == points.front().get_names(); })) { - throw exceptions::IncompatibleStatesException("Incompatible joint names within the new points vector"); - } + this->assert_points_empty(points); + this->assert_contains_empty_state(points); + this->assert_incompatible_joint_names(points); this->joint_names_ = points[0].get_names(); this->add_points(points, durations); } -void JointTrajectory::add_point(const JointState& new_point, const std::chrono::nanoseconds& duration) { - if (new_point.is_empty()) { - throw exceptions::EmptyStateException("Point is empty"); - } else if (this->joint_names_ != new_point.get_names()) { - throw exceptions::IncompatibleStatesException( - "Incompatible joint names between the new point and current trajectory" - ); - } - JointTrajectoryPoint trajectory_point; - trajectory_point.data = new_point.data(); - trajectory_point.duration = duration; - trajectory_point.name = new_point.get_name(); - this->TrajectoryBase::add_point(trajectory_point); +void JointTrajectory::add_point(const JointState& point, const std::chrono::nanoseconds& duration) { + this->add_points({point}, {duration}); } void JointTrajectory::add_points( - const std::vector& new_points, const std::vector& durations + const std::vector& points, const std::vector& durations ) { - if (new_points.empty()) { - throw exceptions::IncompatibleSizeException("No points provided"); - } else if (new_points.size() != durations.size()) { - throw exceptions::IncompatibleSizeException("The size of the points and durations vectors are not equal"); - } else if (std::ranges::any_of(new_points, [&](const auto& p) { return p.is_empty(); })) { - throw exceptions::EmptyStateException("Vector contains at least one point that is empty"); - } else if (!std::ranges::all_of(new_points, [&](const auto& p) { return p.get_names() == this->joint_names_; })) { - throw exceptions::IncompatibleStatesException("Incompatible joint names within the new points vector"); - } - for (unsigned int i = 0; i < new_points.size(); ++i) { - add_point(new_points[i], durations[i]); + this->assert_points_empty(points); + this->assert_points_durations_sizes_equal(points, durations); + this->assert_contains_empty_state(points); + this->assert_incompatible_joint_names(points, this->joint_names_); + for (unsigned int i = 0; i < points.size(); ++i) { + this->TrajectoryBase::add_point( + JointTrajectoryPoint(points[i].get_name(), points[i].data(), durations[i]) + ); } } void JointTrajectory::insert_point( - const JointState& new_point, const std::chrono::nanoseconds& duration, unsigned int index + const JointState& point, const std::chrono::nanoseconds& duration, unsigned int index ) { - if (new_point.is_empty()) { - throw exceptions::EmptyStateException("Point is empty"); - } else if (this->joint_names_ != new_point.get_names()) { - throw exceptions::IncompatibleStatesException( - "Incompatible joint names between the new point and current trajectory" - ); - } - try { - JointTrajectoryPoint trajectory_point; - trajectory_point.data = new_point.data(); - trajectory_point.duration = duration; - trajectory_point.name = new_point.get_name(); - this->TrajectoryBase::insert_point(trajectory_point, index); - } catch (...) { - throw; - } + this->assert_contains_empty_state({point}); + this->assert_incompatible_joint_names({point}, this->joint_names_); + this->TrajectoryBase::insert_point( + JointTrajectoryPoint(point.get_name(), point.data(), duration), index + ); } void JointTrajectory::set_point(const JointState& point, const std::chrono::nanoseconds& duration, unsigned int index) { - if (point.is_empty()) { - throw exceptions::EmptyStateException("Point is empty"); - } else if (point.get_names() != this->joint_names_) { - throw exceptions::IncompatibleStatesException( - "Incompatible joint names between the new point and current trajectory" - ); - } - JointTrajectoryPoint trajectory_point; - trajectory_point.data = point.data(); - trajectory_point.duration = duration; - trajectory_point.name = point.get_name(); - try { - this->TrajectoryBase::set_point(trajectory_point, index); - } catch (...) { - throw; - } + this->assert_contains_empty_state({point}); + this->assert_incompatible_joint_names({point}, this->joint_names_); + this->TrajectoryBase::set_point( + JointTrajectoryPoint(point.get_name(), point.data(), duration), index + ); } void JointTrajectory::set_points( const std::vector& points, const std::vector& durations ) { - if (points.empty()) { - throw exceptions::EmptyStateException("Points vector is empty"); - } else if (points.size() != this->get_size()) { - throw exceptions::IncompatibleSizeException("The size of the current vector and the new vector are not equal"); - } else if (points.size() != durations.size()) { - throw exceptions::IncompatibleSizeException("The size of the points and durations vectors are not equal"); - } - - std::vector trajectory_points; + this->assert_points_empty(points); + this->assert_points_size(points); + this->assert_points_durations_sizes_equal(points, durations); for (unsigned int i = 0; i < points.size(); ++i) { - if (points[i].is_empty()) { - throw exceptions::EmptyStateException("Vector contains at least one point that is empty"); - } else if (points[i].get_names() != this->joint_names_) { - throw exceptions::IncompatibleStatesException( - "Incompatible joint names between the new point and current trajectory" - ); - } - JointTrajectoryPoint trajectory_point; - trajectory_point.data = points[i].data(); - trajectory_point.duration = durations[i]; - trajectory_point.name = points[i].get_name(); - trajectory_points.push_back(trajectory_point); - } - try { - this->TrajectoryBase::set_points(trajectory_points); - } catch (...) { - throw; + this->set_point(points[i], durations[i], i); } } @@ -163,22 +98,28 @@ const std::vector JointTrajectory::get_points() const { } const JointState JointTrajectory::get_point(unsigned int index) const { - try { - return this->operator[](index).first; - } catch (...) { - throw; - } + return this->operator[](index).first; } std::pair JointTrajectory::operator[](unsigned int idx) const { - try { - auto point = this->TrajectoryBase::operator[](idx); - JointState state(point.name, this->joint_names_); - state.set_data(point.data); - auto duration = point.duration; - return std::make_pair(state, duration); - } catch (...) { - throw; + auto point = this->TrajectoryBase::operator[](idx); + JointState state(point.name, this->joint_names_); + state.set_data(point.data); + auto duration = point.duration; + return std::make_pair(state, duration); +} + +void JointTrajectory::assert_incompatible_joint_names(const std::vector& states) const { + if (!states.empty()) { + this->assert_incompatible_joint_names(states, states[0].get_names()); + } +} + +void JointTrajectory::assert_incompatible_joint_names( + const std::vector& states, const std::vector& joint_names +) const { + if (!std::ranges::all_of(states, [&](const auto& state) { return state.get_names() == joint_names; })) { + throw exceptions::IncompatibleStatesException("Incompatible joint names"); } } }// namespace state_representation From 55afa45be0175f3e08bf262da268c3e98d0cc3e1 Mon Sep 17 00:00:00 2001 From: bpapaspyros Date: Fri, 31 Jan 2025 11:44:35 +0100 Subject: [PATCH 50/52] fix: more constructor touch ups and docstring updates --- .../trajectory/CartesianTrajectory.hpp | 11 +++++------ .../trajectory/JointTrajectory.hpp | 9 ++++----- .../src/trajectory/CartesianTrajectory.cpp | 10 ++-------- .../src/trajectory/JointTrajectory.cpp | 8 ++------ 4 files changed, 13 insertions(+), 25 deletions(-) diff --git a/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp b/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp index 6a1f1cd55..554e5962d 100644 --- a/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp +++ b/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp @@ -47,19 +47,18 @@ class CartesianTrajectory : public TrajectoryBase { explicit CartesianTrajectory(const std::string& name, const std::string& reference_frame = "world"); /** - * @brief Constructor with initial point, duration, name, and reference frame provided + * @brief Constructor with name, initial point, and duration provided * @param point the initial point * @param duration the initial duration * @param name the name of the state * @throw EmptyStateException if point is empty - * @throw IncompatibleReferenceFramesException if point has different reference frame */ explicit CartesianTrajectory( const std::string& name, const CartesianState& point, const std::chrono::nanoseconds& duration ); /** - * @brief Constructor with intial points, durations, name and reference frame provided + * @brief Constructor with name, intial points, and durations provided * @param points vector of initial points * @param durations vector of initial durations * @param name the name of the state @@ -73,14 +72,14 @@ class CartesianTrajectory : public TrajectoryBase { ); /** - * @brief Getter of the reference frame as const reference + * @brief Getter of the reference frame * @return the reference frame associated with the trajectory */ const std::string& get_reference_frame() const; /** - * @brief Getter of the reference frame as const reference - * @param the reference frame associated with the trajectory + * @brief Setter of the reference frame that applies a transformation to all existing points to change the reference frame + * @param pose the new pose that needs to be applied to existing points to change the reference frame * @throws EmptyStateException if pose is empty */ void set_reference_frame(const CartesianPose& pose); diff --git a/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp b/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp index 37c007669..1dd18826f 100644 --- a/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp +++ b/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp @@ -33,12 +33,11 @@ class JointTrajectory : public TrajectoryBase { public: /** * @brief Constructor with name and reference frame provided - * @param name the name of the state */ explicit JointTrajectory(const std::string& name = ""); /** - * @brief Constructor with initial point, duration, name, and reference frame provided + * @brief Constructor with name, initial point, and duration provided * @param name the name of the state * @param point the initial point * @param duration the initial duration @@ -46,7 +45,7 @@ class JointTrajectory : public TrajectoryBase { explicit JointTrajectory(const std::string& name, const JointState& point, const std::chrono::nanoseconds& duration); /** - * @brief Constructor with initial points, durations, name, and reference frame provided + * @brief Constructor with name, initial points, and durations provided * @param name the name of the state * @param points vector of initial points * @param durations vector of initial durations @@ -73,7 +72,7 @@ class JointTrajectory : public TrajectoryBase { * @param point the new trajectory point * @param duration the duration for the new point * @throw EmptyStateException if point is empty - * @throw IncompatibleStatesException if point has different robot name + * @throw IncompatibleStatesException if point has different joint names */ void add_point(const JointState& point, const std::chrono::nanoseconds& duration); @@ -83,7 +82,7 @@ class JointTrajectory : public TrajectoryBase { * @param durations the duration for the new point * @throw IncompatibleSizeException if points and durations have different sizes * @throw EmptyStateException if point is empty - * @throw IncompatibleStatesException if point has different robot name + * @throw IncompatibleStatesException if any of the points has different joint names */ void add_points(const std::vector& points, const std::vector& durations); diff --git a/source/state_representation/src/trajectory/CartesianTrajectory.cpp b/source/state_representation/src/trajectory/CartesianTrajectory.cpp index 6945e96c0..d2e51e9cd 100644 --- a/source/state_representation/src/trajectory/CartesianTrajectory.cpp +++ b/source/state_representation/src/trajectory/CartesianTrajectory.cpp @@ -16,10 +16,7 @@ CartesianTrajectory::CartesianTrajectory(const std::string& name, const std::str CartesianTrajectory::CartesianTrajectory( const std::string& name, const CartesianState& point, const std::chrono::nanoseconds& duration ) - : TrajectoryBase(name) { - this->set_type(StateType::CARTESIAN_TRAJECTORY); - this->assert_points_empty({point}); - this->reference_frame_ = point.get_reference_frame(); + : CartesianTrajectory(name, point.get_reference_frame()) { this->add_point(point, duration); } @@ -27,11 +24,8 @@ CartesianTrajectory::CartesianTrajectory( const std::string& name, const std::vector& points, const std::vector& durations ) - : TrajectoryBase(name) { - this->set_type(StateType::CARTESIAN_TRAJECTORY); + : CartesianTrajectory(name) { this->assert_points_empty(points); - this->assert_contains_empty_state(points); - this->assert_same_reference_frame(points); this->reference_frame_ = points[0].get_reference_frame(); this->add_points(points, durations); } diff --git a/source/state_representation/src/trajectory/JointTrajectory.cpp b/source/state_representation/src/trajectory/JointTrajectory.cpp index 776bfddbb..75ccac617 100644 --- a/source/state_representation/src/trajectory/JointTrajectory.cpp +++ b/source/state_representation/src/trajectory/JointTrajectory.cpp @@ -14,8 +14,7 @@ JointTrajectory::JointTrajectory(const std::string& name) : TrajectoryBase(name) { - this->set_type(StateType::JOINT_TRAJECTORY); + : JointTrajectory(name) { this->assert_points_empty({point}); this->joint_names_ = point.get_names(); this->add_point(point, duration); @@ -25,11 +24,8 @@ JointTrajectory::JointTrajectory( const std::string& name, const std::vector& points, const std::vector& durations ) - : TrajectoryBase(name) { - this->set_type(StateType::JOINT_TRAJECTORY); + : JointTrajectory(name) { this->assert_points_empty(points); - this->assert_contains_empty_state(points); - this->assert_incompatible_joint_names(points); this->joint_names_ = points[0].get_names(); this->add_points(points, durations); } From 822401b1b4238b4e4c29b2b9a4a52350892cf86e Mon Sep 17 00:00:00 2001 From: bpapaspyros Date: Fri, 31 Jan 2025 12:04:59 +0100 Subject: [PATCH 51/52] feat: add getters for individual duration & time from start --- .../trajectory/TrajectoryBase.hpp | 30 +++++++++++++++++++ .../test/tests/test_trajectory.cpp | 2 ++ 2 files changed, 32 insertions(+) diff --git a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp index bd2f2dff9..479e2850e 100644 --- a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp +++ b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp @@ -43,12 +43,26 @@ struct TrajectoryPoint { template class TrajectoryBase : public State { public: + /** + * @brief Get the duration of the trajectory point at given index + * @param index the index + * @return the duration of the trajectory point + */ + const std::chrono::nanoseconds& get_duration(unsigned int index) const; + /** * @brief Get list of trajectory point durations * @return the list of trajectory point durations */ const std::vector get_durations() const; + /** + * @brief Get the time from start of the trajectory point at given index + * @param index the index + * @return the time from start of the trajectory point + */ + const std::chrono::nanoseconds get_time_from_start(unsigned int index) const; + /** * @brief Get list of trajectory point times from start * @return the list of trajectory point times from start @@ -317,6 +331,12 @@ inline void TrajectoryBase::set_points(const std::vector +inline const std::chrono::nanoseconds& TrajectoryBase::get_duration(unsigned int index) const { + this->assert_index_in_range(index); + return this->points_[index].duration; +} + template inline const std::vector TrajectoryBase::get_durations() const { std::vector durations; @@ -326,6 +346,16 @@ inline const std::vector TrajectoryBase:: return durations; } +template +inline const std::chrono::nanoseconds TrajectoryBase::get_time_from_start(unsigned int index) const { + this->assert_index_in_range(index); + std::chrono::nanoseconds time_from_start = std::chrono::nanoseconds(0); + for (unsigned int i = 0; i <= index; ++i) { + time_from_start += this->points_[i].duration; + } + return time_from_start; +} + template inline const std::vector TrajectoryBase::get_times_from_start() const { std::vector times_from_start; diff --git a/source/state_representation/test/tests/test_trajectory.cpp b/source/state_representation/test/tests/test_trajectory.cpp index 2101f583e..721c84fbe 100644 --- a/source/state_representation/test/tests/test_trajectory.cpp +++ b/source/state_representation/test/tests/test_trajectory.cpp @@ -354,6 +354,8 @@ TYPED_TEST_P(TrajectoryTest, Getters) { for (unsigned int i = 0; i < this->trajectory->get_size(); ++i) { time_from_start += durations[i]; EXPECT_EQ(times_from_start[i], time_from_start); + EXPECT_EQ(this->trajectory->get_time_from_start(i), time_from_start); + EXPECT_EQ(this->trajectory->get_duration(i), durations[i]); } this->trajectory->reset(); From bd0c01cc8ceec9881d9ea5a1bb6c390efb212256 Mon Sep 17 00:00:00 2001 From: bpapaspyros Date: Fri, 31 Jan 2025 12:10:39 +0100 Subject: [PATCH 52/52] feat: add function that returns the total trajectory duration --- .../trajectory/TrajectoryBase.hpp | 11 +++++++++++ .../test/tests/test_trajectory.cpp | 1 + 2 files changed, 12 insertions(+) diff --git a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp index 479e2850e..16f8bf295 100644 --- a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp +++ b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp @@ -69,6 +69,12 @@ class TrajectoryBase : public State { */ const std::vector get_times_from_start() const; + /** + * @brief Get the total duration of the trajectory + * @return the total duration of the trajectory + */ + const std::chrono::nanoseconds get_trajectory_duration() const; + /** * @brief Get number of points in trajectory * @return the number of points in trajectory @@ -367,6 +373,11 @@ inline const std::vector TrajectoryBase:: return times_from_start; } +template +inline const std::chrono::nanoseconds TrajectoryBase::get_trajectory_duration() const { + return this->get_size() ? this->get_times_from_start().back() : std::chrono::nanoseconds(0); +} + template inline unsigned int TrajectoryBase::get_size() const { return this->points_.size(); diff --git a/source/state_representation/test/tests/test_trajectory.cpp b/source/state_representation/test/tests/test_trajectory.cpp index 721c84fbe..4f47f72e7 100644 --- a/source/state_representation/test/tests/test_trajectory.cpp +++ b/source/state_representation/test/tests/test_trajectory.cpp @@ -357,6 +357,7 @@ TYPED_TEST_P(TrajectoryTest, Getters) { EXPECT_EQ(this->trajectory->get_time_from_start(i), time_from_start); EXPECT_EQ(this->trajectory->get_duration(i), durations[i]); } + EXPECT_EQ(this->trajectory->get_trajectory_duration(), time_from_start); this->trajectory->reset(); EXPECT_EQ(this->trajectory->get_size(), 0);