Add rudimentary stl support (#64)

Author: sjahr

Diff for: README.md

@@ -105,3 +105,18 @@ To rebuild only the `moveit_drake` package:
 rm -rf build/moveit_drake install/moveit_drake
 colcon build --packages-select moveit_drake
 ```
+
+## Known issues
+
+### .stl support
+
+Unfortunately, Drake does not support `.stl` files (11/28/2024, see [drake#19408](https://github.com/RobotLocomotion/drake/issues/19408)).
+We're working around this by replacing the `.stl` files in the urdf string with `.obj` files in the plugin implementations.
+Make sure that the moveit config you're using contains the relevant `.stl` files.
+If it doesn't, take a look into the scripts/ directory.
+We've provided a simple python script to add additional `.obj` files for given `.stl` files. Usage:
+
+```
+./scripts/convert_stl_to_obj.py /PATH/TO/YOUR/MESH/DIR
+```
+Don't forget to rebuild your description package so the `.obj` files are copied into the workspace's install directory.

Diff for: include/ktopt_interface/ktopt_planning_context.hpp

@@ -103,9 +103,6 @@ class KTOptPlanningContext : public planning_interface::PlanningContext
   /// @brief The ROS parameters associated with this motion planner.
   const ktopt_interface::Params params_;
 
-  /// @brief The URDF robot description.
-  std::string robot_description_;
-
   /// @brief The Drake diagram describing the entire system.
   std::unique_ptr<Diagram<double>> diagram_;
 

Diff for: include/moveit/drake/conversions.hpp

@@ -147,4 +147,12 @@ getPiecewisePolynomial(const ::robot_trajectory::RobotTrajectory& robot_trajecto
 void getRobotTrajectory(const ::drake::trajectories::Trajectory<double>& drake_trajectory, const double delta_t,
                         const ::drake::multibody::MultibodyPlant<double>& plant,
                         std::shared_ptr<::robot_trajectory::RobotTrajectory>& moveit_trajectory);
+
+/**
+ * @brief Converts all STL file paths in a URDF string to OBJ file paths
+ *
+ * @param input Input robot description
+ * @return std::string Robot description with all STL file paths replaced by OBJ file paths
+ */
+[[nodiscard]] std::string replaceSTLWithOBJ(const std::string& input);
 }  // namespace moveit::drake

Diff for: moveit_drake.repos

@@ -13,8 +13,8 @@ repositories:
     version: main
   moveit_resources:
     type: git
-    url: https://github.com/moveit/moveit_resources
-    version: ros2
+    url: https://github.com/sjahr/moveit_resources
+    version: moveit_drake
   moveit_msgs:
     type: git
     url: https://github.com/moveit/moveit_msgs

Diff for: scripts/convert_stl_to_obj.py

@@ -0,0 +1,49 @@
+#!/usr/bin/env python3
+
+import os
+import trimesh
+import argparse
+
+
+def convert_stl_to_obj(directory):
+    """
+    Finds all STL files in the given directory and subdirectories, and converts each to an OBJ file.
+
+    Args:
+        directory (str): The path to the directory to search.
+    """
+    for root, _, files in os.walk(directory):
+        for file in files:
+            stl_path = os.path.join(root, file)
+            file_root, file_ext = os.path.splitext(stl_path)
+
+            if file_ext.lower() == ".stl":  # Check if the file is an STL file
+                obj_path = file_root + ".obj"  # Create the OBJ file path
+
+                try:
+                    # Load the STL file
+                    mesh = trimesh.load(stl_path)
+
+                    # Export the mesh as OBJ
+                    mesh.export(obj_path)
+
+                    print(f"Converted: {stl_path} -> {obj_path}")
+                except Exception as e:
+                    print(f"Failed to convert {stl_path}: {e}")
+
+
+def main():
+    parser = argparse.ArgumentParser(description="Convert STL files to OBJ format.")
+    parser.add_argument(
+        "directory", type=str, help="The directory to search for STL files."
+    )
+    args = parser.parse_args()
+
+    if os.path.isdir(args.directory):
+        convert_stl_to_obj(args.directory)
+    else:
+        print("The provided path is not a directory. Please check and try again.")
+
+
+if __name__ == "__main__":
+    main()

Diff for: src/conversions.cpp

@@ -237,4 +237,28 @@ void getRobotTrajectory(const ::drake::trajectories::Trajectory<double>& drake_t
     t_prev = t;
   }
 }
+
+std::string replaceSTLWithOBJ(const std::string& input)
+{
+  std::string result = input;
+  const std::string lower_case_stl = ".stl";
+  const std::string upper_case_stl = ".STL";
+  const std::string replacement = ".obj";
+
+  size_t pos = 0;
+  while ((pos = result.find(lower_case_stl, pos)) != std::string::npos)
+  {
+    result.replace(pos, lower_case_stl.length(), replacement);
+    pos += replacement.length();  // Move past the replacement to avoid infinite loop
+  }
+
+  pos = 0;
+  while ((pos = result.find(upper_case_stl, pos)) != std::string::npos)
+  {
+    result.replace(pos, upper_case_stl.length(), replacement);
+    pos += replacement.length();  // Move past the replacement to avoid infinite loop
+  }
+
+  return result;
+}
 }  // namespace moveit::drake

Diff for: src/ktopt_planning_context.cpp

@@ -1,4 +1,6 @@
 #include <cmath>
+#include <iostream>
+#include <string>
 
 #include <drake/geometry/meshcat_params.h>
 #include <drake/geometry/geometry_frame.h>
@@ -358,8 +360,6 @@ bool KTOptPlanningContext::terminate()
 
 void KTOptPlanningContext::setRobotDescription(const std::string& robot_description)
 {
-  robot_description_ = robot_description;
-
   // also perform some drake related initialisations here
   builder = std::make_unique<DiagramBuilder<double>>();
 
@@ -369,13 +369,11 @@ void KTOptPlanningContext::setRobotDescription(const std::string& robot_descript
 
   auto [plant, scene_graph] = drake::multibody::AddMultibodyPlantSceneGraph(builder.get(), 0.0);
 
-  // TODO:(kamiradi) Figure out object parsing
-  // auto robot_instance = Parser(plant_,
-  // scene_graph_).AddModelsFromString(robot_description_, ".urdf");
+  // Drake cannot handle stl files, so we convert them to obj. Make sure these files are available in your moveit config!
+  const auto description_with_obj = moveit::drake::replaceSTLWithOBJ(robot_description);
+  auto robot_instance =
+      drake::multibody::Parser(&plant, &scene_graph).AddModelsFromString(description_with_obj, ".urdf");
 
-  const char* ModelUrl = params_.drake_robot_description.c_str();
-  const std::string urdf = drake::multibody::PackageMap{}.ResolveUrl(ModelUrl);
-  auto robot_instance = drake::multibody::Parser(&plant, &scene_graph).AddModels(urdf);
   plant.WeldFrames(plant.world_frame(), plant.GetFrameByName(params_.base_frame));
 
   // planning scene transcription