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 \ 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 ad68cb31..de2161bc 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,9 @@ 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/ruckig) + +set(PROJECT_LIBRARIES gtest glog gflags nanoflann::nanoflann kdl_parser orocos-kdl fcl ccd yaml-cpp ruckig::ruckig) set(MAIN_PROJECT_BUILD_DIR ${CMAKE_CURRENT_SOURCE_DIR}/build) diff --git a/README.md b/README.md index a1467d6c..3133e270 100644 --- a/README.md +++ b/README.md @@ -143,11 +143,13 @@ 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", "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'. +Note: If any problems occur regarding which version of Ruckig is built (local version from external/ruckig folder or ros-humble-ruckig version), 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```. diff --git a/apps/CMakeLists.txt b/apps/CMakeLists.txt index 58e51a55..72e6fc72 100644 --- a/apps/CMakeLists.txt +++ b/apps/CMakeLists.txt @@ -59,6 +59,26 @@ 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_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}) +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 +89,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 +104,10 @@ install(TARGETS test_drgbt test_drgbt_random_obstacles test_rgbmtstar - generate_random_scenarios - generate_dataset test_rrtx test_rrtx_random_obstacles + test_rt_rgbt + test_trajectory + generate_random_scenarios + generate_dataset DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/bin) \ No newline at end of file diff --git a/apps/test_drgbt_random_obstacles.cpp b/apps/test_drgbt_random_obstacles.cpp index d9a74709..d45e1158 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,14 @@ int main(int argc, char **argv) planner = std::make_unique(ss, scenario.getStart(), scenario.getGoal()); bool result { planner->solve() }; + 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: " << 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"); + // 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 +189,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" << 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; - 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 +240,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..11a38465 --- /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 + { + "updateCurrentState [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() }; + 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: " << 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"; + // 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" << 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; + + 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/apps/test_trajectory.cpp b/apps/test_trajectory.cpp new file mode 100644 index 00000000..987b2f76 --- /dev/null +++ b/apps/test_trajectory.cpp @@ -0,0 +1,145 @@ +#include "RGBTConnect.h" +#include "ConfigurationReader.h" +#include "CommonFunctions.h" +#include "Trajectory.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->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 << "--------------------------------------------------------------------\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->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 << "--------------------------------------------------------------------\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; +} diff --git a/data/configurations/configuration_drgbt.yaml b/data/configurations/configuration_drgbt.yaml index 6b8cf173..65faa12c 100644 --- a/data/configurations/configuration_drgbt.yaml +++ b/data/configurations/configuration_drgbt.yaml @@ -9,5 +9,5 @@ STATIC_PLANNER_TYPE: "RGBMT*" # Name of a static planner (for obtainin 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 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' +TRAJECTORY_INTERPOLATION: "Spline" # Method for interpolation of trajectory: "None", "Spline" or "Ruckig" GUARANTEED_SAFE_MOTION: false # Whether robot motion is surely safe for environment \ No newline at end of file diff --git a/data/configurations/configuration_rrtx.yaml b/data/configurations/configuration_rrtx.yaml index 0a61a71f..25be4682 100644 --- a/data/configurations/configuration_rrtx.yaml +++ b/data/configurations/configuration_rrtx.yaml @@ -7,4 +7,4 @@ MAX_NEIGHBORS: 1000 # Maximal number of neighbors to consider REPLANNING_THROTTLE: 1 # Process obstacles every N iterations START_BIAS: 0.1 # Probability of sampling start directly 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" +TRAJECTORY_INTERPOLATION: "Spline" # Method for interpolation of trajectory: "None", "Spline" or "Ruckig" diff --git a/data/configurations/configuration_rt_rgbt.yaml b/data/configurations/configuration_rt_rgbt.yaml new file mode 100644 index 00000000..ce9c4bb7 --- /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.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' +TRAJECTORY_INTERPOLATION: "Spline" # Method for interpolation of trajectory: "None", "Spline" or "Ruckig" \ No newline at end of file diff --git a/data/configurations/configuration_splines.yaml b/data/configurations/configuration_trajectory.yaml similarity index 93% rename from data/configurations/configuration_splines.yaml rename to data/configurations/configuration_trajectory.yaml index bef547fd..963e7c69 100644 --- a/data/configurations/configuration_splines.yaml +++ b/data/configurations/configuration_trajectory.yaml @@ -2,7 +2,7 @@ MAX_TIME_COMPUTE_REGULAR: 0.001 # Maximal time in [s] for computing a regula MAX_TIME_COMPUTE_SAFE: 0.003 # Maximal time in [s] for computing a safe spline MAX_TIME_PUBLISH: 0.0005 # Maximal time in [s] for publishing a spline when using 'trajectory_msgs::msg::JointTrajectory' ROS2 topic. Determined by time measurements. MAX_TIME_FINAL: 10.0 # Maximal final time in [s] for a spline to be considered as valid -TIME_STEP: 0.01 # Time step used for e.g. checking whether a spline is collision-free +TIME_STEP: 0.001 # Time step used for e.g. checking whether a spline is collision-free FINAL_JERK_STEP: 1.0 # Final jerk step when using bisection method for finding optimal value of coefficient 'c' FINAL_VELOCITY_STEP: 0.1 # Final velocity step when using bisection method for finding optimal value of final velocity MAX_RADIUS: 1.5 # Maximal radius in [rad] used when updating state (it should be experimentally evaluated) \ No newline at end of file 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/external/ruckig b/external/ruckig new file mode 160000 index 00000000..37b6e7a4 --- /dev/null +++ b/external/ruckig @@ -0,0 +1 @@ +Subproject commit 37b6e7a4c60f5befd2506741d5f7d0ae7eefb3db diff --git a/include/configurations/ConfigurationReader.h b/include/configurations/ConfigurationReader.h index 1bf18ee4..c7e33a71 100644 --- a/include/configurations/ConfigurationReader.h +++ b/include/configurations/ConfigurationReader.h @@ -20,7 +20,8 @@ #include "RGBMTStarConfig.h" #include "DRGBTConfig.h" #include "RRTxConfig.h" -#include "SplinesConfig.h" +#include "TrajectoryConfig.h" +#include "RT_RGBTConfig.h" class ConfigurationReader { diff --git a/include/configurations/DRGBTConfig.h b/include/configurations/DRGBTConfig.h index d537d1ef..bac9cf6b 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; @@ -24,7 +23,7 @@ class DRGBTConfig static planning::PlannerType STATIC_PLANNER_TYPE; // Name of a static planner (for obtaining the predefined path). Available planners: "RGBMT*", "RGBT-Connect", "RBT-Connect" and "RRT-Connect" static planning::RealTimeScheduling REAL_TIME_SCHEDULING; // "FPS" - Fixed Priority Scheduling; "None" - Without real-time scheduling static float MAX_TIME_TASK1; // Maximal time which Task 1 can take from the processor - static planning::TrajectoryInterpolation TRAJECTORY_INTERPOLATION; // Method for interpolation of trajectory: "None" or "Spline" + static planning::TrajectoryInterpolation TRAJECTORY_INTERPOLATION; // Method for interpolation of trajectory: "None", "Spline" or "Ruckig" static bool GUARANTEED_SAFE_MOTION; // Whether robot motion is surely safe for environment }; diff --git a/include/configurations/RRTxConfig.h b/include/configurations/RRTxConfig.h index 9d31325e..ca5d149b 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; @@ -22,7 +21,7 @@ class RRTxConfig static size_t REPLANNING_THROTTLE; // Process obstacles every N iterations static float START_BIAS; // Probability of sampling start directly static float RESOLUTION_COLL_CHECK; // Perform collision check when obstacle moves at most 'RESOLUTION_COLL_CHECK' in [m] - static planning::TrajectoryInterpolation TRAJECTORY_INTERPOLATION; // Method for interpolation of trajectory: "None" or "Spline" + static planning::TrajectoryInterpolation TRAJECTORY_INTERPOLATION; // Method for interpolation of trajectory: "None", "Spline" or "Ruckig" }; #endif // RPMPL_RRTXCONFIG_H \ No newline at end of file diff --git a/include/configurations/RT_RGBTConfig.h b/include/configurations/RT_RGBTConfig.h new file mode 100644 index 00000000..a31ffc76 --- /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 "Ruckig" +}; + +#endif //RPMPL_RTRGBTCONFIG_H \ No newline at end of file diff --git a/include/configurations/SplinesConfig.h b/include/configurations/TrajectoryConfig.h similarity index 79% rename from include/configurations/SplinesConfig.h rename to include/configurations/TrajectoryConfig.h index 7c84621f..fca47b0b 100644 --- a/include/configurations/SplinesConfig.h +++ b/include/configurations/TrajectoryConfig.h @@ -2,10 +2,12 @@ // 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 +typedef unsigned long size_t; + +class TrajectoryConfig { public: static float MAX_TIME_COMPUTE_REGULAR; // Maximal time in [s] for computing a regular spline @@ -16,6 +18,7 @@ class SplinesConfig 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_SPLINESCONFIG_H \ No newline at end of file +#endif //RPMPL_TRAJECTORYCONFIG_H \ No newline at end of file diff --git a/include/planners/PlanningTypes.h b/include/planners/PlanningTypes.h index 8896c748..e78e1420 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 @@ -63,13 +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::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 24f70c23..9e85f807 100644 --- a/include/planners/drbt/DRGBT.h +++ b/include/planners/drbt/DRGBT.h @@ -13,7 +13,8 @@ #include "HorizonState.h" #include "UpdatingState.h" #include "MotionValidity.h" -#include "Splines.h" +#include "Trajectory.h" +#include "TrajectoryRuckig.h" // #include // #include @@ -50,22 +51,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 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; + 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; // 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 new file mode 100644 index 00000000..bc2c3b78 --- /dev/null +++ b/include/planners/drbt/RT_RGBT.h @@ -0,0 +1,46 @@ +// +// Created by nermin on 25.09.25. +// + +#ifndef RPMPL_RTRGBT_H +#define RPMPL_RTRGBT_H + +#include "RT_RGBTConfig.h" +#include "RGBTConnect.h" +#include "UpdatingState.h" +#include "MotionValidity.h" +#include "Trajectory.h" +#include "TrajectoryRuckig.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(); + 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 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' + bool compute_new_target_state; // Whether to compute 'q_target' + }; +} + +#endif //RPMPL_RTRGBT_H \ No newline at end of file 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/include/planners/rrtx/RRTx.h b/include/planners/rrtx/RRTx.h index 712e24b3..96148f16 100644 --- a/include/planners/rrtx/RRTx.h +++ b/include/planners/rrtx/RRTx.h @@ -9,7 +9,8 @@ #include "RRTxConfig.h" #include "UpdatingState.h" #include "MotionValidity.h" -#include "Splines.h" +#include "Trajectory.h" +#include "TrajectoryRuckig.h" // #include // #include @@ -87,9 +88,9 @@ namespace planning::rrtx // Path from start to goal std::vector> path_current; - 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::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 // 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/include/planners/trajectory/AbstractTrajectory.h b/include/planners/trajectory/AbstractTrajectory.h new file mode 100644 index 00000000..33fea9fd --- /dev/null +++ b/include/planners/trajectory/AbstractTrajectory.h @@ -0,0 +1,82 @@ +// +// 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_, 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 + 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/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/include/planners/trajectory/MotionValidity.h b/include/planners/trajectory/MotionValidity.h index bbb99327..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 "Splines.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 setSplines(const std::shared_ptr &splines_) { splines = splines_; } + 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 splines; - 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/Spline.h b/include/planners/trajectory/Spline.h index 6895bb48..7519ab33 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 { @@ -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/include/planners/trajectory/Splines.h b/include/planners/trajectory/Splines.h deleted file mode 100644 index ac655c4a..00000000 --- a/include/planners/trajectory/Splines.h +++ /dev/null @@ -1,50 +0,0 @@ -// -// Created by nermin on 20.07.24. -// - -#ifndef RPMPL_SPLINES_H -#define RPMPL_SPLINES_H - -#include "StateSpace.h" -#include "Spline4.h" -#include "Spline5.h" -#include "CompositeSpline.h" -#include "RRTConnectConfig.h" - -namespace planning::trajectory -{ - class Splines - { - public: - 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, - 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, - float t_iter_remain, float t_max); - - 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' - - private: - 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); - - 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 - }; -} - -#endif //RPMPL_SPLINES_H \ No newline at end of file diff --git a/include/planners/trajectory/Trajectory.h b/include/planners/trajectory/Trajectory.h new file mode 100644 index 00000000..049c3ea1 --- /dev/null +++ b/include/planners/trajectory/Trajectory.h @@ -0,0 +1,47 @@ +// +// Created by nermin on 20.07.24. +// + +#ifndef RPMPL_TRAJECTORY_H +#define RPMPL_TRAJECTORY_H + +#include "AbstractTrajectory.h" +#include "Spline4.h" +#include "Spline5.h" +#include "CompositeSpline.h" +#include "RRTConnectConfig.h" + +namespace planning::trajectory +{ + class Trajectory : public AbstractTrajectory + { + public: + Trajectory(const std::shared_ptr &ss_); + Trajectory(const std::shared_ptr &ss_, planning::trajectory::State current, float max_iter_time_); + ~Trajectory(); + + 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; + + 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); + + private: + 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); + void setSpline(const std::shared_ptr spline_); + + std::shared_ptr spline; + }; +} + +#endif //RPMPL_TRAJECTORY_H \ No newline at end of file diff --git a/include/planners/trajectory/TrajectoryRuckig.h b/include/planners/trajectory/TrajectoryRuckig.h new file mode 100644 index 00000000..c036ba50 --- /dev/null +++ b/include/planners/trajectory/TrajectoryRuckig.h @@ -0,0 +1,40 @@ +// +// Created by nermin on 03.10.25. +// + +#ifndef RPMPL_TRAJECTORYRUCKIG_H +#define RPMPL_TRAJECTORYRUCKIG_H + +#include "AbstractTrajectory.h" +#include + +namespace planning::trajectory +{ + class TrajectoryRuckig : public AbstractTrajectory + { + public: + TrajectoryRuckig(const std::shared_ptr &ss_); + TrajectoryRuckig(const std::shared_ptr &ss_, planning::trajectory::State current, float max_iter_time_); + ~TrajectoryRuckig(); + + 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; + + 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; + + }; +} + +#endif //RPMPL_TRAJECTORYRUCKIG_H \ No newline at end of file diff --git a/include/planners/trajectory/UpdatingState.h b/include/planners/trajectory/UpdatingState.h index 354ecc0b..b39a2e3b 100644 --- a/include/planners/trajectory/UpdatingState.h +++ b/include/planners/trajectory/UpdatingState.h @@ -6,9 +6,9 @@ #define RPMPL_UPDATINGSTATE_H #include "StateSpace.h" -#include "Splines.h" #include "RealVectorSpaceConfig.h" #include "PlanningTypes.h" +#include "AbstractTrajectory.h" namespace planning::drbt { @@ -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 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_; } @@ -48,22 +48,20 @@ namespace planning::trajectory 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 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) - 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 'splines->spline_next' - // will become active (if 'planning::TrajectoryInterpolation::Spline' is used). - + 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) + 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/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/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/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 c14b8f97..367be369 100644 --- a/include/state_spaces/real_vector_space/RealVectorSpace.h +++ b/include/state_spaces/real_vector_space/RealVectorSpace.h @@ -30,13 +30,15 @@ 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, 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/CMakeLists.txt b/src/CMakeLists.txt index 2f2b85c8..1a50058a 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/ruckig/include ) # Optionally glob, but only for CMake 3.12 or later: @@ -35,6 +36,8 @@ target_include_directories(rpmpl_library PUBLIC ${INCLUDE_DIRS}) # All users of this library will need at least C++17 target_compile_features(rpmpl_library PUBLIC cxx_std_17) +target_link_libraries(rpmpl_library PRIVATE ruckig::ruckig) + # IDEs should put the headers in a nice place source_group( TREE "${PROJECT_SOURCE_DIR}/include" diff --git a/src/configurations/ConfigurationReader.cpp b/src/configurations/ConfigurationReader.cpp index 1c3b8b9c..fc8824ce 100644 --- a/src/configurations/ConfigurationReader.cpp +++ b/src/configurations/ConfigurationReader.cpp @@ -9,7 +9,8 @@ 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") }; + YAML::Node RT_RGBTConfigRoot { YAML::LoadFile(root_path + "/data/configurations/configuration_rt_rgbt.yaml") }; // RealVectorSpaceConfigRoot if (RealVectorSpaceConfigRoot["NUM_INTERPOLATION_VALIDITY_CHECKS"].IsDefined()) @@ -248,46 +249,78 @@ 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; + + + // 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/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..61fff7e5 --- /dev/null +++ b/src/configurations/TrajectoryConfig.cpp @@ -0,0 +1,11 @@ +#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.001; +float TrajectoryConfig::FINAL_JERK_STEP = 1.0; +float TrajectoryConfig::FINAL_VELOCITY_STEP = 0.1; +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/PlanningTypes.cpp b/src/planners/PlanningTypes.cpp index f3658de2..244efaf4 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; @@ -43,7 +47,7 @@ namespace planning switch (type) { case planning::RealTimeScheduling::None: - os << "none"; + os << "None"; break; case planning::RealTimeScheduling::FPS: @@ -59,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 2551d589..dd820812 100644 --- a/src/planners/drbt/DRGBT.cpp +++ b/src/planners/drbt/DRGBT.cpp @@ -30,24 +30,42 @@ 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; + switch (DRGBTConfig::TRAJECTORY_INTERPOLATION) + { + case planning::TrajectoryInterpolation::None: + traj = nullptr; + break; + + case planning::TrajectoryInterpolation::Spline: + 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); + break; + + case planning::TrajectoryInterpolation::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); + (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::TRAJECTORY_INTERPOLATION, DRGBTConfig::RESOLUTION_COLL_CHECK, &path, DRGBTConfig::MAX_ITER_TIME); - - splines = 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); - } + (ss, DRGBTConfig::RESOLUTION_COLL_CHECK, DRGBTConfig::MAX_ITER_TIME, &path); // std::cout << "DRGBT planner initialized! \n"; } @@ -81,24 +99,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"; @@ -122,7 +142,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; + + default: + is_valid = motion_validity->check(traj->getTrajPointCurrentIter()); + break; + } + + if (!is_valid) { std::cout << "*************** Collision has been occurred!!! *************** \n"; planner_info->setSuccessState(false); @@ -245,8 +277,8 @@ 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) - max_time -= DRGBTConfig::GUARANTEED_SAFE_MOTION ? SplinesConfig::MAX_TIME_COMPUTE_SAFE : SplinesConfig::MAX_TIME_COMPUTE_REGULAR; + 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); for (size_t idx = 0; idx < horizon.size(); idx++) diff --git a/src/planners/drbt/RT_RGBT.cpp b/src/planners/drbt/RT_RGBT.cpp new file mode 100644 index 00000000..21c6e0eb --- /dev/null +++ b/src/planners/drbt/RT_RGBT.cpp @@ -0,0 +1,231 @@ +#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; + + switch (RT_RGBTConfig::TRAJECTORY_INTERPOLATION) + { + case planning::TrajectoryInterpolation::None: + traj = nullptr; + break; + + case planning::TrajectoryInterpolation::Spline: + traj = std::make_shared + ( + ss, + planning::trajectory::State(q_current->getCoord()), + RT_RGBTConfig::MAX_ITER_TIME + ); + break; + + case planning::TrajectoryInterpolation::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"; +} + +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"; + + // ------------------------------------------------------------------------------- // + // 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); + break; + + default: + is_valid = motion_validity->check(traj->getTrajPointCurrentIter()); + 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 a new target state!!! Retaining the existing one.\n"; +} + +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? +} 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/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; } diff --git a/src/planners/rrtx/RRTx.cpp b/src/planners/rrtx/RRTx.cpp index 08651014..521956a7 100644 --- a/src/planners/rrtx/RRTx.cpp +++ b/src/planners/rrtx/RRTx.cpp @@ -37,20 +37,39 @@ planning::rrtx::RRTx::RRTx(const std::shared_ptr ss_, const st planner_info->setNumIterations(0); planner_info->setNumStates(1); + + switch (RRTxConfig::TRAJECTORY_INTERPOLATION) + { + case planning::TrajectoryInterpolation::None: + traj = nullptr; + break; + + case planning::TrajectoryInterpolation::Spline: + traj = std::make_shared + ( + ss, + planning::trajectory::State(q_current->getCoord()), + RRTxConfig::MAX_ITER_TIME + ); + break; + + case planning::TrajectoryInterpolation::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); + (ss, RRTxConfig::TRAJECTORY_INTERPOLATION, RRTxConfig::MAX_ITER_TIME); + updating_state->setMeasureTime(false); + updating_state->setTrajectory(traj); motion_validity = std::make_shared - (ss, RRTxConfig::TRAJECTORY_INTERPOLATION, RRTxConfig::RESOLUTION_COLL_CHECK, &path, RRTxConfig::MAX_ITER_TIME); - - splines = 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); - } + (ss, RRTxConfig::RESOLUTION_COLL_CHECK, RRTxConfig::MAX_ITER_TIME, &path); RRTConnectConfig::EPS_STEP = RRTxConfig::EPS_STEP; @@ -228,7 +247,19 @@ 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; + + default: + is_valid = motion_validity->check(traj->getTrajPointCurrentIter()); + break; + } + + if (!is_valid) { std::cout << "*************** Collision has been occurred!!! *************** \n"; planner_info->setSuccessState(false); diff --git a/src/planners/trajectory/AbstractTrajectory.cpp b/src/planners/trajectory/AbstractTrajectory.cpp new file mode 100644 index 00000000..c62c0fcf --- /dev/null +++ b/src/planners/trajectory/AbstractTrajectory.cpp @@ -0,0 +1,143 @@ +#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_; + max_iter_time = 0; + max_remaining_iter_time = INFINITY; + max_obs_vel = 0; + + setParams(); +} + +planning::trajectory::AbstractTrajectory::AbstractTrajectory(const std::shared_ptr &ss_, float max_iter_time_) +{ + ss = ss_; + 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 << getPosition(time_current).transpose() << "\n"; + output_file << getPosition(time_final).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"; +} diff --git a/src/planners/trajectory/CompositeSpline.cpp b/src/planners/trajectory/CompositeSpline.cpp index ba8eaee3..929f4e73 100644 --- a/src/planners/trajectory/CompositeSpline.cpp +++ b/src/planners/trajectory/CompositeSpline.cpp @@ -3,17 +3,15 @@ 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(); float time_final_sum { 0 }; for (size_t i = 0; i < subsplines.size(); i++) + { time_final_sum += subsplines[i]->getTimeFinal(); + times_connecting.emplace_back(time_final_sum); + } time_final = time_final_sum; } @@ -21,13 +19,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 +31,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); } diff --git a/src/planners/trajectory/MotionValidity.cpp b/src/planners/trajectory/MotionValidity.cpp index f6ec4e99..83d7c73e 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_; - splines = 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 'splines->spline_current' and 'splines->spline_next' during the current iteration. -bool planning::trajectory::MotionValidity::check_v2() +/// @brief In case traj_interpolation == "Spline" or "Ruckig", 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((splines->spline_current->getTimeCurrent() - splines->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 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::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 = splines->spline_current->getTimeBegin() + num_check * delta_time1; - q_temp = ss->getNewState(splines->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)); - // 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 diff --git a/src/planners/trajectory/Spline.cpp b/src/planners/trajectory/Spline.cpp index 0331ae18..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_ptr 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)) @@ -65,17 +52,17 @@ int planning::trajectory::Spline::checkPositionMonotonicity() Eigen::VectorXf planning::trajectory::Spline::getPosition(float t) { - Eigen::VectorXf q { Eigen::VectorXf::Zero(robot->getNumDOFs()) }; + 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 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/Splines.cpp b/src/planners/trajectory/Splines.cpp deleted file mode 100644 index 84dccf93..00000000 --- a/src/planners/trajectory/Splines.cpp +++ /dev/null @@ -1,377 +0,0 @@ -#include "Splines.h" - -planning::trajectory::Splines::Splines(const std::shared_ptr &ss_, - const std::shared_ptr &q_current_, float 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; - - 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) / SplinesConfig::FINAL_VELOCITY_STEP)) : - std::ceil(std::log2(2 * ss->robot->getMaxVel().maxCoeff() / SplinesConfig::FINAL_VELOCITY_STEP)); -} - -/// @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 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::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) -{ - 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 }; - Eigen::VectorXf q_final_dot_min { Eigen::VectorXf::Zero(ss->num_dimensions) }; - Eigen::VectorXf q_final_dot {}; - - 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; - } - 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); - } - - // 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()) - { - spline_computed = spline_next_new->compute(q_target->getCoord()); - if (spline_computed) - { - spline_next = spline_next_new; - // 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; -} - -/// @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 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) -{ - 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 }; - - float rho_robot {}; - float rho_obs {}; - bool is_safe {}; - bool spline_computed { false }; - bool spline_emergency_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)); - 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() }; - - auto computeRho = [&](Eigen::VectorXf q_coord) -> float - { - float rho { 0 }; - std::shared_ptr skeleton { ss->robot->computeSkeleton(ss->getNewState(q_coord)) }; - for (size_t k = 1; k <= ss->robot->getNumLinks(); k++) - rho = std::max(rho, (q_current->getSkeleton()->col(k) - skeleton->col(k)).norm()); - - return rho; - }; - - while (num_iter++ < max_num_iter && - std::chrono::duration_cast(std::chrono::steady_clock::now() - time_start_).count() < t_max * 1e6) - { - is_safe = false; - // std::cout << "Num. iter. " << num_iter << "\n"; - // std::cout << "q_final: " << q_final.transpose() << "\n"; - - if (spline_next_temp->compute(q_final)) - { - // std::cout << "Spline is computed! \n"; - if (spline_next_temp->getTimeFinal() < t_spline_max) - { - rho_obs = max_obs_vel * (t_iter + spline_next_temp->getTimeFinal()); - if (rho_obs < q_current->getDistance()) - { - rho_robot = computeRho(q_final); - if (rho_obs + rho_robot < q_current->getDistance()) - { - spline_emergency_computed = false; - is_safe = true; - } - } - } - else - { - spline_emergency_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) - ); - - if (spline_emergency_temp->compute()) - { - // std::cout << "Emergency spline is computed! \n"; - rho_obs = max_obs_vel * (t_iter + t_spline_max + spline_emergency_temp->getTimeFinal()); - if (rho_obs < q_current->getDistance()) - { - rho_robot = computeRho(spline_emergency_temp->getPosition(INFINITY)); - if (rho_obs + rho_robot < q_current->getDistance()) - { - *spline_emergency_new = *spline_emergency_temp; - spline_emergency_computed = true; - is_safe = true; - spline_next_temp->setTimeFinal(t_spline_max); - } - } - } - } - } - - // std::cout << "\trho_obs: " << rho_obs << " [m]\t"; - // std::cout << "rho_robot: " << rho_robot << " [m]\t"; - // std::cout << "rho_sum: " << rho_obs + rho_robot << " [m]\t"; - // std::cout << "d_c: " << q_current->getDistance() << " [m]\n"; - // rho_obs = 0; rho_robot = 0; // Reset only for console output - - if (is_safe) - { - // std::cout << "\tRobot is safe! \n"; - *spline_next_new = *spline_next_temp; - spline_computed = true; - q_final_min = q_final; - if (num_iter == 1) - break; - } - else - { - // std::cout << "\tRobot is NOT safe! \n"; - q_final_max = q_final; - } - q_final = (q_final_min + q_final_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::cout << "spline_next: \n" << spline_next << "\n"; - spline_computed = !checkCollision(q_current, t_iter); - } - - 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::Splines::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 }; - size_t num_iter = std::ceil(spline_next->getTimeFinal() / delta_t); - delta_t = spline_next->getTimeFinal() / num_iter; - float rho_robot {}; - float rho_obs {}; - float t_init { 0 }; - 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) - { - // std::cout << "Considering t: " << t << "\n"; - q_final = ss->getNewState(spline_next->getPosition(t)); - if (ss->robot->checkSelfCollision(q_final)) - return true; - - t_iter += delta_t; - rho_obs = max_obs_vel * (t_iter - t_init); - delta_q = (q_final->getCoord() - q_init->getCoord()).cwiseAbs(); - 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 - { - // 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) - { - // std::cout << "\t Spline is NOT guaranteed collision-free! \n"; - return true; - } - break; - } - // else std::cout << "\t OK! rho_robot + rho_obs < q_init->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; -} - -/// @brief Compute an underestimation of distance-to-obstacles 'd_c', i.e., a distance-to-planes, for each robot's link, -/// i.e., compute a distance-to-planes profile function, when the robot takes a configuration 'q'. -/// Planes approximate obstacles, and are generated according to 'nearest_points'. -/// Each plane is moved at the maximal obstacle velocity 'ss->env->getObject(j)->getMaxVel()' towards the robot -/// during the time interval 'delta_t'. -/// @param q Configuration of the robot. -/// @param nearest_points Nearest points between the robot and obstacles. -/// @param delta_t Time interval when planes are moving towards the robot. -/// @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, - const std::shared_ptr> nearest_points, float delta_t) -{ - float d_c_temp {}; - float d_c { INFINITY }; - std::vector d_c_profile(ss->robot->getNumLinks(), 0); - Eigen::Vector3f R {}; // Robot's nearest point - Eigen::Vector3f O {}; // Obstacle's nearest point - Eigen::Vector3f delta_RO {}; - std::shared_ptr skeleton { ss->robot->computeSkeleton(q) }; - - for (size_t i = 0; i < ss->robot->getNumLinks(); i++) - { - d_c_profile[i] = INFINITY; - for (size_t j = 0; j < ss->env->getNumObjects(); j++) - { - O = nearest_points->at(j).col(i).tail(3); - if (O.norm() == INFINITY) - continue; - - R = nearest_points->at(j).col(i).head(3); - - // Move both points, R and O, to maintain a normal plane vector 'R - O' - delta_RO = (R - O).normalized() * ss->env->getObject(j)->getMaxVel() * delta_t; - R += delta_RO; - O += delta_RO; - - d_c_temp = std::min((skeleton->col(i) - O).dot((R - O).normalized()), - (skeleton->col(i+1) - O).dot((R - O).normalized())) - - ss->robot->getCapsuleRadius(i); - if (d_c_temp < 0) - return 0; - - d_c_profile[i] = std::min(d_c_profile[i], d_c_temp); - - // std::cout << "(i, j) = " << "(" << i << ", " << j << "):" << std::endl; - // std::cout << "Robot nearest point: " << R.transpose() << std::endl; - // std::cout << "Obstacle nearest point: " << O.transpose() << std::endl; - // std::cout << "d_c: " << d_c_profile[i] << std::endl; - } - d_c = std::min(d_c, d_c_profile[i]); - } - - q->setDistance(d_c); - q->setDistanceProfile(d_c_profile); - q->setIsRealDistance(false); - - return d_c; -} - -// This function is just for debugging. You can set a desired path for the file to be saved. -void planning::trajectory::Splines::recordTrajectory(bool spline_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 << "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"; -} diff --git a/src/planners/trajectory/Trajectory.cpp b/src/planners/trajectory/Trajectory.cpp new file mode 100644 index 00000000..b65d465b --- /dev/null +++ b/src/planners/trajectory/Trajectory.cpp @@ -0,0 +1,627 @@ +#include "Trajectory.h" + +planning::trajectory::Trajectory::Trajectory(const std::shared_ptr &ss_) : + AbstractTrajectory(ss_) +{ + spline = nullptr; +} + +planning::trajectory::Trajectory::Trajectory + (const std::shared_ptr &ss_, planning::trajectory::State current, float max_iter_time_) : + AbstractTrajectory(ss_, max_iter_time_) +{ + spline = std::make_shared(ss->robot, current.pos, current.vel, current.acc); +} + +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 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(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 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 { ((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_trajectory && t_remain > 0) + { + if (spline_new->compute(target.pos, target_vel)) + { + spline_computed = true; + setSpline(spline_new); + } + else + target_vel_max = target_vel; + + 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"; + } + } + + // Possible current position at the end of iteration + 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 - target.pos).norm() > (current.pos - target.pos).norm()) && t_remain > 0) + { + is_zero_final_vel = true; + spline_computed = spline_new->compute(target.pos); + if (spline_computed) + 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 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(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_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_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 - target.pos).norm() / RRTConnectConfig::EPS_STEP)); + if (max_num_iter <= 0) max_num_iter = 1; + 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 + { + float rho { 0 }; + std::shared_ptr skeleton { ss->robot->computeSkeleton(ss->getNewState(q_coord)) }; + for (size_t k = 1; k <= ss->robot->getNumLinks(); k++) + rho = std::max(rho, (q_current->getSkeleton()->col(k) - skeleton->col(k)).norm()); + + return rho; + }; + + while (num_iter++ < max_num_iter && + std::chrono::duration_cast(std::chrono::steady_clock::now() - time_start_).count() < t_max * 1e6) + { + is_safe = false; + // std::cout << "Num. iter. " << num_iter << "\n"; + // std::cout << "target_pos: " << target_pos.transpose() << "\n"; + + if (spline_temp->compute(target_pos)) + { + // std::cout << "Spline is computed! \n"; + if (spline_temp->getTimeFinal() < t_spline_max) + { + rho_obs = max_obs_vel * (t_iter + spline_temp->getTimeFinal()); + if (rho_obs < q_current->getDistance()) + { + rho_robot = computeRho(target_pos); + if (rho_obs + rho_robot < q_current->getDistance()) + { + spline_emg_computed = false; + is_safe = true; + } + } + } + else + { + spline_emg_temp = std::make_shared + ( + ss->robot, + spline_temp->getPosition(t_spline_max), + spline_temp->getVelocity(t_spline_max), + spline_temp->getAcceleration(t_spline_max) + ); + + if (spline_emg_temp->compute()) + { + // std::cout << "Emergency spline is computed! \n"; + rho_obs = max_obs_vel * (t_iter + t_spline_max + spline_emg_temp->getTimeFinal()); + if (rho_obs < q_current->getDistance()) + { + rho_robot = computeRho(spline_emg_temp->getPosition(INFINITY)); + if (rho_obs + rho_robot < q_current->getDistance()) + { + *spline_emg_new = *spline_emg_temp; + spline_emg_computed = true; + is_safe = true; + spline_temp->setTimeFinal(t_spline_max); + } + } + } + } + } + + // std::cout << "\trho_obs: " << rho_obs << " [m]\t"; + // std::cout << "rho_robot: " << rho_robot << " [m]\t"; + // std::cout << "rho_sum: " << rho_obs + rho_robot << " [m]\t"; + // std::cout << "d_c: " << q_current->getDistance() << " [m]\n"; + // rho_obs = 0; rho_robot = 0; // Reset only for console output + + if (is_safe) + { + // std::cout << "\tRobot is safe! \n"; + *spline_new = *spline_temp; + spline_computed = true; + target_pos_min = target_pos; + if (num_iter == 1) + break; + } + else + { + // std::cout << "\tRobot is NOT safe! \n"; + target_pos_max = target_pos; + } + target_pos = (target_pos_min + target_pos_max) / 2; + } + + if (spline_computed) // Check whether computed splines are collision-free + { + 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); + + if (spline_computed) + setSpline(spline_safe); + } + + return spline_computed; +} + +/// @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_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_safe->getTimeFinal() + RealVectorSpaceConfig::EQUALITY_THRESHOLD; t += delta_t) + { + // std::cout << "Considering t: " << t << "\n"; + q_final = ss->getNewState(spline_safe->getPosition(t)); + if (ss->robot->checkSelfCollision(q_final)) + return false; + + t_iter += delta_t; + rho_obs = max_obs_vel * (t_iter - t_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_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_temp = q_final; + computeDistanceUnderestimation(q_temp, q_current->getNearestPoints(), t_iter); + t_init = t_iter; + + if (q_temp->getDistance() <= 0) + { + // std::cout << "\t Trajectory is NOT safe! \n"; + return false; + } + break; + } + // 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 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, +/// i.e., compute a distance-to-planes profile function, when the robot takes a configuration 'q'. +/// Planes approximate obstacles, and are generated according to 'nearest_points'. +/// Each plane is moved at the maximal obstacle velocity 'ss->env->getObject(j)->getMaxVel()' towards the robot +/// during the time interval 'delta_t'. +/// @param q Configuration of the robot. +/// @param nearest_points Nearest points between the robot and obstacles. +/// @param delta_t Time interval when planes are moving towards the robot. +/// @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::Trajectory::computeDistanceUnderestimation(const std::shared_ptr q, + const std::shared_ptr> nearest_points, float delta_t) +{ + float d_c_temp {}; + float d_c { INFINITY }; + std::vector d_c_profile(ss->robot->getNumLinks(), 0); + Eigen::Vector3f R {}; // Robot's nearest point + Eigen::Vector3f O {}; // Obstacle's nearest point + Eigen::Vector3f delta_RO {}; + std::shared_ptr skeleton { ss->robot->computeSkeleton(q) }; + + for (size_t i = 0; i < ss->robot->getNumLinks(); i++) + { + d_c_profile[i] = INFINITY; + for (size_t j = 0; j < ss->env->getNumObjects(); j++) + { + O = nearest_points->at(j).col(i).tail(3); + if (O.norm() == INFINITY) + continue; + + R = nearest_points->at(j).col(i).head(3); + + // Move both points, R and O, to maintain a normal plane vector 'R - O' + delta_RO = (R - O).normalized() * ss->env->getObject(j)->getMaxVel() * delta_t; + R += delta_RO; + O += delta_RO; + + d_c_temp = std::min((skeleton->col(i) - O).dot((R - O).normalized()), + (skeleton->col(i+1) - O).dot((R - O).normalized())) + - ss->robot->getCapsuleRadius(i); + if (d_c_temp < 0) + return 0; + + d_c_profile[i] = std::min(d_c_profile[i], d_c_temp); + + // std::cout << "(i, j) = " << "(" << i << ", " << j << "):" << std::endl; + // std::cout << "Robot nearest point: " << R.transpose() << std::endl; + // std::cout << "Obstacle nearest point: " << O.transpose() << std::endl; + // std::cout << "d_c: " << d_c_profile[i] << std::endl; + } + d_c = std::min(d_c, d_c_profile[i]); + } + + q->setDistance(d_c); + q->setDistanceProfile(d_c_profile); + q->setIsRealDistance(false); + + return d_c; +} + +/// @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::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->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 'isSafe' function + if (q_current->getDistance() <= 0 || !isSafe(spline_next, q_current, 0)) + spline_computed = false; + } + } + + spline_current->setTimeFinal(t); + subsplines.emplace_back(spline_current); + spline_current = spline_next; + } + + subsplines.emplace_back(spline_current); + setSpline(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::Trajectory::path2traj_v2(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 q_current { nullptr }; + + 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; + // 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_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 + ); + + if (spline_computed && num < max_num_iter) + { + q_current = ss->getNewState(spline_current->getPosition(t)); + 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; + } + + subsplines.emplace_back(spline_current); + setSpline(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::Trajectory::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 target_vel_max {}; + Eigen::VectorXf target_vel_min {}; + Eigen::VectorXf target_vel {}; + Eigen::VectorXf target_pos {}; + 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) + target_pos = (path[i-1]->getCoord() + path[i]->getCoord()) / 2; + else + target_pos = path[i]->getCoord(); + + spline_computed = false; + num_iter = 0; + 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 + { + 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()), + subsplines[i-1]->getAcceleration(subsplines[i-1]->getTimeFinal()) + ) + }; + + if (spline_new->compute(target_pos, target_vel) && + ((monotonic && spline_new->checkPositionMonotonicity() != 0) || !monotonic)) + { + *subsplines[i] = *spline_new; + target_vel_min = target_vel; + spline_computed = true; + } + else + target_vel_max = target_vel; + } + while (++num_iter < max_num_iter); + + if (!spline_computed) + { + spline_computed = subsplines[i]->compute(target_pos) && + ((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) + setSpline(std::make_shared(subsplines)); + else + { + std::cout << "Could not convert path to a trajectory! \n"; + // path2traj_v1(path); // Add path using another method. + } +} + +void planning::trajectory::Trajectory::setSpline(const std::shared_ptr spline_) +{ + spline = spline_; + time_current = 0; + time_final = spline->getTimeFinal(); +} diff --git a/src/planners/trajectory/TrajectoryRuckig.cpp b/src/planners/trajectory/TrajectoryRuckig.cpp new file mode 100644 index 00000000..75b0b954 --- /dev/null +++ b/src/planners/trajectory/TrajectoryRuckig.cpp @@ -0,0 +1,194 @@ +#include "TrajectoryRuckig.h" + +planning::trajectory::TrajectoryRuckig::TrajectoryRuckig(const std::shared_ptr &ss_) : + AbstractTrajectory(ss_), + input(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) +{ + setCurrentState(current); + + for (size_t i = 0; i < ss->num_dimensions; i++) + { + 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; + + if (!input.validate()) + 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 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(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); + 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 { ((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) + { + target.vel = target_vel; + setTargetState(target); + result = otg.calculate(input, traj_new); + + if (result == ruckig::Result::Working || result == ruckig::Result::Finished) + { + traj = traj_new; + time_current = 0; + time_final = traj.get_duration(); + traj_computed = true; + } + else + target_vel_max = target_vel; + + 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"; + } + } + + // 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 - target.pos).norm() > (current.pos - target.pos).norm()) && t_remain > 0) + { + is_zero_final_vel = true; + 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_new; + time_current = 0; + time_final = traj.get_duration(); + traj_computed = true; + } + } + + // std::cout << "\t Trajectory " << (!traj_computed ? "NOT " : "") << "computed with " + // << (!is_zero_final_vel ? "NON-" : "") << "ZERO final velocity. \n"; + return traj_computed; +} + +/// @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([[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; +} + +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); + } +} + +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); + } +} + +Eigen::VectorXf planning::trajectory::TrajectoryRuckig::getPosition(float t) +{ + 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) +{ + 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); + + 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) +{ + 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]; + + return ret; +} diff --git a/src/planners/trajectory/UpdatingState.cpp b/src/planners/trajectory/UpdatingState.cpp index 6b905407..deed5734 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 &q update_v1(q_previous, q_current, q_next_, q_next_, status); break; - case planning::TrajectoryInterpolation::Spline: - update_v2(q_previous, q_current, q_next_, q_next_, status); - break; - default: + update_v2(q_previous, q_current, q_next_, q_next_, status); break; } } @@ -57,11 +54,8 @@ 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: - update_v2(q_previous, q_current, q_next_, q_next_reached_, status); - break; - default: + update_v2(q_previous, q_current, q_next_, q_next_reached_, status); break; } } @@ -117,11 +111,10 @@ 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 '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 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 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, @@ -130,100 +123,133 @@ 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; - 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_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 ? - splines->spline_current->getTimeCurrent(true) + t_spline_max : - splines->spline_current->getTimeEnd() + t_iter + t_spline_max }; - - splines->spline_current->setTimeBegin(splines->spline_current->getTimeEnd()); - splines->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 << "Curr. spline time: " << t_spline_current * 1000 << " [ms] \n"; + float t_iter_remain { max_iter_time - 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->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->getTimeBegin() * 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->clearTrajPointCurrentIter(); + size_t num_checks1 = std::floor((traj->getTimeCurrent() - traj->getTimeBegin()) / + max_iter_time * TrajectoryConfig::NUM_VALIDITY_POINTS_CHECK ); + 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->getTimeBegin() + num_check * delta_time1; + traj->addTrajPointCurrentIter(traj->getPosition(t)); + // std::cout << "t: " << t * 1000 << " [ms]\t" + // << "pos: " << traj->getPosition(t).transpose() << "\t" + // << "vel: " << traj->getVelocity(t).transpose() << "\t" + // << "acc: " << traj->getAcceleration(t).transpose() << "\n"; + } // ----------------------------------------------------------------------------------------- // - bool spline_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) }; - - // std::cout << "Curr. pos: " << current_pos.transpose() << "\n"; - // std::cout << "Curr. vel: " << current_vel.transpose() << "\n"; - // std::cout << "Curr. acc: " << current_acc.transpose() << "\n"; + bool traj_computed { false }; + float t_traj_remain {}; + 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 { - 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, - 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)) }; + 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(); - splines->setCurrentState(q_current); - splines->setTargetState(q_target); - // 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 (guaranteed_safe_motion) - spline_computed = splines->computeSafe(current_pos, current_vel, current_acc, t_iter_remain, t_spline_remain); + traj_computed = traj->computeSafe(current, target, t_iter_remain, t_traj_remain, q_current); else { - if (splines->spline_current->isFinalConf(q_target->getCoord())) // Spline to such 'q_target' already exists! + if (traj->isFinalConf(target.pos)) // Spline to such 'target.pos' 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, target, t_iter_remain, t_traj_remain, non_zero_final_vel); } } - while (!spline_computed && invokeChangeNextState()); - // std::cout << "Elapsed time for spline computing: " << (getElapsedTime() - t_iter) * 1e3 << " [ms] \n"; + while (!traj_computed && invokeChangeNextState()); - if (spline_computed) - { - // std::cout << "New spline is computed! \n"; - splines->spline_current->setTimeEnd(t_spline_current); - splines->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); - } - - // splines->recordTrajectory(spline_computed); // Only for debugging + 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(splines->spline_next->getPosition(splines->spline_next->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' - splines->spline_next->getVelocity(splines->spline_next->getTimeEnd()).norm() / ss->robot->getMaxVel().norm() * SplinesConfig::MAX_RADIUS) + if (ss->getNorm(q_current, q_next) <= // 'q_next' must be reached within the computed radius, and not only 'q_next_reached' + 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 << "Spline next: \n" << splines->spline_next << "\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->getTimeEnd() - traj->getTimeCurrent()) / num_checks2 }; + for (size_t num_check = 1; num_check <= num_checks2; num_check++) + { + t = traj->getTimeCurrent() + num_check * delta_time2; + traj->addTrajPointCurrentIter(traj->getPosition(t)); + // std::cout << "t: " << t * 1000 << " [ms]\t" + // << "pos: " << traj->getPosition(t).transpose() << "\t" + // << "vel: " << traj->getVelocity(t).transpose() << "\t" + // << "acc: " << traj->getAcceleration(t).transpose() << "\n"; + } + // ----------------------------------------------------------------------------------------- // - remaining_time = t_spline_max + SplinesConfig::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' diff --git a/src/state_spaces/real_vector_space/RealVectorSpace.cpp b/src/state_spaces/real_vector_space/RealVectorSpace.cpp index 7d6c22c5..165351b9 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. @@ -199,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++) @@ -210,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()); @@ -234,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"; 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)