Added ur5 support

.gitignore

@@ -1,7 +1,6 @@
 build
 problems.pkl
 *.out
-*.json
 *.sqlite
 *.nsys-rep
 Pipfile

scripts/evaluate_mbm.cpp

@@ -185,8 +185,10 @@ int main(int argc, char* argv[]) {
         run_planning<robots::Panda>(problems, settings, run_name, robot_name);
     } else if (robot_name == "baxter") {
         run_planning<robots::Baxter>(problems, settings, run_name, robot_name);
+    } else if (robot_name == "ur5") {
+        run_planning<robots::Ur5>(problems, settings, run_name, robot_name);
     } else {
         std::cerr << "Unsupported robot type: " << robot_name << "\n";
         return 1;
     }
-}
+}

scripts/single_mbm.cpp

@@ -144,8 +144,10 @@ int main(int argc, char* argv[]) {
         run_planner<robots::Panda>(data, env, settings);
     } else if (robot_name == "baxter") {
         run_planner<robots::Baxter>(data, env, settings);
+    } else if (robot_name == "ur5") {
+        run_planner<robots::Ur5>(data, env, settings);
     } else {
         std::cerr << "Unsupported robot type: " << robot_name << "\n";
         return 1;
     }
-}
+}

src/planning/Robots.hh

@@ -235,4 +235,50 @@ namespace ppln::robots {
             scale_cfg_impl(q);
         }
     };
+
+
+
+
+
+    struct Ur5{
+
+        static constexpr auto name = "ur5";
+        static constexpr std::size_t dimension = 6;
+        using Configuration = std::array<float, dimension>;
+
+        __device__ static constexpr float get_s_m(int i) {
+            constexpr float values[] = {
+                6.2831854820251465, 6.2831854820251465, 6.2831854820251465, 6.2831854820251465, 6.2831854820251465, 6.2831854820251465
+            };
+            return values[i];
+        }
+
+        __device__ static constexpr float get_s_a(int i) {
+            constexpr float values[] = {
+                -3.1415927410125732, -3.1415927410125732, -3.1415927410125732, -3.1415927410125732, -3.1415927410125732, -3.1415927410125732
+            };
+            return values[i];
+        }
+
+        template<size_t I = 0>
+        __device__ __forceinline__ static void scale_cfg_impl(float *q)
+        {
+            if constexpr (I < dimension) {
+                q[I] = q[I] * get_s_m(I) + get_s_a(I);
+                scale_cfg_impl<I + 1>(q);
+            }
+        }
+
+        __device__ __forceinline__ static void scale_cfg(float *q)
+        {
+            scale_cfg_impl(q);
+        }
+
+        inline static void print_robot_config(Configuration &cfg) {
+            for (int i = 0; i < dimension; i++) {
+                std::cout << cfg[i] << ' ';
+            }
+            std::cout << '\n';
+        };
+    };
 }

src/planning/pRRTC.cu

@@ -6,6 +6,7 @@
 #include "src/robots/panda.cuh"
 #include "src/robots/fetch.cuh"
 #include "src/robots/baxter.cuh"
+#include "src/robots/ur5.cuh"
 
 #include <curand.h>
 #include <curand_kernel.h>
@@ -926,5 +927,5 @@ namespace pRRTC {
     template PlannerResult<typename ppln::robots::Panda> solve<ppln::robots::Panda>(std::array<float, 7>&, std::vector<std::array<float, 7>>&, ppln::collision::Environment<float>&, pRRTC_settings&);
     template PlannerResult<typename ppln::robots::Fetch> solve<ppln::robots::Fetch>(std::array<float, 8>&, std::vector<std::array<float, 8>>&, ppln::collision::Environment<float>&, pRRTC_settings&);
     template PlannerResult<typename ppln::robots::Baxter> solve<ppln::robots::Baxter>(std::array<float, 14>&, std::vector<std::array<float, 14>>&, ppln::collision::Environment<float>&, pRRTC_settings&);
-
+    template PlannerResult<typename ppln::robots::Ur5> solve<ppln::robots::Ur5>(std::array<float, 6>&, std::vector<std::array<float, 6>>&, ppln::collision::Environment<float>&, pRRTC_settings&);
 }