import v0.3

.bazelrc

@@ -0,0 +1,2 @@
+build --distdir=./bazel_build
+build --cxxopt=-std=c++17

.gitignore

@@ -7,6 +7,7 @@ src/examples/scripts/env/*
 src/examples/scripts/__pycache__/*
 *.tum
 *cache
+data/
 docs/
 
 *~

BUILD.bazel

@@ -0,0 +1,127 @@
+# LICENSE
+# Bazel Build for KIT Real-Time Time-Optimal Path Parameterization (RTTOPP)
+
+
+package(default_visibility = ["//visibility:public"])
+
+
+# https://docs.bazel.build/versions/master/be/c-cpp.html#cc_library
+cc_library(
+    name = "rttopp",
+    srcs = glob([
+        "src/demo_trajectories.cpp",
+        "include/rttopp/*.h"
+    ]),
+    includes = ["include"],
+    hdrs = ["include/rttopp/rttopp2.h"], # Public interface of the library
+    deps = ["@json//:nlohmann_json",
+            "@com_gitlab_libeigen_eigen//:eigen"
+    ],
+    copts = []
+)
+
+cc_library(
+    name = "param_random_waypoints_example_generic",
+    srcs = glob([
+        "examples/param_random_waypoints_generic.cpp",
+        "include/rttopp/*.h"
+    ]),
+    includes = ["include"],
+    hdrs = ["include/rttopp/rttopp2.h"], # Public interface of the library
+    deps = ["@json//:nlohmann_json",
+            "@com_gitlab_libeigen_eigen//:eigen"
+    ],
+    copts = []
+)
+
+cc_library(
+    name = "param_random_waypoints_iiwa",
+    srcs = glob([
+        "examples/param_random_waypoints_iiwa.cpp",
+        "include/rttopp/*.h"
+    ]),
+    includes = ["include"],
+    hdrs = ["include/rttopp/rttopp2.h"], # Public interface of the library
+    deps = ["@json//:nlohmann_json",
+            "@com_gitlab_libeigen_eigen//:eigen"
+    ],
+    copts = []
+)
+
+cc_library(
+    name = "param_random_waypoints_generic_sampling_example",
+    srcs = glob([
+        "examples/param_random_waypoints_generic_sampling_example.cpp",
+        "include/rttopp/*.h"
+    ]),
+    includes = ["include"],
+    hdrs = ["include/rttopp/rttopp2.h"], # Public interface of the library
+    deps = ["@json//:nlohmann_json",
+            "@com_gitlab_libeigen_eigen//:eigen"
+    ],
+    copts = []
+)
+
+cc_library(
+    name = "param_failed_random_waypoints_example_generic",
+    srcs = glob([
+        "examples/param_failed_random_waypoints_example_generic.cpp",
+        "include/rttopp/*.h"
+    ]),
+    includes = ["include"],
+    hdrs = ["include/rttopp/rttopp2.h"], # Public interface of the library
+    deps = ["@json//:nlohmann_json",
+            "@com_gitlab_libeigen_eigen//:eigen"
+    ],
+    copts = []
+)
+
+
+cc_test(
+    name = "path_velocity_limit_tests",
+    srcs = glob([
+        "tests/path_velocity_limit.cpp",
+        "include/rttopp/*.h"
+    ]),
+    includes = ["include"],
+    deps = [":rttopp",
+            "@json//:nlohmann_json",
+            "@com_gitlab_libeigen_eigen//:eigen",
+            "@com_google_googletest//:gtest",
+            "@com_google_googletest//:gtest_main"
+    ],
+    copts = []
+)
+
+
+cc_test(
+    name = "test_preprocessing",
+    srcs = glob([
+        "tests/test_preprocessing.cpp",
+        "include/rttopp/*.h"
+    ]),
+    includes = ["include"],
+    deps = [":rttopp",
+            "@json//:nlohmann_json",
+            "@com_gitlab_libeigen_eigen//:eigen",
+            "@com_google_googletest//:gtest",
+            "@com_google_googletest//:gtest_main"
+    ],
+    copts = []
+)
+
+cc_test(
+    name = "param_tests",
+    srcs = glob([
+        "tests/param.cpp",
+        "include/rttopp/*.h"
+    ]),
+    includes = ["include"],
+    deps = [":rttopp",
+            "@json//:nlohmann_json",
+            "@com_gitlab_libeigen_eigen//:eigen",
+            "@com_google_googletest//:gtest",
+            "@com_google_googletest//:gtest_main"
+    ],
+    copts = []
+)

CMakeLists.txt

@@ -48,6 +48,14 @@ add_executable(param_random_waypoints_example_iiwa
                examples/param_random_waypoints_iiwa.cpp)
 target_link_libraries(param_random_waypoints_example_iiwa
                       PRIVATE ${PROJECT_NAME})
+add_executable(param_random_waypoints_generic_sampling_example
+               examples/param_random_waypoints_generic_sampling.cpp)
+target_link_libraries(param_random_waypoints_generic_sampling_example
+                      PRIVATE ${PROJECT_NAME})
+add_executable(param_failed_random_waypoints_example_generic
+               examples/param_failed_random_waypoints_generic.cpp)
+target_link_libraries(param_failed_random_waypoints_example_generic
+                      PRIVATE ${PROJECT_NAME} stdc++fs)
 
 add_executable(path_velocity_limit_tests tests/path_velocity_limit.cpp)
 target_link_libraries(path_velocity_limit_tests PRIVATE ${PROJECT_NAME}
@@ -57,3 +65,7 @@ add_test(NAME path_velocity_limit_tests COMMAND path_velocity_limit_tests)
 add_executable(preprocessing_tests tests/test_preprocessing.cpp)
 target_link_libraries(preprocessing_tests PRIVATE ${PROJECT_NAME} gtest_main)
 add_test(NAME preprocessing_tests COMMAND preprocessing_tests)
+
+add_executable(param_tests tests/param.cpp)
+target_link_libraries(param_tests PRIVATE ${PROJECT_NAME} gtest_main)
+add_test(NAME param_tests COMMAND param_tests)

WORKSPACE

@@ -0,0 +1,54 @@
+workspace(name = "rttopp")
+
+load("@bazel_tools//tools/build_defs/repo:http.bzl", "http_archive")
+load("@bazel_tools//tools/build_defs/repo:git.bzl", "git_repository")
+
+
+# Google test
+# Get it from https://github.com/google/benchmark/blob/main/WORKSPACE#L13
+git_repository(
+    name = "com_google_googletest",
+    remote = "https://github.com/google/googletest.git",
+    tag = "release-1.11.0",
+)
+
+git_repository(
+    name = "com_google_benchmark",
+    remote = "https://github.com/google/benchmark.git",
+    tag = "v1.6.1",
+)
+
+http_archive(
+    name = "com_gitlab_libeigen_eigen",
+    sha256 = "0215c6593c4ee9f1f7f28238c4e8995584ebf3b556e9dbf933d84feb98d5b9ef",
+    strip_prefix = "eigen-3.3.8",
+    urls = [
+        "https://gitlab.com/libeigen/eigen/-/archive/3.3.8/eigen-3.3.8.tar.bz2",
+    ],
+    build_file_content =
+"""
+# See https://github.com/ceres-solver/ceres-solver/issues/337.
+cc_library(
+    name = 'eigen',
+    srcs = [],
+    includes = ['.'],
+    hdrs = glob(['Eigen/**',
+                'unsupported/Eigen/**']),
+    visibility = ['//visibility:public'],
+)
+"""
+)
+
+# Boost
+new_local_repository(
+    name = "boost",
+    path = "/usr/include/boost",
+    build_file = "external/boost.BUILD",
+)
+
+# json
+new_local_repository(
+    name = "json",
+    path = "external/json",
+    build_file = "external/json.BUILD",
+)

examples/param_failed_random_waypoints_generic.cpp

@@ -0,0 +1,113 @@
+#include <chrono>
+#include <experimental/filesystem>
+#include <fstream>
+#include <iomanip>
+
+#include <rttopp/demo_trajectories.h>
+#include <rttopp/rttopp2.h>
+
+// This program reads the failed .json file and compute it again for faster
+// development
+constexpr auto N_JOINTS = 6;
+constexpr auto N_TRAJECTORIES = 100;
+
+template <typename EigenVectorType, typename VectorType>
+void fromVectroAsEigenVector(VectorType &v, EigenVectorType &mat) {
+  mat = Eigen::Map<EigenVectorType, Eigen::Unaligned>(v.data(), v.size());
+}
+
+rttopp::Waypoints<N_JOINTS> fromJson(const nlohmann::json &j) {
+  size_t n_waypoints = j["waypoints"].size();
+  rttopp::Waypoints<N_JOINTS> waypoints(n_waypoints);
+
+  for (size_t idx = 0; idx < n_waypoints; ++idx) {
+    auto joint_positions =
+        j["waypoints"][idx]["joints"]["position"].get<std::vector<double>>();
+
+    auto &waypoint = waypoints[idx].joints.position;
+    fromVectroAsEigenVector(joint_positions, waypoint);
+  }
+
+  return waypoints;
+}
+
+int main(int argc, char **argv) {  // NOLINT bugprone-exception-escape
+  if (argc == 1) {
+    std::cout << "Please give the directory to the .json files. Abort.";
+    return EXIT_FAILURE;
+  }
+  // Todo(Xi): what if we want to run a specific file in the directory??
+  rttopp::RTTOPP2<N_JOINTS> topp;
+  Eigen::VectorXd durations(N_TRAJECTORIES), inf_means(N_TRAJECTORIES),
+      inf_std_devs(N_TRAJECTORIES), nums_gridpoints(N_TRAJECTORIES),
+      max_normalized_velocities(N_TRAJECTORIES),
+      max_normalized_accelerations(N_TRAJECTORIES);
+
+  // Read Files
+  size_t i = 0;
+  size_t i_failed = 0;
+  std::string path = argv[1];
+  for (const auto &entry :
+       std::experimental::filesystem::directory_iterator(path)) {
+    std::ifstream ifs(entry.path());
+    nlohmann::json j = nlohmann::json::parse(ifs);
+
+    auto waypoints = fromJson(j);
+
+    // Set constraints
+    rttopp::Constraints<N_JOINTS> constraints;
+    constraints.joints =
+        rttopp::demo_trajectories::generateGenericJointConstraints<N_JOINTS>();
+
+    // set a small velocity at start and end
+    waypoints.front().joints.velocity =
+        0.0 * rttopp::WaypointJointDataType<N_JOINTS>::Ones();
+    waypoints.back().joints.velocity =
+        0.0 * rttopp::WaypointJointDataType<N_JOINTS>::Ones();
+
+    rttopp::Result result = topp.parameterizeFull(constraints, waypoints);
+    if (result.error()) {
+      std::cout << "Error: " << result.message() << std::endl;
+    }
+
+    size_t num_gridpoints;
+    rttopp::Result verification = topp.verifyTrajectory(
+        false, &num_gridpoints, &inf_means[i], &inf_std_devs[i],
+        &max_normalized_velocities[i], &max_normalized_accelerations[i]);
+
+    if (verification.error()) {
+      std::cout << "error in trajectory " << entry.path() << std::endl;
+      std::cout << "-----------------------\n" << std::endl;
+    }
+
+    if (result.error() || verification.error()) {
+      ++i_failed;
+      nlohmann::json j_out = topp.toJson(waypoints);
+
+      // auto new_path = entry.path();
+      // auto file_name = new_path.filename();
+
+      // std::string path_prefix_str = "new_";
+      // std::experimental::filesystem::path new_file_name(path_prefix_str);
+      // new_file_name += file_name;
+      // new_path.replace_filename(new_file_name);
+
+      // std::ofstream of(new_path);
+      std::ofstream of(entry.path());
+
+      // ToDo(Xi): Should we overwrite it?
+      if (!of.is_open()) {
+        std::cout << "Could not write to file: " << entry.path() << std::endl;
+        return 0;
+      }
+
+      of << std::setw(4) << j_out << std::endl;
+    }
+    ++i;
+  }
+
+  std::cout << "Checked " << i << " cases, " << i_failed << " of them failed."
+            << std::endl;
+
+  return 0;
+}

examples/param_random_waypoints_generic.cpp

@@ -5,16 +5,9 @@
 #include <rttopp/demo_trajectories.h>
 #include <rttopp/rttopp2.h>
 
-// TODO(wolfgang): increase this significantly
-// constexpr auto N_TRAJECTORIES = 1000 * 1000;
-// Path 22133 has path positions too close to each other (when using start/goal
-// velocity 1.0 in every axis). When testing with zero start/goal velocity, this
-// error already happens in the first 9000 paths. When using start/goal
-// velocity 1.0 and checking start/goal state for validity, already run 67 has
-// invaid start velocity. when testing with 0.4 start/goal velocity, path 9616
-// has a too high start velocity.
-// TODO(wolfgang): This would be an interesting test case to investigate why the
-// start velocity cannot be reached.
+// When using start/goal velocity 1.0 and checking start/goal state for
+// validity, already run 67 has invaid start velocity. when testing with 0.4
+// start/goal velocity, path 9616 has a too high start velocity.
 constexpr auto N_TRAJECTORIES = 9 * 1000;
 constexpr auto N_WAYPOINTS = 5;
 
@@ -27,7 +20,6 @@ int main(int argc, char** argv) {
       max_normalized_accelerations(N_TRAJECTORIES);
 
   rttopp::Constraints<n_joints> constraints;
-
   constraints.joints =
       rttopp::demo_trajectories::generateGenericJointConstraints<n_joints>();
 
@@ -53,6 +45,7 @@ int main(int argc, char** argv) {
 
     durations[i] =
         std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1).count();
+    // if(i % 10 == 0)
     // std::cout << "topp run " << i << " successfull" << std::endl;
     // std::cout << "computation time " << durations[i]
     //           << " microseconds" << std::endl;
@@ -75,6 +68,7 @@ int main(int argc, char** argv) {
       std::ofstream of(output_path);
 
       if (!of.is_open()) {
+        std::cout << "error in trajectory " << i << std::endl;
         std::cout << "Could not write to file: " << output_path << std::endl;
         return EXIT_FAILURE;
       }