Make base frame a parameter (#63)

Author: sjahr

Diff for: parameters/ktopt_moveit_parameters.yaml

@@ -4,6 +4,11 @@ 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",
   }
+  base_frame: {
+    type: string,
+    description: "Base frame of the robot that is attached to whatever the robot is mounted on.",
+    default_value: "panda_link0",
+  }
   num_iterations: {
     type: int,
     description: "Number of iterations for the Drake mathematical program solver.",

Diff for: src/ktopt_planning_context.cpp

@@ -161,6 +161,7 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res)
   // add collision constraints
   for (int s = 0; s < params_.num_collision_check_points; ++s)
   {
+    // The constraint will be evaluated as if it is bound with variables corresponding to r(s).
     trajopt.AddPathPositionConstraint(std::make_shared<drake::multibody::MinimumDistanceLowerBoundConstraint>(
                                           &plant, params_.collision_check_lower_distance_bound, &plant_context),
                                       static_cast<double>(s) / (params_.num_collision_check_points - 1));
@@ -375,7 +376,7 @@ void KTOptPlanningContext::setRobotDescription(const std::string& robot_descript
   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("panda_link0"));
+  plant.WeldFrames(plant.world_frame(), plant.GetFrameByName(params_.base_frame));
 
   // planning scene transcription
   const auto scene = getPlanningScene();