From 8014e8d06632b075fca8d6c6496294c996a5718f Mon Sep 17 00:00:00 2001 From: Vaios Papaspyros <8146703+bpapaspyros@users.noreply.github.com> Date: Tue, 4 Feb 2025 19:19:36 +0100 Subject: [PATCH 1/2] feat!: split trajectory classes and implement Cartesian/JointState specializations (#216) --- CHANGELOG.md | 1 + .../state_representation/bind_state.cpp | 3 +- source/state_representation/CMakeLists.txt | 2 + .../state_representation/StateType.hpp | 53 +- .../trajectories/Trajectory.hpp | 254 ---------- .../trajectory/CartesianTrajectory.hpp | 182 +++++++ .../trajectory/JointTrajectory.hpp | 169 +++++++ .../trajectory/TrajectoryBase.hpp | 466 ++++++++++++++++++ .../src/trajectory/CartesianTrajectory.cpp | 122 +++++ .../src/trajectory/JointTrajectory.cpp | 113 +++++ .../test/tests/test_trajectory.cpp | 462 +++++++++++++---- 11 files changed, 1441 insertions(+), 386 deletions(-) delete mode 100644 source/state_representation/include/state_representation/trajectories/Trajectory.hpp create mode 100644 source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp create mode 100644 source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp create mode 100644 source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp create mode 100644 source/state_representation/src/trajectory/CartesianTrajectory.cpp create mode 100644 source/state_representation/src/trajectory/JointTrajectory.cpp diff --git a/CHANGELOG.md b/CHANGELOG.md index 5bb44623e..4d694438d 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -30,6 +30,7 @@ Release Versions - feat: add functionality to create a robot model from string (#200) - feat: update dependencies (#272) - feat: add joint types & limit some uses to supported types only (#243) +- feat: split trajectory classes and implement Cartesian/JointState specializations (#216) ## 9.3.2 diff --git a/python/source/state_representation/bind_state.cpp b/python/source/state_representation/bind_state.cpp index bd818babe..152e84f03 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(); diff --git a/source/state_representation/CMakeLists.txt b/source/state_representation/CMakeLists.txt index de4f8f8dc..2ca433346 100644 --- a/source/state_representation/CMakeLists.txt +++ b/source/state_representation/CMakeLists.txt @@ -30,6 +30,8 @@ set(CORE_SOURCES src/parameters/Predicate.cpp src/geometry/Shape.cpp src/geometry/Ellipsoid.cpp + src/trajectory/CartesianTrajectory.cpp + src/trajectory/JointTrajectory.cpp ) if (EXPERIMENTAL_FEATURES) diff --git a/source/state_representation/include/state_representation/StateType.hpp b/source/state_representation/include/state_representation/StateType.hpp index 630e44660..c08db0b74 100644 --- a/source/state_representation/include/state_representation/StateType.hpp +++ b/source/state_representation/include/state_representation/StateType.hpp @@ -13,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, + DIGITAL_IO_STATE = 18, + ANALOG_IO_STATE = 19, + CARTESIAN_TRAJECTORY = 20, + JOINT_TRAJECTORY = 21, #ifdef EXPERIMENTAL_FEATURES - DUAL_QUATERNION_STATE, - DUAL_QUATERNION_POSE, - DUAL_QUATERNION_TWIST + DUAL_QUATERNION_STATE = 22, + DUAL_QUATERNION_POSE = 23, + DUAL_QUATERNION_TWIST = 24 #endif }; @@ -77,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/Trajectory.hpp b/source/state_representation/include/state_representation/trajectories/Trajectory.hpp deleted file mode 100644 index 8a9e18bf5..000000000 --- a/source/state_representation/include/state_representation/trajectories/Trajectory.hpp +++ /dev/null @@ -1,254 +0,0 @@ -#pragma once - -#include -#include - -#include "state_representation/State.hpp" - -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/trajectory/CartesianTrajectory.hpp b/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp new file mode 100644 index 000000000..06af4ad0a --- /dev/null +++ b/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp @@ -0,0 +1,182 @@ +#pragma once + +#include "state_representation/space/cartesian/CartesianPose.hpp" +#include "state_representation/space/cartesian/CartesianState.hpp" +#include "state_representation/trajectory/TrajectoryBase.hpp" + +namespace state_representation { + +/** + * @class CartesianTrajectoryPoint + * @brief A Cartesian trajectory point representation + */ +struct CartesianTrajectoryPoint : public TrajectoryPoint { + /** + * @brief Empty constructor + */ + CartesianTrajectoryPoint() = default; + + /** + * @brief Constructor from Cartesian state and duration + * @param state the Cartesian state used to initialize the trajectory point + * @param duration the intended duration for the trajectory point + */ + CartesianTrajectoryPoint(const CartesianState& state, const std::chrono::nanoseconds& duration) + : TrajectoryPoint(state.get_name(), state.data(), duration) {} + + /** + * @brief Convert the trajectory point to a Cartesian state + * @param reference_frame the underlying reference frame of the trajectory point + * @return the Cartesian state representation of the trajectory point + */ + CartesianState to_cartesian_state(const std::string& reference_frame) const { + CartesianState state(name, reference_frame); + state.set_data(data); + return state; + } +}; + +/** + * @class CartesianTrajectory + * @brief Class to represent a trajectory of Cartesian points and corresponding durations + */ +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"); + + /** + * @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 + */ + explicit CartesianTrajectory( + const std::string& name, const CartesianState& point, const std::chrono::nanoseconds& duration + ); + + /** + * @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 + * @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 + ); + + /** + * @brief Get the reference frame + * @return the reference frame associated with the trajectory + */ + const std::string& get_reference_frame() const; + + /** + * @brief Set 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); + + /** + * @brief Add new point and corresponding duration to trajectory + * @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& point, const std::chrono::nanoseconds& duration); + + /** + * @brief Add new points and corresponding durations to trajectory + * @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& points, const std::vector& durations); + + /** + * @brief Insert new point and corresponding duration to trajectory between two + * already existing points + * @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& point, const std::chrono::nanoseconds& duration, unsigned int index); + + /** + * @brief Get list of trajectory points + * @return queue of the Cartesian states of the trajectory + */ + const std::vector 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 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 + */ + 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 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 + */ + void set_points(const std::vector& points, const std::vector& durations); + + /** + * @brief Operator overload for returning a single trajectory point and + * corresponding duration + * @return the Cartesian state and duration pair that corresponds to the index + */ + 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 new file mode 100644 index 000000000..9a824403a --- /dev/null +++ b/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp @@ -0,0 +1,169 @@ +#pragma once + +#include "state_representation/space/joint/JointState.hpp" +#include "state_representation/trajectory/TrajectoryBase.hpp" + +namespace state_representation { + +/** + * @class JointTrajectoryPoint + * @brief A joint trajectory point representation + */ +struct JointTrajectoryPoint : public TrajectoryPoint { + /** + * @brief Empty constructor + */ + JointTrajectoryPoint() = default; + + /** + * @brief Constructor from joint state and duration + * @param state the Joint state used to initialize the trajectory point + * @param duration the intended duration for the trajectory point + */ + JointTrajectoryPoint(const JointState& state, const std::chrono::nanoseconds& duration) + : TrajectoryPoint(state.get_name(), state.data(), duration) {} + + /** + * @brief Convert the trajectory point to a joint state + * @param joint_names the joint names of the trajectory point + * @return the joint state representation of the trajectory point + */ + JointState to_joint_state(const std::vector& joint_names) const { + JointState state(name, joint_names); + state.set_data(data); + return state; + } +}; + +/** + * @class JointTrajectory + * @brief Class to represent a trajectory of joint points and corresponding durations + */ +class JointTrajectory : public TrajectoryBase { +public: + /** + * @brief Constructor with name and reference frame provided + */ + explicit JointTrajectory(const std::string& name = ""); + + /** + * @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 + */ + explicit JointTrajectory(const std::string& name, const JointState& point, const std::chrono::nanoseconds& duration); + + /** + * @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 + */ + explicit JointTrajectory( + const std::string& name, const std::vector& points, + const std::vector& durations + ); + + /** + * @brief Get the joint names + * @return vector of joint names associated with the trajectory + */ + const std::vector& get_joint_names() const; + + /** + * @brief Set the joint names + * @param joint_names vector of joint names associated with the trajectory + */ + void set_joint_names(const std::vector& joint_names); + + /** + * @brief Add new point and corresponding duration to trajectory + * @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 joint names + */ + void add_point(const JointState& point, const std::chrono::nanoseconds& duration); + + /** + * @brief Add new points and corresponding durations to trajectory + * @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 any of the points has different joint names + */ + 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 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& point, const std::chrono::nanoseconds& duration, unsigned int index); + + /** + * @brief Get list of trajectory points + * @return vector of the Joint states of the trajectory + */ + const std::vector 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 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 IncompatibleStatesException if point has different joint names to the current ones + */ + 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 duration vector of new durations + * @throw IncompatibleSizeException if points and durations have different sizes + * @throw EmptyStateException if point is empty + * @throw IncompatibleStatesException if any of the points has different joint names to the current ones + */ + void set_points(const std::vector& points, const std::vector& durations); + + /** + * @brief Operator overload for returning a single trajectory point and + * corresponding duration + * @return the Joint state and duration pair that corresponds to the index + */ + 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 new file mode 100644 index 000000000..dd26e61eb --- /dev/null +++ b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp @@ -0,0 +1,466 @@ +#pragma once + +#include +#include +#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 { + +/** + * @class TrajectoryPoint + * @brief Struct that contains the basic 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; +}; + +/** + * @class TrajectoryBase + * @brief Core class that contains trajectory-specific operations and data + */ +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 + * @throws EmptyStateException if the trajectory is empty + */ + 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 + * @throws EmptyStateException if the trajectory is empty + */ + 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 + * @throws EmptyStateException if the trajectory is empty + */ + const std::vector get_times_from_start() const; + + /** + * @brief Get the total duration of the trajectory + * @return the total duration of the trajectory + * @throws EmptyStateException if the trajectory is empty + */ + const std::chrono::nanoseconds get_trajectory_duration() const; + + /** + * @brief Get number of points in trajectory + * @return the number of points in trajectory + */ + unsigned int get_size() const; + + /** + * @brief Delete the last point from trajectory + */ + 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 reset(); + +protected: + /** + * @brief Empty constructor + */ + explicit TrajectoryBase() = default; + + /** + * @brief Constructor with name provided + * @param name the name of the state + */ + explicit TrajectoryBase(const std::string& name); + + /** + * @brief Get list of trajectory points + * @return the list of trajectory points + */ + const std::vector 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 index is out of range + */ + const TrajectoryT& get_point(unsigned int index) const; + + /** + * @brief Add new point to trajectory + * @param new_point the new point + */ + void add_point(const TrajectoryT& new_point); + + /** + * @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); + + /** + * @brief Insert new trajectory point between two already existing points + * @param new_point the new point + * @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 index); + + /** + * @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 index the index + * @throw std::out_of_range if index is out of range + */ + 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 + * @throw IncompatibleSizeException if points vector is empty + */ + void set_points(const std::vector& points); + + /** + * @brief Get a single trajectory point and corresponding time at given 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 index) const; + + /** + * @brief Get a single trajectory point and corresponding time at given index + * @param index the index + * @return the trajectory point + * @throw std::out_of_range if index is out of range + */ + 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_not_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_not_contains_empty_state(const std::vector& states) const; + + /** + * @brief Assert that the trajectory is not empty + * @throws EmptyStateException if any of the elements is empty + */ + void assert_trajectory_not_empty() const; + +private: + std::deque points_; +}; + +template +inline TrajectoryBase::TrajectoryBase(const std::string& name) : State(name) {} + +template +inline void TrajectoryBase::reset() { + this->State::reset(); + this->points_.clear(); +} + +template +inline void TrajectoryBase::add_point(const TrajectoryT& new_point) { + this->points_.push_back(new_point); + this->set_empty(false); +} + +template +inline void TrajectoryBase::add_points(const std::vector& new_points) { + this->assert_points_not_empty(new_points); + for (auto point : new_points) { + points_.push_back(point); + } + this->set_empty(false); +} + +template +inline void TrajectoryBase::insert_point(const TrajectoryT& new_point, unsigned int index) { + this->assert_index_in_range(index); + auto it_points = this->points_.begin(); + std::advance(it_points, index); + this->points_.insert(it_points, new_point); + this->set_empty(false); +} + +template +inline void TrajectoryBase::delete_point() { + if (!this->points_.empty()) { + this->points_.pop_back(); + } + if (this->points_.empty()) { + this->set_empty(true); + } +} + +template +inline void TrajectoryBase::delete_point(unsigned int index) { + this->assert_index_in_range(index); + this->points_.erase(this->points_.begin() + index); + if (this->points_.empty()) { + this->set_empty(true); + } +} + +template +inline const std::vector TrajectoryBase::get_points() const { + return std::vector(this->points_.begin(), this->points_.end()); +} + +template +inline const TrajectoryT& TrajectoryBase::get_point(unsigned int index) const { + this->assert_index_in_range(index); + return this->points_[index]; +} + +template +inline TrajectoryT& TrajectoryBase::get_point(unsigned int index) { + this->assert_index_in_range(index); + return this->points_[index]; +} + +template +inline void TrajectoryBase::set_point(const TrajectoryT& point, unsigned int index) { + this->assert_index_in_range(index); + this->points_[index] = point; +} + +template +inline void TrajectoryBase::set_points(const std::vector& points) { + this->assert_points_not_empty(points); + this->assert_points_size(points); + std::copy(points.begin(), points.end(), this->points_.begin()); +} + +template +inline const std::chrono::nanoseconds& TrajectoryBase::get_duration(unsigned int index) const { + this->assert_trajectory_not_empty(); + this->assert_index_in_range(index); + return this->points_[index].duration; +} + +template +inline const std::vector TrajectoryBase::get_durations() const { + this->assert_trajectory_not_empty(); + std::vector durations; + std::for_each(this->points_.begin(), this->points_.end(), [&](const auto& point) { + durations.push_back(point.duration); + }); + return durations; +} + +template +inline const std::chrono::nanoseconds TrajectoryBase::get_time_from_start(unsigned int index) const { + this->assert_trajectory_not_empty(); + this->assert_index_in_range(index); + return std::accumulate( + this->points_.begin(), this->points_.begin() + index + 1, std::chrono::nanoseconds(0), + [&](auto acc, const auto& point) { return acc + point.duration; } + ); + ; +} + +template +inline const std::vector TrajectoryBase::get_times_from_start() const { + this->assert_trajectory_not_empty(); + std::vector times_from_start; + std::chrono::nanoseconds time_from_start = std::chrono::nanoseconds(0); + std::transform( + this->points_.begin(), this->points_.end(), std::back_inserter(times_from_start), + [&](const auto& point) { + time_from_start += point.duration; + return time_from_start; + } + ); + return times_from_start; +} + +template +inline const std::chrono::nanoseconds TrajectoryBase::get_trajectory_duration() const { + return this->get_times_from_start().back(); +} + +template +inline unsigned int TrajectoryBase::get_size() const { + this->assert_trajectory_not_empty(); + return this->points_.size(); +} + +template +inline const TrajectoryT& TrajectoryBase::operator[](unsigned int index) const { + 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"); + } +} + +template +template +inline void TrajectoryBase::assert_points_not_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_not_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_not_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 new file mode 100644 index 000000000..2a70bb1c5 --- /dev/null +++ b/source/state_representation/src/trajectory/CartesianTrajectory.cpp @@ -0,0 +1,122 @@ +#include "state_representation/trajectory/CartesianTrajectory.hpp" +#include "state_representation/space/cartesian/CartesianState.hpp" + +#include "state_representation/exceptions/IncompatibleReferenceFramesException.hpp" +#include + +namespace state_representation { +CartesianTrajectory::CartesianTrajectory() : TrajectoryBase() { + this->set_type(StateType::CARTESIAN_TRAJECTORY); +} + +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 +) + : CartesianTrajectory(name, point.get_reference_frame()) { + this->add_point(point, duration); +} + +CartesianTrajectory::CartesianTrajectory( + const std::string& name, const std::vector& points, + const std::vector& durations +) + : CartesianTrajectory(name) { + this->assert_points_not_empty(points); + this->reference_frame_ = points[0].get_reference_frame(); + this->add_points(points, durations); +} + +const std::string& CartesianTrajectory::get_reference_frame() const { + return this->reference_frame_; +} + +void CartesianTrajectory::set_reference_frame(const CartesianPose& pose) { + this->assert_trajectory_not_empty(); + this->reference_frame_ = pose.get_reference_frame(); + auto points = this->get_points(); + std::transform(points.begin(), points.end(), points.begin(), [&](const auto& point) { return point * pose; }); + this->set_points(points, this->get_durations()); +} + +void CartesianTrajectory::add_point(const CartesianState& point, const std::chrono::nanoseconds& duration) { + this->add_points({point}, {duration}); +} + +void CartesianTrajectory::add_points( + const std::vector& points, const std::vector& durations +) { + this->assert_points_not_empty(points); + this->assert_points_durations_sizes_equal(points, durations); + this->assert_not_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], durations[i])); + } +} + +void CartesianTrajectory::insert_point( + const CartesianState& point, const std::chrono::nanoseconds& duration, unsigned int index +) { + this->assert_not_contains_empty_state({point}); + this->assert_same_reference_frame({point}, this->reference_frame_); + this->TrajectoryBase::insert_point(CartesianTrajectoryPoint(point, duration), index); +} + +void CartesianTrajectory::set_point( + const CartesianState& point, const std::chrono::nanoseconds& duration, unsigned int index +) { + this->assert_not_contains_empty_state({point}); + this->assert_same_reference_frame({point}, this->reference_frame_); + this->TrajectoryBase::set_point(CartesianTrajectoryPoint(point, duration), index); +} + +void CartesianTrajectory::set_points( + const std::vector& points, const std::vector& durations +) { + this->assert_points_not_empty(points); + this->assert_points_size(points); + this->assert_points_durations_sizes_equal(points, durations); + for (unsigned int i = 0; i < points.size(); ++i) { + this->set_point(points[i], durations[i], i); + } +} + +const std::vector CartesianTrajectory::get_points() const { + std::vector points; + auto queue = this->TrajectoryBase::get_points(); + std::transform(queue.begin(), queue.end(), std::back_inserter(points), [&](const auto& point) { + return point.to_cartesian_state(this->reference_frame_); + }); + return points; +} + +CartesianState CartesianTrajectory::get_point(unsigned int index) const { + return this->TrajectoryBase::get_point(index).to_cartesian_state(this->reference_frame_); +} + +std::pair CartesianTrajectory::operator[](unsigned int idx) const { + auto point = this->TrajectoryBase::operator[](idx); + return std::make_pair(point.to_cartesian_state(this->reference_frame_), point.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 new file mode 100644 index 000000000..a0bb03f41 --- /dev/null +++ b/source/state_representation/src/trajectory/JointTrajectory.cpp @@ -0,0 +1,113 @@ +#include + +#include "state_representation/space/joint/JointState.hpp" +#include "state_representation/trajectory/JointTrajectory.hpp" + +#include "state_representation/exceptions/IncompatibleStatesException.hpp" + +namespace state_representation { + +JointTrajectory::JointTrajectory(const std::string& name) : TrajectoryBase(name) { + this->set_type(StateType::JOINT_TRAJECTORY); +} + +JointTrajectory::JointTrajectory( + const std::string& name, const JointState& point, const std::chrono::nanoseconds& duration +) + : JointTrajectory(name) { + this->assert_points_not_empty({point}); + this->joint_names_ = point.get_names(); + this->add_point(point, duration); +} + +JointTrajectory::JointTrajectory( + const std::string& name, const std::vector& points, + const std::vector& durations +) + : JointTrajectory(name) { + this->assert_points_not_empty(points); + this->joint_names_ = points[0].get_names(); + this->add_points(points, durations); +} + +void JointTrajectory::add_point(const JointState& point, const std::chrono::nanoseconds& duration) { + this->add_points({point}, {duration}); +} + +void JointTrajectory::add_points( + const std::vector& points, const std::vector& durations +) { + this->assert_points_not_empty(points); + this->assert_points_durations_sizes_equal(points, durations); + this->assert_not_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], durations[i])); + } +} + +void JointTrajectory::insert_point( + const JointState& point, const std::chrono::nanoseconds& duration, unsigned int index +) { + this->assert_not_contains_empty_state({point}); + this->assert_incompatible_joint_names({point}, this->joint_names_); + this->TrajectoryBase::insert_point(JointTrajectoryPoint(point, duration), index); +} + +void JointTrajectory::set_point(const JointState& point, const std::chrono::nanoseconds& duration, unsigned int index) { + this->assert_not_contains_empty_state({point}); + this->assert_incompatible_joint_names({point}, this->joint_names_); + this->TrajectoryBase::set_point(JointTrajectoryPoint(point, duration), index); +} + +void JointTrajectory::set_points( + const std::vector& points, const std::vector& durations +) { + this->assert_points_not_empty(points); + this->assert_points_size(points); + this->assert_points_durations_sizes_equal(points, durations); + for (unsigned int i = 0; i < points.size(); ++i) { + this->set_point(points[i], durations[i], i); + } +} + +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::vector JointTrajectory::get_points() const { + std::vector points; + auto queue = this->TrajectoryBase::get_points(); + std::transform(queue.begin(), queue.end(), std::back_inserter(points), [&](const auto& point) { + return point.to_joint_state(this->joint_names_); + }); + return points; +} + +const JointState JointTrajectory::get_point(unsigned int index) const { + return this->TrajectoryBase::get_point(index).to_joint_state(this->joint_names_); +} + +std::pair JointTrajectory::operator[](unsigned int idx) const { + auto point = this->TrajectoryBase::operator[](idx); + return std::make_pair(point.to_joint_state(this->joint_names_), point.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 diff --git a/source/state_representation/test/tests/test_trajectory.cpp b/source/state_representation/test/tests/test_trajectory.cpp index ad123bd03..35dbf80c2 100644 --- a/source/state_representation/test/tests/test_trajectory.cpp +++ b/source/state_representation/test/tests/test_trajectory.cpp @@ -1,127 +1,377 @@ -#include "state_representation/space/joint/JointState.hpp" -#include "state_representation/trajectories/Trajectory.hpp" +#include #include -#include - -TEST(TrajectoryTest, CreateTrajectory) { - state_representation::Trajectory trajectory; - std::deque points = trajectory.get_points(); - std::deque times = trajectory.get_times(); - EXPECT_TRUE(points.empty()); - EXPECT_TRUE(times.empty()); +#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/exceptions/IncompatibleStatesException.hpp" + +using namespace state_representation; + +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[]; + + TrajectoryBaseInterface() : TrajectoryBase() {} + TrajectoryBaseInterface(const std::string& name) : TrajectoryBase(name) {} +}; + +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); + } + } + + 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); + } + } + + 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); + } + } + + 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); + } + } + + 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); + } + } + + 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); + } + } + + 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 + ); } -TEST(TrajectoryTest, AddPoint) { - state_representation::Trajectory trajectory; - state_representation::JointState point("robot", 1); +TYPED_TEST_P(TrajectoryTest, AddRemovePoints) { + EXPECT_NO_THROW(this->trajectory = std::make_shared("trajectory")); + EXPECT_THROW(this->trajectory->get_size(), exceptions::EmptyStateException); - std::deque points = trajectory.get_points(); - std::deque times = trajectory.get_times(); + using PointType = typename std::conditional< + std::is_same_v, TrajectoryPoint, + typename std::conditional, CartesianState, JointState>::type>:: + type; - unsigned int prev_size_points = points.size(); - unsigned int prev_size_times = times.size(); + PointType point0; + PointType point1; + PointType point2; + PointType point3; - std::chrono::nanoseconds period(100); - Eigen::ArrayXd positions(1); - positions << 0.2; - point.set_positions(positions); - trajectory.add_point(point, period); + 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()); + } - points = trajectory.get_points(); - times = trajectory.get_times(); + // 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); - unsigned int new_size_points = points.size(); - unsigned int new_size_times = times.size(); + // 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_THROW(this->trajectory->get_size(), exceptions::EmptyStateException); - EXPECT_TRUE(new_size_points == prev_size_points + 1); - EXPECT_TRUE(new_size_times == prev_size_times + 1); + // 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) + }; + 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]); + } + 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]); + } } -TEST(TrajectoryTest, ClearPoint) { - state_representation::Trajectory trajectory; - state_representation::JointState point("robot", 1); +TYPED_TEST_P(TrajectoryTest, Exceptions) { + EXPECT_NO_THROW(this->trajectory = std::make_shared("trajectory")); + EXPECT_THROW(this->trajectory->get_size(), exceptions::EmptyStateException); + + 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; - std::chrono::nanoseconds period(100); - Eigen::ArrayXd positions(1); - positions << 0.2; - point.set_positions(positions); - trajectory.add_point(point, period); + 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()); + } - std::deque points = trajectory.get_points(); - std::deque times = trajectory.get_times(); + 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)); - unsigned int size_points = points.size(); - unsigned int size_times = times.size(); + EXPECT_THROW(this->delete_point_index(10), std::out_of_range); - EXPECT_TRUE(size_points == 1); - EXPECT_TRUE(size_times == 1); + points.push_back(point1); + durations.push_back(std::chrono::nanoseconds(40)); + EXPECT_THROW(this->set_points(points, durations), exceptions::IncompatibleSizeException); - trajectory.clear(); - points = trajectory.get_points(); - times = trajectory.get_times(); + 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); + } - EXPECT_TRUE(points.empty()); - EXPECT_TRUE(times.empty()); + 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); } -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); +TYPED_TEST_P(TrajectoryTest, Getters) { + EXPECT_NO_THROW(this->trajectory = std::make_shared("trajectory")); + EXPECT_THROW(this->trajectory->get_size(), exceptions::EmptyStateException); + + 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()); + } + + 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)); + ASSERT_FALSE(this->trajectory->is_empty()); + 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); + 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_TRUE(this->trajectory->is_empty()); + EXPECT_THROW(this->trajectory->get_size(), exceptions::EmptyStateException); + EXPECT_THROW(this->trajectory->get_durations(), exceptions::EmptyStateException); + 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); + } } -// 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); - -// positions << 0.8; -// 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(); - -// 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); -// } +REGISTER_TYPED_TEST_SUITE_P(TrajectoryTest, AddRemovePoints, Exceptions, Getters); + +using TrajectoryTypes = testing::Types; +INSTANTIATE_TYPED_TEST_SUITE_P(Type, TrajectoryTest, TrajectoryTypes); From a43c74c157e0e49d9dcb226cb32a040d42bd9bfe Mon Sep 17 00:00:00 2001 From: Dominic Reber <71256590+domire8@users.noreply.github.com> Date: Thu, 6 Feb 2025 10:06:16 +0100 Subject: [PATCH 2/2] feat: minor improvements (#223) --- CHANGELOG.md | 2 +- .../trajectory/CartesianTrajectory.hpp | 30 +++--- .../trajectory/JointTrajectory.hpp | 39 ++++---- .../trajectory/TrajectoryBase.hpp | 96 +++++-------------- .../src/trajectory/CartesianTrajectory.cpp | 29 +++--- .../src/trajectory/JointTrajectory.cpp | 54 +++++------ .../test/tests/test_trajectory.cpp | 10 +- 7 files changed, 104 insertions(+), 156 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 4d694438d..1b4bf956b 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -30,7 +30,7 @@ Release Versions - feat: add functionality to create a robot model from string (#200) - feat: update dependencies (#272) - feat: add joint types & limit some uses to supported types only (#243) -- feat: split trajectory classes and implement Cartesian/JointState specializations (#216) +- feat: split trajectory classes and implement Cartesian/JointState specializations (#216, #217, #223) ## 9.3.2 diff --git a/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp b/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp index 06af4ad0a..84bb0ee55 100644 --- a/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp +++ b/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp @@ -56,9 +56,9 @@ class CartesianTrajectory : public TrajectoryBase { /** * @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 - * @param name the name of the state * @throw EmptyStateException if point is empty */ explicit CartesianTrajectory( @@ -67,9 +67,9 @@ class CartesianTrajectory : public TrajectoryBase { /** * @brief Constructor with name, intial points, and durations 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 * @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 @@ -92,6 +92,19 @@ class CartesianTrajectory : public TrajectoryBase { */ void set_reference_frame(const CartesianPose& pose); + /** + * @brief Get list of trajectory points + * @return queue of the Cartesian states of the trajectory + */ + const std::vector 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 Add new point and corresponding duration to trajectory * @param point the new trajectory point @@ -120,19 +133,6 @@ class CartesianTrajectory : public TrajectoryBase { */ void insert_point(const CartesianState& point, const std::chrono::nanoseconds& duration, unsigned int index); - /** - * @brief Get list of trajectory points - * @return queue of the Cartesian states of the trajectory - */ - const std::vector 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 diff --git a/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp b/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp index 9a824403a..03df9623d 100644 --- a/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp +++ b/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp @@ -42,7 +42,7 @@ struct JointTrajectoryPoint : public TrajectoryPoint { class JointTrajectory : public TrajectoryBase { public: /** - * @brief Constructor with name and reference frame provided + * @brief Constructor with optional name */ explicit JointTrajectory(const std::string& name = ""); @@ -77,6 +77,19 @@ class JointTrajectory : public TrajectoryBase { */ void set_joint_names(const std::vector& joint_names); + /** + * @brief Get list of trajectory points + * @return vector of the Joint states of the trajectory + */ + const std::vector 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 Add new point and corresponding duration to trajectory * @param point the new trajectory point @@ -105,19 +118,6 @@ class JointTrajectory : public TrajectoryBase { */ void insert_point(const JointState& point, const std::chrono::nanoseconds& duration, unsigned int index); - /** - * @brief Get list of trajectory points - * @return vector of the Joint states of the trajectory - */ - const std::vector 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 @@ -150,19 +150,18 @@ class JointTrajectory : public TrajectoryBase { /** * @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 + * @throw IncompatibleStatesException if a state has a different joint names */ - void assert_incompatible_joint_names(const std::vector& states) const; + void assert_compatible_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 + * @throw IncompatibleStatesException if a state has a different joint names */ - void assert_incompatible_joint_names( - const std::vector& states, const std::vector& joint_names - ) const; + void assert_compatible_joint_names(const std::vector& states, const std::vector& joint_names) + const; std::vector joint_names_;///< names of the joints }; diff --git a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp index dd26e61eb..1993ef22f 100644 --- a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp +++ b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp @@ -151,13 +151,6 @@ class TrajectoryBase : public State { */ void insert_point(const TrajectoryT& new_point, unsigned int index); - /** - * @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 @@ -177,7 +170,6 @@ class TrajectoryBase : public State { * @brief Get a single trajectory point and corresponding time at given 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 index) const; @@ -221,21 +213,7 @@ class TrajectoryBase : public State { */ 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; + const std::vector& points, const std::vector& durations) const; /** * @brief Assert that vector of State type does not contain empty elements @@ -246,12 +224,6 @@ class TrajectoryBase : public State { requires std::derived_from void assert_not_contains_empty_state(const std::vector& states) const; - /** - * @brief Assert that the trajectory is not empty - * @throws EmptyStateException if any of the elements is empty - */ - void assert_trajectory_not_empty() const; - private: std::deque points_; }; @@ -310,6 +282,7 @@ inline void TrajectoryBase::delete_point(unsigned int index) { template inline const std::vector TrajectoryBase::get_points() const { + this->assert_not_empty(); return std::vector(this->points_.begin(), this->points_.end()); } @@ -319,12 +292,6 @@ inline const TrajectoryT& TrajectoryBase::get_point(unsigned int in return this->points_[index]; } -template -inline TrajectoryT& TrajectoryBase::get_point(unsigned int index) { - this->assert_index_in_range(index); - return this->points_[index]; -} - template inline void TrajectoryBase::set_point(const TrajectoryT& point, unsigned int index) { this->assert_index_in_range(index); @@ -340,44 +307,39 @@ inline void TrajectoryBase::set_points(const std::vector inline const std::chrono::nanoseconds& TrajectoryBase::get_duration(unsigned int index) const { - this->assert_trajectory_not_empty(); + this->assert_not_empty(); this->assert_index_in_range(index); return this->points_[index].duration; } template inline const std::vector TrajectoryBase::get_durations() const { - this->assert_trajectory_not_empty(); + this->assert_not_empty(); std::vector durations; - std::for_each(this->points_.begin(), this->points_.end(), [&](const auto& point) { - durations.push_back(point.duration); - }); + std::for_each( + this->points_.begin(), this->points_.end(), [&](const auto& point) { durations.push_back(point.duration); }); return durations; } template inline const std::chrono::nanoseconds TrajectoryBase::get_time_from_start(unsigned int index) const { - this->assert_trajectory_not_empty(); + this->assert_not_empty(); this->assert_index_in_range(index); return std::accumulate( this->points_.begin(), this->points_.begin() + index + 1, std::chrono::nanoseconds(0), - [&](auto acc, const auto& point) { return acc + point.duration; } - ); - ; + [&](auto acc, const auto& point) { return acc + point.duration; }); } template inline const std::vector TrajectoryBase::get_times_from_start() const { - this->assert_trajectory_not_empty(); + this->assert_not_empty(); std::vector times_from_start; std::chrono::nanoseconds time_from_start = std::chrono::nanoseconds(0); std::transform( - this->points_.begin(), this->points_.end(), std::back_inserter(times_from_start), - [&](const auto& point) { + this->points_.begin(), this->points_.end(), std::back_inserter(times_from_start), [&](const auto& point) { time_from_start += point.duration; return time_from_start; - } - ); + }); return times_from_start; } @@ -388,7 +350,6 @@ inline const std::chrono::nanoseconds TrajectoryBase::get_trajector template inline unsigned int TrajectoryBase::get_size() const { - this->assert_trajectory_not_empty(); return this->points_.size(); } @@ -415,7 +376,7 @@ template template inline void TrajectoryBase::assert_points_not_empty(const std::vector& points) const { if (points.empty()) { - throw exceptions::IncompatibleSizeException("Empty points vector provided!"); + throw exceptions::IncompatibleSizeException("Empty points vector provided"); } } @@ -423,7 +384,9 @@ 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"); + throw exceptions::IncompatibleSizeException( + "The size of the provided vector (" + std::to_string(points.size()) + + ") doesn't correspond to the size of the trajectory (" + std::to_string(this->points_.size()) + ")"); } } @@ -433,18 +396,9 @@ 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); + throw exceptions::IncompatibleSizeException( + "The size of the provided points and durations vectors are not equal (" + std::to_string(points.size()) + + " vs. " + std::to_string(durations.size()) + ")"); } } @@ -452,15 +406,11 @@ template template requires std::derived_from inline void TrajectoryBase::assert_not_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_not_empty() const { - if (this->is_empty()) { - throw exceptions::EmptyStateException("Trajectory is empty"); + if (std::string name; std::ranges::any_of(states, [&](const auto& state) { + name = state.get_name(); + return state.is_empty(); + })) { + throw exceptions::EmptyStateException("Provided state " + name + " is empty"); } } }// namespace state_representation diff --git a/source/state_representation/src/trajectory/CartesianTrajectory.cpp b/source/state_representation/src/trajectory/CartesianTrajectory.cpp index 2a70bb1c5..345ba3eaa 100644 --- a/source/state_representation/src/trajectory/CartesianTrajectory.cpp +++ b/source/state_representation/src/trajectory/CartesianTrajectory.cpp @@ -36,13 +36,25 @@ const std::string& CartesianTrajectory::get_reference_frame() const { } void CartesianTrajectory::set_reference_frame(const CartesianPose& pose) { - this->assert_trajectory_not_empty(); - this->reference_frame_ = pose.get_reference_frame(); auto points = this->get_points(); + this->reference_frame_ = pose.get_reference_frame(); std::transform(points.begin(), points.end(), points.begin(), [&](const auto& point) { return point * pose; }); this->set_points(points, this->get_durations()); } +const std::vector CartesianTrajectory::get_points() const { + std::vector points; + auto queue = this->TrajectoryBase::get_points(); + std::transform(queue.begin(), queue.end(), std::back_inserter(points), [&](const auto& point) { + return point.to_cartesian_state(this->reference_frame_); + }); + return points; +} + +CartesianState CartesianTrajectory::get_point(unsigned int index) const { + return this->TrajectoryBase::get_point(index).to_cartesian_state(this->reference_frame_); +} + void CartesianTrajectory::add_point(const CartesianState& point, const std::chrono::nanoseconds& duration) { this->add_points({point}, {duration}); } @@ -86,19 +98,6 @@ void CartesianTrajectory::set_points( } } -const std::vector CartesianTrajectory::get_points() const { - std::vector points; - auto queue = this->TrajectoryBase::get_points(); - std::transform(queue.begin(), queue.end(), std::back_inserter(points), [&](const auto& point) { - return point.to_cartesian_state(this->reference_frame_); - }); - return points; -} - -CartesianState CartesianTrajectory::get_point(unsigned int index) const { - return this->TrajectoryBase::get_point(index).to_cartesian_state(this->reference_frame_); -} - std::pair CartesianTrajectory::operator[](unsigned int idx) const { auto point = this->TrajectoryBase::operator[](idx); return std::make_pair(point.to_cartesian_state(this->reference_frame_), point.duration); diff --git a/source/state_representation/src/trajectory/JointTrajectory.cpp b/source/state_representation/src/trajectory/JointTrajectory.cpp index a0bb03f41..93d7b74ad 100644 --- a/source/state_representation/src/trajectory/JointTrajectory.cpp +++ b/source/state_representation/src/trajectory/JointTrajectory.cpp @@ -30,6 +30,27 @@ JointTrajectory::JointTrajectory( this->add_points(points, durations); } +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::vector JointTrajectory::get_points() const { + std::vector points; + auto queue = this->TrajectoryBase::get_points(); + std::transform(queue.begin(), queue.end(), std::back_inserter(points), [&](const auto& point) { + return point.to_joint_state(this->joint_names_); + }); + return points; +} + +const JointState JointTrajectory::get_point(unsigned int index) const { + return this->TrajectoryBase::get_point(index).to_joint_state(this->joint_names_); +} + void JointTrajectory::add_point(const JointState& point, const std::chrono::nanoseconds& duration) { this->add_points({point}, {duration}); } @@ -40,7 +61,7 @@ void JointTrajectory::add_points( this->assert_points_not_empty(points); this->assert_points_durations_sizes_equal(points, durations); this->assert_not_contains_empty_state(points); - this->assert_incompatible_joint_names(points, this->joint_names_); + this->assert_compatible_joint_names(points, this->joint_names_); for (unsigned int i = 0; i < points.size(); ++i) { this->TrajectoryBase::add_point(JointTrajectoryPoint(points[i], durations[i])); } @@ -50,13 +71,13 @@ void JointTrajectory::insert_point( const JointState& point, const std::chrono::nanoseconds& duration, unsigned int index ) { this->assert_not_contains_empty_state({point}); - this->assert_incompatible_joint_names({point}, this->joint_names_); + this->assert_compatible_joint_names({point}, this->joint_names_); this->TrajectoryBase::insert_point(JointTrajectoryPoint(point, duration), index); } void JointTrajectory::set_point(const JointState& point, const std::chrono::nanoseconds& duration, unsigned int index) { this->assert_not_contains_empty_state({point}); - this->assert_incompatible_joint_names({point}, this->joint_names_); + this->assert_compatible_joint_names({point}, this->joint_names_); this->TrajectoryBase::set_point(JointTrajectoryPoint(point, duration), index); } @@ -71,39 +92,18 @@ void JointTrajectory::set_points( } } -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::vector JointTrajectory::get_points() const { - std::vector points; - auto queue = this->TrajectoryBase::get_points(); - std::transform(queue.begin(), queue.end(), std::back_inserter(points), [&](const auto& point) { - return point.to_joint_state(this->joint_names_); - }); - return points; -} - -const JointState JointTrajectory::get_point(unsigned int index) const { - return this->TrajectoryBase::get_point(index).to_joint_state(this->joint_names_); -} - std::pair JointTrajectory::operator[](unsigned int idx) const { auto point = this->TrajectoryBase::operator[](idx); return std::make_pair(point.to_joint_state(this->joint_names_), point.duration); } -void JointTrajectory::assert_incompatible_joint_names(const std::vector& states) const { +void JointTrajectory::assert_compatible_joint_names(const std::vector& states) const { if (!states.empty()) { - this->assert_incompatible_joint_names(states, states[0].get_names()); + this->assert_compatible_joint_names(states, states[0].get_names()); } } -void JointTrajectory::assert_incompatible_joint_names( +void JointTrajectory::assert_compatible_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; })) { diff --git a/source/state_representation/test/tests/test_trajectory.cpp b/source/state_representation/test/tests/test_trajectory.cpp index 35dbf80c2..3e1d16b33 100644 --- a/source/state_representation/test/tests/test_trajectory.cpp +++ b/source/state_representation/test/tests/test_trajectory.cpp @@ -160,7 +160,7 @@ TEST(TrajectoryTest, ConstructTrajectory) { TYPED_TEST_P(TrajectoryTest, AddRemovePoints) { EXPECT_NO_THROW(this->trajectory = std::make_shared("trajectory")); - EXPECT_THROW(this->trajectory->get_size(), exceptions::EmptyStateException); + EXPECT_EQ(this->trajectory->get_size(), 0); using PointType = typename std::conditional< std::is_same_v, TrajectoryPoint, @@ -215,7 +215,7 @@ TYPED_TEST_P(TrajectoryTest, AddRemovePoints) { EXPECT_NO_THROW(this->delete_point()); this->expect_equal(point0, 0); EXPECT_NO_THROW(this->delete_point()); - EXPECT_THROW(this->trajectory->get_size(), exceptions::EmptyStateException); + EXPECT_EQ(this->trajectory->get_size(), 0); // additons and insertions of multiple points std::vector points = {point0, point1, point2}; @@ -238,7 +238,7 @@ TYPED_TEST_P(TrajectoryTest, AddRemovePoints) { TYPED_TEST_P(TrajectoryTest, Exceptions) { EXPECT_NO_THROW(this->trajectory = std::make_shared("trajectory")); - EXPECT_THROW(this->trajectory->get_size(), exceptions::EmptyStateException); + EXPECT_EQ(this->trajectory->get_size(), 0); using PointType = typename std::conditional< std::is_same_v, TrajectoryPoint, @@ -307,7 +307,7 @@ TYPED_TEST_P(TrajectoryTest, Exceptions) { TYPED_TEST_P(TrajectoryTest, Getters) { EXPECT_NO_THROW(this->trajectory = std::make_shared("trajectory")); - EXPECT_THROW(this->trajectory->get_size(), exceptions::EmptyStateException); + EXPECT_EQ(this->trajectory->get_size(), 0); using PointType = typename std::conditional< std::is_same_v, TrajectoryPoint, @@ -362,7 +362,7 @@ TYPED_TEST_P(TrajectoryTest, Getters) { this->trajectory->reset(); EXPECT_TRUE(this->trajectory->is_empty()); - EXPECT_THROW(this->trajectory->get_size(), exceptions::EmptyStateException); + EXPECT_EQ(this->trajectory->get_size(), 0); EXPECT_THROW(this->trajectory->get_durations(), exceptions::EmptyStateException); if constexpr (std::is_same_v) { EXPECT_STREQ(this->trajectory->get_reference_frame().c_str(), "world");