diff --git a/README.md b/README.md index a117de3..3c57d8e 100644 --- a/README.md +++ b/README.md @@ -105,3 +105,15 @@ 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 +``` diff --git a/include/ktopt_interface/ktopt_planning_context.hpp b/include/ktopt_interface/ktopt_planning_context.hpp index ded3269..0dd564c 100644 --- a/include/ktopt_interface/ktopt_planning_context.hpp +++ b/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_; diff --git a/include/moveit/drake/conversions.hpp b/include/moveit/drake/conversions.hpp index 78547a3..2194819 100644 --- a/include/moveit/drake/conversions.hpp +++ b/include/moveit/drake/conversions.hpp @@ -147,4 +147,12 @@ getPiecewisePolynomial(const ::robot_trajectory::RobotTrajectory& robot_trajecto void getRobotTrajectory(const ::drake::trajectories::Trajectory& drake_trajectory, const double delta_t, const ::drake::multibody::MultibodyPlant& plant, std::shared_ptr<::robot_trajectory::RobotTrajectory>& moveit_trajectory); + +/** + * @brief Converts all STL file paths in a 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 --git a/moveit_drake.repos b/moveit_drake.repos index 95852bc..fa45e16 100644 --- a/moveit_drake.repos +++ b/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 --git a/scripts/convert_stl_to_obj.py b/scripts/convert_stl_to_obj.py new file mode 100755 index 0000000..0e8ba78 --- /dev/null +++ b/scripts/convert_stl_to_obj.py @@ -0,0 +1,47 @@ +#!/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: + if file.lower().endswith(".stl"): + stl_path = os.path.join(root, file) + obj_path = os.path.splitext(stl_path)[0] + ".obj" + + 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 --git a/src/conversions.cpp b/src/conversions.cpp index 9000098..e99a7f8 100644 --- a/src/conversions.cpp +++ b/src/conversions.cpp @@ -237,4 +237,20 @@ void getRobotTrajectory(const ::drake::trajectories::Trajectory& drake_t t_prev = t; } } + +std::string replaceSTLWithOBJ(const std::string& input) +{ + std::string result = input; + const std::string target = ".stl"; + const std::string replacement = ".obj"; + + size_t pos = 0; + while ((pos = result.find(target, pos)) != std::string::npos) + { + result.replace(pos, target.length(), replacement); + pos += replacement.length(); // Move past the replacement to avoid infinite loop + } + + return result; +} } // namespace moveit::drake diff --git a/src/ktopt_planning_context.cpp b/src/ktopt_planning_context.cpp index f421df3..5f7364a 100644 --- a/src/ktopt_planning_context.cpp +++ b/src/ktopt_planning_context.cpp @@ -1,4 +1,6 @@ #include +#include +#include #include #include @@ -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>(); @@ -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