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