From b5c6f2f0838e9f2f8502c641fb83342cc0f0b5ef Mon Sep 17 00:00:00 2001 From: Kzernobog Date: Sat, 5 Oct 2024 14:58:21 +0100 Subject: [PATCH 1/7] [FIX] Readme correction --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index 0f6574f..52f987c 100644 --- a/README.md +++ b/README.md @@ -78,6 +78,8 @@ ros2 launch moveit_drake pipeline_testbench.launch.py - Use [pre-commit to format your code](https://moveit.ros.org/documentation/contributing/code/#pre-commit-formatting-checks) +Within the container you can run the following command to format the code + # inside the moveit_drake package pre-commit run -a From 36d535b61bfe30f427763acdce9edeafed0997f3 Mon Sep 17 00:00:00 2001 From: Kzernobog Date: Sat, 5 Oct 2024 17:40:33 +0100 Subject: [PATCH 2/7] [WIP] adds Box Constraints --- .../ktopt_planning_context.hpp | 3 + res/ktopt_moveit_parameters.yaml | 5 ++ src/ktopt_planning_context.cpp | 56 +++++++++++++++++++ 3 files changed, 64 insertions(+) diff --git a/include/ktopt_interface/ktopt_planning_context.hpp b/include/ktopt_interface/ktopt_planning_context.hpp index eca811f..469f784 100644 --- a/include/ktopt_interface/ktopt_planning_context.hpp +++ b/include/ktopt_interface/ktopt_planning_context.hpp @@ -23,6 +23,8 @@ #include "drake/visualization/visualization_config.h" #include "drake/visualization/visualization_config_functions.h" #include +#include + namespace ktopt_interface { @@ -51,6 +53,7 @@ using drake::multibody::AddMultibodyPlantSceneGraph; using drake::multibody::MinimumDistanceLowerBoundConstraint; using drake::multibody::MultibodyPlant; using drake::multibody::PackageMap; +using drake::multibody::PositionConstraint; using drake::multibody::Parser; using drake::planning::trajectory_optimization::KinematicTrajectoryOptimization; using drake::solvers::Solve; diff --git a/res/ktopt_moveit_parameters.yaml b/res/ktopt_moveit_parameters.yaml index 4a993c0..abdd887 100644 --- a/res/ktopt_moveit_parameters.yaml +++ b/res/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", } + link_ee: { + type: string, + description: "Name of the end-effector frame as per urdf/sdf", + default_value: "panda_hand" + } 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 1a5e4b2..de494ba 100644 --- a/src/ktopt_planning_context.cpp +++ b/src/ktopt_planning_context.cpp @@ -6,6 +6,7 @@ #include #include + #include "ktopt_interface/ktopt_planning_context.hpp" namespace ktopt_interface @@ -52,6 +53,9 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) RCLCPP_INFO_STREAM(getLogger(), "Planning for group: " << getGroupName()); const int num_positions = plant.num_positions(); const int num_velocities = plant.num_velocities(); + const auto link_ee = params_.link_ee; + const auto link_ee_frame = plant.GetFrameByName(link_ee); + // extract position and velocity bounds std::vector lower_position_bounds; @@ -177,6 +181,58 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) // TODO: These should be parameters trajopt.AddDurationConstraint(0.5, 5); + // process path_constraints + if (!req.path_constraints.position_constraints.empty()) { + for (const auto &position_constraint : + req.path_constraints.position_constraints) { + // Extract the bounding box's center (primitive pose) + const auto &primitive_pose = + position_constraint.constraint_region.primitive_poses[0]; + Eigen::Vector3d box_center(primitive_pose.position.x, + primitive_pose.position.y, + primitive_pose.position.z); + + // Extract dimensions of the bounding box from + // constraint_region.primitives Assuming it is a box + // (shape_msgs::SolidPrimitive::BOX) and has dimensions in [x, y, z] + const auto &primitive = + position_constraint.constraint_region.primitives[0]; + if (primitive.type == shape_msgs::SolidPrimitive::BOX) { + double x_dim = + primitive.dimensions[0] / 2.0; + double y_dim = + primitive.dimensions[1] / 2.0; + double z_dim = + primitive.dimensions[2] / 2.0; + + // Calculate the lower and upper bounds based on the box dimensions + // around the center + Eigen::Vector3d lower_bound = + box_center - Eigen::Vector3d(x_dim, y_dim, z_dim); + Eigen::Vector3d upper_bound = + box_center + Eigen::Vector3d(x_dim, y_dim, z_dim); + + // Create PositionConstraint for the path + PositionConstraint path_constraint( + &plant, + plant.world_frame(), // Reference frame is the world frame + lower_bound, // Lower bound on position + upper_bound, // Upper bound on position + gripper_frame, // Frame of the gripper (end-effector) + Eigen::Vector3d( + 0.0, 0.1, + 0.0), // Reference point in the gripper frame (if any offset) + plant_context.get() // Plant context + ); + + // Add position constraint to each knot point of the trajectory + for (int i = 0; i < 10; ++i) { + trajopt.AddPathPositionConstraint(path_constraint, static_cast(i) / 10.0); + } + } + } + } + // solve the program auto result = Solve(prog); From 18515162ccb8161b4e2c1008df4674465195dbfc Mon Sep 17 00:00:00 2001 From: Kzernobog Date: Mon, 7 Oct 2024 11:49:42 +0100 Subject: [PATCH 3/7] [FIX] processes Moveit's PositionConstraint message within Constraints.msg --- CMakeLists.txt | 6 ++++-- package.xml | 1 + src/ktopt_planning_context.cpp | 25 +++++++++---------------- 3 files changed, 14 insertions(+), 18 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 4f45c84..73adfb3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -23,6 +23,7 @@ find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(rviz_visual_tools REQUIRED) find_package(warehouse_ros REQUIRED) +find_package(shape_msgs REQUIRED) generate_parameter_library(ktopt_moveit_parameters res/ktopt_moveit_parameters.yaml) @@ -35,7 +36,8 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_visual_tools rclcpp rviz_visual_tools - warehouse_ros) + warehouse_ros + shape_msgs) include_directories(include) @@ -51,7 +53,7 @@ add_library( src/conversions.cpp) ament_target_dependencies(moveit_drake rclcpp pluginlib moveit_core moveit_msgs - EIGEN3) + EIGEN3 shape_msgs) target_link_libraries(moveit_drake drake::drake ktopt_moveit_parameters) # Ensure that the plugin finds libdrake.so at runtime diff --git a/package.xml b/package.xml index 0d38704..999d362 100644 --- a/package.xml +++ b/package.xml @@ -14,6 +14,7 @@ moveit_visual_tools rviz_visual_tools warehouse_ros_sqlite + shape_msgs eigen moveit_common diff --git a/src/ktopt_planning_context.cpp b/src/ktopt_planning_context.cpp index de494ba..f929ffb 100644 --- a/src/ktopt_planning_context.cpp +++ b/src/ktopt_planning_context.cpp @@ -5,6 +5,7 @@ #include #include #include +#include #include "ktopt_interface/ktopt_planning_context.hpp" @@ -54,7 +55,7 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) const int num_positions = plant.num_positions(); const int num_velocities = plant.num_velocities(); const auto link_ee = params_.link_ee; - const auto link_ee_frame = plant.GetFrameByName(link_ee); + const auto& link_ee_frame = plant.GetFrameByName(link_ee); // extract position and velocity bounds @@ -197,7 +198,7 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) // (shape_msgs::SolidPrimitive::BOX) and has dimensions in [x, y, z] const auto &primitive = position_constraint.constraint_region.primitives[0]; - if (primitive.type == shape_msgs::SolidPrimitive::BOX) { + if (primitive.type == shape_msgs::msg::SolidPrimitive::BOX) { double x_dim = primitive.dimensions[0] / 2.0; double y_dim = @@ -212,22 +213,14 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) Eigen::Vector3d upper_bound = box_center + Eigen::Vector3d(x_dim, y_dim, z_dim); - // Create PositionConstraint for the path - PositionConstraint path_constraint( - &plant, - plant.world_frame(), // Reference frame is the world frame - lower_bound, // Lower bound on position - upper_bound, // Upper bound on position - gripper_frame, // Frame of the gripper (end-effector) - Eigen::Vector3d( - 0.0, 0.1, - 0.0), // Reference point in the gripper frame (if any offset) - plant_context.get() // Plant context - ); - // Add position constraint to each knot point of the trajectory for (int i = 0; i < 10; ++i) { - trajopt.AddPathPositionConstraint(path_constraint, static_cast(i) / 10.0); + trajopt.AddPathPositionConstraint( + std::make_shared( + &plant, plant.world_frame(), lower_bound, upper_bound, + link_ee_frame, Eigen::Vector3d(0.0, 0.1, 0.0), + &plant_context), + static_cast(i) / 10.0); } } } From 1fef47a3ba4a2d32b6d2f692383108a57f6e4815 Mon Sep 17 00:00:00 2001 From: Kzernobog Date: Mon, 7 Oct 2024 11:50:17 +0100 Subject: [PATCH 4/7] pre-commit --- CMakeLists.txt | 10 +++- .../ktopt_planning_context.hpp | 3 +- src/ktopt_planning_context.cpp | 46 ++++++++----------- 3 files changed, 27 insertions(+), 32 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 73adfb3..d19de03 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -52,8 +52,14 @@ add_library( # Conversions src/conversions.cpp) -ament_target_dependencies(moveit_drake rclcpp pluginlib moveit_core moveit_msgs - EIGEN3 shape_msgs) +ament_target_dependencies( + moveit_drake + rclcpp + pluginlib + moveit_core + moveit_msgs + EIGEN3 + shape_msgs) target_link_libraries(moveit_drake drake::drake ktopt_moveit_parameters) # Ensure that the plugin finds libdrake.so at runtime diff --git a/include/ktopt_interface/ktopt_planning_context.hpp b/include/ktopt_interface/ktopt_planning_context.hpp index 469f784..accbf2b 100644 --- a/include/ktopt_interface/ktopt_planning_context.hpp +++ b/include/ktopt_interface/ktopt_planning_context.hpp @@ -25,7 +25,6 @@ #include #include - namespace ktopt_interface { // declare all namespaces to be used @@ -53,8 +52,8 @@ using drake::multibody::AddMultibodyPlantSceneGraph; using drake::multibody::MinimumDistanceLowerBoundConstraint; using drake::multibody::MultibodyPlant; using drake::multibody::PackageMap; -using drake::multibody::PositionConstraint; using drake::multibody::Parser; +using drake::multibody::PositionConstraint; using drake::planning::trajectory_optimization::KinematicTrajectoryOptimization; using drake::solvers::Solve; using drake::systems::Context; diff --git a/src/ktopt_planning_context.cpp b/src/ktopt_planning_context.cpp index f929ffb..55b2d6f 100644 --- a/src/ktopt_planning_context.cpp +++ b/src/ktopt_planning_context.cpp @@ -7,7 +7,6 @@ #include #include - #include "ktopt_interface/ktopt_planning_context.hpp" namespace ktopt_interface @@ -57,7 +56,6 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) const auto link_ee = params_.link_ee; const auto& link_ee_frame = plant.GetFrameByName(link_ee); - // extract position and velocity bounds std::vector lower_position_bounds; std::vector upper_position_bounds; @@ -183,43 +181,35 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) trajopt.AddDurationConstraint(0.5, 5); // process path_constraints - if (!req.path_constraints.position_constraints.empty()) { - for (const auto &position_constraint : - req.path_constraints.position_constraints) { + if (!req.path_constraints.position_constraints.empty()) + { + for (const auto& position_constraint : req.path_constraints.position_constraints) + { // Extract the bounding box's center (primitive pose) - const auto &primitive_pose = - position_constraint.constraint_region.primitive_poses[0]; - Eigen::Vector3d box_center(primitive_pose.position.x, - primitive_pose.position.y, - primitive_pose.position.z); + const auto& primitive_pose = position_constraint.constraint_region.primitive_poses[0]; + Eigen::Vector3d box_center(primitive_pose.position.x, primitive_pose.position.y, primitive_pose.position.z); // Extract dimensions of the bounding box from // constraint_region.primitives Assuming it is a box // (shape_msgs::SolidPrimitive::BOX) and has dimensions in [x, y, z] - const auto &primitive = - position_constraint.constraint_region.primitives[0]; - if (primitive.type == shape_msgs::msg::SolidPrimitive::BOX) { - double x_dim = - primitive.dimensions[0] / 2.0; - double y_dim = - primitive.dimensions[1] / 2.0; - double z_dim = - primitive.dimensions[2] / 2.0; + const auto& primitive = position_constraint.constraint_region.primitives[0]; + if (primitive.type == shape_msgs::msg::SolidPrimitive::BOX) + { + double x_dim = primitive.dimensions[0] / 2.0; + double y_dim = primitive.dimensions[1] / 2.0; + double z_dim = primitive.dimensions[2] / 2.0; // Calculate the lower and upper bounds based on the box dimensions // around the center - Eigen::Vector3d lower_bound = - box_center - Eigen::Vector3d(x_dim, y_dim, z_dim); - Eigen::Vector3d upper_bound = - box_center + Eigen::Vector3d(x_dim, y_dim, z_dim); + Eigen::Vector3d lower_bound = box_center - Eigen::Vector3d(x_dim, y_dim, z_dim); + Eigen::Vector3d upper_bound = box_center + Eigen::Vector3d(x_dim, y_dim, z_dim); // Add position constraint to each knot point of the trajectory - for (int i = 0; i < 10; ++i) { + for (int i = 0; i < 10; ++i) + { trajopt.AddPathPositionConstraint( - std::make_shared( - &plant, plant.world_frame(), lower_bound, upper_bound, - link_ee_frame, Eigen::Vector3d(0.0, 0.1, 0.0), - &plant_context), + std::make_shared(&plant, plant.world_frame(), lower_bound, upper_bound, link_ee_frame, + Eigen::Vector3d(0.0, 0.1, 0.0), &plant_context), static_cast(i) / 10.0); } } From b0514d92be8a2ea7e1aa45ca46c7130423d58294 Mon Sep 17 00:00:00 2001 From: Kzernobog Date: Tue, 8 Oct 2024 12:27:59 +0100 Subject: [PATCH 5/7] [FIX] addresses review comments --- CMakeLists.txt | 4 +- .../ktopt_planning_context.hpp | 4 + res/ktopt_moveit_parameters.yaml | 16 +++ src/ktopt_planning_context.cpp | 100 ++++++++++-------- 4 files changed, 80 insertions(+), 44 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d19de03..84ef579 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -36,8 +36,8 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_visual_tools rclcpp rviz_visual_tools - warehouse_ros - shape_msgs) + shape_msgs + warehouse_ros) include_directories(include) diff --git a/include/ktopt_interface/ktopt_planning_context.hpp b/include/ktopt_interface/ktopt_planning_context.hpp index accbf2b..0698f92 100644 --- a/include/ktopt_interface/ktopt_planning_context.hpp +++ b/include/ktopt_interface/ktopt_planning_context.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include // relevant drake includes @@ -49,6 +50,7 @@ using drake::geometry::SourceId; using drake::geometry::Sphere; using drake::math::RigidTransformd; using drake::multibody::AddMultibodyPlantSceneGraph; +using drake::multibody::Frame; using drake::multibody::MinimumDistanceLowerBoundConstraint; using drake::multibody::MultibodyPlant; using drake::multibody::PackageMap; @@ -80,6 +82,8 @@ class KTOptPlanningContext : public planning_interface::PlanningContext void setRobotDescription(std::string robot_description); void transcribePlanningScene(const planning_scene::PlanningScene& planning_scene); + void addPathPositionConstraints(KinematicTrajectoryOptimization& trajopt, const MultibodyPlant& plant, + const Frame& link_ee_frame, Context& plant_context); private: const ktopt_interface::Params params_; diff --git a/res/ktopt_moveit_parameters.yaml b/res/ktopt_moveit_parameters.yaml index abdd887..7a06e54 100644 --- a/res/ktopt_moveit_parameters.yaml +++ b/res/ktopt_moveit_parameters.yaml @@ -49,6 +49,22 @@ ktopt_interface: gt_eq<>: [0.0] } } + num_position_constraint_points: { + type: int, + description: "Number of points on the path that moveit's bounding box constraint needs to be imposed", + default_value: 10, + validation: { + gt_eq<>: [0.0] + } + } + num_orientation_constraint_points: { + type: int, + description: "Number of points on the path where moveit's orientation constraint needs to be imposed", + default_value: 10, + validation: { + gt_eq<>: [0.0] + } + } joint_jerk_bound: { type: double, description: "Maximum jerk that is allowed for generated trajectory", diff --git a/src/ktopt_planning_context.cpp b/src/ktopt_planning_context.cpp index 55b2d6f..7c6ad5a 100644 --- a/src/ktopt_planning_context.cpp +++ b/src/ktopt_planning_context.cpp @@ -1,11 +1,10 @@ #include +#include #include -#include #include -#include #include -#include +#include #include "ktopt_interface/ktopt_planning_context.hpp" @@ -31,8 +30,8 @@ KTOptPlanningContext::KTOptPlanningContext(const std::string& name, const std::s void KTOptPlanningContext::solve(planning_interface::MotionPlanDetailedResponse& /*res*/) { - RCLCPP_ERROR(getLogger(), - "KTOptPlanningContext::solve(planning_interface::MotionPlanDetailedResponse&) is not implemented!"); + RCLCPP_ERROR(getLogger(), "KTOptPlanningContext::solve(planning_interface::" + "MotionPlanDetailedResponse&) is not implemented!"); return; } @@ -181,40 +180,7 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) trajopt.AddDurationConstraint(0.5, 5); // process path_constraints - if (!req.path_constraints.position_constraints.empty()) - { - for (const auto& position_constraint : req.path_constraints.position_constraints) - { - // Extract the bounding box's center (primitive pose) - const auto& primitive_pose = position_constraint.constraint_region.primitive_poses[0]; - Eigen::Vector3d box_center(primitive_pose.position.x, primitive_pose.position.y, primitive_pose.position.z); - - // Extract dimensions of the bounding box from - // constraint_region.primitives Assuming it is a box - // (shape_msgs::SolidPrimitive::BOX) and has dimensions in [x, y, z] - const auto& primitive = position_constraint.constraint_region.primitives[0]; - if (primitive.type == shape_msgs::msg::SolidPrimitive::BOX) - { - double x_dim = primitive.dimensions[0] / 2.0; - double y_dim = primitive.dimensions[1] / 2.0; - double z_dim = primitive.dimensions[2] / 2.0; - - // Calculate the lower and upper bounds based on the box dimensions - // around the center - Eigen::Vector3d lower_bound = box_center - Eigen::Vector3d(x_dim, y_dim, z_dim); - Eigen::Vector3d upper_bound = box_center + Eigen::Vector3d(x_dim, y_dim, z_dim); - - // Add position constraint to each knot point of the trajectory - for (int i = 0; i < 10; ++i) - { - trajopt.AddPathPositionConstraint( - std::make_shared(&plant, plant.world_frame(), lower_bound, upper_bound, link_ee_frame, - Eigen::Vector3d(0.0, 0.1, 0.0), &plant_context), - static_cast(i) / 10.0); - } - } - } - } + addPathPositionConstraints(trajopt, plant, link_ee_frame, plant_context); // solve the program auto result = Solve(prog); @@ -238,7 +204,8 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) static_cast(s) / (params_.num_collision_check_points - 1)); } - // The previous solution is used to warm-start the collision checked optimization problem + // The previous solution is used to warm-start the collision checked + // optimization problem auto collision_free_result = Solve(prog); // package up the resulting trajectory @@ -257,7 +224,8 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) plant.SetPositions(&plant_context, traj.value(t)); auto& vis_context = visualizer_->GetMyContextFromRoot(*diagram_context_); visualizer_->ForcedPublish(vis_context); - // Without these sleeps, the visualizer won't give you time to load your browser + // Without these sleeps, the visualizer won't give you time to load your + // browser // TODO: This should not hold up planning time std::this_thread::sleep_for(std::chrono::milliseconds(static_cast(params_.trajectory_time_step * 10000.0))); } @@ -267,6 +235,53 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) return; } +void KTOptPlanningContext::addPathPositionConstraints(KinematicTrajectoryOptimization& trajopt, + const MultibodyPlant& plant, + const Frame& link_ee_frame, + Context& plant_context) +{ + // retrieve the motion planning request + const auto& req = getMotionPlanRequest(); + + // check for path position constraints + if (!req.path_constraints.position_constraints.empty()) + { + for (const auto& position_constraint : req.path_constraints.position_constraints) + { + // Extract the bounding box's center (primitive pose) + const auto& primitive_pose = position_constraint.constraint_region.primitive_poses[0]; + Eigen::Vector3d box_center(primitive_pose.position.x, primitive_pose.position.y, primitive_pose.position.z); + + // Extract dimensions of the bounding box from + // constraint_region.primitives Assuming it is a box + // (shape_msgs::SolidPrimitive::BOX) and has dimensions in [x, y, z] + const auto& primitive = position_constraint.constraint_region.primitives[0]; + if (primitive.type != shape_msgs::msg::SolidPrimitive::BOX) + { + continue; + } + + double x_dim = primitive.dimensions[0] / 2.0; + double y_dim = primitive.dimensions[1] / 2.0; + double z_dim = primitive.dimensions[2] / 2.0; + + // Calculate the lower and upper bounds based on the box dimensions + // around the center + Eigen::Vector3d lower_bound = box_center - Eigen::Vector3d(x_dim, y_dim, z_dim); + Eigen::Vector3d upper_bound = box_center + Eigen::Vector3d(x_dim, y_dim, z_dim); + + // Add position constraint to each knot point of the trajectory + for (int i = 0; i < 10; ++i) + { + trajopt.AddPathPositionConstraint( + std::make_shared(&plant, plant.world_frame(), lower_bound, upper_bound, link_ee_frame, + Eigen::Vector3d(0.0, 0.0, 0.0), &plant_context), + static_cast(i) / (params_.num_position_constraint_points - 1)); + } + } + } +} + bool KTOptPlanningContext::terminate() { RCLCPP_ERROR(getLogger(), "KTOptPlanningContext::terminate() is not implemented!"); @@ -287,7 +302,8 @@ void KTOptPlanningContext::setRobotDescription(std::string robot_description) auto [plant, scene_graph] = AddMultibodyPlantSceneGraph(builder.get(), 0.0); // TODO:(kamiradi) Figure out object parsing - // auto robot_instance = Parser(plant_, scene_graph_).AddModelsFromString(robot_description_, ".urdf"); + // auto robot_instance = Parser(plant_, + // scene_graph_).AddModelsFromString(robot_description_, ".urdf"); const char* ModelUrl = params_.drake_robot_description.c_str(); const std::string urdf = PackageMap{}.ResolveUrl(ModelUrl); From fb4fcb831b782fc488e07586a2572adbd98b7c55 Mon Sep 17 00:00:00 2001 From: Kzernobog Date: Tue, 8 Oct 2024 12:51:59 +0100 Subject: [PATCH 6/7] [FIX] Adds warning, corrects order in cmakelists --- CMakeLists.txt | 6 +++--- res/ktopt_moveit_parameters.yaml | 2 +- src/ktopt_planning_context.cpp | 3 ++- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 84ef579..bf754c9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -54,11 +54,11 @@ add_library( ament_target_dependencies( moveit_drake - rclcpp - pluginlib + EIGEN3 moveit_core moveit_msgs - EIGEN3 + pluginlib + rclcpp shape_msgs) target_link_libraries(moveit_drake drake::drake ktopt_moveit_parameters) diff --git a/res/ktopt_moveit_parameters.yaml b/res/ktopt_moveit_parameters.yaml index 7a06e54..a655093 100644 --- a/res/ktopt_moveit_parameters.yaml +++ b/res/ktopt_moveit_parameters.yaml @@ -6,7 +6,7 @@ ktopt_interface: } link_ee: { type: string, - description: "Name of the end-effector frame as per urdf/sdf", + description: "Name of the end-effector frame as per urdf/srdf", default_value: "panda_hand" } num_iterations: { diff --git a/src/ktopt_planning_context.cpp b/src/ktopt_planning_context.cpp index 7c6ad5a..411b0c2 100644 --- a/src/ktopt_planning_context.cpp +++ b/src/ktopt_planning_context.cpp @@ -258,6 +258,7 @@ void KTOptPlanningContext::addPathPositionConstraints(KinematicTrajectoryOptimiz const auto& primitive = position_constraint.constraint_region.primitives[0]; if (primitive.type != shape_msgs::msg::SolidPrimitive::BOX) { + RCLCPP_WARN(getLogger(), "Expects a bounding box constraint as a SolidPrimitive::BOX"); continue; } @@ -271,7 +272,7 @@ void KTOptPlanningContext::addPathPositionConstraints(KinematicTrajectoryOptimiz Eigen::Vector3d upper_bound = box_center + Eigen::Vector3d(x_dim, y_dim, z_dim); // Add position constraint to each knot point of the trajectory - for (int i = 0; i < 10; ++i) + for (int i = 0; i < params_.num_position_constraint_points; ++i) { trajopt.AddPathPositionConstraint( std::make_shared(&plant, plant.world_frame(), lower_bound, upper_bound, link_ee_frame, From db0fe36dc617e455f25146825e3379696c51e213 Mon Sep 17 00:00:00 2001 From: Kzernobog Date: Tue, 8 Oct 2024 14:15:55 +0100 Subject: [PATCH 7/7] [FIX] removes link_ee --- .../ktopt_interface/ktopt_planning_context.hpp | 2 +- res/ktopt_moveit_parameters.yaml | 5 ----- src/ktopt_planning_context.cpp | 15 +++++++++++---- 3 files changed, 12 insertions(+), 10 deletions(-) diff --git a/include/ktopt_interface/ktopt_planning_context.hpp b/include/ktopt_interface/ktopt_planning_context.hpp index 0698f92..637a63b 100644 --- a/include/ktopt_interface/ktopt_planning_context.hpp +++ b/include/ktopt_interface/ktopt_planning_context.hpp @@ -83,7 +83,7 @@ class KTOptPlanningContext : public planning_interface::PlanningContext void setRobotDescription(std::string robot_description); void transcribePlanningScene(const planning_scene::PlanningScene& planning_scene); void addPathPositionConstraints(KinematicTrajectoryOptimization& trajopt, const MultibodyPlant& plant, - const Frame& link_ee_frame, Context& plant_context); + Context& plant_context); private: const ktopt_interface::Params params_; diff --git a/res/ktopt_moveit_parameters.yaml b/res/ktopt_moveit_parameters.yaml index a655093..972d740 100644 --- a/res/ktopt_moveit_parameters.yaml +++ b/res/ktopt_moveit_parameters.yaml @@ -4,11 +4,6 @@ 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", } - link_ee: { - type: string, - description: "Name of the end-effector frame as per urdf/srdf", - default_value: "panda_hand" - } 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 411b0c2..6771aaa 100644 --- a/src/ktopt_planning_context.cpp +++ b/src/ktopt_planning_context.cpp @@ -52,8 +52,8 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) RCLCPP_INFO_STREAM(getLogger(), "Planning for group: " << getGroupName()); const int num_positions = plant.num_positions(); const int num_velocities = plant.num_velocities(); - const auto link_ee = params_.link_ee; - const auto& link_ee_frame = plant.GetFrameByName(link_ee); + // const auto link_ee = params_.link_ee; + // const auto& link_ee_frame = plant.GetFrameByName(link_ee); // extract position and velocity bounds std::vector lower_position_bounds; @@ -180,7 +180,7 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) trajopt.AddDurationConstraint(0.5, 5); // process path_constraints - addPathPositionConstraints(trajopt, plant, link_ee_frame, plant_context); + addPathPositionConstraints(trajopt, plant, plant_context); // solve the program auto result = Solve(prog); @@ -237,7 +237,6 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) void KTOptPlanningContext::addPathPositionConstraints(KinematicTrajectoryOptimization& trajopt, const MultibodyPlant& plant, - const Frame& link_ee_frame, Context& plant_context) { // retrieve the motion planning request @@ -255,6 +254,14 @@ void KTOptPlanningContext::addPathPositionConstraints(KinematicTrajectoryOptimiz // Extract dimensions of the bounding box from // constraint_region.primitives Assuming it is a box // (shape_msgs::SolidPrimitive::BOX) and has dimensions in [x, y, z] + const auto link_ee = position_constraint.link_name; + if (!plant.HasBodyNamed(link_ee)) + { + RCLCPP_ERROR(getLogger(), "The link specified in the PositionConstraint message does not exist in the Drake " + "plant, please check your URDF"); + continue; + } + const auto& link_ee_frame = plant.GetFrameByName(link_ee); const auto& primitive = position_constraint.constraint_region.primitives[0]; if (primitive.type != shape_msgs::msg::SolidPrimitive::BOX) {