diff --git a/CMakeLists.txt b/CMakeLists.txt index a55bdff..d379f77 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -24,25 +24,33 @@ include_directories( add_library(${PROJECT_NAME} SHARED src/trajectory_generator.cpp + src/logger.cpp src/constant_acceleration_solver.cpp src/path_manager.cpp - src/point.cpp src/path.cpp - src/section_constraint.cpp src/section.cpp - src/linear_segment.cpp - src/blend_segment.cpp - src/segment_constraint.cpp src/utility_functions.cpp + src/value_group.cpp + src/symbol_group.cpp ) target_link_libraries (${PROJECT_NAME} Eigen3::Eigen) +add_executable(test tests/test.cpp) +target_link_libraries (test ${PROJECT_NAME}) + install(TARGETS sotg DESTINATION lib) + +#add_definitions(-DVERBOSE) # Enable printing info and warn messages to cout using default logger + ########### ## Debug ## ########### #add_definitions(-DDEBUG) #uncomment for additional debug output -#add_definitions(-DVERBOSE_TESTS) # print all calculated values \ No newline at end of file +#add_definitions(-DVERBOSE_TESTS) # print all calculated values + + ############# + ## Testing ## + ############# diff --git a/include/sotg/blend_segment.hpp b/include/sotg/blend_segment.hpp deleted file mode 100644 index e36e202..0000000 --- a/include/sotg/blend_segment.hpp +++ /dev/null @@ -1,68 +0,0 @@ -#pragma once - -#include "sotg/section.hpp" -#include "sotg/segment.hpp" -#include "sotg/segment_constraint.hpp" - -namespace SOTG { -namespace detail { - - class KinematicSolver; - - // A container that is used to hold the information necessary to calculate - // positions and velocities during its duration Unlike sections, segments can be - // linear and blended, whereby linear segments behave like cropped sections. - // Blend segments take two references to adjacent sections, allowing for the - // smooth transition between the two. - class BlendSegment : public Segment { - private: - Section& pre_section_; - Section& post_section_; - - SegmentConstraint constraint_; - - double pre_velocity_magnitude_; - double post_velocity_magnitude_; - - public: - BlendSegment(Section& pre_section_ref, Section& post_section_ref, SegmentConstraint constraint, - double pre_vel_mag, double post_vel_mag, double duration, double t_start) - : Segment(duration, t_start) - , pre_section_(pre_section_ref) - , post_section_(post_section_ref) - , constraint_(constraint) - , pre_velocity_magnitude_(pre_vel_mag) - , post_velocity_magnitude_(post_vel_mag) - { - } - - void calcPosAndVel([[maybe_unused]] double t_section, [[maybe_unused]] double t_segment, Point& pos, - Point& vel, const KinematicSolver& solver) const override; - - const Point& getPreBlendDirection() const { return pre_section_.getDirection(); } - const Point& getPostBlendDirection() const { return post_section_.getDirection(); } - - void setPreBlendVelocityMagnitude(double value) { pre_velocity_magnitude_ = value; } - void setPostBlendVelocityMagnitude(double value) { post_velocity_magnitude_ = value; } - - double getPreBlendVelocityMagnitude() const { return pre_velocity_magnitude_; } - double getPostBlendVelocityMagnitude() const { return post_velocity_magnitude_; } - - void setDuration(double duration) { duration_ = duration; } - void setStartTime(double t_start) { start_time_ = t_start; } - - void setStartPoint(const Point& p) { start_point_ = p; } - void setEndPoint(const Point& p) { end_point_ = p; } - - double getStartTime() const { return start_time_; } - double getDuration() const { return duration_; } - double getEndTime() const { return start_time_ + duration_; } - - const Point& getStartPoint() const { return start_point_; } - const Point& getEndPoint() const { return end_point_; } - - Section& getPreBlendSection() const override { return pre_section_; }; - Section& getPostBlendSection() const override { return post_section_; }; - }; -} // namespace detail -} // namespace SOTG \ No newline at end of file diff --git a/include/sotg/constant_acceleration_solver.hpp b/include/sotg/constant_acceleration_solver.hpp index 7574d53..a754bb6 100644 --- a/include/sotg/constant_acceleration_solver.hpp +++ b/include/sotg/constant_acceleration_solver.hpp @@ -6,13 +6,12 @@ #include -#include "sotg/blend_segment.hpp" #include "sotg/kinematic_solver.hpp" -#include "sotg/linear_segment.hpp" #include "sotg/section.hpp" -#include "sotg/segment_constraint.hpp" #include "sotg/utility_functions.hpp" +#include "sotg/result.hpp" + namespace SOTG { namespace detail { @@ -21,65 +20,30 @@ namespace detail { // bang coast bang profile class ConstantAccelerationSolver : public KinematicSolver { private: - void calcSegmentPreparations(const Section& pre_section, const Section& post_section, - std::vector& a_max_post, std::vector& a_max_pre, - double& L_acc_magnitude_post, double& T_acc_post, double& T_acc_pre); - - void calcAccAndVelPerDoF(const Section& section, std::vector& a_max_vec, - std::vector& v_max_vec); + void calcPhaseDurationAndDistance(double a_max, double v_max, double L_total, PhaseComponent& acc_phase_single_dof, + PhaseComponent& coast_phase_single_dof, PhaseComponent& dec_phase_single_dof); - void calcPhaseTimeAndDistance(double& a_max, double& v_max, double L_total, PhaseDoF& acc_phase_single_dof, - PhaseDoF& coast_phase_single_dof, PhaseDoF& dec_phase_single_dof); + void calcDurationAndDistancePerGroup(double& a_max, double& v_max, double distance, + double& duration); - void calcTotalTimeAndDistanceSingleDoF(double& a_max, double& v_max, double total_length, - double& total_time); - - void calcTimesAndLengthsMultiDoF(Phase& acc_phase, Phase& coast_phase, Phase& dec_phase, + void calcDurationAndDistance(Phase& acc_phase, Phase& coast_phase, Phase& dec_phase,Point& start_point, Point& end_point, std::vector& total_time_per_dof, - std::vector& total_length_per_dof, Point diff, - std::vector& a_max_vec, std::vector& v_max_vec); - - void calcSecondBlendingDist(double T_blend, double T_acc_post, double a_max_magnitude_post, - double vel_pre_blend_magnitude, double blending_dist_pre, - double& blending_dist_post, size_t segment_id); + std::vector& total_length_per_dof, std::map& group_acc, std::map& group_vel); - void calcPosAndVelSingleDoFLinear(double section_length, const Phase& phase, + void calcPosAndVelSingleGroup(double section_length, const Phase& phase, double phase_distance_to_p_start, double t_phase, double a_max_reduced, double v_max_reduced, double& pos_magnitude, double& vel_magnitude) const; - void calcVelAndTimeByDistance(const Section& section, double distance, Point& velocity_per_dof, - double& time_when_distance_is_reached); - - void calcPreBlendParams(double blending_dist_pre, const Section& pre_section, Point& A_blend, - double& T_blend, double& vel_pre_blend_magnitude, - double& absolute_blend_start_time); - - void calcPostBlendParams(double blending_dist_pre, const Section& pre_section, Point& C_blend, - double& T_blend, double& vel_pre_blend_magnitude, double& t_abs_start_blend); - - bool isBlendAccelerationTooHigh(const std::vector& a_max, const double& T_blend, - const double& vel_pre_blend_magnitude, - const double& vel_post_blend_magnitude, const Section& pre_section, - const Section& post_section, size_t segment_id); - void setNoBlendingParams(const Section& pre_section, const Section& post_section, double& T_blend, - double& absolute_blend_start_time_with_shift, - double& absolute_blend_end_time_without_shift, Point& A_blend, Point& C_blend, - double& vel_pre_blend_magnitude, double& vel_post_blend_magnitude); public: - Section calcSection(Point& p_start_ref, Point& p_end_ref, SectionConstraint constraint_copy, - size_t section_id) override; - std::shared_ptr calcBlendSegment(Section& pre_section, Section& post_section, - const SegmentConstraint& constraint, size_t segment_id, - std::map& debug_output) override; - - void calcPosAndVelSection(double t_section, const Section& section, Point& pos, Point& vel) const override; + ConstantAccelerationSolver(SymbolGroupMap& symbol_map, const Logger& logger) + : KinematicSolver(symbol_map, logger) + { + } - void calcPosAndVelLinearSegment(double t_section, const LinearSegment& segment, Point& pos, - Point& vel) const override; + Section calcSection(Point& p_start_ref, Point& p_end_ref, size_t section_id) override; - void calcPosAndVelBlendSegment(double t_segment, const BlendSegment& segment, Point& pos, - Point& vel) const override; + void calcPosAndVelSection(double t_section, Section& section, Result& result) const override; }; } // namespace detail } // namespace SOTG \ No newline at end of file diff --git a/include/sotg/kinematic_solver.hpp b/include/sotg/kinematic_solver.hpp index 645d2b8..0db9f59 100644 --- a/include/sotg/kinematic_solver.hpp +++ b/include/sotg/kinematic_solver.hpp @@ -2,11 +2,11 @@ #include #include +#include -#include "sotg/blend_segment.hpp" -#include "sotg/linear_segment.hpp" +#include "sotg/logger.hpp" #include "sotg/section.hpp" -#include "sotg/segment.hpp" +#include "sotg/result.hpp" namespace SOTG { namespace detail { @@ -14,23 +14,19 @@ namespace detail { // segment generation aswell as the calculation of position and velocity for a // specific point in time class KinematicSolver { - public: - virtual Section calcSection(Point& p_start_ref, Point& p_end_ref, SectionConstraint constraint_copy, - size_t section_id) - = 0; - virtual std::shared_ptr - calcBlendSegment(Section& pre_section, Section& post_section, const SegmentConstraint& constraint, - size_t segment_id, std::map& debug_output) - = 0; + protected: + SymbolGroupMap& symbol_map_; + const Logger& logger_; - virtual void calcPosAndVelSection(double t_section, const Section& section, Point& pos, - Point& vel) const = 0; + public: + KinematicSolver(SymbolGroupMap& symbol_map, const Logger& logger) + : symbol_map_(symbol_map), logger_(logger) + { + } - virtual void calcPosAndVelLinearSegment(double t_section, const LinearSegment& segment, Point& pos, - Point& vel) const = 0; + virtual Section calcSection(Point& p_start_ref, Point& p_end_ref, size_t section_id) = 0; - virtual void calcPosAndVelBlendSegment(double t_segment, const BlendSegment& segment, Point& pos, - Point& vel) const = 0; + virtual void calcPosAndVelSection(double t_section, Section& section, Result& result) const = 0; virtual ~KinematicSolver() = default; }; diff --git a/include/sotg/linear_segment.hpp b/include/sotg/linear_segment.hpp deleted file mode 100644 index 858f439..0000000 --- a/include/sotg/linear_segment.hpp +++ /dev/null @@ -1,45 +0,0 @@ -#pragma once - -#include "sotg/segment.hpp" - -namespace SOTG { -namespace detail { - - class KinematicSolver; - - // A container that is used to hold the information necessary to calculate - // positions and velocities during its duration Unlike sections, segments can be - // linear and blended, whereby linear segments behave like cropped sections. - // Blend segments take two references to adjacent sections, allowing for the - // smooth transition between the two. - class LinearSegment : public Segment { - private: - Section& section_; - - public: - LinearSegment(Section& section, double duration, double t_start) - : Segment(duration, t_start) - , section_(section) - { - } - - void calcPosAndVel([[maybe_unused]] double t_section, [[maybe_unused]] double t_segment, Point& pos, - Point& vel, const KinematicSolver& solver) const override; - - void setDuration(double duration) { duration_ = duration; } - void setStartTime(double t_start) { start_time_ = t_start; } - - void setStartPoint(const Point& p) { start_point_ = p; } - void setEndPoint(const Point& p) { end_point_ = p; } - - double getStartTime() const { return start_time_; } - double getDuration() const { return duration_; } - double getEndTime() const { return start_time_ + duration_; } - - const Point& getStartPoint() const { return start_point_; } - const Point& getEndPoint() const { return end_point_; } - - Section& getSection() const override { return section_; } - }; -} // namespace detail -} // namespace SOTG \ No newline at end of file diff --git a/include/sotg/logger.hpp b/include/sotg/logger.hpp new file mode 100644 index 0000000..517c526 --- /dev/null +++ b/include/sotg/logger.hpp @@ -0,0 +1,18 @@ +#pragma once + +#include + +namespace SOTG { + +/*The Logger class can be used in order to pass SOTG internal warnings and debugging info to the user instead + * of printing them to directly to cout*/ +class Logger { +public: + enum MsgType { INFO, WARNING, DEBUG }; + + virtual void log(const std::string& message, MsgType type = INFO) const; + + virtual ~Logger() = default; +}; + +} // namespace SOTG \ No newline at end of file diff --git a/include/sotg/path.hpp b/include/sotg/path.hpp index 90d312b..13b6d61 100644 --- a/include/sotg/path.hpp +++ b/include/sotg/path.hpp @@ -1,4 +1,3 @@ - #pragma once #include @@ -18,10 +17,10 @@ class Path { public: void addPoint(Point point); - void addPoint(const std::vector& old_point); - void addPoint(const std::vector>& old_point); + size_t getNumWaypoints() { return waypoints_.size(); }; - Point& getPoint(size_t index); + Point& getPoint(std::string name); + Point& getPoint(size_t index) { return waypoints_[index]; } std::vector::iterator begin() { return waypoints_.begin(); } std::vector::iterator end() { return waypoints_.end(); } diff --git a/include/sotg/path_manager.hpp b/include/sotg/path_manager.hpp index fca488b..9af2ee8 100644 --- a/include/sotg/path_manager.hpp +++ b/include/sotg/path_manager.hpp @@ -5,13 +5,9 @@ #include #include -#include "sotg/blend_segment.hpp" #include "sotg/kinematic_solver.hpp" -#include "sotg/linear_segment.hpp" #include "sotg/path.hpp" #include "sotg/section.hpp" -#include "sotg/section_constraint.hpp" -#include "sotg/segment_constraint.hpp" namespace SOTG { namespace detail { @@ -22,34 +18,26 @@ namespace detail { private: Path path_; std::list
sections_; - std::list> segments_; - - std::vector section_constraints_; - std::vector segment_constraints_; std::shared_ptr kinematic_solver_; std::vector>& debug_info_vec_; void resetSections(); - void resetSegments(); - void generateBlendSegments(); - void generateLinearSegments(); public: PathManager(std::shared_ptr solver, - std::vector>& debug_info_vec); + std::vector>& debug_info_vec); - void resetPath(Path path, std::vector section_constraints, - std::vector segment_constraints); + void resetPath(Path path); int getNumSections() { return sections_.size(); } - int getNumSegments() { return segments_.size(); } - const std::list>& getSegments() const { return segments_; } + const std::list
& getSections() const { return sections_; } - const Section& getSectionAtTime(double time); - const Segment& getSegmentAtTime(double time); + Section& getSectionAtTime(double time); + + const Path& getPath() { return path_; } std::ostream& operator<<(std::ostream& out); }; diff --git a/include/sotg/phase.hpp b/include/sotg/phase.hpp index abfaa0d..a9b5379 100644 --- a/include/sotg/phase.hpp +++ b/include/sotg/phase.hpp @@ -2,8 +2,8 @@ #include #include - -#include "sotg/phase.hpp" +#include +#include namespace SOTG { namespace detail { @@ -24,7 +24,7 @@ namespace detail { } } - struct PhaseDoF { + struct PhaseComponent { double duration = 0.0; double length = 0.0; double distance_p_start = 0.0; @@ -32,7 +32,7 @@ namespace detail { // A specific kinematic state that applies to a specific part of a section struct Phase { - std::vector components; + std::map components; double duration = 0.0; double length = 0.0; diff --git a/include/sotg/point.hpp b/include/sotg/point.hpp index 3e9e1d8..59fecea 100644 --- a/include/sotg/point.hpp +++ b/include/sotg/point.hpp @@ -5,11 +5,14 @@ #include #include #include +#include #include #include +#include "sotg/symbol_group.hpp" #include "sotg/utility_functions.hpp" +#include "sotg/value_group.hpp" namespace SOTG { @@ -17,48 +20,48 @@ namespace SOTG { // unit of a particular value class Point { private: - std::vector values_; - int orientation_index_ = -1; + ValueGroupMap value_map_; + SymbolGroupMap symbol_map_; + std::string name_ = "ND"; + int id_ = -1; public: - Point(); - explicit Point(std::vector vec_list); - - void addValue(double value) { values_.push_back(value); }; - void setOrientationIndex(int new_index) { orientation_index_ = new_index; }; - void zeros(size_t num_components); - int getOrientationIndex() const { return orientation_index_; } + Point(SymbolGroupMap symbol_map, std::string name = "ND") : symbol_map_(symbol_map), name_(name) + { + for (auto& [key, symbols] : symbol_map) { + value_map_.insert({key, ValueGroup(symbols)}); + } + } + Point() = delete; - size_t size() const { return values_.size(); }; - double getValue(int index) const { return values_[index]; }; - Point operator+(const Point& p2) const; - Point operator-(const Point& p2) const; - Point operator-() const; - Point operator/(const double& scalar) const; - Point operator*(const double& scalar) const; - double operator[](size_t index) const; - double norm(); + std::string getName() { return name_; } + ValueGroupMap& getValueMap() { return value_map_; } + const SymbolGroupMap& getSymbolMap() const { return symbol_map_; } - Point getLocation(); - Point getOrientation(); + ValueGroup& operator[](std::string key) { return value_map_.at(key); } + Point operator-(Point& p) { + Point diff(symbol_map_); + for (auto [group_name, symbols] : symbol_map_) { + if (symbols.isQuaternion()) { + diff[group_name] = p[group_name].getQuaternionValues().inverse() * (*this)[group_name].getQuaternionValues(); + } else { + diff[group_name] = Eigen::VectorXd((*this)[group_name].getCartesianValues() - p[group_name].getCartesianValues()); + } + } + return diff; + } - std::vector::iterator begin() { return values_.begin(); } - std::vector::iterator end() { return values_.end(); } + int getID() const { return id_; } + void setID(int id) { id_ = id; } friend std::ostream& operator<<(std::ostream& out, const Point& point) { - if (point.values_.size() < 1) { - return out << "[](" << point.orientation_index_ << ")"; - } - out << "[ "; - for (size_t i = 0; i < point.values_.size() - 1; i++) { - out << point.values_[i] << ", "; - } - if (point.orientation_index_ == -1) { - out << point.values_.back() << " ]"; - } else { - out << point.values_.back() << " ](" << point.orientation_index_ << ")"; + out << "[[ " << point.name_ << " ] {"; + for (auto& [key, value] : point.value_map_) { + out << " " << key << " : " << value << ","; } + out << "\b"; + out << " }]"; return out; } diff --git a/include/sotg/result.hpp b/include/sotg/result.hpp new file mode 100644 index 0000000..9147c3d --- /dev/null +++ b/include/sotg/result.hpp @@ -0,0 +1,24 @@ +#pragma once + +#include "sotg/symbol_group.hpp" +#include "sotg/value_group.hpp" +#include "sotg/point.hpp" + +namespace SOTG { + +class Result +{ +private: + SymbolGroupMap& symbol_map_; + Point location, velocity; + +public: + Result(SymbolGroupMap& symbol_map) : symbol_map_(symbol_map), location(symbol_map, "location"), velocity(symbol_map, "velocity") { + + } + + Point& getLocation() { return location; } + Point& getVelocity() { return velocity; } +}; + +} // Namespace SOTG \ No newline at end of file diff --git a/include/sotg/section.hpp b/include/sotg/section.hpp index 7cbe875..842b4f9 100644 --- a/include/sotg/section.hpp +++ b/include/sotg/section.hpp @@ -5,7 +5,6 @@ #include "sotg/phase.hpp" #include "sotg/point.hpp" -#include "sotg/section_constraint.hpp" namespace SOTG { namespace detail { @@ -16,17 +15,17 @@ namespace detail { private: Point& start_point_; Point& end_point_; - Point diff_; - Point dir_; + + const SymbolGroupMap& symbol_groups_; + double length_; - SectionConstraint constraint_; std::vector phases_; int index_slowest_dof_ = -1; - std::vector adapted_acceleration_; - std::vector adapted_velocity_; + std::map a_max_; + std::map v_max_; double duration_ = 0.0; double start_time_ = 0.0; @@ -40,41 +39,33 @@ namespace detail { const Point& getStartPoint() const { return start_point_; } const Point& getEndPoint() const { return end_point_; } - double getAccMaxLinear() const { return constraint_.getAccelerationMagnitudeLinear(); } - double getAccMaxAngular() const { return constraint_.getAccelerationMagnitudeAngular(); } - double getVelMaxLinear() const { return constraint_.getVelocityMagnitudeLinear(); } - double getVelMaxAngular() const { return constraint_.getVelocityMagnitudeAngular(); } - double getLength() const { return length_; } - const Point& getDirection() const { return dir_; } - const Point& getDifference() const { return diff_; } - void setLength(double length) { length_ = length; } - void setDirection(const Point& dir) { dir_ = dir; } - void setDifference(const Point& diff) { diff_ = diff; } - - Section(Point& p_start_ref, Point& p_end_ref, SectionConstraint constraint_copy, size_t section_id); + // const Point& getDirection() const { return dir_; } + // const Point& getDifference() const { return diff_; } + // void setLength(double length) { length_ = length; } + // void setDirection(const Point& dir) { dir_ = dir; } + // void setDifference(const Point& diff) { diff_ = diff; } + + Section(Point& p_start_ref, Point& p_end_ref, size_t section_id) + : start_point_(p_start_ref) + , end_point_(p_end_ref) + , symbol_groups_(p_start_ref.getSymbolMap()) + , id_(section_id) + { + } void setIndexSlowestDoF(int index) { index_slowest_dof_ = index; } void setDuration(double time) { duration_ = time; } double getDuration() const { return duration_; } - const Phase& getPhaseByDistance(double distance) const; + std::map& getAccelerations() { return a_max_; } + std::map& getVelocities() { return v_max_; } - const std::vector& getAdaptedAcceleration() const { return adapted_acceleration_; } - const std::vector& getAdaptedVelocity() const { return adapted_velocity_; } - void setAdaptedAcceleration(const std::vector& new_adapted_acceleration) - { - adapted_acceleration_ = new_adapted_acceleration; - } - void setAdaptedVelocity(const std::vector& new_adapted_velocity) - { - adapted_velocity_ = new_adapted_velocity; - } - - const Phase& getPhaseByTime(double time) const; + Phase& getPhaseByDistance(double distance); + Phase& getPhaseByTime(double time); void setPhases(const std::vector& new_phases) { phases_ = new_phases; } - const std::vector& getPhases() const { return phases_; } + std::vector& getPhases() { return phases_; } double getStartTime() const { return start_time_; } double getEndTime() const { return start_time_ + duration_; } @@ -82,10 +73,7 @@ namespace detail { int getIndexSlowestDoF() const { return index_slowest_dof_; } - const Phase& getPhaseByType(PhaseType type) const; - - void setTimeShift(double shift) { time_shift_ = shift; } - double getTimeShift() const { return time_shift_; } + Phase& getPhaseByType(PhaseType type); void setID(size_t new_id) { id_ = new_id; } size_t getID() { return id_; } diff --git a/include/sotg/section_constraint.hpp b/include/sotg/section_constraint.hpp deleted file mode 100644 index 4be46f7..0000000 --- a/include/sotg/section_constraint.hpp +++ /dev/null @@ -1,23 +0,0 @@ -#pragma once - -namespace SOTG { - -// Contains the limiting factors for a section -class SectionConstraint { -private: - double acceleration_magnitude_linear_; - double acceleration_magnitude_angular_; - - double velocity_magnitude_linear_; - double velocity_magnitude_angular_; - -public: - SectionConstraint(double acc_lin, double acc_ang, double vel_lin, double vel_ang); - - double getAccelerationMagnitudeLinear() const { return acceleration_magnitude_linear_; } - double getAccelerationMagnitudeAngular() const { return acceleration_magnitude_angular_; } - double getVelocityMagnitudeLinear() const { return velocity_magnitude_linear_; } - double getVelocityMagnitudeAngular() const { return velocity_magnitude_angular_; } -}; - -} // namespace SOTG \ No newline at end of file diff --git a/include/sotg/segment.hpp b/include/sotg/segment.hpp deleted file mode 100644 index 5914341..0000000 --- a/include/sotg/segment.hpp +++ /dev/null @@ -1,65 +0,0 @@ -#pragma once - -#include "sotg/section.hpp" - -namespace SOTG { -namespace detail { - - class KinematicSolver; - // A container that is used to hold the information necessary to calculate positions and velocities during its - // duration Unlike sections, segments can be linear and blended, whereby linear segments behave like cropped - // sections. Blend segments take two references to adjacent sections, allowing for the smooth transition - // between the two. - class Segment { - protected: - Segment(double duration, double t_start) - : duration_(duration) - , start_time_(t_start) - , id_(-1) - { - } - - double duration_; - double start_time_; - - Point start_point_; - Point end_point_; - - int id_; - - public: - virtual void calcPosAndVel([[maybe_unused]] double t_section, [[maybe_unused]] double t_segment, - Point& pos, Point& vel, const KinematicSolver& solver) const = 0; - - void setDuration(double duration) { duration_ = duration; } - void setStartTime(double t_start) { start_time_ = t_start; } - - void setStartPoint(const Point& p) { start_point_ = p; } - void setEndPoint(const Point& p) { end_point_ = p; } - - void setID(int num) { id_ = num; } - - double getStartTime() const { return start_time_; } - double getDuration() const { return duration_; } - double getEndTime() const { return start_time_ + duration_; } - - const Point& getStartPoint() const { return start_point_; } - const Point& getEndPoint() const { return end_point_; } - - int getID() const { return id_; } - - virtual Section& getPreBlendSection() const - { - throw std::runtime_error("Called getPreBlendSection from LinearSegment"); - }; - virtual Section& getPostBlendSection() const - { - throw std::runtime_error("Called getPostBlendSection from LinearSegment"); - }; - - virtual Section& getSection() const { throw std::runtime_error("Called getSection from BlendSegment"); }; - - virtual ~Segment() = default; - }; -} // namespace detail -} // namespace SOTG \ No newline at end of file diff --git a/include/sotg/segment_constraint.hpp b/include/sotg/segment_constraint.hpp deleted file mode 100644 index 67f310e..0000000 --- a/include/sotg/segment_constraint.hpp +++ /dev/null @@ -1,17 +0,0 @@ -#pragma once - -namespace SOTG { - -// Contains limiting factors for segments -class SegmentConstraint { -private: - double blending_distance_; - -public: - SegmentConstraint(); - explicit SegmentConstraint(double blend_dist); - - double getBlendDistance() const { return blending_distance_; } -}; - -} // namespace SOTG \ No newline at end of file diff --git a/include/sotg/sotg.hpp b/include/sotg/sotg.hpp index 001943d..b3726cb 100644 --- a/include/sotg/sotg.hpp +++ b/include/sotg/sotg.hpp @@ -1,6 +1,8 @@ #pragma once +#include "sotg/logger.hpp" #include "sotg/path.hpp" -#include "sotg/section_constraint.hpp" -#include "sotg/segment_constraint.hpp" -#include "sotg/trajectory_generator.hpp" \ No newline at end of file +#include "sotg/point.hpp" +#include "sotg/symbol_group.hpp" +#include "sotg/trajectory_generator.hpp" +#include "sotg/result.hpp" \ No newline at end of file diff --git a/include/sotg/symbol_group.hpp b/include/sotg/symbol_group.hpp new file mode 100644 index 0000000..90d8b9c --- /dev/null +++ b/include/sotg/symbol_group.hpp @@ -0,0 +1,63 @@ + +#pragma once + +#include +#include +#include +#include +#include + +namespace SOTG { + +// The Blueprint for are SymbolGroupValue object. +// SymbolGroups are used in a SymbolMap to define the structure of the Poses that the trajectorie should cover. +// A SymbolGroup defines part of a Pose that share the same unit, e.g. Position in metres or Euler Angles in +// radians Exemple: a SymbolGroup could define two Positions x,y that are jointly limited in accelration and +// velocity, together with another SymbolGroup that defines a single Euler Angle that has its own angular +// acceleration and velocity limits, forms the basis for a 2D trajectory. + +class SymbolGroup { +private: + std::vector symbols_; + bool is_quaternion_ {false}; + +public: + SymbolGroup(std::initializer_list list) + : symbols_(list) + { + } + + SymbolGroup() { } + + std::vector::iterator begin() { return symbols_.begin(); } + std::vector::iterator end() { return symbols_.end(); } + size_t size() const { return symbols_.size(); } + + const std::vector& getSymbols() const { return symbols_; } + size_t getIndex(std::string key); + + std::string str() const; + + std::string operator[](size_t index) const { return symbols_[index]; } + bool operator==(const SymbolGroup& symbol_group) const; + bool operator!=(const SymbolGroup& symbol_group) const; + + bool isQuaternion() const { return is_quaternion_; } + void setQuaternion(bool val) { is_quaternion_ = val; } + + friend std::ostream& operator<<(std::ostream& out, const SymbolGroup& sg) + { + out << "["; + for (auto& symbol : sg.symbols_) { + out << " " << symbol << ","; + } + out << "\b"; + out << " ]"; + + return out; + } +}; + +using SymbolGroupMap = std::unordered_map; + +} // namespace SOTG \ No newline at end of file diff --git a/include/sotg/trajectory_generator.hpp b/include/sotg/trajectory_generator.hpp index fdcfd89..1c63e5d 100644 --- a/include/sotg/trajectory_generator.hpp +++ b/include/sotg/trajectory_generator.hpp @@ -5,12 +5,14 @@ #include "sotg/constant_acceleration_solver.hpp" #include "sotg/kinematic_solver.hpp" +#include "sotg/logger.hpp" #include "sotg/path.hpp" #include "sotg/path_manager.hpp" +#include "sotg/path_manager.hpp" #include "sotg/point.hpp" #include "sotg/section.hpp" -#include "sotg/section_constraint.hpp" -#include "sotg/segment_constraint.hpp" +#include "sotg/symbol_group.hpp" +#include "sotg/result.hpp" namespace SOTG { @@ -18,18 +20,24 @@ namespace SOTG { // specifc point in time class TrajectoryGenerator { private: + SymbolGroupMap symbol_group_map_; + + std::shared_ptr default_logger_; + const Logger& logger_; + std::unique_ptr path_manager_; std::shared_ptr kinematic_solver_; std::vector> debug_info_vec_; public: - TrajectoryGenerator(); + TrajectoryGenerator(SymbolGroupMap symbol_groups); + TrajectoryGenerator(SymbolGroupMap symbol_groups, const Logger& logger); double getDuration(); - void calcPositionAndVelocity(double time, Point& pos, Point& vel, int& id, bool disable_blending = false); - void resetPath(Path path, std::vector section_constraints, - std::vector segment_constraints); + int getNumPassedWaypoints(double tick); + void calcPositionAndVelocity(double time, Result& result); + void resetPath(Path path); std::vector>& getDebugInfo() { return debug_info_vec_; }; }; diff --git a/include/sotg/value_group.hpp b/include/sotg/value_group.hpp new file mode 100644 index 0000000..ecf23f2 --- /dev/null +++ b/include/sotg/value_group.hpp @@ -0,0 +1,97 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "sotg/symbol_group.hpp" + +namespace SOTG { + +// The Blueprint for are SymbolGroupValue object. +// SymbolGroups are used in a SymbolMap to define the structure of the Poses that the trajectorie should cover. +// A SymbolGroup defines part of a Pose that share the same unit, e.g. Position in metres or Euler Angles in +// radians Exemple: a SymbolGroup could define two Positions x,y that are jointly limited in accelration and +// velocity, together with another SymbolGroup that defines a single Euler Angle that has its own angular +// acceleration and velocity limits, forms the basis for a 2D trajectory. + +class ValueGroup { +private: + SymbolGroup symbol_group_; + + Eigen::VectorXd values_; + Eigen::Quaterniond quat_values_; + + double a_max_{-1}; + double v_max_{-1}; + + double blend_radius_{0}; + + bool is_quaternion_{false}; + +public: + ValueGroup(SymbolGroup symbol_group) + : symbol_group_(symbol_group), is_quaternion_(symbol_group.isQuaternion()) + { + values_ = Eigen::VectorXd(symbol_group_.size()); + } + ValueGroup() = delete; + + // Todo initialize with Eigen::Vector and Eigen::Quaternion + void operator=(std::initializer_list list); + void operator=(Eigen::VectorXd vec); + void operator=(Eigen::Quaterniond quat); + //void operator=(const ValueGroup& vg); + double& operator[](std::string key); + ValueGroup operator+(const ValueGroup& value_group) const; + ValueGroup operator*(double scalar) const; + + const SymbolGroup& getSymbols() const { return symbol_group_; } + const Eigen::VectorXd getCartesianValues() const { return values_; } + const Eigen::Quaterniond getQuaternionValues() const { return quat_values_; } + + double getMaxAcc() const { return a_max_; } + double getMaxVel() const { return v_max_; } + double getBlendRadius() const { return blend_radius_; } + + void setConstraints(double a_max, double v_max, double blend_radius = 0.0) { + a_max_ = a_max; + v_max_ = v_max; + blend_radius_ = blend_radius; + } + void setMaxAcc(double a_max) { a_max_ = a_max; } + void setMaxVel(double v_max) { v_max_ = v_max; } + void setBlendRadius(double blend_radius) { blend_radius_ = blend_radius; } + bool isQuaternion() const { return is_quaternion_; } + + friend std::ostream& operator<<(std::ostream& out, const ValueGroup& vg) + { + auto& symbols = vg.getSymbols(); + out << "{"; + if (vg.isQuaternion()) { + out << " " << symbols[0] << " : " << std::to_string(vg.quat_values_.w()) << ","; + out << " " << symbols[1] << " : " << std::to_string(vg.quat_values_.x()) << ","; + out << " " << symbols[2] << " : " << std::to_string(vg.quat_values_.y()) << ","; + out << " " << symbols[3] << " : " << std::to_string(vg.quat_values_.z()) << ","; + + } else { + for (size_t i = 0; i < vg.getSymbols().size(); i++) { + out << " " << symbols[i] << " : " << std::to_string(vg.values_[i]) << ","; + } + } + out << "\b"; + out << " }"; + + return out; + } +}; + +using ValueGroupMap = std::unordered_map; + +} // namespace SOTG \ No newline at end of file diff --git a/src/blend_segment.cpp b/src/blend_segment.cpp deleted file mode 100644 index aadd383..0000000 --- a/src/blend_segment.cpp +++ /dev/null @@ -1,12 +0,0 @@ -#include "sotg/blend_segment.hpp" - -#include "sotg/kinematic_solver.hpp" - -using namespace SOTG; -using namespace detail; - -void BlendSegment::calcPosAndVel([[maybe_unused]] double t_section, double t_segment, Point& pos, Point& vel, - const KinematicSolver& solver) const -{ - solver.calcPosAndVelBlendSegment(t_segment, *this, pos, vel); -} \ No newline at end of file diff --git a/src/constant_acceleration_solver.cpp b/src/constant_acceleration_solver.cpp index 4a901c4..e86420a 100644 --- a/src/constant_acceleration_solver.cpp +++ b/src/constant_acceleration_solver.cpp @@ -4,83 +4,35 @@ using namespace SOTG; using namespace detail; int findIndexOfMax(const std::vector& values); -double calcVecNorm(const std::vector& vec); double calcPhaseLength(const Phase& phase); double calcPhaseLength(const Phase& phase) { double sum = 0.0; for (auto& component : phase.components) { - sum += std::pow(component.length, 2); + sum += std::pow(component.second.length, 2); } return std::sqrt(sum); } -void ConstantAccelerationSolver::calcAccAndVelPerDoF(const Section& section, std::vector& a_max_vec, - std::vector& v_max_vec) +int findIndexOfMax(const std::vector& values) { - Point p_start = section.getStartPoint(); - Point p_end = section.getEndPoint(); - - double a_max_lin = section.getAccMaxLinear(); - double a_max_ang = section.getAccMaxAngular(); - double v_max_lin = section.getVelMaxLinear(); - double v_max_ang = section.getVelMaxAngular(); - - Point dir_lin; - Point diff_lin; - - Point dir_ang; - Point diff_ang; - if (p_start.getOrientationIndex() != -1) { - for (int i = 0; i < p_start.getOrientationIndex(); i++) { - diff_lin.addValue(p_end[i] - p_start[i]); - } - double diff_lin_mag = diff_lin.norm(); - - if (utility::nearlyZero(diff_lin_mag)) { - dir_lin.zeros(diff_lin.size()); - } else { - dir_lin = diff_lin / diff_lin_mag; - } - - for (size_t i = p_start.getOrientationIndex(); i < p_start.size(); i++) { - diff_ang.addValue(p_end[i] - p_start[i]); - } - double diff_ang_mag = diff_ang.norm(); - - if (utility::nearlyZero(diff_ang_mag)) { - dir_ang.zeros(diff_ang.size()); - } else { - dir_ang = diff_ang / diff_ang_mag; - } - } else { - for (size_t i = 0; i < p_start.size(); i++) { - diff_lin.addValue(p_end[i] - p_start[i]); - } - double diff_lin_mag = diff_lin.norm(); - - if (utility::nearlyZero(diff_lin_mag)) { - dir_lin.zeros(diff_lin.size()); - } else { - dir_lin = diff_lin / diff_lin_mag; + int index_max = 0; + double max_value = values[0]; + for (size_t i = 1; i < values.size(); i++) { + if (values[i] > max_value) { + max_value = values[i]; + index_max = i; } } - for (auto& dir : dir_lin) { - a_max_vec.push_back(std::abs(a_max_lin * dir)); - v_max_vec.push_back(std::abs(v_max_lin * dir)); - } - for (auto& dir : dir_ang) { - a_max_vec.push_back(std::abs(a_max_ang * dir)); - v_max_vec.push_back(std::abs(v_max_ang * dir)); - } + return index_max; } -void ConstantAccelerationSolver::calcPhaseTimeAndDistance(double& a_max, double& v_max, double L_total, - PhaseDoF& acc_phase_single_dof, - PhaseDoF& coast_phase_single_dof, - PhaseDoF& dec_phase_single_dof) +void ConstantAccelerationSolver::calcPhaseDurationAndDistance(double a_max, double v_max, double L_total, + PhaseComponent& acc_phase_single_dof, + PhaseComponent& coast_phase_single_dof, + PhaseComponent& dec_phase_single_dof) { double T_acc; double T_coast; @@ -121,28 +73,10 @@ void ConstantAccelerationSolver::calcPhaseTimeAndDistance(double& a_max, double& coast_phase_single_dof.distance_p_start = L_acc; dec_phase_single_dof.distance_p_start = L_acc + L_coast; - - if (L_coast < 0.0 && !(std::abs(L_coast) < 1e-4)) { - std::cout << "Warning::KinematicSolver: velocity should not be decreased in this step!" << std::endl; - } -} - -int findIndexOfMax(const std::vector& values) -{ - int index_max = 0; - double max_value = values[0]; - for (size_t i = 1; i < values.size(); i++) { - if (values[i] > max_value) { - max_value = values[i]; - index_max = i; - } - } - - return index_max; } -void ConstantAccelerationSolver::calcTotalTimeAndDistanceSingleDoF(double& a_max, double& v_max, - double total_length, double& total_time) +void ConstantAccelerationSolver::calcDurationAndDistancePerGroup(double& a_max, double& v_max, double distance, + double& duration) { double T_acc; double T_coast; @@ -167,121 +101,138 @@ void ConstantAccelerationSolver::calcTotalTimeAndDistanceSingleDoF(double& a_max L_dec = -0.5 * a_max * std::pow(T_dec, 2) + v_max * T_dec; - L_coast = total_length - L_acc - L_dec; + L_coast = distance - L_acc - L_dec; if (utility::nearlyZero(v_max)) { T_coast = 0.0; } else { T_coast = L_coast / v_max; } - total_time = T_acc + T_coast + T_dec; + duration = T_acc + T_coast + T_dec; if (L_coast < 0.0 && !(std::abs(L_coast) < 1e-6)) { - double v_max_reduced = std::sqrt(total_length * a_max); -#ifdef DEBUG - std::cout << "Info::KinematicSolver: Decreasing maximum velocity" << std::endl; -#endif - calcTotalTimeAndDistanceSingleDoF(a_max, v_max_reduced, total_length, total_time); + double v_max_reduced = std::sqrt(distance * a_max); + + calcDurationAndDistancePerGroup(a_max, v_max_reduced, distance, duration); v_max = v_max_reduced; } } -void ConstantAccelerationSolver::calcTimesAndLengthsMultiDoF(Phase& acc_phase, Phase& coast_phase, - Phase& dec_phase, - std::vector& total_time_per_dof, - std::vector& total_length_per_dof, Point diff, - std::vector& a_max_vec, - std::vector& v_max_vec) +void ConstantAccelerationSolver::calcDurationAndDistance(Phase& acc_phase, Phase& coast_phase, + Phase& dec_phase, Point& start_point, Point& end_point, + std::vector& duration_per_group, + std::vector& distance_per_group, std::map& group_acc, std::map& group_vel) { - for (size_t i = 0; i < diff.size(); i++) { - double total_length_dof = std::abs(diff[i]); - double total_time_dof = 0.0; + std::vector group_names; + for (const auto& [group_name, symbols] : symbol_map_) + { + double duration; + double distance; - calcTotalTimeAndDistanceSingleDoF(a_max_vec[i], v_max_vec[i], total_length_dof, total_time_dof); + if(symbols.isQuaternion()) { + distance = end_point[group_name].getQuaternionValues().angularDistance(start_point[group_name].getQuaternionValues()); - total_time_per_dof.push_back(total_time_dof); - total_length_per_dof.push_back(total_length_dof); + } else { + distance = (end_point[group_name].getCartesianValues() - start_point[group_name].getCartesianValues()).norm(); + } + distance_per_group.push_back(distance); + + double a_max = end_point[group_name].getMaxAcc(); + double v_max = end_point[group_name].getMaxVel(); + + calcDurationAndDistancePerGroup(a_max,v_max, distance, duration); + + if (v_max < end_point[group_name].getMaxVel()) { + logger_.log("[Section between Points " + std::to_string(start_point.getID()) + "(" + start_point.getName() + ")" + " and " + std::to_string(end_point.getID()) + "(" + end_point.getName() + ")]" + + "[" + group_name + "] KinematicSolver: Decreasing maximum velocity from " + + std::to_string(end_point[group_name].getMaxVel()) + " to " + std::to_string(v_max), + Logger::INFO); + } + + group_vel[group_name] = v_max; + group_acc[group_name] = a_max; + duration_per_group.push_back(duration); + group_names.push_back(group_name); } - int index_slowest_dof = findIndexOfMax(total_time_per_dof); + int index_slowest_group = findIndexOfMax(duration_per_group); - // phase sync - // https://theses.hal.science/tel-01285383/document p.62 ff. - for (size_t i = 0; i < diff.size(); i++) { - double lambda = 0.0; // lambda is defined in theses - if (!utility::nearlyZero(total_length_per_dof[index_slowest_dof])) { - lambda = total_length_per_dof[i] / total_length_per_dof[index_slowest_dof]; + for (size_t i = 0; i < distance_per_group.size(); ++i) + { + double lambda = 0.0; // Time sync between groups + if (!utility::nearlyZero(distance_per_group[index_slowest_group])) { + lambda = distance_per_group[i] / distance_per_group[index_slowest_group]; } - double acc_scaling_factor = 0.0; - double vel_scaling_factor = 0.0; - if (!utility::nearlyZero(a_max_vec[i])) { - acc_scaling_factor = a_max_vec[index_slowest_dof] / a_max_vec[i]; - } - if (!utility::nearlyZero(v_max_vec[i])) { - vel_scaling_factor = v_max_vec[index_slowest_dof] / v_max_vec[i]; - } + std::string current_group = group_names[i]; + double a_max = group_acc[current_group]; + double v_max = group_vel[current_group]; - a_max_vec[i] *= lambda * acc_scaling_factor; - v_max_vec[i] *= lambda * vel_scaling_factor; + double reduced_a_max = lambda * a_max; + double reduced_v_max = lambda * v_max; - double section_length = std::abs(diff[i]); - PhaseDoF acc_phase_single_dof, coast_phase_single_dof, dec_phase_single_dof; - calcPhaseTimeAndDistance(a_max_vec[i], v_max_vec[i], section_length, acc_phase_single_dof, - coast_phase_single_dof, dec_phase_single_dof); + group_acc[current_group] = reduced_a_max; + group_vel[current_group] = reduced_v_max; - acc_phase.components.push_back(acc_phase_single_dof); - coast_phase.components.push_back(coast_phase_single_dof); - dec_phase.components.push_back(dec_phase_single_dof); + double section_length = distance_per_group[i]; + PhaseComponent acc_phase_per_group, coast_phase_per_group, dec_phase_per_group; + calcPhaseDurationAndDistance(reduced_a_max, reduced_v_max, section_length, acc_phase_per_group, + coast_phase_per_group, dec_phase_per_group); + + acc_phase.components[current_group] = acc_phase_per_group; + coast_phase.components[current_group] = coast_phase_per_group; + dec_phase.components[current_group] = dec_phase_per_group; } // Find the first component duration that is not 0.0 because all of them have the same value or 0.0 auto it_acc = std::find_if(acc_phase.components.begin(), acc_phase.components.end(), - [](const PhaseDoF& phase_dof) { return !(utility::nearlyZero(phase_dof.duration)); }); + [](const auto &it) { return !(utility::nearlyZero(it.second.duration)); }); if (it_acc != acc_phase.components.end()) { - acc_phase.duration = it_acc->duration; + acc_phase.duration = it_acc->second.duration; } auto it_coast = std::find_if(coast_phase.components.begin(), coast_phase.components.end(), - [](const PhaseDoF& phase_dof) { return !(utility::nearlyZero(phase_dof.duration)); }); + [](const auto &it) { return !(utility::nearlyZero(it.second.duration)); }); if (it_coast != coast_phase.components.end()) { - coast_phase.duration = it_coast->duration; + coast_phase.duration = it_coast->second.duration; } auto it_dec = std::find_if(dec_phase.components.begin(), dec_phase.components.end(), - [](const PhaseDoF& phase_dof) { return !(utility::nearlyZero(phase_dof.duration)); }); + [](const auto &it) { return !(utility::nearlyZero(it.second.duration)); }); if (it_dec != dec_phase.components.end()) { - dec_phase.duration = it_dec->duration; + dec_phase.duration = it_dec->second.duration; } } -Section ConstantAccelerationSolver::calcSection(Point& p_start_ref, Point& p_end_ref, - SectionConstraint constraint_copy, size_t section_id) +Section ConstantAccelerationSolver::calcSection(Point& p_start_ref, Point& p_end_ref, size_t section_id) { - Section section(p_start_ref, p_end_ref, constraint_copy, section_id); + /* + + Calculate the maximum velocity that can be used in a trajectory between two waypoints(Section), this might be smaller or the same as the maximum set by the user. + Different ValueGroups, might traverse the same path between two points at different speeds, therfor all ValueGroups need to be time synchronised, + see https://www.diag.uniroma1.it/~deluca/rob1_en/14_TrajectoryPlanningCartesian.pdf such that all ValueGroups start and finish at the same time. - std::vector reduced_acceleration_per_dof; - std::vector reduced_velocity_per_dof; + */ - calcAccAndVelPerDoF(section, reduced_acceleration_per_dof, reduced_velocity_per_dof); + Section section(p_start_ref, p_end_ref, section_id); - Point p_start = section.getStartPoint(); - Point p_end = section.getEndPoint(); - Point diff = p_end - p_start; - - std::vector total_time_per_dof; - std::vector total_length_per_dof; + std::vector duration_per_group; + std::vector distance_per_group; std::vector phases; Phase acc_phase, coast_phase, dec_phase; - calcTimesAndLengthsMultiDoF(acc_phase, coast_phase, dec_phase, total_time_per_dof, total_length_per_dof, diff, - reduced_acceleration_per_dof, reduced_velocity_per_dof); + std::map& v_max = section.getVelocities(); + std::map& a_max = section.getAccelerations(); - int index_slowest_dof = findIndexOfMax(total_time_per_dof); - double T_total = total_time_per_dof[index_slowest_dof]; + + calcDurationAndDistance(acc_phase, coast_phase, dec_phase, p_start_ref, p_end_ref, duration_per_group, distance_per_group, a_max, v_max); + + // slowest ValueGroup is identified in order to slow all other groups such that they require the same time. + int index_slowest_dof = findIndexOfMax(duration_per_group); + double T_total = duration_per_group[index_slowest_dof]; section.setIndexSlowestDoF(index_slowest_dof); section.setDuration(T_total); @@ -303,316 +254,15 @@ Section ConstantAccelerationSolver::calcSection(Point& p_start_ref, Point& p_end section.setPhases(phases); - section.setAdaptedAcceleration(reduced_acceleration_per_dof); - section.setAdaptedVelocity(reduced_velocity_per_dof); - return section; } -double calcVecNorm(const std::vector& vec) // into util -{ - double sum = 0.0; - for (auto& component : vec) { - sum += std::pow(component, 2); - } - return std::sqrt(sum); -} - -void ConstantAccelerationSolver::calcPreBlendParams(double blending_dist_pre, const Section& pre_section, - Point& A_blend, double& T_blend, - double& vel_pre_blend_magnitude, - double& absolute_blend_start_time_with_shift) -{ - const Point& A = pre_section.getStartPoint(); - const Point& AB = pre_section.getDifference(); - double length_AB = pre_section.getLength(); - - A_blend = A + AB * ((length_AB - blending_dist_pre) / length_AB); - - double absolute_blend_start_time_without_shift; - Point vel_pre_blend; - calcVelAndTimeByDistance(pre_section, length_AB - blending_dist_pre, vel_pre_blend, - absolute_blend_start_time_without_shift); - vel_pre_blend_magnitude = vel_pre_blend.norm(); - - if (!utility::nearlyZero(vel_pre_blend_magnitude)) { - T_blend = 2 * blending_dist_pre / vel_pre_blend_magnitude; - } else { - T_blend = 0.0; - } - - absolute_blend_start_time_with_shift = absolute_blend_start_time_without_shift - pre_section.getTimeShift(); -} - -void ConstantAccelerationSolver::calcPostBlendParams(double blending_dist_post, const Section& post_section, - Point& C_blend, double& T_blend, - double& vel_post_blend_magnitude, - double& absolute_blend_end_time_without_shift) -{ - const Point& B = post_section.getStartPoint(); - const Point& BC = post_section.getDifference(); - double length_BC = post_section.getLength(); - - if (utility::nearlyZero(length_BC)) { - C_blend = B; - } else { - C_blend = B + BC * (blending_dist_post / length_BC); - } - - Point vel_post_blend; - calcVelAndTimeByDistance(post_section, blending_dist_post, vel_post_blend, - absolute_blend_end_time_without_shift); - vel_post_blend_magnitude = vel_post_blend.norm(); - - if (!utility::nearlyZero(vel_post_blend_magnitude)) { - T_blend = 2 * blending_dist_post / vel_post_blend_magnitude; - } else { - T_blend = 0.0; - } -} - -void ConstantAccelerationSolver::calcSecondBlendingDist(double T_blend, double T_acc_second, - double a_max_magnitude_second, - double vel_first_blend_magnitude, - double blending_dist_first, double& blending_dist_second, - [[maybe_unused]] size_t segment_id) -{ - if (utility::nearlyZero(vel_first_blend_magnitude)) { - blending_dist_second = 0.0; - return; - } - - // If "second" values are post values, the "first" values will be pre values. And vice versa - if (T_blend < T_acc_second) { -#ifdef DEBUG - std::cout << "[" << segment_id << "]" - << " Info: Blending into acceleration phase" << std::endl; -#endif - blending_dist_second = 2 * std::pow(blending_dist_first, 2) * a_max_magnitude_second - / std::pow(vel_first_blend_magnitude, 2); - } else { -#ifdef DEBUG - std::cout << "[" << segment_id << "]" - << " Info: Blending into constant velocity phase" << std::endl; -#endif - blending_dist_second - = blending_dist_first * a_max_magnitude_second * T_acc_second / vel_first_blend_magnitude; - } -} - -void ConstantAccelerationSolver::calcSegmentPreparations(const Section& pre_section, const Section& post_section, - std::vector& a_max_post, - std::vector& a_max_pre, - double& L_acc_magnitude_post, double& T_acc_post, - double& T_acc_pre) -{ - const Phase& acc_phase_post = post_section.getPhaseByType(PhaseType::ConstantAcceleration); - T_acc_post = acc_phase_post.duration; - a_max_post = post_section.getAdaptedAcceleration(); - - const Phase& acc_phase_pre = pre_section.getPhaseByType(PhaseType::ConstantAcceleration); - T_acc_pre = acc_phase_pre.duration; - a_max_pre = pre_section.getAdaptedAcceleration(); - - L_acc_magnitude_post = 0.0; - for (auto& component : acc_phase_post.components) { - L_acc_magnitude_post += std::pow(component.length, 2); - } - L_acc_magnitude_post = std::sqrt(L_acc_magnitude_post); -} - -bool ConstantAccelerationSolver::isBlendAccelerationTooHigh(const std::vector& a_max_blend, - const double& T_blend, - const double& vel_pre_blend_magnitude, - const double& vel_post_blend_magnitude, - const Section& pre_section, - const Section& post_section, size_t segment_id) -{ - const Point& dir_AB = pre_section.getDirection(); - const Point& dir_BC = post_section.getDirection(); - - Point blend_acc, blend_acc_linear, blend_acc_angular; - double blend_acc_linear_mag, blend_acc_angular_mag; - double a_max_linear_mag, a_max_angular_mag; - std::vector a_max_linear, a_max_angular; - if (!utility::nearlyZero(T_blend)) { - blend_acc = (dir_BC * vel_post_blend_magnitude - dir_AB * vel_pre_blend_magnitude) / T_blend; - size_t o_index = blend_acc.getOrientationIndex(); - - for (size_t i = 0; i < blend_acc.size(); ++i) { - if (i < o_index) { - blend_acc_linear.addValue(blend_acc[i]); - a_max_linear.push_back(a_max_blend[i]); - } else { - blend_acc_angular.addValue(blend_acc[i]); - a_max_angular.push_back(a_max_blend[i]); - } - } - blend_acc_linear_mag = blend_acc_linear.norm(); - blend_acc_angular_mag = blend_acc_angular.norm(); - - a_max_linear_mag = calcVecNorm(a_max_linear); - a_max_angular_mag = calcVecNorm(a_max_angular); - - } else { - blend_acc_angular_mag = 0.0; - blend_acc_linear_mag = 0.0; - - a_max_linear_mag = 0.0; - a_max_angular_mag = 0.0; - } - - if (blend_acc_linear_mag > a_max_linear_mag && !utility::nearlyZero(blend_acc_linear_mag)) { - std::cout << "[" << segment_id << "]" - << " Warning: Eceeding maximum linear acceleration!" << std::endl; - std::cout << "[" << segment_id << "]" - << " Linear Acceleration magnitude would be " << blend_acc_linear_mag << " m/s^2, but only " - << a_max_linear_mag << " m/s^2 is allowed" << std::endl; - std::cout << "[" << segment_id << "]" - << " Deactivating blending in this segment" << std::endl; - return true; - } else if (blend_acc_angular_mag > a_max_angular_mag && !utility::nearlyZero(blend_acc_angular_mag)) { - std::cout << "[" << segment_id << "]" - << " Warning: Eceeding maximum angular acceleration!" << std::endl; - std::cout << "[" << segment_id << "]" - << " Angular acceleration magnitude would be " << blend_acc_angular_mag << " 1/s^2, but only " - << a_max_angular_mag << " 1/s^2 is allowed" << std::endl; - std::cout << "[" << segment_id << "]" - << " Deactivating blending in this segment" << std::endl; - return true; - } else - return false; -} - -void ConstantAccelerationSolver::setNoBlendingParams(const Section& pre_section, const Section& post_section, - double& T_blend, double& t_abs_start_blend_with_shift, - double& t_abs_end_blend_without_shift, Point& A_blend, - Point& C_blend, double& vel_pre_blend_magnitude, - double& vel_post_blend_magnitude) -{ - T_blend = 0.0; - t_abs_start_blend_with_shift = pre_section.getEndTime() - pre_section.getTimeShift(); - t_abs_end_blend_without_shift = post_section.getStartTime(); - A_blend = post_section.getStartPoint(); - C_blend = A_blend; - vel_pre_blend_magnitude = 0.0; - vel_post_blend_magnitude = 0.0; -} - -std::shared_ptr -ConstantAccelerationSolver::calcBlendSegment(Section& pre_section, Section& post_section, - const SegmentConstraint& constraint, size_t segment_id, - std::map& debug_output) -{ - std::cout << "\n-- Segment " << segment_id << " --\n" << std::endl; - /* Blending from A' to C' across B with constant acceleration - https://www.diag.uniroma1.it/~deluca/rob1_en/14_TrajectoryPlanningCartesian.pdf - - B - / \ - A'.-'-. C' - / \ - A C - - */ - - double length_AB = pre_section.getLength(); - double length_BC = post_section.getLength(); - double a_max_magnitude_post, a_max_magnitude_pre, L_acc_magnitude_post, T_acc_post, T_acc_pre; - - std::vector a_max_post, a_max_pre, a_max_blend; - - calcSegmentPreparations(pre_section, post_section, a_max_post, a_max_pre, L_acc_magnitude_post, T_acc_post, - T_acc_pre); - - a_max_magnitude_post = calcVecNorm(a_max_post); - a_max_magnitude_pre = calcVecNorm(a_max_pre); - - if (a_max_magnitude_post >= a_max_magnitude_pre) { - a_max_blend = a_max_post; - } else { - a_max_blend = a_max_pre; - } - - double blending_dist_pre = constraint.getBlendDistance(); - - if (blending_dist_pre > length_AB / 2) { - std::cout << "[" << segment_id << "]" - << " Warning: Pre blending distance is croped" << std::endl; - blending_dist_pre = length_AB / 2; - } - - Point A_blend; - double T_blend, vel_pre_blend_magnitude, t_abs_start_blend_with_shift; - calcPreBlendParams(blending_dist_pre, pre_section, A_blend, T_blend, vel_pre_blend_magnitude, - t_abs_start_blend_with_shift); - - double blending_dist_post = 0.0; - calcSecondBlendingDist(T_blend, T_acc_post, a_max_magnitude_post, vel_pre_blend_magnitude, blending_dist_pre, - blending_dist_post, segment_id); - - Point vel_post_blend; - Point C_blend; - double vel_post_blend_magnitude; - double t_abs_end_blend_without_shift; - - if (blending_dist_post >= length_BC / 2) { - blending_dist_post = length_BC / 2; - - calcPostBlendParams(blending_dist_post, post_section, C_blend, T_blend, vel_post_blend_magnitude, - t_abs_end_blend_without_shift); - - calcSecondBlendingDist(T_blend, T_acc_pre, a_max_magnitude_pre, vel_post_blend_magnitude, - blending_dist_post, blending_dist_pre, segment_id); - - calcPreBlendParams(blending_dist_pre, pre_section, A_blend, T_blend, vel_pre_blend_magnitude, - t_abs_start_blend_with_shift); - - } else { - calcPosAndVelSection(T_blend, post_section, C_blend, vel_post_blend); - vel_post_blend_magnitude = vel_post_blend.norm(); - - calcVelAndTimeByDistance(post_section, blending_dist_post, vel_post_blend, t_abs_end_blend_without_shift); - } - - if (isBlendAccelerationTooHigh(a_max_blend, T_blend, vel_pre_blend_magnitude, vel_post_blend_magnitude, - pre_section, post_section, segment_id)) { - setNoBlendingParams(pre_section, post_section, T_blend, t_abs_start_blend_with_shift, - t_abs_end_blend_without_shift, A_blend, C_blend, vel_pre_blend_magnitude, - vel_post_blend_magnitude); - } - - // Absolute Time denotes a point in time relative to the first waypoint in the path - // Time shift is the result of blending in between the sections, thereby reducing the time until the next - // section is reached There are three other time frames to be aware of: Section, Segment and Phase relative - // times denoted as t_section, t_segment and t_phase respectively - double t_abs_end_blend_with_shift = t_abs_start_blend_with_shift + T_blend; - double time_shift = t_abs_end_blend_without_shift - t_abs_end_blend_with_shift; - - post_section.setTimeShift(time_shift); - - std::shared_ptr segment(new BlendSegment(pre_section, post_section, constraint, - vel_pre_blend_magnitude, vel_post_blend_magnitude, - T_blend, t_abs_start_blend_with_shift)); - - segment->setStartPoint(A_blend); - segment->setEndPoint(C_blend); - segment->setID(segment_id); - - debug_output["pre_blend_dist"] = blending_dist_pre; - debug_output["post_blend_dist"] = blending_dist_post; - debug_output["pre_blend_vel"] = vel_pre_blend_magnitude; - debug_output["post_blend_vel"] = vel_post_blend_magnitude; - - return segment; -} - -void ConstantAccelerationSolver::calcPosAndVelSingleDoFLinear(double section_dof_length, const Phase& phase, - double phase_distance_to_p_start, double t_phase, - double a_max_reduced, double v_max_reduced, - double& pos, double& vel) const +void ConstantAccelerationSolver::calcPosAndVelSingleGroup(double section_distance, const Phase& phase, + double phase_distance_to_p_start, double t_phase, + double a_max_reduced, double v_max_reduced, + double& pos, double& vel) const { - double p_i {0}, v_i {0}; + double p_i{0}, v_i{0}; if (phase.type == PhaseType::ConstantAcceleration) { p_i = 0.5 * a_max_reduced * std::pow(t_phase, 2); v_i = a_max_reduced * t_phase; @@ -623,125 +273,63 @@ void ConstantAccelerationSolver::calcPosAndVelSingleDoFLinear(double section_dof p_i = -0.5 * a_max_reduced * std::pow(t_phase, 2) + v_max_reduced * t_phase + phase_distance_to_p_start; v_i = -a_max_reduced * t_phase + v_max_reduced; } - if (utility::nearlyZero(section_dof_length)) { + if (utility::nearlyZero(section_distance)) { pos = 0; } else { - pos = p_i / section_dof_length; + pos = p_i / section_distance; } vel = v_i; } -void ConstantAccelerationSolver::calcVelAndTimeByDistance(const Section& section, double distance, - Point& velocity_per_dof, double& t_abs) +void ConstantAccelerationSolver::calcPosAndVelSection(double t_section, Section& section, Result& result) const { - const Point& dir = section.getDirection(); - - std::vector a_max_vec = section.getAdaptedAcceleration(); - std::vector v_max_vec = section.getAdaptedVelocity(); - - const Phase& phase = section.getPhaseByDistance(distance); - - // calculate the time it takes to cross "distance" in phase space - double T_distance = 0; - for (size_t i = 0; i < phase.components.size(); ++i) { - double distance_dof = std::abs(dir[i]) * distance - phase.components[i].distance_p_start; - - if (phase.type == PhaseType::ConstantAcceleration) { - if (!utility::nearlyZero(a_max_vec[i])) { - T_distance = std::sqrt(2 * distance_dof / a_max_vec[i]); - } - } else if (phase.type == PhaseType::ConstantVelocity) { - if (!utility::nearlyZero(v_max_vec[i])) { - T_distance = distance_dof / v_max_vec[i]; - } - } else if (phase.type == PhaseType::ConstantDeacceleration) { - if (!utility::nearlyZero(a_max_vec[i])) { - double radicand = std::pow(v_max_vec[i] / a_max_vec[i], 2) - 2.0 / a_max_vec[i] * distance_dof; - if (utility::nearlyZero(radicand)) { - radicand = 0.0; - } - if (radicand >= 0.0) { - T_distance = v_max_vec[i] / a_max_vec[i] - std::sqrt(radicand); - } - } - } else { - throw std::runtime_error("Error::KinematicSolver: Unrecognized phase type"); - } - - if (!utility::nearlyZero(T_distance)) { - break; - } - } - - double t_phase = T_distance; - double t_section_blend_start = t_phase + phase.t_start; - t_abs = t_section_blend_start + section.getStartTime(); + Point p_start = section.getStartPoint(); + Point p_end = section.getEndPoint(); - // calculate velocity per dof at time when distance is reached + Point diff = p_end - p_start; - double section_length = section.getLength(); - for (size_t i = 0; i < dir.size(); ++i) { - double section_dof_length = dir[i] * section_length; - double _, velocity; - calcPosAndVelSingleDoFLinear(section_dof_length, phase, phase.components[i].distance_p_start, t_phase, - a_max_vec[i], v_max_vec[i], _, velocity); - velocity_per_dof.addValue(velocity); - } -} + Phase& phase = section.getPhaseByTime(t_section); + double t_phase = t_section - phase.t_start; -void ConstantAccelerationSolver::calcPosAndVelLinearSegment(double t_section, const LinearSegment& segment, - Point& pos, Point& vel) const -{ - const Section& section = segment.getSection(); + for (const auto& [group_name, symbols] : symbol_map_) + { + double a_max = section.getAccelerations()[group_name]; + double v_max = section.getVelocities()[group_name]; - calcPosAndVelSection(t_section, section, pos, vel); -} + ValueGroup diff_group = diff[group_name]; + ValueGroup& loc = result.getLocation()[group_name]; + ValueGroup& vel = result.getVelocity()[group_name]; -void ConstantAccelerationSolver::calcPosAndVelBlendSegment(double t_segment, const BlendSegment& segment, - Point& pos, Point& vel) const -{ - Point dir_AB = segment.getPreBlendDirection(); - Point dir_BC = segment.getPostBlendDirection(); + if (symbols.isQuaternion()) { + double diff_angle = Eigen::AngleAxisd(diff_group.getQuaternionValues()).angle(); - double duration = segment.getDuration(); + // pos_interpolation_factor == [0,1] based on the requested point in time + double pos_interpolation_factor, vel_interpolation_factor; + calcPosAndVelSingleGroup(diff_angle, phase, phase.components[group_name].distance_p_start, t_phase, + a_max, v_max, pos_interpolation_factor, vel_interpolation_factor); - Point A_blend = segment.getStartPoint(); - double vel_blend_pre_magnitude = segment.getPreBlendVelocityMagnitude(); - double vel_blend_post_magnitude = segment.getPostBlendVelocityMagnitude(); + Eigen::Quaterniond startQuat = p_start[group_name].getQuaternionValues(); + Eigen::Quaterniond endQuat = p_end[group_name].getQuaternionValues(); - pos = A_blend + dir_AB * vel_blend_pre_magnitude * t_segment - + (dir_BC * vel_blend_post_magnitude - dir_AB * vel_blend_pre_magnitude) * std::pow(t_segment, 2) - / (2 * duration); + loc = startQuat.slerp(pos_interpolation_factor, endQuat); - vel = dir_AB * vel_blend_pre_magnitude - + (dir_BC * vel_blend_post_magnitude - dir_AB * vel_blend_pre_magnitude) * t_segment / duration; - pos.setOrientationIndex(dir_AB.getOrientationIndex()); - vel.setOrientationIndex(dir_AB.getOrientationIndex()); -} + Eigen::Vector3d vel_dir = Eigen::AngleAxisd(diff_group.getQuaternionValues().normalized()).axis(); + Eigen::Vector3d ang_vel = vel_dir * vel_interpolation_factor; -void ConstantAccelerationSolver::calcPosAndVelSection(double t_section, const Section& section, Point& pos, - Point& vel) const -{ - Point p_start = section.getStartPoint(); - Point p_end = section.getEndPoint(); - Point diff = p_end - p_start; + // vel is a Quaternion for convinience, it is used as a container for the angle velocity but no longer represent a rotation or orientation + vel = Eigen::Quaterniond(0.0, ang_vel.x(), ang_vel.y(), ang_vel.z()); - const std::vector& a_max_vec = section.getAdaptedAcceleration(); - const std::vector& v_max_vec = section.getAdaptedVelocity(); + } else { - const Phase& phase = section.getPhaseByTime(t_section); - double t_phase = t_section - phase.t_start; - for (size_t i = 0; i < p_start.size(); i++) { - double pos_relative_magnitude, vel_magnitude; - calcPosAndVelSingleDoFLinear(std::abs(diff[i]), phase, phase.components[i].distance_p_start, t_phase, - a_max_vec[i], v_max_vec[i], pos_relative_magnitude, vel_magnitude); + double diff_magnitude = diff_group.getCartesianValues().norm(); + ValueGroup dir_group = diff_group * (1.0 / diff_magnitude); - double pos_component = p_start[i] + pos_relative_magnitude * diff[i]; - double vel_component = vel_magnitude * utility::sign(diff[i]); + double pos_magnitude, vel_magnitude; + calcPosAndVelSingleGroup(diff_magnitude, phase, phase.components[group_name].distance_p_start, t_phase, + a_max, v_max, pos_magnitude, vel_magnitude); + loc = p_start[group_name] + diff_group * pos_magnitude; + vel = dir_group * vel_magnitude; + } - pos.addValue(pos_component); - vel.addValue(vel_component); } - pos.setOrientationIndex(p_start.getOrientationIndex()); - vel.setOrientationIndex(p_start.getOrientationIndex()); } diff --git a/src/linear_segment.cpp b/src/linear_segment.cpp deleted file mode 100644 index f8b8369..0000000 --- a/src/linear_segment.cpp +++ /dev/null @@ -1,11 +0,0 @@ -#include "sotg/linear_segment.hpp" - -#include "sotg/kinematic_solver.hpp" -using namespace SOTG; -using namespace detail; - -void LinearSegment::calcPosAndVel(double t_section, [[maybe_unused]] double t_segment, Point& pos, Point& vel, - const KinematicSolver& solver) const -{ - solver.calcPosAndVelLinearSegment(t_section, *this, pos, vel); -} \ No newline at end of file diff --git a/src/logger.cpp b/src/logger.cpp new file mode 100644 index 0000000..864d909 --- /dev/null +++ b/src/logger.cpp @@ -0,0 +1,24 @@ +#include "sotg/logger.hpp" + +using namespace SOTG; + +void Logger::log( [[maybe_unused]] const std::string& message, [[maybe_unused]] MsgType type) const +{ +#ifdef VERBOSE + switch (type) { + case INFO: + std::cout << "[ INFO] " << message << std::endl; + break; + case WARNING: + std::cout << "[ WARN] " << message << std::endl; + break; + case DEBUG: +#ifdef DEBUG + std::cout << "[DEBUG] " << message << std::endl; +#endif + break; + default: + throw std::runtime_error("SOTG Logger received message with unknown Message Type, Content: " + message); + } +#endif +} diff --git a/src/path.cpp b/src/path.cpp index 054b543..a155de5 100644 --- a/src/path.cpp +++ b/src/path.cpp @@ -1,56 +1,23 @@ + #include "sotg/path.hpp" #include using namespace SOTG; -void Path::addPoint(Point point) { waypoints_.push_back(point); } +void Path::addPoint(Point point) { + int point_id = waypoints_.size() + 1; + point.setID(point_id); + waypoints_.push_back(point); } -// should be in pitasc glue -void Path::addPoint(const std::vector& old_point) +Point& Path::getPoint(std::string name) { - Point new_point; - // Expecting xyz, abc - Eigen::VectorXd linear_point = old_point.front(); - Eigen::VectorXd angular_point = old_point.back(); - - for (auto i = 0; i < linear_point.size(); ++i) { - new_point.addValue(linear_point[i]); - } - new_point.setOrientationIndex(linear_point.size()); - for (auto i = 0; i < angular_point.size(); ++i) { - new_point.addValue(angular_point[i]); - } - - waypoints_.push_back(new_point); -} - -void Path::addPoint(const std::vector>& old_point) -{ - Point new_point; - - std::vector linear_point = old_point.front(); - std::vector angular_point = old_point.back(); - - for (size_t i = 0; i < linear_point.size(); ++i) { - new_point.addValue(linear_point[i]); - } - new_point.setOrientationIndex(linear_point.size()); - for (size_t i = 0; i < angular_point.size(); ++i) { - new_point.addValue(angular_point[i]); - } - - waypoints_.push_back(new_point); -} - -Point& Path::getPoint(size_t index) -{ - if (index > waypoints_.size() - 1) { - throw std::runtime_error("Index out of bounds, trying to access " + std::to_string(index) - + "# waypoint from a path with " + std::to_string(waypoints_.size()) - + " waypoints total"); + for (auto& point : waypoints_) { + if (point.getName().compare(name) == 0) { + return point; + } } - return waypoints_[index]; + throw std::runtime_error("No Point named \"" + name + "\" was found"); } std::ostream& Path::operator<<(std::ostream& out) diff --git a/src/path_manager.cpp b/src/path_manager.cpp index d90f28b..2b5d49d 100644 --- a/src/path_manager.cpp +++ b/src/path_manager.cpp @@ -6,7 +6,7 @@ using namespace SOTG; using namespace detail; PathManager::PathManager(std::shared_ptr solver_ptr, - std::vector>& debug_info_vec_tg) + std::vector>& debug_info_vec_tg) : kinematic_solver_(solver_ptr) , debug_info_vec_(debug_info_vec_tg) { @@ -18,17 +18,16 @@ void PathManager::resetSections() size_t num_sections = path_.getNumWaypoints() - 1; for (size_t i = 0; i < num_sections; i++) { - Section section - = kinematic_solver_->calcSection(path_.getPoint(i), path_.getPoint(i + 1), section_constraints_[i], i); + Section section = kinematic_solver_->calcSection(path_.getPoint(i), path_.getPoint(i + 1), i); sections_.push_back(section); } - double current_time = 0.0; + double section_time_offset = 0.0; for (Section& section : sections_) { double duration = section.getDuration(); - section.setStartTime(current_time); - current_time += duration; + section.setStartTime(section_time_offset); + section_time_offset += duration; #ifdef DEBUG std::cout << "Section start time: " << section.getStartTime() << ", duration: " << section.getDuration() @@ -37,118 +36,11 @@ void PathManager::resetSections() } } -void PathManager::generateBlendSegments() -{ - // Iterate over all corners that could be blended - // std::iterator is neccesary because of std::list - // If sections was a vector instead of a list the code would look like this: - // - // for( size_t i = 1; i < sections.size(); ++i) - // { - // ... - // Segment blend_segment = kinematic_solver_->calcSegment(sections[i-1], sections[i], ...); - // ... - // } - - int blend_corner_index = 0; - size_t blend_segment_id = 1; - std::list
::iterator it = sections_.begin(); - Section* last_section_addr = &(*it); - for (++it; it != sections_.end(); ++it) { - SegmentConstraint segment_constraint = segment_constraints_[blend_corner_index]; - - Section* current_section_addr = &(*it); - std::map debug_info; - std::shared_ptr blend_segment = kinematic_solver_->calcBlendSegment( - *last_section_addr, *current_section_addr, segment_constraint, blend_segment_id, debug_info); - last_section_addr = current_section_addr; - ++blend_corner_index; - blend_segment_id += 2; // There is always one linear Segment in between - - segments_.push_back(blend_segment); - debug_info_vec_.push_back(debug_info); - } -} - -void PathManager::generateLinearSegments() -{ - if (segments_.size() < 1) { - std::shared_ptr single_segment( - new LinearSegment(sections_.front(), sections_.front().getEndTime(), 0.0)); - single_segment->setID(0); - - segments_.push_back(single_segment); - } else { - size_t lin_segment_id = 0; - std::list>::iterator it_segments; - - // Generate linear Segments inbetween the already existing blend segments, if any exist - double last_t_end = 0; - for (it_segments = segments_.begin(); it_segments != segments_.end(); ++it_segments) { - std::shared_ptr blend_segment = *it_segments; - Section& pre_section = blend_segment->getPreBlendSection(); - - double duration = blend_segment->getStartTime() - last_t_end; - double t_start = last_t_end; - std::shared_ptr segment(new LinearSegment(pre_section, duration, t_start)); - - last_t_end = blend_segment->getEndTime(); - - segment->setID(lin_segment_id); - lin_segment_id += 2; // There is always one blend Segment in between - - segments_.insert(it_segments, segment); - } - - // Generate the last linear Segment - it_segments = std::prev(segments_.end()); - std::shared_ptr last_blend_segment = *it_segments; - Section& last_section = last_blend_segment->getPostBlendSection(); - - double t_end_without_shift = last_section.getEndTime(); - double t_end_with_shift = t_end_without_shift - last_section.getTimeShift(); - double duration = t_end_with_shift - last_blend_segment->getEndTime(); - - std::shared_ptr last_segment(new LinearSegment(last_section, duration, last_t_end)); - - last_segment->setID(lin_segment_id); - - segments_.push_back(last_segment); - -#ifdef DEBUG - for (auto& segment : segments_) { - std::cout << "Segment " << segment->getID() << " start time: " << segment->getStartTime() - << ", duration: " << segment->getDuration() << std::endl; - } -#endif - } -} - -void PathManager::resetSegments() -{ - segments_.clear(); - - generateBlendSegments(); - - generateLinearSegments(); -} - -void PathManager::resetPath(Path new_path, std::vector new_section_constraints, - std::vector new_segment_constraints) +void PathManager::resetPath(Path new_path) { path_ = new_path; - segment_constraints_ = new_segment_constraints; - section_constraints_ = new_section_constraints; - - if (path_.getNumWaypoints() != section_constraints_.size() + 1) { - std::runtime_error("PathPlanner: Wrong amount of constrains to path, " - + std::to_string(section_constraints_.size()) + " constraints where given, but " - + std::to_string(path_.getNumWaypoints()) + " are needed"); - } resetSections(); - - resetSegments(); } std::ostream& PathManager::operator<<(std::ostream& out) @@ -168,31 +60,13 @@ std::ostream& PathManager::operator<<(std::ostream& out) return out; } -const Segment& PathManager::getSegmentAtTime(double time) -{ - double last_t_end = 0.0; - for (std::shared_ptr segment : segments_) { - double t_end = segment->getStartTime() + segment->getDuration(); - if (time > last_t_end && (time < t_end || (utility::nearlyEqual(time, t_end, 1e-6)))) { - return *segment; - } - last_t_end = t_end; - } - if (utility::nearlyEqual(time, 0.0, 1e-6)) { - return *(segments_.front()); - } - - throw std::runtime_error("Requested time \"" + std::to_string(time) - + "\" could not be mapped to any segment!"); -} - -const Section& PathManager::getSectionAtTime(double time) +Section& PathManager::getSectionAtTime(double time) { double last_t_end = 0.0; - for (const Section& section : sections_) { + for (Section& section : sections_) { // Explanation for time_shift in the Section class - double time_shift = section.getTimeShift(); - double t_end = section.getStartTime() + section.getDuration() - time_shift; + // double time_shift = section.getTimeShift(); + double t_end = section.getStartTime() + section.getDuration(); // - time_shift; if (time > last_t_end && (time < t_end || (utility::nearlyEqual(time, t_end, 1e-6)))) { return section; } diff --git a/src/point.cpp b/src/point.cpp deleted file mode 100644 index 762004c..0000000 --- a/src/point.cpp +++ /dev/null @@ -1,135 +0,0 @@ -#include "sotg/point.hpp" - -using namespace SOTG; - -Point::Point() { } - -Point::Point(std::vector vec_list) -{ - for (auto& vec : vec_list) - for (auto i = 0; i < vec.size(); ++i) - addValue(vec[i]); - setOrientationIndex(vec_list[0].size()); -} - -Point Point::operator+(const Point& p2) const -{ - Point new_point; - if (values_.size() != p2.size()) - throw std::runtime_error("Error: Adding two points of different size!"); - else { - if (p2.getOrientationIndex() != orientation_index_) { - if (p2.getOrientationIndex() != -1) - new_point.setOrientationIndex(p2.getOrientationIndex()); - else if (orientation_index_ != -1) - new_point.setOrientationIndex(orientation_index_); - } - for (size_t i = 0; i < values_.size(); i++) { - new_point.addValue(values_[i] + p2.getValue(i)); - } - new_point.setOrientationIndex(orientation_index_); - return new_point; - } -} - -Point Point::operator-() const -{ - Point new_point; - for (auto& value : values_) - new_point.addValue(-value); - new_point.setOrientationIndex(orientation_index_); - return new_point; -} - -Point Point::operator-(const Point& p2) const -{ - Point new_point; - if (values_.size() != p2.size()) { - std::ostringstream os1, os2; - os1 << *this; - os2 << p2; - - throw std::runtime_error("Trying to substract two points of different size!, left Point:" + os1.str() - + ", right Point:" + os2.str()); - } else { - if (p2.getOrientationIndex() != orientation_index_) { - if (p2.getOrientationIndex() != -1) - new_point.setOrientationIndex(p2.getOrientationIndex()); - else if (orientation_index_ != -1) - new_point.setOrientationIndex(orientation_index_); - } - for (size_t i = 0; i < values_.size(); i++) { - new_point.addValue(values_[i] - p2.getValue(i)); - } - new_point.setOrientationIndex(orientation_index_); - return new_point; - } -} - -double Point::operator[](size_t index) const -{ - if (index >= values_.size()) { - throw std::runtime_error("Index out of bounds, trying to access " + std::to_string(index) - + "# value from a point with " + std::to_string(values_.size()) - + " values total"); - } - return values_[index]; -} - -double Point::norm() -{ - double sum = 0.0; - for (auto& value : values_) { - sum += std::pow(value, 2); - } - - return std::sqrt(sum); -} - -void Point::zeros(size_t num_components) { values_ = std::vector(num_components); } - -Point Point::operator/(const double& scalar) const -{ - if (utility::nearlyZero(scalar)) { - std::ostringstream os; - os << *this; - throw std::runtime_error("Trying to divide the point " + os.str() + " by 0!"); - } - - Point new_point; - for (double value : values_) { - new_point.addValue(value / scalar); - } - new_point.setOrientationIndex(orientation_index_); - return new_point; -} - -Point Point::operator*(const double& scalar) const -{ - Point new_point; - for (double value : values_) { - new_point.addValue(value * scalar); - } - new_point.setOrientationIndex(orientation_index_); - return new_point; -} - -Point Point::getLocation() -{ - Point new_point; - for (size_t i = 0; i < values_.size(); ++i) { - if (int(i) < orientation_index_) - new_point.addValue(values_[i]); - } - return new_point; -} - -Point Point::getOrientation() -{ - Point new_point; - for (size_t i = 0; i < values_.size(); ++i) { - if (int(i) >= orientation_index_) - new_point.addValue(values_[i]); - } - return new_point; -} \ No newline at end of file diff --git a/src/section.cpp b/src/section.cpp index 85deade..215a2c8 100644 --- a/src/section.cpp +++ b/src/section.cpp @@ -3,22 +3,7 @@ using namespace SOTG; using namespace detail; -Section::Section(Point& p_start_ref, Point& p_end_ref, SectionConstraint constraint_copy, size_t section_id) - : start_point_(p_start_ref) - , end_point_(p_end_ref) - , constraint_(constraint_copy) - , id_(section_id) -{ - diff_ = end_point_ - start_point_; - length_ = diff_.norm(); - if (utility::nearlyZero(length_)) { - dir_.zeros(diff_.size()); - } else { - dir_ = diff_ / length_; - } -} - -const Phase& Section::getPhaseByTime(double time) const +Phase& Section::getPhaseByTime(double time) { double previous_time = 0.0; for (auto& phase : phases_) { @@ -37,7 +22,7 @@ const Phase& Section::getPhaseByTime(double time) const } } -const Phase& Section::getPhaseByType(PhaseType type) const +Phase& Section::getPhaseByType(PhaseType type) { auto it = std::find_if(phases_.begin(), phases_.end(), [&](const Phase& phase) { return type == phase.type; }); @@ -49,7 +34,7 @@ const Phase& Section::getPhaseByType(PhaseType type) const } } -const Phase& Section::getPhaseByDistance(double distance) const +Phase& Section::getPhaseByDistance(double distance) { // if (utility::nearlyZero(distance)) { // return getPhaseByType(PhaseType::ConstantAcceleration); diff --git a/src/section_constraint.cpp b/src/section_constraint.cpp deleted file mode 100644 index 933d3e9..0000000 --- a/src/section_constraint.cpp +++ /dev/null @@ -1,12 +0,0 @@ -#include "sotg/section_constraint.hpp" - -using namespace SOTG; - -SectionConstraint::SectionConstraint(double acc_lin, double acc_ang, double vel_lin, double vel_ang) -{ - acceleration_magnitude_linear_ = acc_lin; - acceleration_magnitude_angular_ = acc_ang; - - velocity_magnitude_linear_ = vel_lin; - velocity_magnitude_angular_ = vel_ang; -} \ No newline at end of file diff --git a/src/segment_constraint.cpp b/src/segment_constraint.cpp deleted file mode 100644 index db7b59f..0000000 --- a/src/segment_constraint.cpp +++ /dev/null @@ -1,7 +0,0 @@ -#include "sotg/segment_constraint.hpp" - -using namespace SOTG; - -SegmentConstraint::SegmentConstraint() { } - -SegmentConstraint::SegmentConstraint(double blend_dist) { blending_distance_ = blend_dist; } \ No newline at end of file diff --git a/src/symbol_group.cpp b/src/symbol_group.cpp new file mode 100644 index 0000000..74e47bf --- /dev/null +++ b/src/symbol_group.cpp @@ -0,0 +1,39 @@ +#include "sotg/symbol_group.hpp" + +#include +#include + +using namespace SOTG; + +bool SymbolGroup::operator==(const SymbolGroup& symbol_group) const +{ + return symbol_group.getSymbols() == symbols_; +} +bool SymbolGroup::operator!=(const SymbolGroup& symbol_group) const +{ + return symbol_group.getSymbols() != symbols_; +} + +std::string SymbolGroup::str() const +{ + std::string output; + output.append("[ "); + for (auto& symbol : symbols_) { + output.append(symbol); + output.append(","); + } + output.pop_back(); + output.append(" ]"); + + return output; +} + +size_t SymbolGroup::getIndex(std::string key) +{ + auto it = std::find(symbols_.begin(), symbols_.end(), key); + if (it != symbols_.end()) { + return it - symbols_.begin(); + } else { + throw std::runtime_error("Symbol \"" + key + "\" not in symbol group"); + } +} diff --git a/src/trajectory_generator.cpp b/src/trajectory_generator.cpp index 0e7d6d5..9acaa6f 100644 --- a/src/trajectory_generator.cpp +++ b/src/trajectory_generator.cpp @@ -6,49 +6,50 @@ using namespace SOTG; using namespace detail; -TrajectoryGenerator::TrajectoryGenerator() - : kinematic_solver_(new detail::ConstantAccelerationSolver) +TrajectoryGenerator::TrajectoryGenerator(SymbolGroupMap symbol_groups) + : symbol_group_map_(symbol_groups) + , default_logger_(new Logger()) + , logger_(*default_logger_) + , kinematic_solver_(new detail::ConstantAccelerationSolver(symbol_group_map_, logger_)) { path_manager_ = std::unique_ptr(new detail::PathManager(kinematic_solver_, debug_info_vec_)); } -void TrajectoryGenerator::resetPath(Path path, std::vector section_constraints, - std::vector segment_constraints) +TrajectoryGenerator::TrajectoryGenerator(SymbolGroupMap symbol_groups, const Logger& logger) + : symbol_group_map_(symbol_groups) + , logger_(logger) + , kinematic_solver_(new detail::ConstantAccelerationSolver(symbol_group_map_, logger_)) { - path_manager_->resetPath(path, section_constraints, segment_constraints); + path_manager_ + = std::unique_ptr(new detail::PathManager(kinematic_solver_, debug_info_vec_)); } +void TrajectoryGenerator::resetPath(Path path) { path_manager_->resetPath(path); } + double TrajectoryGenerator::getDuration() { double total_time = 0.0; - for (std::shared_ptr segment : path_manager_->getSegments()) { - total_time += segment->getDuration(); + for (const auto section : path_manager_->getSections()) { + total_time += section.getDuration(); } return total_time; } -void TrajectoryGenerator::calcPositionAndVelocity(double time, Point& pos, Point& vel, int& id, - bool disable_blending) +int SOTG::TrajectoryGenerator::getNumPassedWaypoints(double tick) { - if (disable_blending) { - const Section& section = path_manager_->getSectionAtTime(time); - - double t_section = time - section.getStartTime(); + const Section& section = path_manager_->getSectionAtTime(tick); + const Point& section_start_point = section.getStartPoint(); - kinematic_solver_->calcPosAndVelSection(t_section, section, pos, vel); - } else { - const Section& section = path_manager_->getSectionAtTime(time); - const Segment& segment = path_manager_->getSegmentAtTime(time); + return section_start_point.getID(); - double t_segment = time - segment.getStartTime(); +} - // t_section needs to be time shifted because the start times for the section where calculated without - // knowing how much time would be saved by blending between sections - double t_section = time - (section.getStartTime() - section.getTimeShift()); +void TrajectoryGenerator::calcPositionAndVelocity(double time, Result& result) +{ + Section& section = path_manager_->getSectionAtTime(time); - segment.calcPosAndVel(t_section, t_segment, pos, vel, *kinematic_solver_); + double t_section = time - section.getStartTime(); - id = segment.getID(); - } + kinematic_solver_->calcPosAndVelSection(t_section, section, result); } \ No newline at end of file diff --git a/src/value_group.cpp b/src/value_group.cpp new file mode 100644 index 0000000..82f7d5b --- /dev/null +++ b/src/value_group.cpp @@ -0,0 +1,104 @@ + +#include "sotg/value_group.hpp" + +#include + +using namespace SOTG; + +void ValueGroup::operator=(std::initializer_list list) +{ + if (symbol_group_.size() != list.size()) { + throw std::runtime_error("Amount of symbols and values does not match"); + } + + if(is_quaternion_) { + quat_values_ = Eigen::Quaterniond(list.begin()[0],list.begin()[1],list.begin()[2],list.begin()[3]); + } else { + for (size_t i = 0; i < list.size(); i++) { + values_(i) = list.begin()[i]; + } + } + +} + +void ValueGroup::operator=(Eigen::Quaterniond quat) +{ + if (symbol_group_.size() != 4) { + throw std::runtime_error("A quaternion requires 4 Symbols, " + std::to_string(symbol_group_.size()) + + " provided"); + } + + quat_values_ = quat; +} + +// void ValueGroup::operator=(const ValueGroup& vg) +// { +// is_quaternion_ = vg.is_quaternion_; +// values_ = vg.values_; +// quat_values_ = vg.quat_values_; +// a_max_ = vg.a_max_; +// v_max_ = vg.v_max_; +// blend_radius_ = blend_radius_; +// } + +void ValueGroup::operator=(Eigen::VectorXd vec) { + + if (is_quaternion_) { + quat_values_ = Eigen::Quaterniond(vec[0], vec[1], vec[2], vec[3]); + } else { + values_ = vec; + } +} + +double& ValueGroup::operator[](std::string key) +{ + size_t index = symbol_group_.getIndex(key); + if (!symbol_group_.isQuaternion()) { + return values_[index]; + } else { + switch (index) { + case 0: + return quat_values_.w(); + break; + case 1: + return quat_values_.x(); + break; + case 2: + return quat_values_.y(); + break; + case 3: + return quat_values_.z(); + break; + default: + throw std::runtime_error("Requested symbol: \"" + key + "\" with index: " + std::to_string(index) + + " but quaternions only have 4 components"); + break; + } + } +} + +ValueGroup ValueGroup::operator+(const ValueGroup& value_group) const +{ + if (value_group.getSymbols() != symbol_group_) { + throw std::runtime_error("Adding value group with symbols: \"" + value_group.getSymbols().str() + + "\" to value group with different symbols: \"" + symbol_group_.str() + + "\" is not allowed"); + } + + ValueGroup out(symbol_group_); + if (this->isQuaternion()) { + // handle that + } else { + + out = this->getCartesianValues() + value_group.getCartesianValues(); + } + return out; + +} + +ValueGroup ValueGroup::operator*(double scalar) const +{ + ValueGroup out(symbol_group_); + out = this->getCartesianValues() * scalar; + return out; +} \ No newline at end of file diff --git a/tests/test.cpp b/tests/test.cpp new file mode 100644 index 0000000..8dacd47 --- /dev/null +++ b/tests/test.cpp @@ -0,0 +1,100 @@ +#include + +#include "sotg/sotg.hpp" + +Eigen::Quaterniond eulerToQuat(double roll, double pitch, double yaw); +std::string vec_to_string(Eigen::Vector3d vec); +Eigen::Vector3d quatToEuler(Eigen::Quaterniond quat); + +class TestLogger : public SOTG::Logger +{ +public: + void log(const std::string& message, [[maybe_unused]] MsgType type = INFO) const override final + { + std::cout << message << std::endl; + } +}; + +int main() +{ + SOTG::SymbolGroupMap symbol_map; + symbol_map["pos"] = {"x", "y", "z"}; + symbol_map["rot"] = {"w", "x", "y", "z"}; + symbol_map["rot"].setQuaternion(true); + + SOTG::Point p1(symbol_map, "p1"); + p1["pos"] = {0.0, 0.0, 0.0}; + p1["rot"] = {0.0, 0.0, 0.0, 1.0}; + + p1["pos"].setConstraints(1.0, 1.0); + p1["rot"].setConstraints(1.0, 1.0); + + SOTG::Point p2(symbol_map, "p2"); + p2["pos"] = {1.0, 0.0, 0.0}; + p2["rot"] = {1.0, 0.0, 0.0, 0.0}; + + p2["pos"].setConstraints(1.0, 5.0); + p2["rot"].setConstraints(1.0, 1.0); + + + // SOTG::SymbolGroupMap symb_map2; + // symb_map2["A1"] = {"a"}; + // symb_map2["A2"] = {"a"}; + // symb_map2["A3"] = {"a"}; + // symb_map2["A4"] = {"a"}; + // symb_map2["A5"] = {"a"}; + // symb_map2["A6"] = {"a"}; + + // SOTG::Point p2(symb_map2, "p2"); + // p2["A3"] = {0.25}; + // p2["A5"] = {0.0}; + + SOTG::Path my_path; + my_path.addPoint(p1); + my_path.addPoint(p2); + my_path.addPoint(p1); + + + TestLogger logger; + SOTG::TrajectoryGenerator tg(symbol_map, logger); + + tg.resetPath(my_path); + + double duration = tg.getDuration(); + + std::cout << "Duration: " << duration << "s" << std::endl; + + SOTG::Result result(symbol_map); + + for (size_t i = 0; i < 100; ++i) + { + double tick = i/100.0 * duration; + tg.calcPositionAndVelocity(tick, result); + Eigen::Vector3d euler_ang = quatToEuler(result.getLocation()["rot"].getQuaternionValues()); + Eigen::Vector3d euler_omega = result.getVelocity()["rot"].getQuaternionValues().vec(); + std::cout << " --- " << tick << "s --- " << std::endl; + std::cout << result.getLocation() << std::endl; + std::cout << result.getVelocity() << std::endl; + std::cout << "Angle: " << vec_to_string(euler_ang) << std::endl; + std::cout << "Omega: " << vec_to_string(euler_omega) << std::endl; + std::cout << " --- " << std::endl; + + } +} + +std::string vec_to_string(Eigen::Vector3d vec) { + const double PI = std::atan(1.0)*4; + vec *= 180/PI; + return "{" + std::to_string(vec.x()) + ", " + std::to_string(vec.y()) + ", " + std::to_string(vec.z()) + " }"; +} + +Eigen::Quaterniond eulerToQuat(double roll, double pitch, double yaw) +{ + using namespace Eigen; + Quaterniond quat; + quat = AngleAxisd(roll, Vector3d::UnitX()) * AngleAxisd(pitch, Vector3d::UnitY()) + * AngleAxisd(yaw, Vector3d::UnitZ()); + return quat; +} + +Eigen::Vector3d quatToEuler(Eigen::Quaterniond quat) { return quat.toRotationMatrix().eulerAngles(0, 1, 2); } \ No newline at end of file