From a66ac26b1f108e32650f3b05264e3b8c0cb4686b Mon Sep 17 00:00:00 2001 From: Nermin Covic Date: Mon, 22 Sep 2025 12:22:05 +0200 Subject: [PATCH 01/40] Added function to check if two vectors are linearly dependent --- include/state_spaces/StateSpace.h | 2 ++ .../real_vector_space/RealVectorSpace.h | 2 ++ .../real_vector_space/RealVectorSpace.cpp | 15 +++++++++++++++ 3 files changed, 19 insertions(+) diff --git a/include/state_spaces/StateSpace.h b/include/state_spaces/StateSpace.h index 95263c77..a14bccbc 100644 --- a/include/state_spaces/StateSpace.h +++ b/include/state_spaces/StateSpace.h @@ -44,6 +44,8 @@ namespace base const std::shared_ptr q2, const std::vector> &limits_ = {}) = 0; virtual std::shared_ptr pruneEdge2(const std::shared_ptr q1, const std::shared_ptr q2, float max_edge_length) = 0; + virtual bool checkLinearDependency(const std::shared_ptr q0, const std::shared_ptr q1, + const std::shared_ptr q2) = 0; virtual void preprocessPath(const std::vector> &original_path, std::vector> &new_path, float max_edge_length) = 0; diff --git a/include/state_spaces/real_vector_space/RealVectorSpace.h b/include/state_spaces/real_vector_space/RealVectorSpace.h index c14b8f97..5c295633 100644 --- a/include/state_spaces/real_vector_space/RealVectorSpace.h +++ b/include/state_spaces/real_vector_space/RealVectorSpace.h @@ -37,6 +37,8 @@ namespace base const std::shared_ptr q2, const std::vector> &limits_) override; std::shared_ptr pruneEdge2(const std::shared_ptr q1, const std::shared_ptr q2, float max_edge_length) override; + bool checkLinearDependency(const std::shared_ptr q0, const std::shared_ptr q1, + const std::shared_ptr q2) override; void preprocessPath(const std::vector> &original_path, std::vector> &new_path, float max_edge_length) override; diff --git a/src/state_spaces/real_vector_space/RealVectorSpace.cpp b/src/state_spaces/real_vector_space/RealVectorSpace.cpp index 7d6c22c5..0369b233 100644 --- a/src/state_spaces/real_vector_space/RealVectorSpace.cpp +++ b/src/state_spaces/real_vector_space/RealVectorSpace.cpp @@ -182,6 +182,21 @@ std::shared_ptr base::RealVectorSpace::pruneEdge2(const std::shared return q2; } +/// @brief Check linear dependency of vectors [q0,q1] and [q1,q2]. +/// @return True if vectors are linearly dependent (collinear), and false otherwise. +bool base::RealVectorSpace::checkLinearDependency(const std::shared_ptr q0, const std::shared_ptr q1, + const std::shared_ptr q2) +{ + for (size_t k = 1; k < num_dimensions; k++) + { + if (std::abs((q2->getCoord(k) - q1->getCoord(k)) / (q1->getCoord(k) - q0->getCoord(k)) - + (q2->getCoord(k-1) - q1->getCoord(k-1)) / (q1->getCoord(k-1) - q0->getCoord(k-1))) > + RealVectorSpaceConfig::EQUALITY_THRESHOLD) // Two vectors are non-linearly dependent + return false; + } + return true; +} + /// @brief Generate a new path 'new_path' from a path 'original_path' in a way that the distance between two adjacent nodes /// is fixed (if possible) to a length of 'max_edge_length'. Geometrically, the new path remains the same as the original one, /// but only their nodes may differ. From 8ce32f90ccc05f96807f36f7f86007c3206499f2 Mon Sep 17 00:00:00 2001 From: Nermin Covic Date: Mon, 22 Sep 2025 12:23:23 +0200 Subject: [PATCH 02/40] Minor fixes... --- data/planar_2dof/scenario_test/scenario_test.yaml | 3 +++ include/robots/AbstractRobot.h | 2 +- include/state_spaces/State.h | 2 +- include/state_spaces/real_vector_space/CollisionAndDistance.h | 2 +- include/state_spaces/real_vector_space/RealVectorSpace.h | 4 ++-- tests/tests_realvectorspacestate.h | 2 +- 6 files changed, 9 insertions(+), 6 deletions(-) diff --git a/data/planar_2dof/scenario_test/scenario_test.yaml b/data/planar_2dof/scenario_test/scenario_test.yaml index 00c1e4a1..829d4e59 100644 --- a/data/planar_2dof/scenario_test/scenario_test.yaml +++ b/data/planar_2dof/scenario_test/scenario_test.yaml @@ -26,6 +26,9 @@ robot: min_dist_start_goal: 0.1 # If greater than zero, 'q_start' and 'q_goal' will be generated randomly, where the workspace distance between them is minimally 'min_dist_start_goal' WS_center: [0.0, 0.0, 0.0] # Workspace center point in [m] WS_radius: 2 # Workspace radius in [m] assuming spherical workspace shape + max_vel: [1, 1] # Maximal velocity of each robot's joint in [rad/s] + max_acc: [10, 10] # Maximal acceleration of each robot's joint in [rad/s²] + max_jerk: [100, 100] # Maximal jerk of each robot's joint in [rad/s³] testing: max_num: 1000 # Maximal number of tests that should be carried out diff --git a/include/robots/AbstractRobot.h b/include/robots/AbstractRobot.h index 0ae7c39a..4a03b90d 100644 --- a/include/robots/AbstractRobot.h +++ b/include/robots/AbstractRobot.h @@ -9,7 +9,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/include/state_spaces/State.h b/include/state_spaces/State.h index 135cdc02..b62e5e5f 100644 --- a/include/state_spaces/State.h +++ b/include/state_spaces/State.h @@ -7,7 +7,7 @@ #ifndef RPMPL_STATE_H #define RPMPL_STATE_H -#include +#include #include #include #include diff --git a/include/state_spaces/real_vector_space/CollisionAndDistance.h b/include/state_spaces/real_vector_space/CollisionAndDistance.h index 30e814ef..b0fa1534 100644 --- a/include/state_spaces/real_vector_space/CollisionAndDistance.h +++ b/include/state_spaces/real_vector_space/CollisionAndDistance.h @@ -7,7 +7,7 @@ #include #include -#include +#include #include // #include // #include diff --git a/include/state_spaces/real_vector_space/RealVectorSpace.h b/include/state_spaces/real_vector_space/RealVectorSpace.h index 5c295633..367be369 100644 --- a/include/state_spaces/real_vector_space/RealVectorSpace.h +++ b/include/state_spaces/real_vector_space/RealVectorSpace.h @@ -30,9 +30,9 @@ namespace base bool isEqual(const std::shared_ptr q1, const std::shared_ptr q2) override; bool isEqual(const Eigen::VectorXf &q1_coord, const Eigen::VectorXf &q2_coord) override; std::shared_ptr interpolateEdge - (const std::shared_ptr q1, const std::shared_ptr q2, float step, float dist) override; + (const std::shared_ptr q1, const std::shared_ptr q2, float step, float dist = -1) override; std::tuple> interpolateEdge2 - (const std::shared_ptr q1, const std::shared_ptr q2, float step, float dist) override; + (const std::shared_ptr q1, const std::shared_ptr q2, float step, float dist = -1) override; std::shared_ptr pruneEdge(const std::shared_ptr q1, const std::shared_ptr q2, const std::vector> &limits_) override; std::shared_ptr pruneEdge2(const std::shared_ptr q1, diff --git a/tests/tests_realvectorspacestate.h b/tests/tests_realvectorspacestate.h index b8bc590f..f2d6db73 100644 --- a/tests/tests_realvectorspacestate.h +++ b/tests/tests_realvectorspacestate.h @@ -2,7 +2,7 @@ // Created by dinko on 28.5.21.. // #include "RealVectorSpaceState.h" -#include +#include TEST(RealVectorSpaceStateTest, testConstructor) From e8d75bd6947d97ccd1f6586ad7d7230cc4cd74cb Mon Sep 17 00:00:00 2001 From: Nermin Covic Date: Mon, 22 Sep 2025 12:24:23 +0200 Subject: [PATCH 03/40] Fixed a bug when more than two splines exist. --- include/planners/trajectory/CompositeSpline.h | 3 +++ src/planners/trajectory/CompositeSpline.cpp | 20 +++++++++---------- 2 files changed, 13 insertions(+), 10 deletions(-) diff --git a/include/planners/trajectory/CompositeSpline.h b/include/planners/trajectory/CompositeSpline.h index 82246b33..d17d3255 100644 --- a/include/planners/trajectory/CompositeSpline.h +++ b/include/planners/trajectory/CompositeSpline.h @@ -35,6 +35,9 @@ namespace planning::trajectory bool compute([[maybe_unused]] const Eigen::VectorXf &q_final, [[maybe_unused]] const Eigen::VectorXf &q_final_dot, [[maybe_unused]] const Eigen::VectorXf &q_final_ddot) override { return false; } bool checkConstraints([[maybe_unused]] size_t idx, [[maybe_unused]] float t_f) override { return false; } + + private: + std::vector times_connecting; }; } diff --git a/src/planners/trajectory/CompositeSpline.cpp b/src/planners/trajectory/CompositeSpline.cpp index ba8eaee3..5003ff26 100644 --- a/src/planners/trajectory/CompositeSpline.cpp +++ b/src/planners/trajectory/CompositeSpline.cpp @@ -13,7 +13,10 @@ planning::trajectory::CompositeSpline::CompositeSpline(const std::vectorgetTimeFinal(); + times_connecting.emplace_back(time_final_sum); + } time_final = time_final_sum; } @@ -21,13 +24,10 @@ planning::trajectory::CompositeSpline::~CompositeSpline() {} size_t planning::trajectory::CompositeSpline::findSubsplineIdx(float t) { - float time_final_sum { 0 }; - for (size_t i = 0; i < subsplines.size(); i++) + for (size_t idx = 0; idx < subsplines.size(); idx++) { - if (t < subsplines[i]->getTimeFinal() + time_final_sum) - return i; - else - time_final_sum += subsplines[i]->getTimeFinal(); + if (t < times_connecting[idx]) + return idx; } return subsplines.size()-1; @@ -36,28 +36,28 @@ size_t planning::trajectory::CompositeSpline::findSubsplineIdx(float t) Eigen::VectorXf planning::trajectory::CompositeSpline::getPosition(float t) { size_t idx { findSubsplineIdx(t) }; - float t_offset { idx > 0 ? subsplines[idx-1]->getTimeFinal() : 0 }; + float t_offset { idx > 0 ? times_connecting[idx-1] : 0 }; return subsplines[idx]->getPosition(t - t_offset); } Eigen::VectorXf planning::trajectory::CompositeSpline::getVelocity(float t) { size_t idx { findSubsplineIdx(t) }; - float t_offset { idx > 0 ? subsplines[idx-1]->getTimeFinal() : 0 }; + float t_offset { idx > 0 ? times_connecting[idx-1] : 0 }; return subsplines[idx]->getVelocity(t - t_offset); } Eigen::VectorXf planning::trajectory::CompositeSpline::getAcceleration(float t) { size_t idx { findSubsplineIdx(t) }; - float t_offset { idx > 0 ? subsplines[idx-1]->getTimeFinal() : 0 }; + float t_offset { idx > 0 ? times_connecting[idx-1] : 0 }; return subsplines[idx]->getAcceleration(t - t_offset); } Eigen::VectorXf planning::trajectory::CompositeSpline::getJerk(float t) { size_t idx { findSubsplineIdx(t) }; - float t_offset { idx > 0 ? subsplines[idx-1]->getTimeFinal() : 0 }; + float t_offset { idx > 0 ? times_connecting[idx-1] : 0 }; return subsplines[idx]->getJerk(t - t_offset); } From 4501ed0b5ca224b72bad065c99fb4df92204ee13 Mon Sep 17 00:00:00 2001 From: Nermin Covic Date: Mon, 22 Sep 2025 12:25:13 +0200 Subject: [PATCH 04/40] Changed a way the path is preprocessed. --- .../real_vector_space/RealVectorSpace.cpp | 51 +++++-------------- 1 file changed, 14 insertions(+), 37 deletions(-) diff --git a/src/state_spaces/real_vector_space/RealVectorSpace.cpp b/src/state_spaces/real_vector_space/RealVectorSpace.cpp index 0369b233..165351b9 100644 --- a/src/state_spaces/real_vector_space/RealVectorSpace.cpp +++ b/src/state_spaces/real_vector_space/RealVectorSpace.cpp @@ -214,9 +214,6 @@ void base::RealVectorSpace::preprocessPath(const std::vector> path { init_path.front() }; - std::shared_ptr q0 { nullptr }; - std::shared_ptr q1 { nullptr }; - std::shared_ptr q2 { nullptr }; // std::cout << "Initial path is: \n"; // for (size_t i = 0; i < init_path.size(); i++) @@ -225,20 +222,8 @@ void base::RealVectorSpace::preprocessPath(const std::vectorgetCoord(k) - q1->getCoord(k)) / (q1->getCoord(k) - q0->getCoord(k)) - - (q2->getCoord(k-1) - q1->getCoord(k-1)) / (q1->getCoord(k-1) - q0->getCoord(k-1))) > - RealVectorSpaceConfig::EQUALITY_THRESHOLD) - { - path.emplace_back(q1); - break; - } - } + if (!checkLinearDependency(init_path[i-1], init_path[i], init_path[i+1])) + path.emplace_back(init_path[i]); } path.emplace_back(init_path.back()); @@ -249,30 +234,22 @@ void base::RealVectorSpace::preprocessPath(const std::vector q_new { nullptr }; for (size_t i = 1; i < path.size(); i++) { - status = base::State::Status::Advanced; - q0 = path[i-1]; - q1 = path[i]; + dist = getNorm(path[i-1], path[i]); + N = std::floor(getNorm(path[i-1], path[i]) / max_edge_length); + dist_new = dist / (N+1); - while (status == base::State::Status::Advanced) - { - dist = getNorm(q0, q1); - if (dist > max_edge_length) - { - q0 = interpolateEdge(q0, q1, max_edge_length, dist); - status = base::State::Status::Advanced; - } - else - { - q0 = q1; - status = base::State::Status::Reached; - } - new_path.emplace_back(q0); - } + for (size_t j = 1; j <= N; j++) + { + q_new = interpolateEdge(path[i-1], path[i], j * dist_new, dist); + new_path.emplace_back(q_new); + } + new_path.emplace_back(path[i]); } // std::cout << "Preprocessed path is: \n"; From 3f0e747bb10b2d72ebfe20dd1528e92ac26d9ee6 Mon Sep 17 00:00:00 2001 From: Nermin Covic Date: Mon, 22 Sep 2025 12:26:36 +0200 Subject: [PATCH 05/40] Added methods for converting a path to a corresponding trajectory. --- include/planners/trajectory/Splines.h | 11 +- src/planners/trajectory/Splines.cpp | 287 +++++++++++++++++++++++++- 2 files changed, 295 insertions(+), 3 deletions(-) diff --git a/include/planners/trajectory/Splines.h b/include/planners/trajectory/Splines.h index ac655c4a..1e54c4ce 100644 --- a/include/planners/trajectory/Splines.h +++ b/include/planners/trajectory/Splines.h @@ -16,13 +16,18 @@ namespace planning::trajectory class Splines { public: + Splines(const std::shared_ptr &ss_); Splines(const std::shared_ptr &ss_, const std::shared_ptr &q_current_, float max_iter_time_); - bool computeRegular(Eigen::VectorXf ¤t_pos, Eigen::VectorXf ¤t_vel, Eigen::VectorXf ¤t_acc, + bool computeRegular(const Eigen::VectorXf ¤t_pos, const Eigen::VectorXf ¤t_vel, const Eigen::VectorXf ¤t_acc, float t_iter_remain, float t_max, bool non_zero_final_vel); - bool computeSafe(Eigen::VectorXf ¤t_pos, Eigen::VectorXf ¤t_vel, Eigen::VectorXf ¤t_acc, + bool computeSafe(const Eigen::VectorXf ¤t_pos, const Eigen::VectorXf ¤t_vel, const Eigen::VectorXf ¤t_acc, float t_iter_remain, float t_max); + void path2traj_v1(const std::vector> &path); + void path2traj_v2(const std::vector> &path); + void path2traj_v3(const std::vector> &path, bool must_visit); + inline void setCurrentState(const std::shared_ptr q_current_) { q_current = q_current_; } inline void setTargetState(const std::shared_ptr q_target_) { q_target = q_target_; } inline void setMaxRemainingIterTime(float max_remaining_iter_time_) { max_remaining_iter_time = max_remaining_iter_time_; } @@ -30,8 +35,10 @@ namespace planning::trajectory std::shared_ptr spline_current; // Current spline that 'q_current' is following in the current iteration std::shared_ptr spline_next; // Next spline generated from 'q_current' to 'q_target' + std::shared_ptr composite_spline; // Composite spline from the start to a desired target configuration private: + void setParams(); bool checkCollision(std::shared_ptr q_init, float t_iter); float computeDistanceUnderestimation(const std::shared_ptr q, const std::shared_ptr> nearest_points, float delta_t); diff --git a/src/planners/trajectory/Splines.cpp b/src/planners/trajectory/Splines.cpp index 84dccf93..9ba4b913 100644 --- a/src/planners/trajectory/Splines.cpp +++ b/src/planners/trajectory/Splines.cpp @@ -1,5 +1,18 @@ #include "Splines.h" +planning::trajectory::Splines::Splines(const std::shared_ptr &ss_) +{ + ss = ss_; + q_current = nullptr; + max_iter_time = 0; + max_remaining_iter_time = INFINITY; + max_obs_vel = 0; + spline_current = nullptr; + spline_next = nullptr; + composite_spline = nullptr; + setParams(); +} + planning::trajectory::Splines::Splines(const std::shared_ptr &ss_, const std::shared_ptr &q_current_, float max_iter_time_) { @@ -17,7 +30,12 @@ planning::trajectory::Splines::Splines(const std::shared_ptr & spline_current = std::make_shared(ss->robot, q_current->getCoord()); spline_next = spline_current; + composite_spline = nullptr; + setParams(); +} +void planning::trajectory::Splines::setParams() +{ all_robot_vel_same = true; for (size_t i = 1; i < ss->num_dimensions; i++) { @@ -346,7 +364,274 @@ float planning::trajectory::Splines::computeDistanceUnderestimation(const std::s return d_c; } -// This function is just for debugging. You can set a desired path for the file to be saved. +/// @brief A method (v1) to convert a path 'path' to a corresponding trajectory. +/// Converting this path to trajectory (i.e., assigning time instances to these points) will be automatically done by this function. +/// This is done by creating a sequence of quintic splines in a way that all constraints on robot's maximal velocity, +/// acceleration and jerk are surely always satisfied. +/// @param path Path containing all points that robot should visit. +/// @note Be careful since the distance between each two adjacent points from 'path' should not be too long! +/// The robot motion between them is generally not a straight line in C-space. +/// Consider using 'preprocessPath' function from 'base::RealVectorSpace' class before using this function. +void planning::trajectory::Splines::path2traj_v1(const std::vector> &path) +{ + std::vector> subsplines {}; + + spline_current = std::make_shared + ( + ss->robot, + path.front()->getCoord(), + Eigen::VectorXf::Zero(ss->num_dimensions), + Eigen::VectorXf::Zero(ss->num_dimensions) + ); + spline_current->compute(path[1]->getCoord()); + + float t {}, t_max {}; + bool spline_computed { false }; + size_t num {}; + const size_t max_num_iter { 5 }; + + for (size_t i = 2; i < path.size(); i++) + { + spline_computed = false; + num = 0; + t = 0; + t_max = spline_current->getTimeFinal(); + // std::cout << "i: " << i << " ---------------------------\n"; + // std::cout << "t_max: " << t_max << " [s] \n"; + + while (!spline_computed && num++ < max_num_iter) + { + t = (num < max_num_iter) ? + (t + t_max) / 2 : + t_max; // Solution surely exists, and 'spline_computed' will become true. + // std::cout << "t: " << t << " [s] \n"; + + spline_next = std::make_shared + ( + ss->robot, + spline_current->getPosition(t), + spline_current->getVelocity(t), + spline_current->getAcceleration(t) + ); + spline_computed = spline_next->compute(path[i]->getCoord()); + + if (spline_computed && num < max_num_iter) + { + q_current = ss->getNewState(spline_current->getPosition(t)); + ss->computeDistance(q_current); // Required by 'checkCollision' function + if (q_current->getDistance() <= 0 || checkCollision(q_current, 0)) + spline_computed = false; + } + } + + spline_current->setTimeFinal(t); + subsplines.emplace_back(spline_current); + spline_current = spline_next; + } + + subsplines.emplace_back(spline_current); + composite_spline = std::make_shared(subsplines); +} + +/// @brief A method (v2) to convert a path 'path' to a corresponding trajectory. +/// Converting this path to trajectory (i.e., assigning time instances to these points) will be automatically done by this function. +/// This is done by creating a sequence of quintic splines in a way that all constraints on robot's maximal velocity, +/// acceleration and jerk are surely always satisfied. +/// @param path Path containing all points that robot should visit. +/// @note Be careful since the distance between each two adjacent points from 'path' should not be too long! +/// The robot motion between them is generally not a straight line in C-space. +/// Consider using 'preprocessPath' function from 'base::RealVectorSpace' class before using this function. +void planning::trajectory::Splines::path2traj_v2(const std::vector> &path) +{ + std::vector> subsplines {}; + + spline_current = std::make_shared + ( + ss->robot, + path.front()->getCoord(), + Eigen::VectorXf::Zero(ss->num_dimensions), + Eigen::VectorXf::Zero(ss->num_dimensions) + ); + spline_current->compute(path[1]->getCoord()); + + float t {}, t_max {}; + bool spline_computed { false }; + size_t num {}; + const size_t max_num_iter { 5 }; + bool non_zero_final_vel { false }; + + for (size_t i = 2; i < path.size(); i++) + { + spline_computed = false; + num = 0; + t = 0; + t_max = spline_current->getTimeFinal(); + non_zero_final_vel = (i < path.size()-1) ? ss->checkLinearDependency(path[i-1], path[i], path[i+1]) : false; + q_target = path[i]; + // std::cout << "i: " << i << " ---------------------------\n"; + // std::cout << "t_max: " << t_max << " [s] \n"; + + while (!spline_computed && num++ < max_num_iter) + { + t = (num < max_num_iter) ? + (t + t_max) / 2 : + t_max; // Solution surely exists, and 'spline_computed' will become true. + // std::cout << "t: " << t << " [s] \n"; + + q_current = path[i-1]; // Required for the estimation of final vector velocity + spline_computed = computeRegular( + spline_current->getPosition(t), + spline_current->getVelocity(t), + spline_current->getAcceleration(t), + max_remaining_iter_time, + SplinesConfig::MAX_TIME_COMPUTE_REGULAR, + non_zero_final_vel + ); + + if (spline_computed && num < max_num_iter) + { + q_current = ss->getNewState(spline_current->getPosition(t)); + ss->computeDistance(q_current); // Required by 'checkCollision' function + if (q_current->getDistance() <= 0 || checkCollision(q_current, 0)) + spline_computed = false; + } + } + + spline_current->setTimeFinal(t); + subsplines.emplace_back(spline_current); + spline_current = spline_next; + } + + subsplines.emplace_back(spline_current); + composite_spline = std::make_shared(subsplines); +} + +/// @brief A method (v3) to convert a path 'path' to a corresponding trajectory. +/// Converting this path to trajectory (i.e., assigning time instances to these points) will be automatically done by this function. +/// This is done by creating a sequence of quintic splines in a way that all constraints on robot's maximal velocity, +/// acceleration and jerk are surely always satisfied. +/// @param path Path containing all points that robot should visit. +/// @param must_visit Whether path points must be visited. +/// @note Be careful since the distance between each two adjacent points from 'path' should not be too long! +/// The robot motion between them is generally not a straight line in C-space. +/// Consider using 'preprocessPath' function from 'base::RealVectorSpace' class before using this function. +void planning::trajectory::Splines::path2traj_v3(const std::vector> &path, bool must_visit) +{ + std::vector> subsplines(path.size(), nullptr); + bool spline_computed { false }; + size_t num_iter {}; + size_t max_num_iter { 5 }; + float delta_t_max {}; + Eigen::VectorXf q_final_dot_max {}; + Eigen::VectorXf q_final_dot_min {}; + Eigen::VectorXf q_final_dot {}; + Eigen::VectorXf q_final {}; + std::vector vel_coeff(path.size(), 1.0); + const float vel_coeff_const { 0.9 }; + auto time_start = std::chrono::steady_clock::now(); + float max_time { 1.0 }; + bool monotonic { true }; + + subsplines.front() = std::make_shared + ( + ss->robot, + path.front()->getCoord(), + Eigen::VectorXf::Zero(ss->num_dimensions), + Eigen::VectorXf::Zero(ss->num_dimensions) + ); + + for (size_t i = 1; i < path.size(); i++) + { + subsplines[i] = std::make_shared + ( + ss->robot, + subsplines[i-1]->getPosition(subsplines[i-1]->getTimeFinal()), + subsplines[i-1]->getVelocity(subsplines[i-1]->getTimeFinal()), + subsplines[i-1]->getAcceleration(subsplines[i-1]->getTimeFinal()) + ); + + if (i == path.size() - 1) // Final configuration will be reached, thus final velocity and acceleration must be zero! + { + spline_computed = subsplines[i]->compute(path.back()->getCoord()) && subsplines[i]->checkPositionMonotonicity() != 0; + if (!spline_computed) + { + // std::cout << "Spline not computed! \n"; + vel_coeff[--i] *= vel_coeff_const; + --i; + } + else + break; + } + + if (i > 1) + monotonic = (!ss->checkLinearDependency(path[i-2], path[i-1], path[i])) ? false : true; + + if (!must_visit) + q_final = (path[i-1]->getCoord() + path[i]->getCoord()) / 2; + else + q_final = path[i]->getCoord(); + + spline_computed = false; + num_iter = 0; + delta_t_max = ((q_final - path[i-1]->getCoord()).cwiseQuotient(ss->robot->getMaxVel())).cwiseAbs().maxCoeff(); + q_final_dot_max = (q_final - path[i-1]->getCoord()) / delta_t_max; + q_final_dot_min = Eigen::VectorXf::Zero(ss->num_dimensions); + + do + { + q_final_dot = vel_coeff[i] * (q_final_dot_max + q_final_dot_min) / 2; + std::shared_ptr spline_new { + std::make_shared + ( + ss->robot, + subsplines[i-1]->getPosition(subsplines[i-1]->getTimeFinal()), + subsplines[i-1]->getVelocity(subsplines[i-1]->getTimeFinal()), + subsplines[i-1]->getAcceleration(subsplines[i-1]->getTimeFinal()) + ) + }; + + if (spline_new->compute(q_final, q_final_dot) && + ((monotonic && spline_new->checkPositionMonotonicity() != 0) || !monotonic)) + { + *subsplines[i] = *spline_new; + q_final_dot_min = q_final_dot; + spline_computed = true; + } + else + q_final_dot_max = q_final_dot; + } + while (++num_iter < max_num_iter); + + if (!spline_computed) + { + spline_computed = subsplines[i]->compute(q_final) && + ((monotonic && subsplines[i]->checkPositionMonotonicity() != 0) || !monotonic); + if (!spline_computed) + { + // std::cout << "Spline not computed! \n"; + vel_coeff[--i] *= vel_coeff_const; + --i; + } + // else std::cout << "Spline computed with zero final velocity! \n"; + } + + if (std::chrono::duration_cast(std::chrono::steady_clock::now() - time_start).count() * 1e-3 > max_time) + { + spline_computed = false; + break; + } + } + + subsplines.erase(subsplines.begin()); + if (spline_computed) + composite_spline = std::make_shared(subsplines); + else + std::cout << "Could not convert path to trajectory! \n"; + // path2traj(path); // Add path using another method. +} + +// This function is just for debugging. It operates in real-time by logging 'spline_current' and 'spline_next'. +// You can set a desired output path for the file to be saved. void planning::trajectory::Splines::recordTrajectory(bool spline_computed) { std::ofstream output_file {}; From e90e3993e17f128b5a1d8b9e1402c652e2ea925f Mon Sep 17 00:00:00 2001 From: Nermin Covic Date: Mon, 22 Sep 2025 12:27:00 +0200 Subject: [PATCH 06/40] Other corresponding changes... --- src/planners/trajectory/Splines.cpp | 35 ++++++++++++++++------------- 1 file changed, 19 insertions(+), 16 deletions(-) diff --git a/src/planners/trajectory/Splines.cpp b/src/planners/trajectory/Splines.cpp index 9ba4b913..4b150763 100644 --- a/src/planners/trajectory/Splines.cpp +++ b/src/planners/trajectory/Splines.cpp @@ -60,44 +60,47 @@ void planning::trajectory::Splines::setParams() /// @param t_max Maximal available time in [s] for a spline computing /// @param non_zero_final_vel Whether final spline velocity can be non-zero /// @return The success of a spline computation -bool planning::trajectory::Splines::computeRegular(Eigen::VectorXf ¤t_pos, Eigen::VectorXf ¤t_vel, Eigen::VectorXf ¤t_acc, - float t_iter_remain, float t_max, bool non_zero_final_vel) +bool planning::trajectory::Splines::computeRegular(const Eigen::VectorXf ¤t_pos, const Eigen::VectorXf ¤t_vel, + const Eigen::VectorXf ¤t_acc, float t_iter_remain, float t_max, bool non_zero_final_vel) { std::chrono::steady_clock::time_point time_start_ { std::chrono::steady_clock::now() }; spline_next = std::make_shared(ss->robot, current_pos, current_vel, current_acc); std::shared_ptr spline_next_new { std::make_shared(ss->robot, current_pos, current_vel, current_acc) }; bool spline_computed { false }; - Eigen::VectorXf new_current_pos {}; // Possible current position at the end of iteration if (non_zero_final_vel) { float num_iter { 0 }; - float delta_t_max { ((q_target->getCoord() - current_pos).cwiseQuotient(ss->robot->getMaxVel())).cwiseAbs().maxCoeff() }; - Eigen::VectorXf q_final_dot_max { (q_target->getCoord() - current_pos) / delta_t_max }; + float delta_t_max { ((q_target->getCoord() - q_current->getCoord()).cwiseQuotient(ss->robot->getMaxVel())).cwiseAbs().maxCoeff() }; + Eigen::VectorXf q_final_dot_max { (q_target->getCoord() - q_current->getCoord()) / delta_t_max }; Eigen::VectorXf q_final_dot_min { Eigen::VectorXf::Zero(ss->num_dimensions) }; - Eigen::VectorXf q_final_dot {}; + Eigen::VectorXf q_final_dot { q_final_dot_max }; while (num_iter++ < max_num_iter_spline_regular && std::chrono::duration_cast(std::chrono::steady_clock::now() - time_start_).count() < t_max * 1e6) { - q_final_dot = (q_final_dot_max + q_final_dot_min) / 2; - // std::cout << "Num. iter. " << num_iter << "\t q_final_dot: " << q_final_dot.transpose() << "\n"; - if (spline_next_new->compute(q_target->getCoord(), q_final_dot)) { *spline_next = *spline_next_new; q_final_dot_min = q_final_dot; spline_computed = true; + if (num_iter == 1) + break; } else q_final_dot_max = q_final_dot; - } - new_current_pos = spline_computed ? - spline_next->getPosition(t_iter_remain) : - spline_current->getPosition(spline_current->getTimeCurrent() + t_iter_remain); + q_final_dot = (q_final_dot_max + q_final_dot_min) / 2; + // std::cout << "Num. iter. " << num_iter << "\t q_final_dot: " << q_final_dot.transpose() << "\n"; + } } + + Eigen::VectorXf new_current_pos { // Possible current position at the end of iteration + spline_computed ? + spline_next->getPosition(t_iter_remain) : + spline_current->getPosition(spline_current->getTimeCurrent() + t_iter_remain) + }; // If spline was not computed or robot is getting away from 'new_current_pos' if (!spline_computed || @@ -123,8 +126,8 @@ bool planning::trajectory::Splines::computeRegular(Eigen::VectorXf ¤t_pos, /// @param t_iter_remain Remaining time in [s] in the current iteration /// @param t_max Maximal available time in [s] for a spline computing /// @return The success of a spline computation -bool planning::trajectory::Splines::computeSafe(Eigen::VectorXf ¤t_pos, Eigen::VectorXf ¤t_vel, Eigen::VectorXf ¤t_acc, - float t_iter_remain, float t_max) +bool planning::trajectory::Splines::computeSafe(const Eigen::VectorXf ¤t_pos, const Eigen::VectorXf ¤t_vel, + const Eigen::VectorXf ¤t_acc, float t_iter_remain, float t_max) { std::chrono::steady_clock::time_point time_start_ { std::chrono::steady_clock::now() }; std::shared_ptr spline_next_new @@ -276,6 +279,7 @@ bool planning::trajectory::Splines::checkCollision(std::shared_ptr t_iter += delta_t; rho_obs = max_obs_vel * (t_iter - t_init); delta_q = (q_final->getCoord() - q_init->getCoord()).cwiseAbs(); + ss->robot->computeEnclosingRadii(q_init); for (size_t i = 0; i < ss->robot->getNumDOFs(); i++) { rho_robot = q_init->getEnclosingRadii()->col(i+1).dot(delta_q); @@ -284,7 +288,6 @@ bool planning::trajectory::Splines::checkCollision(std::shared_ptr // std::cout << "********** Possible collision ********** \n"; q_init = q_final; computeDistanceUnderestimation(q_init, q_current->getNearestPoints(), t_iter); - ss->robot->computeEnclosingRadii(q_init); t_init = t_iter; if (q_init->getDistance() <= 0) From de284c7afc0f06b803e15ff48950a4b184be1b42 Mon Sep 17 00:00:00 2001 From: Nermin Covic Date: Mon, 22 Sep 2025 12:27:29 +0200 Subject: [PATCH 07/40] Created a file for testing 'path2traj' method. --- apps/CMakeLists.txt | 29 +++++--- apps/test_trajectory.cpp | 145 +++++++++++++++++++++++++++++++++++++++ 2 files changed, 163 insertions(+), 11 deletions(-) create mode 100644 apps/test_trajectory.cpp diff --git a/apps/CMakeLists.txt b/apps/CMakeLists.txt index 58e51a55..0d3b628d 100644 --- a/apps/CMakeLists.txt +++ b/apps/CMakeLists.txt @@ -59,6 +59,21 @@ target_compile_features(test_rgbmtstar PRIVATE cxx_std_17) target_link_libraries(test_rgbmtstar PUBLIC rpmpl_library ${PROJECT_LIBRARIES}) target_include_directories(test_rgbmtstar PUBLIC ${PROJECT_SOURCE_DIR}/apps) +add_executable(test_rrtx test_rrtx.cpp) +target_compile_features(test_rrtx PRIVATE cxx_std_17) +target_link_libraries(test_rrtx PUBLIC rpmpl_library ${PROJECT_LIBRARIES}) +target_include_directories(test_rrtx PUBLIC ${PROJECT_SOURCE_DIR}/apps) + +add_executable(test_rrtx_random_obstacles test_rrtx_random_obstacles.cpp) +target_compile_features(test_rrtx_random_obstacles PRIVATE cxx_std_17) +target_link_libraries(test_rrtx_random_obstacles PUBLIC rpmpl_library ${PROJECT_LIBRARIES}) +target_include_directories(test_rrtx_random_obstacles PUBLIC ${PROJECT_SOURCE_DIR}/apps) + +add_executable(test_trajectory test_trajectory.cpp) +target_compile_features(test_trajectory PRIVATE cxx_std_17) +target_link_libraries(test_trajectory PUBLIC rpmpl_library ${PROJECT_LIBRARIES}) +target_include_directories(test_trajectory PUBLIC ${PROJECT_SOURCE_DIR}/apps) + add_executable(generate_random_scenarios generate_random_scenarios.cpp) target_compile_features(generate_random_scenarios PRIVATE cxx_std_17) target_link_libraries(generate_random_scenarios PUBLIC rpmpl_library ${PROJECT_LIBRARIES}) @@ -69,15 +84,6 @@ target_compile_features(generate_dataset PRIVATE cxx_std_17) target_link_libraries(generate_dataset PUBLIC rpmpl_library ${PROJECT_LIBRARIES}) target_include_directories(generate_dataset PUBLIC ${PROJECT_SOURCE_DIR}/apps) -add_executable(test_rrtx test_rrtx.cpp) -target_compile_features(test_rrtx PRIVATE cxx_std_17) -target_link_libraries(test_rrtx PUBLIC rpmpl_library ${PROJECT_LIBRARIES}) -target_include_directories(test_rrtx PUBLIC ${PROJECT_SOURCE_DIR}/apps) - -add_executable(test_rrtx_random_obstacles test_rrtx_random_obstacles.cpp) -target_compile_features(test_rrtx_random_obstacles PRIVATE cxx_std_17) -target_link_libraries(test_rrtx_random_obstacles PUBLIC rpmpl_library ${PROJECT_LIBRARIES}) -target_include_directories(test_rrtx_random_obstacles PUBLIC ${PROJECT_SOURCE_DIR}/apps) install(TARGETS test_nanoflann @@ -93,8 +99,9 @@ install(TARGETS test_drgbt test_drgbt_random_obstacles test_rgbmtstar - generate_random_scenarios - generate_dataset test_rrtx test_rrtx_random_obstacles + test_trajectory + generate_random_scenarios + generate_dataset DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/bin) \ No newline at end of file diff --git a/apps/test_trajectory.cpp b/apps/test_trajectory.cpp new file mode 100644 index 00000000..01c4b472 --- /dev/null +++ b/apps/test_trajectory.cpp @@ -0,0 +1,145 @@ +#include "RGBTConnect.h" +#include "ConfigurationReader.h" +#include "CommonFunctions.h" +#include "Splines.h" + +int main(int argc, char **argv) +{ + std::string scenario_file_path + { + "/data/planar_2dof/scenario_test/scenario_test.yaml" + // "/data/planar_2dof/scenario1/scenario1.yaml" + // "/data/planar_2dof/scenario2/scenario2.yaml" + // "/data/planar_2dof/scenario3/scenario3.yaml" + + // "/data/planar_10dof/scenario_test/scenario_test.yaml" + // "/data/planar_10dof/scenario1/scenario1.yaml" + // "/data/planar_10dof/scenario2/scenario2.yaml" + + // "/data/xarm6/scenario_test/scenario_test.yaml" + // "/data/xarm6/scenario1/scenario1.yaml" + // "/data/xarm6/scenario2/scenario2.yaml" + // "/data/xarm6/scenario3/scenario3.yaml" + }; + + initGoogleLogging(argv); + int clp = commandLineParser(argc, argv, scenario_file_path); + if (clp != 0) return clp; + + const std::string project_path { getProjectPath() }; + const std::string directory_path { project_path + scenario_file_path.substr(0, scenario_file_path.find_last_of("/\\")) + "/RGBTConnect_data" }; + std::filesystem::create_directory(directory_path); + ConfigurationReader::initConfiguration(project_path); + YAML::Node node { YAML::LoadFile(project_path + scenario_file_path) }; + + const size_t max_num_tests { node["testing"]["max_num"].as() }; + const size_t num_random_obstacles { node["random_obstacles"]["num"].as() }; + Eigen::Vector3f obs_dim {}; + for (size_t i = 0; i < 3; i++) + obs_dim(i) = node["random_obstacles"]["dim"][i].as(); + + scenario::Scenario scenario(scenario_file_path, project_path); + std::shared_ptr ss { scenario.getStateSpace() }; + std::shared_ptr q_start { scenario.getStart() }; + std::shared_ptr q_goal { scenario.getGoal() }; + std::shared_ptr env { scenario.getEnvironment() }; + std::unique_ptr planner { nullptr }; + std::shared_ptr trajectory { nullptr }; + + bool result { false }; + size_t num_obs { env->getNumObjects() }; + while (!result && num_random_obstacles > 0) + { + env->removeObjects(num_obs); + initRandomObstacles(num_random_obstacles, obs_dim, scenario); + planner = std::make_unique(ss, q_start, q_goal); + result = planner->solve(); + LOG(INFO) << "A path to the goal can " << (result ? "" : "not ") << "be found!"; + } + + LOG(INFO) << "Using scenario: " << project_path + scenario_file_path; + LOG(INFO) << "Environment parts: " << env->getNumObjects(); + LOG(INFO) << "Number of DOFs: " << ss->num_dimensions; + LOG(INFO) << "State space type: " << ss->getStateSpaceType(); + LOG(INFO) << "Start: " << q_start; + LOG(INFO) << "Goal: " << q_goal; + + size_t num_test { 0 }; + size_t num_success { 0 }; + std::vector comp_times1 {}, comp_times2 {}; + std::vector final_times1 {}, final_times2 {}; + const float delta_time { 0.005 }; + + while (num_test++ < max_num_tests) + { + try + { + std::ofstream output_file {}; + output_file.open(directory_path + "/trajectory" + std::to_string(num_test) + ".log", std::ofstream::out); + + LOG(INFO) << "Test number " << num_test << " of " << max_num_tests; + planner = std::make_unique(ss, q_start, q_goal); + result = planner->solve(); + + LOG(INFO) << planner->getPlannerType() << " planning finished with " << (result ? "SUCCESS!" : "FAILURE!"); + LOG(INFO) << "Number of states: " << planner->getPlannerInfo()->getNumStates(); + LOG(INFO) << "Number of iterations: " << planner->getPlannerInfo()->getNumIterations(); + LOG(INFO) << "Planning time: " << planner->getPlannerInfo()->getPlanningTime() << " [s]"; + + if (result) + { + num_success++; + // LOG(INFO) << "Found path: "; + // std::vector> path { planner->getPath() }; + // for (size_t i = 0; i < path.size(); i++) + // std::cout << i << ": " << path.at(i)->getCoord().transpose() << std::endl; + + std::vector> new_path { }; + ss->preprocessPath(planner->getPath(), new_path, 10*RRTConnectConfig::EPS_STEP); + + output_file << "Path: \n"; + for (size_t i = 0; i < new_path.size(); i++) + output_file << new_path.at(i)->getCoord().transpose() << "\n"; + output_file << "--------------------------------------------------------------------\n"; + + trajectory = std::make_shared(ss); + auto time1 = std::chrono::steady_clock::now(); + trajectory->path2traj_v1(new_path); + comp_times1.emplace_back(std::chrono::duration_cast(std::chrono::steady_clock::now() - time1).count() * 1e-3); + final_times1.emplace_back(trajectory->composite_spline->getTimeFinal()); + + output_file << "Trajectory 1 (position): \n"; + for (float t = 0; t < trajectory->composite_spline->getTimeFinal(); t += delta_time) + output_file << trajectory->composite_spline->getPosition(t).transpose() << "\n"; + output_file << "--------------------------------------------------------------------\n"; + + auto time2 = std::chrono::steady_clock::now(); + trajectory->path2traj_v2(new_path); + comp_times2.emplace_back(std::chrono::duration_cast(std::chrono::steady_clock::now() - time2).count() * 1e-3); + final_times2.emplace_back(trajectory->composite_spline->getTimeFinal()); + + output_file << "Trajectory 2 (position): \n"; + for (float t = 0; t < trajectory->composite_spline->getTimeFinal(); t += delta_time) + output_file << trajectory->composite_spline->getPosition(t).transpose() << "\n"; + output_file << "--------------------------------------------------------------------\n"; + output_file.close(); + } + + // LOG(INFO) << "Planner data is saved at: " << directory_path + "/test" + std::to_string(num_test) + ".log"; + // planner->outputPlannerData(directory_path + "/test" + std::to_string(num_test) + ".log"); + LOG(INFO) << "\n--------------------------------------------------------------------\n\n"; + } + catch (std::exception &e) + { + LOG(ERROR) << e.what(); + } + } + LOG(INFO) << "Success rate: " << (float) num_success / max_num_tests * 100 << " [%]"; + LOG(INFO) << "Comp. times1: " << getMean(comp_times1) << " [ms]"; + LOG(INFO) << "Comp. times2: " << getMean(comp_times2) << " [ms]"; + LOG(INFO) << "Final times1: " << getMean(final_times1) << " [s]"; + LOG(INFO) << "Final times2: " << getMean(final_times2) << " [s]"; + + google::ShutDownCommandLineFlags(); + return 0; +} From 2509b6374c2b3a4a07069f9ed4fe90a7800241d1 Mon Sep 17 00:00:00 2001 From: Nermin Covic Date: Tue, 23 Sep 2025 17:36:32 +0200 Subject: [PATCH 08/40] Renamed 'Splines' class to 'Trajectory' class --- apps/test_trajectory.cpp | 4 ++-- include/planners/drbt/DRGBT.h | 2 +- include/planners/rrtx/RRTx.h | 2 +- include/planners/trajectory/MotionValidity.h | 4 ++-- include/planners/trajectory/Splines.h | 12 +++++------ include/planners/trajectory/UpdatingState.h | 4 ++-- src/planners/drbt/DRGBT.cpp | 2 +- src/planners/rrtx/RRTx.cpp | 2 +- src/planners/trajectory/Splines.cpp | 22 ++++++++++---------- 9 files changed, 27 insertions(+), 27 deletions(-) diff --git a/apps/test_trajectory.cpp b/apps/test_trajectory.cpp index 01c4b472..02acffbb 100644 --- a/apps/test_trajectory.cpp +++ b/apps/test_trajectory.cpp @@ -44,7 +44,7 @@ int main(int argc, char **argv) std::shared_ptr q_goal { scenario.getGoal() }; std::shared_ptr env { scenario.getEnvironment() }; std::unique_ptr planner { nullptr }; - std::shared_ptr trajectory { nullptr }; + std::shared_ptr trajectory { nullptr }; bool result { false }; size_t num_obs { env->getNumObjects() }; @@ -102,7 +102,7 @@ int main(int argc, char **argv) output_file << new_path.at(i)->getCoord().transpose() << "\n"; output_file << "--------------------------------------------------------------------\n"; - trajectory = std::make_shared(ss); + trajectory = std::make_shared(ss); auto time1 = std::chrono::steady_clock::now(); trajectory->path2traj_v1(new_path); comp_times1.emplace_back(std::chrono::duration_cast(std::chrono::steady_clock::now() - time1).count() * 1e-3); diff --git a/include/planners/drbt/DRGBT.h b/include/planners/drbt/DRGBT.h index 24f70c23..c28fe278 100644 --- a/include/planners/drbt/DRGBT.h +++ b/include/planners/drbt/DRGBT.h @@ -62,7 +62,7 @@ namespace planning::drbt std::vector> predefined_path; // The predefined path that is being followed size_t num_lateral_states; // Number of lateral states float max_edge_length; // Maximal edge length when acquiring a new predefined path - std::shared_ptr splines; // Everything related to splines + std::shared_ptr splines; // Everything related to splines std::shared_ptr updating_state; // Class for updating current state std::shared_ptr motion_validity; // Class for checking validity of motion std::vector> visited_states; diff --git a/include/planners/rrtx/RRTx.h b/include/planners/rrtx/RRTx.h index 712e24b3..79a76e8c 100644 --- a/include/planners/rrtx/RRTx.h +++ b/include/planners/rrtx/RRTx.h @@ -87,7 +87,7 @@ namespace planning::rrtx // Path from start to goal std::vector> path_current; - std::shared_ptr splines; // Everything related to splines + std::shared_ptr splines; // Everything related to splines std::shared_ptr updating_state; // Class for updating current state std::shared_ptr motion_validity; // Class for checking validity of motion diff --git a/include/planners/trajectory/MotionValidity.h b/include/planners/trajectory/MotionValidity.h index bbb99327..b3bdb0bb 100644 --- a/include/planners/trajectory/MotionValidity.h +++ b/include/planners/trajectory/MotionValidity.h @@ -19,7 +19,7 @@ namespace planning::trajectory ~MotionValidity() {} bool check(const std::shared_ptr &q_previous, const std::shared_ptr &q_current); - inline void setSplines(const std::shared_ptr &splines_) { splines = splines_; } + inline void setSplines(const std::shared_ptr &splines_) { splines = splines_; } private: bool check_v1(const std::shared_ptr &q_previous, const std::shared_ptr &q_current); @@ -30,7 +30,7 @@ namespace planning::trajectory float resolution_coll_check; std::vector>* path; float max_iter_time; - std::shared_ptr splines; + std::shared_ptr splines; size_t num_checks; // Maximal number of validity checks when robot moves from previous to current configuration, // while the obstacles are moving simultaneously. }; diff --git a/include/planners/trajectory/Splines.h b/include/planners/trajectory/Splines.h index 1e54c4ce..ba5154a6 100644 --- a/include/planners/trajectory/Splines.h +++ b/include/planners/trajectory/Splines.h @@ -2,8 +2,8 @@ // Created by nermin on 20.07.24. // -#ifndef RPMPL_SPLINES_H -#define RPMPL_SPLINES_H +#ifndef RPMPL_TRAJECTORY_H +#define RPMPL_TRAJECTORY_H #include "StateSpace.h" #include "Spline4.h" @@ -13,11 +13,11 @@ namespace planning::trajectory { - class Splines + class Trajectory { public: - Splines(const std::shared_ptr &ss_); - Splines(const std::shared_ptr &ss_, const std::shared_ptr &q_current_, float max_iter_time_); + Trajectory(const std::shared_ptr &ss_); + Trajectory(const std::shared_ptr &ss_, const std::shared_ptr &q_current_, float max_iter_time_); bool computeRegular(const Eigen::VectorXf ¤t_pos, const Eigen::VectorXf ¤t_vel, const Eigen::VectorXf ¤t_acc, float t_iter_remain, float t_max, bool non_zero_final_vel); @@ -54,4 +54,4 @@ namespace planning::trajectory }; } -#endif //RPMPL_SPLINES_H \ No newline at end of file +#endif //RPMPL_TRAJECTORY_H \ No newline at end of file diff --git a/include/planners/trajectory/UpdatingState.h b/include/planners/trajectory/UpdatingState.h index 354ecc0b..5da3ce8a 100644 --- a/include/planners/trajectory/UpdatingState.h +++ b/include/planners/trajectory/UpdatingState.h @@ -27,7 +27,7 @@ namespace planning::trajectory const std::shared_ptr q_next_, base::State::Status &status); void update(std::shared_ptr &q_previous, std::shared_ptr &q_current, const std::shared_ptr q_next_, const std::shared_ptr q_next_reached_, base::State::Status &status); - inline void setSplines(const std::shared_ptr &splines_) { splines = splines_; } + inline void setSplines(const std::shared_ptr &splines_) { splines = splines_; } inline void setGuaranteedSafeMotion(bool guaranteed_safe_motion_) { guaranteed_safe_motion = guaranteed_safe_motion_; } inline void setNonZeroFinalVel(bool non_zero_final_vel_) { non_zero_final_vel = non_zero_final_vel_; } inline void setMaxRemainingIterTime(float max_remaining_iter_time_) { max_remaining_iter_time = max_remaining_iter_time_; } @@ -51,7 +51,7 @@ namespace planning::trajectory float max_iter_time; // Maximal iteration time in [s] // Additional info (not mandatory to be set): - std::shared_ptr splines; + std::shared_ptr splines; bool all_robot_vel_same; // Whether all joint velocities are the same bool guaranteed_safe_motion; // Whether robot motion is surely safe for environment bool non_zero_final_vel; // Whether final spline velocity can be non-zero (available only when 'guaranteed_safe_motion' is false) diff --git a/src/planners/drbt/DRGBT.cpp b/src/planners/drbt/DRGBT.cpp index 2551d589..f48d4cc4 100644 --- a/src/planners/drbt/DRGBT.cpp +++ b/src/planners/drbt/DRGBT.cpp @@ -43,7 +43,7 @@ planning::drbt::DRGBT::DRGBT(const std::shared_ptr ss_, const splines = nullptr; if (DRGBTConfig::TRAJECTORY_INTERPOLATION == planning::TrajectoryInterpolation::Spline) { - splines = std::make_shared(ss, q_current, DRGBTConfig::MAX_ITER_TIME); + splines = std::make_shared(ss, q_current, DRGBTConfig::MAX_ITER_TIME); splines->setMaxRemainingIterTime(DRGBTConfig::MAX_ITER_TIME - DRGBTConfig::MAX_TIME_TASK1); updating_state->setSplines(splines); motion_validity->setSplines(splines); diff --git a/src/planners/rrtx/RRTx.cpp b/src/planners/rrtx/RRTx.cpp index 08651014..466344dc 100644 --- a/src/planners/rrtx/RRTx.cpp +++ b/src/planners/rrtx/RRTx.cpp @@ -47,7 +47,7 @@ planning::rrtx::RRTx::RRTx(const std::shared_ptr ss_, const st splines = nullptr; if (RRTxConfig::TRAJECTORY_INTERPOLATION == planning::TrajectoryInterpolation::Spline) { - splines = std::make_shared(ss, q_current, RRTxConfig::MAX_ITER_TIME); + splines = std::make_shared(ss, q_current, RRTxConfig::MAX_ITER_TIME); updating_state->setSplines(splines); motion_validity->setSplines(splines); } diff --git a/src/planners/trajectory/Splines.cpp b/src/planners/trajectory/Splines.cpp index 4b150763..d4ae0bd8 100644 --- a/src/planners/trajectory/Splines.cpp +++ b/src/planners/trajectory/Splines.cpp @@ -1,6 +1,6 @@ #include "Splines.h" -planning::trajectory::Splines::Splines(const std::shared_ptr &ss_) +planning::trajectory::Trajectory::Trajectory(const std::shared_ptr &ss_) { ss = ss_; q_current = nullptr; @@ -13,7 +13,7 @@ planning::trajectory::Splines::Splines(const std::shared_ptr & setParams(); } -planning::trajectory::Splines::Splines(const std::shared_ptr &ss_, +planning::trajectory::Trajectory::Trajectory(const std::shared_ptr &ss_, const std::shared_ptr &q_current_, float max_iter_time_) { ss = ss_; @@ -34,7 +34,7 @@ planning::trajectory::Splines::Splines(const std::shared_ptr & setParams(); } -void planning::trajectory::Splines::setParams() +void planning::trajectory::Trajectory::setParams() { all_robot_vel_same = true; for (size_t i = 1; i < ss->num_dimensions; i++) @@ -60,7 +60,7 @@ void planning::trajectory::Splines::setParams() /// @param t_max Maximal available time in [s] for a spline computing /// @param non_zero_final_vel Whether final spline velocity can be non-zero /// @return The success of a spline computation -bool planning::trajectory::Splines::computeRegular(const Eigen::VectorXf ¤t_pos, const Eigen::VectorXf ¤t_vel, +bool planning::trajectory::Trajectory::computeRegular(const Eigen::VectorXf ¤t_pos, const Eigen::VectorXf ¤t_vel, const Eigen::VectorXf ¤t_acc, float t_iter_remain, float t_max, bool non_zero_final_vel) { std::chrono::steady_clock::time_point time_start_ { std::chrono::steady_clock::now() }; @@ -126,7 +126,7 @@ bool planning::trajectory::Splines::computeRegular(const Eigen::VectorXf ¤ /// @param t_iter_remain Remaining time in [s] in the current iteration /// @param t_max Maximal available time in [s] for a spline computing /// @return The success of a spline computation -bool planning::trajectory::Splines::computeSafe(const Eigen::VectorXf ¤t_pos, const Eigen::VectorXf ¤t_vel, +bool planning::trajectory::Trajectory::computeSafe(const Eigen::VectorXf ¤t_pos, const Eigen::VectorXf ¤t_vel, const Eigen::VectorXf ¤t_acc, float t_iter_remain, float t_max) { std::chrono::steady_clock::time_point time_start_ { std::chrono::steady_clock::now() }; @@ -257,7 +257,7 @@ bool planning::trajectory::Splines::computeSafe(const Eigen::VectorXf ¤t_p /// @param t_iter Elapsed time from the beginning of iteration to a time instance when 'spline' is starting. /// @return True if the collision occurs. False if not. /// @note 'q_init' must have a distance-to-obstacles or its underestimation! -bool planning::trajectory::Splines::checkCollision(std::shared_ptr q_init, float t_iter) +bool planning::trajectory::Trajectory::checkCollision(std::shared_ptr q_init, float t_iter) { // std::chrono::steady_clock::time_point time_start_ { std::chrono::steady_clock::now() }; float delta_t { SplinesConfig::TIME_STEP }; @@ -317,7 +317,7 @@ bool planning::trajectory::Splines::checkCollision(std::shared_ptr /// @return Underestimation of distance-to-obstacles. /// Note that if 'd_c' is negative, it means that one or more robot's links are penetrating through the plane, /// or they are located on the other side of the plane. -float planning::trajectory::Splines::computeDistanceUnderestimation(const std::shared_ptr q, +float planning::trajectory::Trajectory::computeDistanceUnderestimation(const std::shared_ptr q, const std::shared_ptr> nearest_points, float delta_t) { float d_c_temp {}; @@ -375,7 +375,7 @@ float planning::trajectory::Splines::computeDistanceUnderestimation(const std::s /// @note Be careful since the distance between each two adjacent points from 'path' should not be too long! /// The robot motion between them is generally not a straight line in C-space. /// Consider using 'preprocessPath' function from 'base::RealVectorSpace' class before using this function. -void planning::trajectory::Splines::path2traj_v1(const std::vector> &path) +void planning::trajectory::Trajectory::path2traj_v1(const std::vector> &path) { std::vector> subsplines {}; @@ -444,7 +444,7 @@ void planning::trajectory::Splines::path2traj_v1(const std::vector> &path) +void planning::trajectory::Trajectory::path2traj_v2(const std::vector> &path) { std::vector> subsplines {}; @@ -518,7 +518,7 @@ void planning::trajectory::Splines::path2traj_v2(const std::vector> &path, bool must_visit) +void planning::trajectory::Trajectory::path2traj_v3(const std::vector> &path, bool must_visit) { std::vector> subsplines(path.size(), nullptr); bool spline_computed { false }; @@ -635,7 +635,7 @@ void planning::trajectory::Splines::path2traj_v3(const std::vector Date: Tue, 23 Sep 2025 17:38:04 +0200 Subject: [PATCH 09/40] Included 'ros_reflexxes' library within 'RPMPLv2'. --- .gitmodules | 4 ++++ CMakeLists.txt | 8 ++++++-- src/CMakeLists.txt | 1 + 3 files changed, 11 insertions(+), 2 deletions(-) diff --git a/.gitmodules b/.gitmodules index e69de29b..70747955 100644 --- a/.gitmodules +++ b/.gitmodules @@ -0,0 +1,4 @@ +[submodule "external/ros_reflexxes"] +path = external/ros_reflexxes +url = https://github.com/PickNikRobotics/ros_reflexxes.git +branch = foxy \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index ad68cb31..a73200a9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -20,7 +20,7 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON) message("------------------ Using compiler: ${CMAKE_CXX_COMPILER} ------------------") if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Werror) + add_compile_options(-Wall -Wextra -Wpedantic) #-Werror) endif() # Only do these if this is the main project, and not if it is included through add_subdirectory @@ -62,7 +62,10 @@ find_package(yaml-cpp REQUIRED) find_package(fcl 0.7 REQUIRED) find_package(nanoflann REQUIRED) -set(PROJECT_LIBRARIES gtest glog gflags nanoflann::nanoflann kdl_parser orocos-kdl fcl ccd yaml-cpp) +add_subdirectory(external/ros_reflexxes/libreflexxestype2) +include_directories(external/ros_reflexxes/libreflexxestype2/include) + +set(PROJECT_LIBRARIES gtest glog gflags nanoflann::nanoflann kdl_parser orocos-kdl fcl ccd yaml-cpp reflexxes) set(MAIN_PROJECT_BUILD_DIR ${CMAKE_CURRENT_SOURCE_DIR}/build) @@ -77,6 +80,7 @@ add_subdirectory(src) # The executable code is here add_subdirectory(apps) +add_subdirectory(external/ros_reflexxes/libreflexxestype2/src/RMLPositionSampleApplications) # Testing only available if this is the main app # Emergency override MODERN_CMAKE_BUILD_TESTING provided as well diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 2f2b85c8..0e3d3a33 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -18,6 +18,7 @@ set(INCLUDE_DIRS ${PROJECT_SOURCE_DIR}/include/scenario ${PROJECT_SOURCE_DIR}/include/configurations ${PROJECT_SOURCE_DIR}/include/benchmark + ${PROJECT_SOURCE_DIR}/external/ros_reflexxes/libreflexxestype2/include ) # Optionally glob, but only for CMake 3.12 or later: From edd1e33aba4a0db3c5542b3269d267c6a14a8c0a Mon Sep 17 00:00:00 2001 From: Nermin Covic Date: Tue, 23 Sep 2025 17:53:16 +0200 Subject: [PATCH 10/40] Renaming 'splines' to 'traj' --- include/planners/drbt/DRGBT.h | 2 +- include/planners/rrtx/RRTx.h | 2 +- include/planners/trajectory/MotionValidity.h | 4 +- include/planners/trajectory/UpdatingState.h | 6 +- src/planners/drbt/DRGBT.cpp | 10 ++-- src/planners/rrtx/RRTx.cpp | 8 +-- src/planners/trajectory/MotionValidity.cpp | 30 +++++----- src/planners/trajectory/UpdatingState.cpp | 58 ++++++++++---------- 8 files changed, 60 insertions(+), 60 deletions(-) diff --git a/include/planners/drbt/DRGBT.h b/include/planners/drbt/DRGBT.h index c28fe278..298691e2 100644 --- a/include/planners/drbt/DRGBT.h +++ b/include/planners/drbt/DRGBT.h @@ -62,7 +62,7 @@ namespace planning::drbt std::vector> predefined_path; // The predefined path that is being followed size_t num_lateral_states; // Number of lateral states float max_edge_length; // Maximal edge length when acquiring a new predefined path - std::shared_ptr splines; // Everything related to splines + std::shared_ptr traj; // Everything related to trajectory std::shared_ptr updating_state; // Class for updating current state std::shared_ptr motion_validity; // Class for checking validity of motion std::vector> visited_states; diff --git a/include/planners/rrtx/RRTx.h b/include/planners/rrtx/RRTx.h index 79a76e8c..154c2a2f 100644 --- a/include/planners/rrtx/RRTx.h +++ b/include/planners/rrtx/RRTx.h @@ -87,7 +87,7 @@ namespace planning::rrtx // Path from start to goal std::vector> path_current; - std::shared_ptr splines; // Everything related to splines + std::shared_ptr traj; // Everything related to trajectory std::shared_ptr updating_state; // Class for updating current state std::shared_ptr motion_validity; // Class for checking validity of motion diff --git a/include/planners/trajectory/MotionValidity.h b/include/planners/trajectory/MotionValidity.h index b3bdb0bb..19b5722e 100644 --- a/include/planners/trajectory/MotionValidity.h +++ b/include/planners/trajectory/MotionValidity.h @@ -19,7 +19,7 @@ namespace planning::trajectory ~MotionValidity() {} bool check(const std::shared_ptr &q_previous, const std::shared_ptr &q_current); - inline void setSplines(const std::shared_ptr &splines_) { splines = splines_; } + inline void setTraj(const std::shared_ptr &traj_) { traj = traj_; } private: bool check_v1(const std::shared_ptr &q_previous, const std::shared_ptr &q_current); @@ -30,7 +30,7 @@ namespace planning::trajectory float resolution_coll_check; std::vector>* path; float max_iter_time; - std::shared_ptr splines; + std::shared_ptr traj; size_t num_checks; // Maximal number of validity checks when robot moves from previous to current configuration, // while the obstacles are moving simultaneously. }; diff --git a/include/planners/trajectory/UpdatingState.h b/include/planners/trajectory/UpdatingState.h index 5da3ce8a..7803298c 100644 --- a/include/planners/trajectory/UpdatingState.h +++ b/include/planners/trajectory/UpdatingState.h @@ -27,7 +27,7 @@ namespace planning::trajectory const std::shared_ptr q_next_, base::State::Status &status); void update(std::shared_ptr &q_previous, std::shared_ptr &q_current, const std::shared_ptr q_next_, const std::shared_ptr q_next_reached_, base::State::Status &status); - inline void setSplines(const std::shared_ptr &splines_) { splines = splines_; } + inline void setTraj(const std::shared_ptr &traj_) { traj = traj_; } inline void setGuaranteedSafeMotion(bool guaranteed_safe_motion_) { guaranteed_safe_motion = guaranteed_safe_motion_; } inline void setNonZeroFinalVel(bool non_zero_final_vel_) { non_zero_final_vel = non_zero_final_vel_; } inline void setMaxRemainingIterTime(float max_remaining_iter_time_) { max_remaining_iter_time = max_remaining_iter_time_; } @@ -51,7 +51,7 @@ namespace planning::trajectory float max_iter_time; // Maximal iteration time in [s] // Additional info (not mandatory to be set): - std::shared_ptr splines; + std::shared_ptr traj; bool all_robot_vel_same; // Whether all joint velocities are the same bool guaranteed_safe_motion; // Whether robot motion is surely safe for environment bool non_zero_final_vel; // Whether final spline velocity can be non-zero (available only when 'guaranteed_safe_motion' is false) @@ -61,7 +61,7 @@ namespace planning::trajectory // If false, elapsed time will be computed (default: false). // It should always be false when simulation pacing is used, since then a time measuring will not be correct! - float remaining_time; // Return value of 'update_v2' function. Remaining time in [s] after which the new spline 'splines->spline_next' + float remaining_time; // Return value of 'update_v2' function. Remaining time in [s] after which the new spline 'traj->spline_next' // will become active (if 'planning::TrajectoryInterpolation::Spline' is used). std::shared_ptr q_next; diff --git a/src/planners/drbt/DRGBT.cpp b/src/planners/drbt/DRGBT.cpp index f48d4cc4..eb7ad7e8 100644 --- a/src/planners/drbt/DRGBT.cpp +++ b/src/planners/drbt/DRGBT.cpp @@ -40,13 +40,13 @@ planning::drbt::DRGBT::DRGBT(const std::shared_ptr ss_, const motion_validity = std::make_shared (ss, DRGBTConfig::TRAJECTORY_INTERPOLATION, DRGBTConfig::RESOLUTION_COLL_CHECK, &path, DRGBTConfig::MAX_ITER_TIME); - splines = nullptr; + traj = nullptr; if (DRGBTConfig::TRAJECTORY_INTERPOLATION == planning::TrajectoryInterpolation::Spline) { - splines = std::make_shared(ss, q_current, DRGBTConfig::MAX_ITER_TIME); - splines->setMaxRemainingIterTime(DRGBTConfig::MAX_ITER_TIME - DRGBTConfig::MAX_TIME_TASK1); - updating_state->setSplines(splines); - motion_validity->setSplines(splines); + traj = std::make_shared(ss, q_current, DRGBTConfig::MAX_ITER_TIME); + traj->setMaxRemainingIterTime(DRGBTConfig::MAX_ITER_TIME - DRGBTConfig::MAX_TIME_TASK1); + updating_state->setTraj(traj); + motion_validity->setTraj(traj); } // std::cout << "DRGBT planner initialized! \n"; diff --git a/src/planners/rrtx/RRTx.cpp b/src/planners/rrtx/RRTx.cpp index 466344dc..70463e4e 100644 --- a/src/planners/rrtx/RRTx.cpp +++ b/src/planners/rrtx/RRTx.cpp @@ -44,12 +44,12 @@ planning::rrtx::RRTx::RRTx(const std::shared_ptr ss_, const st motion_validity = std::make_shared (ss, RRTxConfig::TRAJECTORY_INTERPOLATION, RRTxConfig::RESOLUTION_COLL_CHECK, &path, RRTxConfig::MAX_ITER_TIME); - splines = nullptr; + traj = nullptr; if (RRTxConfig::TRAJECTORY_INTERPOLATION == planning::TrajectoryInterpolation::Spline) { - splines = std::make_shared(ss, q_current, RRTxConfig::MAX_ITER_TIME); - updating_state->setSplines(splines); - motion_validity->setSplines(splines); + traj = std::make_shared(ss, q_current, RRTxConfig::MAX_ITER_TIME); + updating_state->setTraj(traj); + motion_validity->setTraj(traj); } RRTConnectConfig::EPS_STEP = RRTxConfig::EPS_STEP; diff --git a/src/planners/trajectory/MotionValidity.cpp b/src/planners/trajectory/MotionValidity.cpp index f6ec4e99..6a70f5cc 100644 --- a/src/planners/trajectory/MotionValidity.cpp +++ b/src/planners/trajectory/MotionValidity.cpp @@ -9,7 +9,7 @@ planning::trajectory::MotionValidity::MotionValidity(const std::shared_ptrenv->getNumObjects(); i++) @@ -68,37 +68,37 @@ bool planning::trajectory::MotionValidity::check_v1(const std::shared_ptrspline_current' and 'splines->spline_next' during the current iteration. +// when the robot moves over splines 'traj->spline_current' and 'traj->spline_next' during the current iteration. bool planning::trajectory::MotionValidity::check_v2() { // std::cout << "Checking the validity of motion while updating environment... \n"; std::shared_ptr q_temp { nullptr }; - size_t num_checks1 = std::ceil((splines->spline_current->getTimeCurrent() - splines->spline_current->getTimeBegin()) * num_checks / max_iter_time); + size_t num_checks1 = std::ceil((traj->spline_current->getTimeCurrent() - traj->spline_current->getTimeBegin()) * num_checks / max_iter_time); size_t num_checks2 { num_checks - num_checks1 }; - float delta_time1 { (splines->spline_current->getTimeCurrent() - splines->spline_current->getTimeBegin()) / num_checks1 }; - float delta_time2 { (splines->spline_next->getTimeEnd() - splines->spline_next->getTimeCurrent()) / num_checks2 }; + float delta_time1 { (traj->spline_current->getTimeCurrent() - traj->spline_current->getTimeBegin()) / num_checks1 }; + float delta_time2 { (traj->spline_next->getTimeEnd() - traj->spline_next->getTimeCurrent()) / num_checks2 }; float t { 0 }; - // std::cout << "Current spline times: " << splines->spline_current->getTimeBegin() * 1000 << " [ms] \t" - // << splines->spline_current->getTimeCurrent() * 1000 << " [ms] \t" - // << splines->spline_current->getTimeEnd() * 1000 << " [ms] \n"; - // std::cout << "Next spline times: " << splines->spline_next->getTimeBegin() * 1000 << " [ms] \t" - // << splines->spline_next->getTimeCurrent() * 1000 << " [ms] \t" - // << splines->spline_next->getTimeEnd() * 1000 << " [ms] \n"; + // std::cout << "Current spline times: " << traj->spline_current->getTimeBegin() * 1000 << " [ms] \t" + // << traj->spline_current->getTimeCurrent() * 1000 << " [ms] \t" + // << traj->spline_current->getTimeEnd() * 1000 << " [ms] \n"; + // std::cout << "Next spline times: " << traj->spline_next->getTimeBegin() * 1000 << " [ms] \t" + // << traj->spline_next->getTimeCurrent() * 1000 << " [ms] \t" + // << traj->spline_next->getTimeEnd() * 1000 << " [ms] \n"; for (size_t num_check = 1; num_check <= num_checks; num_check++) { if (num_check <= num_checks1) { - t = splines->spline_current->getTimeBegin() + num_check * delta_time1; - q_temp = ss->getNewState(splines->spline_current->getPosition(t)); + t = traj->spline_current->getTimeBegin() + num_check * delta_time1; + q_temp = ss->getNewState(traj->spline_current->getPosition(t)); // std::cout << "t: " << t * 1000 << " [ms]\t from curr. spline \t" << q_temp << "\n"; ss->env->updateEnvironment(delta_time1); } else { - t = splines->spline_next->getTimeCurrent() + (num_check - num_checks1) * delta_time2; - q_temp = ss->getNewState(splines->spline_next->getPosition(t)); + t = traj->spline_next->getTimeCurrent() + (num_check - num_checks1) * delta_time2; + q_temp = ss->getNewState(traj->spline_next->getPosition(t)); // std::cout << "t: " << t * 1000 << " [ms]\t from next spline \t" << q_temp << "\n"; ss->env->updateEnvironment(delta_time2); } diff --git a/src/planners/trajectory/UpdatingState.cpp b/src/planners/trajectory/UpdatingState.cpp index 6b905407..310328c5 100644 --- a/src/planners/trajectory/UpdatingState.cpp +++ b/src/planners/trajectory/UpdatingState.cpp @@ -18,7 +18,7 @@ planning::trajectory::UpdatingState::UpdatingState(const std::shared_ptr // << (status == base::State::Status::Reached ? "Reached" : "") << "\n"; } -/// @brief Update a current state of the robot using 'splines->spline_current'. -/// Compute a new spline 'splines->spline_next', or remain 'splines->spline_current'. -/// Move 'q_current' towards 'q_next_reached' while following 'splines->spline_next'. +/// @brief Update a current state of the robot using 'traj->spline_current'. +/// Compute a new spline 'traj->spline_next', or remain 'traj->spline_current'. +/// Move 'q_current' towards 'q_next_reached' while following 'traj->spline_next'. /// 'q_current' will be updated to a robot position from the end of current iteration. /// @note The new spline will be computed in a way that all constraints on robot's maximal velocity, /// acceleration and jerk are surely always satisfied. @@ -130,7 +130,7 @@ void planning::trajectory::UpdatingState::update_v2(std::shared_ptr q_previous = q_current; q_next = q_next_; q_next_reached = q_next_reached_; - splines->spline_current = splines->spline_next; + traj->spline_current = traj->spline_next; float t_spline_max { (guaranteed_safe_motion ? SplinesConfig::MAX_TIME_COMPUTE_SAFE : SplinesConfig::MAX_TIME_COMPUTE_REGULAR) - SplinesConfig::MAX_TIME_PUBLISH * measure_time }; @@ -140,24 +140,24 @@ void planning::trajectory::UpdatingState::update_v2(std::shared_ptr float t_iter_remain { max_iter_time - t_iter - t_spline_max }; float t_spline_current { measure_time ? - splines->spline_current->getTimeCurrent(true) + t_spline_max : - splines->spline_current->getTimeEnd() + t_iter + t_spline_max }; + traj->spline_current->getTimeCurrent(true) + t_spline_max : + traj->spline_current->getTimeEnd() + t_iter + t_spline_max }; - splines->spline_current->setTimeBegin(splines->spline_current->getTimeEnd()); - splines->spline_current->setTimeCurrent(t_spline_current); + traj->spline_current->setTimeBegin(traj->spline_current->getTimeEnd()); + traj->spline_current->setTimeCurrent(t_spline_current); // std::cout << "Iter. time: " << t_iter * 1000 << " [ms] \n"; // std::cout << "Max. spline time: " << t_spline_max * 1000 << " [ms] \n"; // std::cout << "Remain. time: " << t_iter_remain * 1000 << " [ms] \n"; - // std::cout << "Begin spline time: " << splines->spline_current->getTimeBegin() * 1000 << " [ms] \n"; + // std::cout << "Begin spline time: " << traj->spline_current->getTimeBegin() * 1000 << " [ms] \n"; // std::cout << "Curr. spline time: " << t_spline_current * 1000 << " [ms] \n"; // ----------------------------------------------------------------------------------------- // - bool spline_computed { false }; + bool traj_computed { false }; float t_spline_remain {}; - Eigen::VectorXf current_pos { splines->spline_current->getPosition(t_spline_current) }; - Eigen::VectorXf current_vel { splines->spline_current->getVelocity(t_spline_current) }; - Eigen::VectorXf current_acc { splines->spline_current->getAcceleration(t_spline_current) }; + Eigen::VectorXf current_pos { traj->spline_current->getPosition(t_spline_current) }; + Eigen::VectorXf current_vel { traj->spline_current->getVelocity(t_spline_current) }; + Eigen::VectorXf current_acc { traj->spline_current->getAcceleration(t_spline_current) }; // std::cout << "Curr. pos: " << current_pos.transpose() << "\n"; // std::cout << "Curr. vel: " << current_vel.transpose() << "\n"; @@ -174,50 +174,50 @@ void planning::trajectory::UpdatingState::update_v2(std::shared_ptr current_vel.norm() / ss->robot->getMaxVel().norm() * SplinesConfig::MAX_RADIUS) }; std::shared_ptr q_target { std::get<1>(ss->interpolateEdge2(q_current, q_next_reached, step)) }; - splines->setCurrentState(q_current); - splines->setTargetState(q_target); + traj->setCurrentState(q_current); + traj->setTargetState(q_target); // std::cout << "q_target: " << q_target << "\n"; // std::cout << "q_next: " << q_next << "\n"; // std::cout << "q_next_reached: " << q_next_reached << "\n"; if (guaranteed_safe_motion) - spline_computed = splines->computeSafe(current_pos, current_vel, current_acc, t_iter_remain, t_spline_remain); + traj_computed = traj->computeSafe(current_pos, current_vel, current_acc, t_iter_remain, t_spline_remain); else { - if (splines->spline_current->isFinalConf(q_target->getCoord())) // Spline to such 'q_target' already exists! + if (traj->spline_current->isFinalConf(q_target->getCoord())) // Spline to such 'q_target' already exists! break; - spline_computed = splines->computeRegular(current_pos, current_vel, current_acc, t_iter_remain, t_spline_remain, non_zero_final_vel); + traj_computed = traj->computeRegular(current_pos, current_vel, current_acc, t_iter_remain, t_spline_remain, non_zero_final_vel); } } - while (!spline_computed && invokeChangeNextState()); + while (!traj_computed && invokeChangeNextState()); // std::cout << "Elapsed time for spline computing: " << (getElapsedTime() - t_iter) * 1e3 << " [ms] \n"; - if (spline_computed) + if (traj_computed) { // std::cout << "New spline is computed! \n"; - splines->spline_current->setTimeEnd(t_spline_current); - splines->spline_next->setTimeEnd(t_iter_remain); + traj->spline_current->setTimeEnd(t_spline_current); + traj->spline_next->setTimeEnd(t_iter_remain); } else { // std::cout << "Continuing with the previous spline! \n"; - splines->spline_next = splines->spline_current; - splines->spline_next->setTimeEnd(t_spline_current + t_iter_remain); + traj->spline_next = traj->spline_current; + traj->spline_next->setTimeEnd(t_spline_current + t_iter_remain); } - // splines->recordTrajectory(spline_computed); // Only for debugging + // traj->recordTrajectory(traj_computed); // Only for debugging - q_current = ss->getNewState(splines->spline_next->getPosition(splines->spline_next->getTimeEnd())); // Current robot position at the end of iteration + q_current = ss->getNewState(traj->spline_next->getPosition(traj->spline_next->getTimeEnd())); // Current robot position at the end of iteration if (status != base::State::Status::Trapped) { if (ss->getNorm(q_current, q_next) < // 'q_next' must be reached within the computed radius, and not only 'q_next_reached' - splines->spline_next->getVelocity(splines->spline_next->getTimeEnd()).norm() / ss->robot->getMaxVel().norm() * SplinesConfig::MAX_RADIUS) + traj->spline_next->getVelocity(traj->spline_next->getTimeEnd()).norm() / ss->robot->getMaxVel().norm() * SplinesConfig::MAX_RADIUS) status = base::State::Status::Reached; else status = base::State::Status::Advanced; } - // std::cout << "Spline next: \n" << splines->spline_next << "\n"; + // std::cout << "Spline next: \n" << traj->spline_next << "\n"; // std::cout << "q_current: " << q_current << "\n"; // std::cout << "Status: " << (status == base::State::Status::Advanced ? "Advanced" : "") // << (status == base::State::Status::Trapped ? "Trapped" : "") From 4e6d548ab964c0b72a320ed6983a54beac042577 Mon Sep 17 00:00:00 2001 From: Nermin Covic Date: Wed, 24 Sep 2025 13:24:28 +0200 Subject: [PATCH 11/40] "Splines.h" -> "Trajectory.h" --- apps/test_trajectory.cpp | 2 +- include/planners/drbt/DRGBT.h | 2 +- include/planners/rrtx/RRTx.h | 2 +- include/planners/trajectory/MotionValidity.h | 2 +- include/planners/trajectory/{Splines.h => Trajectory.h} | 0 include/planners/trajectory/UpdatingState.h | 2 +- src/planners/trajectory/{Splines.cpp => Trajectory.cpp} | 2 +- 7 files changed, 6 insertions(+), 6 deletions(-) rename include/planners/trajectory/{Splines.h => Trajectory.h} (100%) rename src/planners/trajectory/{Splines.cpp => Trajectory.cpp} (99%) diff --git a/apps/test_trajectory.cpp b/apps/test_trajectory.cpp index 02acffbb..ab5daf02 100644 --- a/apps/test_trajectory.cpp +++ b/apps/test_trajectory.cpp @@ -1,7 +1,7 @@ #include "RGBTConnect.h" #include "ConfigurationReader.h" #include "CommonFunctions.h" -#include "Splines.h" +#include "Trajectory.h" int main(int argc, char **argv) { diff --git a/include/planners/drbt/DRGBT.h b/include/planners/drbt/DRGBT.h index 298691e2..41a92c6b 100644 --- a/include/planners/drbt/DRGBT.h +++ b/include/planners/drbt/DRGBT.h @@ -13,7 +13,7 @@ #include "HorizonState.h" #include "UpdatingState.h" #include "MotionValidity.h" -#include "Splines.h" +#include "Trajectory.h" // #include // #include diff --git a/include/planners/rrtx/RRTx.h b/include/planners/rrtx/RRTx.h index 154c2a2f..7bee8918 100644 --- a/include/planners/rrtx/RRTx.h +++ b/include/planners/rrtx/RRTx.h @@ -9,7 +9,7 @@ #include "RRTxConfig.h" #include "UpdatingState.h" #include "MotionValidity.h" -#include "Splines.h" +#include "Trajectory.h" // #include // #include diff --git a/include/planners/trajectory/MotionValidity.h b/include/planners/trajectory/MotionValidity.h index 19b5722e..65dce0fc 100644 --- a/include/planners/trajectory/MotionValidity.h +++ b/include/planners/trajectory/MotionValidity.h @@ -6,7 +6,7 @@ #define RPMPL_MOTIONVALIDITY_H #include "StateSpace.h" -#include "Splines.h" +#include "Trajectory.h" #include "PlanningTypes.h" namespace planning::trajectory diff --git a/include/planners/trajectory/Splines.h b/include/planners/trajectory/Trajectory.h similarity index 100% rename from include/planners/trajectory/Splines.h rename to include/planners/trajectory/Trajectory.h diff --git a/include/planners/trajectory/UpdatingState.h b/include/planners/trajectory/UpdatingState.h index 7803298c..938b324d 100644 --- a/include/planners/trajectory/UpdatingState.h +++ b/include/planners/trajectory/UpdatingState.h @@ -6,7 +6,7 @@ #define RPMPL_UPDATINGSTATE_H #include "StateSpace.h" -#include "Splines.h" +#include "Trajectory.h" #include "RealVectorSpaceConfig.h" #include "PlanningTypes.h" diff --git a/src/planners/trajectory/Splines.cpp b/src/planners/trajectory/Trajectory.cpp similarity index 99% rename from src/planners/trajectory/Splines.cpp rename to src/planners/trajectory/Trajectory.cpp index d4ae0bd8..d9c04838 100644 --- a/src/planners/trajectory/Splines.cpp +++ b/src/planners/trajectory/Trajectory.cpp @@ -1,4 +1,4 @@ -#include "Splines.h" +#include "Trajectory.h" planning::trajectory::Trajectory::Trajectory(const std::shared_ptr &ss_) { From 212af70f6e4a374ec395b47b4901a7ef5e19233c Mon Sep 17 00:00:00 2001 From: Nermin Date: Thu, 2 Oct 2025 13:42:28 +0200 Subject: [PATCH 12/40] Optimizing 'computeRegular' function. --- src/planners/trajectory/Trajectory.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/planners/trajectory/Trajectory.cpp b/src/planners/trajectory/Trajectory.cpp index d9c04838..aed8514c 100644 --- a/src/planners/trajectory/Trajectory.cpp +++ b/src/planners/trajectory/Trajectory.cpp @@ -64,7 +64,6 @@ bool planning::trajectory::Trajectory::computeRegular(const Eigen::VectorXf &cur const Eigen::VectorXf ¤t_acc, float t_iter_remain, float t_max, bool non_zero_final_vel) { std::chrono::steady_clock::time_point time_start_ { std::chrono::steady_clock::now() }; - spline_next = std::make_shared(ss->robot, current_pos, current_vel, current_acc); std::shared_ptr spline_next_new { std::make_shared(ss->robot, current_pos, current_vel, current_acc) }; bool spline_computed { false }; @@ -82,11 +81,9 @@ bool planning::trajectory::Trajectory::computeRegular(const Eigen::VectorXf &cur { if (spline_next_new->compute(q_target->getCoord(), q_final_dot)) { - *spline_next = *spline_next_new; - q_final_dot_min = q_final_dot; + spline_next = spline_next_new; spline_computed = true; - if (num_iter == 1) - break; + break; } else q_final_dot_max = q_final_dot; From 654583401960e3cabe1fc0b33b19a6ef9ab024c5 Mon Sep 17 00:00:00 2001 From: Nermin Date: Thu, 2 Oct 2025 13:56:03 +0200 Subject: [PATCH 13/40] Rename 'SplineConfig' to 'TrajectoryConfig' --- CMakeLists.txt | 11 ++-- README.md | 2 +- data/configurations/configuration_drgbt.yaml | 4 +- ...nes.yaml => configuration_trajectory.yaml} | 0 .../scenario_random_obstacles.yaml | 6 +-- include/configurations/ConfigurationReader.h | 2 +- .../{SplinesConfig.h => TrajectoryConfig.h} | 8 +-- include/planners/trajectory/Spline.h | 2 +- src/configurations/ConfigurationReader.cpp | 52 +++++++++---------- src/configurations/SplinesConfig.cpp | 10 ---- src/configurations/TrajectoryConfig.cpp | 10 ++++ src/planners/drbt/DRGBT.cpp | 2 +- src/planners/trajectory/Spline.cpp | 4 +- src/planners/trajectory/Spline4.cpp | 8 +-- src/planners/trajectory/Spline5.cpp | 8 +-- src/planners/trajectory/Trajectory.cpp | 8 +-- src/planners/trajectory/UpdatingState.cpp | 10 ++-- 17 files changed, 74 insertions(+), 73 deletions(-) rename data/configurations/{configuration_splines.yaml => configuration_trajectory.yaml} (100%) rename include/configurations/{SplinesConfig.h => TrajectoryConfig.h} (90%) delete mode 100644 src/configurations/SplinesConfig.cpp create mode 100644 src/configurations/TrajectoryConfig.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index a73200a9..e3c45520 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -20,7 +20,7 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON) message("------------------ Using compiler: ${CMAKE_CXX_COMPILER} ------------------") if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) #-Werror) + add_compile_options(-Wall -Wextra -Wpedantic -Werror) endif() # Only do these if this is the main project, and not if it is included through add_subdirectory @@ -62,10 +62,11 @@ find_package(yaml-cpp REQUIRED) find_package(fcl 0.7 REQUIRED) find_package(nanoflann REQUIRED) -add_subdirectory(external/ros_reflexxes/libreflexxestype2) -include_directories(external/ros_reflexxes/libreflexxestype2/include) +# add_subdirectory(external/ros_reflexxes/libreflexxestype2) +# include_directories(external/ros_reflexxes/libreflexxestype2/include) -set(PROJECT_LIBRARIES gtest glog gflags nanoflann::nanoflann kdl_parser orocos-kdl fcl ccd yaml-cpp reflexxes) +set(PROJECT_LIBRARIES gtest glog gflags nanoflann::nanoflann kdl_parser orocos-kdl fcl ccd yaml-cpp) +#reflexxes) set(MAIN_PROJECT_BUILD_DIR ${CMAKE_CURRENT_SOURCE_DIR}/build) @@ -80,7 +81,7 @@ add_subdirectory(src) # The executable code is here add_subdirectory(apps) -add_subdirectory(external/ros_reflexxes/libreflexxestype2/src/RMLPositionSampleApplications) +# add_subdirectory(external/ros_reflexxes/libreflexxestype2/src/RMLPositionSampleApplications) # Testing only available if this is the main app # Emergency override MODERN_CMAKE_BUILD_TESTING provided as well diff --git a/README.md b/README.md index a1467d6c..f077ad87 100644 --- a/README.md +++ b/README.md @@ -143,7 +143,7 @@ In the file ```/data/configurations/configuration_drgbt.yaml```, you can set the - ```STATIC_PLANNER_TYPE```: Type of a static planner (for obtaining the predefined path). Available planners: "RGBMT*", "RGBT-Connect", "RBT-Connect" and "RRT-Connect"; - ```REAL_TIME_SCHEDULING```: Available real-time scheduling is "FPS" - Fixed Priority Scheduling; If you set "None", no real-time scheduling will be used; - ```MAX_TIME_TASK1```: Maximal time in [s] which Task 1 (computing the next configuration) can take from the processor. It must be less than ```MAX_ITER_TIME```. Default: 0.020; -- ```TRAJECTORY_INTERPOLATION```: Method for interpolation of trajectory: 'None' or 'Spline'. If 'None' is used, the robot always moves at its highest speed, i.e., an advancing step for moving from 'q_current' towards 'q_next' in C-space is determined by maximal robot's velocity. On the other hand, if 'Spline' is used, then a quintic spline from 'q_current' to 'q_next' is computed in order to satisfy all constaints on robot's maximal velocity, acceleration and jerk. All configuration parameters considering splines can be set in the file ```/data/configurations/configuration_splines.yaml```. +- ```TRAJECTORY_INTERPOLATION```: Method for interpolation of trajectory: 'None' or 'Spline'. If 'None' is used, the robot always moves at its highest speed, i.e., an advancing step for moving from 'q_current' towards 'q_next' in C-space is determined by maximal robot's velocity. On the other hand, if 'Spline' is used, then a quintic spline from 'q_current' to 'q_next' is computed in order to satisfy all constaints on robot's maximal velocity, acceleration and jerk. All configuration parameters considering splines can be set in the file ```/data/configurations/configuration_trajectory.yaml```. - ```GUARANTEED_SAFE_MOTION```: Whether robot motion is surely safe for environment. If collision eventually occurs, it will be at robot's zero velocity, meaning that an obstacle hit the robot, and not vice versa. This feature is intended to be used only for real/practical applications, thus it can be used only when ```TRAJECTORY_INTERPOLATION``` is set to 'Spline'. diff --git a/data/configurations/configuration_drgbt.yaml b/data/configurations/configuration_drgbt.yaml index 6b8cf173..0c18b266 100644 --- a/data/configurations/configuration_drgbt.yaml +++ b/data/configurations/configuration_drgbt.yaml @@ -1,5 +1,5 @@ MAX_NUM_ITER: 1000000 # Maximal number of algorithm iterations -MAX_ITER_TIME: 0.050 # Maximal runtime of a single iteration in [s] +MAX_ITER_TIME: 0.005 # Maximal runtime of a single iteration in [s] MAX_PLANNING_TIME: 10 # Maximal algorithm runtime in [s] INIT_HORIZON_SIZE: 10 # Initial horizon size TRESHOLD_WEIGHT: 0.5 # Treshold for the replanning assessment. Range: between 0 and 1 @@ -7,7 +7,7 @@ D_CRIT: 0.05 # Critical distance in W-space to compute c MAX_NUM_MODIFY_ATTEMPTS: 10 # Maximal number of attempts when modifying bad or critical states STATIC_PLANNER_TYPE: "RGBMT*" # Name of a static planner (for obtaining the predefined path). Default: "RGBMT*" or "RGBT-Connect" REAL_TIME_SCHEDULING: "FPS" # "FPS" - Fixed Priority Scheduling; "None" - Without real-time scheduling -MAX_TIME_TASK1: 0.050 # Maximal time in [s] which Task 1 can take from the processor +MAX_TIME_TASK1: 0.005 # Maximal time in [s] which Task 1 can take from the processor RESOLUTION_COLL_CHECK: 0.01 # Perform collision check when obstacle moves at most 'RESOLUTION_COLL_CHECK' in [m] TRAJECTORY_INTERPOLATION: "Spline" # Method for interpolation of trajectory: 'None' or 'Spline' GUARANTEED_SAFE_MOTION: false # Whether robot motion is surely safe for environment \ No newline at end of file diff --git a/data/configurations/configuration_splines.yaml b/data/configurations/configuration_trajectory.yaml similarity index 100% rename from data/configurations/configuration_splines.yaml rename to data/configurations/configuration_trajectory.yaml diff --git a/data/xarm6/scenario_random_obstacles/scenario_random_obstacles.yaml b/data/xarm6/scenario_random_obstacles/scenario_random_obstacles.yaml index 4345b258..ff3ae3bf 100644 --- a/data/xarm6/scenario_random_obstacles/scenario_random_obstacles.yaml +++ b/data/xarm6/scenario_random_obstacles/scenario_random_obstacles.yaml @@ -6,8 +6,8 @@ environment: min_dist_tol: 0.05 # Minimal distance tolerance for a static obstacle to not be included into a dynamic scene random_obstacles: - init_num: 0 # Number of random obstacles to start with the testing - max_num: 50 # Maximal number of random obstacles to be added + init_num: 10 # Number of random obstacles to start with the testing + max_num: 10 # Maximal number of random obstacles to be added max_vel: 1.6 # Maximal velocity of each obstacle in [m/s] max_acc: 0 # Maximal acceleration of each obstacle in [m/s²] dim: [0.01, 0.01, 0.01] # Dimensions of each random obstacle in [m] @@ -33,6 +33,6 @@ robot: testing: init_num: 1 # Number of testing to start with init_num_success: 0 # Initial number of already achieved successful tests - max_num: 1000 # Maximal number of tests that should be carried out + max_num: 100 # Maximal number of tests that should be carried out reach_successful_tests: false # If true, run totally 'max_num' successful tests motion_type: "light_directions" # Options: "straight", "circular", "two_tunnels", "random_directions", "light_directions". See 'Environment.cpp' for more details. diff --git a/include/configurations/ConfigurationReader.h b/include/configurations/ConfigurationReader.h index 1bf18ee4..7c79bc31 100644 --- a/include/configurations/ConfigurationReader.h +++ b/include/configurations/ConfigurationReader.h @@ -20,7 +20,7 @@ #include "RGBMTStarConfig.h" #include "DRGBTConfig.h" #include "RRTxConfig.h" -#include "SplinesConfig.h" +#include "TrajectoryConfig.h" class ConfigurationReader { diff --git a/include/configurations/SplinesConfig.h b/include/configurations/TrajectoryConfig.h similarity index 90% rename from include/configurations/SplinesConfig.h rename to include/configurations/TrajectoryConfig.h index 7c84621f..04f5c123 100644 --- a/include/configurations/SplinesConfig.h +++ b/include/configurations/TrajectoryConfig.h @@ -2,10 +2,10 @@ // Created by nermin on 23.05.24. // -#ifndef RPMPL_SPLINESCONFIG_H -#define RPMPL_SPLINESCONFIG_H +#ifndef RPMPL_TRAJECTORYCONFIG_H +#define RPMPL_TRAJECTORYCONFIG_H -class SplinesConfig +class TrajectoryConfig { public: static float MAX_TIME_COMPUTE_REGULAR; // Maximal time in [s] for computing a regular spline @@ -18,4 +18,4 @@ class SplinesConfig static float MAX_RADIUS; // Maximal radius in [rad] used when updating state (it should be experimentally evaluated) }; -#endif //RPMPL_SPLINESCONFIG_H \ No newline at end of file +#endif //RPMPL_TRAJECTORYCONFIG_H \ No newline at end of file diff --git a/include/planners/trajectory/Spline.h b/include/planners/trajectory/Spline.h index 6895bb48..b7012e8f 100644 --- a/include/planners/trajectory/Spline.h +++ b/include/planners/trajectory/Spline.h @@ -9,7 +9,7 @@ #include "AbstractRobot.h" #include "RealVectorSpaceConfig.h" -#include "SplinesConfig.h" +#include "TrajectoryConfig.h" namespace planning::trajectory { diff --git a/src/configurations/ConfigurationReader.cpp b/src/configurations/ConfigurationReader.cpp index 1c3b8b9c..128d9ff4 100644 --- a/src/configurations/ConfigurationReader.cpp +++ b/src/configurations/ConfigurationReader.cpp @@ -9,7 +9,7 @@ void ConfigurationReader::initConfiguration(const std::string &root_path) YAML::Node RGBMTStarConfigRoot { YAML::LoadFile(root_path + "/data/configurations/configuration_rgbmtstar.yaml") }; YAML::Node DRGBTConfigRoot { YAML::LoadFile(root_path + "/data/configurations/configuration_drgbt.yaml") }; YAML::Node RRTxConfigRoot { YAML::LoadFile(root_path + "/data/configurations/configuration_rrtx.yaml") }; - YAML::Node SplinesConfigRoot { YAML::LoadFile(root_path + "/data/configurations/configuration_splines.yaml") }; + YAML::Node TrajectoryConfigRoot { YAML::LoadFile(root_path + "/data/configurations/configuration_trajectory.yaml") }; // RealVectorSpaceConfigRoot if (RealVectorSpaceConfigRoot["NUM_INTERPOLATION_VALIDITY_CHECKS"].IsDefined()) @@ -248,46 +248,46 @@ void ConfigurationReader::initConfiguration(const std::string &root_path) else LOG(INFO) << "RRTxConfig::TRAJECTORY_INTERPOLATION is not defined! Using default value of " << RRTxConfig::TRAJECTORY_INTERPOLATION; - // SplinesConfigRoot - if (SplinesConfigRoot["MAX_TIME_COMPUTE_REGULAR"].IsDefined()) - SplinesConfig::MAX_TIME_COMPUTE_REGULAR = SplinesConfigRoot["MAX_TIME_COMPUTE_REGULAR"].as(); + // TrajectoryConfigRoot + if (TrajectoryConfigRoot["MAX_TIME_COMPUTE_REGULAR"].IsDefined()) + TrajectoryConfig::MAX_TIME_COMPUTE_REGULAR = TrajectoryConfigRoot["MAX_TIME_COMPUTE_REGULAR"].as(); else - LOG(INFO) << "SplinesConfig::MAX_TIME_COMPUTE_REGULAR is not defined! Using default value of " << SplinesConfig::MAX_TIME_COMPUTE_REGULAR; + LOG(INFO) << "TrajectoryConfig::MAX_TIME_COMPUTE_REGULAR is not defined! Using default value of " << TrajectoryConfig::MAX_TIME_COMPUTE_REGULAR; - if (SplinesConfigRoot["MAX_TIME_COMPUTE_SAFE"].IsDefined()) - SplinesConfig::MAX_TIME_COMPUTE_SAFE = SplinesConfigRoot["MAX_TIME_COMPUTE_SAFE"].as(); + if (TrajectoryConfigRoot["MAX_TIME_COMPUTE_SAFE"].IsDefined()) + TrajectoryConfig::MAX_TIME_COMPUTE_SAFE = TrajectoryConfigRoot["MAX_TIME_COMPUTE_SAFE"].as(); else - LOG(INFO) << "SplinesConfig::MAX_TIME_COMPUTE_SAFE is not defined! Using default value of " << SplinesConfig::MAX_TIME_COMPUTE_SAFE; + LOG(INFO) << "TrajectoryConfig::MAX_TIME_COMPUTE_SAFE is not defined! Using default value of " << TrajectoryConfig::MAX_TIME_COMPUTE_SAFE; - if (SplinesConfigRoot["MAX_TIME_PUBLISH"].IsDefined()) - SplinesConfig::MAX_TIME_PUBLISH = SplinesConfigRoot["MAX_TIME_PUBLISH"].as(); + if (TrajectoryConfigRoot["MAX_TIME_PUBLISH"].IsDefined()) + TrajectoryConfig::MAX_TIME_PUBLISH = TrajectoryConfigRoot["MAX_TIME_PUBLISH"].as(); else - LOG(INFO) << "SplinesConfig::MAX_TIME_PUBLISH is not defined! Using default value of " << SplinesConfig::MAX_TIME_PUBLISH; + LOG(INFO) << "TrajectoryConfig::MAX_TIME_PUBLISH is not defined! Using default value of " << TrajectoryConfig::MAX_TIME_PUBLISH; - if (SplinesConfigRoot["MAX_TIME_FINAL"].IsDefined()) - SplinesConfig::MAX_TIME_FINAL = SplinesConfigRoot["MAX_TIME_FINAL"].as(); + if (TrajectoryConfigRoot["MAX_TIME_FINAL"].IsDefined()) + TrajectoryConfig::MAX_TIME_FINAL = TrajectoryConfigRoot["MAX_TIME_FINAL"].as(); else - LOG(INFO) << "SplinesConfig::MAX_TIME_FINAL is not defined! Using default value of " << SplinesConfig::MAX_TIME_FINAL; + LOG(INFO) << "TrajectoryConfig::MAX_TIME_FINAL is not defined! Using default value of " << TrajectoryConfig::MAX_TIME_FINAL; - if (SplinesConfigRoot["TIME_STEP"].IsDefined()) - SplinesConfig::TIME_STEP = SplinesConfigRoot["TIME_STEP"].as(); + if (TrajectoryConfigRoot["TIME_STEP"].IsDefined()) + TrajectoryConfig::TIME_STEP = TrajectoryConfigRoot["TIME_STEP"].as(); else - LOG(INFO) << "SplinesConfig::TIME_STEP is not defined! Using default value of " << SplinesConfig::TIME_STEP; + LOG(INFO) << "TrajectoryConfig::TIME_STEP is not defined! Using default value of " << TrajectoryConfig::TIME_STEP; - if (SplinesConfigRoot["FINAL_JERK_STEP"].IsDefined()) - SplinesConfig::FINAL_JERK_STEP = SplinesConfigRoot["FINAL_JERK_STEP"].as(); + if (TrajectoryConfigRoot["FINAL_JERK_STEP"].IsDefined()) + TrajectoryConfig::FINAL_JERK_STEP = TrajectoryConfigRoot["FINAL_JERK_STEP"].as(); else - LOG(INFO) << "SplinesConfig::FINAL_JERK_STEP is not defined! Using default value of " << SplinesConfig::FINAL_JERK_STEP; + LOG(INFO) << "TrajectoryConfig::FINAL_JERK_STEP is not defined! Using default value of " << TrajectoryConfig::FINAL_JERK_STEP; - if (SplinesConfigRoot["FINAL_VELOCITY_STEP"].IsDefined()) - SplinesConfig::FINAL_VELOCITY_STEP = SplinesConfigRoot["FINAL_VELOCITY_STEP"].as(); + if (TrajectoryConfigRoot["FINAL_VELOCITY_STEP"].IsDefined()) + TrajectoryConfig::FINAL_VELOCITY_STEP = TrajectoryConfigRoot["FINAL_VELOCITY_STEP"].as(); else - LOG(INFO) << "SplinesConfig::FINAL_VELOCITY_STEP is not defined! Using default value of " << SplinesConfig::FINAL_VELOCITY_STEP; + LOG(INFO) << "TrajectoryConfig::FINAL_VELOCITY_STEP is not defined! Using default value of " << TrajectoryConfig::FINAL_VELOCITY_STEP; - if (SplinesConfigRoot["MAX_RADIUS"].IsDefined()) - SplinesConfig::MAX_RADIUS = SplinesConfigRoot["MAX_RADIUS"].as(); + if (TrajectoryConfigRoot["MAX_RADIUS"].IsDefined()) + TrajectoryConfig::MAX_RADIUS = TrajectoryConfigRoot["MAX_RADIUS"].as(); else - LOG(INFO) << "SplinesConfig::MAX_RADIUS is not defined! Using default value of " << SplinesConfig::MAX_RADIUS; + LOG(INFO) << "TrajectoryConfig::MAX_RADIUS is not defined! Using default value of " << TrajectoryConfig::MAX_RADIUS; LOG(INFO) << "Configuration parameters read successfully!"; } diff --git a/src/configurations/SplinesConfig.cpp b/src/configurations/SplinesConfig.cpp deleted file mode 100644 index 5879cfea..00000000 --- a/src/configurations/SplinesConfig.cpp +++ /dev/null @@ -1,10 +0,0 @@ -#include "SplinesConfig.h" - -float SplinesConfig::MAX_TIME_COMPUTE_REGULAR = 0.001; -float SplinesConfig::MAX_TIME_COMPUTE_SAFE = 0.003; -float SplinesConfig::MAX_TIME_PUBLISH = 0.0005; -float SplinesConfig::MAX_TIME_FINAL = 10.0; -float SplinesConfig::TIME_STEP = 0.01; -float SplinesConfig::FINAL_JERK_STEP = 1.0; -float SplinesConfig::FINAL_VELOCITY_STEP = 0.1; -float SplinesConfig::MAX_RADIUS = 5.0; \ No newline at end of file diff --git a/src/configurations/TrajectoryConfig.cpp b/src/configurations/TrajectoryConfig.cpp new file mode 100644 index 00000000..0f9074ef --- /dev/null +++ b/src/configurations/TrajectoryConfig.cpp @@ -0,0 +1,10 @@ +#include "TrajectoryConfig.h" + +float TrajectoryConfig::MAX_TIME_COMPUTE_REGULAR = 0.001; +float TrajectoryConfig::MAX_TIME_COMPUTE_SAFE = 0.003; +float TrajectoryConfig::MAX_TIME_PUBLISH = 0.0005; +float TrajectoryConfig::MAX_TIME_FINAL = 10.0; +float TrajectoryConfig::TIME_STEP = 0.01; +float TrajectoryConfig::FINAL_JERK_STEP = 1.0; +float TrajectoryConfig::FINAL_VELOCITY_STEP = 0.1; +float TrajectoryConfig::MAX_RADIUS = 5.0; \ No newline at end of file diff --git a/src/planners/drbt/DRGBT.cpp b/src/planners/drbt/DRGBT.cpp index eb7ad7e8..4b4ac77b 100644 --- a/src/planners/drbt/DRGBT.cpp +++ b/src/planners/drbt/DRGBT.cpp @@ -246,7 +246,7 @@ void planning::drbt::DRGBT::generateGBur() float time_elapsed {}; float max_time { DRGBTConfig::MAX_TIME_TASK1 }; if (DRGBTConfig::TRAJECTORY_INTERPOLATION == planning::TrajectoryInterpolation::Spline) - max_time -= DRGBTConfig::GUARANTEED_SAFE_MOTION ? SplinesConfig::MAX_TIME_COMPUTE_SAFE : SplinesConfig::MAX_TIME_COMPUTE_REGULAR; + max_time -= DRGBTConfig::GUARANTEED_SAFE_MOTION ? TrajectoryConfig::MAX_TIME_COMPUTE_SAFE : TrajectoryConfig::MAX_TIME_COMPUTE_REGULAR; planner_info->setTask1Interrupted(false); for (size_t idx = 0; idx < horizon.size(); idx++) diff --git a/src/planners/trajectory/Spline.cpp b/src/planners/trajectory/Spline.cpp index 0331ae18..86773cf9 100644 --- a/src/planners/trajectory/Spline.cpp +++ b/src/planners/trajectory/Spline.cpp @@ -35,10 +35,10 @@ bool planning::trajectory::Spline::isFinalConf(const Eigen::VectorXf &q) /// @return Return 0 if the function is non monotonic. int planning::trajectory::Spline::checkPositionMonotonicity(size_t idx) { - float value { getVelocity(SplinesConfig::TIME_STEP, idx) }; + float value { getVelocity(TrajectoryConfig::TIME_STEP, idx) }; int monotonic { value > 0 ? 1 : -1 }; - for (float t = 2*SplinesConfig::TIME_STEP; t <= times_final[idx]; t += SplinesConfig::TIME_STEP) + for (float t = 2*TrajectoryConfig::TIME_STEP; t <= times_final[idx]; t += TrajectoryConfig::TIME_STEP) { value = getVelocity(t, idx); if ((value > 0 && monotonic == -1) || (value < 0 && monotonic == 1)) diff --git a/src/planners/trajectory/Spline4.cpp b/src/planners/trajectory/Spline4.cpp index 9924941b..a8e38637 100644 --- a/src/planners/trajectory/Spline4.cpp +++ b/src/planners/trajectory/Spline4.cpp @@ -77,7 +77,7 @@ bool planning::trajectory::Spline4::compute(const Eigen::VectorXf &q_final_dot, float t_f_left { 0 }, t_f_right { 0 }; float b_left {}, b_right {}; float a_left {}, a_right {}; - const size_t max_num_iter = std::ceil(std::log2(2 * robot->getMaxJerk(0) / SplinesConfig::FINAL_JERK_STEP)); + const size_t max_num_iter = std::ceil(std::log2(2 * robot->getMaxJerk(0) / TrajectoryConfig::FINAL_JERK_STEP)); for (size_t idx = 0; idx < robot->getNumDOFs(); idx++) { @@ -303,7 +303,7 @@ float planning::trajectory::Spline4::computeFinalTime(size_t idx, float q_f_dot, for (size_t i = 0; i < t_sol.size(); i++) { // std::cout << "t_sol: " << t_sol[i] << " [s] \n"; - if (t_sol[i] > 0 && t_sol[i] < SplinesConfig::MAX_TIME_FINAL) + if (t_sol[i] > 0 && t_sol[i] < TrajectoryConfig::MAX_TIME_FINAL) t_f.emplace_back(t_sol[i]); } @@ -387,12 +387,12 @@ std::vector planning::trajectory::Spline4::getPositionExtremumTimes(size_ float pos_prev { getPosition(0, idx, times_final[idx]) }; bool rising { getVelocity(0, idx, times_final[idx]) > 0 ? true : false }; - for (float t = SplinesConfig::TIME_STEP; t <= times_final[idx]; t += SplinesConfig::TIME_STEP) + for (float t = TrajectoryConfig::TIME_STEP; t <= times_final[idx]; t += TrajectoryConfig::TIME_STEP) { pos_curr = getPosition(t, idx, times_final[idx]); if ((pos_curr > pos_prev && !rising) || (pos_curr < pos_prev && rising)) { - t_extrema.emplace_back((2*t - SplinesConfig::TIME_STEP) * 0.5); + t_extrema.emplace_back((2*t - TrajectoryConfig::TIME_STEP) * 0.5); rising = !rising; } pos_prev = pos_curr; diff --git a/src/planners/trajectory/Spline5.cpp b/src/planners/trajectory/Spline5.cpp index 3746373d..c0afda0a 100644 --- a/src/planners/trajectory/Spline5.cpp +++ b/src/planners/trajectory/Spline5.cpp @@ -78,7 +78,7 @@ bool planning::trajectory::Spline5::compute(const Eigen::VectorXf &q_final, cons float t_f_opt { 0 }; float t_f_left { 0 }, t_f_right { 0 }; Eigen::Vector3f abc_left {}, abc_right {}; // a, b and c coefficients, respectively - const size_t max_num_iter = std::ceil(std::log2(2 * robot->getMaxJerk(0) / SplinesConfig::FINAL_JERK_STEP)); + const size_t max_num_iter = std::ceil(std::log2(2 * robot->getMaxJerk(0) / TrajectoryConfig::FINAL_JERK_STEP)); for (size_t idx = 0; idx < robot->getNumDOFs(); idx++) { @@ -269,7 +269,7 @@ float planning::trajectory::Spline5::computeFinalTime(size_t idx, float q_f, flo for (size_t i = 0; i < t_sol.size(); i++) { // std::cout << "t_sol: " << t_sol[i] << " [s] \n"; - if (t_sol[i] > 0 && t_sol[i] < SplinesConfig::MAX_TIME_FINAL) + if (t_sol[i] > 0 && t_sol[i] < TrajectoryConfig::MAX_TIME_FINAL) t_f.emplace_back(t_sol[i]); } @@ -382,12 +382,12 @@ std::vector planning::trajectory::Spline5::getPositionExtremumTimes(size_ float pos_prev { getPosition(0, idx, times_final[idx]) }; bool rising { getVelocity(0, idx, times_final[idx]) > 0 ? true : false }; - for (float t = SplinesConfig::TIME_STEP; t <= times_final[idx]; t += SplinesConfig::TIME_STEP) + for (float t = TrajectoryConfig::TIME_STEP; t <= times_final[idx]; t += TrajectoryConfig::TIME_STEP) { pos_curr = getPosition(t, idx, times_final[idx]); if ((pos_curr > pos_prev && !rising) || (pos_curr < pos_prev && rising)) { - t_extrema.emplace_back((2*t - SplinesConfig::TIME_STEP) * 0.5); + t_extrema.emplace_back((2*t - TrajectoryConfig::TIME_STEP) * 0.5); rising = !rising; } pos_prev = pos_curr; diff --git a/src/planners/trajectory/Trajectory.cpp b/src/planners/trajectory/Trajectory.cpp index aed8514c..db53c7bb 100644 --- a/src/planners/trajectory/Trajectory.cpp +++ b/src/planners/trajectory/Trajectory.cpp @@ -47,8 +47,8 @@ void planning::trajectory::Trajectory::setParams() } max_num_iter_spline_regular = all_robot_vel_same ? - std::ceil(std::log2(2 * ss->robot->getMaxVel(0) / SplinesConfig::FINAL_VELOCITY_STEP)) : - std::ceil(std::log2(2 * ss->robot->getMaxVel().maxCoeff() / SplinesConfig::FINAL_VELOCITY_STEP)); + std::ceil(std::log2(2 * ss->robot->getMaxVel(0) / TrajectoryConfig::FINAL_VELOCITY_STEP)) : + std::ceil(std::log2(2 * ss->robot->getMaxVel().maxCoeff() / TrajectoryConfig::FINAL_VELOCITY_STEP)); } /// @brief Compute a regular spline that is not surely safe for environment, meaning that, @@ -257,7 +257,7 @@ bool planning::trajectory::Trajectory::computeSafe(const Eigen::VectorXf ¤ bool planning::trajectory::Trajectory::checkCollision(std::shared_ptr q_init, float t_iter) { // std::chrono::steady_clock::time_point time_start_ { std::chrono::steady_clock::now() }; - float delta_t { SplinesConfig::TIME_STEP }; + float delta_t { TrajectoryConfig::TIME_STEP }; size_t num_iter = std::ceil(spline_next->getTimeFinal() / delta_t); delta_t = spline_next->getTimeFinal() / num_iter; float rho_robot {}; @@ -484,7 +484,7 @@ void planning::trajectory::Trajectory::path2traj_v2(const std::vectorgetVelocity(t), spline_current->getAcceleration(t), max_remaining_iter_time, - SplinesConfig::MAX_TIME_COMPUTE_REGULAR, + TrajectoryConfig::MAX_TIME_COMPUTE_REGULAR, non_zero_final_vel ); diff --git a/src/planners/trajectory/UpdatingState.cpp b/src/planners/trajectory/UpdatingState.cpp index 310328c5..09e2044e 100644 --- a/src/planners/trajectory/UpdatingState.cpp +++ b/src/planners/trajectory/UpdatingState.cpp @@ -132,8 +132,8 @@ void planning::trajectory::UpdatingState::update_v2(std::shared_ptr q_next_reached = q_next_reached_; traj->spline_current = traj->spline_next; - float t_spline_max { (guaranteed_safe_motion ? SplinesConfig::MAX_TIME_COMPUTE_SAFE : SplinesConfig::MAX_TIME_COMPUTE_REGULAR) - - SplinesConfig::MAX_TIME_PUBLISH * measure_time }; + float t_spline_max { (guaranteed_safe_motion ? TrajectoryConfig::MAX_TIME_COMPUTE_SAFE : TrajectoryConfig::MAX_TIME_COMPUTE_REGULAR) - + TrajectoryConfig::MAX_TIME_PUBLISH * measure_time }; float t_iter { getElapsedTime() }; if (max_iter_time - max_remaining_iter_time - t_iter < t_spline_max) t_spline_max = max_iter_time - max_remaining_iter_time - t_iter; @@ -171,7 +171,7 @@ void planning::trajectory::UpdatingState::update_v2(std::shared_ptr break; float step { std::max(ss->robot->getMaxVel().norm() * max_iter_time, - current_vel.norm() / ss->robot->getMaxVel().norm() * SplinesConfig::MAX_RADIUS) }; + current_vel.norm() / ss->robot->getMaxVel().norm() * TrajectoryConfig::MAX_RADIUS) }; std::shared_ptr q_target { std::get<1>(ss->interpolateEdge2(q_current, q_next_reached, step)) }; traj->setCurrentState(q_current); @@ -211,7 +211,7 @@ void planning::trajectory::UpdatingState::update_v2(std::shared_ptr if (status != base::State::Status::Trapped) { if (ss->getNorm(q_current, q_next) < // 'q_next' must be reached within the computed radius, and not only 'q_next_reached' - traj->spline_next->getVelocity(traj->spline_next->getTimeEnd()).norm() / ss->robot->getMaxVel().norm() * SplinesConfig::MAX_RADIUS) + traj->spline_next->getVelocity(traj->spline_next->getTimeEnd()).norm() / ss->robot->getMaxVel().norm() * TrajectoryConfig::MAX_RADIUS) status = base::State::Status::Reached; else status = base::State::Status::Advanced; @@ -223,7 +223,7 @@ void planning::trajectory::UpdatingState::update_v2(std::shared_ptr // << (status == base::State::Status::Trapped ? "Trapped" : "") // << (status == base::State::Status::Reached ? "Reached" : "") << "\n"; - remaining_time = t_spline_max + SplinesConfig::MAX_TIME_PUBLISH * measure_time - (getElapsedTime() - t_iter); + remaining_time = t_spline_max + TrajectoryConfig::MAX_TIME_PUBLISH * measure_time - (getElapsedTime() - t_iter); } // This function will change 'q_next' and 'q_next_reached' From 695fff95bea33c2b2ac7ce93f8896e66a101bfb5 Mon Sep 17 00:00:00 2001 From: Nermin Date: Thu, 2 Oct 2025 18:05:35 +0200 Subject: [PATCH 14/40] Minor updates... --- include/configurations/DRGBTConfig.h | 1 - include/configurations/RRTxConfig.h | 1 - include/configurations/TrajectoryConfig.h | 3 +++ include/planners/drbt/DRGBT.h | 32 +++++++++++------------ src/configurations/TrajectoryConfig.cpp | 3 ++- src/planners/trajectory/Trajectory.cpp | 2 ++ 6 files changed, 23 insertions(+), 19 deletions(-) diff --git a/include/configurations/DRGBTConfig.h b/include/configurations/DRGBTConfig.h index d537d1ef..f8bb2dc7 100644 --- a/include/configurations/DRGBTConfig.h +++ b/include/configurations/DRGBTConfig.h @@ -5,7 +5,6 @@ #ifndef RPMPL_DRGBTCONFIG_H #define RPMPL_DRGBTCONFIG_H -#include #include "PlanningTypes.h" typedef unsigned long size_t; diff --git a/include/configurations/RRTxConfig.h b/include/configurations/RRTxConfig.h index 9d31325e..699df4fc 100644 --- a/include/configurations/RRTxConfig.h +++ b/include/configurations/RRTxConfig.h @@ -5,7 +5,6 @@ #ifndef RPMPL_RRTXCONFIG_H #define RPMPL_RRTXCONFIG_H -#include #include "PlanningTypes.h" typedef unsigned long size_t; diff --git a/include/configurations/TrajectoryConfig.h b/include/configurations/TrajectoryConfig.h index 04f5c123..fca47b0b 100644 --- a/include/configurations/TrajectoryConfig.h +++ b/include/configurations/TrajectoryConfig.h @@ -5,6 +5,8 @@ #ifndef RPMPL_TRAJECTORYCONFIG_H #define RPMPL_TRAJECTORYCONFIG_H +typedef unsigned long size_t; + class TrajectoryConfig { public: @@ -16,6 +18,7 @@ class TrajectoryConfig static float FINAL_JERK_STEP; // Final jerk step when using bisection method for finding optimal value of coefficient 'c' static float FINAL_VELOCITY_STEP; // Final velocity step when using bisection method for finding optimal value of final velocity static float MAX_RADIUS; // Maximal radius in [rad] used when updating state (it should be experimentally evaluated) + static size_t NUM_VALIDITY_POINTS_CHECK; // Number of validity points from trajectory to be checked within 'MotionValidity' class }; #endif //RPMPL_TRAJECTORYCONFIG_H \ No newline at end of file diff --git a/include/planners/drbt/DRGBT.h b/include/planners/drbt/DRGBT.h index 41a92c6b..218bde81 100644 --- a/include/planners/drbt/DRGBT.h +++ b/include/planners/drbt/DRGBT.h @@ -50,22 +50,22 @@ namespace planning::drbt std::unique_ptr initStaticPlanner(float max_planning_time); virtual void replan(float max_planning_time); - std::vector> horizon; // List of all horizon states and their information - std::shared_ptr q_current; // Current robot configuration - std::shared_ptr q_previous; // Previous robot configuration - std::shared_ptr q_next; // Next robot configuration - std::shared_ptr q_next_previous; // Next robot configuration from the previous iteration - float d_max_mean; // Averaged maximal distance-to-obstacles through iterations - size_t horizon_size; // Number of states that is required to be in the horizon - bool replanning_required; // Whether predefined path replanning is explicitly required - base::State::Status status; // The status of proceeding from 'q_current' towards 'q_next' - std::vector> predefined_path; // The predefined path that is being followed - size_t num_lateral_states; // Number of lateral states - float max_edge_length; // Maximal edge length when acquiring a new predefined path - std::shared_ptr traj; // Everything related to trajectory - std::shared_ptr updating_state; // Class for updating current state - std::shared_ptr motion_validity; // Class for checking validity of motion - std::vector> visited_states; + std::vector> horizon; // List of all horizon states and their information + std::shared_ptr q_current; // Current robot configuration + std::shared_ptr q_previous; // Previous robot configuration + std::shared_ptr q_next; // Next robot configuration + std::shared_ptr q_next_previous; // Next robot configuration from the previous iteration + float d_max_mean; // Averaged maximal distance-to-obstacles through iterations + size_t horizon_size; // Number of states that is required to be in the horizon + bool replanning_required; // Whether predefined path replanning is explicitly required + base::State::Status status; // The status of proceeding from 'q_current' towards 'q_next' + std::vector> predefined_path; // The predefined path that is being followed + size_t num_lateral_states; // Number of lateral states + float max_edge_length; // Maximal edge length when acquiring a new predefined path + std::shared_ptr traj; // Everything related to trajectory + std::shared_ptr updating_state; // Class for updating current state + std::shared_ptr motion_validity; // Class for checking validity of motion + std::vector> visited_states; // Set of visited states when choosing 'q_next' }; } diff --git a/src/configurations/TrajectoryConfig.cpp b/src/configurations/TrajectoryConfig.cpp index 0f9074ef..d6640bea 100644 --- a/src/configurations/TrajectoryConfig.cpp +++ b/src/configurations/TrajectoryConfig.cpp @@ -7,4 +7,5 @@ float TrajectoryConfig::MAX_TIME_FINAL = 10.0; float TrajectoryConfig::TIME_STEP = 0.01; float TrajectoryConfig::FINAL_JERK_STEP = 1.0; float TrajectoryConfig::FINAL_VELOCITY_STEP = 0.1; -float TrajectoryConfig::MAX_RADIUS = 5.0; \ No newline at end of file +float TrajectoryConfig::MAX_RADIUS = 5.0; +size_t TrajectoryConfig::NUM_VALIDITY_POINTS_CHECK = 10; \ No newline at end of file diff --git a/src/planners/trajectory/Trajectory.cpp b/src/planners/trajectory/Trajectory.cpp index db53c7bb..f54c024f 100644 --- a/src/planners/trajectory/Trajectory.cpp +++ b/src/planners/trajectory/Trajectory.cpp @@ -36,6 +36,8 @@ planning::trajectory::Trajectory::Trajectory(const std::shared_ptrnum_dimensions; i++) { From 9e44b18ea3c805e79ab6f1d5b756dfc59304f137 Mon Sep 17 00:00:00 2001 From: Nermin Date: Thu, 2 Oct 2025 18:06:47 +0200 Subject: [PATCH 15/40] 'MotionValidity' class updated. --- include/planners/trajectory/MotionValidity.h | 18 ++-- include/planners/trajectory/Trajectory.h | 3 +- src/planners/trajectory/MotionValidity.cpp | 103 ++++++------------- 3 files changed, 42 insertions(+), 82 deletions(-) diff --git a/include/planners/trajectory/MotionValidity.h b/include/planners/trajectory/MotionValidity.h index 65dce0fc..67475c68 100644 --- a/include/planners/trajectory/MotionValidity.h +++ b/include/planners/trajectory/MotionValidity.h @@ -6,33 +6,27 @@ #define RPMPL_MOTIONVALIDITY_H #include "StateSpace.h" -#include "Trajectory.h" #include "PlanningTypes.h" +#include "TrajectoryConfig.h" namespace planning::trajectory { class MotionValidity { public: - MotionValidity(const std::shared_ptr &ss_, planning::TrajectoryInterpolation traj_interpolation_, - float resolution_coll_check_, std::vector>* path_, float max_iter_time_); + MotionValidity(const std::shared_ptr &ss_, float resolution_coll_check_, float max_iter_time_, + std::vector>* path_); ~MotionValidity() {} bool check(const std::shared_ptr &q_previous, const std::shared_ptr &q_current); - inline void setTraj(const std::shared_ptr &traj_) { traj = traj_; } + bool check(const std::vector &traj_points_current_iter); private: - bool check_v1(const std::shared_ptr &q_previous, const std::shared_ptr &q_current); - bool check_v2(); - std::shared_ptr ss; - planning::TrajectoryInterpolation traj_interpolation; float resolution_coll_check; - std::vector>* path; float max_iter_time; - std::shared_ptr traj; - size_t num_checks; // Maximal number of validity checks when robot moves from previous to current configuration, - // while the obstacles are moving simultaneously. + std::vector>* path; + }; } diff --git a/include/planners/trajectory/Trajectory.h b/include/planners/trajectory/Trajectory.h index ba5154a6..31bccc9d 100644 --- a/include/planners/trajectory/Trajectory.h +++ b/include/planners/trajectory/Trajectory.h @@ -36,7 +36,8 @@ namespace planning::trajectory std::shared_ptr spline_current; // Current spline that 'q_current' is following in the current iteration std::shared_ptr spline_next; // Next spline generated from 'q_current' to 'q_target' std::shared_ptr composite_spline; // Composite spline from the start to a desired target configuration - + std::vector traj_points_current_iter; // Trajectory points from the current iteration to be validated within 'MotionValidity' + private: void setParams(); bool checkCollision(std::shared_ptr q_init, float t_iter); diff --git a/src/planners/trajectory/MotionValidity.cpp b/src/planners/trajectory/MotionValidity.cpp index 6a70f5cc..465f6126 100644 --- a/src/planners/trajectory/MotionValidity.cpp +++ b/src/planners/trajectory/MotionValidity.cpp @@ -1,15 +1,12 @@ #include "MotionValidity.h" planning::trajectory::MotionValidity::MotionValidity(const std::shared_ptr &ss_, - planning::TrajectoryInterpolation traj_interpolation_, float resolution_coll_check_, - std::vector>* path_, float max_iter_time_) + float resolution_coll_check_, float max_iter_time_, std::vector>* path_) { ss = ss_; - traj_interpolation = traj_interpolation_; resolution_coll_check = resolution_coll_check_; - path = path_; max_iter_time = max_iter_time_; - traj = nullptr; + path = path_; float max_vel_obs { 0 }; for (size_t i = 0; i < ss->env->getNumObjects(); i++) @@ -18,100 +15,68 @@ planning::trajectory::MotionValidity::MotionValidity(const std::shared_ptrenv->getObject(i)->getMaxVel(); } - num_checks = std::ceil((max_vel_obs * max_iter_time) / resolution_coll_check); // In order to obtain check when obstacle moves at most 'resolution_coll_check' [m] - if (num_checks < 2) - num_checks = 2; // Default value in case 'max_vel_obs' is too small + // Maximal number of validity points from trajectory when robot moves from previous to current configuration, + // in order to obtain the check when obstacle moves at most 'resolution_coll_check' [m] (while the obstacles are moving simultaneously). + TrajectoryConfig::NUM_VALIDITY_POINTS_CHECK = std::ceil((max_vel_obs * max_iter_time) / resolution_coll_check); + if (TrajectoryConfig::NUM_VALIDITY_POINTS_CHECK < 2) + TrajectoryConfig::NUM_VALIDITY_POINTS_CHECK = 2; // Default value in case 'max_vel_obs' is too small } -/// @brief Discretely check the validity of motion. During this checking obstacles are moving simultaneously. -/// Finally, environment is updated within this function. +/// @brief In case traj_interpolation == "None", discretely check the validity of motion +/// when the robot moves from 'q_previous' to 'q_current'. +/// During this checking obstacles are moving simultaneously. Finally, environment is updated within this function. /// @return Validity of motion. /// @note In reality, this motion would happen continuously during the execution of the current algorithm iteration. bool planning::trajectory::MotionValidity::check(const std::shared_ptr &q_previous, const std::shared_ptr &q_current) -{ - switch (traj_interpolation) - { - case planning::TrajectoryInterpolation::None: - return check_v1(q_previous, q_current); - - case planning::TrajectoryInterpolation::Spline: - return check_v2(); - - default: - return false; - } -} - -// In case traj_interpolation == "None", discretely check the validity of motion -// when the robot moves from 'q_previous' to 'q_current'. -bool planning::trajectory::MotionValidity::check_v1(const std::shared_ptr &q_previous, const std::shared_ptr &q_current) { // std::cout << "Checking the validity of motion while updating environment... \n"; - std::shared_ptr q_temp { nullptr }; + std::shared_ptr q { nullptr }; float dist { ss->getNorm(q_previous, q_current) }; - float delta_time { max_iter_time / num_checks }; + float delta_time { max_iter_time / TrajectoryConfig::NUM_VALIDITY_POINTS_CHECK }; - for (size_t num_check = 1; num_check <= num_checks; num_check++) + for (size_t num_check = 1; num_check <= TrajectoryConfig::NUM_VALIDITY_POINTS_CHECK; num_check++) { ss->env->updateEnvironment(delta_time); - q_temp = ss->interpolateEdge(q_previous, q_current, dist * num_check / num_checks, dist); - if (!ss->isValid(q_temp) || ss->robot->checkSelfCollision(q_temp)) - break; + + q = ss->interpolateEdge(q_previous, q_current, dist * num_check / TrajectoryConfig::NUM_VALIDITY_POINTS_CHECK, dist); + if (!ss->isValid(q) || ss->robot->checkSelfCollision(q)) + return false; + + path->emplace_back(q); } // std::cout << "Environment objects: \n"; // for (size_t i = 0; i < ss->env->getNumObjects(); i++) // std::cout << "i = " << i << " : " << ss->env->getObject(i)->getPosition().transpose() << "\n"; - path->emplace_back(q_temp); return true; } -// In case traj_interpolation == "Spline", discretely check the validity of motion -// when the robot moves over splines 'traj->spline_current' and 'traj->spline_next' during the current iteration. -bool planning::trajectory::MotionValidity::check_v2() +/// @brief In case traj_interpolation == "Spline" or "Reflexxes", discretely check the validity of motion +/// when the robot moves over the current trajectory during the current iteration. +/// During this checking obstacles are moving simultaneously. Finally, environment is updated within this function. +/// @return Validity of motion. +/// @note In reality, this motion would happen continuously during the execution of the current algorithm iteration. +bool planning::trajectory::MotionValidity::check(const std::vector &traj_points_current_iter) { // std::cout << "Checking the validity of motion while updating environment... \n"; - std::shared_ptr q_temp { nullptr }; - size_t num_checks1 = std::ceil((traj->spline_current->getTimeCurrent() - traj->spline_current->getTimeBegin()) * num_checks / max_iter_time); - size_t num_checks2 { num_checks - num_checks1 }; - float delta_time1 { (traj->spline_current->getTimeCurrent() - traj->spline_current->getTimeBegin()) / num_checks1 }; - float delta_time2 { (traj->spline_next->getTimeEnd() - traj->spline_next->getTimeCurrent()) / num_checks2 }; - float t { 0 }; - - // std::cout << "Current spline times: " << traj->spline_current->getTimeBegin() * 1000 << " [ms] \t" - // << traj->spline_current->getTimeCurrent() * 1000 << " [ms] \t" - // << traj->spline_current->getTimeEnd() * 1000 << " [ms] \n"; - // std::cout << "Next spline times: " << traj->spline_next->getTimeBegin() * 1000 << " [ms] \t" - // << traj->spline_next->getTimeCurrent() * 1000 << " [ms] \t" - // << traj->spline_next->getTimeEnd() * 1000 << " [ms] \n"; + std::shared_ptr q { nullptr }; + float delta_time { max_iter_time / TrajectoryConfig::NUM_VALIDITY_POINTS_CHECK }; - for (size_t num_check = 1; num_check <= num_checks; num_check++) + for (Eigen::VectorXf point : traj_points_current_iter) { - if (num_check <= num_checks1) - { - t = traj->spline_current->getTimeBegin() + num_check * delta_time1; - q_temp = ss->getNewState(traj->spline_current->getPosition(t)); - // std::cout << "t: " << t * 1000 << " [ms]\t from curr. spline \t" << q_temp << "\n"; - ss->env->updateEnvironment(delta_time1); - } - else - { - t = traj->spline_next->getTimeCurrent() + (num_check - num_checks1) * delta_time2; - q_temp = ss->getNewState(traj->spline_next->getPosition(t)); - // std::cout << "t: " << t * 1000 << " [ms]\t from next spline \t" << q_temp << "\n"; - ss->env->updateEnvironment(delta_time2); - } + ss->env->updateEnvironment(delta_time); - if (!ss->isValid(q_temp) || ss->robot->checkSelfCollision(q_temp)) + q = ss->getNewState(point); + if (!ss->isValid(q) || ss->robot->checkSelfCollision(q)) return false; + + path->emplace_back(q); } // std::cout << "Environment objects: \n"; // for (size_t i = 0; i < ss->env->getNumObjects(); i++) // std::cout << "i = " << i << " : " << ss->env->getObject(i)->getPosition().transpose() << "\n"; - // std::cout << q_temp << "\n"; - path->emplace_back(q_temp); return true; -} +} \ No newline at end of file From 317612ae3c44b48118e96818cbf72a0cb9f7c96f Mon Sep 17 00:00:00 2001 From: Nermin Date: Thu, 2 Oct 2025 18:09:48 +0200 Subject: [PATCH 16/40] Therefore, 'UpdatingState' class is also modified. --- include/planners/trajectory/UpdatingState.h | 6 +- src/planners/drbt/DRGBT.cpp | 32 ++++-- src/planners/rrtx/RRTx.cpp | 15 +-- src/planners/trajectory/UpdatingState.cpp | 105 +++++++++++++------- 4 files changed, 103 insertions(+), 55 deletions(-) diff --git a/include/planners/trajectory/UpdatingState.h b/include/planners/trajectory/UpdatingState.h index 938b324d..7fa76243 100644 --- a/include/planners/trajectory/UpdatingState.h +++ b/include/planners/trajectory/UpdatingState.h @@ -27,7 +27,7 @@ namespace planning::trajectory const std::shared_ptr q_next_, base::State::Status &status); void update(std::shared_ptr &q_previous, std::shared_ptr &q_current, const std::shared_ptr q_next_, const std::shared_ptr q_next_reached_, base::State::Status &status); - inline void setTraj(const std::shared_ptr &traj_) { traj = traj_; } + inline void setTrajectory(const std::shared_ptr &traj_) { traj = traj_; } inline void setGuaranteedSafeMotion(bool guaranteed_safe_motion_) { guaranteed_safe_motion = guaranteed_safe_motion_; } inline void setNonZeroFinalVel(bool non_zero_final_vel_) { non_zero_final_vel = non_zero_final_vel_; } inline void setMaxRemainingIterTime(float max_remaining_iter_time_) { max_remaining_iter_time = max_remaining_iter_time_; } @@ -51,7 +51,7 @@ namespace planning::trajectory float max_iter_time; // Maximal iteration time in [s] // Additional info (not mandatory to be set): - std::shared_ptr traj; + std::shared_ptr traj; // Trajectory which is generated using the proposed approach from RPMPLv2 library bool all_robot_vel_same; // Whether all joint velocities are the same bool guaranteed_safe_motion; // Whether robot motion is surely safe for environment bool non_zero_final_vel; // Whether final spline velocity can be non-zero (available only when 'guaranteed_safe_motion' is false) @@ -62,7 +62,7 @@ namespace planning::trajectory // It should always be false when simulation pacing is used, since then a time measuring will not be correct! float remaining_time; // Return value of 'update_v2' function. Remaining time in [s] after which the new spline 'traj->spline_next' - // will become active (if 'planning::TrajectoryInterpolation::Spline' is used). + // will become active (if 'planning::TrajectoryInterpolation::None' is not used). std::shared_ptr q_next; std::shared_ptr q_next_reached; diff --git a/src/planners/drbt/DRGBT.cpp b/src/planners/drbt/DRGBT.cpp index 4b4ac77b..7d7dae6c 100644 --- a/src/planners/drbt/DRGBT.cpp +++ b/src/planners/drbt/DRGBT.cpp @@ -31,22 +31,26 @@ planning::drbt::DRGBT::DRGBT(const std::shared_ptr ss_, const max_edge_length = ss->robot->getMaxVel().norm() * DRGBTConfig::MAX_ITER_TIME; updating_state = std::make_shared - (ss, DRGBTConfig::TRAJECTORY_INTERPOLATION, DRGBTConfig::MAX_ITER_TIME); + (ss, DRGBTConfig::TRAJECTORY_INTERPOLATION, DRGBTConfig::MAX_ITER_TIME); updating_state->setGuaranteedSafeMotion(DRGBTConfig::GUARANTEED_SAFE_MOTION); updating_state->setMaxRemainingIterTime(DRGBTConfig::MAX_ITER_TIME - DRGBTConfig::MAX_TIME_TASK1); updating_state->setMeasureTime(false); updating_state->setDRGBTinstance(this); motion_validity = std::make_shared - (ss, DRGBTConfig::TRAJECTORY_INTERPOLATION, DRGBTConfig::RESOLUTION_COLL_CHECK, &path, DRGBTConfig::MAX_ITER_TIME); + (ss, DRGBTConfig::RESOLUTION_COLL_CHECK, DRGBTConfig::MAX_ITER_TIME, &path); - traj = nullptr; - if (DRGBTConfig::TRAJECTORY_INTERPOLATION == planning::TrajectoryInterpolation::Spline) + switch (DRGBTConfig::TRAJECTORY_INTERPOLATION) { + case planning::TrajectoryInterpolation::None: + traj = nullptr; + break; + + case planning::TrajectoryInterpolation::Spline: traj = std::make_shared(ss, q_current, DRGBTConfig::MAX_ITER_TIME); traj->setMaxRemainingIterTime(DRGBTConfig::MAX_ITER_TIME - DRGBTConfig::MAX_TIME_TASK1); - updating_state->setTraj(traj); - motion_validity->setTraj(traj); + updating_state->setTrajectory(traj); + break; } // std::cout << "DRGBT planner initialized! \n"; @@ -122,7 +126,19 @@ bool planning::drbt::DRGBT::solve() // ------------------------------------------------------------------------------- // // Update environment and check if the collision occurs - if (!motion_validity->check(q_previous, q_current)) + bool is_valid { false }; + switch (DRGBTConfig::TRAJECTORY_INTERPOLATION) + { + case planning::TrajectoryInterpolation::None: + is_valid = motion_validity->check(q_previous, q_current); + break; + + case planning::TrajectoryInterpolation::Spline: + is_valid = motion_validity->check(traj->traj_points_current_iter); + break; + } + + if (!is_valid) { std::cout << "*************** Collision has been occurred!!! *************** \n"; planner_info->setSuccessState(false); @@ -245,7 +261,7 @@ void planning::drbt::DRGBT::generateGBur() size_t max_num_attempts {}; float time_elapsed {}; float max_time { DRGBTConfig::MAX_TIME_TASK1 }; - if (DRGBTConfig::TRAJECTORY_INTERPOLATION == planning::TrajectoryInterpolation::Spline) + if (DRGBTConfig::TRAJECTORY_INTERPOLATION != planning::TrajectoryInterpolation::None) max_time -= DRGBTConfig::GUARANTEED_SAFE_MOTION ? TrajectoryConfig::MAX_TIME_COMPUTE_SAFE : TrajectoryConfig::MAX_TIME_COMPUTE_REGULAR; planner_info->setTask1Interrupted(false); diff --git a/src/planners/rrtx/RRTx.cpp b/src/planners/rrtx/RRTx.cpp index 70463e4e..064c9df5 100644 --- a/src/planners/rrtx/RRTx.cpp +++ b/src/planners/rrtx/RRTx.cpp @@ -39,17 +39,20 @@ planning::rrtx::RRTx::RRTx(const std::shared_ptr ss_, const st planner_info->setNumStates(1); updating_state = std::make_shared - (ss, RRTxConfig::TRAJECTORY_INTERPOLATION, RRTxConfig::MAX_ITER_TIME); + (ss, RRTxConfig::TRAJECTORY_INTERPOLATION, RRTxConfig::MAX_ITER_TIME); motion_validity = std::make_shared - (ss, RRTxConfig::TRAJECTORY_INTERPOLATION, RRTxConfig::RESOLUTION_COLL_CHECK, &path, RRTxConfig::MAX_ITER_TIME); + (ss, RRTxConfig::RESOLUTION_COLL_CHECK, RRTxConfig::MAX_ITER_TIME, &path); - traj = nullptr; - if (RRTxConfig::TRAJECTORY_INTERPOLATION == planning::TrajectoryInterpolation::Spline) + switch (RRTxConfig::TRAJECTORY_INTERPOLATION) { + case planning::TrajectoryInterpolation::Spline: traj = std::make_shared(ss, q_current, RRTxConfig::MAX_ITER_TIME); - updating_state->setTraj(traj); - motion_validity->setTraj(traj); + updating_state->setTrajectory(traj); + break; + + default: + traj = nullptr; } RRTConnectConfig::EPS_STEP = RRTxConfig::EPS_STEP; diff --git a/src/planners/trajectory/UpdatingState.cpp b/src/planners/trajectory/UpdatingState.cpp index 09e2044e..118571e7 100644 --- a/src/planners/trajectory/UpdatingState.cpp +++ b/src/planners/trajectory/UpdatingState.cpp @@ -42,9 +42,6 @@ void planning::trajectory::UpdatingState::update(std::shared_ptr &q case planning::TrajectoryInterpolation::Spline: update_v2(q_previous, q_current, q_next_, q_next_, status); break; - - default: - break; } } @@ -60,9 +57,6 @@ void planning::trajectory::UpdatingState::update(std::shared_ptr &q case planning::TrajectoryInterpolation::Spline: update_v2(q_previous, q_current, q_next_, q_next_reached_, status); break; - - default: - break; } } @@ -121,7 +115,7 @@ void planning::trajectory::UpdatingState::update_v1(std::shared_ptr /// Compute a new spline 'traj->spline_next', or remain 'traj->spline_current'. /// Move 'q_current' towards 'q_next_reached' while following 'traj->spline_next'. /// 'q_current' will be updated to a robot position from the end of current iteration. -/// @note The new spline will be computed in a way that all constraints on robot's maximal velocity, +/// @note The new trajectory will be computed in a way that all constraints on robot's maximal velocity, /// acceleration and jerk are surely always satisfied. /// @note If 'q_next_reached' is not relevant in the algorithm, pass 'q_next' instead of it. void planning::trajectory::UpdatingState::update_v2(std::shared_ptr &q_previous, std::shared_ptr &q_current, @@ -132,42 +126,41 @@ void planning::trajectory::UpdatingState::update_v2(std::shared_ptr q_next_reached = q_next_reached_; traj->spline_current = traj->spline_next; - float t_spline_max { (guaranteed_safe_motion ? TrajectoryConfig::MAX_TIME_COMPUTE_SAFE : TrajectoryConfig::MAX_TIME_COMPUTE_REGULAR) - - TrajectoryConfig::MAX_TIME_PUBLISH * measure_time }; + float t_traj_max { (guaranteed_safe_motion ? TrajectoryConfig::MAX_TIME_COMPUTE_SAFE : TrajectoryConfig::MAX_TIME_COMPUTE_REGULAR) - + TrajectoryConfig::MAX_TIME_PUBLISH * measure_time }; float t_iter { getElapsedTime() }; - if (max_iter_time - max_remaining_iter_time - t_iter < t_spline_max) - t_spline_max = max_iter_time - max_remaining_iter_time - t_iter; + if (max_iter_time - max_remaining_iter_time - t_iter < t_traj_max) + t_traj_max = max_iter_time - max_remaining_iter_time - t_iter; - float t_iter_remain { max_iter_time - t_iter - t_spline_max }; - float t_spline_current { measure_time ? - traj->spline_current->getTimeCurrent(true) + t_spline_max : - traj->spline_current->getTimeEnd() + t_iter + t_spline_max }; + float t_iter_remain { max_iter_time - t_iter - t_traj_max }; + float t_traj_current { measure_time ? + traj->spline_current->getTimeCurrent(true) + t_traj_max : + traj->spline_current->getTimeEnd() + t_iter + t_traj_max }; traj->spline_current->setTimeBegin(traj->spline_current->getTimeEnd()); - traj->spline_current->setTimeCurrent(t_spline_current); + traj->spline_current->setTimeCurrent(t_traj_current); - // std::cout << "Iter. time: " << t_iter * 1000 << " [ms] \n"; - // std::cout << "Max. spline time: " << t_spline_max * 1000 << " [ms] \n"; - // std::cout << "Remain. time: " << t_iter_remain * 1000 << " [ms] \n"; - // std::cout << "Begin spline time: " << traj->spline_current->getTimeBegin() * 1000 << " [ms] \n"; - // std::cout << "Curr. spline time: " << t_spline_current * 1000 << " [ms] \n"; + // std::cout << "Iter. time: " << t_iter * 1000 << " [ms] \n"; + // std::cout << "Max. trajectory time: " << t_traj_max * 1000 << " [ms] \n"; + // std::cout << "Remain. time: " << t_iter_remain * 1000 << " [ms] \n"; + // std::cout << "Begin trajectory time: " << traj->spline_current->getTimeBegin() * 1000 << " [ms] \n"; + // std::cout << "Curr. trajectory time: " << t_traj_current * 1000 << " [ms] \n"; // ----------------------------------------------------------------------------------------- // bool traj_computed { false }; - float t_spline_remain {}; - Eigen::VectorXf current_pos { traj->spline_current->getPosition(t_spline_current) }; - Eigen::VectorXf current_vel { traj->spline_current->getVelocity(t_spline_current) }; - Eigen::VectorXf current_acc { traj->spline_current->getAcceleration(t_spline_current) }; - + float t_traj_remain {}; + Eigen::VectorXf current_pos { traj->spline_current->getPosition(t_traj_current) }; + Eigen::VectorXf current_vel { traj->spline_current->getVelocity(t_traj_current) }; + Eigen::VectorXf current_acc { traj->spline_current->getAcceleration(t_traj_current) }; // std::cout << "Curr. pos: " << current_pos.transpose() << "\n"; // std::cout << "Curr. vel: " << current_vel.transpose() << "\n"; // std::cout << "Curr. acc: " << current_acc.transpose() << "\n"; do { - t_spline_remain = t_spline_max - (getElapsedTime() - t_iter); - // std::cout << "t_spline_remain: " << t_spline_remain * 1e3 << " [ms] \n"; - if (t_spline_remain < 0) + t_traj_remain = t_traj_max - (getElapsedTime() - t_iter); + // std::cout << "t_traj_remain: " << t_traj_remain * 1e3 << " [ms] \n"; + if (t_traj_remain < 0) break; float step { std::max(ss->robot->getMaxVel().norm() * max_iter_time, @@ -181,28 +174,27 @@ void planning::trajectory::UpdatingState::update_v2(std::shared_ptr // std::cout << "q_next_reached: " << q_next_reached << "\n"; if (guaranteed_safe_motion) - traj_computed = traj->computeSafe(current_pos, current_vel, current_acc, t_iter_remain, t_spline_remain); + traj_computed = traj->computeSafe(current_pos, current_vel, current_acc, t_iter_remain, t_traj_remain); else { if (traj->spline_current->isFinalConf(q_target->getCoord())) // Spline to such 'q_target' already exists! break; - traj_computed = traj->computeRegular(current_pos, current_vel, current_acc, t_iter_remain, t_spline_remain, non_zero_final_vel); + traj_computed = traj->computeRegular(current_pos, current_vel, current_acc, t_iter_remain, t_traj_remain, non_zero_final_vel); } } while (!traj_computed && invokeChangeNextState()); - // std::cout << "Elapsed time for spline computing: " << (getElapsedTime() - t_iter) * 1e3 << " [ms] \n"; if (traj_computed) { - // std::cout << "New spline is computed! \n"; - traj->spline_current->setTimeEnd(t_spline_current); + // std::cout << "New trajectory is computed! \n"; + traj->spline_current->setTimeEnd(t_traj_current); traj->spline_next->setTimeEnd(t_iter_remain); } else { - // std::cout << "Continuing with the previous spline! \n"; + // std::cout << "Continuing with the previous trajectory! \n"; traj->spline_next = traj->spline_current; - traj->spline_next->setTimeEnd(t_spline_current + t_iter_remain); + traj->spline_next->setTimeEnd(t_traj_current + t_iter_remain); } // traj->recordTrajectory(traj_computed); // Only for debugging @@ -210,20 +202,57 @@ void planning::trajectory::UpdatingState::update_v2(std::shared_ptr q_current = ss->getNewState(traj->spline_next->getPosition(traj->spline_next->getTimeEnd())); // Current robot position at the end of iteration if (status != base::State::Status::Trapped) { - if (ss->getNorm(q_current, q_next) < // 'q_next' must be reached within the computed radius, and not only 'q_next_reached' + if (ss->getNorm(q_current, q_next) <= // 'q_next' must be reached within the computed radius, and not only 'q_next_reached' traj->spline_next->getVelocity(traj->spline_next->getTimeEnd()).norm() / ss->robot->getMaxVel().norm() * TrajectoryConfig::MAX_RADIUS) status = base::State::Status::Reached; else status = base::State::Status::Advanced; } + // std::cout << "Elapsed time for trajectory computing: " << (getElapsedTime() - t_iter) * 1e3 << " [ms] \n"; // std::cout << "Spline next: \n" << traj->spline_next << "\n"; // std::cout << "q_current: " << q_current << "\n"; // std::cout << "Status: " << (status == base::State::Status::Advanced ? "Advanced" : "") // << (status == base::State::Status::Trapped ? "Trapped" : "") // << (status == base::State::Status::Reached ? "Reached" : "") << "\n"; + // std::cout << "Current spline times: " << traj->spline_current->getTimeBegin() * 1000 << " [ms] \t" + // << traj->spline_current->getTimeCurrent() * 1000 << " [ms] \t" + // << traj->spline_current->getTimeEnd() * 1000 << " [ms] \n"; + // std::cout << "Next spline times: " << traj->spline_next->getTimeBegin() * 1000 << " [ms] \t" + // << traj->spline_next->getTimeCurrent() * 1000 << " [ms] \t" + // << traj->spline_next->getTimeEnd() * 1000 << " [ms] \n"; + + // ----------------------------------------------------------------------------------------- // + // Store trajectory points from the current iteration to be validated later within 'MotionValidity' + traj->traj_points_current_iter.clear(); + size_t num_checks1 = std::floor((traj->spline_current->getTimeCurrent() - traj->spline_current->getTimeBegin()) / + max_iter_time * TrajectoryConfig::NUM_VALIDITY_POINTS_CHECK ); + float delta_time1 { (traj->spline_current->getTimeCurrent() - traj->spline_current->getTimeBegin()) / num_checks1 }; + float t { 0 }; + for (size_t num_check = 1; num_check <= num_checks1; num_check++) + { + t = traj->spline_current->getTimeBegin() + num_check * delta_time1; + traj->traj_points_current_iter.emplace_back(traj->spline_current->getPosition(t)); + // std::cout << "t: " << t * 1000 << " [ms]\t" + // << "pos: " << traj->spline_current->getPosition(t).transpose() << "\t" + // << "vel: " << traj->spline_current->getVelocity(t).transpose() << "\t" + // << "acc: " << traj->spline_current->getAcceleration(t).transpose() << "\n"; + } + + size_t num_checks2 { TrajectoryConfig::NUM_VALIDITY_POINTS_CHECK - num_checks1 }; + float delta_time2 { (traj->spline_next->getTimeEnd() - traj->spline_next->getTimeCurrent()) / num_checks2 }; + for (size_t num_check = 1; num_check <= num_checks2; num_check++) + { + t = traj->spline_next->getTimeCurrent() + num_check * delta_time2; + traj->traj_points_current_iter.emplace_back(traj->spline_next->getPosition(t)); + // std::cout << "t: " << t * 1000 << " [ms]\t" + // << "pos: " << traj->spline_next->getPosition(t).transpose() << "\t" + // << "vel: " << traj->spline_next->getVelocity(t).transpose() << "\t" + // << "acc: " << traj->spline_next->getAcceleration(t).transpose() << "\n"; + } + // ----------------------------------------------------------------------------------------- // - remaining_time = t_spline_max + TrajectoryConfig::MAX_TIME_PUBLISH * measure_time - (getElapsedTime() - t_iter); + remaining_time = t_traj_max + TrajectoryConfig::MAX_TIME_PUBLISH * measure_time - (getElapsedTime() - t_iter); } // This function will change 'q_next' and 'q_next_reached' From e46425ebf481900910265563c811837272b04ab0 Mon Sep 17 00:00:00 2001 From: Nermin Date: Thu, 2 Oct 2025 18:11:06 +0200 Subject: [PATCH 17/40] Default values reverted. --- data/configurations/configuration_drgbt.yaml | 4 ++-- .../scenario_random_obstacles.yaml | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/data/configurations/configuration_drgbt.yaml b/data/configurations/configuration_drgbt.yaml index 0c18b266..6b8cf173 100644 --- a/data/configurations/configuration_drgbt.yaml +++ b/data/configurations/configuration_drgbt.yaml @@ -1,5 +1,5 @@ MAX_NUM_ITER: 1000000 # Maximal number of algorithm iterations -MAX_ITER_TIME: 0.005 # Maximal runtime of a single iteration in [s] +MAX_ITER_TIME: 0.050 # Maximal runtime of a single iteration in [s] MAX_PLANNING_TIME: 10 # Maximal algorithm runtime in [s] INIT_HORIZON_SIZE: 10 # Initial horizon size TRESHOLD_WEIGHT: 0.5 # Treshold for the replanning assessment. Range: between 0 and 1 @@ -7,7 +7,7 @@ D_CRIT: 0.05 # Critical distance in W-space to compute c MAX_NUM_MODIFY_ATTEMPTS: 10 # Maximal number of attempts when modifying bad or critical states STATIC_PLANNER_TYPE: "RGBMT*" # Name of a static planner (for obtaining the predefined path). Default: "RGBMT*" or "RGBT-Connect" REAL_TIME_SCHEDULING: "FPS" # "FPS" - Fixed Priority Scheduling; "None" - Without real-time scheduling -MAX_TIME_TASK1: 0.005 # Maximal time in [s] which Task 1 can take from the processor +MAX_TIME_TASK1: 0.050 # Maximal time in [s] which Task 1 can take from the processor RESOLUTION_COLL_CHECK: 0.01 # Perform collision check when obstacle moves at most 'RESOLUTION_COLL_CHECK' in [m] TRAJECTORY_INTERPOLATION: "Spline" # Method for interpolation of trajectory: 'None' or 'Spline' GUARANTEED_SAFE_MOTION: false # Whether robot motion is surely safe for environment \ No newline at end of file diff --git a/data/xarm6/scenario_random_obstacles/scenario_random_obstacles.yaml b/data/xarm6/scenario_random_obstacles/scenario_random_obstacles.yaml index ff3ae3bf..4345b258 100644 --- a/data/xarm6/scenario_random_obstacles/scenario_random_obstacles.yaml +++ b/data/xarm6/scenario_random_obstacles/scenario_random_obstacles.yaml @@ -6,8 +6,8 @@ environment: min_dist_tol: 0.05 # Minimal distance tolerance for a static obstacle to not be included into a dynamic scene random_obstacles: - init_num: 10 # Number of random obstacles to start with the testing - max_num: 10 # Maximal number of random obstacles to be added + init_num: 0 # Number of random obstacles to start with the testing + max_num: 50 # Maximal number of random obstacles to be added max_vel: 1.6 # Maximal velocity of each obstacle in [m/s] max_acc: 0 # Maximal acceleration of each obstacle in [m/s²] dim: [0.01, 0.01, 0.01] # Dimensions of each random obstacle in [m] @@ -33,6 +33,6 @@ robot: testing: init_num: 1 # Number of testing to start with init_num_success: 0 # Initial number of already achieved successful tests - max_num: 100 # Maximal number of tests that should be carried out + max_num: 1000 # Maximal number of tests that should be carried out reach_successful_tests: false # If true, run totally 'max_num' successful tests motion_type: "light_directions" # Options: "straight", "circular", "two_tunnels", "random_directions", "light_directions". See 'Environment.cpp' for more details. From e6274162e1313585faa9c28ac9970d58e4cc000d Mon Sep 17 00:00:00 2001 From: Nermin Date: Thu, 2 Oct 2025 18:35:11 +0200 Subject: [PATCH 18/40] New dynamic planner is created: Real-time RGBT alongside all corresponding files. --- apps/CMakeLists.txt | 6 + apps/test_drgbt_random_obstacles.cpp | 46 +-- apps/test_rt_rgbt.cpp | 220 +++++++++++++ .../configurations/configuration_rt_rgbt.yaml | 6 + include/configurations/ConfigurationReader.h | 1 + include/configurations/RT_RGBTConfig.h | 23 ++ include/planners/PlanningTypes.h | 15 +- include/planners/drbt/RT_RGBT.h | 44 +++ src/configurations/ConfigurationReader.cpp | 33 ++ src/configurations/RT_RGBTConfig.cpp | 8 + src/planners/PlanningTypes.cpp | 4 + src/planners/drbt/RT_RGBT.cpp | 299 ++++++++++++++++++ 12 files changed, 684 insertions(+), 21 deletions(-) create mode 100644 apps/test_rt_rgbt.cpp create mode 100644 data/configurations/configuration_rt_rgbt.yaml create mode 100644 include/configurations/RT_RGBTConfig.h create mode 100644 include/planners/drbt/RT_RGBT.h create mode 100644 src/configurations/RT_RGBTConfig.cpp create mode 100644 src/planners/drbt/RT_RGBT.cpp diff --git a/apps/CMakeLists.txt b/apps/CMakeLists.txt index 0d3b628d..72e6fc72 100644 --- a/apps/CMakeLists.txt +++ b/apps/CMakeLists.txt @@ -69,6 +69,11 @@ target_compile_features(test_rrtx_random_obstacles PRIVATE cxx_std_17) target_link_libraries(test_rrtx_random_obstacles PUBLIC rpmpl_library ${PROJECT_LIBRARIES}) target_include_directories(test_rrtx_random_obstacles PUBLIC ${PROJECT_SOURCE_DIR}/apps) +add_executable(test_rt_rgbt test_rt_rgbt.cpp) +target_compile_features(test_rt_rgbt PRIVATE cxx_std_17) +target_link_libraries(test_rt_rgbt PUBLIC rpmpl_library ${PROJECT_LIBRARIES}) +target_include_directories(test_rt_rgbt PUBLIC ${PROJECT_SOURCE_DIR}/apps) + add_executable(test_trajectory test_trajectory.cpp) target_compile_features(test_trajectory PRIVATE cxx_std_17) target_link_libraries(test_trajectory PUBLIC rpmpl_library ${PROJECT_LIBRARIES}) @@ -101,6 +106,7 @@ install(TARGETS test_rgbmtstar test_rrtx test_rrtx_random_obstacles + test_rt_rgbt test_trajectory generate_random_scenarios generate_dataset diff --git a/apps/test_drgbt_random_obstacles.cpp b/apps/test_drgbt_random_obstacles.cpp index d9a74709..88f576a5 100644 --- a/apps/test_drgbt_random_obstacles.cpp +++ b/apps/test_drgbt_random_obstacles.cpp @@ -34,9 +34,10 @@ int main(int argc, char **argv) if (clp != 0) return clp; const std::string project_path { getProjectPath() }; - const std::string directory_path { project_path + scenario_file_path.substr(0, scenario_file_path.find_last_of("/\\")) + "/DRGBT_data" }; - std::filesystem::create_directory(directory_path); ConfigurationReader::initConfiguration(project_path); + const std::string directory_path { project_path + scenario_file_path.substr(0, scenario_file_path.find_last_of("/\\")) + + "/DRGBT_data_" + planning::trajectory_interpolation_map2[DRGBTConfig::TRAJECTORY_INTERPOLATION] }; + std::filesystem::create_directory(directory_path); YAML::Node node { YAML::LoadFile(project_path + scenario_file_path) }; YAML::Node node2 { YAML::LoadFile(project_path + random_scenarios_path) }; @@ -64,7 +65,8 @@ int main(int argc, char **argv) std::ofstream output_file {}; if (init_num_test == 1) { - output_file.open(directory_path + "/results" + std::to_string(init_num_obs) + ".log", std::ofstream::out); + output_file.open(directory_path + "/results_" + std::to_string(init_num_obs) + "obs_" + + std::to_string(size_t(DRGBTConfig::MAX_ITER_TIME * 1000)) + "ms.log", std::ofstream::out); output_file << "Using scenario: " << scenario_file_path << std::endl; output_file << "Dynamic planner: " << planning::PlannerType::DRGBT << std::endl; output_file << "Static planner for replanning: " << DRGBTConfig::STATIC_PLANNER_TYPE << std::endl; @@ -99,6 +101,7 @@ int main(int argc, char **argv) std::vector path_lengths {}; size_t num_test { init_num_test }; size_t num_success_tests { init_num_success_test }; + float num_real_success_tests { init_num_success_test }; while (true) { @@ -158,11 +161,13 @@ int main(int argc, char **argv) planner = std::make_unique(ss, scenario.getStart(), scenario.getGoal()); bool result { planner->solve() }; + num_real_success_tests += 1 - (goal - planner->getPath().back()->getCoord()).norm() / (goal - start).norm(); LOG(INFO) << planner->getPlannerType() << " planning finished with " << (result ? "SUCCESS!" : "FAILURE!"); + LOG(INFO) << "Real success: " << 1 - (goal - planner->getPath().back()->getCoord()).norm() / (goal - start).norm(); LOG(INFO) << "Number of iterations: " << planner->getPlannerInfo()->getNumIterations(); LOG(INFO) << "Algorithm time: " << planner->getPlannerInfo()->getPlanningTime() << " [s]"; - LOG(INFO) << "Task 1 interrupted: " << (planner->getPlannerInfo()->getTask1Interrupted() ? "true" : "false"); + // LOG(INFO) << "Task 1 interrupted: " << (planner->getPlannerInfo()->getTask1Interrupted() ? "true" : "false"); // LOG(INFO) << "Planner data is saved at: " << directory_path + "/test" + std::to_string(init_num_obs) + "_" + std::to_string(num_test) + ".log"; // planner->outputPlannerData(directory_path + "/test" + std::to_string(init_num_obs) + "_" + std::to_string(num_test) + ".log"); @@ -183,36 +188,40 @@ int main(int argc, char **argv) num_success_tests++; } - output_file.open(directory_path + "/results" + std::to_string(init_num_obs) + ".log", std::ofstream::app); + output_file.open(directory_path + "/results_" + std::to_string(init_num_obs) + "obs_" + + std::to_string(size_t(DRGBTConfig::MAX_ITER_TIME * 1000)) + "ms.log", std::ofstream::app); output_file << "Test number: " << num_test << std::endl; output_file << "Number of successful tests: " << num_success_tests << " of " << num_test << " = " << 100.0 * num_success_tests / num_test << " %" << std::endl; output_file << "Success:\n" << result << std::endl; + output_file << "Real success:\n" << 1 - (goal - planner->getPath().back()->getCoord()).norm() / (goal - start).norm() << std::endl; output_file << "Number of iterations:\n" << planner->getPlannerInfo()->getNumIterations() << std::endl; output_file << "Algorithm execution time [s]:\n" << planner->getPlannerInfo()->getPlanningTime() << std::endl; output_file << "Path length [rad]:\n" << (result ? path_length : INFINITY) << std::endl; - output_file << "Task 1 interrupted:\n" << planner->getPlannerInfo()->getTask1Interrupted() << std::endl; + // output_file << "Task 1 interrupted:\n" << planner->getPlannerInfo()->getTask1Interrupted() << std::endl; // if (result) // { - // std::vector> routine_times { planner->getPlannerInfo()->getRoutineTimes() }; - // for (size_t idx = 0; idx < routines.size(); idx++) - // { - // // LOG(INFO) << "Routine " << routines[idx]; - // // LOG(INFO) << "\tAverage time: " << getMean(routine_times[idx]) << " +- " << getStd(routine_times[idx]); - // // LOG(INFO) << "\tMaximal time: " << *std::max_element(routine_times[idx].begin(), routine_times[idx].end()); - // // LOG(INFO) << "\tData size: " << routine_times[idx].size(); + // std::vector> routine_times { planner->getPlannerInfo()->getRoutineTimes() }; + // for (size_t idx = 0; idx < routines.size(); idx++) + // { + // LOG(INFO) << "Routine " << routines[idx]; + // LOG(INFO) << "\tAverage time: " << getMean(routine_times[idx]) << " +- " << getStd(routine_times[idx]); + // LOG(INFO) << "\tMaximal time: " << *std::max_element(routine_times[idx].begin(), routine_times[idx].end()); + // LOG(INFO) << "\tData size: " << routine_times[idx].size(); - // output_file << "Routine " << routines[idx] << " times: " << std::endl; - // for (float t : routine_times[idx]) - // output_file << t << std::endl; - // } + // output_file << "Routine " << routines[idx] << " times: " << std::endl; + // for (float t : routine_times[idx]) + // output_file << t << std::endl; + // } // } output_file << "--------------------------------------------------------------------\n"; output_file.close(); - LOG(INFO) << "Number of successful tests: " << num_success_tests << " of " << num_test + LOG(INFO) << "Number of successful tests: " << num_success_tests << " of " << num_test << " = " << 100.0 * num_success_tests / num_test << " %"; + LOG(INFO) << "Number of real successful tests: " << num_real_success_tests << " of " << num_test + << " = " << 100.0 * num_real_success_tests / num_test << " %"; LOG(INFO) << "--------------------------------------------------------------------\n\n"; } catch (std::exception &e) @@ -230,6 +239,7 @@ int main(int argc, char **argv) init_num_obs += (init_num_obs > 0) ? std::pow(10, std::floor(std::log10(init_num_obs))) : 1; LOG(INFO) << "Success rate: " << 100.0 * num_success_tests / (num_test - 1) << " [%]"; + LOG(INFO) << "Real success rate: " << 100.0 * num_real_success_tests / (num_test - 1) << " [%]"; LOG(INFO) << "Average algorithm execution time: " << getMean(alg_times) << " +- " << getStd(alg_times) << " [s]"; LOG(INFO) << "Average iteration execution time: " << getMean(iter_times) * 1e3 << " +- " << getStd(iter_times) * 1e3 << " [ms]"; LOG(INFO) << "Average number of iterations: " << getMean(num_iters) << " +- " << getStd(num_iters); diff --git a/apps/test_rt_rgbt.cpp b/apps/test_rt_rgbt.cpp new file mode 100644 index 00000000..f93bf8c4 --- /dev/null +++ b/apps/test_rt_rgbt.cpp @@ -0,0 +1,220 @@ +#include "RT_RGBT.h" +#include "ConfigurationReader.h" +#include "CommonFunctions.h" + +int main(int argc, char **argv) +{ + std::string scenario_file_path + { + // "/data/planar_2dof/scenario_random_obstacles/scenario_random_obstacles.yaml" + + "/data/xarm6/scenario_random_obstacles/scenario_random_obstacles.yaml" + + // "/data/spatial_10dof/scenario_random_obstacles/scenario_random_obstacles.yaml" + // "/data/spatial_14dof/scenario_random_obstacles/scenario_random_obstacles.yaml" + // "/data/spatial_18dof/scenario_random_obstacles/scenario_random_obstacles.yaml" + // "/data/spatial_22dof/scenario_random_obstacles/scenario_random_obstacles.yaml" + }; + const std::string random_scenarios_path { scenario_file_path.substr(0, scenario_file_path.find_last_of("/\\")) + "/random_scenarios.yaml" }; + + std::vector routines // Routines of which the time executions are stored + { + "update [ms]" // 0 + }; + + // -------------------------------------------------------------------------------------- // + + initGoogleLogging(argv); + int clp = commandLineParser(argc, argv, scenario_file_path); + if (clp != 0) return clp; + + const std::string project_path { getProjectPath() }; + ConfigurationReader::initConfiguration(project_path); + const std::string directory_path { project_path + scenario_file_path.substr(0, scenario_file_path.find_last_of("/\\")) + + "/RT_RGBT_data_" + planning::trajectory_interpolation_map2[RT_RGBTConfig::TRAJECTORY_INTERPOLATION] }; + std::filesystem::create_directory(directory_path); + YAML::Node node { YAML::LoadFile(project_path + scenario_file_path) }; + YAML::Node node2 { YAML::LoadFile(project_path + random_scenarios_path) }; + + size_t init_num_obs { node["random_obstacles"]["init_num"].as() }; + const size_t max_num_obs { node["random_obstacles"]["max_num"].as() }; + const float max_vel_obs { node["random_obstacles"]["max_vel"].as() }; + const float max_acc_obs { node["random_obstacles"]["max_acc"].as() }; + Eigen::Vector3f obs_dim {}; + for (size_t i = 0; i < 3; i++) + obs_dim(i) = node["random_obstacles"]["dim"][i].as(); + + [[maybe_unused]] float min_dist_start_goal { node["robot"]["min_dist_start_goal"].as() }; + size_t init_num_test { node["testing"]["init_num"].as() }; + size_t init_num_success_test { node["testing"]["init_num_success"].as() }; + const size_t max_num_tests { node["testing"]["max_num"].as() }; + bool reach_successful_tests { node["testing"]["reach_successful_tests"].as() }; + + while (init_num_obs <= max_num_obs) + { + LOG(INFO) << "Number of obstacles " << init_num_obs << " of " << max_num_obs; + + std::ofstream output_file {}; + if (init_num_test == 1) + { + output_file.open(directory_path + "/results_" + std::to_string(init_num_obs) + "obs_" + + std::to_string(size_t(RT_RGBTConfig::MAX_ITER_TIME * 1000)) + "ms.log", std::ofstream::out); + output_file << "Using scenario: " << scenario_file_path << std::endl; + output_file << "Dynamic planner: " << planning::PlannerType::RT_RGBT << std::endl; + output_file << "Maximal algorithm time [s]: " << RT_RGBTConfig::MAX_PLANNING_TIME << std::endl; + output_file << "Resolution for collision checking [m]: " << RT_RGBTConfig::RESOLUTION_COLL_CHECK << std::endl; + output_file << "Trajectory interpolation: " << RT_RGBTConfig::TRAJECTORY_INTERPOLATION << std::endl; + output_file << "Maximal iteration time [s]: " << RT_RGBTConfig::MAX_ITER_TIME << std::endl; + output_file << "--------------------------------------------------------------------\n"; + output_file << "Number of obstacles: " << init_num_obs << std::endl; + output_file << "Obstacles motion: " << "random" << std::endl; + output_file << "Maximal velocity of each obstacle [m/s]: " << max_vel_obs << std::endl; + output_file << "Maximal acceleration of each obstacle [m/s²]: " << max_acc_obs << std::endl; + output_file << "--------------------------------------------------------------------\n"; + output_file.close(); + } + + std::vector alg_times {}; + std::vector iter_times {}; + std::vector num_iters {}; + std::vector path_lengths {}; + size_t num_test { init_num_test }; + size_t num_success_tests { init_num_success_test }; + float num_real_success_tests { init_num_success_test }; + + while (true) + { + try + { + scenario::Scenario scenario(scenario_file_path, project_path); + std::shared_ptr ss { scenario.getStateSpace() }; + std::shared_ptr env { scenario.getEnvironment() }; + std::unique_ptr planner { nullptr }; + + env->setBaseRadius(std::max(ss->robot->getCapsuleRadius(0), ss->robot->getCapsuleRadius(1)) + obs_dim.norm()); + env->setRobotMaxVel(ss->robot->getMaxVel(0)); // Only velocity of the first joint matters + + Eigen::VectorXf start { Eigen::VectorXf::Zero(ss->num_dimensions) }; + Eigen::VectorXf goal { Eigen::VectorXf::Zero(ss->num_dimensions) }; + for (size_t i = 0; i < ss->num_dimensions; i++) + { + start(i) = node2["scenario_" + std::to_string(init_num_obs)]["run_" + std::to_string(num_test-1)]["start"][i].as(); + goal(i) = node2["scenario_" + std::to_string(init_num_obs)]["run_" + std::to_string(num_test-1)]["goal"][i].as(); + } + scenario.setStart(ss->getNewState(start)); + scenario.setGoal(ss->getNewState(goal)); + + Eigen::Vector3f pos {}, vel {}; + for (size_t j = 0; j < init_num_obs; j++) + { + for (size_t i = 0; i < 3; i++) + { + pos(i) = node2["scenario_" + std::to_string(init_num_obs)]["run_" + std::to_string(num_test-1)] + ["object_" + std::to_string(j)]["pos"][i].as(); + vel(i) = node2["scenario_" + std::to_string(init_num_obs)]["run_" + std::to_string(num_test-1)] + ["object_" + std::to_string(j)]["vel"][i].as(); + } + + std::shared_ptr object { nullptr }; + object = std::make_shared(obs_dim, pos, Eigen::Quaternionf::Identity(), "dynamic_obstacle"); + object->setMaxVel(max_vel_obs); + object->setMaxAcc(max_acc_obs); + env->addObject(object, vel); + } + // ------------------------------------------------------------------------------- // + + LOG(INFO) << "Test number: " << num_test; + LOG(INFO) << "Using scenario: " << project_path + scenario_file_path; + LOG(INFO) << "Environment parts: " << env->getNumObjects(); + LOG(INFO) << "Number of DOFs: " << ss->num_dimensions; + LOG(INFO) << "State space type: " << ss->getStateSpaceType(); + LOG(INFO) << "Start: " << scenario.getStart(); + LOG(INFO) << "Goal: " << scenario.getGoal(); + + planner = std::make_unique(ss, scenario.getStart(), scenario.getGoal()); + bool result { planner->solve() }; + num_real_success_tests += 1 - (goal - planner->getPath().back()->getCoord()).norm() / (goal - start).norm(); + + LOG(INFO) << planner->getPlannerType() << " planning finished with " << (result ? "SUCCESS!" : "FAILURE!"); + LOG(INFO) << "Real success: " << 1 - (goal - planner->getPath().back()->getCoord()).norm() / (goal - start).norm(); + LOG(INFO) << "Number of iterations: " << planner->getPlannerInfo()->getNumIterations(); + LOG(INFO) << "Algorithm time: " << planner->getPlannerInfo()->getPlanningTime() << " [s]"; + // LOG(INFO) << "Planner data is saved at: " << directory_path + "/test" + std::to_string(init_num_obs) + "_" + std::to_string(num_test) + ".log"; + // planner->outputPlannerData(directory_path + "/test" + std::to_string(init_num_obs) + "_" + std::to_string(num_test) + ".log"); + + float path_length { 0 }; + if (result) + { + // LOG(INFO) << "Found path: "; + std::vector> path { planner->getPath() }; + for (size_t i = 0; i < path.size(); i++) + { + // std::cout << i << ": " << path.at(i)->getCoord().transpose() << std::endl; + if (i > 0) path_length += ss->getNorm(path.at(i-1), path.at(i)); + } + path_lengths.emplace_back(path_length); + alg_times.emplace_back(planner->getPlannerInfo()->getPlanningTime()); + iter_times.emplace_back(planner->getPlannerInfo()->getPlanningTime() / planner->getPlannerInfo()->getNumIterations()); + num_iters.emplace_back(planner->getPlannerInfo()->getNumIterations()); + num_success_tests++; + } + + + output_file.open(directory_path + "/results_" + std::to_string(init_num_obs) + "obs_" + + std::to_string(size_t(RT_RGBTConfig::MAX_ITER_TIME * 1000)) + "ms.log", std::ofstream::app); + output_file << "Test number: " << num_test << std::endl; + output_file << "Number of successful tests: " << num_success_tests << " of " << num_test + << " = " << 100.0 * num_success_tests / num_test << " %" << std::endl; + output_file << "Success:\n" << result << std::endl; + output_file << "Real success:\n" << 1 - (goal - planner->getPath().back()->getCoord()).norm() / (goal - start).norm() << std::endl; + output_file << "Number of iterations:\n" << planner->getPlannerInfo()->getNumIterations() << std::endl; + output_file << "Algorithm execution time [s]:\n" << planner->getPlannerInfo()->getPlanningTime() << std::endl; + output_file << "Path length [rad]:\n" << (result ? path_length : INFINITY) << std::endl; + + std::vector> routine_times { planner->getPlannerInfo()->getRoutineTimes() }; + for (size_t idx = 0; idx < routines.size(); idx++) + { + LOG(INFO) << "Routine " << routines[idx]; + LOG(INFO) << "\tAverage time: " << getMean(routine_times[idx]) << " +- " << getStd(routine_times[idx]); + LOG(INFO) << "\tMaximal time: " << *std::max_element(routine_times[idx].begin(), routine_times[idx].end()); + LOG(INFO) << "\tData size: " << routine_times[idx].size(); + + output_file << "Routine " << routines[idx] << " times: " << std::endl; + for (float t : routine_times[idx]) + output_file << t << std::endl; + } + + output_file << "--------------------------------------------------------------------\n"; + output_file.close(); + LOG(INFO) << "Number of successful tests: " << num_success_tests << " of " << num_test + << " = " << 100.0 * num_success_tests / num_test << " %"; + LOG(INFO) << "Number of real successful tests: " << num_real_success_tests << " of " << num_test + << " = " << 100.0 * num_real_success_tests / num_test << " %"; + LOG(INFO) << "--------------------------------------------------------------------\n\n"; + } + catch (std::exception &e) + { + LOG(ERROR) << e.what(); + } + + num_test++; + if ((reach_successful_tests && num_success_tests == max_num_tests) || (!reach_successful_tests && num_test > max_num_tests)) + break; + } + + init_num_test = 1; + init_num_success_test = 0; + init_num_obs += (init_num_obs > 0) ? std::pow(10, std::floor(std::log10(init_num_obs))) : 1; + + LOG(INFO) << "Success rate: " << 100.0 * num_success_tests / (num_test - 1) << " [%]"; + LOG(INFO) << "Real success rate: " << 100.0 * num_real_success_tests / (num_test - 1) << " [%]"; + LOG(INFO) << "Average algorithm execution time: " << getMean(alg_times) << " +- " << getStd(alg_times) << " [s]"; + LOG(INFO) << "Average iteration execution time: " << getMean(iter_times) * 1e3 << " +- " << getStd(iter_times) * 1e3 << " [ms]"; + LOG(INFO) << "Average number of iterations: " << getMean(num_iters) << " +- " << getStd(num_iters); + LOG(INFO) << "Average path length: " << getMean(path_lengths) << " +- " << getStd(path_lengths) << " [rad]"; + LOG(INFO) << "\n--------------------------------------------------------------------\n\n"; + } + + google::ShutDownCommandLineFlags(); + return 0; +} diff --git a/data/configurations/configuration_rt_rgbt.yaml b/data/configurations/configuration_rt_rgbt.yaml new file mode 100644 index 00000000..25d58d50 --- /dev/null +++ b/data/configurations/configuration_rt_rgbt.yaml @@ -0,0 +1,6 @@ +MAX_NUM_ITER: 1000000 # Maximal number of algorithm iterations +MAX_ITER_TIME: 0.090 # Maximal runtime of a single iteration in [s] +MAX_PLANNING_TIME: 10 # Maximal algorithm runtime in [s] +RESOLUTION_COLL_CHECK: 0.01 # Perform collision check when obstacle moves at most 'RESOLUTION_COLL_CHECK' in [m] +GOAL_PROBABILITY: 0.9 # Probability for choosing 'q_goal' as 'q_target' +TRAJECTORY_INTERPOLATION: "Spline" # Method for interpolation of trajectory: 'None', 'Spline' or 'Reflexxes' \ No newline at end of file diff --git a/include/configurations/ConfigurationReader.h b/include/configurations/ConfigurationReader.h index 7c79bc31..c7e33a71 100644 --- a/include/configurations/ConfigurationReader.h +++ b/include/configurations/ConfigurationReader.h @@ -21,6 +21,7 @@ #include "DRGBTConfig.h" #include "RRTxConfig.h" #include "TrajectoryConfig.h" +#include "RT_RGBTConfig.h" class ConfigurationReader { diff --git a/include/configurations/RT_RGBTConfig.h b/include/configurations/RT_RGBTConfig.h new file mode 100644 index 00000000..0750bbff --- /dev/null +++ b/include/configurations/RT_RGBTConfig.h @@ -0,0 +1,23 @@ +// +// Created by nermin on 25.09.25. +// + +#ifndef RPMPL_RTRGBTCONFIG_H +#define RPMPL_RTRGBTCONFIG_H + +#include "PlanningTypes.h" + +typedef unsigned long size_t; + +class RT_RGBTConfig +{ +public: + static size_t MAX_NUM_ITER; // Maximal number of algorithm iterations + static float MAX_ITER_TIME; // Maximal runtime of a single iteration + static float MAX_PLANNING_TIME; // Maximal algorithm runtime + static float RESOLUTION_COLL_CHECK; // Perform collision check when obstacle moves at most 'RESOLUTION_COLL_CHECK' in [m] + static float GOAL_PROBABILITY; // Probability for choosing 'q_goal' as 'q_target' + static planning::TrajectoryInterpolation TRAJECTORY_INTERPOLATION; // Method for interpolation of trajectory: "None", "Spline" or "Reflexxes" +}; + +#endif //RPMPL_RTRGBTCONFIG_H \ No newline at end of file diff --git a/include/planners/PlanningTypes.h b/include/planners/PlanningTypes.h index 8896c748..2bb853b3 100644 --- a/include/planners/PlanningTypes.h +++ b/include/planners/PlanningTypes.h @@ -35,7 +35,8 @@ namespace planning RGBTConnect, RGBMTStar, DRGBT, - RRTx + RRTx, + RT_RGBT }; static std::unordered_map planner_type_map = @@ -45,7 +46,9 @@ namespace planning { "RBT-Connect", planning::PlannerType::RBTConnect }, { "RGBT-Connect", planning::PlannerType::RGBTConnect }, { "RGBMT*", planning::PlannerType::RGBMTStar }, - { "DRGBT", planning::PlannerType::DRGBT } + { "DRGBT", planning::PlannerType::DRGBT }, + { "RRTx", planning::PlannerType::RRTx }, + { "RT-RGBT", planning::PlannerType::RT_RGBT } }; enum class RealTimeScheduling @@ -69,7 +72,13 @@ namespace planning static std::unordered_map trajectory_interpolation_map = { { "None", planning::TrajectoryInterpolation::None }, - { "Spline", planning::TrajectoryInterpolation::Spline} + { "Spline", planning::TrajectoryInterpolation::Spline } + }; + + static std::unordered_map trajectory_interpolation_map2 = + { + { planning::TrajectoryInterpolation::None, "None" }, + { planning::TrajectoryInterpolation::Spline, "Spline" } }; std::ostream &operator<<(std::ostream &os, const planning::PlannerType &type); diff --git a/include/planners/drbt/RT_RGBT.h b/include/planners/drbt/RT_RGBT.h new file mode 100644 index 00000000..1620d2a8 --- /dev/null +++ b/include/planners/drbt/RT_RGBT.h @@ -0,0 +1,44 @@ +// +// Created by nermin on 25.09.25. +// + +#ifndef RPMPL_RTRGBT_H +#define RPMPL_RTRGBT_H + +#include "RGBTConnect.h" +#include "MotionValidity.h" +#include "Trajectory.h" +#include "RT_RGBTConfig.h" + +// #include +// #include +// WARNING: You need to be very careful with using LOG(INFO) for console output, due to a possible "stack smashing detected" error. +// If you get this error, just use std::cout for console output. + +namespace planning::drbt +{ + class RT_RGBT : public planning::rbt::RGBTConnect + { + public: + RT_RGBT(const std::shared_ptr ss_); + RT_RGBT(const std::shared_ptr ss_, + const std::shared_ptr q_start_, const std::shared_ptr q_goal_); + ~RT_RGBT(); + + bool solve() override; + void computeTargetState(); + void update(); + bool checkTerminatingCondition(base::State::Status status) override; + void outputPlannerData(const std::string &filename, bool output_states_and_paths = true, bool append_output = false) const override; + + protected: + std::shared_ptr q_current; // Current robot configuration + std::shared_ptr q_target; // Target (next) robot configuration + std::shared_ptr traj; // Trajectory which is generated using the proposed approach from RPMPLv2 library + std::shared_ptr motion_validity; // Class for checking validity of motion + float max_edge_length; // Distance between 'q_current' and 'q_target' + bool compute_new_target_state; // Whether to compute 'q_target' + }; +} + +#endif //RPMPL_RTRGBT_H \ No newline at end of file diff --git a/src/configurations/ConfigurationReader.cpp b/src/configurations/ConfigurationReader.cpp index 128d9ff4..fc8824ce 100644 --- a/src/configurations/ConfigurationReader.cpp +++ b/src/configurations/ConfigurationReader.cpp @@ -10,6 +10,7 @@ void ConfigurationReader::initConfiguration(const std::string &root_path) YAML::Node DRGBTConfigRoot { YAML::LoadFile(root_path + "/data/configurations/configuration_drgbt.yaml") }; YAML::Node RRTxConfigRoot { YAML::LoadFile(root_path + "/data/configurations/configuration_rrtx.yaml") }; YAML::Node TrajectoryConfigRoot { YAML::LoadFile(root_path + "/data/configurations/configuration_trajectory.yaml") }; + YAML::Node RT_RGBTConfigRoot { YAML::LoadFile(root_path + "/data/configurations/configuration_rt_rgbt.yaml") }; // RealVectorSpaceConfigRoot if (RealVectorSpaceConfigRoot["NUM_INTERPOLATION_VALIDITY_CHECKS"].IsDefined()) @@ -289,5 +290,37 @@ void ConfigurationReader::initConfiguration(const std::string &root_path) else LOG(INFO) << "TrajectoryConfig::MAX_RADIUS is not defined! Using default value of " << TrajectoryConfig::MAX_RADIUS; + + // RT_RGBTConfigRoot + if (RT_RGBTConfigRoot["MAX_NUM_ITER"].IsDefined()) + RT_RGBTConfig::MAX_NUM_ITER = RT_RGBTConfigRoot["MAX_NUM_ITER"].as(); + else + LOG(INFO) << "RT_RGBTConfig::MAX_NUM_ITER is not defined! Using default value of " << RT_RGBTConfig::MAX_NUM_ITER; + + if (RT_RGBTConfigRoot["MAX_ITER_TIME"].IsDefined()) + RT_RGBTConfig::MAX_ITER_TIME = RT_RGBTConfigRoot["MAX_ITER_TIME"].as(); + else + LOG(INFO) << "RT_RGBTConfig::MAX_ITER_TIME is not defined! Using default value of " << RT_RGBTConfig::MAX_ITER_TIME; + + if (RT_RGBTConfigRoot["MAX_PLANNING_TIME"].IsDefined()) + RT_RGBTConfig::MAX_PLANNING_TIME = RT_RGBTConfigRoot["MAX_PLANNING_TIME"].as(); + else + LOG(INFO) << "RT_RGBTConfig::MAX_PLANNING_TIME is not defined! Using default value of " << RT_RGBTConfig::MAX_PLANNING_TIME; + + if (RT_RGBTConfigRoot["RESOLUTION_COLL_CHECK"].IsDefined()) + RT_RGBTConfig::RESOLUTION_COLL_CHECK = RT_RGBTConfigRoot["RESOLUTION_COLL_CHECK"].as(); + else + LOG(INFO) << "RT_RGBTConfig::RESOLUTION_COLL_CHECK is not defined! Using default value of " << RT_RGBTConfig::RESOLUTION_COLL_CHECK; + + if (RT_RGBTConfigRoot["GOAL_PROBABILITY"].IsDefined()) + RT_RGBTConfig::GOAL_PROBABILITY = RT_RGBTConfigRoot["GOAL_PROBABILITY"].as(); + else + LOG(INFO) << "RT_RGBTConfig::GOAL_PROBABILITY is not defined! Using default value of " << RT_RGBTConfig::GOAL_PROBABILITY; + + if (RT_RGBTConfigRoot["TRAJECTORY_INTERPOLATION"].IsDefined()) + RT_RGBTConfig::TRAJECTORY_INTERPOLATION = planning::trajectory_interpolation_map[RT_RGBTConfigRoot["TRAJECTORY_INTERPOLATION"].as()]; + else + LOG(INFO) << "RT_RGBTConfig::TRAJECTORY_INTERPOLATION is not defined! Using default value of " << RT_RGBTConfig::TRAJECTORY_INTERPOLATION; + LOG(INFO) << "Configuration parameters read successfully!"; } diff --git a/src/configurations/RT_RGBTConfig.cpp b/src/configurations/RT_RGBTConfig.cpp new file mode 100644 index 00000000..6d20207d --- /dev/null +++ b/src/configurations/RT_RGBTConfig.cpp @@ -0,0 +1,8 @@ +#include "RT_RGBTConfig.h" + +size_t RT_RGBTConfig::MAX_NUM_ITER = 1e9; +float RT_RGBTConfig::MAX_ITER_TIME = 0.050; +float RT_RGBTConfig::MAX_PLANNING_TIME = 10; +float RT_RGBTConfig::RESOLUTION_COLL_CHECK = 0.01; +float RT_RGBTConfig::GOAL_PROBABILITY = 0.5; +planning::TrajectoryInterpolation RT_RGBTConfig::TRAJECTORY_INTERPOLATION = planning::TrajectoryInterpolation::Spline; \ No newline at end of file diff --git a/src/planners/PlanningTypes.cpp b/src/planners/PlanningTypes.cpp index f3658de2..c6461b3c 100644 --- a/src/planners/PlanningTypes.cpp +++ b/src/planners/PlanningTypes.cpp @@ -33,6 +33,10 @@ namespace planning case planning::PlannerType::RRTx: os << "RRTx"; break; + + case planning::PlannerType::RT_RGBT: + os << "RT-RGBT"; + break; } return os; diff --git a/src/planners/drbt/RT_RGBT.cpp b/src/planners/drbt/RT_RGBT.cpp new file mode 100644 index 00000000..23623378 --- /dev/null +++ b/src/planners/drbt/RT_RGBT.cpp @@ -0,0 +1,299 @@ +#include "RT_RGBT.h" + +planning::drbt::RT_RGBT::RT_RGBT(const std::shared_ptr ss_) : RGBTConnect(ss_) +{ + planner_type = planning::PlannerType::RT_RGBT; +} + +planning::drbt::RT_RGBT::RT_RGBT(const std::shared_ptr ss_, const std::shared_ptr q_start_, + const std::shared_ptr q_goal_) : RGBTConnect(ss_) +{ + // std::cout << "Initializing RT_RGBT planner... \n"; + q_start = q_start_; + q_goal = q_goal_; + compute_new_target_state = true; + + planner_type = planning::PlannerType::RT_RGBT; + if (!ss->isValid(q_start) || ss->robot->checkSelfCollision(q_start)) + throw std::domain_error("Start position is invalid!"); + + q_current = q_start; + q_target = nullptr; + + planner_info->setNumStates(1); + planner_info->setNumIterations(0); + path.emplace_back(q_start); // State 'q_start' is added to the realized path + max_edge_length = ss->robot->getMaxVel().norm() * RT_RGBTConfig::MAX_ITER_TIME; + + motion_validity = std::make_shared + (ss, RT_RGBTConfig::RESOLUTION_COLL_CHECK, RT_RGBTConfig::MAX_ITER_TIME, &path); + + switch (RT_RGBTConfig::TRAJECTORY_INTERPOLATION) + { + case planning::TrajectoryInterpolation::None: + traj = nullptr; + break; + + case planning::TrajectoryInterpolation::Spline: + traj = std::make_shared(ss, q_current, RT_RGBTConfig::MAX_ITER_TIME); + break; + } + // std::cout << "RT_RGBT planner initialized! \n"; +} + +planning::drbt::RT_RGBT::~RT_RGBT() +{ + path.clear(); +} + +bool planning::drbt::RT_RGBT::solve() +{ + time_alg_start = std::chrono::steady_clock::now(); // Start the algorithm clock + time_iter_start = time_alg_start; + + // Initial iteration: Obtaining an inital path using specified static planner + // std::cout << "Iteration: " << planner_info->getNumIterations() << "\n"; + planner_info->setNumIterations(planner_info->getNumIterations() + 1); + planner_info->addIterationTime(getElapsedTime(time_iter_start)); + // std::cout << "----------------------------------------------------------------------------------------\n"; + + base::State::Status status { base::State::Status::None }; + bool is_valid { true }; + + while (true) + { + // std::cout << "Iteration: " << planner_info->getNumIterations() << "\n"; + time_iter_start = std::chrono::steady_clock::now(); // Start the iteration clock + + // ------------------------------------------------------------------------------- // + // Possibly compute a new target state + if (compute_new_target_state) + { + computeTargetState(); + // std::cout << "q_target: " << q_target << "\n"; + } + // else std::cout << "Using previous q_target \n"; + + // ------------------------------------------------------------------------------- // + // Update environment and check if the collision occurs + switch (RT_RGBTConfig::TRAJECTORY_INTERPOLATION) + { + case planning::TrajectoryInterpolation::None: + is_valid = motion_validity->check(q_current, q_target); + q_current = q_target; + break; + + case planning::TrajectoryInterpolation::Spline: + update(); + is_valid = motion_validity->check(traj->traj_points_current_iter); + break; + } + + if (!is_valid) + { + std::cout << "*************** Collision has been occurred!!! *************** \n"; + planner_info->setSuccessState(false); + planner_info->setPlanningTime(planner_info->getIterationTimes().back()); + return false; + } + + // ------------------------------------------------------------------------------- // + // Planner info and terminating condition + planner_info->setNumIterations(planner_info->getNumIterations() + 1); + planner_info->addIterationTime(getElapsedTime(time_alg_start)); + if (checkTerminatingCondition(status)) + return planner_info->getSuccessState(); + + // std::cout << "----------------------------------------------------------------------------------------\n"; + } +} + +// Get a random state 'q_target' with uniform distribution, which is centered around 'q_current'. +// The edge from 'q_current' to 'q_target' is collision-free. +void planning::drbt::RT_RGBT::computeTargetState() +{ + auto time_compute_target = std::chrono::steady_clock::now(); + std::shared_ptr q_rand { nullptr }; + base::State::Status status { base::State::Status::None }; + + while (getElapsedTime(time_compute_target) < RT_RGBTConfig::MAX_ITER_TIME / 2) + { + q_rand = (float(rand()) / RAND_MAX < RT_RGBTConfig::GOAL_PROBABILITY) ? + q_goal : + ss->getRandomState(); + + q_rand = std::get<1>(ss->interpolateEdge2(q_current, q_rand, float(rand()) / RAND_MAX * max_edge_length)); + q_rand = ss->pruneEdge(q_current, q_rand); + ss->computeDistance(q_current, true); + tie(status, q_rand) = extendGenSpine(q_current, q_rand, true); + // std::cout << "d_c: " << q_current->getDistance() << "\n"; + // std::cout << "q_rand: " << q_rand << "\n"; + + if (status != base::State::Status::Trapped) + { + q_target = q_rand; + return; + } + } + + std::cout << "TIME'S UP when finding q_target!!! \n"; +} + +/// @brief Update a current state of the robot using 'traj->spline_current'. +/// Compute a new spline 'traj->spline_next'. +/// Move 'q_current' towards 'q_target' while following 'traj->spline_next'. +/// 'q_current' will be updated to a robot position from the end of current iteration. +/// @note The new trajectory will be computed in a way that all constraints on robot's maximal velocity, +/// acceleration and jerk are surely always satisfied. +void planning::drbt::RT_RGBT::update() +{ + auto time_update { std::chrono::steady_clock::now() }; // Start the algorithm clock + + traj->spline_current = traj->spline_next; + float t_traj_max { TrajectoryConfig::MAX_TIME_COMPUTE_REGULAR }; + float t_iter { getElapsedTime(time_iter_start) }; + if (RT_RGBTConfig::MAX_ITER_TIME - t_iter < t_traj_max) + t_traj_max = RT_RGBTConfig::MAX_ITER_TIME - t_iter; + + float t_iter_remain { RT_RGBTConfig::MAX_ITER_TIME - t_iter - t_traj_max }; + float t_traj_current { traj->spline_current->getTimeEnd() + t_iter + t_traj_max }; + + traj->spline_current->setTimeBegin(traj->spline_current->getTimeEnd()); + traj->spline_current->setTimeCurrent(t_traj_current); + // ----------------------------------------------------------------------------------------- // + + bool traj_computed { false }; + float t_traj_remain { t_traj_max - (getElapsedTime(time_iter_start) - t_iter) }; + // std::cout << "t_traj_remain: " << t_traj_remain * 1e3 << " [ms] \n"; + + if (t_traj_remain > 0) + { + Eigen::VectorXf current_pos { traj->spline_current->getPosition(t_traj_current) }; + Eigen::VectorXf current_vel { traj->spline_current->getVelocity(t_traj_current) }; + Eigen::VectorXf current_acc { traj->spline_current->getAcceleration(t_traj_current) }; + + // std::cout << "Curr. pos: " << current_pos.transpose() << "\n"; + // std::cout << "Curr. vel: " << current_vel.transpose() << "\n"; + // std::cout << "Curr. acc: " << current_acc.transpose() << "\n"; + + traj->setCurrentState(q_current); + traj->setTargetState(q_target); + // std::cout << "q_target: " << q_target << "\n"; + + traj_computed = traj->computeRegular(current_pos, current_vel, current_acc, t_iter_remain, t_traj_remain, true); + + // Try to compute again, but with ZERO final velocity + t_traj_remain = t_traj_max - (getElapsedTime(time_iter_start) - t_iter); + if (traj_computed && traj->spline_next->getTimeFinal() < t_iter_remain && !traj->spline_next->getIsZeroFinalVel() && t_traj_remain > 0) + traj_computed = traj->computeRegular(current_pos, current_vel, current_acc, t_iter_remain, t_traj_remain, false); + } + + if (traj_computed) + { + // std::cout << "New trajectory is computed! \n"; + traj->spline_current->setTimeEnd(t_traj_current); + traj->spline_next->setTimeEnd(t_iter_remain); + } + else + { + // std::cout << "Continuing with the previous trajectory! \n"; + traj->spline_next = traj->spline_current; + traj->spline_next->setTimeEnd(t_traj_current + t_iter_remain); + } + + q_current = ss->getNewState(traj->spline_next->getPosition(traj->spline_next->getTimeEnd())); // Current robot position at the end of iteration + // std::cout << "q_current: " << q_current << "\n"; + + // If 'q_current' is far away from 'q_target', do not change 'q_target' + compute_new_target_state = (ss->getNorm(q_current, q_target) > + traj->spline_next->getVelocity(traj->spline_next->getTimeEnd()).norm() / ss->robot->getMaxVel().norm() * TrajectoryConfig::MAX_RADIUS) ? + false : true; + + planner_info->addRoutineTime(getElapsedTime(time_update, planning::TimeUnit::ms), 0); + // std::cout << "Elapsed time for trajectory computing: " << (getElapsedTime(time_update) - t_iter) * 1e6 << " [us] \n"; + + // ----------------------------------------------------------------------------------------- // + // Store trajectory points from the current iteration to be validated later within 'MotionValidity' + traj->traj_points_current_iter.clear(); + size_t num_checks1 = std::ceil((traj->spline_current->getTimeCurrent() - traj->spline_current->getTimeBegin()) * TrajectoryConfig::NUM_VALIDITY_POINTS_CHECK / RT_RGBTConfig::MAX_ITER_TIME); + float delta_time1 { (traj->spline_current->getTimeCurrent() - traj->spline_current->getTimeBegin()) / num_checks1 }; + float t { 0 }; + for (size_t num_check = 1; num_check <= num_checks1; num_check++) + { + t = traj->spline_current->getTimeBegin() + num_check * delta_time1; + traj->traj_points_current_iter.emplace_back(traj->spline_current->getPosition(t)); + } + + size_t num_checks2 { TrajectoryConfig::NUM_VALIDITY_POINTS_CHECK - num_checks1 }; + float delta_time2 { (traj->spline_next->getTimeEnd() - traj->spline_next->getTimeCurrent()) / num_checks2 }; + for (size_t num_check = 1; num_check <= num_checks2; num_check++) + { + t = traj->spline_next->getTimeCurrent() + num_check * delta_time2; + traj->traj_points_current_iter.emplace_back(traj->spline_next->getPosition(t)); + } +} + +bool planning::drbt::RT_RGBT::checkTerminatingCondition([[maybe_unused]] base::State::Status status) +{ + float time_current { getElapsedTime(time_alg_start) }; + // std::cout << "Time elapsed: " << time_current * 1e3 << " [ms] \n"; + + if (ss->isEqual(q_current, q_goal)) + { + std::cout << "Goal configuration has been successfully reached! \n"; + planner_info->setSuccessState(true); + planner_info->setPlanningTime(time_current); + return true; + } + + if (time_current >= RT_RGBTConfig::MAX_PLANNING_TIME) + { + std::cout << "Maximal planning time has been reached! \n"; + planner_info->setSuccessState(false); + planner_info->setPlanningTime(time_current); + return true; + } + + if (planner_info->getNumIterations() >= RT_RGBTConfig::MAX_NUM_ITER) + { + std::cout << "Maximal number of iterations has been reached! \n"; + planner_info->setSuccessState(false); + planner_info->setPlanningTime(time_current); + return true; + } + + return false; +} + +void planning::drbt::RT_RGBT::outputPlannerData(const std::string &filename, bool output_states_and_paths, bool append_output) const +{ + std::ofstream output_file {}; + std::ios_base::openmode mode { std::ofstream::out }; + if (append_output) + mode = std::ofstream::app; + + output_file.open(filename, mode); + if (output_file.is_open()) + { + output_file << "Space Type: " << ss->getStateSpaceType() << std::endl; + output_file << "Dimensionality: " << ss->num_dimensions << std::endl; + output_file << "Planner type: " << planner_type << std::endl; + output_file << "Planner info:\n"; + output_file << "\t Succesfull: " << (planner_info->getSuccessState() ? "yes" : "no") << std::endl; + output_file << "\t Number of iterations: " << planner_info->getNumIterations() << std::endl; + output_file << "\t Planning time [s]: " << planner_info->getPlanningTime() << std::endl; + if (output_states_and_paths) + { + if (path.size() > 0) + { + output_file << "Path:" << std::endl; + for (size_t i = 0; i < path.size(); i++) + output_file << path.at(i) << std::endl; + } + } + output_file << std::string(25, '-') << std::endl; + output_file.close(); + } + else + throw "Cannot open file"; // std::something exception perhaps? +} From f7025e1382f0a9d1abb5cd7c9226ef785119ae2c Mon Sep 17 00:00:00 2001 From: Nermin Date: Thu, 2 Oct 2025 18:49:39 +0200 Subject: [PATCH 19/40] Corresponding changes... --- apps/test_drgbt_random_obstacles.cpp | 2 +- apps/test_rt_rgbt.cpp | 2 +- include/planners/rbt/RGBTConnect.h | 4 ++-- src/planners/rbt/RGBTConnect.cpp | 18 ++++++++++-------- 4 files changed, 14 insertions(+), 12 deletions(-) diff --git a/apps/test_drgbt_random_obstacles.cpp b/apps/test_drgbt_random_obstacles.cpp index 88f576a5..7137591e 100644 --- a/apps/test_drgbt_random_obstacles.cpp +++ b/apps/test_drgbt_random_obstacles.cpp @@ -101,7 +101,7 @@ int main(int argc, char **argv) std::vector path_lengths {}; size_t num_test { init_num_test }; size_t num_success_tests { init_num_success_test }; - float num_real_success_tests { init_num_success_test }; + float num_real_success_tests = init_num_success_test; while (true) { diff --git a/apps/test_rt_rgbt.cpp b/apps/test_rt_rgbt.cpp index f93bf8c4..2709c3fc 100644 --- a/apps/test_rt_rgbt.cpp +++ b/apps/test_rt_rgbt.cpp @@ -80,7 +80,7 @@ int main(int argc, char **argv) std::vector path_lengths {}; size_t num_test { init_num_test }; size_t num_success_tests { init_num_success_test }; - float num_real_success_tests { init_num_success_test }; + float num_real_success_tests = init_num_success_test; while (true) { diff --git a/include/planners/rbt/RGBTConnect.h b/include/planners/rbt/RGBTConnect.h index 9c5b231b..bb2e0eea 100644 --- a/include/planners/rbt/RGBTConnect.h +++ b/include/planners/rbt/RGBTConnect.h @@ -28,9 +28,9 @@ namespace planning::rbt protected: std::tuple> extendGenSpine - (const std::shared_ptr q, const std::shared_ptr q_e); + (const std::shared_ptr q, const std::shared_ptr q_e, bool use_real_dist = false); std::tuple>>> extendGenSpine2 - (const std::shared_ptr q, const std::shared_ptr q_e); + (const std::shared_ptr q, const std::shared_ptr q_e, bool use_real_dist = false); base::State::Status connectGenSpine(const std::shared_ptr tree, const std::shared_ptr q, const std::shared_ptr q_e); }; diff --git a/src/planners/rbt/RGBTConnect.cpp b/src/planners/rbt/RGBTConnect.cpp index be975fe4..0bc3d2ef 100644 --- a/src/planners/rbt/RGBTConnect.cpp +++ b/src/planners/rbt/RGBTConnect.cpp @@ -70,8 +70,8 @@ bool planning::rbt::RGBTConnect::solve() // Generalized spine is generated from 'q' towards 'q_e' // 'q_new' is the final state from the generalized spine -std::tuple> - planning::rbt::RGBTConnect::extendGenSpine(const std::shared_ptr q, const std::shared_ptr q_e) +std::tuple> planning::rbt::RGBTConnect::extendGenSpine + (const std::shared_ptr q, const std::shared_ptr q_e, bool use_real_dist) { float d_c { ss->computeDistance(q) }; if (d_c <= 0) @@ -83,8 +83,9 @@ std::tuple> for (size_t i = 0; i < RGBTConnectConfig::NUM_LAYERS; i++) { tie(status, q_new) = extendSpine(q_new, q_e); - d_c = ss->computeDistanceUnderestimation(q_new, q->getNearestPoints()); - // d_c = ss->computeDistance(q_new); // If you want to use real distance + d_c = use_real_dist ? + ss->computeDistance(q_new) : + ss->computeDistanceUnderestimation(q_new, q->getNearestPoints()); if (d_c < RBTConnectConfig::D_CRIT || status != base::State::Status::Advanced) break; } @@ -93,8 +94,8 @@ std::tuple> // Generalized spine is generated from 'q' towards 'q_e' // 'q_new_list' contains all states from the generalized spine -std::tuple>>> - planning::rbt::RGBTConnect::extendGenSpine2(const std::shared_ptr q, const std::shared_ptr q_e) +std::tuple>>> planning::rbt::RGBTConnect::extendGenSpine2 + (const std::shared_ptr q, const std::shared_ptr q_e, bool use_real_dist) { float d_c { ss->computeDistance(q) }; std::shared_ptr q_new { q }; @@ -107,8 +108,9 @@ std::tuplecomputeDistanceUnderestimation(q_new, q->getNearestPoints()); - // d_c = ss->computeDistance(q_new); // If you want to use real distance + d_c = use_real_dist ? + ss->computeDistance(q_new) : + ss->computeDistanceUnderestimation(q_new, q->getNearestPoints()); if (d_c < RBTConnectConfig::D_CRIT || status == base::State::Status::Reached) break; } From dc16ab4d9e5669cdb08b2ced3665f8ad01fc3b22 Mon Sep 17 00:00:00 2001 From: Nermin Date: Fri, 3 Oct 2025 18:45:40 +0200 Subject: [PATCH 20/40] Successfully included 'ruckig' library. --- .gitmodules | 4 ---- CMakeLists.txt | 8 ++------ include/planners/drbt/RT_RGBT.h | 3 ++- src/CMakeLists.txt | 1 - 4 files changed, 4 insertions(+), 12 deletions(-) diff --git a/.gitmodules b/.gitmodules index 70747955..e69de29b 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,4 +0,0 @@ -[submodule "external/ros_reflexxes"] -path = external/ros_reflexxes -url = https://github.com/PickNikRobotics/ros_reflexxes.git -branch = foxy \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index e3c45520..b6963b04 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -61,12 +61,9 @@ include_directories(${OROCOS_KDL_INCLUDE_DIR}) find_package(yaml-cpp REQUIRED) find_package(fcl 0.7 REQUIRED) find_package(nanoflann REQUIRED) +find_package(ruckig REQUIRED) -# add_subdirectory(external/ros_reflexxes/libreflexxestype2) -# include_directories(external/ros_reflexxes/libreflexxestype2/include) - -set(PROJECT_LIBRARIES gtest glog gflags nanoflann::nanoflann kdl_parser orocos-kdl fcl ccd yaml-cpp) -#reflexxes) +set(PROJECT_LIBRARIES gtest glog gflags nanoflann::nanoflann kdl_parser orocos-kdl fcl ccd yaml-cpp ruckig) set(MAIN_PROJECT_BUILD_DIR ${CMAKE_CURRENT_SOURCE_DIR}/build) @@ -81,7 +78,6 @@ add_subdirectory(src) # The executable code is here add_subdirectory(apps) -# add_subdirectory(external/ros_reflexxes/libreflexxestype2/src/RMLPositionSampleApplications) # Testing only available if this is the main app # Emergency override MODERN_CMAKE_BUILD_TESTING provided as well diff --git a/include/planners/drbt/RT_RGBT.h b/include/planners/drbt/RT_RGBT.h index 1620d2a8..85d14e81 100644 --- a/include/planners/drbt/RT_RGBT.h +++ b/include/planners/drbt/RT_RGBT.h @@ -5,10 +5,11 @@ #ifndef RPMPL_RTRGBT_H #define RPMPL_RTRGBT_H +#include "RT_RGBTConfig.h" #include "RGBTConnect.h" #include "MotionValidity.h" #include "Trajectory.h" -#include "RT_RGBTConfig.h" +#include // #include // #include diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 0e3d3a33..2f2b85c8 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -18,7 +18,6 @@ set(INCLUDE_DIRS ${PROJECT_SOURCE_DIR}/include/scenario ${PROJECT_SOURCE_DIR}/include/configurations ${PROJECT_SOURCE_DIR}/include/benchmark - ${PROJECT_SOURCE_DIR}/external/ros_reflexxes/libreflexxestype2/include ) # Optionally glob, but only for CMake 3.12 or later: From 5964203a048fa45facd34dab5cd9adeea6547334 Mon Sep 17 00:00:00 2001 From: Nermin Date: Sat, 4 Oct 2025 17:42:29 +0200 Subject: [PATCH 21/40] 'TrajectoryRuckig' class is made as a bridge between Ruckig and DRGBT. --- .../planners/trajectory/TrajectoryRuckig.h | 72 +++++++ src/planners/trajectory/TrajectoryRuckig.cpp | 183 ++++++++++++++++++ 2 files changed, 255 insertions(+) create mode 100644 include/planners/trajectory/TrajectoryRuckig.h create mode 100644 src/planners/trajectory/TrajectoryRuckig.cpp diff --git a/include/planners/trajectory/TrajectoryRuckig.h b/include/planners/trajectory/TrajectoryRuckig.h new file mode 100644 index 00000000..f67f7f63 --- /dev/null +++ b/include/planners/trajectory/TrajectoryRuckig.h @@ -0,0 +1,72 @@ +// +// Created by nermin on 03.10.25. +// + +#ifndef RPMPL_TrajectoryRuckig_H +#define RPMPL_TrajectoryRuckig_H + +#include "StateSpace.h" +#include "TrajectoryConfig.h" +#include "RealVectorSpaceConfig.h" + +#include + +namespace planning::trajectory +{ + class TrajectoryRuckig + { + public: + TrajectoryRuckig(const std::shared_ptr &ss_, const Eigen::VectorXf &q_current_, float max_iter_time_); + ~TrajectoryRuckig() {} + + bool computeRegular(const Eigen::VectorXf ¤t_pos, const Eigen::VectorXf ¤t_vel, + const Eigen::VectorXf ¤t_acc, float t_iter_remain, float t_max, bool non_zero_final_vel); + + Eigen::VectorXf getPosition(float t); + Eigen::VectorXf getVelocity(float t); + Eigen::VectorXf getAcceleration(float t); + void addTrajPointCurrentIter(const Eigen::VectorXf &pos); + void clearTrajPointCurrentIter(); + bool isFinalConf(const Eigen::VectorXf &q); + + float getTimeCurrent(bool measure_time = false); + inline float getTimeBegin() const { return time_begin; } + inline float getTimeEnd() const { return time_end; } + inline float getTimeFinal() const { return time_final; } + inline bool getIsZeroFinalVel() const { return is_zero_final_vel; } + inline const std::vector &getTrajPointCurrentIter() const { return traj_points_current_iter; } + + void setTimeStart(float time_start_offset_); + inline void setTimeBegin(float time_begin_) { time_begin = time_begin_; } + inline void setTimeEnd(float time_end_) { time_end = time_end_; } + inline void setCurrentState(const Eigen::VectorXf &q_current_) { q_current = q_current_; } + inline void setTargetState(const Eigen::VectorXf &q_target_) { q_target = q_target_; } + inline void setTimeCurrent(float time_current_) { time_current = time_current_; } + + private: + ruckig::InputParameter input; + ruckig::Trajectory traj; + ruckig::Trajectory traj_temp; + ruckig::Ruckig otg; + + std::shared_ptr ss; + Eigen::VectorXf q_current; // Current robot configuration + Eigen::VectorXf q_target; // Target robot configuration to which the robot is currently heading to, as well as the configuration where the trajectory is ending + bool all_robot_vel_same; // Whether all joint velocities are the same + size_t max_num_iter_trajectory; // Maximal number of iterations when computing trajectory + float max_iter_time; // Maximal iteration time + float remaining_iter_time; // Remaining iteration time till the end of the current iteration + float time_current; // Current time for a trajectory + float time_final; // Final time for a trajectory + float time_final_temp; // Final time (temp) for a trajectory + float time_begin; // Time instance in [s] when a trajectory begins in the current iteration + float time_end; // Time instance in [s] when a trajectory ends in the current iteration + std::chrono::steady_clock::time_point time_start; // Start time point when a trajectory is created + float time_start_offset; // Time offset in [s] which determines how much earlier 'time_start' is created + bool is_zero_final_vel; // Whether final velocity is zero. If not, robot will move at constant velocity after 'time_final'. + std::vector traj_points_current_iter; // Trajectory points from the current iteration to be validated within 'MotionValidity' + + }; +} + +#endif //RPMPL_TrajectoryRuckig_H \ No newline at end of file diff --git a/src/planners/trajectory/TrajectoryRuckig.cpp b/src/planners/trajectory/TrajectoryRuckig.cpp new file mode 100644 index 00000000..3ecb57df --- /dev/null +++ b/src/planners/trajectory/TrajectoryRuckig.cpp @@ -0,0 +1,183 @@ +#include "TrajectoryRuckig.h" + +planning::trajectory::TrajectoryRuckig::TrajectoryRuckig(const std::shared_ptr &ss_, const Eigen::VectorXf &q_current_, float max_iter_time_) : + input(ss->num_dimensions), + traj(ss->num_dimensions), + traj_temp(ss->num_dimensions), + otg(ss->num_dimensions) +{ + ss = ss_; + q_current = q_current_; + max_iter_time = max_iter_time_; + time_current = 0; + time_final = 0; + time_final_temp = 0; + time_begin = 0; + time_end = 0; + time_start = std::chrono::steady_clock::now(); + time_start_offset = 0; + is_zero_final_vel = true; + traj_points_current_iter = {}; + + input.current_position = q_current.cast(); + input.current_velocity = Eigen::VectorXd::Zero(ss->num_dimensions); + input.current_acceleration = Eigen::VectorXd::Zero(ss->num_dimensions); + + input.max_velocity = ss->robot->getMaxVel().cast(); + input.max_acceleration = ss->robot->getMaxAcc().cast(); + input.max_jerk = ss->robot->getMaxJerk().cast(); + + all_robot_vel_same = true; + for (size_t i = 1; i < ss->num_dimensions; i++) + { + if (std::abs(ss->robot->getMaxVel(i) - ss->robot->getMaxVel(i-1)) > RealVectorSpaceConfig::EQUALITY_THRESHOLD) + { + all_robot_vel_same = false; + break; + } + } + + max_num_iter_trajectory = all_robot_vel_same ? + std::ceil(std::log2(2 * ss->robot->getMaxVel(0) / TrajectoryConfig::FINAL_VELOCITY_STEP)) : + std::ceil(std::log2(2 * ss->robot->getMaxVel().maxCoeff() / TrajectoryConfig::FINAL_VELOCITY_STEP)); +} + +/// @brief Compute a regular trajectory that is not surely safe for environment, meaning that, +/// if collision eventually occurs, it may be at robot's non-zero velocity. +/// @param current_pos Current robot's position +/// @param current_vel Current robot's velocity +/// @param current_acc Current robot's acceleration +/// @param t_iter_remain Remaining time in [s] in the current iteration +/// @param t_max Maximal available time in [s] for a trajectory computing +/// @param non_zero_final_vel Whether final velocity can be non-zero +/// @return The success of a trajectory computation +bool planning::trajectory::TrajectoryRuckig::computeRegular(const Eigen::VectorXf ¤t_pos, const Eigen::VectorXf ¤t_vel, + const Eigen::VectorXf ¤t_acc, float t_iter_remain, float t_max, bool non_zero_final_vel) +{ + std::chrono::steady_clock::time_point time_start_ { std::chrono::steady_clock::now() }; + float t_remain { t_max }; + bool traj_computed { false }; + ruckig::Result result { ruckig::Result::Working }; + + input.current_position = current_pos.cast(); + input.current_velocity = current_vel.cast(); + input.current_acceleration = current_acc.cast(); + + if (non_zero_final_vel) + { + is_zero_final_vel = false; + size_t num_iter { 0 }; + float delta_t_max { ((q_target - q_current).cwiseQuotient(ss->robot->getMaxVel())).cwiseAbs().maxCoeff() }; + Eigen::VectorXf q_final_dot_max { (q_target - q_current) / delta_t_max }; + Eigen::VectorXf q_final_dot_min { Eigen::VectorXf::Zero(ss->num_dimensions) }; + Eigen::VectorXf q_final_dot { q_final_dot_max }; + + while (!traj_computed && num_iter++ < max_num_iter_trajectory && t_remain > 0) + { + input.target_position = q_target.cast(); + input.target_velocity = q_final_dot.cast(); + input.target_acceleration = Eigen::VectorXd::Zero(ss->num_dimensions); + result = otg.calculate(input, traj_temp); + + if (result == ruckig::Result::Finished) + { + traj = traj_temp; + time_current = 0; + time_final = traj.get_duration(); + traj_computed = true; + } + else + q_final_dot_max = q_final_dot; + + q_final_dot = (q_final_dot_max + q_final_dot_min) / 2; + t_remain -= std::chrono::duration_cast(std::chrono::steady_clock::now() - time_start_).count() / 1e6; + std::cout << "Num. iter. " << num_iter << "\t q_final_dot: " << q_final_dot.transpose() << "\n"; + } + } + + // Possible current position at the end of iteration + Eigen::VectorXf new_current_pos { getPosition(time_current + t_iter_remain) }; + + // If trajectory was not computed or robot is getting away from 'new_current_pos' + if ((!traj_computed || (new_current_pos - q_target).norm() > (current_pos - q_target).norm()) && t_remain > 0) + { + is_zero_final_vel = true; + input.target_velocity = Eigen::VectorXd::Zero(ss->num_dimensions); + result = otg.calculate(input, traj_temp); + + if (result == ruckig::Result::Finished) + { + traj = traj_temp; + time_current = 0; + time_final = traj.get_duration(); + traj_computed = true; + std::cout << "\t Trajectory computed with ZERO final velocity. \n"; + } + } + else std::cout << "\t Trajectory computed with NON-ZERO final velocity. \n"; + + return traj_computed; +} + +Eigen::VectorXf planning::trajectory::TrajectoryRuckig::getPosition(float t) +{ + ruckig::EigenVector pos; + traj.at_time(t, pos); + return pos.cast(); +} + +Eigen::VectorXf planning::trajectory::TrajectoryRuckig::getVelocity(float t) +{ + [[maybe_unused]] ruckig::EigenVector pos, acc; + ruckig::EigenVector vel; + + traj.at_time(t, pos, vel, acc); + return vel.cast(); +} + +Eigen::VectorXf planning::trajectory::TrajectoryRuckig::getAcceleration(float t) +{ + [[maybe_unused]] ruckig::EigenVector pos, vel; + ruckig::EigenVector acc; + + traj.at_time(t, pos, vel, acc); + return acc.cast(); +} + +void planning::trajectory::TrajectoryRuckig::addTrajPointCurrentIter(const Eigen::VectorXf &pos) +{ + traj_points_current_iter.emplace_back(pos); +} + +void planning::trajectory::TrajectoryRuckig::clearTrajPointCurrentIter() +{ + traj_points_current_iter.clear(); +} + +/// @brief Check whether 'q' is a final configuration of the trajectory. +/// @param q Configuration to be checked. +/// @return True if yes, false if not. +bool planning::trajectory::TrajectoryRuckig::isFinalConf(const Eigen::VectorXf &q) +{ + return ((q - getPosition(time_final)).norm() < RealVectorSpaceConfig::EQUALITY_THRESHOLD) ? true : false; +} + +/// @brief Get current time of a spline. +/// @param measure_time If true, current time will be automatically computed/measured (default: false). +/// @note 'measure_time' should always be false when simulation pacing is used, since then a time measuring will not be correct! +/// In such case, it is assumed that user was previously set 'measure_time' to a correct value. +float planning::trajectory::TrajectoryRuckig::getTimeCurrent(bool measure_time) +{ + if (!measure_time) + return time_current; + + time_current = std::chrono::duration_cast(std::chrono::steady_clock::now() - time_start).count() * 1e-9 + - time_start_offset; + return time_current; +} + +void planning::trajectory::TrajectoryRuckig::setTimeStart(float time_start_offset_) +{ + time_start = std::chrono::steady_clock::now(); + time_start_offset = time_start_offset_; +} From a6330d56daf341bc49faf3f961b9c3c51bb87e3f Mon Sep 17 00:00:00 2001 From: Nermin Date: Sat, 4 Oct 2025 17:47:13 +0200 Subject: [PATCH 22/40] Corresponding addings due to Ruckig. --- README.md | 2 +- .../configurations/configuration_rt_rgbt.yaml | 2 +- include/configurations/RT_RGBTConfig.h | 2 +- include/planners/PlanningTypes.h | 9 ++++-- include/planners/drbt/DRGBT.h | 4 ++- include/planners/rrtx/RRTx.h | 7 +++-- src/planners/PlanningTypes.cpp | 10 +++++-- src/planners/drbt/DRGBT.cpp | 12 +++++++- src/planners/rrtx/RRTx.cpp | 28 +++++++++++++++++-- src/planners/trajectory/MotionValidity.cpp | 2 +- 10 files changed, 60 insertions(+), 18 deletions(-) diff --git a/README.md b/README.md index f077ad87..d49c4950 100644 --- a/README.md +++ b/README.md @@ -143,7 +143,7 @@ In the file ```/data/configurations/configuration_drgbt.yaml```, you can set the - ```STATIC_PLANNER_TYPE```: Type of a static planner (for obtaining the predefined path). Available planners: "RGBMT*", "RGBT-Connect", "RBT-Connect" and "RRT-Connect"; - ```REAL_TIME_SCHEDULING```: Available real-time scheduling is "FPS" - Fixed Priority Scheduling; If you set "None", no real-time scheduling will be used; - ```MAX_TIME_TASK1```: Maximal time in [s] which Task 1 (computing the next configuration) can take from the processor. It must be less than ```MAX_ITER_TIME```. Default: 0.020; -- ```TRAJECTORY_INTERPOLATION```: Method for interpolation of trajectory: 'None' or 'Spline'. If 'None' is used, the robot always moves at its highest speed, i.e., an advancing step for moving from 'q_current' towards 'q_next' in C-space is determined by maximal robot's velocity. On the other hand, if 'Spline' is used, then a quintic spline from 'q_current' to 'q_next' is computed in order to satisfy all constaints on robot's maximal velocity, acceleration and jerk. All configuration parameters considering splines can be set in the file ```/data/configurations/configuration_trajectory.yaml```. +- ```TRAJECTORY_INTERPOLATION```: Method for interpolation of trajectory: 'None', 'Spline' or 'Ruckig'. If 'None' is used, the robot always moves at its highest speed, i.e., an advancing step for moving from 'q_current' towards 'q_next' in C-space is determined by maximal robot's velocity. On the other hand, if 'Spline' is used, then a quintic spline from 'q_current' to 'q_next' is computed in order to satisfy all constaints on robot's maximal velocity, acceleration and jerk. If 'Ruckig' is used, then trajectory is generated using Ruckig library. All configuration parameters considering splines can be set in the file ```/data/configurations/configuration_trajectory.yaml```. - ```GUARANTEED_SAFE_MOTION```: Whether robot motion is surely safe for environment. If collision eventually occurs, it will be at robot's zero velocity, meaning that an obstacle hit the robot, and not vice versa. This feature is intended to be used only for real/practical applications, thus it can be used only when ```TRAJECTORY_INTERPOLATION``` is set to 'Spline'. diff --git a/data/configurations/configuration_rt_rgbt.yaml b/data/configurations/configuration_rt_rgbt.yaml index 25d58d50..98925346 100644 --- a/data/configurations/configuration_rt_rgbt.yaml +++ b/data/configurations/configuration_rt_rgbt.yaml @@ -3,4 +3,4 @@ MAX_ITER_TIME: 0.090 # Maximal runtime of a single iteration i MAX_PLANNING_TIME: 10 # Maximal algorithm runtime in [s] RESOLUTION_COLL_CHECK: 0.01 # Perform collision check when obstacle moves at most 'RESOLUTION_COLL_CHECK' in [m] GOAL_PROBABILITY: 0.9 # Probability for choosing 'q_goal' as 'q_target' -TRAJECTORY_INTERPOLATION: "Spline" # Method for interpolation of trajectory: 'None', 'Spline' or 'Reflexxes' \ No newline at end of file +TRAJECTORY_INTERPOLATION: "Spline" # Method for interpolation of trajectory: 'None', 'Spline' or 'Ruckig' \ No newline at end of file diff --git a/include/configurations/RT_RGBTConfig.h b/include/configurations/RT_RGBTConfig.h index 0750bbff..a31ffc76 100644 --- a/include/configurations/RT_RGBTConfig.h +++ b/include/configurations/RT_RGBTConfig.h @@ -17,7 +17,7 @@ class RT_RGBTConfig static float MAX_PLANNING_TIME; // Maximal algorithm runtime static float RESOLUTION_COLL_CHECK; // Perform collision check when obstacle moves at most 'RESOLUTION_COLL_CHECK' in [m] static float GOAL_PROBABILITY; // Probability for choosing 'q_goal' as 'q_target' - static planning::TrajectoryInterpolation TRAJECTORY_INTERPOLATION; // Method for interpolation of trajectory: "None", "Spline" or "Reflexxes" + static planning::TrajectoryInterpolation TRAJECTORY_INTERPOLATION; // Method for interpolation of trajectory: "None", "Spline" or "Ruckig" }; #endif //RPMPL_RTRGBTCONFIG_H \ No newline at end of file diff --git a/include/planners/PlanningTypes.h b/include/planners/PlanningTypes.h index 2bb853b3..e78e1420 100644 --- a/include/planners/PlanningTypes.h +++ b/include/planners/PlanningTypes.h @@ -66,19 +66,22 @@ namespace planning enum class TrajectoryInterpolation { None, - Spline + Spline, + Ruckig }; static std::unordered_map trajectory_interpolation_map = { { "None", planning::TrajectoryInterpolation::None }, - { "Spline", planning::TrajectoryInterpolation::Spline } + { "Spline", planning::TrajectoryInterpolation::Spline }, + { "Ruckig", planning::TrajectoryInterpolation::Ruckig } }; static std::unordered_map trajectory_interpolation_map2 = { { planning::TrajectoryInterpolation::None, "None" }, - { planning::TrajectoryInterpolation::Spline, "Spline" } + { planning::TrajectoryInterpolation::Spline, "Spline" }, + { planning::TrajectoryInterpolation::Ruckig, "Ruckig" } }; std::ostream &operator<<(std::ostream &os, const planning::PlannerType &type); diff --git a/include/planners/drbt/DRGBT.h b/include/planners/drbt/DRGBT.h index 218bde81..20465eda 100644 --- a/include/planners/drbt/DRGBT.h +++ b/include/planners/drbt/DRGBT.h @@ -14,6 +14,7 @@ #include "UpdatingState.h" #include "MotionValidity.h" #include "Trajectory.h" +#include "TrajectoryRuckig.h" // #include // #include @@ -62,7 +63,8 @@ namespace planning::drbt std::vector> predefined_path; // The predefined path that is being followed size_t num_lateral_states; // Number of lateral states float max_edge_length; // Maximal edge length when acquiring a new predefined path - std::shared_ptr traj; // Everything related to trajectory + std::shared_ptr traj; // Trajectory which is generated using splines from RPMPLv2 library + std::shared_ptr traj_ruckig; // Trajectory which is generated using Ruckig library std::shared_ptr updating_state; // Class for updating current state std::shared_ptr motion_validity; // Class for checking validity of motion std::vector> visited_states; // Set of visited states when choosing 'q_next' diff --git a/include/planners/rrtx/RRTx.h b/include/planners/rrtx/RRTx.h index 7bee8918..4bc6f58d 100644 --- a/include/planners/rrtx/RRTx.h +++ b/include/planners/rrtx/RRTx.h @@ -87,9 +87,10 @@ namespace planning::rrtx // Path from start to goal std::vector> path_current; - std::shared_ptr traj; // Everything related to trajectory - std::shared_ptr updating_state; // Class for updating current state - std::shared_ptr motion_validity; // Class for checking validity of motion + std::shared_ptr traj; // Trajectory which is generated using splines from RPMPLv2 library + std::shared_ptr traj_ruckig; // Trajectory which is generated using Ruckig library + std::shared_ptr updating_state; // Class for updating current state + std::shared_ptr motion_validity; // Class for checking validity of motion // Helper method to calculate distance between states (using getNorm) float distance(const std::shared_ptr q1, const std::shared_ptr q2) const; diff --git a/src/planners/PlanningTypes.cpp b/src/planners/PlanningTypes.cpp index c6461b3c..244efaf4 100644 --- a/src/planners/PlanningTypes.cpp +++ b/src/planners/PlanningTypes.cpp @@ -47,7 +47,7 @@ namespace planning switch (type) { case planning::RealTimeScheduling::None: - os << "none"; + os << "None"; break; case planning::RealTimeScheduling::FPS: @@ -63,11 +63,15 @@ namespace planning switch (type) { case planning::TrajectoryInterpolation::None: - os << "none"; + os << "None"; break; case planning::TrajectoryInterpolation::Spline: - os << "spline"; + os << "Spline"; + break; + + case planning::TrajectoryInterpolation::Ruckig: + os << "Ruckig"; break; } diff --git a/src/planners/drbt/DRGBT.cpp b/src/planners/drbt/DRGBT.cpp index 7d7dae6c..fe3b8387 100644 --- a/src/planners/drbt/DRGBT.cpp +++ b/src/planners/drbt/DRGBT.cpp @@ -44,6 +44,7 @@ planning::drbt::DRGBT::DRGBT(const std::shared_ptr ss_, const { case planning::TrajectoryInterpolation::None: traj = nullptr; + traj_ruckig = nullptr; break; case planning::TrajectoryInterpolation::Spline: @@ -51,6 +52,11 @@ planning::drbt::DRGBT::DRGBT(const std::shared_ptr ss_, const traj->setMaxRemainingIterTime(DRGBTConfig::MAX_ITER_TIME - DRGBTConfig::MAX_TIME_TASK1); updating_state->setTrajectory(traj); break; + + case planning::TrajectoryInterpolation::Ruckig: + traj_ruckig = std::make_shared(ss, q_current->getCoord(), DRGBTConfig::MAX_ITER_TIME); + updating_state->setTrajectory(traj_ruckig); + break; } // std::cout << "DRGBT planner initialized! \n"; @@ -134,7 +140,11 @@ bool planning::drbt::DRGBT::solve() break; case planning::TrajectoryInterpolation::Spline: - is_valid = motion_validity->check(traj->traj_points_current_iter); + is_valid = motion_validity->check(traj->getTrajPointCurrentIter()); + break; + + case planning::TrajectoryInterpolation::Ruckig: + is_valid = motion_validity->check(traj_ruckig->getTrajPointCurrentIter()); break; } diff --git a/src/planners/rrtx/RRTx.cpp b/src/planners/rrtx/RRTx.cpp index 064c9df5..9e0e6959 100644 --- a/src/planners/rrtx/RRTx.cpp +++ b/src/planners/rrtx/RRTx.cpp @@ -46,13 +46,19 @@ planning::rrtx::RRTx::RRTx(const std::shared_ptr ss_, const st switch (RRTxConfig::TRAJECTORY_INTERPOLATION) { + case planning::TrajectoryInterpolation::None: + traj = nullptr; + break; + case planning::TrajectoryInterpolation::Spline: traj = std::make_shared(ss, q_current, RRTxConfig::MAX_ITER_TIME); updating_state->setTrajectory(traj); break; - default: - traj = nullptr; + case planning::TrajectoryInterpolation::Ruckig: + traj_ruckig = std::make_shared(ss, q_current->getCoord(), RRTxConfig::MAX_ITER_TIME); + updating_state->setTrajectory(traj_ruckig); + break; } RRTConnectConfig::EPS_STEP = RRTxConfig::EPS_STEP; @@ -231,7 +237,23 @@ bool planning::rrtx::RRTx::solve() // std::cout << "*************** Real-time is broken. " << -time_iter_remain << " [ms] exceeded!!! *************** \n"; // Update environment and check if the collision occurs - if (!motion_validity->check(q_previous, q_current)) + bool is_valid { false }; + switch (RRTxConfig::TRAJECTORY_INTERPOLATION) + { + case planning::TrajectoryInterpolation::None: + is_valid = motion_validity->check(q_previous, q_current); + break; + + case planning::TrajectoryInterpolation::Spline: + is_valid = motion_validity->check(traj->getTrajPointCurrentIter()); + break; + + case planning::TrajectoryInterpolation::Ruckig: + is_valid = motion_validity->check(traj_ruckig->getTrajPointCurrentIter()); + break; + } + + if (!is_valid) { std::cout << "*************** Collision has been occurred!!! *************** \n"; planner_info->setSuccessState(false); diff --git a/src/planners/trajectory/MotionValidity.cpp b/src/planners/trajectory/MotionValidity.cpp index 465f6126..83d7c73e 100644 --- a/src/planners/trajectory/MotionValidity.cpp +++ b/src/planners/trajectory/MotionValidity.cpp @@ -52,7 +52,7 @@ bool planning::trajectory::MotionValidity::check(const std::shared_ptr Date: Sat, 4 Oct 2025 17:50:53 +0200 Subject: [PATCH 23/40] Minor changes in 'Trajectory' class. --- include/planners/trajectory/Trajectory.h | 6 +++++- src/planners/trajectory/Trajectory.cpp | 26 ++++++++++++++++-------- 2 files changed, 23 insertions(+), 9 deletions(-) diff --git a/include/planners/trajectory/Trajectory.h b/include/planners/trajectory/Trajectory.h index 31bccc9d..62846948 100644 --- a/include/planners/trajectory/Trajectory.h +++ b/include/planners/trajectory/Trajectory.h @@ -24,10 +24,14 @@ namespace planning::trajectory bool computeSafe(const Eigen::VectorXf ¤t_pos, const Eigen::VectorXf ¤t_vel, const Eigen::VectorXf ¤t_acc, float t_iter_remain, float t_max); + void addTrajPointCurrentIter(const Eigen::VectorXf &pos); + void clearTrajPointCurrentIter(); + void path2traj_v1(const std::vector> &path); void path2traj_v2(const std::vector> &path); void path2traj_v3(const std::vector> &path, bool must_visit); + inline const std::vector &getTrajPointCurrentIter() const { return traj_points_current_iter; } inline void setCurrentState(const std::shared_ptr q_current_) { q_current = q_current_; } inline void setTargetState(const std::shared_ptr q_target_) { q_target = q_target_; } inline void setMaxRemainingIterTime(float max_remaining_iter_time_) { max_remaining_iter_time = max_remaining_iter_time_; } @@ -36,7 +40,6 @@ namespace planning::trajectory std::shared_ptr spline_current; // Current spline that 'q_current' is following in the current iteration std::shared_ptr spline_next; // Next spline generated from 'q_current' to 'q_target' std::shared_ptr composite_spline; // Composite spline from the start to a desired target configuration - std::vector traj_points_current_iter; // Trajectory points from the current iteration to be validated within 'MotionValidity' private: void setParams(); @@ -52,6 +55,7 @@ namespace planning::trajectory size_t max_num_iter_spline_regular; // Maximal number of iterations when computing regular spline float max_iter_time; // Maximal iteration time float max_remaining_iter_time; // Maximal remaining iteration time till the end of the current iteration + std::vector traj_points_current_iter; // Trajectory points from the current iteration to be validated within 'MotionValidity' }; } diff --git a/src/planners/trajectory/Trajectory.cpp b/src/planners/trajectory/Trajectory.cpp index f54c024f..0df90d73 100644 --- a/src/planners/trajectory/Trajectory.cpp +++ b/src/planners/trajectory/Trajectory.cpp @@ -66,44 +66,44 @@ bool planning::trajectory::Trajectory::computeRegular(const Eigen::VectorXf &cur const Eigen::VectorXf ¤t_acc, float t_iter_remain, float t_max, bool non_zero_final_vel) { std::chrono::steady_clock::time_point time_start_ { std::chrono::steady_clock::now() }; + float t_remain { t_max }; std::shared_ptr spline_next_new { std::make_shared(ss->robot, current_pos, current_vel, current_acc) }; bool spline_computed { false }; if (non_zero_final_vel) { - float num_iter { 0 }; + size_t num_iter { 0 }; float delta_t_max { ((q_target->getCoord() - q_current->getCoord()).cwiseQuotient(ss->robot->getMaxVel())).cwiseAbs().maxCoeff() }; Eigen::VectorXf q_final_dot_max { (q_target->getCoord() - q_current->getCoord()) / delta_t_max }; Eigen::VectorXf q_final_dot_min { Eigen::VectorXf::Zero(ss->num_dimensions) }; Eigen::VectorXf q_final_dot { q_final_dot_max }; - while (num_iter++ < max_num_iter_spline_regular && - std::chrono::duration_cast(std::chrono::steady_clock::now() - time_start_).count() < t_max * 1e6) + while (!spline_computed && num_iter++ < max_num_iter_spline_regular && t_remain > 0) { if (spline_next_new->compute(q_target->getCoord(), q_final_dot)) { spline_next = spline_next_new; spline_computed = true; - break; } else q_final_dot_max = q_final_dot; q_final_dot = (q_final_dot_max + q_final_dot_min) / 2; + t_remain -= std::chrono::duration_cast(std::chrono::steady_clock::now() - time_start_).count() / 1e6; // std::cout << "Num. iter. " << num_iter << "\t q_final_dot: " << q_final_dot.transpose() << "\n"; } } - Eigen::VectorXf new_current_pos { // Possible current position at the end of iteration - spline_computed ? + // Possible current position at the end of iteration + Eigen::VectorXf new_current_pos { spline_computed ? spline_next->getPosition(t_iter_remain) : spline_current->getPosition(spline_current->getTimeCurrent() + t_iter_remain) }; // If spline was not computed or robot is getting away from 'new_current_pos' - if (!spline_computed || - (new_current_pos - q_target->getCoord()).norm() > (current_pos - q_target->getCoord()).norm()) + if ((!spline_computed || (new_current_pos - q_target->getCoord()).norm() > (current_pos - q_target->getCoord()).norm()) && + t_remain > 0) { spline_computed = spline_next_new->compute(q_target->getCoord()); if (spline_computed) @@ -366,6 +366,16 @@ float planning::trajectory::Trajectory::computeDistanceUnderestimation(const std return d_c; } +void planning::trajectory::Trajectory::addTrajPointCurrentIter(const Eigen::VectorXf &pos) +{ + traj_points_current_iter.emplace_back(pos); +} + +void planning::trajectory::Trajectory::clearTrajPointCurrentIter() +{ + traj_points_current_iter.clear(); +} + /// @brief A method (v1) to convert a path 'path' to a corresponding trajectory. /// Converting this path to trajectory (i.e., assigning time instances to these points) will be automatically done by this function. /// This is done by creating a sequence of quintic splines in a way that all constraints on robot's maximal velocity, From 31822f1e92aad946067dac22b9516ef5b82a39d7 Mon Sep 17 00:00:00 2001 From: Nermin Date: Sat, 4 Oct 2025 17:52:55 +0200 Subject: [PATCH 24/40] 'update_v3' function introduced to update current state when Rucking is used. --- include/planners/trajectory/UpdatingState.h | 33 ++--- src/planners/trajectory/UpdatingState.cpp | 141 +++++++++++++++++++- 2 files changed, 156 insertions(+), 18 deletions(-) diff --git a/include/planners/trajectory/UpdatingState.h b/include/planners/trajectory/UpdatingState.h index 7fa76243..e5fb65fd 100644 --- a/include/planners/trajectory/UpdatingState.h +++ b/include/planners/trajectory/UpdatingState.h @@ -6,9 +6,10 @@ #define RPMPL_UPDATINGSTATE_H #include "StateSpace.h" -#include "Trajectory.h" #include "RealVectorSpaceConfig.h" #include "PlanningTypes.h" +#include "Trajectory.h" +#include "TrajectoryRuckig.h" namespace planning::drbt { @@ -28,6 +29,7 @@ namespace planning::trajectory void update(std::shared_ptr &q_previous, std::shared_ptr &q_current, const std::shared_ptr q_next_, const std::shared_ptr q_next_reached_, base::State::Status &status); inline void setTrajectory(const std::shared_ptr &traj_) { traj = traj_; } + inline void setTrajectory(const std::shared_ptr &traj_ruckig_) { traj_ruckig = traj_ruckig_; } inline void setGuaranteedSafeMotion(bool guaranteed_safe_motion_) { guaranteed_safe_motion = guaranteed_safe_motion_; } inline void setNonZeroFinalVel(bool non_zero_final_vel_) { non_zero_final_vel = non_zero_final_vel_; } inline void setMaxRemainingIterTime(float max_remaining_iter_time_) { max_remaining_iter_time = max_remaining_iter_time_; } @@ -43,27 +45,28 @@ namespace planning::trajectory const std::shared_ptr q_next_, const std::shared_ptr q_next_reached_, base::State::Status &status); void update_v2(std::shared_ptr &q_previous, std::shared_ptr &q_current, const std::shared_ptr q_next_, const std::shared_ptr q_next_reached_, base::State::Status &status); + void update_v3(std::shared_ptr &q_previous, std::shared_ptr &q_current, + const std::shared_ptr q_next_, const std::shared_ptr q_next_reached_, base::State::Status &status); bool invokeChangeNextState(); float getElapsedTime(); std::shared_ptr ss; planning::TrajectoryInterpolation traj_interpolation; - float max_iter_time; // Maximal iteration time in [s] + float max_iter_time; // Maximal iteration time in [s] // Additional info (not mandatory to be set): - std::shared_ptr traj; // Trajectory which is generated using the proposed approach from RPMPLv2 library - bool all_robot_vel_same; // Whether all joint velocities are the same - bool guaranteed_safe_motion; // Whether robot motion is surely safe for environment - bool non_zero_final_vel; // Whether final spline velocity can be non-zero (available only when 'guaranteed_safe_motion' is false) - float max_remaining_iter_time; // Maximal remaining iteration time in [s] till the end of the current iteration - std::chrono::steady_clock::time_point time_iter_start; // Time point when the current iteration started - bool measure_time; // If true, elapsed time when computing a spline will be exactly measured. - // If false, elapsed time will be computed (default: false). - // It should always be false when simulation pacing is used, since then a time measuring will not be correct! - - float remaining_time; // Return value of 'update_v2' function. Remaining time in [s] after which the new spline 'traj->spline_next' - // will become active (if 'planning::TrajectoryInterpolation::None' is not used). - + std::shared_ptr traj; // Trajectory which is generated using splines from RPMPLv2 library + std::shared_ptr traj_ruckig; // Trajectory which is generated using Ruckig library + bool all_robot_vel_same; // Whether all joint velocities are the same + bool guaranteed_safe_motion; // Whether robot motion is surely safe for environment + bool non_zero_final_vel; // Whether final spline velocity can be non-zero (available only when 'guaranteed_safe_motion' is false) + float max_remaining_iter_time; // Maximal remaining iteration time in [s] till the end of the current iteration + std::chrono::steady_clock::time_point time_iter_start; // Time point when the current iteration started + bool measure_time; // If true, elapsed time when computing a spline will be exactly measured. + // If false, elapsed time will be computed (default: false). + // It should always be false when simulation pacing is used, since then a time measuring will not be correct! + float remaining_time; // Return value of 'update_v2' or 'update_v3' function. Remaining time in [s] after which the new trajectory + // will become active (if 'planning::TrajectoryInterpolation::None' is not used). std::shared_ptr q_next; std::shared_ptr q_next_reached; planning::drbt::DRGBT* drgbt_instance; diff --git a/src/planners/trajectory/UpdatingState.cpp b/src/planners/trajectory/UpdatingState.cpp index 118571e7..e9758b29 100644 --- a/src/planners/trajectory/UpdatingState.cpp +++ b/src/planners/trajectory/UpdatingState.cpp @@ -19,6 +19,7 @@ planning::trajectory::UpdatingState::UpdatingState(const std::shared_ptr &q case planning::TrajectoryInterpolation::Spline: update_v2(q_previous, q_current, q_next_, q_next_, status); break; + + case planning::TrajectoryInterpolation::Ruckig: + update_v3(q_previous, q_current, q_next_, q_next_, status); + break; } } @@ -57,6 +62,10 @@ void planning::trajectory::UpdatingState::update(std::shared_ptr &q case planning::TrajectoryInterpolation::Spline: update_v2(q_previous, q_current, q_next_, q_next_reached_, status); break; + + case planning::TrajectoryInterpolation::Ruckig: + update_v3(q_previous, q_current, q_next_, q_next_reached_, status); + break; } } @@ -224,7 +233,7 @@ void planning::trajectory::UpdatingState::update_v2(std::shared_ptr // ----------------------------------------------------------------------------------------- // // Store trajectory points from the current iteration to be validated later within 'MotionValidity' - traj->traj_points_current_iter.clear(); + traj->clearTrajPointCurrentIter(); size_t num_checks1 = std::floor((traj->spline_current->getTimeCurrent() - traj->spline_current->getTimeBegin()) / max_iter_time * TrajectoryConfig::NUM_VALIDITY_POINTS_CHECK ); float delta_time1 { (traj->spline_current->getTimeCurrent() - traj->spline_current->getTimeBegin()) / num_checks1 }; @@ -232,7 +241,7 @@ void planning::trajectory::UpdatingState::update_v2(std::shared_ptr for (size_t num_check = 1; num_check <= num_checks1; num_check++) { t = traj->spline_current->getTimeBegin() + num_check * delta_time1; - traj->traj_points_current_iter.emplace_back(traj->spline_current->getPosition(t)); + traj->addTrajPointCurrentIter(traj->spline_current->getPosition(t)); // std::cout << "t: " << t * 1000 << " [ms]\t" // << "pos: " << traj->spline_current->getPosition(t).transpose() << "\t" // << "vel: " << traj->spline_current->getVelocity(t).transpose() << "\t" @@ -244,7 +253,7 @@ void planning::trajectory::UpdatingState::update_v2(std::shared_ptr for (size_t num_check = 1; num_check <= num_checks2; num_check++) { t = traj->spline_next->getTimeCurrent() + num_check * delta_time2; - traj->traj_points_current_iter.emplace_back(traj->spline_next->getPosition(t)); + traj->addTrajPointCurrentIter(traj->spline_next->getPosition(t)); // std::cout << "t: " << t * 1000 << " [ms]\t" // << "pos: " << traj->spline_next->getPosition(t).transpose() << "\t" // << "vel: " << traj->spline_next->getVelocity(t).transpose() << "\t" @@ -255,6 +264,132 @@ void planning::trajectory::UpdatingState::update_v2(std::shared_ptr remaining_time = t_traj_max + TrajectoryConfig::MAX_TIME_PUBLISH * measure_time - (getElapsedTime() - t_iter); } +/// @brief Update a current state of the robot using Ruckig library. +/// Move 'q_current' towards 'q_next_reached'. +/// 'q_current' will be updated to a robot position from the end of current iteration. +/// @note All constraints on robot's maximal velocity and acceleration are surely always satisfied. +/// @note If 'q_next_reached' is not relevant in the algorithm, pass 'q_next' instead of it. +void planning::trajectory::UpdatingState::update_v3(std::shared_ptr &q_previous, std::shared_ptr &q_current, + const std::shared_ptr q_next_, const std::shared_ptr q_next_reached_, base::State::Status &status) +{ + q_previous = q_current; + q_next = q_next_; + q_next_reached = q_next_reached_; + + float t_traj_max { (guaranteed_safe_motion ? TrajectoryConfig::MAX_TIME_COMPUTE_SAFE : TrajectoryConfig::MAX_TIME_COMPUTE_REGULAR) - + TrajectoryConfig::MAX_TIME_PUBLISH * measure_time }; + float t_iter { getElapsedTime() }; + if (max_iter_time - max_remaining_iter_time - t_iter < t_traj_max) + t_traj_max = max_iter_time - max_remaining_iter_time - t_iter; + + float t_iter_remain { max_iter_time - t_iter - t_traj_max }; + float t_traj_current { measure_time ? + traj_ruckig->getTimeCurrent(true) + t_traj_max : + traj_ruckig->getTimeEnd() + t_iter + t_traj_max }; + + traj_ruckig->setTimeBegin(traj_ruckig->getTimeEnd()); + traj_ruckig->setTimeCurrent(t_traj_current); + + // std::cout << "Iter. time: " << t_iter * 1000 << " [ms] \n"; + // std::cout << "Max. trajectory time: " << t_traj_max * 1000 << " [ms] \n"; + // std::cout << "Remain. time: " << t_iter_remain * 1000 << " [ms] \n"; + // std::cout << "Curr. trajectory time: " << t_traj_current * 1000 << " [ms] \n"; + + // ----------------------------------------------------------------------------------------- // + // Store trajectory points from the current iteration to be validated later within 'MotionValidity' + traj_ruckig->clearTrajPointCurrentIter(); + size_t num_checks1 = std::floor((traj_ruckig->getTimeCurrent() - traj_ruckig->getTimeBegin()) / + max_iter_time * TrajectoryConfig::NUM_VALIDITY_POINTS_CHECK ); + float delta_time1 { (traj_ruckig->getTimeCurrent() - traj_ruckig->getTimeBegin()) / num_checks1 }; + float t { 0 }; + for (size_t num_check = 1; num_check <= num_checks1; num_check++) + { + t = traj_ruckig->getTimeBegin() + num_check * delta_time1; + traj_ruckig->addTrajPointCurrentIter(traj_ruckig->getPosition(t)); + std::cout << "t: " << t * 1000 << " [ms]\t" + << "pos: " << traj_ruckig->getPosition(t).transpose() << "\t" + << "vel: " << traj_ruckig->getVelocity(t).transpose() << "\t" + << "acc: " << traj_ruckig->getAcceleration(t).transpose() << "\n"; + } + // ----------------------------------------------------------------------------------------- // + + bool traj_computed { false }; + float t_traj_remain {}; + Eigen::VectorXf current_pos { traj_ruckig->getPosition(t_traj_current) }; + Eigen::VectorXf current_vel { traj_ruckig->getVelocity(t_traj_current) }; + Eigen::VectorXf current_acc { traj_ruckig->getAcceleration(t_traj_current) }; + // std::cout << "Curr. pos: " << current_pos.transpose() << "\n"; + // std::cout << "Curr. vel: " << current_vel.transpose() << "\n"; + // std::cout << "Curr. acc: " << current_acc.transpose() << "\n"; + + do + { + t_traj_remain = t_traj_max - (getElapsedTime() - t_iter); + // std::cout << "t_traj_remain: " << t_traj_remain * 1e3 << " [ms] \n"; + if (t_traj_remain < 0) + break; + + float step { std::max(ss->robot->getMaxVel().norm() * max_iter_time, + current_vel.norm() / ss->robot->getMaxVel().norm() * TrajectoryConfig::MAX_RADIUS) }; + std::shared_ptr q_target { std::get<1>(ss->interpolateEdge2(q_current, q_next_reached, step)) }; + + traj_ruckig->setCurrentState(q_current->getCoord()); + traj_ruckig->setTargetState(q_target->getCoord()); + // std::cout << "q_target: " << q_target << "\n"; + // std::cout << "q_next: " << q_next << "\n"; + // std::cout << "q_next_reached: " << q_next_reached << "\n"; + + if (traj_ruckig->isFinalConf(q_target->getCoord())) // Trajectory to such 'q_target' already exists! + break; + traj_computed = traj_ruckig->computeRegular(current_pos, current_vel, current_acc, t_iter_remain, t_traj_remain, non_zero_final_vel); + } + while (!traj_computed && invokeChangeNextState()); + + if (traj_computed) + { + // std::cout << "New trajectory is computed! \n"; + traj_ruckig->setTimeEnd(t_iter_remain); + } + else + { + // std::cout << "Continuing with the previous trajectory! \n"; + traj_ruckig->setTimeEnd(t_traj_current + t_iter_remain); + } + + q_current = ss->getNewState(traj_ruckig->getPosition(traj_ruckig->getTimeEnd())); // Current robot position at the end of iteration + if (status != base::State::Status::Trapped) + { + if (ss->getNorm(q_current, q_next) <= // 'q_next' must be reached within the computed radius, and not only 'q_next_reached' + traj_ruckig->getVelocity(traj_ruckig->getTimeEnd()).norm() / ss->robot->getMaxVel().norm() * TrajectoryConfig::MAX_RADIUS) + status = base::State::Status::Reached; + else + status = base::State::Status::Advanced; + } + + std::cout << "Elapsed time for trajectory computing: " << (getElapsedTime() - t_iter) * 1e3 << " [ms] \n"; + // std::cout << "q_current: " << q_current << "\n"; + // std::cout << "Status: " << (status == base::State::Status::Advanced ? "Advanced" : "") + // << (status == base::State::Status::Trapped ? "Trapped" : "") + // << (status == base::State::Status::Reached ? "Reached" : "") << "\n"; + + // ----------------------------------------------------------------------------------------- // + // Store trajectory points from the current iteration to be validated later within 'MotionValidity' + size_t num_checks2 { TrajectoryConfig::NUM_VALIDITY_POINTS_CHECK - num_checks1 }; + float delta_time2 { (traj_ruckig->getTimeEnd() - traj_ruckig->getTimeCurrent()) / num_checks2 }; + for (size_t num_check = 1; num_check <= num_checks2; num_check++) + { + t = traj_ruckig->getTimeCurrent() + num_check * delta_time2; + traj_ruckig->addTrajPointCurrentIter(traj_ruckig->getPosition(t)); + std::cout << "t: " << t * 1000 << " [ms]\t" + << "pos: " << traj_ruckig->getPosition(t).transpose() << "\t" + << "vel: " << traj_ruckig->getVelocity(t).transpose() << "\t" + << "acc: " << traj_ruckig->getAcceleration(t).transpose() << "\n"; + } + // ----------------------------------------------------------------------------------------- // + + remaining_time = t_traj_max + TrajectoryConfig::MAX_TIME_PUBLISH * measure_time - (getElapsedTime() - t_iter); +} + // This function will change 'q_next' and 'q_next_reached' bool planning::trajectory::UpdatingState::invokeChangeNextState() { From f25dc9a9971298a49a9a47b9ff5ff86c593d0017 Mon Sep 17 00:00:00 2001 From: Nermin Date: Sun, 5 Oct 2025 13:32:32 +0200 Subject: [PATCH 25/40] Finally, Ruckig is included from external folder. --- .gitmodules | 4 ++++ CMakeLists.txt | 7 +++++-- README.md | 2 +- data/configurations/configuration_drgbt.yaml | 2 +- data/configurations/configuration_rrtx.yaml | 2 +- data/configurations/configuration_rt_rgbt.yaml | 2 +- data/configurations/configuration_trajectory.yaml | 2 +- include/configurations/DRGBTConfig.h | 2 +- include/configurations/RRTxConfig.h | 2 +- src/CMakeLists.txt | 3 +++ src/configurations/TrajectoryConfig.cpp | 2 +- 11 files changed, 20 insertions(+), 10 deletions(-) diff --git a/.gitmodules b/.gitmodules index e69de29b..0bd80d68 100644 --- a/.gitmodules +++ b/.gitmodules @@ -0,0 +1,4 @@ +[submodule "external/ruckig"] +path = external/ruckig +url = https://github.com/pantor/ruckig.git +branch = v0.15.3 \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index b6963b04..4fad14e0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -61,9 +61,12 @@ include_directories(${OROCOS_KDL_INCLUDE_DIR}) find_package(yaml-cpp REQUIRED) find_package(fcl 0.7 REQUIRED) find_package(nanoflann REQUIRED) -find_package(ruckig REQUIRED) -set(PROJECT_LIBRARIES gtest glog gflags nanoflann::nanoflann kdl_parser orocos-kdl fcl ccd yaml-cpp ruckig) +# If any problems occur about which version of Ruckig is built, just type the following in terminal: +# export LD_LIBRARY_PATH= Date: Sun, 5 Oct 2025 13:33:43 +0200 Subject: [PATCH 26/40] Some lines are commented out. --- src/planners/drbt/replanning.cpp | 2 +- src/planners/trajectory/Trajectory.cpp | 5 +++-- src/planners/trajectory/UpdatingState.cpp | 20 ++++++++++---------- 3 files changed, 14 insertions(+), 13 deletions(-) diff --git a/src/planners/drbt/replanning.cpp b/src/planners/drbt/replanning.cpp index f4a25086..275af970 100644 --- a/src/planners/drbt/replanning.cpp +++ b/src/planners/drbt/replanning.cpp @@ -88,7 +88,7 @@ void planning::drbt::DRGBT::replan(float max_planning_time) status = base::State::Status::Reached; replanning_required = false; q_next = std::make_shared(q_current, 0, q_current); - planner_info->addRoutineTime(planner->getPlannerInfo()->getPlanningTime() * 1e3, 0); // replan + // planner_info->addRoutineTime(planner->getPlannerInfo()->getPlanningTime() * 1e3, 0); // replan } else // New path is not found, and just continue with the previous motion. We can also impose the robot to stop. throw std::runtime_error("New path is not found! "); diff --git a/src/planners/trajectory/Trajectory.cpp b/src/planners/trajectory/Trajectory.cpp index 0df90d73..cd5c495f 100644 --- a/src/planners/trajectory/Trajectory.cpp +++ b/src/planners/trajectory/Trajectory.cpp @@ -93,6 +93,8 @@ bool planning::trajectory::Trajectory::computeRegular(const Eigen::VectorXf &cur t_remain -= std::chrono::duration_cast(std::chrono::steady_clock::now() - time_start_).count() / 1e6; // std::cout << "Num. iter. " << num_iter << "\t q_final_dot: " << q_final_dot.transpose() << "\n"; } + + // if (spline_computed) std::cout << "\t Spline computed with NON-ZERO final velocity. \n"; } // Possible current position at the end of iteration @@ -112,8 +114,7 @@ bool planning::trajectory::Trajectory::computeRegular(const Eigen::VectorXf &cur // std::cout << "\t Spline computed with ZERO final velocity. \n"; } } - // else std::cout << "\t Spline computed with NON-ZERO final velocity. \n"; - + return spline_computed; } diff --git a/src/planners/trajectory/UpdatingState.cpp b/src/planners/trajectory/UpdatingState.cpp index e9758b29..e1268197 100644 --- a/src/planners/trajectory/UpdatingState.cpp +++ b/src/planners/trajectory/UpdatingState.cpp @@ -306,10 +306,10 @@ void planning::trajectory::UpdatingState::update_v3(std::shared_ptr { t = traj_ruckig->getTimeBegin() + num_check * delta_time1; traj_ruckig->addTrajPointCurrentIter(traj_ruckig->getPosition(t)); - std::cout << "t: " << t * 1000 << " [ms]\t" - << "pos: " << traj_ruckig->getPosition(t).transpose() << "\t" - << "vel: " << traj_ruckig->getVelocity(t).transpose() << "\t" - << "acc: " << traj_ruckig->getAcceleration(t).transpose() << "\n"; + // std::cout << "t: " << t * 1000 << " [ms]\t" + // << "pos: " << traj_ruckig->getPosition(t).transpose() << "\t" + // << "vel: " << traj_ruckig->getVelocity(t).transpose() << "\t" + // << "acc: " << traj_ruckig->getAcceleration(t).transpose() << "\n"; } // ----------------------------------------------------------------------------------------- // @@ -365,8 +365,8 @@ void planning::trajectory::UpdatingState::update_v3(std::shared_ptr else status = base::State::Status::Advanced; } - - std::cout << "Elapsed time for trajectory computing: " << (getElapsedTime() - t_iter) * 1e3 << " [ms] \n"; + + // std::cout << "Elapsed time for trajectory computing: " << (getElapsedTime() - t_iter) * 1e3 << " [ms] \n"; // std::cout << "q_current: " << q_current << "\n"; // std::cout << "Status: " << (status == base::State::Status::Advanced ? "Advanced" : "") // << (status == base::State::Status::Trapped ? "Trapped" : "") @@ -380,10 +380,10 @@ void planning::trajectory::UpdatingState::update_v3(std::shared_ptr { t = traj_ruckig->getTimeCurrent() + num_check * delta_time2; traj_ruckig->addTrajPointCurrentIter(traj_ruckig->getPosition(t)); - std::cout << "t: " << t * 1000 << " [ms]\t" - << "pos: " << traj_ruckig->getPosition(t).transpose() << "\t" - << "vel: " << traj_ruckig->getVelocity(t).transpose() << "\t" - << "acc: " << traj_ruckig->getAcceleration(t).transpose() << "\n"; + // std::cout << "t: " << t * 1000 << " [ms]\t" + // << "pos: " << traj_ruckig->getPosition(t).transpose() << "\t" + // << "vel: " << traj_ruckig->getVelocity(t).transpose() << "\t" + // << "acc: " << traj_ruckig->getAcceleration(t).transpose() << "\n"; } // ----------------------------------------------------------------------------------------- // From 3bfad6f56787914e7fee5b0ba4fad0ec8618831c Mon Sep 17 00:00:00 2001 From: Nermin Date: Sun, 5 Oct 2025 13:35:27 +0200 Subject: [PATCH 27/40] Multiple fixes in 'TrajectoryRuckig', so it now finally works with arbitrarly number of DoFs. --- .../planners/trajectory/TrajectoryRuckig.h | 10 +- src/planners/trajectory/TrajectoryRuckig.cpp | 126 +++++++++++++----- 2 files changed, 95 insertions(+), 41 deletions(-) diff --git a/include/planners/trajectory/TrajectoryRuckig.h b/include/planners/trajectory/TrajectoryRuckig.h index f67f7f63..a91efdd4 100644 --- a/include/planners/trajectory/TrajectoryRuckig.h +++ b/include/planners/trajectory/TrajectoryRuckig.h @@ -22,6 +22,8 @@ namespace planning::trajectory bool computeRegular(const Eigen::VectorXf ¤t_pos, const Eigen::VectorXf ¤t_vel, const Eigen::VectorXf ¤t_acc, float t_iter_remain, float t_max, bool non_zero_final_vel); + void setCurrentState(const Eigen::VectorXf ¤t_pos, const Eigen::VectorXf ¤t_vel, const Eigen::VectorXf ¤t_acc); + void setTargetState(const Eigen::VectorXf &target_pos, const Eigen::VectorXf &target_vel, const Eigen::VectorXf &target_acc); Eigen::VectorXf getPosition(float t); Eigen::VectorXf getVelocity(float t); Eigen::VectorXf getAcceleration(float t); @@ -44,10 +46,10 @@ namespace planning::trajectory inline void setTimeCurrent(float time_current_) { time_current = time_current_; } private: - ruckig::InputParameter input; - ruckig::Trajectory traj; - ruckig::Trajectory traj_temp; - ruckig::Ruckig otg; + ruckig::InputParameter input; + // ruckig::OutputParameter output; + ruckig::Trajectory traj; + ruckig::Trajectory traj_temp; std::shared_ptr ss; Eigen::VectorXf q_current; // Current robot configuration diff --git a/src/planners/trajectory/TrajectoryRuckig.cpp b/src/planners/trajectory/TrajectoryRuckig.cpp index 3ecb57df..dff1a71b 100644 --- a/src/planners/trajectory/TrajectoryRuckig.cpp +++ b/src/planners/trajectory/TrajectoryRuckig.cpp @@ -1,10 +1,10 @@ #include "TrajectoryRuckig.h" -planning::trajectory::TrajectoryRuckig::TrajectoryRuckig(const std::shared_ptr &ss_, const Eigen::VectorXf &q_current_, float max_iter_time_) : - input(ss->num_dimensions), - traj(ss->num_dimensions), - traj_temp(ss->num_dimensions), - otg(ss->num_dimensions) +planning::trajectory::TrajectoryRuckig::TrajectoryRuckig(const std::shared_ptr &ss_, + const Eigen::VectorXf &q_current_, float max_iter_time_) : + input(ss_->num_dimensions), + traj(ss_->num_dimensions), + traj_temp(ss_->num_dimensions) { ss = ss_; q_current = q_current_; @@ -19,13 +19,20 @@ planning::trajectory::TrajectoryRuckig::TrajectoryRuckig(const std::shared_ptr(); - input.current_velocity = Eigen::VectorXd::Zero(ss->num_dimensions); - input.current_acceleration = Eigen::VectorXd::Zero(ss->num_dimensions); + for (size_t i = 0; i < ss->num_dimensions; i++) + { + input.current_position[i] = q_current(i); + input.current_velocity[i] = 0; + input.current_acceleration[i] = 0; + + input.max_velocity[i] = ss->robot->getMaxVel(i); + input.max_acceleration[i] = ss->robot->getMaxAcc(i); + input.max_jerk[i] = ss->robot->getMaxJerk(i); + } + input.synchronization = ruckig::Synchronization::Time; - input.max_velocity = ss->robot->getMaxVel().cast(); - input.max_acceleration = ss->robot->getMaxAcc().cast(); - input.max_jerk = ss->robot->getMaxJerk().cast(); + if (!input.validate()) + throw std::runtime_error("Invalid input parameters for Ruckig"); all_robot_vel_same = true; for (size_t i = 1; i < ss->num_dimensions; i++) @@ -58,10 +65,8 @@ bool planning::trajectory::TrajectoryRuckig::computeRegular(const Eigen::VectorX float t_remain { t_max }; bool traj_computed { false }; ruckig::Result result { ruckig::Result::Working }; - - input.current_position = current_pos.cast(); - input.current_velocity = current_vel.cast(); - input.current_acceleration = current_acc.cast(); + ruckig::Ruckig otg(ss->num_dimensions); + setCurrentState(current_pos, current_vel, current_acc); if (non_zero_final_vel) { @@ -73,13 +78,11 @@ bool planning::trajectory::TrajectoryRuckig::computeRegular(const Eigen::VectorX Eigen::VectorXf q_final_dot { q_final_dot_max }; while (!traj_computed && num_iter++ < max_num_iter_trajectory && t_remain > 0) - { - input.target_position = q_target.cast(); - input.target_velocity = q_final_dot.cast(); - input.target_acceleration = Eigen::VectorXd::Zero(ss->num_dimensions); + { + setTargetState(q_target, q_final_dot, Eigen::VectorXf::Zero(ss->num_dimensions)); result = otg.calculate(input, traj_temp); - - if (result == ruckig::Result::Finished) + + if (result == ruckig::Result::Working || result == ruckig::Result::Finished) { traj = traj_temp; time_current = 0; @@ -91,7 +94,7 @@ bool planning::trajectory::TrajectoryRuckig::computeRegular(const Eigen::VectorX q_final_dot = (q_final_dot_max + q_final_dot_min) / 2; t_remain -= std::chrono::duration_cast(std::chrono::steady_clock::now() - time_start_).count() / 1e6; - std::cout << "Num. iter. " << num_iter << "\t q_final_dot: " << q_final_dot.transpose() << "\n"; + // std::cout << "Num. iter. " << num_iter << "\t q_final_dot: " << q_final_dot.transpose() << "\n"; } } @@ -102,46 +105,95 @@ bool planning::trajectory::TrajectoryRuckig::computeRegular(const Eigen::VectorX if ((!traj_computed || (new_current_pos - q_target).norm() > (current_pos - q_target).norm()) && t_remain > 0) { is_zero_final_vel = true; - input.target_velocity = Eigen::VectorXd::Zero(ss->num_dimensions); + setTargetState(q_target, Eigen::VectorXf::Zero(ss->num_dimensions), Eigen::VectorXf::Zero(ss->num_dimensions)); result = otg.calculate(input, traj_temp); - if (result == ruckig::Result::Finished) + if (result == ruckig::Result::Working || result == ruckig::Result::Finished) { traj = traj_temp; time_current = 0; time_final = traj.get_duration(); traj_computed = true; - std::cout << "\t Trajectory computed with ZERO final velocity. \n"; } } - else std::cout << "\t Trajectory computed with NON-ZERO final velocity. \n"; + // std::cout << "\t Trajectory " << (!traj_computed ? "NOT " : "") << "computed with " + // << (!is_zero_final_vel ? "NON-" : "") << "ZERO final velocity. \n"; return traj_computed; } +void planning::trajectory::TrajectoryRuckig::setCurrentState(const Eigen::VectorXf ¤t_pos, + const Eigen::VectorXf ¤t_vel, const Eigen::VectorXf ¤t_acc) +{ + for (size_t i = 0; i < ss->num_dimensions; i++) + { + input.current_position[i] = current_pos(i); + input.current_velocity[i] = current_vel(i); + input.current_acceleration[i] = current_acc(i); + } +} + +void planning::trajectory::TrajectoryRuckig::setTargetState(const Eigen::VectorXf &target_pos, + const Eigen::VectorXf &target_vel, const Eigen::VectorXf &target_acc) +{ + for (size_t i = 0; i < ss->num_dimensions; i++) + { + input.target_position[i] = target_pos(i); + input.target_velocity[i] = target_vel(i); + input.target_acceleration[i] = target_acc(i); + } +} + Eigen::VectorXf planning::trajectory::TrajectoryRuckig::getPosition(float t) { - ruckig::EigenVector pos; - traj.at_time(t, pos); - return pos.cast(); + ruckig::StandardVector pos(ss->num_dimensions); + + if (time_final == 0) + pos = input.current_position; + else + traj.at_time(t, pos); + + Eigen::VectorXf ret(ss->num_dimensions); + for (size_t i = 0; i < ss->num_dimensions; i++) + ret(i) = pos[i]; + + return ret; } Eigen::VectorXf planning::trajectory::TrajectoryRuckig::getVelocity(float t) { - [[maybe_unused]] ruckig::EigenVector pos, acc; - ruckig::EigenVector vel; + ruckig::StandardVector vel(ss->num_dimensions); + [[maybe_unused]] ruckig::StandardVector pos(ss->num_dimensions); + [[maybe_unused]] ruckig::StandardVector acc(ss->num_dimensions); + + if (time_final == 0) + vel = input.current_velocity; + else + traj.at_time(t, pos, vel, acc); - traj.at_time(t, pos, vel, acc); - return vel.cast(); + Eigen::VectorXf ret(ss->num_dimensions); + for (size_t i = 0; i < ss->num_dimensions; i++) + ret(i) = vel[i]; + + return ret; } Eigen::VectorXf planning::trajectory::TrajectoryRuckig::getAcceleration(float t) { - [[maybe_unused]] ruckig::EigenVector pos, vel; - ruckig::EigenVector acc; + ruckig::StandardVector acc(ss->num_dimensions); + [[maybe_unused]] ruckig::StandardVector pos(ss->num_dimensions); + [[maybe_unused]] ruckig::StandardVector vel(ss->num_dimensions); + + if (time_final == 0) + acc = input.current_acceleration; + else + traj.at_time(t, pos, vel, acc); + + Eigen::VectorXf ret(ss->num_dimensions); + for (size_t i = 0; i < ss->num_dimensions; i++) + ret(i) = acc[i]; - traj.at_time(t, pos, vel, acc); - return acc.cast(); + return ret; } void planning::trajectory::TrajectoryRuckig::addTrajPointCurrentIter(const Eigen::VectorXf &pos) From 0b1827d461a1e7f8c9fea254a5eb0370a5263791 Mon Sep 17 00:00:00 2001 From: Nermin Date: Sun, 5 Oct 2025 13:39:22 +0200 Subject: [PATCH 28/40] -Werror is disabled since Ruckig cannot be built then. --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 4fad14e0..5d88fd36 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -20,7 +20,7 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON) message("------------------ Using compiler: ${CMAKE_CXX_COMPILER} ------------------") if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Werror) + add_compile_options(-Wall -Wextra -Wpedantic)# -Werror) endif() # Only do these if this is the main project, and not if it is included through add_subdirectory From d77507e64a7810fdec5646ec77c8a12f4718a099 Mon Sep 17 00:00:00 2001 From: Nermin Date: Sun, 5 Oct 2025 19:00:30 +0200 Subject: [PATCH 29/40] Minor changes... --- apps/test_rt_rgbt.cpp | 3 +-- data/configurations/configuration_rt_rgbt.yaml | 2 +- src/planners/drbt/DRGBT.cpp | 14 ++++++++------ src/planners/rrtx/RRTx.cpp | 1 + 4 files changed, 11 insertions(+), 9 deletions(-) diff --git a/apps/test_rt_rgbt.cpp b/apps/test_rt_rgbt.cpp index 2709c3fc..72c6fbd2 100644 --- a/apps/test_rt_rgbt.cpp +++ b/apps/test_rt_rgbt.cpp @@ -19,7 +19,7 @@ int main(int argc, char **argv) std::vector routines // Routines of which the time executions are stored { - "update [ms]" // 0 + "updateCurrentState [ms]" // 0 }; // -------------------------------------------------------------------------------------- // @@ -159,7 +159,6 @@ int main(int argc, char **argv) num_success_tests++; } - output_file.open(directory_path + "/results_" + std::to_string(init_num_obs) + "obs_" + std::to_string(size_t(RT_RGBTConfig::MAX_ITER_TIME * 1000)) + "ms.log", std::ofstream::app); output_file << "Test number: " << num_test << std::endl; diff --git a/data/configurations/configuration_rt_rgbt.yaml b/data/configurations/configuration_rt_rgbt.yaml index 6bfeec1d..ce9c4bb7 100644 --- a/data/configurations/configuration_rt_rgbt.yaml +++ b/data/configurations/configuration_rt_rgbt.yaml @@ -1,5 +1,5 @@ MAX_NUM_ITER: 1000000 # Maximal number of algorithm iterations -MAX_ITER_TIME: 0.090 # Maximal runtime of a single iteration in [s] +MAX_ITER_TIME: 0.050 # Maximal runtime of a single iteration in [s] MAX_PLANNING_TIME: 10 # Maximal algorithm runtime in [s] RESOLUTION_COLL_CHECK: 0.01 # Perform collision check when obstacle moves at most 'RESOLUTION_COLL_CHECK' in [m] GOAL_PROBABILITY: 0.9 # Probability for choosing 'q_goal' as 'q_target' diff --git a/src/planners/drbt/DRGBT.cpp b/src/planners/drbt/DRGBT.cpp index fe3b8387..46d8e4a6 100644 --- a/src/planners/drbt/DRGBT.cpp +++ b/src/planners/drbt/DRGBT.cpp @@ -91,24 +91,26 @@ bool planning::drbt::DRGBT::solve() // ------------------------------------------------------------------------------- // // Since the environment may change, a new distance is required! // auto time_computeDistance { std::chrono::steady_clock::now() }; - ss->computeDistance(q_current, true); // ~ 1 [ms] + ss->computeDistance(q_current, true); // planner_info->addRoutineTime(getElapsedTime(time_computeDistance, planning::TimeUnit::us), 1); // std::cout << "d_c: " << q_current->getDistance() << " [m] \n"; // ------------------------------------------------------------------------------- // if (status != base::State::Status::Advanced) - generateHorizon(); // ~ 2 [us] + generateHorizon(); - updateHorizon(); // ~ 10 [us] - generateGBur(); // ~ 10 [ms] Time consuming routine... - computeNextState(); // ~ 1 [us] + updateHorizon(); + generateGBur(); + computeNextState(); + // ------------------------------------------------------------------------------- // + // Compute a trajectory and update current state // auto time_updateCurrentState { std::chrono::steady_clock::now() }; visited_states = { q_next }; updating_state->setNonZeroFinalVel(q_next->getIsReached() && q_next->getIndex() != -1 && q_next->getStatus() != planning::drbt::HorizonState::Status::Goal); updating_state->setTimeIterStart(time_iter_start); - updating_state->update(q_previous, q_current, q_next->getState(), q_next->getStateReached(), status); // ~ 1 [ms] + updating_state->update(q_previous, q_current, q_next->getState(), q_next->getStateReached(), status); // planner_info->addRoutineTime(getElapsedTime(time_updateCurrentState, planning::TimeUnit::us), 5); // std::cout << "Time elapsed: " << getElapsedTime(time_iter_start, planning::TimeUnit::ms) << " [ms] \n"; diff --git a/src/planners/rrtx/RRTx.cpp b/src/planners/rrtx/RRTx.cpp index 9e0e6959..9fb5c3fb 100644 --- a/src/planners/rrtx/RRTx.cpp +++ b/src/planners/rrtx/RRTx.cpp @@ -48,6 +48,7 @@ planning::rrtx::RRTx::RRTx(const std::shared_ptr ss_, const st { case planning::TrajectoryInterpolation::None: traj = nullptr; + traj_ruckig = nullptr; break; case planning::TrajectoryInterpolation::Spline: From 764f25c4dd78a014a4ad6df9c3699716758e13fc Mon Sep 17 00:00:00 2001 From: Nermin Date: Sun, 5 Oct 2025 19:01:09 +0200 Subject: [PATCH 30/40] RT-RGBT adapted to operate with Ruckig. --- include/planners/drbt/RT_RGBT.h | 8 ++- src/planners/drbt/RT_RGBT.cpp | 122 +++++++------------------------- 2 files changed, 29 insertions(+), 101 deletions(-) diff --git a/include/planners/drbt/RT_RGBT.h b/include/planners/drbt/RT_RGBT.h index 85d14e81..ebaec6cb 100644 --- a/include/planners/drbt/RT_RGBT.h +++ b/include/planners/drbt/RT_RGBT.h @@ -7,9 +7,10 @@ #include "RT_RGBTConfig.h" #include "RGBTConnect.h" +#include "UpdatingState.h" #include "MotionValidity.h" #include "Trajectory.h" -#include +#include "TrajectoryRuckig.h" // #include // #include @@ -28,14 +29,15 @@ namespace planning::drbt bool solve() override; void computeTargetState(); - void update(); bool checkTerminatingCondition(base::State::Status status) override; void outputPlannerData(const std::string &filename, bool output_states_and_paths = true, bool append_output = false) const override; protected: std::shared_ptr q_current; // Current robot configuration std::shared_ptr q_target; // Target (next) robot configuration - std::shared_ptr traj; // Trajectory which is generated using the proposed approach from RPMPLv2 library + std::shared_ptr traj; // Trajectory which is generated using splines from RPMPLv2 library + std::shared_ptr traj_ruckig; // Trajectory which is generated using Ruckig library + std::shared_ptr updating_state; // Class for updating current state std::shared_ptr motion_validity; // Class for checking validity of motion float max_edge_length; // Distance between 'q_current' and 'q_target' bool compute_new_target_state; // Whether to compute 'q_target' diff --git a/src/planners/drbt/RT_RGBT.cpp b/src/planners/drbt/RT_RGBT.cpp index 23623378..0aad9605 100644 --- a/src/planners/drbt/RT_RGBT.cpp +++ b/src/planners/drbt/RT_RGBT.cpp @@ -25,6 +25,9 @@ planning::drbt::RT_RGBT::RT_RGBT(const std::shared_ptr ss_, co path.emplace_back(q_start); // State 'q_start' is added to the realized path max_edge_length = ss->robot->getMaxVel().norm() * RT_RGBTConfig::MAX_ITER_TIME; + updating_state = std::make_shared + (ss, RT_RGBTConfig::TRAJECTORY_INTERPOLATION, RT_RGBTConfig::MAX_ITER_TIME); + motion_validity = std::make_shared (ss, RT_RGBTConfig::RESOLUTION_COLL_CHECK, RT_RGBTConfig::MAX_ITER_TIME, &path); @@ -32,10 +35,17 @@ planning::drbt::RT_RGBT::RT_RGBT(const std::shared_ptr ss_, co { case planning::TrajectoryInterpolation::None: traj = nullptr; + traj_ruckig = nullptr; break; case planning::TrajectoryInterpolation::Spline: traj = std::make_shared(ss, q_current, RT_RGBTConfig::MAX_ITER_TIME); + updating_state->setTrajectory(traj); + break; + + case planning::TrajectoryInterpolation::Ruckig: + traj_ruckig = std::make_shared(ss, q_current->getCoord(), RT_RGBTConfig::MAX_ITER_TIME); + updating_state->setTrajectory(traj_ruckig); break; } // std::cout << "RT_RGBT planner initialized! \n"; @@ -74,18 +84,28 @@ bool planning::drbt::RT_RGBT::solve() } // else std::cout << "Using previous q_target \n"; + // ------------------------------------------------------------------------------- // + // Compute a trajectory and update current state + auto time_updateCurrentState { std::chrono::steady_clock::now() }; + updating_state->setNonZeroFinalVel(!ss->isEqual(q_target, q_goal)); + updating_state->setTimeIterStart(time_iter_start); + updating_state->update(q_current, q_current, q_target, status); + planner_info->addRoutineTime(getElapsedTime(time_updateCurrentState, planning::TimeUnit::ms), 0); + // ------------------------------------------------------------------------------- // // Update environment and check if the collision occurs switch (RT_RGBTConfig::TRAJECTORY_INTERPOLATION) { case planning::TrajectoryInterpolation::None: is_valid = motion_validity->check(q_current, q_target); - q_current = q_target; break; case planning::TrajectoryInterpolation::Spline: - update(); - is_valid = motion_validity->check(traj->traj_points_current_iter); + is_valid = motion_validity->check(traj->getTrajPointCurrentIter()); + break; + + case planning::TrajectoryInterpolation::Ruckig: + is_valid = motion_validity->check(traj_ruckig->getTrajPointCurrentIter()); break; } @@ -136,101 +156,7 @@ void planning::drbt::RT_RGBT::computeTargetState() } } - std::cout << "TIME'S UP when finding q_target!!! \n"; -} - -/// @brief Update a current state of the robot using 'traj->spline_current'. -/// Compute a new spline 'traj->spline_next'. -/// Move 'q_current' towards 'q_target' while following 'traj->spline_next'. -/// 'q_current' will be updated to a robot position from the end of current iteration. -/// @note The new trajectory will be computed in a way that all constraints on robot's maximal velocity, -/// acceleration and jerk are surely always satisfied. -void planning::drbt::RT_RGBT::update() -{ - auto time_update { std::chrono::steady_clock::now() }; // Start the algorithm clock - - traj->spline_current = traj->spline_next; - float t_traj_max { TrajectoryConfig::MAX_TIME_COMPUTE_REGULAR }; - float t_iter { getElapsedTime(time_iter_start) }; - if (RT_RGBTConfig::MAX_ITER_TIME - t_iter < t_traj_max) - t_traj_max = RT_RGBTConfig::MAX_ITER_TIME - t_iter; - - float t_iter_remain { RT_RGBTConfig::MAX_ITER_TIME - t_iter - t_traj_max }; - float t_traj_current { traj->spline_current->getTimeEnd() + t_iter + t_traj_max }; - - traj->spline_current->setTimeBegin(traj->spline_current->getTimeEnd()); - traj->spline_current->setTimeCurrent(t_traj_current); - // ----------------------------------------------------------------------------------------- // - - bool traj_computed { false }; - float t_traj_remain { t_traj_max - (getElapsedTime(time_iter_start) - t_iter) }; - // std::cout << "t_traj_remain: " << t_traj_remain * 1e3 << " [ms] \n"; - - if (t_traj_remain > 0) - { - Eigen::VectorXf current_pos { traj->spline_current->getPosition(t_traj_current) }; - Eigen::VectorXf current_vel { traj->spline_current->getVelocity(t_traj_current) }; - Eigen::VectorXf current_acc { traj->spline_current->getAcceleration(t_traj_current) }; - - // std::cout << "Curr. pos: " << current_pos.transpose() << "\n"; - // std::cout << "Curr. vel: " << current_vel.transpose() << "\n"; - // std::cout << "Curr. acc: " << current_acc.transpose() << "\n"; - - traj->setCurrentState(q_current); - traj->setTargetState(q_target); - // std::cout << "q_target: " << q_target << "\n"; - - traj_computed = traj->computeRegular(current_pos, current_vel, current_acc, t_iter_remain, t_traj_remain, true); - - // Try to compute again, but with ZERO final velocity - t_traj_remain = t_traj_max - (getElapsedTime(time_iter_start) - t_iter); - if (traj_computed && traj->spline_next->getTimeFinal() < t_iter_remain && !traj->spline_next->getIsZeroFinalVel() && t_traj_remain > 0) - traj_computed = traj->computeRegular(current_pos, current_vel, current_acc, t_iter_remain, t_traj_remain, false); - } - - if (traj_computed) - { - // std::cout << "New trajectory is computed! \n"; - traj->spline_current->setTimeEnd(t_traj_current); - traj->spline_next->setTimeEnd(t_iter_remain); - } - else - { - // std::cout << "Continuing with the previous trajectory! \n"; - traj->spline_next = traj->spline_current; - traj->spline_next->setTimeEnd(t_traj_current + t_iter_remain); - } - - q_current = ss->getNewState(traj->spline_next->getPosition(traj->spline_next->getTimeEnd())); // Current robot position at the end of iteration - // std::cout << "q_current: " << q_current << "\n"; - - // If 'q_current' is far away from 'q_target', do not change 'q_target' - compute_new_target_state = (ss->getNorm(q_current, q_target) > - traj->spline_next->getVelocity(traj->spline_next->getTimeEnd()).norm() / ss->robot->getMaxVel().norm() * TrajectoryConfig::MAX_RADIUS) ? - false : true; - - planner_info->addRoutineTime(getElapsedTime(time_update, planning::TimeUnit::ms), 0); - // std::cout << "Elapsed time for trajectory computing: " << (getElapsedTime(time_update) - t_iter) * 1e6 << " [us] \n"; - - // ----------------------------------------------------------------------------------------- // - // Store trajectory points from the current iteration to be validated later within 'MotionValidity' - traj->traj_points_current_iter.clear(); - size_t num_checks1 = std::ceil((traj->spline_current->getTimeCurrent() - traj->spline_current->getTimeBegin()) * TrajectoryConfig::NUM_VALIDITY_POINTS_CHECK / RT_RGBTConfig::MAX_ITER_TIME); - float delta_time1 { (traj->spline_current->getTimeCurrent() - traj->spline_current->getTimeBegin()) / num_checks1 }; - float t { 0 }; - for (size_t num_check = 1; num_check <= num_checks1; num_check++) - { - t = traj->spline_current->getTimeBegin() + num_check * delta_time1; - traj->traj_points_current_iter.emplace_back(traj->spline_current->getPosition(t)); - } - - size_t num_checks2 { TrajectoryConfig::NUM_VALIDITY_POINTS_CHECK - num_checks1 }; - float delta_time2 { (traj->spline_next->getTimeEnd() - traj->spline_next->getTimeCurrent()) / num_checks2 }; - for (size_t num_check = 1; num_check <= num_checks2; num_check++) - { - t = traj->spline_next->getTimeCurrent() + num_check * delta_time2; - traj->traj_points_current_iter.emplace_back(traj->spline_next->getPosition(t)); - } + std::cout << "TIME'S UP when finding a new target state!!! Retaining the existing one.\n"; } bool planning::drbt::RT_RGBT::checkTerminatingCondition([[maybe_unused]] base::State::Status status) From 58d7951720cb7da58cf502170d1728df48b06a55 Mon Sep 17 00:00:00 2001 From: Nermin Date: Mon, 6 Oct 2025 10:16:19 +0200 Subject: [PATCH 31/40] "Real" success cannot be negative. --- apps/test_drgbt_random_obstacles.cpp | 7 ++++--- apps/test_rt_rgbt.cpp | 7 ++++--- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/apps/test_drgbt_random_obstacles.cpp b/apps/test_drgbt_random_obstacles.cpp index 7137591e..d45e1158 100644 --- a/apps/test_drgbt_random_obstacles.cpp +++ b/apps/test_drgbt_random_obstacles.cpp @@ -161,10 +161,11 @@ int main(int argc, char **argv) planner = std::make_unique(ss, scenario.getStart(), scenario.getGoal()); bool result { planner->solve() }; - num_real_success_tests += 1 - (goal - planner->getPath().back()->getCoord()).norm() / (goal - start).norm(); + float real_result { std::max(0.0f, 1 - (goal - planner->getPath().back()->getCoord()).norm() / (goal - start).norm()) }; + num_real_success_tests += real_result; LOG(INFO) << planner->getPlannerType() << " planning finished with " << (result ? "SUCCESS!" : "FAILURE!"); - LOG(INFO) << "Real success: " << 1 - (goal - planner->getPath().back()->getCoord()).norm() / (goal - start).norm(); + LOG(INFO) << "Real success: " << real_result; LOG(INFO) << "Number of iterations: " << planner->getPlannerInfo()->getNumIterations(); LOG(INFO) << "Algorithm time: " << planner->getPlannerInfo()->getPlanningTime() << " [s]"; // LOG(INFO) << "Task 1 interrupted: " << (planner->getPlannerInfo()->getTask1Interrupted() ? "true" : "false"); @@ -194,7 +195,7 @@ int main(int argc, char **argv) output_file << "Number of successful tests: " << num_success_tests << " of " << num_test << " = " << 100.0 * num_success_tests / num_test << " %" << std::endl; output_file << "Success:\n" << result << std::endl; - output_file << "Real success:\n" << 1 - (goal - planner->getPath().back()->getCoord()).norm() / (goal - start).norm() << std::endl; + output_file << "Real success:\n" << real_result << std::endl; output_file << "Number of iterations:\n" << planner->getPlannerInfo()->getNumIterations() << std::endl; output_file << "Algorithm execution time [s]:\n" << planner->getPlannerInfo()->getPlanningTime() << std::endl; output_file << "Path length [rad]:\n" << (result ? path_length : INFINITY) << std::endl; diff --git a/apps/test_rt_rgbt.cpp b/apps/test_rt_rgbt.cpp index 72c6fbd2..11a38465 100644 --- a/apps/test_rt_rgbt.cpp +++ b/apps/test_rt_rgbt.cpp @@ -133,10 +133,11 @@ int main(int argc, char **argv) planner = std::make_unique(ss, scenario.getStart(), scenario.getGoal()); bool result { planner->solve() }; - num_real_success_tests += 1 - (goal - planner->getPath().back()->getCoord()).norm() / (goal - start).norm(); + float real_result { std::max(0.0f, 1 - (goal - planner->getPath().back()->getCoord()).norm() / (goal - start).norm()) }; + num_real_success_tests += real_result; LOG(INFO) << planner->getPlannerType() << " planning finished with " << (result ? "SUCCESS!" : "FAILURE!"); - LOG(INFO) << "Real success: " << 1 - (goal - planner->getPath().back()->getCoord()).norm() / (goal - start).norm(); + LOG(INFO) << "Real success: " << real_result; LOG(INFO) << "Number of iterations: " << planner->getPlannerInfo()->getNumIterations(); LOG(INFO) << "Algorithm time: " << planner->getPlannerInfo()->getPlanningTime() << " [s]"; // LOG(INFO) << "Planner data is saved at: " << directory_path + "/test" + std::to_string(init_num_obs) + "_" + std::to_string(num_test) + ".log"; @@ -165,7 +166,7 @@ int main(int argc, char **argv) output_file << "Number of successful tests: " << num_success_tests << " of " << num_test << " = " << 100.0 * num_success_tests / num_test << " %" << std::endl; output_file << "Success:\n" << result << std::endl; - output_file << "Real success:\n" << 1 - (goal - planner->getPath().back()->getCoord()).norm() / (goal - start).norm() << std::endl; + output_file << "Real success:\n" << real_result << std::endl; output_file << "Number of iterations:\n" << planner->getPlannerInfo()->getNumIterations() << std::endl; output_file << "Algorithm execution time [s]:\n" << planner->getPlannerInfo()->getPlanningTime() << std::endl; output_file << "Path length [rad]:\n" << (result ? path_length : INFINITY) << std::endl; From 745a4dea523623701f6f6fbb5bdb1788d2077776 Mon Sep 17 00:00:00 2001 From: Nermin Covic <52970761+ncovic1@users.noreply.github.com> Date: Mon, 6 Oct 2025 16:40:33 +0200 Subject: [PATCH 32/40] Updated rpmplv2.yaml to build with external Ruckig library. --- .github/workflows/rpmplv2.yaml | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/.github/workflows/rpmplv2.yaml b/.github/workflows/rpmplv2.yaml index d26abdd8..161c6b24 100644 --- a/.github/workflows/rpmplv2.yaml +++ b/.github/workflows/rpmplv2.yaml @@ -10,7 +10,13 @@ jobs: runs-on: ubuntu-22.04 steps: - uses: actions/checkout@v3 - + with: + submodules: true # make sure submodules are fetched + fetch-depth: 0 # fetch full history (needed for submodules) + + - name: Init submodules + run: git submodule update --init --recursive + - name: Install dependencies run: | sudo apt update && sudo apt upgrade && sudo apt install -y libunwind-dev apt-transport-https \ From 43fea54b988701244334d90d256d3f6c068be81f Mon Sep 17 00:00:00 2001 From: Nermin Date: Mon, 6 Oct 2025 16:47:52 +0200 Subject: [PATCH 33/40] Adding external/ruckig folder --- external/ruckig | 1 + 1 file changed, 1 insertion(+) create mode 160000 external/ruckig diff --git a/external/ruckig b/external/ruckig new file mode 160000 index 00000000..37b6e7a4 --- /dev/null +++ b/external/ruckig @@ -0,0 +1 @@ +Subproject commit 37b6e7a4c60f5befd2506741d5f7d0ae7eefb3db From fb69664de1d899e1740b467221347fd8f5026e4b Mon Sep 17 00:00:00 2001 From: Nermin Covic Date: Thu, 16 Oct 2025 15:00:41 +0200 Subject: [PATCH 34/40] ReadMe updated. --- CMakeLists.txt | 2 -- README.md | 4 +++- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5d88fd36..de2161bc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -62,8 +62,6 @@ find_package(yaml-cpp REQUIRED) find_package(fcl 0.7 REQUIRED) find_package(nanoflann REQUIRED) -# If any problems occur about which version of Ruckig is built, just type the following in terminal: -# export LD_LIBRARY_PATH=/build/rpmpl_library/external/ruckig:$LD_LIBRARY_PATH``` -Finally, in the file ```/apps/test_drgbt_random_obstacles.cpp```, you can set via ```routines``` which routines' execution times should be stored during the testing. File ```/data/xarm6/scenario_random_obstacles/scenario_random_obstacles_routine_times.log``` will contain all logged execution times. +Finally, in the file ```/apps/test_drgbt_random_obstacles.cpp```, you can set via ```routines``` which routines' execution times should be stored during the testing. File ```/data/xarm6/scenario_random_obstacles/DRGBT_data_/results_obs_ms.log``` will contain all logged execution times. ## 3.4 Test planners All test files are available within the folder ```/apps```. For example, open ```test_rgbmtstar.cpp```. You can set the file path of desired scenario via ```scenario_file_path```, and maximal number of tests in ```max_num_tests```. From a8683d2c318cc1c3de64d14306ed83d85b05c303 Mon Sep 17 00:00:00 2001 From: Nermin Covic Date: Sat, 18 Oct 2025 20:03:39 +0200 Subject: [PATCH 35/40] 'AbstractTrajectory' class is created. --- .../planners/trajectory/AbstractTrajectory.h | 84 ++++++++++ .../trajectory/AbstractTrajectory.cpp | 150 ++++++++++++++++++ 2 files changed, 234 insertions(+) create mode 100644 include/planners/trajectory/AbstractTrajectory.h create mode 100644 src/planners/trajectory/AbstractTrajectory.cpp diff --git a/include/planners/trajectory/AbstractTrajectory.h b/include/planners/trajectory/AbstractTrajectory.h new file mode 100644 index 00000000..eb6b9d56 --- /dev/null +++ b/include/planners/trajectory/AbstractTrajectory.h @@ -0,0 +1,84 @@ +// +// Created by nermin on 17.10.25. +// + +#ifndef RPMPL_ABSTRACTTRAJECTORY_H +#define RPMPL_ABSTRACTTRAJECTORY_H + +#include "StateSpace.h" +#include "TrajectoryConfig.h" +#include "RealVectorSpaceConfig.h" + +namespace planning::trajectory +{ + class State + { + public: + State() {} + State(size_t num_DOFs); + State(const Eigen::VectorXf &pos_); + State(const Eigen::VectorXf &pos_, const Eigen::VectorXf &vel_, const Eigen::VectorXf &acc_); + + Eigen::VectorXf pos; + Eigen::VectorXf vel; + Eigen::VectorXf acc; + }; + + class AbstractTrajectory + { + public: + AbstractTrajectory(const std::shared_ptr &ss_); + AbstractTrajectory(const std::shared_ptr &ss_, planning::trajectory::State current_, float max_iter_time_); + virtual ~AbstractTrajectory() = 0; + + virtual bool computeRegular(planning::trajectory::State current_, planning::trajectory::State target_, + float t_iter_remain, float t_max, bool non_zero_final_vel) = 0; + virtual bool computeSafe(planning::trajectory::State current_, planning::trajectory::State target_, + float t_iter_remain, float t_max, const std::shared_ptr q_current) = 0; + + virtual Eigen::VectorXf getPosition(float t) = 0; + virtual Eigen::VectorXf getVelocity(float t) = 0; + virtual Eigen::VectorXf getAcceleration(float t) = 0; + + float getTimeCurrent(bool measure_time = false); + inline float getTimeBegin() const { return time_begin; } + inline float getTimeEnd() const { return time_end; } + inline float getTimeFinal() const { return time_final; } + inline bool getIsZeroFinalVel() const { return is_zero_final_vel; } + inline const std::vector &getTrajPointCurrentIter() const { return traj_points_current_iter; } + + void setTimeStart(float time_start_offset_); + inline void setTimeBegin(float time_begin_) { time_begin = time_begin_; } + inline void setTimeEnd(float time_end_) { time_end = time_end_; } + inline void setTimeCurrent(float time_current_) { time_current = time_current_; } + inline void setMaxRemainingIterTime(float max_remaining_iter_time_) { max_remaining_iter_time = max_remaining_iter_time_; } + + bool isFinalConf(const Eigen::VectorXf &pos); + void addTrajPointCurrentIter(const Eigen::VectorXf &pos); + void clearTrajPointCurrentIter(); + void recordTrajectory(bool traj_computed); + + protected: + void setParams(); + + std::shared_ptr ss; // State space of the robot + planning::trajectory::State current; // Current robot's state + planning::trajectory::State target; // Target robot's state + float max_iter_time; // Maximal iteration time + float max_remaining_iter_time; // Maximal remaining iteration time in [s] till the end of the current iteration + float time_current; // Elapsed time in [s] from a time instant when the trajectory is created. It is used to determine a current robot's position, velocity and acceleration. + float time_final; // Final time for the trajectory + float time_begin; // Time instance in [s] when the trajectory begins in the current iteration + float time_end; // Time instance in [s] when the trajectory ends in the current iteration + std::chrono::steady_clock::time_point time_start; // Start time point when the trajectory is created + float time_start_offset; // Time offset in [s] which determines how much earlier 'time_start' is created + bool is_zero_final_vel; // Whether final velocity is zero. If not, robot will move at constant velocity after 'time_final'. + bool all_robot_vel_same; // Whether all joint velocities are the same + float max_obs_vel; // Maximal velocity of dynamic obstacles used to generate dynamic bubbles + size_t max_num_iter_trajectory; // Maximal number of iterations when computing trajectory + std::vector traj_points_current_iter; // Trajectory points from the current iteration to be validated within 'MotionValidity' + + }; +} + +#endif //RPMPL_ABSTRACTTRAJECTORY_H \ No newline at end of file diff --git a/src/planners/trajectory/AbstractTrajectory.cpp b/src/planners/trajectory/AbstractTrajectory.cpp new file mode 100644 index 00000000..b0c80908 --- /dev/null +++ b/src/planners/trajectory/AbstractTrajectory.cpp @@ -0,0 +1,150 @@ +#include "AbstractTrajectory.h" + +planning::trajectory::State::State(size_t num_DOFs) +{ + pos = Eigen::VectorXf::Zero(num_DOFs); + vel = Eigen::VectorXf::Zero(num_DOFs); + acc = Eigen::VectorXf::Zero(num_DOFs); +} + +planning::trajectory::State::State(const Eigen::VectorXf &pos_) +{ + pos = pos_; + vel = Eigen::VectorXf::Zero(pos.size()); + acc = Eigen::VectorXf::Zero(pos.size()); +} + +planning::trajectory::State::State(const Eigen::VectorXf &pos_, const Eigen::VectorXf &vel_, const Eigen::VectorXf &acc_) +{ + pos = pos_; + vel = vel_; + acc = acc_; +} + +planning::trajectory::AbstractTrajectory::AbstractTrajectory(const std::shared_ptr &ss_) +{ + ss = ss_; + current = planning::trajectory::State(ss->num_dimensions); + target = current; + + max_iter_time = 0; + max_remaining_iter_time = INFINITY; + max_obs_vel = 0; + + setParams(); +} + +planning::trajectory::AbstractTrajectory::AbstractTrajectory + (const std::shared_ptr &ss_, planning::trajectory::State current_, float max_iter_time_) +{ + ss = ss_; + current = current_; + target = current; + + max_iter_time = max_iter_time_; + max_remaining_iter_time = 0; + time_current = 0; + time_final = 0; + time_begin = 0; + time_end = 0; + time_start = std::chrono::steady_clock::now(); + time_start_offset = 0; + is_zero_final_vel = true; + + max_obs_vel = 0; + for (size_t i = 0; i < ss->env->getNumObjects(); i++) + { + if (ss->env->getObject(i)->getMaxVel() > max_obs_vel) + max_obs_vel = ss->env->getObject(i)->getMaxVel(); + } + + setParams(); +} + +planning::trajectory::AbstractTrajectory::~AbstractTrajectory() {} + +void planning::trajectory::AbstractTrajectory::setParams() +{ + traj_points_current_iter = {}; + + all_robot_vel_same = true; + for (size_t i = 1; i < ss->num_dimensions; i++) + { + if (std::abs(ss->robot->getMaxVel(i) - ss->robot->getMaxVel(i-1)) > RealVectorSpaceConfig::EQUALITY_THRESHOLD) + { + all_robot_vel_same = false; + break; + } + } + + max_num_iter_trajectory = all_robot_vel_same ? + std::ceil(std::log2(2 * ss->robot->getMaxVel(0) / TrajectoryConfig::FINAL_VELOCITY_STEP)) : + std::ceil(std::log2(2 * ss->robot->getMaxVel().maxCoeff() / TrajectoryConfig::FINAL_VELOCITY_STEP)); +} + +/// @brief Get current time of the trajectory. +/// @param measure_time If true, current time will be automatically computed/measured (default: false). +/// @note 'measure_time' should always be false when simulation pacing is used, since then a time measuring will not be correct! +/// In such case, it is assumed that user was previously set 'measure_time' to a correct value. +float planning::trajectory::AbstractTrajectory::getTimeCurrent(bool measure_time) +{ + if (!measure_time) + return time_current; + + time_current = std::chrono::duration_cast + (std::chrono::steady_clock::now() - time_start).count() * 1e-9 - time_start_offset; + return time_current; +} + +void planning::trajectory::AbstractTrajectory::setTimeStart(float time_start_offset_) +{ + time_start = std::chrono::steady_clock::now(); + time_start_offset = time_start_offset_; +} + +/// @brief Check whether 'q' is a final configuration of the trajectory. +/// @param pos Configuration to be checked. +/// @return True if yes, false if not. +bool planning::trajectory::AbstractTrajectory::isFinalConf(const Eigen::VectorXf &pos) +{ + return ((pos - getPosition(time_final)).norm() < RealVectorSpaceConfig::EQUALITY_THRESHOLD) ? true : false; +} + +void planning::trajectory::AbstractTrajectory::addTrajPointCurrentIter(const Eigen::VectorXf &pos) +{ + traj_points_current_iter.emplace_back(pos); +} + +void planning::trajectory::AbstractTrajectory::clearTrajPointCurrentIter() +{ + traj_points_current_iter.clear(); +} + +// This function is just for debugging. It operates in real-time by logging 'spline_current' and 'spline'. +// You can hardcode a desired output path for the file to be saved. +void planning::trajectory::AbstractTrajectory::recordTrajectory(bool traj_computed) +{ + std::ofstream output_file {}; + output_file.open("/home/spear/xarm6-etf-lab/src/etf_modules/RPMPLv2/data/planar_2dof/scenario_random_obstacles/temp/visualize_trajectory.log", + std::ofstream::app); + + output_file << "Current pos. & target pos.: \n"; + if (traj_computed) + { + output_file << current.pos.transpose() << "\n"; + output_file << target.pos.transpose() << "\n"; + } + else + output_file << INFINITY << "\n"; + + output_file << "All points: \n"; + for (float t = getTimeCurrent(); t < getTimeFinal(); t += 0.001) + output_file << getPosition(t).transpose() << "\n"; + output_file << getPosition(getTimeFinal()).transpose() << "\n"; + + output_file << "Only points until the end of the current iteration: \n"; + for (float t = getTimeCurrent(); t < getTimeEnd(); t += 0.001) + output_file << getPosition(t).transpose() << "\n"; + output_file << getPosition(getTimeEnd()).transpose() << "\n"; + output_file << "--------------------------------------------------------------------\n"; +} From 818b6e09af25228a9dd137134c4248d10e2d51f5 Mon Sep 17 00:00:00 2001 From: Nermin Covic Date: Sat, 18 Oct 2025 21:47:07 +0200 Subject: [PATCH 36/40] A lot of corresponding changes... --- .../planners/trajectory/AbstractTrajectory.h | 8 +- include/planners/trajectory/Trajectory.h | 47 +- .../planners/trajectory/TrajectoryRuckig.h | 70 +-- .../trajectory/AbstractTrajectory.cpp | 15 +- src/planners/trajectory/Trajectory.cpp | 416 ++++++++---------- src/planners/trajectory/TrajectoryRuckig.cpp | 168 +++---- 6 files changed, 288 insertions(+), 436 deletions(-) diff --git a/include/planners/trajectory/AbstractTrajectory.h b/include/planners/trajectory/AbstractTrajectory.h index eb6b9d56..33fea9fd 100644 --- a/include/planners/trajectory/AbstractTrajectory.h +++ b/include/planners/trajectory/AbstractTrajectory.h @@ -28,12 +28,12 @@ namespace planning::trajectory { public: AbstractTrajectory(const std::shared_ptr &ss_); - AbstractTrajectory(const std::shared_ptr &ss_, planning::trajectory::State current_, float max_iter_time_); + AbstractTrajectory(const std::shared_ptr &ss_, float max_iter_time_); virtual ~AbstractTrajectory() = 0; - virtual bool computeRegular(planning::trajectory::State current_, planning::trajectory::State target_, + virtual bool computeRegular(planning::trajectory::State current, planning::trajectory::State target, float t_iter_remain, float t_max, bool non_zero_final_vel) = 0; - virtual bool computeSafe(planning::trajectory::State current_, planning::trajectory::State target_, + virtual bool computeSafe(planning::trajectory::State current, planning::trajectory::State target, float t_iter_remain, float t_max, const std::shared_ptr q_current) = 0; virtual Eigen::VectorXf getPosition(float t) = 0; @@ -62,8 +62,6 @@ namespace planning::trajectory void setParams(); std::shared_ptr ss; // State space of the robot - planning::trajectory::State current; // Current robot's state - planning::trajectory::State target; // Target robot's state float max_iter_time; // Maximal iteration time float max_remaining_iter_time; // Maximal remaining iteration time in [s] till the end of the current iteration float time_current; // Elapsed time in [s] from a time instant when the trajectory is created. It is used to determine a current robot's position, velocity and acceleration. diff --git a/include/planners/trajectory/Trajectory.h b/include/planners/trajectory/Trajectory.h index 62846948..049c3ea1 100644 --- a/include/planners/trajectory/Trajectory.h +++ b/include/planners/trajectory/Trajectory.h @@ -5,7 +5,7 @@ #ifndef RPMPL_TRAJECTORY_H #define RPMPL_TRAJECTORY_H -#include "StateSpace.h" +#include "AbstractTrajectory.h" #include "Spline4.h" #include "Spline5.h" #include "CompositeSpline.h" @@ -13,49 +13,34 @@ namespace planning::trajectory { - class Trajectory + class Trajectory : public AbstractTrajectory { public: Trajectory(const std::shared_ptr &ss_); - Trajectory(const std::shared_ptr &ss_, const std::shared_ptr &q_current_, float max_iter_time_); + Trajectory(const std::shared_ptr &ss_, planning::trajectory::State current, float max_iter_time_); + ~Trajectory(); - bool computeRegular(const Eigen::VectorXf ¤t_pos, const Eigen::VectorXf ¤t_vel, const Eigen::VectorXf ¤t_acc, - float t_iter_remain, float t_max, bool non_zero_final_vel); - bool computeSafe(const Eigen::VectorXf ¤t_pos, const Eigen::VectorXf ¤t_vel, const Eigen::VectorXf ¤t_acc, - float t_iter_remain, float t_max); + bool computeRegular(planning::trajectory::State current, planning::trajectory::State target, + float t_iter_remain, float t_max, bool non_zero_final_vel) override; + bool computeSafe(planning::trajectory::State current, planning::trajectory::State target, + float t_iter_remain, float t_max, const std::shared_ptr q_current) override; - void addTrajPointCurrentIter(const Eigen::VectorXf &pos); - void clearTrajPointCurrentIter(); + inline Eigen::VectorXf getPosition(float t) override { return spline->getPosition(t); } + inline Eigen::VectorXf getVelocity(float t) override { return spline->getVelocity(t); } + inline Eigen::VectorXf getAcceleration(float t) override { return spline->getAcceleration(t); } void path2traj_v1(const std::vector> &path); void path2traj_v2(const std::vector> &path); void path2traj_v3(const std::vector> &path, bool must_visit); - inline const std::vector &getTrajPointCurrentIter() const { return traj_points_current_iter; } - inline void setCurrentState(const std::shared_ptr q_current_) { q_current = q_current_; } - inline void setTargetState(const std::shared_ptr q_target_) { q_target = q_target_; } - inline void setMaxRemainingIterTime(float max_remaining_iter_time_) { max_remaining_iter_time = max_remaining_iter_time_; } - void recordTrajectory(bool spline_computed); - - std::shared_ptr spline_current; // Current spline that 'q_current' is following in the current iteration - std::shared_ptr spline_next; // Next spline generated from 'q_current' to 'q_target' - std::shared_ptr composite_spline; // Composite spline from the start to a desired target configuration - private: - void setParams(); - bool checkCollision(std::shared_ptr q_init, float t_iter); + bool isSafe(const std::shared_ptr spline_safe, + const std::shared_ptr q_current, float t_iter); float computeDistanceUnderestimation(const std::shared_ptr q, const std::shared_ptr> nearest_points, float delta_t); - - std::shared_ptr ss; - std::shared_ptr q_current; // Current robot configuration - std::shared_ptr q_target; // Target robot configuration to which the robot is currently heading to, as well as the configuration where the spline is ending - bool all_robot_vel_same; // Whether all joint velocities are the same - float max_obs_vel; // Maximal velocity of dynamic obstacles used to generate dynamic bubbles - size_t max_num_iter_spline_regular; // Maximal number of iterations when computing regular spline - float max_iter_time; // Maximal iteration time - float max_remaining_iter_time; // Maximal remaining iteration time till the end of the current iteration - std::vector traj_points_current_iter; // Trajectory points from the current iteration to be validated within 'MotionValidity' + void setSpline(const std::shared_ptr spline_); + + std::shared_ptr spline; }; } diff --git a/include/planners/trajectory/TrajectoryRuckig.h b/include/planners/trajectory/TrajectoryRuckig.h index a91efdd4..c036ba50 100644 --- a/include/planners/trajectory/TrajectoryRuckig.h +++ b/include/planners/trajectory/TrajectoryRuckig.h @@ -2,73 +2,39 @@ // Created by nermin on 03.10.25. // -#ifndef RPMPL_TrajectoryRuckig_H -#define RPMPL_TrajectoryRuckig_H - -#include "StateSpace.h" -#include "TrajectoryConfig.h" -#include "RealVectorSpaceConfig.h" +#ifndef RPMPL_TRAJECTORYRUCKIG_H +#define RPMPL_TRAJECTORYRUCKIG_H +#include "AbstractTrajectory.h" #include namespace planning::trajectory { - class TrajectoryRuckig + class TrajectoryRuckig : public AbstractTrajectory { public: - TrajectoryRuckig(const std::shared_ptr &ss_, const Eigen::VectorXf &q_current_, float max_iter_time_); - ~TrajectoryRuckig() {} + TrajectoryRuckig(const std::shared_ptr &ss_); + TrajectoryRuckig(const std::shared_ptr &ss_, planning::trajectory::State current, float max_iter_time_); + ~TrajectoryRuckig(); - bool computeRegular(const Eigen::VectorXf ¤t_pos, const Eigen::VectorXf ¤t_vel, - const Eigen::VectorXf ¤t_acc, float t_iter_remain, float t_max, bool non_zero_final_vel); + bool computeRegular(planning::trajectory::State current, planning::trajectory::State target, + float t_iter_remain, float t_max, bool non_zero_final_vel) override; + bool computeSafe(planning::trajectory::State current, planning::trajectory::State target, + float t_iter_remain, float t_max, const std::shared_ptr q_current) override; - void setCurrentState(const Eigen::VectorXf ¤t_pos, const Eigen::VectorXf ¤t_vel, const Eigen::VectorXf ¤t_acc); - void setTargetState(const Eigen::VectorXf &target_pos, const Eigen::VectorXf &target_vel, const Eigen::VectorXf &target_acc); - Eigen::VectorXf getPosition(float t); - Eigen::VectorXf getVelocity(float t); - Eigen::VectorXf getAcceleration(float t); - void addTrajPointCurrentIter(const Eigen::VectorXf &pos); - void clearTrajPointCurrentIter(); - bool isFinalConf(const Eigen::VectorXf &q); - - float getTimeCurrent(bool measure_time = false); - inline float getTimeBegin() const { return time_begin; } - inline float getTimeEnd() const { return time_end; } - inline float getTimeFinal() const { return time_final; } - inline bool getIsZeroFinalVel() const { return is_zero_final_vel; } - inline const std::vector &getTrajPointCurrentIter() const { return traj_points_current_iter; } - - void setTimeStart(float time_start_offset_); - inline void setTimeBegin(float time_begin_) { time_begin = time_begin_; } - inline void setTimeEnd(float time_end_) { time_end = time_end_; } - inline void setCurrentState(const Eigen::VectorXf &q_current_) { q_current = q_current_; } - inline void setTargetState(const Eigen::VectorXf &q_target_) { q_target = q_target_; } - inline void setTimeCurrent(float time_current_) { time_current = time_current_; } + Eigen::VectorXf getPosition(float t) override; + Eigen::VectorXf getVelocity(float t) override; + Eigen::VectorXf getAcceleration(float t) override; private: + void setCurrentState(const planning::trajectory::State ¤t); + void setTargetState(const planning::trajectory::State &target); + ruckig::InputParameter input; // ruckig::OutputParameter output; ruckig::Trajectory traj; - ruckig::Trajectory traj_temp; - - std::shared_ptr ss; - Eigen::VectorXf q_current; // Current robot configuration - Eigen::VectorXf q_target; // Target robot configuration to which the robot is currently heading to, as well as the configuration where the trajectory is ending - bool all_robot_vel_same; // Whether all joint velocities are the same - size_t max_num_iter_trajectory; // Maximal number of iterations when computing trajectory - float max_iter_time; // Maximal iteration time - float remaining_iter_time; // Remaining iteration time till the end of the current iteration - float time_current; // Current time for a trajectory - float time_final; // Final time for a trajectory - float time_final_temp; // Final time (temp) for a trajectory - float time_begin; // Time instance in [s] when a trajectory begins in the current iteration - float time_end; // Time instance in [s] when a trajectory ends in the current iteration - std::chrono::steady_clock::time_point time_start; // Start time point when a trajectory is created - float time_start_offset; // Time offset in [s] which determines how much earlier 'time_start' is created - bool is_zero_final_vel; // Whether final velocity is zero. If not, robot will move at constant velocity after 'time_final'. - std::vector traj_points_current_iter; // Trajectory points from the current iteration to be validated within 'MotionValidity' }; } -#endif //RPMPL_TrajectoryRuckig_H \ No newline at end of file +#endif //RPMPL_TRAJECTORYRUCKIG_H \ No newline at end of file diff --git a/src/planners/trajectory/AbstractTrajectory.cpp b/src/planners/trajectory/AbstractTrajectory.cpp index b0c80908..c62c0fcf 100644 --- a/src/planners/trajectory/AbstractTrajectory.cpp +++ b/src/planners/trajectory/AbstractTrajectory.cpp @@ -23,10 +23,7 @@ planning::trajectory::State::State(const Eigen::VectorXf &pos_, const Eigen::Vec planning::trajectory::AbstractTrajectory::AbstractTrajectory(const std::shared_ptr &ss_) { - ss = ss_; - current = planning::trajectory::State(ss->num_dimensions); - target = current; - + ss = ss_; max_iter_time = 0; max_remaining_iter_time = INFINITY; max_obs_vel = 0; @@ -34,13 +31,9 @@ planning::trajectory::AbstractTrajectory::AbstractTrajectory(const std::shared_p setParams(); } -planning::trajectory::AbstractTrajectory::AbstractTrajectory - (const std::shared_ptr &ss_, planning::trajectory::State current_, float max_iter_time_) +planning::trajectory::AbstractTrajectory::AbstractTrajectory(const std::shared_ptr &ss_, float max_iter_time_) { ss = ss_; - current = current_; - target = current; - max_iter_time = max_iter_time_; max_remaining_iter_time = 0; time_current = 0; @@ -131,8 +124,8 @@ void planning::trajectory::AbstractTrajectory::recordTrajectory(bool traj_comput output_file << "Current pos. & target pos.: \n"; if (traj_computed) { - output_file << current.pos.transpose() << "\n"; - output_file << target.pos.transpose() << "\n"; + output_file << getPosition(time_current).transpose() << "\n"; + output_file << getPosition(time_final).transpose() << "\n"; } else output_file << INFINITY << "\n"; diff --git a/src/planners/trajectory/Trajectory.cpp b/src/planners/trajectory/Trajectory.cpp index cd5c495f..d8308672 100644 --- a/src/planners/trajectory/Trajectory.cpp +++ b/src/planners/trajectory/Trajectory.cpp @@ -1,157 +1,115 @@ #include "Trajectory.h" -planning::trajectory::Trajectory::Trajectory(const std::shared_ptr &ss_) +planning::trajectory::Trajectory::Trajectory(const std::shared_ptr &ss_) : + AbstractTrajectory(ss_) { - ss = ss_; - q_current = nullptr; - max_iter_time = 0; - max_remaining_iter_time = INFINITY; - max_obs_vel = 0; - spline_current = nullptr; - spline_next = nullptr; - composite_spline = nullptr; - setParams(); + spline = nullptr; } -planning::trajectory::Trajectory::Trajectory(const std::shared_ptr &ss_, - const std::shared_ptr &q_current_, float max_iter_time_) +planning::trajectory::Trajectory::Trajectory + (const std::shared_ptr &ss_, planning::trajectory::State current, float max_iter_time_) : + AbstractTrajectory(ss_, max_iter_time) { - ss = ss_; - q_current = q_current_; - max_iter_time = max_iter_time_; - max_remaining_iter_time = 0; - - max_obs_vel = 0; - for (size_t i = 0; i < ss->env->getNumObjects(); i++) - { - if (ss->env->getObject(i)->getMaxVel() > max_obs_vel) - max_obs_vel = ss->env->getObject(i)->getMaxVel(); - } - - spline_current = std::make_shared(ss->robot, q_current->getCoord()); - spline_next = spline_current; - composite_spline = nullptr; - setParams(); + spline = std::make_shared(ss->robot, current.pos, current.vel, current.acc); } -void planning::trajectory::Trajectory::setParams() -{ - traj_points_current_iter = {}; - - all_robot_vel_same = true; - for (size_t i = 1; i < ss->num_dimensions; i++) - { - if (std::abs(ss->robot->getMaxVel(i) - ss->robot->getMaxVel(i-1)) > RealVectorSpaceConfig::EQUALITY_THRESHOLD) - { - all_robot_vel_same = false; - break; - } - } - - max_num_iter_spline_regular = all_robot_vel_same ? - std::ceil(std::log2(2 * ss->robot->getMaxVel(0) / TrajectoryConfig::FINAL_VELOCITY_STEP)) : - std::ceil(std::log2(2 * ss->robot->getMaxVel().maxCoeff() / TrajectoryConfig::FINAL_VELOCITY_STEP)); -} +planning::trajectory::Trajectory::~Trajectory() {} /// @brief Compute a regular spline that is not surely safe for environment, meaning that, /// if collision eventually occurs, it may be at robot's non-zero velocity. -/// @param current_pos Current robot's position -/// @param current_vel Current robot's velocity -/// @param current_acc Current robot's acceleration +/// @param current Current robot's state +/// @param target Target robot's state /// @param t_iter_remain Remaining time in [s] in the current iteration /// @param t_max Maximal available time in [s] for a spline computing /// @param non_zero_final_vel Whether final spline velocity can be non-zero /// @return The success of a spline computation -bool planning::trajectory::Trajectory::computeRegular(const Eigen::VectorXf ¤t_pos, const Eigen::VectorXf ¤t_vel, - const Eigen::VectorXf ¤t_acc, float t_iter_remain, float t_max, bool non_zero_final_vel) +bool planning::trajectory::Trajectory::computeRegular(planning::trajectory::State current, + planning::trajectory::State target, float t_iter_remain, float t_max, bool non_zero_final_vel) { - std::chrono::steady_clock::time_point time_start_ { std::chrono::steady_clock::now() }; + std::chrono::steady_clock::time_point time_start_ { std::chrono::steady_clock::now() }; float t_remain { t_max }; - std::shared_ptr spline_next_new - { std::make_shared(ss->robot, current_pos, current_vel, current_acc) }; bool spline_computed { false }; + std::shared_ptr spline_new + { std::make_shared(ss->robot, current.pos, current.vel, current.acc) }; if (non_zero_final_vel) { + is_zero_final_vel = false; size_t num_iter { 0 }; - float delta_t_max { ((q_target->getCoord() - q_current->getCoord()).cwiseQuotient(ss->robot->getMaxVel())).cwiseAbs().maxCoeff() }; - Eigen::VectorXf q_final_dot_max { (q_target->getCoord() - q_current->getCoord()) / delta_t_max }; - Eigen::VectorXf q_final_dot_min { Eigen::VectorXf::Zero(ss->num_dimensions) }; - Eigen::VectorXf q_final_dot { q_final_dot_max }; + float delta_t_max { ((target.pos - current.pos).cwiseQuotient(ss->robot->getMaxVel())).cwiseAbs().maxCoeff() }; + Eigen::VectorXf target_vel_max { (target.pos - current.pos) / delta_t_max }; + Eigen::VectorXf target_vel_min { Eigen::VectorXf::Zero(ss->num_dimensions) }; + Eigen::VectorXf target_vel { target_vel_max }; - while (!spline_computed && num_iter++ < max_num_iter_spline_regular && t_remain > 0) + while (!spline_computed && num_iter++ < max_num_iter_trajectory && t_remain > 0) { - if (spline_next_new->compute(q_target->getCoord(), q_final_dot)) + if (spline_new->compute(target.pos, target_vel)) { - spline_next = spline_next_new; spline_computed = true; + setSpline(spline_new); } else - q_final_dot_max = q_final_dot; + target_vel_max = target_vel; - q_final_dot = (q_final_dot_max + q_final_dot_min) / 2; - t_remain -= std::chrono::duration_cast(std::chrono::steady_clock::now() - time_start_).count() / 1e6; - // std::cout << "Num. iter. " << num_iter << "\t q_final_dot: " << q_final_dot.transpose() << "\n"; + target_vel = (target_vel_max + target_vel_min) / 2; + t_remain -= std::chrono::duration_cast + (std::chrono::steady_clock::now() - time_start_).count() / 1e6; + // std::cout << "Num. iter. " << num_iter << "\t target_vel: " << target_vel.transpose() << "\n"; } - - // if (spline_computed) std::cout << "\t Spline computed with NON-ZERO final velocity. \n"; } // Possible current position at the end of iteration - Eigen::VectorXf new_current_pos { spline_computed ? - spline_next->getPosition(t_iter_remain) : - spline_current->getPosition(spline_current->getTimeCurrent() + t_iter_remain) - }; + Eigen::VectorXf new_current_pos { getPosition(time_current + t_iter_remain) }; // If spline was not computed or robot is getting away from 'new_current_pos' - if ((!spline_computed || (new_current_pos - q_target->getCoord()).norm() > (current_pos - q_target->getCoord()).norm()) && - t_remain > 0) + if ((!spline_computed || (new_current_pos - target.pos).norm() > (current.pos - target.pos).norm()) && t_remain > 0) { - spline_computed = spline_next_new->compute(q_target->getCoord()); + is_zero_final_vel = true; + spline_computed = spline_new->compute(target.pos); if (spline_computed) - { - spline_next = spline_next_new; - // std::cout << "\t Spline computed with ZERO final velocity. \n"; - } + setSpline(spline_new); } + std::cout << "\t Spline " << (!spline_computed ? "NOT " : "") << "computed with " + << (!is_zero_final_vel ? "NON-" : "") << "ZERO final velocity. \n"; + std::cout << "Spline: \n" << spline << "\n"; return spline_computed; } /// @brief Compute a safe spline that will render a robot motion surely safe for environment. /// If collision eventually occurs, it will be at robot's zero velocity, meaning that an obstacle hit the robot, and not vice versa. -/// @param current_pos Current robot's position -/// @param current_vel Current robot's velocity -/// @param current_acc Current robot's acceleration +/// @param current Current robot's state +/// @param target Target robot's state /// @param t_iter_remain Remaining time in [s] in the current iteration /// @param t_max Maximal available time in [s] for a spline computing +/// @param q_current Current robot's configuration /// @return The success of a spline computation -bool planning::trajectory::Trajectory::computeSafe(const Eigen::VectorXf ¤t_pos, const Eigen::VectorXf ¤t_vel, - const Eigen::VectorXf ¤t_acc, float t_iter_remain, float t_max) +bool planning::trajectory::Trajectory::computeSafe(planning::trajectory::State current, + planning::trajectory::State target, float t_iter_remain, float t_max, const std::shared_ptr q_current) { std::chrono::steady_clock::time_point time_start_ { std::chrono::steady_clock::now() }; - std::shared_ptr spline_next_new - { std::make_shared(ss->robot, current_pos, current_vel, current_acc) }; - std::shared_ptr spline_next_temp - { std::make_shared(ss->robot, current_pos, current_vel, current_acc) }; - std::shared_ptr spline_emergency_new - { std::make_shared(ss->robot, current_pos, current_vel, current_acc) }; - std::shared_ptr spline_emergency_temp { nullptr }; + std::shared_ptr spline_new + { std::make_shared(ss->robot, current.pos, current.vel, current.acc) }; + std::shared_ptr spline_temp + { std::make_shared(ss->robot, current.pos, current.vel, current.acc) }; + std::shared_ptr spline_emg_new + { std::make_shared(ss->robot, current.pos, current.vel, current.acc) }; + std::shared_ptr spline_emg_temp { nullptr }; float rho_robot {}; float rho_obs {}; bool is_safe {}; bool spline_computed { false }; - bool spline_emergency_computed { false }; + bool spline_emg_computed { false }; float t_iter { max_iter_time - t_iter_remain }; float t_spline_max { t_iter_remain + (max_iter_time - max_remaining_iter_time) }; int num_iter { 0 }; int max_num_iter = std::ceil(std::log2(RealVectorSpaceConfig::NUM_INTERPOLATION_VALIDITY_CHECKS * - (current_pos - q_target->getCoord()).norm() / RRTConnectConfig::EPS_STEP)); + (current.pos - target.pos).norm() / RRTConnectConfig::EPS_STEP)); if (max_num_iter <= 0) max_num_iter = 1; - Eigen::VectorXf q_final_min { current_pos }; - Eigen::VectorXf q_final_max { q_target->getCoord() }; - Eigen::VectorXf q_final { q_target->getCoord() }; + Eigen::VectorXf target_pos_min { current.pos }; + Eigen::VectorXf target_pos_max { target.pos }; + Eigen::VectorXf target_pos { target.pos }; auto computeRho = [&](Eigen::VectorXf q_coord) -> float { @@ -168,47 +126,47 @@ bool planning::trajectory::Trajectory::computeSafe(const Eigen::VectorXf ¤ { is_safe = false; // std::cout << "Num. iter. " << num_iter << "\n"; - // std::cout << "q_final: " << q_final.transpose() << "\n"; + // std::cout << "target_pos: " << target_pos.transpose() << "\n"; - if (spline_next_temp->compute(q_final)) + if (spline_temp->compute(target_pos)) { // std::cout << "Spline is computed! \n"; - if (spline_next_temp->getTimeFinal() < t_spline_max) + if (spline_temp->getTimeFinal() < t_spline_max) { - rho_obs = max_obs_vel * (t_iter + spline_next_temp->getTimeFinal()); + rho_obs = max_obs_vel * (t_iter + spline_temp->getTimeFinal()); if (rho_obs < q_current->getDistance()) { - rho_robot = computeRho(q_final); + rho_robot = computeRho(target_pos); if (rho_obs + rho_robot < q_current->getDistance()) { - spline_emergency_computed = false; + spline_emg_computed = false; is_safe = true; } } } else { - spline_emergency_temp = std::make_shared + spline_emg_temp = std::make_shared ( ss->robot, - spline_next_temp->getPosition(t_spline_max), - spline_next_temp->getVelocity(t_spline_max), - spline_next_temp->getAcceleration(t_spline_max) + spline_temp->getPosition(t_spline_max), + spline_temp->getVelocity(t_spline_max), + spline_temp->getAcceleration(t_spline_max) ); - if (spline_emergency_temp->compute()) + if (spline_emg_temp->compute()) { // std::cout << "Emergency spline is computed! \n"; - rho_obs = max_obs_vel * (t_iter + t_spline_max + spline_emergency_temp->getTimeFinal()); + rho_obs = max_obs_vel * (t_iter + t_spline_max + spline_emg_temp->getTimeFinal()); if (rho_obs < q_current->getDistance()) { - rho_robot = computeRho(spline_emergency_temp->getPosition(INFINITY)); + rho_robot = computeRho(spline_emg_temp->getPosition(INFINITY)); if (rho_obs + rho_robot < q_current->getDistance()) { - *spline_emergency_new = *spline_emergency_temp; - spline_emergency_computed = true; + *spline_emg_new = *spline_emg_temp; + spline_emg_computed = true; is_safe = true; - spline_next_temp->setTimeFinal(t_spline_max); + spline_temp->setTimeFinal(t_spline_max); } } } @@ -224,86 +182,97 @@ bool planning::trajectory::Trajectory::computeSafe(const Eigen::VectorXf ¤ if (is_safe) { // std::cout << "\tRobot is safe! \n"; - *spline_next_new = *spline_next_temp; + *spline_new = *spline_temp; spline_computed = true; - q_final_min = q_final; + target_pos_min = target_pos; if (num_iter == 1) break; } else { // std::cout << "\tRobot is NOT safe! \n"; - q_final_max = q_final; + target_pos_max = target_pos; } - q_final = (q_final_min + q_final_max) / 2; + target_pos = (target_pos_min + target_pos_max) / 2; } if (spline_computed) // Check whether computed splines are collision-free { - spline_next = spline_emergency_computed ? - std::make_shared - (std::vector>({ spline_next_new, spline_emergency_new })) : - spline_next_new; + std::shared_ptr spline_safe + { + spline_emg_computed ? + std::make_shared + (std::vector>({ spline_new, spline_emg_new })) : + spline_new + }; + + // std::cout << "Spline: \n" << spline << "\n"; + spline_computed = isSafe(spline_safe, q_current, t_iter); - // std::cout << "spline_next: \n" << spline_next << "\n"; - spline_computed = !checkCollision(q_current, t_iter); + if (spline_computed) + setSpline(spline_safe); } return spline_computed; } -/// @brief Check whether 'spline_next' is collision-free during the spline time interval [0, 'spline_next->getTimeFinal()'] -/// @param q_init Initial configuration from where bur spines are generated. -/// @param t_iter Elapsed time from the beginning of iteration to a time instance when 'spline' is starting. -/// @return True if the collision occurs. False if not. -/// @note 'q_init' must have a distance-to-obstacles or its underestimation! -bool planning::trajectory::Trajectory::checkCollision(std::shared_ptr q_init, float t_iter) +/// @brief Check whether the computed spline is safe (i.e., collision-free during the time interval [0, 'spline_safe->getTimeFinal()']) +/// @param spline_safe Safe spline which needs to be validated. +/// @param q_current Current configuration from where bur spines are generated. +/// @param nearest_points Nearest points between the robot and obstacles. +/// @param t_iter Elapsed time from the beginning of iteration to a time instance when the spline is starting. +/// @return True if safe. False if not. +/// @note 'q_current' must have a distance-to-obstacles or its underestimation! +bool planning::trajectory::Trajectory::isSafe(const std::shared_ptr spline_safe, + const std::shared_ptr q_current, float t_iter) { // std::chrono::steady_clock::time_point time_start_ { std::chrono::steady_clock::now() }; float delta_t { TrajectoryConfig::TIME_STEP }; - size_t num_iter = std::ceil(spline_next->getTimeFinal() / delta_t); - delta_t = spline_next->getTimeFinal() / num_iter; + size_t num_iter = std::ceil(spline_safe->getTimeFinal() / delta_t); + delta_t = spline_safe->getTimeFinal() / num_iter; float rho_robot {}; float rho_obs {}; float t_init { 0 }; + std::shared_ptr q_temp { q_current }; std::shared_ptr q_final { nullptr }; Eigen::VectorXf delta_q {}; - for (float t = delta_t; t <= spline_next->getTimeFinal() + RealVectorSpaceConfig::EQUALITY_THRESHOLD; t += delta_t) + for (float t = delta_t; t <= spline_safe->getTimeFinal() + RealVectorSpaceConfig::EQUALITY_THRESHOLD; t += delta_t) { // std::cout << "Considering t: " << t << "\n"; - q_final = ss->getNewState(spline_next->getPosition(t)); + q_final = ss->getNewState(spline_safe->getPosition(t)); if (ss->robot->checkSelfCollision(q_final)) - return true; + return false; t_iter += delta_t; rho_obs = max_obs_vel * (t_iter - t_init); - delta_q = (q_final->getCoord() - q_init->getCoord()).cwiseAbs(); - ss->robot->computeEnclosingRadii(q_init); + delta_q = (q_final->getCoord() - q_temp->getCoord()).cwiseAbs(); + ss->robot->computeEnclosingRadii(q_temp); + for (size_t i = 0; i < ss->robot->getNumDOFs(); i++) { - rho_robot = q_init->getEnclosingRadii()->col(i+1).dot(delta_q); - if (rho_robot + rho_obs >= q_init->getDistanceProfile(i)) // Possible collision + rho_robot = q_temp->getEnclosingRadii()->col(i+1).dot(delta_q); + if (rho_robot + rho_obs >= q_temp->getDistanceProfile(i)) // Possible collision { // std::cout << "********** Possible collision ********** \n"; - q_init = q_final; - computeDistanceUnderestimation(q_init, q_current->getNearestPoints(), t_iter); + q_temp = q_final; + computeDistanceUnderestimation(q_temp, q_current->getNearestPoints(), t_iter); t_init = t_iter; - if (q_init->getDistance() <= 0) + if (q_temp->getDistance() <= 0) { - // std::cout << "\t Spline is NOT guaranteed collision-free! \n"; - return true; + // std::cout << "\t Trajectory is NOT safe! \n"; + return false; } break; } - // else std::cout << "\t OK! rho_robot + rho_obs < q_init->getDistance() \n"; + // else std::cout << "\t OK! rho_robot + rho_obs < q_temp->getDistance() \n"; } } // auto t_elapsed { std::chrono::duration_cast(std::chrono::steady_clock::now() - time_start_).count() }; - // std::cout << "Elapsed time for spline checking: " << t_elapsed << " [us] \n"; - return false; + // std::cout << "Elapsed time for the spline collision checking: " << t_elapsed << " [us] \n"; + return true; } /// @brief Compute an underestimation of distance-to-obstacles 'd_c', i.e., a distance-to-planes, for each robot's link, @@ -367,16 +336,6 @@ float planning::trajectory::Trajectory::computeDistanceUnderestimation(const std return d_c; } -void planning::trajectory::Trajectory::addTrajPointCurrentIter(const Eigen::VectorXf &pos) -{ - traj_points_current_iter.emplace_back(pos); -} - -void planning::trajectory::Trajectory::clearTrajPointCurrentIter() -{ - traj_points_current_iter.clear(); -} - /// @brief A method (v1) to convert a path 'path' to a corresponding trajectory. /// Converting this path to trajectory (i.e., assigning time instances to these points) will be automatically done by this function. /// This is done by creating a sequence of quintic splines in a way that all constraints on robot's maximal velocity, @@ -388,14 +347,18 @@ void planning::trajectory::Trajectory::clearTrajPointCurrentIter() void planning::trajectory::Trajectory::path2traj_v1(const std::vector> &path) { std::vector> subsplines {}; + std::shared_ptr spline_current + { + std::make_shared( + ss->robot, + path.front()->getCoord(), + Eigen::VectorXf::Zero(ss->num_dimensions), + Eigen::VectorXf::Zero(ss->num_dimensions) + ) + }; + std::shared_ptr spline_next { nullptr }; + std::shared_ptr q_current { nullptr }; - spline_current = std::make_shared - ( - ss->robot, - path.front()->getCoord(), - Eigen::VectorXf::Zero(ss->num_dimensions), - Eigen::VectorXf::Zero(ss->num_dimensions) - ); spline_current->compute(path[1]->getCoord()); float t {}, t_max {}; @@ -431,8 +394,8 @@ void planning::trajectory::Trajectory::path2traj_v1(const std::vectorgetNewState(spline_current->getPosition(t)); - ss->computeDistance(q_current); // Required by 'checkCollision' function - if (q_current->getDistance() <= 0 || checkCollision(q_current, 0)) + ss->computeDistance(q_current); // Required by 'isSafe' function + if (q_current->getDistance() <= 0 || !isSafe(spline_next, q_current, 0)) spline_computed = false; } } @@ -443,7 +406,7 @@ void planning::trajectory::Trajectory::path2traj_v1(const std::vector(subsplines); + setSpline(std::make_shared(subsplines)); } /// @brief A method (v2) to convert a path 'path' to a corresponding trajectory. @@ -457,14 +420,17 @@ void planning::trajectory::Trajectory::path2traj_v1(const std::vector> &path) { std::vector> subsplines {}; - - spline_current = std::make_shared - ( - ss->robot, - path.front()->getCoord(), - Eigen::VectorXf::Zero(ss->num_dimensions), - Eigen::VectorXf::Zero(ss->num_dimensions) - ); + std::shared_ptr spline_current + { + std::make_shared( + ss->robot, + path.front()->getCoord(), + Eigen::VectorXf::Zero(ss->num_dimensions), + Eigen::VectorXf::Zero(ss->num_dimensions) + ) + }; + std::shared_ptr q_current { nullptr }; + spline_current->compute(path[1]->getCoord()); float t {}, t_max {}; @@ -480,7 +446,6 @@ void planning::trajectory::Trajectory::path2traj_v2(const std::vectorgetTimeFinal(); non_zero_final_vel = (i < path.size()-1) ? ss->checkLinearDependency(path[i-1], path[i], path[i+1]) : false; - q_target = path[i]; // std::cout << "i: " << i << " ---------------------------\n"; // std::cout << "t_max: " << t_max << " [s] \n"; @@ -491,11 +456,18 @@ void planning::trajectory::Trajectory::path2traj_v2(const std::vectorgetPosition(t), - spline_current->getVelocity(t), - spline_current->getAcceleration(t), + spline_computed = computeRegular + ( + planning::trajectory::State + ( + spline_current->getPosition(t), + spline_current->getVelocity(t), + spline_current->getAcceleration(t) + ), + planning::trajectory::State + ( + path[i]->getCoord() + ), max_remaining_iter_time, TrajectoryConfig::MAX_TIME_COMPUTE_REGULAR, non_zero_final_vel @@ -504,19 +476,19 @@ void planning::trajectory::Trajectory::path2traj_v2(const std::vectorgetNewState(spline_current->getPosition(t)); - ss->computeDistance(q_current); // Required by 'checkCollision' function - if (q_current->getDistance() <= 0 || checkCollision(q_current, 0)) + ss->computeDistance(q_current); // Required by 'isSafe' function + if (q_current->getDistance() <= 0 || !isSafe(spline, q_current, 0)) spline_computed = false; } } spline_current->setTimeFinal(t); subsplines.emplace_back(spline_current); - spline_current = spline_next; + spline_current = spline; } subsplines.emplace_back(spline_current); - composite_spline = std::make_shared(subsplines); + setSpline(std::make_shared(subsplines)); } /// @brief A method (v3) to convert a path 'path' to a corresponding trajectory. @@ -535,13 +507,13 @@ void planning::trajectory::Trajectory::path2traj_v3(const std::vector vel_coeff(path.size(), 1.0); const float vel_coeff_const { 0.9 }; - auto time_start = std::chrono::steady_clock::now(); + auto time_start_ { std::chrono::steady_clock::now() }; float max_time { 1.0 }; bool monotonic { true }; @@ -580,22 +552,22 @@ void planning::trajectory::Trajectory::path2traj_v3(const std::vectorcheckLinearDependency(path[i-2], path[i-1], path[i])) ? false : true; if (!must_visit) - q_final = (path[i-1]->getCoord() + path[i]->getCoord()) / 2; + target_pos = (path[i-1]->getCoord() + path[i]->getCoord()) / 2; else - q_final = path[i]->getCoord(); + target_pos = path[i]->getCoord(); spline_computed = false; num_iter = 0; - delta_t_max = ((q_final - path[i-1]->getCoord()).cwiseQuotient(ss->robot->getMaxVel())).cwiseAbs().maxCoeff(); - q_final_dot_max = (q_final - path[i-1]->getCoord()) / delta_t_max; - q_final_dot_min = Eigen::VectorXf::Zero(ss->num_dimensions); + delta_t_max = ((target_pos - path[i-1]->getCoord()).cwiseQuotient(ss->robot->getMaxVel())).cwiseAbs().maxCoeff(); + target_vel_max = (target_pos - path[i-1]->getCoord()) / delta_t_max; + target_vel_min = Eigen::VectorXf::Zero(ss->num_dimensions); do { - q_final_dot = vel_coeff[i] * (q_final_dot_max + q_final_dot_min) / 2; - std::shared_ptr spline_new { - std::make_shared - ( + target_vel = vel_coeff[i] * (target_vel_max + target_vel_min) / 2; + std::shared_ptr spline_new + { + std::make_shared( ss->robot, subsplines[i-1]->getPosition(subsplines[i-1]->getTimeFinal()), subsplines[i-1]->getVelocity(subsplines[i-1]->getTimeFinal()), @@ -603,21 +575,21 @@ void planning::trajectory::Trajectory::path2traj_v3(const std::vectorcompute(q_final, q_final_dot) && + if (spline_new->compute(target_pos, target_vel) && ((monotonic && spline_new->checkPositionMonotonicity() != 0) || !monotonic)) { *subsplines[i] = *spline_new; - q_final_dot_min = q_final_dot; + target_vel_min = target_vel; spline_computed = true; } else - q_final_dot_max = q_final_dot; + target_vel_max = target_vel; } while (++num_iter < max_num_iter); if (!spline_computed) { - spline_computed = subsplines[i]->compute(q_final) && + spline_computed = subsplines[i]->compute(target_pos) && ((monotonic && subsplines[i]->checkPositionMonotonicity() != 0) || !monotonic); if (!spline_computed) { @@ -628,7 +600,8 @@ void planning::trajectory::Trajectory::path2traj_v3(const std::vector(std::chrono::steady_clock::now() - time_start).count() * 1e-3 > max_time) + if (std::chrono::duration_cast + (std::chrono::steady_clock::now() - time_start_).count() * 1e-3 > max_time) { spline_computed = false; break; @@ -636,40 +609,19 @@ void planning::trajectory::Trajectory::path2traj_v3(const std::vector(subsplines); + setSpline(std::make_shared(subsplines)); else - std::cout << "Could not convert path to trajectory! \n"; - // path2traj(path); // Add path using another method. + { + std::cout << "Could not convert path to a trajectory! \n"; + // path2traj_v1(path); // Add path using another method. + } } -// This function is just for debugging. It operates in real-time by logging 'spline_current' and 'spline_next'. -// You can set a desired output path for the file to be saved. -void planning::trajectory::Trajectory::recordTrajectory(bool spline_computed) +void planning::trajectory::Trajectory::setSpline(const std::shared_ptr spline_) { - std::ofstream output_file {}; - output_file.open("/home/spear/xarm6-etf-lab/src/etf_modules/RPMPLv2/data/planar_2dof/scenario_random_obstacles/temp/visualize_trajectory.log", - std::ofstream::app); - - output_file << "q_current - q_target \n"; - if (spline_computed) - { - output_file << q_current->getCoord().transpose() << "\n"; - output_file << q_target->getCoord().transpose() << "\n"; - } - else - output_file << INFINITY << "\n"; - - output_file << "q_spline: \n"; - for (float t = spline_next->getTimeCurrent(); t < spline_next->getTimeFinal(); t += 0.001) - output_file << spline_next->getPosition(t).transpose() << "\n"; - output_file << spline_next->getPosition(spline_next->getTimeFinal()).transpose() << "\n"; - - output_file << "q_spline (realized): \n"; - for (float t = spline_current->getTimeBegin(); t < spline_current->getTimeCurrent(); t += 0.001) - output_file << spline_current->getPosition(t).transpose() << "\n"; - for (float t = spline_next->getTimeCurrent(); t < spline_next->getTimeEnd(); t += 0.001) - output_file << spline_next->getPosition(t).transpose() << "\n"; - output_file << spline_next->getPosition(spline_next->getTimeEnd()).transpose() << "\n"; - output_file << "--------------------------------------------------------------------\n"; + spline = spline_; + time_current = 0; + time_final = spline->getTimeFinal(); } diff --git a/src/planners/trajectory/TrajectoryRuckig.cpp b/src/planners/trajectory/TrajectoryRuckig.cpp index dff1a71b..b875579e 100644 --- a/src/planners/trajectory/TrajectoryRuckig.cpp +++ b/src/planners/trajectory/TrajectoryRuckig.cpp @@ -1,30 +1,21 @@ #include "TrajectoryRuckig.h" -planning::trajectory::TrajectoryRuckig::TrajectoryRuckig(const std::shared_ptr &ss_, - const Eigen::VectorXf &q_current_, float max_iter_time_) : +planning::trajectory::TrajectoryRuckig::TrajectoryRuckig(const std::shared_ptr &ss_) : + AbstractTrajectory(ss_), input(ss_->num_dimensions), - traj(ss_->num_dimensions), - traj_temp(ss_->num_dimensions) + traj(ss_->num_dimensions) +{} + +planning::trajectory::TrajectoryRuckig::TrajectoryRuckig + (const std::shared_ptr &ss_, planning::trajectory::State current, float max_iter_time_) : + AbstractTrajectory(ss_, max_iter_time), + input(ss_->num_dimensions), + traj(ss_->num_dimensions) { - ss = ss_; - q_current = q_current_; - max_iter_time = max_iter_time_; - time_current = 0; - time_final = 0; - time_final_temp = 0; - time_begin = 0; - time_end = 0; - time_start = std::chrono::steady_clock::now(); - time_start_offset = 0; - is_zero_final_vel = true; - traj_points_current_iter = {}; - + setCurrentState(current); + for (size_t i = 0; i < ss->num_dimensions; i++) { - input.current_position[i] = q_current(i); - input.current_velocity[i] = 0; - input.current_acceleration[i] = 0; - input.max_velocity[i] = ss->robot->getMaxVel(i); input.max_acceleration[i] = ss->robot->getMaxAcc(i); input.max_jerk[i] = ss->robot->getMaxJerk(i); @@ -32,69 +23,60 @@ planning::trajectory::TrajectoryRuckig::TrajectoryRuckig(const std::shared_ptrnum_dimensions; i++) - { - if (std::abs(ss->robot->getMaxVel(i) - ss->robot->getMaxVel(i-1)) > RealVectorSpaceConfig::EQUALITY_THRESHOLD) - { - all_robot_vel_same = false; - break; - } - } - - max_num_iter_trajectory = all_robot_vel_same ? - std::ceil(std::log2(2 * ss->robot->getMaxVel(0) / TrajectoryConfig::FINAL_VELOCITY_STEP)) : - std::ceil(std::log2(2 * ss->robot->getMaxVel().maxCoeff() / TrajectoryConfig::FINAL_VELOCITY_STEP)); + throw std::runtime_error("Invalid input parameters for Ruckig!"); } +planning::trajectory::TrajectoryRuckig::~TrajectoryRuckig() {} + /// @brief Compute a regular trajectory that is not surely safe for environment, meaning that, /// if collision eventually occurs, it may be at robot's non-zero velocity. -/// @param current_pos Current robot's position -/// @param current_vel Current robot's velocity -/// @param current_acc Current robot's acceleration +/// @param current Current robot's state +/// @param target Target robot's state /// @param t_iter_remain Remaining time in [s] in the current iteration /// @param t_max Maximal available time in [s] for a trajectory computing /// @param non_zero_final_vel Whether final velocity can be non-zero /// @return The success of a trajectory computation -bool planning::trajectory::TrajectoryRuckig::computeRegular(const Eigen::VectorXf ¤t_pos, const Eigen::VectorXf ¤t_vel, - const Eigen::VectorXf ¤t_acc, float t_iter_remain, float t_max, bool non_zero_final_vel) +bool planning::trajectory::TrajectoryRuckig::computeRegular(planning::trajectory::State current, + planning::trajectory::State target, float t_iter_remain, float t_max, bool non_zero_final_vel) { std::chrono::steady_clock::time_point time_start_ { std::chrono::steady_clock::now() }; float t_remain { t_max }; bool traj_computed { false }; ruckig::Result result { ruckig::Result::Working }; ruckig::Ruckig otg(ss->num_dimensions); - setCurrentState(current_pos, current_vel, current_acc); + ruckig::Trajectory traj_new(ss->num_dimensions); + setCurrentState(current); + setTargetState(target); if (non_zero_final_vel) { is_zero_final_vel = false; size_t num_iter { 0 }; - float delta_t_max { ((q_target - q_current).cwiseQuotient(ss->robot->getMaxVel())).cwiseAbs().maxCoeff() }; - Eigen::VectorXf q_final_dot_max { (q_target - q_current) / delta_t_max }; - Eigen::VectorXf q_final_dot_min { Eigen::VectorXf::Zero(ss->num_dimensions) }; - Eigen::VectorXf q_final_dot { q_final_dot_max }; + float delta_t_max { ((target.pos - current.pos).cwiseQuotient(ss->robot->getMaxVel())).cwiseAbs().maxCoeff() }; + Eigen::VectorXf target_vel_max { (target.pos - current.pos) / delta_t_max }; + Eigen::VectorXf target_vel_min { Eigen::VectorXf::Zero(ss->num_dimensions) }; + Eigen::VectorXf target_vel { target_vel_max }; while (!traj_computed && num_iter++ < max_num_iter_trajectory && t_remain > 0) { - setTargetState(q_target, q_final_dot, Eigen::VectorXf::Zero(ss->num_dimensions)); - result = otg.calculate(input, traj_temp); + target.vel = target_vel; + setTargetState(target); + result = otg.calculate(input, traj_new); if (result == ruckig::Result::Working || result == ruckig::Result::Finished) { - traj = traj_temp; + traj = traj_new; time_current = 0; time_final = traj.get_duration(); traj_computed = true; } else - q_final_dot_max = q_final_dot; + target_vel_max = target_vel; - q_final_dot = (q_final_dot_max + q_final_dot_min) / 2; - t_remain -= std::chrono::duration_cast(std::chrono::steady_clock::now() - time_start_).count() / 1e6; - // std::cout << "Num. iter. " << num_iter << "\t q_final_dot: " << q_final_dot.transpose() << "\n"; + target_vel = (target_vel_max + target_vel_min) / 2; + t_remain -= std::chrono::duration_cast + (std::chrono::steady_clock::now() - time_start_).count() / 1e6; + // std::cout << "Num. iter. " << num_iter << "\t target_vel: " << target_vel.transpose() << "\n"; } } @@ -102,15 +84,16 @@ bool planning::trajectory::TrajectoryRuckig::computeRegular(const Eigen::VectorX Eigen::VectorXf new_current_pos { getPosition(time_current + t_iter_remain) }; // If trajectory was not computed or robot is getting away from 'new_current_pos' - if ((!traj_computed || (new_current_pos - q_target).norm() > (current_pos - q_target).norm()) && t_remain > 0) + if ((!traj_computed || (new_current_pos - target.pos).norm() > (current.pos - target.pos).norm()) && t_remain > 0) { is_zero_final_vel = true; - setTargetState(q_target, Eigen::VectorXf::Zero(ss->num_dimensions), Eigen::VectorXf::Zero(ss->num_dimensions)); - result = otg.calculate(input, traj_temp); + target.vel = Eigen::VectorXf::Zero(ss->num_dimensions); + setTargetState(target); + result = otg.calculate(input, traj_new); if (result == ruckig::Result::Working || result == ruckig::Result::Finished) { - traj = traj_temp; + traj = traj_new; time_current = 0; time_final = traj.get_duration(); traj_computed = true; @@ -122,25 +105,38 @@ bool planning::trajectory::TrajectoryRuckig::computeRegular(const Eigen::VectorX return traj_computed; } -void planning::trajectory::TrajectoryRuckig::setCurrentState(const Eigen::VectorXf ¤t_pos, - const Eigen::VectorXf ¤t_vel, const Eigen::VectorXf ¤t_acc) +/// @brief Compute a safe trajectory that will render a robot motion surely safe for environment. +/// If collision eventually occurs, it will be at robot's zero velocity, meaning that an obstacle hit the robot, and not vice versa. +/// @param current Current robot's state +/// @param target Target robot's state +/// @param t_iter_remain Remaining time in [s] in the current iteration +/// @param t_max Maximal available time in [s] for a trajectory computing +/// @param q_current Current robot's configuration +/// @return The success of a trajectory computation +bool planning::trajectory::TrajectoryRuckig::computeSafe(planning::trajectory::State current, + planning::trajectory::State target, float t_iter_remain, float t_max, const std::shared_ptr q_current) +{ + // TODO + return false; +} + +void planning::trajectory::TrajectoryRuckig::setCurrentState(const planning::trajectory::State ¤t) { for (size_t i = 0; i < ss->num_dimensions; i++) { - input.current_position[i] = current_pos(i); - input.current_velocity[i] = current_vel(i); - input.current_acceleration[i] = current_acc(i); + input.current_position[i] = current.pos(i); + input.current_velocity[i] = current.vel(i); + input.current_acceleration[i] = current.acc(i); } } -void planning::trajectory::TrajectoryRuckig::setTargetState(const Eigen::VectorXf &target_pos, - const Eigen::VectorXf &target_vel, const Eigen::VectorXf &target_acc) +void planning::trajectory::TrajectoryRuckig::setTargetState(const planning::trajectory::State &target) { for (size_t i = 0; i < ss->num_dimensions; i++) { - input.target_position[i] = target_pos(i); - input.target_velocity[i] = target_vel(i); - input.target_acceleration[i] = target_acc(i); + input.target_position[i] = target.pos(i); + input.target_velocity[i] = target.vel(i); + input.target_acceleration[i] = target.acc(i); } } @@ -195,41 +191,3 @@ Eigen::VectorXf planning::trajectory::TrajectoryRuckig::getAcceleration(float t) return ret; } - -void planning::trajectory::TrajectoryRuckig::addTrajPointCurrentIter(const Eigen::VectorXf &pos) -{ - traj_points_current_iter.emplace_back(pos); -} - -void planning::trajectory::TrajectoryRuckig::clearTrajPointCurrentIter() -{ - traj_points_current_iter.clear(); -} - -/// @brief Check whether 'q' is a final configuration of the trajectory. -/// @param q Configuration to be checked. -/// @return True if yes, false if not. -bool planning::trajectory::TrajectoryRuckig::isFinalConf(const Eigen::VectorXf &q) -{ - return ((q - getPosition(time_final)).norm() < RealVectorSpaceConfig::EQUALITY_THRESHOLD) ? true : false; -} - -/// @brief Get current time of a spline. -/// @param measure_time If true, current time will be automatically computed/measured (default: false). -/// @note 'measure_time' should always be false when simulation pacing is used, since then a time measuring will not be correct! -/// In such case, it is assumed that user was previously set 'measure_time' to a correct value. -float planning::trajectory::TrajectoryRuckig::getTimeCurrent(bool measure_time) -{ - if (!measure_time) - return time_current; - - time_current = std::chrono::duration_cast(std::chrono::steady_clock::now() - time_start).count() * 1e-9 - - time_start_offset; - return time_current; -} - -void planning::trajectory::TrajectoryRuckig::setTimeStart(float time_start_offset_) -{ - time_start = std::chrono::steady_clock::now(); - time_start_offset = time_start_offset_; -} From b9f64d36a568a169c2dd960e5b9308c213482135 Mon Sep 17 00:00:00 2001 From: Nermin Covic Date: Sat, 18 Oct 2025 21:49:29 +0200 Subject: [PATCH 37/40] A lot of variables and methods are deleted since they are moved to 'AbstractTrajectory' class. --- include/planners/trajectory/Spline.h | 14 ---- src/planners/trajectory/CompositeSpline.cpp | 5 -- src/planners/trajectory/Spline.cpp | 89 +++++++-------------- 3 files changed, 28 insertions(+), 80 deletions(-) diff --git a/include/planners/trajectory/Spline.h b/include/planners/trajectory/Spline.h index b7012e8f..7519ab33 100644 --- a/include/planners/trajectory/Spline.h +++ b/include/planners/trajectory/Spline.h @@ -25,7 +25,6 @@ namespace planning::trajectory virtual bool compute(const Eigen::VectorXf &q_final, const Eigen::VectorXf &q_final_dot) = 0; virtual bool compute(const Eigen::VectorXf &q_final, const Eigen::VectorXf &q_final_dot, const Eigen::VectorXf &q_final_ddot) = 0; virtual bool checkConstraints(size_t idx, float t_f) = 0; - bool isFinalConf(const Eigen::VectorXf &q); virtual std::vector getPositionExtremumTimes(size_t idx) = 0; virtual std::vector getVelocityExtremumTimes(size_t idx) = 0; @@ -49,17 +48,9 @@ namespace planning::trajectory float getCoeff(size_t i, size_t j) const { return coeff(i, j); } float getTimeFinal() const { return time_final; } float getTimeFinal(size_t idx) const { return times_final[idx]; } - float getTimeCurrent(bool measure_time = false); - float getTimeBegin() const { return time_begin; } - float getTimeEnd() const { return time_end; } bool getIsZeroFinalVel() const { return is_zero_final_vel; } bool getIsZeroFinalAcc() const { return is_zero_final_acc; } - - void setTimeStart(float time_start_offset_); void setTimeFinal(float time_final_) { time_final = time_final_; } - void setTimeCurrent(float time_current_) { time_current = time_current_; } - void setTimeBegin(float time_begin_) { time_begin = time_begin_; } - void setTimeEnd(float time_end_) { time_end = time_end_; } friend std::ostream &operator<<(std::ostream &os, const std::shared_ptr spline); @@ -67,13 +58,8 @@ namespace planning::trajectory size_t order; std::shared_ptr robot; Eigen::MatrixXf coeff; // Num. of rows = 'num_DOFs', and num. of columns = 'order+1'. Form: sum{j=0, num_DOFs-1} coeff(i,j) * t^j - std::chrono::steady_clock::time_point time_start; // Start time point when a spline is created - float time_start_offset; // Time offset in [s] which determines how much earlier 'time_start' is created std::vector times_final; // Final time in [s] for each spline. After this time, velocity, acceleration and jerk are zero (if 'is_zero_final_vel' is true and 'is_zero_final_acc' is true), while position remains constant. float time_final; // Maximal time from 'times_final', or a time set by user when prunning spline - float time_current; // Elapsed time in [s] from a time instant when a spline is created. It is used to determine a current robot's position, velocity and acceleration. - float time_begin; // Time instance in [s] when a spline begins in the current iteration - float time_end; // Time instance in [s] when a spline ends in the current iteration bool is_zero_final_vel; // Whether final velocity is zero. If not, robot will move at constant velocity (if 'is_zero_final_acc' is true) after 'times_final[idx]'. bool is_zero_final_acc; // Whether final acceleration is zero. If not, robot will move at constant acceleration after 'times_final[idx]'. std::vector> subsplines; // Contains a sequence of splines. Relevant only to 'CompositeSpline'. diff --git a/src/planners/trajectory/CompositeSpline.cpp b/src/planners/trajectory/CompositeSpline.cpp index 5003ff26..929f4e73 100644 --- a/src/planners/trajectory/CompositeSpline.cpp +++ b/src/planners/trajectory/CompositeSpline.cpp @@ -3,11 +3,6 @@ planning::trajectory::CompositeSpline::CompositeSpline(const std::vector> &subsplines_) { subsplines = subsplines_; - time_start = std::chrono::steady_clock::now(); - time_start_offset = 0; - time_current = 0; - time_begin = 0; - time_end = 0; is_zero_final_vel = subsplines.back()->getIsZeroFinalVel(); is_zero_final_acc = subsplines.back()->getIsZeroFinalAcc(); diff --git a/src/planners/trajectory/Spline.cpp b/src/planners/trajectory/Spline.cpp index 86773cf9..fa168876 100644 --- a/src/planners/trajectory/Spline.cpp +++ b/src/planners/trajectory/Spline.cpp @@ -6,13 +6,8 @@ planning::trajectory::Spline::Spline(size_t order_, const std::shared_ptrgetNumDOFs(), order + 1); coeff.col(0) = q_current; // All initial conditions are zero, except position - time_start = std::chrono::steady_clock::now(); - time_start_offset = 0; times_final = std::vector(robot->getNumDOFs(), 0); time_final = 0; - time_current = 0; - time_begin = 0; - time_end = 0; is_zero_final_vel = true; is_zero_final_acc = true; subsplines = {}; @@ -20,14 +15,6 @@ planning::trajectory::Spline::Spline(size_t order_, const std::shared_ptrgetNumDOFs()) }; + Eigen::VectorXf pos { Eigen::VectorXf::Zero(robot->getNumDOFs()) }; for (size_t i = 0; i < robot->getNumDOFs(); i++) - q(i) = getPosition(t, i); + pos(i) = getPosition(t, i); - // std::cout << "Robot position at time " << t << " [s] is " << q.transpose() << "\n"; - return q; + // std::cout << "Robot position at time " << t << " [s] is " << pos.transpose() << "\n"; + return pos; } float planning::trajectory::Spline::getPosition(float t, size_t idx) { - float q { 0 }; + float pos { 0 }; float delta_t { 0 }; float vel_final { 0 }; float acc_final { 0 }; @@ -98,24 +85,24 @@ float planning::trajectory::Spline::getPosition(float t, size_t idx) t = 0; for (size_t i = 0; i <= order; i++) - q += coeff(idx, i) * std::pow(t, i); + pos += coeff(idx, i) * std::pow(t, i); - return q + vel_final * delta_t + acc_final * delta_t*delta_t * 0.5; + return pos + vel_final * delta_t + acc_final * delta_t*delta_t * 0.5; } Eigen::VectorXf planning::trajectory::Spline::getVelocity(float t) { - Eigen::VectorXf q { Eigen::VectorXf::Zero(robot->getNumDOFs()) }; + Eigen::VectorXf vel { Eigen::VectorXf::Zero(robot->getNumDOFs()) }; for (size_t i = 0; i < robot->getNumDOFs(); i++) - q(i) = getVelocity(t, i); + vel(i) = getVelocity(t, i); - // std::cout << "Robot velocity at time " << t << " [s] is " << q.transpose() << "\n"; - return q; + // std::cout << "Robot velocity at time " << t << " [s] is " << vel.transpose() << "\n"; + return vel; } float planning::trajectory::Spline::getVelocity(float t, size_t idx) { - float q { 0 }; + float vel { 0 }; float delta_t { 0 }; float acc_final { 0 }; @@ -132,19 +119,19 @@ float planning::trajectory::Spline::getVelocity(float t, size_t idx) t = 0; for (size_t i = 1; i <= order; i++) - q += coeff(idx, i) * i * std::pow(t, i-1); + vel += coeff(idx, i) * i * std::pow(t, i-1); - return q + acc_final * delta_t; + return vel + acc_final * delta_t; } Eigen::VectorXf planning::trajectory::Spline::getAcceleration(float t) { - Eigen::VectorXf q { Eigen::VectorXf::Zero(robot->getNumDOFs()) }; + Eigen::VectorXf acc { Eigen::VectorXf::Zero(robot->getNumDOFs()) }; for (size_t i = 0; i < robot->getNumDOFs(); i++) - q(i) = getAcceleration(t, i); + acc(i) = getAcceleration(t, i); - // std::cout << "Robot acceleration at time " << t << " [s] is " << q.transpose() << "\n"; - return q; + // std::cout << "Robot acceleration at time " << t << " [s] is " << acc.transpose() << "\n"; + return acc; } float planning::trajectory::Spline::getAcceleration(float t, size_t idx) @@ -154,21 +141,21 @@ float planning::trajectory::Spline::getAcceleration(float t, size_t idx) else if (t < 0) t = 0; - float q { 0 }; + float acc { 0 }; for (size_t i = 2; i <= order; i++) - q += coeff(idx, i) * i * (i-1) * std::pow(t, i-2); + acc += coeff(idx, i) * i * (i-1) * std::pow(t, i-2); - return q; + return acc; } Eigen::VectorXf planning::trajectory::Spline::getJerk(float t) { - Eigen::VectorXf q { Eigen::VectorXf::Zero(robot->getNumDOFs()) }; + Eigen::VectorXf jerk { Eigen::VectorXf::Zero(robot->getNumDOFs()) }; for (size_t i = 0; i < robot->getNumDOFs(); i++) - q(i) = getJerk(t, i); + jerk(i) = getJerk(t, i); - // std::cout << "Robot jerk at time " << t << " [s] is " << q.transpose() << "\n"; - return q; + // std::cout << "Robot jerk at time " << t << " [s] is " << jerk.transpose() << "\n"; + return jerk; } float planning::trajectory::Spline::getJerk(float t, size_t idx) @@ -178,31 +165,11 @@ float planning::trajectory::Spline::getJerk(float t, size_t idx) else if (t < 0) t = 0; - float q { 0 }; + float jerk { 0 }; for (size_t i = 3; i <= order; i++) - q += coeff(idx, i) * i * (i-1) * (i-2) * std::pow(t, i-3); - - return q; -} + jerk += coeff(idx, i) * i * (i-1) * (i-2) * std::pow(t, i-3); -/// @brief Get current time of a spline. -/// @param measure_time If true, current time will be automatically computed/measured (default: false). -/// @note 'measure_time' should always be false when simulation pacing is used, since then a time measuring will not be correct! -/// In such case, it is assumed that user was previously set 'measure_time' to a correct value. -float planning::trajectory::Spline::getTimeCurrent(bool measure_time) -{ - if (!measure_time) - return time_current; - - time_current = std::chrono::duration_cast(std::chrono::steady_clock::now() - time_start).count() * 1e-9 - - time_start_offset; - return time_current; -} - -void planning::trajectory::Spline::setTimeStart(float time_start_offset_) -{ - time_start = std::chrono::steady_clock::now(); - time_start_offset = time_start_offset_; + return jerk; } namespace planning::trajectory From fc96ee64636cb9509b32187fcbe480996d0b6970 Mon Sep 17 00:00:00 2001 From: Nermin Covic Date: Sat, 18 Oct 2025 21:59:14 +0200 Subject: [PATCH 38/40] Other corresponding changes... --- apps/test_trajectory.cpp | 12 +- include/planners/drbt/DRGBT.h | 3 +- include/planners/drbt/RT_RGBT.h | 3 +- include/planners/rrtx/RRTx.h | 4 +- include/planners/trajectory/UpdatingState.h | 11 +- src/planners/drbt/DRGBT.cpp | 44 +-- src/planners/drbt/RT_RGBT.cpp | 38 +-- src/planners/rrtx/RRTx.cpp | 37 +-- src/planners/trajectory/UpdatingState.cpp | 280 +++++--------------- 9 files changed, 151 insertions(+), 281 deletions(-) diff --git a/apps/test_trajectory.cpp b/apps/test_trajectory.cpp index ab5daf02..987b2f76 100644 --- a/apps/test_trajectory.cpp +++ b/apps/test_trajectory.cpp @@ -106,21 +106,21 @@ int main(int argc, char **argv) auto time1 = std::chrono::steady_clock::now(); trajectory->path2traj_v1(new_path); comp_times1.emplace_back(std::chrono::duration_cast(std::chrono::steady_clock::now() - time1).count() * 1e-3); - final_times1.emplace_back(trajectory->composite_spline->getTimeFinal()); + final_times1.emplace_back(trajectory->getTimeFinal()); output_file << "Trajectory 1 (position): \n"; - for (float t = 0; t < trajectory->composite_spline->getTimeFinal(); t += delta_time) - output_file << trajectory->composite_spline->getPosition(t).transpose() << "\n"; + for (float t = 0; t < trajectory->getTimeFinal(); t += delta_time) + output_file << trajectory->getPosition(t).transpose() << "\n"; output_file << "--------------------------------------------------------------------\n"; auto time2 = std::chrono::steady_clock::now(); trajectory->path2traj_v2(new_path); comp_times2.emplace_back(std::chrono::duration_cast(std::chrono::steady_clock::now() - time2).count() * 1e-3); - final_times2.emplace_back(trajectory->composite_spline->getTimeFinal()); + final_times2.emplace_back(trajectory->getTimeFinal()); output_file << "Trajectory 2 (position): \n"; - for (float t = 0; t < trajectory->composite_spline->getTimeFinal(); t += delta_time) - output_file << trajectory->composite_spline->getPosition(t).transpose() << "\n"; + for (float t = 0; t < trajectory->getTimeFinal(); t += delta_time) + output_file << trajectory->getPosition(t).transpose() << "\n"; output_file << "--------------------------------------------------------------------\n"; output_file.close(); } diff --git a/include/planners/drbt/DRGBT.h b/include/planners/drbt/DRGBT.h index 20465eda..9e85f807 100644 --- a/include/planners/drbt/DRGBT.h +++ b/include/planners/drbt/DRGBT.h @@ -63,8 +63,7 @@ namespace planning::drbt std::vector> predefined_path; // The predefined path that is being followed size_t num_lateral_states; // Number of lateral states float max_edge_length; // Maximal edge length when acquiring a new predefined path - std::shared_ptr traj; // Trajectory which is generated using splines from RPMPLv2 library - std::shared_ptr traj_ruckig; // Trajectory which is generated using Ruckig library + std::shared_ptr traj; // Trajectory which is generated from 'q_current' towards 'q_next' std::shared_ptr updating_state; // Class for updating current state std::shared_ptr motion_validity; // Class for checking validity of motion std::vector> visited_states; // Set of visited states when choosing 'q_next' diff --git a/include/planners/drbt/RT_RGBT.h b/include/planners/drbt/RT_RGBT.h index ebaec6cb..bc2c3b78 100644 --- a/include/planners/drbt/RT_RGBT.h +++ b/include/planners/drbt/RT_RGBT.h @@ -35,8 +35,7 @@ namespace planning::drbt protected: std::shared_ptr q_current; // Current robot configuration std::shared_ptr q_target; // Target (next) robot configuration - std::shared_ptr traj; // Trajectory which is generated using splines from RPMPLv2 library - std::shared_ptr traj_ruckig; // Trajectory which is generated using Ruckig library + std::shared_ptr traj; // Trajectory which is generated from 'q_current' towards 'q_target' std::shared_ptr updating_state; // Class for updating current state std::shared_ptr motion_validity; // Class for checking validity of motion float max_edge_length; // Distance between 'q_current' and 'q_target' diff --git a/include/planners/rrtx/RRTx.h b/include/planners/rrtx/RRTx.h index 4bc6f58d..96148f16 100644 --- a/include/planners/rrtx/RRTx.h +++ b/include/planners/rrtx/RRTx.h @@ -10,6 +10,7 @@ #include "UpdatingState.h" #include "MotionValidity.h" #include "Trajectory.h" +#include "TrajectoryRuckig.h" // #include // #include @@ -87,8 +88,7 @@ namespace planning::rrtx // Path from start to goal std::vector> path_current; - std::shared_ptr traj; // Trajectory which is generated using splines from RPMPLv2 library - std::shared_ptr traj_ruckig; // Trajectory which is generated using Ruckig library + std::shared_ptr traj; // Trajectory which is generated from 'q_current' towards 'q_next' std::shared_ptr updating_state; // Class for updating current state std::shared_ptr motion_validity; // Class for checking validity of motion diff --git a/include/planners/trajectory/UpdatingState.h b/include/planners/trajectory/UpdatingState.h index e5fb65fd..b39a2e3b 100644 --- a/include/planners/trajectory/UpdatingState.h +++ b/include/planners/trajectory/UpdatingState.h @@ -8,8 +8,7 @@ #include "StateSpace.h" #include "RealVectorSpaceConfig.h" #include "PlanningTypes.h" -#include "Trajectory.h" -#include "TrajectoryRuckig.h" +#include "AbstractTrajectory.h" namespace planning::drbt { @@ -28,8 +27,7 @@ namespace planning::trajectory const std::shared_ptr q_next_, base::State::Status &status); void update(std::shared_ptr &q_previous, std::shared_ptr &q_current, const std::shared_ptr q_next_, const std::shared_ptr q_next_reached_, base::State::Status &status); - inline void setTrajectory(const std::shared_ptr &traj_) { traj = traj_; } - inline void setTrajectory(const std::shared_ptr &traj_ruckig_) { traj_ruckig = traj_ruckig_; } + inline void setTrajectory(const std::shared_ptr &traj_) { traj = traj_; } inline void setGuaranteedSafeMotion(bool guaranteed_safe_motion_) { guaranteed_safe_motion = guaranteed_safe_motion_; } inline void setNonZeroFinalVel(bool non_zero_final_vel_) { non_zero_final_vel = non_zero_final_vel_; } inline void setMaxRemainingIterTime(float max_remaining_iter_time_) { max_remaining_iter_time = max_remaining_iter_time_; } @@ -45,8 +43,6 @@ namespace planning::trajectory const std::shared_ptr q_next_, const std::shared_ptr q_next_reached_, base::State::Status &status); void update_v2(std::shared_ptr &q_previous, std::shared_ptr &q_current, const std::shared_ptr q_next_, const std::shared_ptr q_next_reached_, base::State::Status &status); - void update_v3(std::shared_ptr &q_previous, std::shared_ptr &q_current, - const std::shared_ptr q_next_, const std::shared_ptr q_next_reached_, base::State::Status &status); bool invokeChangeNextState(); float getElapsedTime(); @@ -55,8 +51,7 @@ namespace planning::trajectory float max_iter_time; // Maximal iteration time in [s] // Additional info (not mandatory to be set): - std::shared_ptr traj; // Trajectory which is generated using splines from RPMPLv2 library - std::shared_ptr traj_ruckig; // Trajectory which is generated using Ruckig library + std::shared_ptr traj; // Trajectory which is generated from 'q_current' towards 'q_next' bool all_robot_vel_same; // Whether all joint velocities are the same bool guaranteed_safe_motion; // Whether robot motion is surely safe for environment bool non_zero_final_vel; // Whether final spline velocity can be non-zero (available only when 'guaranteed_safe_motion' is false) diff --git a/src/planners/drbt/DRGBT.cpp b/src/planners/drbt/DRGBT.cpp index 46d8e4a6..dd820812 100644 --- a/src/planners/drbt/DRGBT.cpp +++ b/src/planners/drbt/DRGBT.cpp @@ -30,35 +30,43 @@ planning::drbt::DRGBT::DRGBT(const std::shared_ptr ss_, const path.emplace_back(q_start); // State 'q_start' is added to the realized path max_edge_length = ss->robot->getMaxVel().norm() * DRGBTConfig::MAX_ITER_TIME; - updating_state = std::make_shared - (ss, DRGBTConfig::TRAJECTORY_INTERPOLATION, DRGBTConfig::MAX_ITER_TIME); - updating_state->setGuaranteedSafeMotion(DRGBTConfig::GUARANTEED_SAFE_MOTION); - updating_state->setMaxRemainingIterTime(DRGBTConfig::MAX_ITER_TIME - DRGBTConfig::MAX_TIME_TASK1); - updating_state->setMeasureTime(false); - updating_state->setDRGBTinstance(this); - - motion_validity = std::make_shared - (ss, DRGBTConfig::RESOLUTION_COLL_CHECK, DRGBTConfig::MAX_ITER_TIME, &path); - switch (DRGBTConfig::TRAJECTORY_INTERPOLATION) { case planning::TrajectoryInterpolation::None: traj = nullptr; - traj_ruckig = nullptr; break; case planning::TrajectoryInterpolation::Spline: - traj = std::make_shared(ss, q_current, DRGBTConfig::MAX_ITER_TIME); + traj = std::make_shared + ( + ss, + planning::trajectory::State(q_current->getCoord()), + DRGBTConfig::MAX_ITER_TIME + ); traj->setMaxRemainingIterTime(DRGBTConfig::MAX_ITER_TIME - DRGBTConfig::MAX_TIME_TASK1); - updating_state->setTrajectory(traj); break; case planning::TrajectoryInterpolation::Ruckig: - traj_ruckig = std::make_shared(ss, q_current->getCoord(), DRGBTConfig::MAX_ITER_TIME); - updating_state->setTrajectory(traj_ruckig); + traj = std::make_shared + ( + ss, + planning::trajectory::State(q_current->getCoord()), + DRGBTConfig::MAX_ITER_TIME + ); break; } + updating_state = std::make_shared + (ss, DRGBTConfig::TRAJECTORY_INTERPOLATION, DRGBTConfig::MAX_ITER_TIME); + updating_state->setGuaranteedSafeMotion(DRGBTConfig::GUARANTEED_SAFE_MOTION); + updating_state->setMaxRemainingIterTime(DRGBTConfig::MAX_ITER_TIME - DRGBTConfig::MAX_TIME_TASK1); + updating_state->setMeasureTime(false); + updating_state->setDRGBTinstance(this); + updating_state->setTrajectory(traj); + + motion_validity = std::make_shared + (ss, DRGBTConfig::RESOLUTION_COLL_CHECK, DRGBTConfig::MAX_ITER_TIME, &path); + // std::cout << "DRGBT planner initialized! \n"; } @@ -141,13 +149,9 @@ bool planning::drbt::DRGBT::solve() is_valid = motion_validity->check(q_previous, q_current); break; - case planning::TrajectoryInterpolation::Spline: + default: is_valid = motion_validity->check(traj->getTrajPointCurrentIter()); break; - - case planning::TrajectoryInterpolation::Ruckig: - is_valid = motion_validity->check(traj_ruckig->getTrajPointCurrentIter()); - break; } if (!is_valid) diff --git a/src/planners/drbt/RT_RGBT.cpp b/src/planners/drbt/RT_RGBT.cpp index 0aad9605..21c6e0eb 100644 --- a/src/planners/drbt/RT_RGBT.cpp +++ b/src/planners/drbt/RT_RGBT.cpp @@ -25,29 +25,39 @@ planning::drbt::RT_RGBT::RT_RGBT(const std::shared_ptr ss_, co path.emplace_back(q_start); // State 'q_start' is added to the realized path max_edge_length = ss->robot->getMaxVel().norm() * RT_RGBTConfig::MAX_ITER_TIME; - updating_state = std::make_shared - (ss, RT_RGBTConfig::TRAJECTORY_INTERPOLATION, RT_RGBTConfig::MAX_ITER_TIME); - - motion_validity = std::make_shared - (ss, RT_RGBTConfig::RESOLUTION_COLL_CHECK, RT_RGBTConfig::MAX_ITER_TIME, &path); - switch (RT_RGBTConfig::TRAJECTORY_INTERPOLATION) { case planning::TrajectoryInterpolation::None: traj = nullptr; - traj_ruckig = nullptr; break; case planning::TrajectoryInterpolation::Spline: - traj = std::make_shared(ss, q_current, RT_RGBTConfig::MAX_ITER_TIME); - updating_state->setTrajectory(traj); + traj = std::make_shared + ( + ss, + planning::trajectory::State(q_current->getCoord()), + RT_RGBTConfig::MAX_ITER_TIME + ); break; case planning::TrajectoryInterpolation::Ruckig: - traj_ruckig = std::make_shared(ss, q_current->getCoord(), RT_RGBTConfig::MAX_ITER_TIME); - updating_state->setTrajectory(traj_ruckig); + traj = std::make_shared + ( + ss, + planning::trajectory::State(q_current->getCoord()), + RT_RGBTConfig::MAX_ITER_TIME + ); break; } + + updating_state = std::make_shared + (ss, RT_RGBTConfig::TRAJECTORY_INTERPOLATION, RT_RGBTConfig::MAX_ITER_TIME); + updating_state->setMeasureTime(false); + updating_state->setTrajectory(traj); + + motion_validity = std::make_shared + (ss, RT_RGBTConfig::RESOLUTION_COLL_CHECK, RT_RGBTConfig::MAX_ITER_TIME, &path); + // std::cout << "RT_RGBT planner initialized! \n"; } @@ -100,13 +110,9 @@ bool planning::drbt::RT_RGBT::solve() is_valid = motion_validity->check(q_current, q_target); break; - case planning::TrajectoryInterpolation::Spline: + default: is_valid = motion_validity->check(traj->getTrajPointCurrentIter()); break; - - case planning::TrajectoryInterpolation::Ruckig: - is_valid = motion_validity->check(traj_ruckig->getTrajPointCurrentIter()); - break; } if (!is_valid) diff --git a/src/planners/rrtx/RRTx.cpp b/src/planners/rrtx/RRTx.cpp index 9fb5c3fb..521956a7 100644 --- a/src/planners/rrtx/RRTx.cpp +++ b/src/planners/rrtx/RRTx.cpp @@ -37,31 +37,40 @@ planning::rrtx::RRTx::RRTx(const std::shared_ptr ss_, const st planner_info->setNumIterations(0); planner_info->setNumStates(1); - - updating_state = std::make_shared - (ss, RRTxConfig::TRAJECTORY_INTERPOLATION, RRTxConfig::MAX_ITER_TIME); - - motion_validity = std::make_shared - (ss, RRTxConfig::RESOLUTION_COLL_CHECK, RRTxConfig::MAX_ITER_TIME, &path); switch (RRTxConfig::TRAJECTORY_INTERPOLATION) { case planning::TrajectoryInterpolation::None: traj = nullptr; - traj_ruckig = nullptr; break; case planning::TrajectoryInterpolation::Spline: - traj = std::make_shared(ss, q_current, RRTxConfig::MAX_ITER_TIME); - updating_state->setTrajectory(traj); + traj = std::make_shared + ( + ss, + planning::trajectory::State(q_current->getCoord()), + RRTxConfig::MAX_ITER_TIME + ); break; case planning::TrajectoryInterpolation::Ruckig: - traj_ruckig = std::make_shared(ss, q_current->getCoord(), RRTxConfig::MAX_ITER_TIME); - updating_state->setTrajectory(traj_ruckig); + traj = std::make_shared + ( + ss, + planning::trajectory::State(q_current->getCoord()), + RRTxConfig::MAX_ITER_TIME + ); break; } + updating_state = std::make_shared + (ss, RRTxConfig::TRAJECTORY_INTERPOLATION, RRTxConfig::MAX_ITER_TIME); + updating_state->setMeasureTime(false); + updating_state->setTrajectory(traj); + + motion_validity = std::make_shared + (ss, RRTxConfig::RESOLUTION_COLL_CHECK, RRTxConfig::MAX_ITER_TIME, &path); + RRTConnectConfig::EPS_STEP = RRTxConfig::EPS_STEP; eta = std::sqrt(ss->num_dimensions); @@ -245,13 +254,9 @@ bool planning::rrtx::RRTx::solve() is_valid = motion_validity->check(q_previous, q_current); break; - case planning::TrajectoryInterpolation::Spline: + default: is_valid = motion_validity->check(traj->getTrajPointCurrentIter()); break; - - case planning::TrajectoryInterpolation::Ruckig: - is_valid = motion_validity->check(traj_ruckig->getTrajPointCurrentIter()); - break; } if (!is_valid) diff --git a/src/planners/trajectory/UpdatingState.cpp b/src/planners/trajectory/UpdatingState.cpp index e1268197..a3a6546f 100644 --- a/src/planners/trajectory/UpdatingState.cpp +++ b/src/planners/trajectory/UpdatingState.cpp @@ -19,7 +19,6 @@ planning::trajectory::UpdatingState::UpdatingState(const std::shared_ptr &q update_v1(q_previous, q_current, q_next_, q_next_, status); break; - case planning::TrajectoryInterpolation::Spline: + default: update_v2(q_previous, q_current, q_next_, q_next_, status); break; - - case planning::TrajectoryInterpolation::Ruckig: - update_v3(q_previous, q_current, q_next_, q_next_, status); - break; } } @@ -59,13 +54,9 @@ void planning::trajectory::UpdatingState::update(std::shared_ptr &q update_v1(q_previous, q_current, q_next_, q_next_reached_, status); break; - case planning::TrajectoryInterpolation::Spline: + default: update_v2(q_previous, q_current, q_next_, q_next_reached_, status); break; - - case planning::TrajectoryInterpolation::Ruckig: - update_v3(q_previous, q_current, q_next_, q_next_reached_, status); - break; } } @@ -120,9 +111,8 @@ void planning::trajectory::UpdatingState::update_v1(std::shared_ptr // << (status == base::State::Status::Reached ? "Reached" : "") << "\n"; } -/// @brief Update a current state of the robot using 'traj->spline_current'. -/// Compute a new spline 'traj->spline_next', or remain 'traj->spline_current'. -/// Move 'q_current' towards 'q_next_reached' while following 'traj->spline_next'. +/// @brief Update a current state of the robot by computing a trajectory 'traj'. +/// Move 'q_current' towards 'q_next_reached'. /// 'q_current' will be updated to a robot position from the end of current iteration. /// @note The new trajectory will be computed in a way that all constraints on robot's maximal velocity, /// acceleration and jerk are surely always satisfied. @@ -133,194 +123,66 @@ void planning::trajectory::UpdatingState::update_v2(std::shared_ptr q_previous = q_current; q_next = q_next_; q_next_reached = q_next_reached_; - traj->spline_current = traj->spline_next; - float t_traj_max { (guaranteed_safe_motion ? TrajectoryConfig::MAX_TIME_COMPUTE_SAFE : TrajectoryConfig::MAX_TIME_COMPUTE_REGULAR) - - TrajectoryConfig::MAX_TIME_PUBLISH * measure_time }; + float t_traj_max + { + (guaranteed_safe_motion ? + TrajectoryConfig::MAX_TIME_COMPUTE_SAFE : + TrajectoryConfig::MAX_TIME_COMPUTE_REGULAR) + - TrajectoryConfig::MAX_TIME_PUBLISH * measure_time + }; + float t_iter { getElapsedTime() }; if (max_iter_time - max_remaining_iter_time - t_iter < t_traj_max) t_traj_max = max_iter_time - max_remaining_iter_time - t_iter; float t_iter_remain { max_iter_time - t_iter - t_traj_max }; - float t_traj_current { measure_time ? - traj->spline_current->getTimeCurrent(true) + t_traj_max : - traj->spline_current->getTimeEnd() + t_iter + t_traj_max }; + float t_traj_current + { + measure_time ? + traj->getTimeCurrent(true) + t_traj_max : + traj->getTimeEnd() + t_iter + t_traj_max + }; - traj->spline_current->setTimeBegin(traj->spline_current->getTimeEnd()); - traj->spline_current->setTimeCurrent(t_traj_current); + traj->setTimeBegin(traj->getTimeEnd()); + traj->setTimeCurrent(t_traj_current); // std::cout << "Iter. time: " << t_iter * 1000 << " [ms] \n"; // std::cout << "Max. trajectory time: " << t_traj_max * 1000 << " [ms] \n"; // std::cout << "Remain. time: " << t_iter_remain * 1000 << " [ms] \n"; - // std::cout << "Begin trajectory time: " << traj->spline_current->getTimeBegin() * 1000 << " [ms] \n"; + // std::cout << "Begin trajectory time: " << traj->getTimeBegin() * 1000 << " [ms] \n"; // std::cout << "Curr. trajectory time: " << t_traj_current * 1000 << " [ms] \n"; - // ----------------------------------------------------------------------------------------- // - - bool traj_computed { false }; - float t_traj_remain {}; - Eigen::VectorXf current_pos { traj->spline_current->getPosition(t_traj_current) }; - Eigen::VectorXf current_vel { traj->spline_current->getVelocity(t_traj_current) }; - Eigen::VectorXf current_acc { traj->spline_current->getAcceleration(t_traj_current) }; - // std::cout << "Curr. pos: " << current_pos.transpose() << "\n"; - // std::cout << "Curr. vel: " << current_vel.transpose() << "\n"; - // std::cout << "Curr. acc: " << current_acc.transpose() << "\n"; - - do - { - t_traj_remain = t_traj_max - (getElapsedTime() - t_iter); - // std::cout << "t_traj_remain: " << t_traj_remain * 1e3 << " [ms] \n"; - if (t_traj_remain < 0) - break; - - float step { std::max(ss->robot->getMaxVel().norm() * max_iter_time, - current_vel.norm() / ss->robot->getMaxVel().norm() * TrajectoryConfig::MAX_RADIUS) }; - std::shared_ptr q_target { std::get<1>(ss->interpolateEdge2(q_current, q_next_reached, step)) }; - - traj->setCurrentState(q_current); - traj->setTargetState(q_target); - // std::cout << "q_target: " << q_target << "\n"; - // std::cout << "q_next: " << q_next << "\n"; - // std::cout << "q_next_reached: " << q_next_reached << "\n"; - - if (guaranteed_safe_motion) - traj_computed = traj->computeSafe(current_pos, current_vel, current_acc, t_iter_remain, t_traj_remain); - else - { - if (traj->spline_current->isFinalConf(q_target->getCoord())) // Spline to such 'q_target' already exists! - break; - traj_computed = traj->computeRegular(current_pos, current_vel, current_acc, t_iter_remain, t_traj_remain, non_zero_final_vel); - } - } - while (!traj_computed && invokeChangeNextState()); - - if (traj_computed) - { - // std::cout << "New trajectory is computed! \n"; - traj->spline_current->setTimeEnd(t_traj_current); - traj->spline_next->setTimeEnd(t_iter_remain); - } - else - { - // std::cout << "Continuing with the previous trajectory! \n"; - traj->spline_next = traj->spline_current; - traj->spline_next->setTimeEnd(t_traj_current + t_iter_remain); - } - // traj->recordTrajectory(traj_computed); // Only for debugging - - q_current = ss->getNewState(traj->spline_next->getPosition(traj->spline_next->getTimeEnd())); // Current robot position at the end of iteration - if (status != base::State::Status::Trapped) - { - if (ss->getNorm(q_current, q_next) <= // 'q_next' must be reached within the computed radius, and not only 'q_next_reached' - traj->spline_next->getVelocity(traj->spline_next->getTimeEnd()).norm() / ss->robot->getMaxVel().norm() * TrajectoryConfig::MAX_RADIUS) - status = base::State::Status::Reached; - else - status = base::State::Status::Advanced; - } - - // std::cout << "Elapsed time for trajectory computing: " << (getElapsedTime() - t_iter) * 1e3 << " [ms] \n"; - // std::cout << "Spline next: \n" << traj->spline_next << "\n"; - // std::cout << "q_current: " << q_current << "\n"; - // std::cout << "Status: " << (status == base::State::Status::Advanced ? "Advanced" : "") - // << (status == base::State::Status::Trapped ? "Trapped" : "") - // << (status == base::State::Status::Reached ? "Reached" : "") << "\n"; - // std::cout << "Current spline times: " << traj->spline_current->getTimeBegin() * 1000 << " [ms] \t" - // << traj->spline_current->getTimeCurrent() * 1000 << " [ms] \t" - // << traj->spline_current->getTimeEnd() * 1000 << " [ms] \n"; - // std::cout << "Next spline times: " << traj->spline_next->getTimeBegin() * 1000 << " [ms] \t" - // << traj->spline_next->getTimeCurrent() * 1000 << " [ms] \t" - // << traj->spline_next->getTimeEnd() * 1000 << " [ms] \n"; - // ----------------------------------------------------------------------------------------- // // Store trajectory points from the current iteration to be validated later within 'MotionValidity' traj->clearTrajPointCurrentIter(); - size_t num_checks1 = std::floor((traj->spline_current->getTimeCurrent() - traj->spline_current->getTimeBegin()) / + size_t num_checks1 = std::floor((traj->getTimeCurrent() - traj->getTimeBegin()) / max_iter_time * TrajectoryConfig::NUM_VALIDITY_POINTS_CHECK ); - float delta_time1 { (traj->spline_current->getTimeCurrent() - traj->spline_current->getTimeBegin()) / num_checks1 }; + float delta_time1 { (traj->getTimeCurrent() - traj->getTimeBegin()) / num_checks1 }; float t { 0 }; for (size_t num_check = 1; num_check <= num_checks1; num_check++) { - t = traj->spline_current->getTimeBegin() + num_check * delta_time1; - traj->addTrajPointCurrentIter(traj->spline_current->getPosition(t)); + t = traj->getTimeBegin() + num_check * delta_time1; + traj->addTrajPointCurrentIter(traj->getPosition(t)); // std::cout << "t: " << t * 1000 << " [ms]\t" - // << "pos: " << traj->spline_current->getPosition(t).transpose() << "\t" - // << "vel: " << traj->spline_current->getVelocity(t).transpose() << "\t" - // << "acc: " << traj->spline_current->getAcceleration(t).transpose() << "\n"; - } - - size_t num_checks2 { TrajectoryConfig::NUM_VALIDITY_POINTS_CHECK - num_checks1 }; - float delta_time2 { (traj->spline_next->getTimeEnd() - traj->spline_next->getTimeCurrent()) / num_checks2 }; - for (size_t num_check = 1; num_check <= num_checks2; num_check++) - { - t = traj->spline_next->getTimeCurrent() + num_check * delta_time2; - traj->addTrajPointCurrentIter(traj->spline_next->getPosition(t)); - // std::cout << "t: " << t * 1000 << " [ms]\t" - // << "pos: " << traj->spline_next->getPosition(t).transpose() << "\t" - // << "vel: " << traj->spline_next->getVelocity(t).transpose() << "\t" - // << "acc: " << traj->spline_next->getAcceleration(t).transpose() << "\n"; + // << "pos: " << traj->getPosition(t).transpose() << "\t" + // << "vel: " << traj->getVelocity(t).transpose() << "\t" + // << "acc: " << traj->getAcceleration(t).transpose() << "\n"; } // ----------------------------------------------------------------------------------------- // - - remaining_time = t_traj_max + TrajectoryConfig::MAX_TIME_PUBLISH * measure_time - (getElapsedTime() - t_iter); -} - -/// @brief Update a current state of the robot using Ruckig library. -/// Move 'q_current' towards 'q_next_reached'. -/// 'q_current' will be updated to a robot position from the end of current iteration. -/// @note All constraints on robot's maximal velocity and acceleration are surely always satisfied. -/// @note If 'q_next_reached' is not relevant in the algorithm, pass 'q_next' instead of it. -void planning::trajectory::UpdatingState::update_v3(std::shared_ptr &q_previous, std::shared_ptr &q_current, - const std::shared_ptr q_next_, const std::shared_ptr q_next_reached_, base::State::Status &status) -{ - q_previous = q_current; - q_next = q_next_; - q_next_reached = q_next_reached_; - - float t_traj_max { (guaranteed_safe_motion ? TrajectoryConfig::MAX_TIME_COMPUTE_SAFE : TrajectoryConfig::MAX_TIME_COMPUTE_REGULAR) - - TrajectoryConfig::MAX_TIME_PUBLISH * measure_time }; - float t_iter { getElapsedTime() }; - if (max_iter_time - max_remaining_iter_time - t_iter < t_traj_max) - t_traj_max = max_iter_time - max_remaining_iter_time - t_iter; - float t_iter_remain { max_iter_time - t_iter - t_traj_max }; - float t_traj_current { measure_time ? - traj_ruckig->getTimeCurrent(true) + t_traj_max : - traj_ruckig->getTimeEnd() + t_iter + t_traj_max }; - - traj_ruckig->setTimeBegin(traj_ruckig->getTimeEnd()); - traj_ruckig->setTimeCurrent(t_traj_current); - - // std::cout << "Iter. time: " << t_iter * 1000 << " [ms] \n"; - // std::cout << "Max. trajectory time: " << t_traj_max * 1000 << " [ms] \n"; - // std::cout << "Remain. time: " << t_iter_remain * 1000 << " [ms] \n"; - // std::cout << "Curr. trajectory time: " << t_traj_current * 1000 << " [ms] \n"; - - // ----------------------------------------------------------------------------------------- // - // Store trajectory points from the current iteration to be validated later within 'MotionValidity' - traj_ruckig->clearTrajPointCurrentIter(); - size_t num_checks1 = std::floor((traj_ruckig->getTimeCurrent() - traj_ruckig->getTimeBegin()) / - max_iter_time * TrajectoryConfig::NUM_VALIDITY_POINTS_CHECK ); - float delta_time1 { (traj_ruckig->getTimeCurrent() - traj_ruckig->getTimeBegin()) / num_checks1 }; - float t { 0 }; - for (size_t num_check = 1; num_check <= num_checks1; num_check++) - { - t = traj_ruckig->getTimeBegin() + num_check * delta_time1; - traj_ruckig->addTrajPointCurrentIter(traj_ruckig->getPosition(t)); - // std::cout << "t: " << t * 1000 << " [ms]\t" - // << "pos: " << traj_ruckig->getPosition(t).transpose() << "\t" - // << "vel: " << traj_ruckig->getVelocity(t).transpose() << "\t" - // << "acc: " << traj_ruckig->getAcceleration(t).transpose() << "\n"; - } - // ----------------------------------------------------------------------------------------- // - bool traj_computed { false }; float t_traj_remain {}; - Eigen::VectorXf current_pos { traj_ruckig->getPosition(t_traj_current) }; - Eigen::VectorXf current_vel { traj_ruckig->getVelocity(t_traj_current) }; - Eigen::VectorXf current_acc { traj_ruckig->getAcceleration(t_traj_current) }; - // std::cout << "Curr. pos: " << current_pos.transpose() << "\n"; - // std::cout << "Curr. vel: " << current_vel.transpose() << "\n"; - // std::cout << "Curr. acc: " << current_acc.transpose() << "\n"; + planning::trajectory::State current + ( + traj->getPosition(t_traj_current), + traj->getVelocity(t_traj_current), + traj->getAcceleration(t_traj_current) + ); + planning::trajectory::State target(ss->num_dimensions); + std::cout << "Curr. pos: " << current.pos.transpose() << "\n"; + std::cout << "Curr. vel: " << current.vel.transpose() << "\n"; + std::cout << "Curr. acc: " << current.acc.transpose() << "\n"; do { @@ -329,61 +191,61 @@ void planning::trajectory::UpdatingState::update_v3(std::shared_ptr if (t_traj_remain < 0) break; - float step { std::max(ss->robot->getMaxVel().norm() * max_iter_time, - current_vel.norm() / ss->robot->getMaxVel().norm() * TrajectoryConfig::MAX_RADIUS) }; - std::shared_ptr q_target { std::get<1>(ss->interpolateEdge2(q_current, q_next_reached, step)) }; + float step = std::max(ss->robot->getMaxVel().norm() * max_iter_time, + current.vel.norm() / ss->robot->getMaxVel().norm() * TrajectoryConfig::MAX_RADIUS); + target.pos = (std::get<1>(ss->interpolateEdge2(q_current, q_next_reached, step)))->getCoord(); - traj_ruckig->setCurrentState(q_current->getCoord()); - traj_ruckig->setTargetState(q_target->getCoord()); - // std::cout << "q_target: " << q_target << "\n"; + std::cout << "target pos: " << target.pos.transpose() << "\n"; // std::cout << "q_next: " << q_next << "\n"; // std::cout << "q_next_reached: " << q_next_reached << "\n"; - if (traj_ruckig->isFinalConf(q_target->getCoord())) // Trajectory to such 'q_target' already exists! - break; - traj_computed = traj_ruckig->computeRegular(current_pos, current_vel, current_acc, t_iter_remain, t_traj_remain, non_zero_final_vel); + if (guaranteed_safe_motion) + traj_computed = traj->computeSafe(current, target, t_iter_remain, t_traj_remain, q_current); + else + { + if (traj->isFinalConf(target.pos)) // Spline to such 'target.pos' already exists! + break; + traj_computed = traj->computeRegular(current, target, t_iter_remain, t_traj_remain, non_zero_final_vel); + } } while (!traj_computed && invokeChangeNextState()); - if (traj_computed) - { - // std::cout << "New trajectory is computed! \n"; - traj_ruckig->setTimeEnd(t_iter_remain); - } - else - { - // std::cout << "Continuing with the previous trajectory! \n"; - traj_ruckig->setTimeEnd(t_traj_current + t_iter_remain); - } + traj->setTimeEnd(!traj_computed * t_traj_current + t_iter_remain); + std::cout << "New trajectory is " << (traj_computed ? "computed!\n" : "NOT computed! Continuing with the previous trajectory!\n"); + // traj->recordTrajectory(traj_computed); // Only for debugging - q_current = ss->getNewState(traj_ruckig->getPosition(traj_ruckig->getTimeEnd())); // Current robot position at the end of iteration + q_current = ss->getNewState(traj->getPosition(traj->getTimeEnd())); // Current robot position at the end of iteration + if (status != base::State::Status::Trapped) { if (ss->getNorm(q_current, q_next) <= // 'q_next' must be reached within the computed radius, and not only 'q_next_reached' - traj_ruckig->getVelocity(traj_ruckig->getTimeEnd()).norm() / ss->robot->getMaxVel().norm() * TrajectoryConfig::MAX_RADIUS) + traj->getVelocity(traj->getTimeEnd()).norm() / ss->robot->getMaxVel().norm() * TrajectoryConfig::MAX_RADIUS) status = base::State::Status::Reached; else status = base::State::Status::Advanced; } - - // std::cout << "Elapsed time for trajectory computing: " << (getElapsedTime() - t_iter) * 1e3 << " [ms] \n"; - // std::cout << "q_current: " << q_current << "\n"; - // std::cout << "Status: " << (status == base::State::Status::Advanced ? "Advanced" : "") - // << (status == base::State::Status::Trapped ? "Trapped" : "") - // << (status == base::State::Status::Reached ? "Reached" : "") << "\n"; + std::cout << "Elapsed time for trajectory computing: " << (getElapsedTime() - t_iter) * 1e6 << " [us] \n"; + std::cout << "q_current: " << q_current << "\n"; + std::cout << "Status: " << (status == base::State::Status::Advanced ? "Advanced" : "") + << (status == base::State::Status::Trapped ? "Trapped" : "") + << (status == base::State::Status::Reached ? "Reached" : "") << "\n"; + std::cout << "Trajectory times: " << traj->getTimeBegin() * 1000 << " [ms] \t" + << traj->getTimeCurrent() * 1000 << " [ms] \t" + << traj->getTimeEnd() * 1000 << " [ms] \n"; + // ----------------------------------------------------------------------------------------- // // Store trajectory points from the current iteration to be validated later within 'MotionValidity' size_t num_checks2 { TrajectoryConfig::NUM_VALIDITY_POINTS_CHECK - num_checks1 }; - float delta_time2 { (traj_ruckig->getTimeEnd() - traj_ruckig->getTimeCurrent()) / num_checks2 }; + float delta_time2 { (traj->getTimeEnd() - traj->getTimeCurrent()) / num_checks2 }; for (size_t num_check = 1; num_check <= num_checks2; num_check++) { - t = traj_ruckig->getTimeCurrent() + num_check * delta_time2; - traj_ruckig->addTrajPointCurrentIter(traj_ruckig->getPosition(t)); + t = traj->getTimeCurrent() + num_check * delta_time2; + traj->addTrajPointCurrentIter(traj->getPosition(t)); // std::cout << "t: " << t * 1000 << " [ms]\t" - // << "pos: " << traj_ruckig->getPosition(t).transpose() << "\t" - // << "vel: " << traj_ruckig->getVelocity(t).transpose() << "\t" - // << "acc: " << traj_ruckig->getAcceleration(t).transpose() << "\n"; + // << "pos: " << traj->getPosition(t).transpose() << "\t" + // << "vel: " << traj->getVelocity(t).transpose() << "\t" + // << "acc: " << traj->getAcceleration(t).transpose() << "\n"; } // ----------------------------------------------------------------------------------------- // From 3db012b69964ae9c33f8d32cf46debd0af99378a Mon Sep 17 00:00:00 2001 From: Nermin Covic Date: Sat, 18 Oct 2025 22:18:26 +0200 Subject: [PATCH 39/40] Console outputs commented. --- src/planners/trajectory/Trajectory.cpp | 8 +++--- src/planners/trajectory/TrajectoryRuckig.cpp | 7 +++--- src/planners/trajectory/UpdatingState.cpp | 26 ++++++++++---------- 3 files changed, 21 insertions(+), 20 deletions(-) diff --git a/src/planners/trajectory/Trajectory.cpp b/src/planners/trajectory/Trajectory.cpp index d8308672..b65d465b 100644 --- a/src/planners/trajectory/Trajectory.cpp +++ b/src/planners/trajectory/Trajectory.cpp @@ -8,7 +8,7 @@ planning::trajectory::Trajectory::Trajectory(const std::shared_ptr &ss_, planning::trajectory::State current, float max_iter_time_) : - AbstractTrajectory(ss_, max_iter_time) + AbstractTrajectory(ss_, max_iter_time_) { spline = std::make_shared(ss->robot, current.pos, current.vel, current.acc); } @@ -70,9 +70,9 @@ bool planning::trajectory::Trajectory::computeRegular(planning::trajectory::Stat setSpline(spline_new); } - std::cout << "\t Spline " << (!spline_computed ? "NOT " : "") << "computed with " - << (!is_zero_final_vel ? "NON-" : "") << "ZERO final velocity. \n"; - std::cout << "Spline: \n" << spline << "\n"; + // std::cout << "\t Spline " << (!spline_computed ? "NOT " : "") << "computed with " + // << (!is_zero_final_vel ? "NON-" : "") << "ZERO final velocity. \n"; + // std::cout << "Spline: \n" << spline << "\n"; return spline_computed; } diff --git a/src/planners/trajectory/TrajectoryRuckig.cpp b/src/planners/trajectory/TrajectoryRuckig.cpp index b875579e..75b0b954 100644 --- a/src/planners/trajectory/TrajectoryRuckig.cpp +++ b/src/planners/trajectory/TrajectoryRuckig.cpp @@ -8,7 +8,7 @@ planning::trajectory::TrajectoryRuckig::TrajectoryRuckig(const std::shared_ptr &ss_, planning::trajectory::State current, float max_iter_time_) : - AbstractTrajectory(ss_, max_iter_time), + AbstractTrajectory(ss_, max_iter_time_), input(ss_->num_dimensions), traj(ss_->num_dimensions) { @@ -113,8 +113,9 @@ bool planning::trajectory::TrajectoryRuckig::computeRegular(planning::trajectory /// @param t_max Maximal available time in [s] for a trajectory computing /// @param q_current Current robot's configuration /// @return The success of a trajectory computation -bool planning::trajectory::TrajectoryRuckig::computeSafe(planning::trajectory::State current, - planning::trajectory::State target, float t_iter_remain, float t_max, const std::shared_ptr q_current) +bool planning::trajectory::TrajectoryRuckig::computeSafe([[maybe_unused]] planning::trajectory::State current, + [[maybe_unused]] planning::trajectory::State target, [[maybe_unused]] float t_iter_remain, [[maybe_unused]] float t_max, + [[maybe_unused]] const std::shared_ptr q_current) { // TODO return false; diff --git a/src/planners/trajectory/UpdatingState.cpp b/src/planners/trajectory/UpdatingState.cpp index a3a6546f..deed5734 100644 --- a/src/planners/trajectory/UpdatingState.cpp +++ b/src/planners/trajectory/UpdatingState.cpp @@ -180,9 +180,9 @@ void planning::trajectory::UpdatingState::update_v2(std::shared_ptr traj->getAcceleration(t_traj_current) ); planning::trajectory::State target(ss->num_dimensions); - std::cout << "Curr. pos: " << current.pos.transpose() << "\n"; - std::cout << "Curr. vel: " << current.vel.transpose() << "\n"; - std::cout << "Curr. acc: " << current.acc.transpose() << "\n"; + // std::cout << "Curr. pos: " << current.pos.transpose() << "\n"; + // std::cout << "Curr. vel: " << current.vel.transpose() << "\n"; + // std::cout << "Curr. acc: " << current.acc.transpose() << "\n"; do { @@ -195,7 +195,7 @@ void planning::trajectory::UpdatingState::update_v2(std::shared_ptr current.vel.norm() / ss->robot->getMaxVel().norm() * TrajectoryConfig::MAX_RADIUS); target.pos = (std::get<1>(ss->interpolateEdge2(q_current, q_next_reached, step)))->getCoord(); - std::cout << "target pos: " << target.pos.transpose() << "\n"; + // std::cout << "target pos: " << target.pos.transpose() << "\n"; // std::cout << "q_next: " << q_next << "\n"; // std::cout << "q_next_reached: " << q_next_reached << "\n"; @@ -211,7 +211,7 @@ void planning::trajectory::UpdatingState::update_v2(std::shared_ptr while (!traj_computed && invokeChangeNextState()); traj->setTimeEnd(!traj_computed * t_traj_current + t_iter_remain); - std::cout << "New trajectory is " << (traj_computed ? "computed!\n" : "NOT computed! Continuing with the previous trajectory!\n"); + // std::cout << "New trajectory is " << (traj_computed ? "computed!\n" : "NOT computed! Continuing with the previous trajectory!\n"); // traj->recordTrajectory(traj_computed); // Only for debugging q_current = ss->getNewState(traj->getPosition(traj->getTimeEnd())); // Current robot position at the end of iteration @@ -225,14 +225,14 @@ void planning::trajectory::UpdatingState::update_v2(std::shared_ptr status = base::State::Status::Advanced; } - std::cout << "Elapsed time for trajectory computing: " << (getElapsedTime() - t_iter) * 1e6 << " [us] \n"; - std::cout << "q_current: " << q_current << "\n"; - std::cout << "Status: " << (status == base::State::Status::Advanced ? "Advanced" : "") - << (status == base::State::Status::Trapped ? "Trapped" : "") - << (status == base::State::Status::Reached ? "Reached" : "") << "\n"; - std::cout << "Trajectory times: " << traj->getTimeBegin() * 1000 << " [ms] \t" - << traj->getTimeCurrent() * 1000 << " [ms] \t" - << traj->getTimeEnd() * 1000 << " [ms] \n"; + // std::cout << "Elapsed time for trajectory computing: " << (getElapsedTime() - t_iter) * 1e6 << " [us] \n"; + // std::cout << "q_current: " << q_current << "\n"; + // std::cout << "Status: " << (status == base::State::Status::Advanced ? "Advanced" : "") + // << (status == base::State::Status::Trapped ? "Trapped" : "") + // << (status == base::State::Status::Reached ? "Reached" : "") << "\n"; + // std::cout << "Trajectory times: " << traj->getTimeBegin() * 1000 << " [ms] \t" + // << traj->getTimeCurrent() * 1000 << " [ms] \t" + // << traj->getTimeEnd() * 1000 << " [ms] \n"; // ----------------------------------------------------------------------------------------- // // Store trajectory points from the current iteration to be validated later within 'MotionValidity' From 2cf138e35de4fb895a05fd89b10a666a555456a5 Mon Sep 17 00:00:00 2001 From: Nermin Covic Date: Thu, 23 Oct 2025 12:28:47 +0200 Subject: [PATCH 40/40] Added 'convertPathToTraj' method for converting a path to a corresponding trajectory. --- apps/test_trajectory.cpp | 27 +++++----- .../planners/trajectory/AbstractTrajectory.h | 1 + include/planners/trajectory/Trajectory.h | 10 ++-- .../planners/trajectory/TrajectoryRuckig.h | 7 ++- src/planners/trajectory/Trajectory.cpp | 33 +++++++++--- src/planners/trajectory/TrajectoryRuckig.cpp | 54 +++++++++++++++++-- 6 files changed, 101 insertions(+), 31 deletions(-) diff --git a/apps/test_trajectory.cpp b/apps/test_trajectory.cpp index 987b2f76..5cab859e 100644 --- a/apps/test_trajectory.cpp +++ b/apps/test_trajectory.cpp @@ -2,6 +2,7 @@ #include "ConfigurationReader.h" #include "CommonFunctions.h" #include "Trajectory.h" +#include "TrajectoryRuckig.h" int main(int argc, char **argv) { @@ -44,7 +45,8 @@ int main(int argc, char **argv) std::shared_ptr q_goal { scenario.getGoal() }; std::shared_ptr env { scenario.getEnvironment() }; std::unique_ptr planner { nullptr }; - std::shared_ptr trajectory { nullptr }; + std::shared_ptr trajectory1 { nullptr }; + std::shared_ptr trajectory2 { nullptr }; bool result { false }; size_t num_obs { env->getNumObjects() }; @@ -102,25 +104,26 @@ int main(int argc, char **argv) output_file << new_path.at(i)->getCoord().transpose() << "\n"; output_file << "--------------------------------------------------------------------\n"; - trajectory = std::make_shared(ss); + trajectory1 = std::make_shared(ss); auto time1 = std::chrono::steady_clock::now(); - trajectory->path2traj_v1(new_path); + trajectory1->convertPathToTraj(new_path); comp_times1.emplace_back(std::chrono::duration_cast(std::chrono::steady_clock::now() - time1).count() * 1e-3); - final_times1.emplace_back(trajectory->getTimeFinal()); + final_times1.emplace_back(trajectory1->getTimeFinal()); - output_file << "Trajectory 1 (position): \n"; - for (float t = 0; t < trajectory->getTimeFinal(); t += delta_time) - output_file << trajectory->getPosition(t).transpose() << "\n"; + output_file << "Trajectory ours (position): \n"; + for (float t = 0; t <= trajectory1->getTimeFinal(); t += delta_time) + output_file << trajectory1->getPosition(t).transpose() << "\n"; output_file << "--------------------------------------------------------------------\n"; + trajectory2 = std::make_shared(ss, new_path.size()-2); auto time2 = std::chrono::steady_clock::now(); - trajectory->path2traj_v2(new_path); + trajectory2->convertPathToTraj(new_path); comp_times2.emplace_back(std::chrono::duration_cast(std::chrono::steady_clock::now() - time2).count() * 1e-3); - final_times2.emplace_back(trajectory->getTimeFinal()); + final_times2.emplace_back(trajectory2->getTimeFinal()); - output_file << "Trajectory 2 (position): \n"; - for (float t = 0; t < trajectory->getTimeFinal(); t += delta_time) - output_file << trajectory->getPosition(t).transpose() << "\n"; + output_file << "Trajectory ruckig (position): \n"; + for (float t = 0; t <= trajectory2->getTimeFinal(); t += delta_time) + output_file << trajectory2->getPosition(t).transpose() << "\n"; output_file << "--------------------------------------------------------------------\n"; output_file.close(); } diff --git a/include/planners/trajectory/AbstractTrajectory.h b/include/planners/trajectory/AbstractTrajectory.h index 33fea9fd..4dfabc0b 100644 --- a/include/planners/trajectory/AbstractTrajectory.h +++ b/include/planners/trajectory/AbstractTrajectory.h @@ -35,6 +35,7 @@ namespace planning::trajectory float t_iter_remain, float t_max, bool non_zero_final_vel) = 0; virtual bool computeSafe(planning::trajectory::State current, planning::trajectory::State target, float t_iter_remain, float t_max, const std::shared_ptr q_current) = 0; + virtual bool convertPathToTraj(const std::vector> &path) = 0; virtual Eigen::VectorXf getPosition(float t) = 0; virtual Eigen::VectorXf getVelocity(float t) = 0; diff --git a/include/planners/trajectory/Trajectory.h b/include/planners/trajectory/Trajectory.h index 049c3ea1..ef6cfda4 100644 --- a/include/planners/trajectory/Trajectory.h +++ b/include/planners/trajectory/Trajectory.h @@ -29,10 +29,8 @@ namespace planning::trajectory inline Eigen::VectorXf getVelocity(float t) override { return spline->getVelocity(t); } inline Eigen::VectorXf getAcceleration(float t) override { return spline->getAcceleration(t); } - void path2traj_v1(const std::vector> &path); - void path2traj_v2(const std::vector> &path); - void path2traj_v3(const std::vector> &path, bool must_visit); - + bool convertPathToTraj(const std::vector> &path) override; + private: bool isSafe(const std::shared_ptr spline_safe, const std::shared_ptr q_current, float t_iter); @@ -40,6 +38,10 @@ namespace planning::trajectory const std::shared_ptr> nearest_points, float delta_t); void setSpline(const std::shared_ptr spline_); + bool convertPathToTraj_v1(const std::vector> &path); + bool convertPathToTraj_v2(const std::vector> &path); + bool convertPathToTraj_v3(const std::vector> &path, bool must_visit = false); + std::shared_ptr spline; }; } diff --git a/include/planners/trajectory/TrajectoryRuckig.h b/include/planners/trajectory/TrajectoryRuckig.h index c036ba50..7fd13c9b 100644 --- a/include/planners/trajectory/TrajectoryRuckig.h +++ b/include/planners/trajectory/TrajectoryRuckig.h @@ -13,7 +13,7 @@ namespace planning::trajectory class TrajectoryRuckig : public AbstractTrajectory { public: - TrajectoryRuckig(const std::shared_ptr &ss_); + TrajectoryRuckig(const std::shared_ptr &ss_, size_t num_waypoints); TrajectoryRuckig(const std::shared_ptr &ss_, planning::trajectory::State current, float max_iter_time_); ~TrajectoryRuckig(); @@ -25,13 +25,16 @@ namespace planning::trajectory Eigen::VectorXf getPosition(float t) override; Eigen::VectorXf getVelocity(float t) override; Eigen::VectorXf getAcceleration(float t) override; + + bool convertPathToTraj(const std::vector> &path) override; private: + void setParams(); void setCurrentState(const planning::trajectory::State ¤t); void setTargetState(const planning::trajectory::State &target); ruckig::InputParameter input; - // ruckig::OutputParameter output; + ruckig::OutputParameter output; ruckig::Trajectory traj; }; diff --git a/src/planners/trajectory/Trajectory.cpp b/src/planners/trajectory/Trajectory.cpp index b65d465b..58267f89 100644 --- a/src/planners/trajectory/Trajectory.cpp +++ b/src/planners/trajectory/Trajectory.cpp @@ -336,15 +336,23 @@ float planning::trajectory::Trajectory::computeDistanceUnderestimation(const std return d_c; } +void planning::trajectory::Trajectory::setSpline(const std::shared_ptr spline_) +{ + spline = spline_; + time_current = 0; + time_final = spline->getTimeFinal(); +} + /// @brief A method (v1) to convert a path 'path' to a corresponding trajectory. /// Converting this path to trajectory (i.e., assigning time instances to these points) will be automatically done by this function. /// This is done by creating a sequence of quintic splines in a way that all constraints on robot's maximal velocity, /// acceleration and jerk are surely always satisfied. /// @param path Path containing all points that robot should visit. +/// @return Success of converting a path to a corresponding trajectory. /// @note Be careful since the distance between each two adjacent points from 'path' should not be too long! /// The robot motion between them is generally not a straight line in C-space. /// Consider using 'preprocessPath' function from 'base::RealVectorSpace' class before using this function. -void planning::trajectory::Trajectory::path2traj_v1(const std::vector> &path) +bool planning::trajectory::Trajectory::convertPathToTraj_v1(const std::vector> &path) { std::vector> subsplines {}; std::shared_ptr spline_current @@ -407,6 +415,8 @@ void planning::trajectory::Trajectory::path2traj_v1(const std::vector(subsplines)); + + return true; } /// @brief A method (v2) to convert a path 'path' to a corresponding trajectory. @@ -414,10 +424,11 @@ void planning::trajectory::Trajectory::path2traj_v1(const std::vector> &path) +bool planning::trajectory::Trajectory::convertPathToTraj_v2(const std::vector> &path) { std::vector> subsplines {}; std::shared_ptr spline_current @@ -489,6 +500,8 @@ void planning::trajectory::Trajectory::path2traj_v2(const std::vector(subsplines)); + + return true; } /// @brief A method (v3) to convert a path 'path' to a corresponding trajectory. @@ -497,10 +510,11 @@ void planning::trajectory::Trajectory::path2traj_v2(const std::vector> &path, bool must_visit) +bool planning::trajectory::Trajectory::convertPathToTraj_v3(const std::vector> &path, bool must_visit) { std::vector> subsplines(path.size(), nullptr); bool spline_computed { false }; @@ -611,17 +625,20 @@ void planning::trajectory::Trajectory::path2traj_v3(const std::vector(subsplines)); + return true; + } else { std::cout << "Could not convert path to a trajectory! \n"; - // path2traj_v1(path); // Add path using another method. + return false; } } -void planning::trajectory::Trajectory::setSpline(const std::shared_ptr spline_) +bool planning::trajectory::Trajectory::convertPathToTraj(const std::vector> &path) { - spline = spline_; - time_current = 0; - time_final = spline->getTimeFinal(); + return convertPathToTraj_v1(path); // The best results are obtained with this one. + // return convertPathToTraj_v2(path); + // return convertPathToTraj_v3(path); } diff --git a/src/planners/trajectory/TrajectoryRuckig.cpp b/src/planners/trajectory/TrajectoryRuckig.cpp index 75b0b954..2a1f9ba0 100644 --- a/src/planners/trajectory/TrajectoryRuckig.cpp +++ b/src/planners/trajectory/TrajectoryRuckig.cpp @@ -1,19 +1,27 @@ #include "TrajectoryRuckig.h" -planning::trajectory::TrajectoryRuckig::TrajectoryRuckig(const std::shared_ptr &ss_) : +planning::trajectory::TrajectoryRuckig::TrajectoryRuckig(const std::shared_ptr &ss_, size_t num_waypoints) : AbstractTrajectory(ss_), - input(ss_->num_dimensions), - traj(ss_->num_dimensions) -{} + input(ss_->num_dimensions, num_waypoints), + output(ss_->num_dimensions, num_waypoints), + traj(ss_->num_dimensions, num_waypoints) +{ + setParams(); +} planning::trajectory::TrajectoryRuckig::TrajectoryRuckig (const std::shared_ptr &ss_, planning::trajectory::State current, float max_iter_time_) : AbstractTrajectory(ss_, max_iter_time_), input(ss_->num_dimensions), + output(ss_->num_dimensions), traj(ss_->num_dimensions) { setCurrentState(current); - + setParams(); +} + +void planning::trajectory::TrajectoryRuckig::setParams() +{ for (size_t i = 0; i < ss->num_dimensions; i++) { input.max_velocity[i] = ss->robot->getMaxVel(i); @@ -192,3 +200,39 @@ Eigen::VectorXf planning::trajectory::TrajectoryRuckig::getAcceleration(float t) return ret; } + +bool planning::trajectory::TrajectoryRuckig::convertPathToTraj(const std::vector> &path) +{ + planning::trajectory::State start(path.front()->getCoord()); + planning::trajectory::State goal(path.back()->getCoord()); + setCurrentState(start); + setTargetState(goal); + + if (path.size() > 2) + { + ruckig::StandardVector pos(ss->num_dimensions); + for (size_t idx = 1; idx < path.size()-1; idx++) + { + for (size_t i = 0; i < ss->num_dimensions; i++) + pos[i] = path[idx]->getCoord(i); + + input.intermediate_positions.emplace_back(pos); + } + } + + ruckig::Result result { ruckig::Result::Working }; + ruckig::Ruckig otg(ss->num_dimensions); + + result = otg.calculate(input, traj); + // std::cout << "result: " << result << "\n"; + + if (result == ruckig::Result::Working || result == ruckig::Result::Finished) + { + traj = output.trajectory; + time_current = 0; + time_final = traj.get_duration(); + return true; + } + + return false; +}