Ability to set a robot_description that is not within the drake_model…

…s package (#69)

* Ability to set a robot_description that is not within the drake_models package

* Fix typo

* Add instructions on how to use external_robot_description to the README

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

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: {

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();