From c873ba5a9e764f0f746d79cf73676cc2b48053fd Mon Sep 17 00:00:00 2001
From: Kzernobog <adityakamireddypalli@gmail.com>
Date: Wed, 18 Sep 2024 15:15:54 +0100
Subject: [PATCH] adds urdf as a param

---
 res/ktopt_moveit_parameters.yaml | 8 ++++++++
 src/ktopt_planning_context.cpp   | 5 +++--
 2 files changed, 11 insertions(+), 2 deletions(-)

diff --git a/res/ktopt_moveit_parameters.yaml b/res/ktopt_moveit_parameters.yaml
index dce6e71..b11c8b0 100644
--- a/res/ktopt_moveit_parameters.yaml
+++ b/res/ktopt_moveit_parameters.yaml
@@ -1,4 +1,12 @@
 ktopt_interface:
+  drake_robot_description: {
+    type: string,
+    description: "Robot description to be loaded by the internal Drake MultibodyPlant",
+    default_value: "package://drake_models/franka_description/urdf/panda_arm_hand.urdf",
+    # validation: {
+    #   gt_eq<>: [1]
+    # }
+  }
   num_iterations: {
     type: int,
     description: "Number of iterations for the Drake mathematical program solver.",
diff --git a/src/ktopt_planning_context.cpp b/src/ktopt_planning_context.cpp
index dc26dee..1fee418 100644
--- a/src/ktopt_planning_context.cpp
+++ b/src/ktopt_planning_context.cpp
@@ -182,8 +182,9 @@ void KTOptPlanningContext::setRobotDescription(std::string robot_description)
   // auto robot_instance = Parser(plant_, scene_graph_).AddModelsFromString(robot_description_, ".urdf");
 
   // HACK: For now loading directly from drake's package map
-  const char* ModelUrl = "package://drake_models/franka_description/"
-                         "urdf/panda_arm_hand.urdf";
+  // const char* ModelUrl = "package://drake_models/franka_description/"
+  //                        "urdf/panda_arm_hand.urdf";
+  const char* ModelUrl = params_.drake_robot_description.c_str();
   const std::string urdf = PackageMap{}.ResolveUrl(ModelUrl);
   auto robot_instance = Parser(&plant, &scene_graph).AddModels(urdf);
   plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("panda_link0"));