diff --git a/README.md b/README.md index 87319cf..84d73f5 100644 --- a/README.md +++ b/README.md @@ -85,6 +85,24 @@ ros2 launch moveit_drake constrained_planning_demo.launch.py ``` +## Configuration + +### Use another robot + +By default, Drake uses descriptions located within the [drake_models](https://github.com/RobotLocomotion/models) package. Modify the `drake_robot_description` parameter if you want to use a different description: +```yaml +drake_robot_description: "package://drake_models/pr2_description/urdf/pr2_simplified.urdf" +``` + +In case you want to use other robots, you can specify the global path to the description package using the `external_robot_description` parameter: +```yaml +external_robot_description: ["/home/user/xarm_ws/src/xarm_ros2/xarm_description"] +``` +Drake will crawl down the directory tree starting at the given path. You can also specify multiple paths in the array. + +If you already provide the transform `world->base_frame` you might also want to leave the parameter `base_frame` empty. + + ## Development ### Formatting diff --git a/parameters/ktopt_moveit_parameters.yaml b/parameters/ktopt_moveit_parameters.yaml index eb7858a..2266c93 100644 --- a/parameters/ktopt_moveit_parameters.yaml +++ b/parameters/ktopt_moveit_parameters.yaml @@ -4,9 +4,14 @@ ktopt_interface: description: "Robot description to be loaded by the internal Drake MultibodyPlant.", default_value: "package://drake_models/franka_description/urdf/panda_arm_hand.urdf", } + external_robot_description: { + type: string_array, + description: "If your robot description is not available within the drake_models package, you can specify an array of global paths to search for the URDFs.", + default_value: [], + } base_frame: { type: string, - description: "Base frame of the robot that is attached to whatever the robot is mounted on.", + description: "Base frame of the robot that is attached to whatever the robot is mounted on. Leave it empty in case you already provide the transform.", default_value: "panda_link0", } num_iterations: { diff --git a/src/ktopt_planning_context.cpp b/src/ktopt_planning_context.cpp index b588e66..93bb8a5 100644 --- a/src/ktopt_planning_context.cpp +++ b/src/ktopt_planning_context.cpp @@ -371,10 +371,15 @@ void KTOptPlanningContext::setRobotDescription(const std::string& robot_descript // 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"); + auto robot_instance = drake::multibody::Parser(&plant, &scene_graph); - plant.WeldFrames(plant.world_frame(), plant.GetFrameByName(params_.base_frame)); + for (const auto& path : params_.external_robot_description) + robot_instance.package_map().PopulateFromFolder(path); + + robot_instance.AddModelsFromString(description_with_obj, ".urdf"); + + if (!params_.base_frame.empty()) + plant.WeldFrames(plant.world_frame(), plant.GetFrameByName(params_.base_frame)); // planning scene transcription const auto scene = getPlanningScene();