Path to trajectory conversion

.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 \

.gitmodules

@@ -0,0 +1,4 @@
+[submodule "external/ruckig"]
+path = external/ruckig
+url = https://github.com/pantor/ruckig.git
+branch = v0.15.3

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)
 

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=<absolute_path_to_your_build_directory>/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<number>.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_<TRAJECTORY_INTERPOLATION>/results_<num_obstacles>obs_<MAX_ITER_TIME>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```. 

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)

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<float> 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<planning::drbt::DRGBT>(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<std::vector<float>> 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<std::vector<float>> 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);