diff --git a/parameters/ktopt_moveit_parameters.yaml b/parameters/ktopt_moveit_parameters.yaml index 82de886..eb7858a 100644 --- a/parameters/ktopt_moveit_parameters.yaml +++ b/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 --git a/src/ktopt_planning_context.cpp b/src/ktopt_planning_context.cpp index 65afb68..f421df3 100644 --- a/src/ktopt_planning_context.cpp +++ b/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( &plant, params_.collision_check_lower_distance_bound, &plant_context), static_cast(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();